IMU fusion

Wheel odometry measures speed by counting motor turns and heading by integrating the steering command, and neither sees slip. A gyro measures rotation directly, and an accelerometer reacts to a change of speed at once. imu_bridge brings both into ROS, calibrated and in the car’s axes, and combines them with the wheels. It publishes no drive topic and cannot move the car.

The package is src/imu_bridge/GitHub, with two nodes in a chain. imu_bridge_node calibrates each IMU on its own and publishes one topic per IMU, without combining them. vehicle_state_node reads those two topics and /odom, and is the only node that combines the two IMUs with each other or with the wheels. The mathematics is in convert.py, imu_health.py and vehicle_state.py, none of which import ROS; the excerpts below are from vehicle_state.py with docstrings and input checks removed. Each car’s IMU mount rotations and calibration are in its profile, src/racerbot_car/cars/<id>/imu/ (car 1Car 1, car 2Car 2), and the calibration tools and the BNO085 bridge are in tools/imu_calibration/GitHub. imu_bridge_node refuses to start if the car’s imu/car.yaml is missing, rather than run with another car’s mounts.

Topics

imu_bridge_node

imu_bridge_node turns each IMU’s raw readings into calibrated readings in the car’s axes, and reports whether the primary IMU can be trusted. It handles the VESC IMU and the BNO085 separately and never combines them: /imu/vesc/data and /imu/bno/data each carry one IMU, and /imu/data is a copy of whichever one is primary. The primary parameter in imu_bridge.yaml picks it once at startup, and its default, auto, picks the BNO085 when a BNO085 port is set and the VESC IMU otherwise. The neural racer reads /imu/data and /imu/status, and vehicle_state_node reads the two per-IMU topics.

Each sample goes through three steps before it is published.

1. Units. The VESC reports acceleration in g and rotation in degrees per second. One g is 9.80665 m/s², and 180°/s is $\pi$ rad/s.
2. Axes. Each IMU is mounted in its own orientation. The mount rotation from the car’s imu/car.yaml turns its axes into the car’s: $x$ forward, $y$ left, $z$ up. Getting this wrong swaps or flips axes, so a forward push reads as sideways acceleration.
3. Bias. A gyro reads slightly non-zero when nothing is turning. For the first 3 s while the car is still, the node averages the gyro and subtracts that average from then on. It counts only samples where the wheels read under 0.03 m/s and every gyro axis reads under 0.05 rad/s, and any movement restarts the window.

/imu/status is a JSON verdict on the primary IMU only, latched so a node that starts late gets the current one at once. A fault on the other IMU does not show there; vehicle_state_node stops using that IMU instead, as described below.

The gravity check uses the one thing an IMU at rest must measure: gravity, 9.80665 m/s², pointing straight up in car axes. If the still IMU reads a different magnitude, or a direction tilted too far from $+z$, the units or the mount rotation are wrong. That fault latches until the node restarts, because nothing later can fix a wrong mount. The neural racer commands zero unless the state is ok.

vehicle_state_node

vehicle_state_node is where the two IMUs are fused, with each other and with the wheels. It subscribes to both calibrated IMU topics from imu_bridge_node and to /odom from vesc_to_odom_node, and uses each sensor for a different job:

A gyro counts as usable while its samples are fresh and its mount passed the gravity check. When one gyro drops out, the measured steering and the gyro odometry carry on with the other. The fused speed never switches accelerometers: if the accel_source IMU drops out, /odom/fused is the wheel speed until it returns. With both IMUs gone, /odom/imu is exactly /odom and /steering/measured reads NaN. gap_follow reads /odom/fused with fused_speed:=true and, on car 1, for traction control. It reads /steering/measured for adaptive grip and, on car 1, the understeer check (see IMU features). slam_toolbox uses the gyro odometry with map_course gyro_odom:=true, and record_run records all three outputs.

Fused speed

vehicle_state_node fuses the wheel speed with one IMU’s accelerometer, set by accel_source (bno by default, or vesc). The other IMU’s accelerometer is not used. Car 2’s configuration also names the BNO085, which it does not have yet, so its /odom/fused is exactly the wheel speed until the BNO085 is fitted or accel_source is set to vesc.

What a Kalman filter does

Two sources measure the car’s speed, each with a different weakness. The wheels give the speed directly but are wrong during wheelspin. The accelerometer gives the change in speed, so integrating it gives a speed, but any small bias in it grows into a large error over time. A Kalman filter combines them: it keeps a best estimate and how uncertain that estimate is, lets the accelerometer move the estimate between wheel readings, and lets each wheel reading pull it back, by an amount that depends on how much it trusts each.

vehicle_state_node keeps two numbers: the speed $v$ and the accelerometer’s bias $b$, the offset it reads when it should read zero. It also keeps a 2×2 matrix $P$ whose diagonal holds each one’s variance, the square of its uncertainty.

