robot_localization delayed yaw response

Question

Our robot has wheel odometry and an IMU (gyro plus accelerometers) and we are migrating from robot_pose_ekf to robot_localization. We expect to add additional IMUs as well as gps once this base configuration is working. I am sending test data to robot_localization as follows:

    vx = 0.1 
   imu.angular_velocity = 0.05 for the first 30 seconds, then -0.1 for 30 sec and then 0.2 
   acclerometers are all set to 0 

All the other inputs are excluded using "false" entries in the sensor selection matrix. We do not specify an absolute yaw value because we are not directly measuring one. robot_localization diganostics notices this and outputs a warning that Yaw is not being sent and that can result in unbounded errors.

The output from robot_localization agrees with the input for x linear twist (0.1) and the z angular twist initially agrees with imu.angular_velocity, but when imu.angular_velocity changes (at 30 seconds), the z angular twist output from robot_localization persists as the original value. This remains the case at 60 seconds when the input changes again, the output is still around the original input value. When first setting up robot_localization, I did not change the covariance matrix and, in that case, the z angular twist output would stay near 0 for the first 5 seconds or so and then jump to equal the imu.angular_velocity. When I modified the covariance matrix, increasing the value for vYaw, then the output jumps much sooner to the input value. However, when the input value changes, the output does not.

I would attach the robot_localization.launch file and a bag file, but ros.answers does not support those file types for upload, only image files.

two_d_mode is set true. differential and relative modes are set false. Here are the frame settings and sensor selection matrices:

  <!-- Defaults to "map" if unspecified -->
  <param name="map_frame" value="map"/>
  <!-- Defaults to "odom" if unspecified -->
  <param name="odom_frame" value="odometry/filtered"/>
  <!-- Defaults to "base_link" if unspecified -->
  <param name="base_link_frame" value="base_footprint"/>
  <!-- Defaults to the value of "odom_frame" if unspecified -->
  <param name="world_frame" value="odometry/filtered"/>

  <!-- we are going to only use vx because that is all the wheel encoders actually report -->
  <rosparam param="odom0_config">[false, false, false,
                                  false, false, false,
                                  true,  false, false,
                                  false, false, false,
                                  false, false, false]</rosparam>

  <!-- we are going to only use vyaw, ax, ay, az because the robot does not roll significantly in the other axes (and two_d_mode is set to true) -->
  <rosparam param="imu0_config">[false, false, false,
                                 false, false, false,
                                 false, false, false,
                                 false,  false,  true,
                                 true,  true,  true]</rosparam>




 

---

Originally posted by dan on ROS Answers with karma: 875 on 2015-08-10

Post score: 1

---

Original comments

Comment by Tom Moore on 2015-08-11:
Can you use Google Drive or DropBox to post a bag file? It would also be useful to post a sample IMU message. Thanks!

Comment by dan on 2015-08-11:
Here is a bag file and an example IMU message from the original issue, as well as the robot_localization.launch file: robot_localization_link

Comment by dan on 2015-08-11:
I found that if I set the vz covariance to a high value the output tracks better (see source file in dropbox). However, it does not make sense to continuously be setting that and also, I thought that one advantage of the UKF was that it tracks the covariance based on the statistics of the data.

Comment by Tom Moore on 2015-08-11:
The measurement covariances will always be critical. Usually, when convergence is slow, it means your initial estimate covariance for that variable is too low, or the measurement covariance for that variable is too high. I don't have DropBox access right now, so I'll check it out later today.

Comment by dan on 2015-08-11:
The original issue was that I was not setting an initial angular vel covariance (so they were all 0), as I thought UKF would build an estimate quickly. Setting a large initial covariance helped, but there is still more lag in the ang vel output than I would like. Is there a way to tune that?

Comment by dan on 2015-08-11:
replaced the bag file, the robot_localization.launch file and the source code file in the dropbox folder with updated ones that show the updated results where the robot_localization output of z angular velocity changes appropriately, but fairly slowly.

Answer 1

Hi Dan,

Here's an example of one of your IMU messages:

header: 
  seq: 241
  stamp: 
    secs: 1439336777
    nsecs: 766463406
  frame_id: Imu_link
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0
orientation_covariance: [1000000.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 1000000.0]
angular_velocity: 
  x: 0.0
  y: 0.0
  z: 0.05
angular_velocity_covariance: [1e-06, 0.0, 0.0, 0.0, 1e-06, 0.0, 0.0, 0.0, 1000.0]
linear_acceleration: 
  x: 0.0
  y: 0.0
  z: 0.0
linear_acceleration_covariance: [0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001]

Your issue, at least in part, is that your yaw velocity covariance (last value in angular_velocity_covariance) is massive. You're giving the filter a yaw velocity with a variance of 1000.

Apologies if you're aware of this, but just so we're on the same page, there are two covariance matrices that are driving the behavior you're seeing. The first is the initial_estimate_covariance in the launch file. It specifies the initial covariance matrix for the entire state estimate. In it, you have the yaw velocity variance set to 1e-3. The other matrix of consequence is the measurement covariance matrix, which is specified in the IMU message itself. In it, you have a huge variance for your yaw velocity, so when you take in your first measurement, the filter trusts its own error estimate for yaw velocity (with value 0 and variance 1e-3) much more than the value from your measurement (with value 0.05 and variance 1000).

To summarize, to increase the speed of convergence, do these three things:

1. Change the initial_estimate_covariance matrix so that the value at (12, 12) is something large, e.g., 100.
2. Change your IMU message so that the angular_velocity_covariance is much smaller (e.g., 0.02). If your IMU has any kind of documentation, you can probably get a reasonable value from there.
3. Ditch all of the Mahalanobis distance settings in the advanced section of the launch file. I plan to modify that the template launch file so that all the advanced parameters are commented out by default.

---

Originally posted by Tom Moore with karma: 13689 on 2015-08-12

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by dan on 2015-08-12:
Works nicely! I was confused about the relationships of the various covariance matrices. Should I be sending the IMU covariance values with every update even though they don't change? Also, how do I estimate those values? I am using the L3GD20H

Comment by Tom Moore on 2015-08-12:
Every IMU message is handled independently, so you must fill out that field in every message, even if the value is constant. A brief look through your document didn't provide any answers re: accuracy. You may have to poke around the web and look for error rates. When in doubt, over-estimate.

Comment by dan on 2015-08-12:
OK, thanks for the help. The datasheet says at a 50Hz output rate, the gyro has a "rate noise density" of 0.11 degrees per sec / sqrt(Hz). I will look further into what that spec means. Thanks for your help on this. I am posting another question about acceleration, Hope that is OK :/