Small drift with real robot

[MichaelT64]

Hi Manan,
sorry it's me again ;-)

I started to work with my real robot and there is a small drift in all three directions and also in the three orientations (values after about 1200 seconds), the robot stood still:

michael@robby-ros:~/ros2_3rd_party_ws$ ros2 topic echo /odometry/filtered --flow-style --once
header:
  stamp:
    sec: 1776785150
    nanosec: 241768158
  frame_id: odom
child_frame_id: base_footprint
pose:
  pose:
    position:
      x: 0.09247610209229912
      y: 0.055439491185446894
      z: 0.01089715114191163
    orientation:
      x: 0.011167582319421252
      y: 0.000933221540147652
      z: 0.13046826898890287
      w: 0.9913891491184151
  covariance: [1707.768615139582, 5.7091724181962355e-06, -0.0003553362289948946, 3.528207213058707e-07, 1.8313063815868658e-06, 4.7250047563642576e-05, 5.709172418196233e-06, 1707.7690711317334, -0.007126136874846765, -1.9229777307085832e-06, -2.2076063097527308e-07, -8.571359981699787e-06, -0.0003553362289948946, -0.007126136874846765, 1708.0748318167325, -4.643591406645115e-08, -5.263241320258877e-08, -3.5526158883863962e-06, 3.528207213058707e-07, -1.9229777307085832e-06, -4.6435914066449456e-08, 7.80177170751125e-06, -1.1586719411408499e-07, -0.000157251152758533, 1.8313063815868658e-06, -2.2076063097527308e-07, -5.2632413202588556e-08, -1.1586719411410596e-07, 1.068783303315602e-05, 0.0012170684031736312, 4.7250047563642576e-05, -8.571359981699787e-06, -3.5526158883863962e-06, -0.000157251152758533, 0.0012170684031736312, 0.0991368564662035]
twist:
  twist:
    linear:
      x: 1.011048692067737e-09
      y: 1.5146587399879294e-09
      z: 7.390027847141912e-05
    angular:
      x: 0.0010877663599699458
      y: 0.0014709944055831706
      z: -0.00014709557475285162
  covariance: [9.53546828065342e-05, 6.523128319685484e-19, -3.910241673124626e-15, 4.376660154306975e-13, -5.147201123503665e-13, 4.3663422568119004e-13, 6.523128319685381e-19, 9.535468280654853e-05, -1.2072564188808236e-14, 2.117454313938842e-12, -7.27552102279188e-13, -9.6452957093442e-14, -3.910241673124626e-15, -1.207256418880824e-14, 0.009575230382599035, 6.55032292259054e-11, 6.498641256452512e-11, 3.25506498813273e-10, 4.3766601543069446e-13, 2.117454313938868e-12, 6.550322922583892e-11, 0.0012665620925990794, 3.0203435819293384e-06, 1.5791807103914993e-08, -5.147201123503568e-13, -7.275521022791898e-13, 6.498641256452507e-11, 3.020343581929365e-06, 0.0012412489516709449, -8.838118098149977e-08, 4.3663422568119004e-13, -9.6452957093442e-14, 3.255064988132738e-10, 1.579180710391499e-08, -8.838118098149977e-08, 6.869822717555358e-05]
---

An example of my imu data:

michael@robby-ros:~/ros2_3rd_party_ws$ ros2 topic echo /imu/data --flow-style --once
header:
  stamp:
    sec: 1776785268
    nanosec: 281810472
  frame_id: imu_link
orientation:
  x: 0.011637636460363865
  y: 0.0008382266387343407
  z: 0.14493729174137115
  w: 0.9893720746040344
orientation_covariance: [1.98498221e-08, 3.83160428e-09, -2.47076906e-10, 3.83160428e-09, 1.3021946e-08, -4.30599945e-09, -2.47076906e-10, -4.30599945e-09, 6.88897697e-08]
angular_velocity:
  x: -0.0015963378257583827
  y: 0.0002873407839797436
  z: 0.002830839072121307
angular_velocity_covariance: [4.56587168e-06, 1.18437869e-06, 8.78951037e-07, 1.18437869e-06, 2.22839975e-06, -2.84728062e-07, 8.78951037e-07, -2.84728062e-07, 4.9531171e-06]
linear_acceleration:
  x: -0.2897091358591388
  y: 0.01991560237246488
  z: 9.699741346102286
linear_acceleration_covariance: [0.00652297, 0.00036781, 0.00134905, 0.00036781, 0.02387888, -0.00118197, 0.00134905, -0.00118197, 0.00971446]
---

And for completness the original odometry data:

michael@robby-ros:~/ros2_3rd_party_ws$ ros2 topic echo /odometry/wheels --flow-style --once
header:
  stamp:
    sec: 1776785310
    nanosec: 281742814
  frame_id: odom
child_frame_id: base_footprint
pose:
  pose:
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]
twist:
  twist:
    linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [3.3e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.3e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0028]
---

It seems to me that the noise imu data has higher precedence than the stable odom data, but I am not an expert in this.

[manankharwar]

@MichaelT64 I can take a look here! Thanks for opening the issue.

[manankharwar]

Hi Michael,

