Revision history [back]

So you're fusing only velocity data. Every velocity measurement contains error. When you integrate the velocity measurements into a pose estimate, you also integrate the errors. Drift will always be an issue when you have no absolute pose data included.

But note that your wheel encoder odometry yaw is also accumulating error, before you even send it to the EKF. The robot's odometry will just be counting encoder ticks, which is also subject to error.

In any case, the root of the issue here is that your IMU is likely noisier/more biased than your wheel odometry. Try fusing yaw velocity from your wheel odometry and the IMU, and see what happens.

So you're fusing only velocity data. Every velocity measurement contains error. When you integrate the velocity measurements into a pose estimate, you also integrate the errors. Drift will always be an issue when you have no absolute pose data included.

But note that your wheel encoder odometry yaw is also accumulating error, before you even send it to the EKF. The robot's odometry will just be counting encoder ticks, which is also subject to error.

In any case, the root of the issue here is that your IMU is likely noisier/more biased than your wheel odometry. Try fusing yaw velocity from your wheel odometry and the IMU, and see what happens.

So you're fusing only velocity data. Every velocity measurement contains error. When you integrate the velocity measurements into a pose estimate, you also integrate the errors. Drift will always be an issue when you have no absolute pose data included.

But note that your wheel encoder odometry yaw is also accumulating error, before you even send it to the EKF. The robot's odometry will just be counting encoder ticks, which is also subject to error.

In any case, the root of the issue here is that your IMU is likely noisier/more biased than your wheel odometry. Try fusing yaw velocity from your wheel odometry and the IMU, and see what happens.

EDIT after question update: yeah, your covariance values for angular velocity are preventing the filter from caring about the measurements from wheel odometry. Your wheel encoders have a Z angular velocity variance of 0.03 (standard deviation of 0.173 rad/sec per measurement), whereas your imu/data_raw input is reporting an angular velocity variance of 1.2184696791468346e-07 (standard deviation of 0.0003491 rad/sec per measurement). That means you are telling the filter that your IMU is many orders of magnitude more trustworthy than your wheel odometry. You might as well not even be fusing wheel odometry yaw velocity in that case.

