Revision history [back]

So my advice is to fuse one sensor at a time, but that comes with two caveats:

1. Don't fuse one _variable_ at a time. If a given sensor provides more than one variable, fuse everything you want from that sensor. Look at your odometry config:

   odom0_config: [false, false, false,
                  false, false, false,
                  true , false, false,
                  false, false, false,
                  false, false, false]
   

   You are fusing _only_ X velocity. The filter is estimating a 15-dimensional state, so every relevant pose variable (in this case, as you have two_d_mode set to _true_, that means X, Y, and yaw) needs to be either measured as a pose (i.e., directly) or as a velocity. Please change your odometry config to this (set Y velocity and yaw velocity to _true_).

   odom0_config: [false, false, false,
                  false, false, false,
                  true , true, false,
                  false, false, true,
                  false, false, false]
   

   As you have it, the filter has to make predictions of the future state based on variables that are not being measured. The explosion in covariance will mean that the output will be completely unstable.

2. If you have a sensor that only produces a single variable (e.g., a barometer that only produces Z position), then you should send some "fake" values for all the other variables to "clamp" them to a fixed value (read: 0). That, or just fuse the 0 values that come in for those variables, e.g., if the barometer data comes in on a PoseWithCovarianceStamped message, then go ahead and fuse all the other pose variables, which will probably have a 0 value anyway.

My initial guess as to why this would be occurring would be this package doesn't handle the fusing of one data point very well, but that would surprise me. I have noticed that the acceleration values in the state matrix are non-zero, and this was pretty confusing to me, considering I never introduced acceleration values.

The magic of matrix inversion and correlated variables means that, even if you don't directly measure, e.g., acceleration, but you do measure velocity, the filter will "create" the accelerations.

Just as a note, I just tried inputing all my data into robot_pose_ekf, and I got a much more realistic result.

That's because, IIRC, robot_pose_ekf doesn't let you control what you're fusing from the sensor input. It just grabs all the 2D pose data from your input messages and fuses it. Increased flexibility, in this case, equates to increased complexity.