Thanks for the detailed drift report. I can see what's happening... the noisy IMU is being weighted too heavily while the stable encoder data is being under-utilized. To pin down the exact cause, I need three things:

---

1. YOUR ACTUAL CONFIGURATION FILE

Please send your fusioncore.yaml config. Specifically, I need to check:

• imu.accel_noise (default 0.1)
• zupt.velocity_threshold (default 0.05 m/s)
• zupt.angular_threshold (default 0.05 rad/s)
• adaptive.window (default 50)
• adaptive.alpha (default 0.01)
• encoder.vel_noise (default 0.05)

If you're using any custom values, that explains a lot.

---

2. A ROSBAG FROM THE DRIFT TEST

Run this while your robot sits completely still for 120+ seconds:

ros2 bag record /imu/data /odom/wheels /fusion/odom -o drift_test

Then send the resulting drift_test/ directory (usually a few MB).

This recording will show me exactly how the filter is weighting each sensor over time.

---

3. QUICK TECHNICAL DETAILS

Please confirm:

✓ Your IMU type: Is it 6-axis (gyro + accel only) or 9-axis (includes magnetometer)?

• Look at the IMU spec sheet or search your driver docs for "magnetometer"
• This determines whether heading can be validated independently

✓ How long was the filter running before it sat still?

• Did you initialize, let it run for a minute, then stop?
• Or did you initialize and immediately stop?
• The adaptive noise needs ~30 seconds of data to converge

✓ Drift pattern: Looking at the odometry data you sent, is the drift:

• Linear (position increases steadily, like constant velocity)?
• Exponential (position change accelerates over time)?
• Step-like (sudden jumps then stable)?

---

Why this matters:

Your IMU reports Z-acceleration noise of ~0.29 m/s², but the default config assumes imu.accel_noise: 0.1. If your actual IMU is noisier than configured, the filter will trust it too much. The ZUPT (zero-velocity update) should clamp this, but if it's not firing... or firing too weakly... the IMU noise will accumulate into drift.

The rosbag will let me see whether ZUPT is actually engaging, and whether the adaptive noise is learning the true sensor characteristics.

Thanks,
Manan

[MichaelT64]

Here my answers:
1.
my fusioncore.yaml is a copy of the original (except base_frame).
2.
rosbag file is added as zip (from start several minutes long)
3.

• the IMU is a MPU9250, which has a magnetometer, but it is not used in the driver.
• the /odometry/filtered data are values after about 1200 seconds running (the robot stood still the whole time)
• the drift pattern is hard to say, I think jittering describes it at best

drift_test.zip

Let me know if you need more
Michael

[josephduchesne]

Just as some feedback on this sort of issue: this is relatively common with the old ros2 ekf package as well if you have no fixed sources of input (that is, no x/y/z position sources to prevent integration errors/small non-zero velocity or acceleration components from "running away").

Do you have a gps position input?

Common reasons are temperature/vibration related perturbations, the robot not being level causing an accelerometer bias (although I believe this package intends to try to filter that out), or the robot odometry wobbling slightly in place.

I've found that it's often best to have a zeroing filter between the ekf layer and inputs if the robot is known to be stationary. One way to do that is to zero odom+IMU inputs if your drivetrain's cmd_vel times out/has been zero for some duration. Obviously this approach isn't a good idea if your robot needs to track movement that bypasses the drivetrain.

[manankharwar]

Hi @MichaelT64

Thanks for the rosbag.... it makes everything very clear.

The good news: this is a ~60 second warmup transient, not ongoing drift.

Looking at the bag data, almost all the position offset happens in the first 60 seconds. After that, position barely moves for the remaining 8+ minutes:

t=0s: 0cm
t=60s: 9cm ← almost all drift
t=300s: 10.4cm ← essentially unchanged
t=525s: 9.7cm ← slightly recovering

Root cause: Your IMU has a constant -0.22 m/s² bias on the X accelerometer (your robot is likely very slightly tilted, or the MPU9250 has a small calibration offset). The filter has an accelerometer bias estimator built in, but it starts at zero and takes ~60 seconds to converge to the true bias. During those 60 seconds, the filter briefly thinks the robot is accelerating in X, which integrates into the ~10cm offset. Once the bias is learned, ZUPT holds position steady.

The yaw drift is tiny (2.2° over 9 minutes).... the gyro bias is very small and ZUPT is handling it well.

You could possibly wait 60 seconds... (I don't know how convenient that is for an open-source package @josephduchesne, you can chime in here)

OR

I've pushed a fix:
Fix just pushed: I've added an init.stationary_window parameter. Add this to your config:

init.stationary_window: 2.0

This makes the filter collect 2 seconds of IMU data at startup, estimate the bias directly, and initialize with the correct values. The startup transient should drop from ~10cm to under 1cm.

Pull and rebuild:

git pull
colcon build --packages-select fusioncore_ros

Note: the window only activates if your robot is stationary at startup. If you start moving before 2 seconds, it falls back to the old behavior automatically.

cc: @josephduchesne

[manankharwar]

Hey @MichaelT64, just checking... did init.stationary_window: 2.0 sort out the startup drift? Would love to know if the 60s transient is gone.