Predict, at each accelerometer sample $a$, $\Delta t$ seconds apart. The speed changes by the bias-corrected acceleration times the time, and the uncertainty grows, because the accelerometer is noisy and the bias can drift:

$F$ says how the state carries forward: $v$ keeps its value and loses $b\Delta t$, and $b$ stays the same. $Q$ adds the new uncertainty, with ${\sigma }_a$ = 0.15 (sigma_accel) and ${\sigma }_b$ = 0.02 (sigma_bias_rw).

Update, at each wheel reading $z$. The innovation $y=z-v$ is how far the wheels disagree with the prediction. The filter moves the estimate part of the way toward the wheels:

$S$ is the total uncertainty of the disagreement: the prediction’s variance $P_{00}$ plus the wheels’ own, ${\sigma }_w^2$ with ${\sigma }_w$ = 0.08 m/s (sigma_wheel). The gain $K_0=P_{00}/S$ is the fraction of the disagreement the speed accepts. When the prediction is much less certain than the wheels, $K_0$ is near 1 and the wheels win. When it is much more certain, $K_0$ is near 0 and the wheels are mostly ignored. The second entry, $K_1$, lets a persistent disagreement correct the bias too, which is how the filter learns the accelerometer’s offset while driving.

def predict(self, accel, dt):
    cfg = self.cfg
    self.v += (accel - self.b) * dt
    (p00, p01), (p10, p11) = self.P
    # F P F^T + Q, written out for F = [[1, -dt], [0, 1]].
    n00 = p00 - dt * (p01 + p10) + dt * dt * p11 + cfg.sigma_accel ** 2 * dt
    n01 = p01 - dt * p11
    n11 = p11 + cfg.sigma_bias_rw ** 2 * dt
    self.P = [[n00, n01], [n01, n11]]

def update(self, z):
    cfg = self.cfg
    (p00, p01), (p10, p11) = self.P
    s = p00 + cfg.sigma_wheel ** 2
    k0, k1 = p00 / s, p10 / s
    y = z - self.v
    self.v += k0 * y
    self.b += k1 * y
    self.P = [[p00 - k0 * p00, p01 - k0 * p01],
              [p10 - k1 * p00, p11 - k1 * p01]]

The matrix products are written out by hand for the 2×2 case, so the module needs no linear-algebra library.

Rejecting wheelspin

If $y$ is too large to be noise, the wheels are probably spinning or locked, and that reading is skipped. “Too large” is four standard deviations of the expected disagreement (slip_gate):

For a normally distributed disagreement, that happens by chance in fewer than one reading in 15,000. A skip can last at most 0.5 s (max_slip_time). After that, a disagreement that long is more likely a wrong accelerometer than slip, so the filter believes the wheels again and restarts its speed from them.

The hard bound

Whatever the filter believes, the published speed stays within 0.3 m/s of the wheel speed (max_divergence):

The bound limits the damage from a filter that has gone wrong, for example on a wrongly mounted sensor. Without a working accelerometer the output is exactly the wheel speed. In a synthetic test with noisy wheels and a vibrating IMU, the fused speed’s error was 0.37 to 0.46 times the wheels’ own.

The sensor is not at the rear axle

The car’s speed is defined at the rear axle, but the BNO085 sits on the front axle, 0.36 m ahead (bno_x_offset), and the VESC IMU 0.18 m ahead (vesc_x_offset). In a turn, a point a distance $d$ ahead of the turning axle moves on a circle and accelerates toward the circle’s centre. For a body turning at yaw rate $\omega$, that point’s acceleration along the car’s $x$ axis is smaller than the axle’s by ${\omega }^{2}d$, the centripetal term $\mathbf{\omega }\times (\mathbf{\omega }\times \mathbf{r})$. At 1 rad/s that is 0.36 m/s² on the front axle, which the filter would read as braking in every corner. The node adds it back:

def accel_at_base_link(accel_x, yaw_rate, x_offset):
    return accel_x + yaw_rate * yaw_rate * x_offset

/odom/fused is /odom with this speed in place of the wheel speed. gap_follow uses it with fused_speed:=true, on map_course or on its own launch.

Both accelerometers

A second filter, used only with accel_source: both, that reads both accelerometers at once. Its state has four entries, the speed, the acceleration and one bias for each accelerometer, $\mathbf{x}=[v,a,b_{\text{vesc}},b_{\text{bno}}]$. The process noise comes from a white-jerk model, with sigma_jerk (6.0) setting how fast the true acceleration may change, sigma_accel_bno and sigma_accel_vesc (1.0 m/s² each) the accelerometers’ noise, and sigma_accel0 (3.0 m/s²) the starting uncertainty of $a$. Unlike the two-state filter, it integrates neither accelerometer open loop: the wheel speed measures $v$, and each accelerometer measures $a$ plus its own bias. These four values are starting guesses that have not been measured, and the default stays bno, so nothing changes unless the setting is changed.

