IMU covariance value output

[samkys]

Great driver but I have a question:

When trying to fuse in the IMU data into the EKF implemented via the robot_localization package I have a problem. I can get it to work ok just not great. Then looking through the IMU message that is published all of the covariance values are 0.

I have a problem with this,

http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Imu.html

1)

# If the covariance of the measurement is known, it should be filled in (if all you know is the 
# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
# A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
# data a covariance will have to be assumed or gotten from some other source
#
# If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation 
# estimate), please set element 0 of the associated covariance matrix to -1
# If you are interpreting this message, please check for a value of -1 in the first element of each 
# covariance matrix, and disregard the associated estimate.

http://docs.ros.org/melodic/api/robot_localization/html/preparing_sensor_data.html

2) Covariance: Covariance values matter to robot_localization. robot_pose_ekf attempts to fuse all pose variables in an odometry message. Some robots’ drivers have been written to accommodate its requirements. This means that if a given sensor does not produce a certain variable (e.g., a robot that doesn’t report Z position), then the only way to get robot_pose_ekf to ignore it is to inflate its variance to a very large value (on the order of 1e3) so that the variable in question is effectively ignored. This practice is both unnecessary and even detrimental to the performance of robot_localization. The exception is the case where you have a second input source measuring the variable in question, in which case inflated covariances will work.

I believe I have some code for this somewhere if this would be useful to you all. It would take me a little bit to put it together as I need to find it and apply it.

What would be included in that fix:

1) Covariance values would be retrieved from a yaml file as input parameters. 2) Covariance for the orientation estimate I believe is available as a value from the IMU EKF output. (This one may take a little longer to put together as I am not immediately finding it.)

[bsb808]

Thank you bringing up the issue.

To clarify, the driver publishes a few different things that can be integrated into robot_localization.

1. The nav/odom (nav_msgs/Odometry) message is the EKF combined IMU/GPS. The covariance estimates (pose and twist) are provided by the instrument and used to populate the message covariance.
2. The gps/fix (sensor_msgs/NavSatFix) message from the standalone GPS. The covariance estimates are provided by the instrument and used to populate the message covariance.
3. The imu/data (sensor_msgs/Imu) message from the standalone IMU. No covariance estimates are provided.

So, what you are proposing is to add functionality to populate the covariance estimates in the sensor_msgs/Imu messages - true?

This would be particularly useful for the -25 models (AHRS only). It appears that the "5.2.6 Attitude Uncertainty, Euler Angles (0x82, 0x0A)" sensor interface packet would provide orientation_covariance data directly from the sensor.

I think it would be ideal if the driver would allow for...

1. Getting orientation_covariance data directly from the sensor via the comm. protocol OR allow the user set static covariance data via a yaml file.
2. Allow user to specify static angular_velocity_covariance and linear_acceleration_covariance values via yaml file.

[samkys]

Yup, in the -25 model is the one I am referring to and on the imu/data message as that is what we have and are using. Give me a bit to dig through the source as it has changed a bit since last time I dug into it.

[samkys]

As a side note, the spacing and stuff in the code is hard to look at, randomly stuff is indented or less indented. I want to correct that at the same time in the source file. Edit: (Since this is not directly related to the initial I will open a new PR and Issue for this alone. First need to fix the covariance items.)

[samkys]

@bsb808 Another thing I noticed that will need to be done in order for us to actually get the true orientation uncertainty values.

1) The IMU data set does not contain uncertainty values, so the way the driver is currently setup we cannot publish the orientation covariance directly by pulling from the AHRS data packet. ie the IMU Data set (0x80) only contains values. 2) The Filter data set has everything for the IMU message. Problem with this route, it's filtered.

Two options to go forward:

1) Based on parameter, we published filtered IMU data from the switch statement in the Filter data group (0x82) or we publish it from the AHRS data group (0x80) Pros: We would have the choice of either option and more flexibility. We take full advantage of the work the manufacture has put into the product. Cons: In the AHRS mode, all covariances would be static. The driver starts to grow in size.

2) We could leave it roughly as is and have some class variable or something that gets assigned when a filtered data packet is received and then do the assignment in the AHRS callback. Pros: Not much changes. Cons: Probably not the best coding practice. Not as flexible as it is more of a hacky solution.

[samkys]

I vote we go with option 1. Let me know what you all would like to see as I have forked the driver and plan to implement 1 either way but can submit PR for whichever method everyone else wants to see.

[samkys]

See PR #21 it is an update to PR #20

[samkys]

@bsb808 @tonybaltovski

On my forked repo I have updated the driver again. This is on the master branch as PR #21 is set to track my cleanup branch.

Now the driver gets the Kalman Filtered IMU data from the IMU itself. The benefit to this: 1) We get accelerations with gravity removed. 2) We get orientation uncertainty values. 3) We get filtered acceleration and angular velocities (ie biases removed) 4) We still can run the standard IMU information (ie the Complementary filter) 5) The Kalman filter seems more stable (Up for debate, this is just a visual look at the data right now in RVIZ.)

I have tested it with my -25 model and it seems to work. I am going to be using it for a while now so will let you know if I run into anything.

The reason I did not want to setup a new PR is because #21 is still open and pending, I don't want to stack them up etc. like happened with #20 and #21. If you want me to set one up let me know.

[samkys]

Solved. See PR #21