Revision history [back]

Your sample odometry message has no covariance given to it. Therefore, the EKF will give it a very low covariance value, which is probably not accurate. You will have to go into the source code of your odom and IMU drivers and manually chanhge the covariance values so that they're accurate. If they aren't, it can cause the filter to oscillate. Also, since you're fusing the yaw value from both odom and IMU, you will want to have one of them set to relative or differential. Maybe try the latter first and see if it helps.