Measured steering

/steering/measured is the one output that uses both gyros at once: vehicle_state_node averages their yaw rates and divides by the fused speed. A car on a circle of radius $R$ turns at yaw rate $\omega =v/R$, and by the bicycle model its front wheels point at $\tan \delta =L/R$. Eliminating $R$ gives the steering angle from the measured yaw rate and speed:

def steering_from_yaw_rate(yaw_rate, speed, wheelbase, min_speed):
    if not _finite(yaw_rate, speed) or abs(speed) < min_speed:
        return math.nan
    return math.atan(wheelbase * yaw_rate / speed)

This is the steering angle the car achieved, including understeer, slip and servo lag, which the servo command cannot show. Comparing it with the command measures how well the steering follows. It is published on /steering/measured as a JointState named steering. Below 0.3 m/s (min_steering_speed) the division by $v$ amplifies noise too much, so it reads NaN. It also reads NaN when both gyros are usable and their yaw rates steadily disagree by more than 0.15 rad/s, because one of them is wrong and there is no telling which. With only one gyro usable, its yaw rate is used alone. Treat NaN as unknown, never as zero.

Gyro odometry for SLAM

/odom/imu integrates the same steps as /odom. The distance of each step is still the wheels’: vesc_to_odom’s own pose change, projected onto its previous heading. The heading change comes from a gyro instead of the steering model, and unlike the measured steering it is never an average of the two: each step takes it from the first usable source in heading_sources, ['bno', 'vesc']:

1. The BNO085’s gyro, integrated over the step.
2. The VESC’s gyro, if the BNO085 is stale, mounted wrongly or has a gap in the step.
3. vesc_to_odom’s own heading change, if neither gyro is usable or the two disagree. With this fallback the result is exactly /odom.

A gyro step moves along the heading at the middle of the step, not the start. On an arc of length $s$ with heading change $\Delta \psi$, the chord from start to end points along ${\psi }_0+\Delta \psi /2$, so this is exact for a circular arc to second order. A model step moves along its start heading, as vesc_to_odom does, so the fallback reproduces /odom exactly.

distance = (x - x0) * math.cos(yaw0) + (y - y0) * math.sin(yaw0)
found = self.est.heading_change(t_recv, t0, stamp)
if found is None:
    source, dheading = MODEL, wrap_angle(yaw - yaw0)
else:
    source, dheading = found
px, py, pt = self.pose
along = pt if source == MODEL else pt + 0.5 * dheading
self.pose = (px + distance * math.cos(along), py + distance * math.sin(along),
             pt + dheading)

slam_toolbox reads odometry from the transform tree, not from a topic. With publish_odom_tf: true, the node therefore publishes an extra transform, odom_imu to odom, chosen so that odom_imu to base_link equals the gyro pose, while vesc_to_odom keeps publishing odom to base_link. map_course gyro_odom:=true turns that on and moves SLAM to odom_imu together. See Coordinate frames.

The node watches /tf and stops publishing the extra transform for good if anything else gives odom a parent, because a frame with two parents breaks the tree.

Parameters

Set in vehicle_state.yamlGitHub.

Run it

With bringup running:

ros2 launch imu_bridge vehicle_state_launch.py

This starts the BNO085 bridge, imu_bridge_node and vehicle_state_node. Keep the car still for about 3 s after it starts. map_course starts the same chain by default (imu:=true). bno:=false runs the VESC IMU alone. Car 2’s BNO085 mount yaw is not measured yet, so on car 2 trust the yaw rate but not the forward and sideways acceleration; keep fused_speed off there until the mount step of calibrate_imu.py has been redone. A VESC IMU turned off in VESC Tool reads all zeros, and /imu/status then reports fault with an all-zero reason. Both cars’ VESC IMUs were off until October 2026 and now work.

Check it

ros2 topic echo /imu/status --once                 # "state": "ok"
ros2 topic hz /imu/bno/data                        # about 250 Hz on car 1
ros2 topic echo /imu/data --field angular_velocity
ros2 topic echo /steering/measured --field position

Turn the car by hand, slowly, with the wheels on the floor: angular_velocity.z is positive while it turns left. Push the car forward firmly and /imu/bno/data’s linear_acceleration.x jumps positive while y stays within about ±0.3 m/s².

Troubleshooting

Limitations

• Neither car’s BNO085 mount yaw has been checked with a forward push, and car 2’s BNO085 position is nominal. The fused speed and gyro odometry have been built and checked off the car but not yet driven on.
• The VESC IMU’s samples are stamped when they arrive, so each is late by the serial round trip, about 20 ms.
• No orientation is published in car axes.