I'd also check your process noise covariance for yaw velocity, and make sure it's not too small, lest the filter ignore measurements because it trusts its own internal model too much (and the angular velocity model is literally just projecting the current velocity to the next timestep, so it shouldn't).

So you're fusing only velocity data. Every velocity measurement contains error. When you integrate the velocity measurements into a pose estimate, you also integrate the errors. Drift will always be an issue when you have no absolute pose data included.

But note that your wheel encoder odometry yaw is also accumulating error, before you even send it to the EKF. The robot's odometry will just be counting encoder ticks, which is also subject to error.

In any case, the root of the issue here is that your IMU is likely noisier/more biased than your wheel odometry. Try fusing yaw velocity from your wheel odometry and the IMU, and see what happens.

EDIT after question update: yeah, your covariance values for angular velocity are preventing the filter from caring about the measurements from wheel odometry. Your wheel encoders have a Z angular velocity variance of 0.03 (standard deviation of 0.173 rad/sec per measurement), whereas your imu/data_raw input is reporting an angular velocity variance of 1.2184696791468346e-07 (standard deviation of 0.0003491 rad/sec per measurement). That means you are telling the filter that your IMU is many orders of magnitude more trustworthy than your wheel odometry. You might as well not even be fusing wheel odometry yaw velocity in that case.

I'd also check your process noise covariance for yaw velocity, and make sure it's not too small, lest the filter ignore measurements because it trusts its own internal model too much (and the angular velocity model is literally just projecting the current velocity to the next timestep, so it shouldn't).

EDIT after second question update: OK, so odom fusion works by itself. My money is on sensor frequency now. What is your odometry message frequency vs. the IMU frequency? Even with equivalent covariance values, if you have 10 IMU messages for every odometry message, your odometry data will be effectively lost.

So you're fusing only velocity data. Every velocity measurement contains error. When you integrate the velocity measurements into a pose estimate, you also integrate the errors. Drift will always be an issue when you have no absolute pose data included.

But note that your wheel encoder odometry yaw is also accumulating error, before you even send it to the EKF. The robot's odometry will just be counting encoder ticks, which is also subject to error.

In any case, the root of the issue here is that your IMU is likely noisier/more biased than your wheel odometry. Try fusing yaw velocity from your wheel odometry and the IMU, and see what happens.

EDIT after question update: yeah, your covariance values for angular velocity are preventing the filter from caring about the measurements from wheel odometry. Your wheel encoders have a Z angular velocity variance of 0.03 (standard deviation of 0.173 rad/sec per measurement), whereas your imu/data_raw input is reporting an angular velocity variance of 1.2184696791468346e-07 (standard deviation of 0.0003491 rad/sec per measurement). That means you are telling the filter that your IMU is many orders of magnitude more trustworthy than your wheel odometry. You might as well not even be fusing wheel odometry yaw velocity in that case.

I'd also check your process noise covariance for yaw velocity, and make sure it's not too small, lest the filter ignore measurements because it trusts its own internal model too much (and the angular velocity model is literally just projecting the current velocity to the next timestep, so it shouldn't).

EDIT after second question update: OK, so odom fusion works by itself. My money is on sensor frequency now. What is your odometry message frequency vs. the IMU frequency? Even with equivalent covariance values, if you have 10 IMU messages for every odometry message, your odometry data will be effectively lost.

EDIT after debug file

OK, you have two problems.

First, your IMU is really off. As an example, at one of your time steps, your wheel odometry is showing an angular velocity of -0.4103, but in that same moment, the IMU is reading -0.61551. So what you're seeing in your filter output is the filter splitting the difference between the two, with a bias towards the wheel odom. It's fusing the data from both. If you _just_ include IMU rotational data (turn off the angular velocity for wheel odom), I'm guessing the output would be far uglier.

Your process noise covariance is too high, given the noise in your sensors. What's happening is that you get an odom velocity with a tiny covariance, on the order of e-11. The filter does a predict, which adds the process noise to that quantity in its internal covariance estimate. It ends up with a covariance of something e-5. So when we do the correct step, the wheel odometry data is trusted completely, and the filter effectively jumps to the velocity in the wheel odom data. Then you get an IMU message. Again, the filter predicts across that time delta, and its internal covariance estimate moves to something on the order of e-5. The IMU data has a covariance on the order of e-7, so the filter again just trusts the IMU data completely, and jumps to that velocity. So in this case, the more frequent sensor will always "win."

My recommendation is that you stop trying to force the filter to behave a certain way, and start by giving your sensors realistic covariance values. Drive the robot in a square and measure the integrated wheel odometry yaw error after that square. Divide that total error by the number of measurements, convert to variance, and use that as your angular velocity covariance. Repeat for the IMU. You'll need some way to ground truth it.

Once you are satisfied that your sensors have accurate covariance values, then get the filter's process noise to a point where the prediction step doesn't make the filter's internal covariance huge w.r.t. the measurements.

But be warned: the IMU is clearly off. The filter will always include _some_ of its measurement in the output, so if it's over-estimating your turns by a lot, it's going to drag the estimate in that direction.

So you're fusing only velocity data. Every velocity measurement contains error. When you integrate the velocity measurements into a pose estimate, you also integrate the errors. Drift will always be an issue when you have no absolute pose data included.

But note that your wheel encoder odometry yaw is also accumulating error, before you even send it to the EKF. The robot's odometry will just be counting encoder ticks, which is also subject to error.

In any case, the root of the issue here is that your IMU is likely noisier/more biased than your wheel odometry. Try fusing yaw velocity from your wheel odometry and the IMU, and see what happens.

EDIT after question update: yeah, your covariance values for angular velocity are preventing the filter from caring about the measurements from wheel odometry. Your wheel encoders have a Z angular velocity variance of 0.03 (standard deviation of 0.173 rad/sec per measurement), whereas your imu/data_raw input is reporting an angular velocity variance of 1.2184696791468346e-07 (standard deviation of 0.0003491 rad/sec per measurement). That means you are telling the filter that your IMU is many orders of magnitude more trustworthy than your wheel odometry. You might as well not even be fusing wheel odometry yaw velocity in that case.

I'd also check your process noise covariance for yaw velocity, and make sure it's not too small, lest the filter ignore measurements because it trusts its own internal model too much (and the angular velocity model is literally just projecting the current velocity to the next timestep, so it shouldn't).

EDIT after second question update: OK, so odom fusion works by itself. My money is on sensor frequency now. What is your odometry message frequency vs. the IMU frequency? Even with equivalent covariance values, if you have 10 IMU messages for every odometry message, your odometry data will be effectively lost.

EDIT after debug file

OK, you have two problems.

First, your IMU is really off. As an example, at one of your time steps, your wheel odometry is showing an angular velocity of -0.4103, but in that same moment, the IMU is reading -0.61551. If you _just_ include IMU rotational data (turn off the angular velocity for wheel odom), I'm guessing the output would be far a bit uglier.

Your process noise covariance is too high, given the noise in your sensors. What's happening is that you get an odom velocity with a tiny covariance, on the order of e-11. The filter does a predict, which adds the process noise to that quantity in its internal covariance estimate. It ends up with a covariance of something e-5. So when we do the correct step, the wheel odometry data is trusted completely, and the filter effectively jumps to the velocity in the wheel odom data. Then you get an IMU message. Again, the filter predicts across that time delta, and its internal covariance estimate moves to something on the order of e-5. The IMU data has a covariance on the order of e-7, so the filter again just trusts the IMU data completely, and jumps to that velocity. So in this case, the more frequent sensor will always "win."

My recommendation is that you stop trying to force the filter to behave a certain way, and start by giving your sensors realistic covariance values. Drive the robot in a square and measure the integrated wheel odometry yaw error after that square. Divide that total error by the number of measurements, convert to variance, and use that as your angular velocity covariance. Repeat for the IMU. You'll need some way to ground truth it.

Once you are satisfied that your sensors have accurate covariance values, then get the filter's process noise to a point where the prediction step doesn't make the filter's internal covariance huge w.r.t. the measurements.

But be warned: the IMU is clearly off. The filter will always include _some_ of its measurement in the output, so if it's over-estimating your turns by a lot, it's going to drag the estimate in that direction.