Revision history [back]

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.