Revision history [back]

A few notes:

• You are fusing yaw from your beacon poses and from the IMU. Do they have the same world frame? In other words, if you point your robot at some direction (say, magnetic north) and drive straight, do the IMU and your beacon have the same heading estimate? It seems that, if your IMU has a compass, it's going to read 0 at magnetic north. Unless you have your beacon system set up such that forward motion towards magnetic north produces a manufactured heading of 0, you're going to see your headings rapidly jump between both estimates.
• I'm not sure that you need a dual EKF setup, because...
• Integration of IMU data alone, or even IMU with some pose reference, will not get what you want. IMUs have accelerometers with biases in them, and those accelerations will be integrated into velocities, which are going to cause your robot's pose estimate to move rapidly away, and then once in a while get "jerked" back towards the pose estimate (whenever it's received). I have yet to add acceleration bias correction for the IMU data to the filter, but even if I did, I think you'd want a velocity reference.

Your setup is pretty much analagous to setups with GPS + IMU alone, and it's not a use case that is especially well-supported by r_l. If I added IMU bias correction, it might help, but time is an increasingly rare commodity. Another option is that you can try differentiating your poses and sending that velocity to the EKF as well. You're giving it the same info twice, but it might make things a bit smoother overall.