Fuse wheel odometry, IMU and visual odometry with an EKF

You have a differential drive robot whose encoders and IMU already feed a filter, with a lidar drawing a clean 2D map through SLAM Toolbox. The next sensor, a depth camera, promises visual odometry and a 3D map, and suddenly three sources claim the pose while the controller can believe only one. This guide gives you the extended Kalman filter set-up that settles it: the update-rate budget for each sensor, the states each one may claim, the frame and timestamp rules that keep the pose from jumping, and the three drives that prove the fusion works. The numbers assume a 0.5 m/s indoor robot on ROS 2; double the speed and every inter-update gap here doubles with it.

Why fuse wheel odometry, IMU and visual odometry instead of running one alone?

Fuse the three because each fails alone: wheel odometry drifts 1-2% of distance traveled even before one wheel slips, an integrated gyro heading wanders by degrees per minute, and visual odometry drops out entirely on glass, glare and textureless floor.

Wheel odometry is smooth and fast, but its error only grows: a 1-2% speed-scale error on a clean floor becomes 0.16-0.32 m of drift over a 16 m loop, and one slipped wheel adds more in an instant. A gyro tracks yaw rate tightly at 50 Hz or faster, yet integrating that rate into a heading accumulates degrees of drift per minute, while a magnetometer heading bends by tens of degrees near drive motors and steel frames. Visual odometry from a depth camera returns local absolute position at 15-30 Hz but drops out on glass, glare and featureless floor. The filter weights all three by their declared uncertainty, so each one carries the pose exactly where the others are weakest.

The same weighting logic applies to feature-based visual odometry, and the sparse-feature sensor-fusion validation guide shows three checks for trusting it. For what a depth camera contributes to mapping as opposed to pose, the visual odometry versus depth mapping comparison separates the two jobs.

What update rate should each sensor run at?

Feed encoders at 100 Hz or faster, the IMU at 50 Hz or more and visual odometry at 15-30 Hz, and set the filter to output 30-50 Hz, so the pose never waits on the slowest sensor.

Every hertz in that budget buys a shorter dead-reckoning gap, because between two updates the filter coasts on the last accepted velocity. At 0.5 m/s the coasting distance per update is:

Rotation follows the same rule: during a 90°/s spin, a 50 Hz IMU lets the heading move 1.8° between updates, and doubling the rate halves that to 0.9°. That is the whole argument for the 50 Hz line.

Counting is never the bottleneck on the wheel side. A 500 PPR quadrature encoder yields 2,000 counts per motor revolution; through a 30:1 gearbox onto a 100 mm wheel, 314 mm of travel per turn, that is 60,000 counts per wheel revolution, so 0.5 m/s produces about 95,000 counts per second and a 100 Hz loop receives roughly 950 counts per cycle.

Visual odometry cannot outrun the camera: consumer depth cameras stream at about 30 fps, so 15-30 Hz is the honest ceiling. If publishing stalls before the sensor does, the ROS 2 image publishing frame-rate guide locates the bottleneck.

Which states should each sensor feed the filter?

Assign each sensor the states it measures best: wheel odometry supplies body velocity, the IMU supplies angular rate and roll and pitch, and visual odometry alone supplies absolute position, because two inputs claiming the same absolute state fight and the pose oscillates.

The common ROS 2 filter keeps a state vector of 15 values: position, orientation, linear velocity, angular velocity and linear acceleration. Each input gets its own configuration entry where you mark the variables that input supplies and leave the rest false. Two assignments do most of the work on a differential drive. Only visual odometry claims absolute position; the wheels supply forward velocity and yaw rate, which the filter integrates just across the 16.7 mm and 33 ms a visual update arrives late. And the planar clamp stays on, so roll, pitch and height stay pinned at zero instead of absorbing IMU noise.

Yaw needs one owner. Gyro rate plus visual odometry works well indoors; an absolute magnetometer heading fights the drive motors' field and can fight the visual yaw too. If you do feed an absolute IMU heading, it must be tilt-compensated first — this tilt-compensated IMU heading walkthrough shows that calculation end to end.

How do I wire a depth camera into the filter as visual odometry?

Run a visual odometry node on the depth stream, silence its own transform with publish_tf set to false, and feed its stamped pose to the filter, which becomes the only publisher of the odometry-to-base transform.

The wiring is six steps, and step 5 is the one people skip:

1. Publish depth and color with timestamps set at capture, and confirm the tracker pairs frames by timestamp or sequence number; when color trails depth by a frame, the color-lags-depth pairing fix shows the symptom and the correction.
2. Write the static transform from the robot's base frame to the camera: a translation in metres plus the rotation that maps robot-forward into the camera's own axes. A wrong sign here makes visual odometry report sideways motion while the wheels say forward.
3. Start the visual odometry node on the depth stream at 15-30 Hz. In the RTAB-Map odometry nodes the documented switch is publish_tf — set it to false so the mapper keeps only its map-frame corrections and publishes no transform of its own.
4. Feed the stamped visual pose to the filter as its position input, beside your existing wheel odometry and IMU entries, with a covariance that tells the truth about the tracker's scatter.
5. Make the EKF the only publisher of the odometry-to-base transform. A second publisher left running from an earlier configuration makes the pose alternate between two opinions every cycle.
6. Drive or hand-carry the robot 2 m and plot wheels, IMU, visual and fused poses together. The fused curve should be the smooth one; an oscillation at the visual rate points back to step 2.

One rate trap remains: a depth stream sharing a hub with other cameras can lose frames silently, and a visual odometry rate that sags to 15 Hz doubles the coasting gap to 33 mm. The USB hub bandwidth budget shows how to count the stream budget before adding hardware.

What frames and timestamps have to line up?

Put every input on the robot's own clock and give each sensor a correct static mount transform, because a timestamp from arrival time or one wrong camera axis makes the filter swallow pose spikes that look exactly like real motion.

The transform tree has three owners and no fourth: the SLAM package publishes the map-to-odometry correction, the EKF publishes odometry-to-base, and static publishers fix each sensor mount to the base. The mount transforms are where visual odometry dies quietly, because the camera's own axes point differently from the robot's, and one swapped axis turns forward motion into lateral drift that the filter faithfully averages.

Timestamps must carry capture time, not arrival time. An IMU sample stamped when the driver received it lags by the transport delay, and the filter reads that as a yaw that happened late; the frozen or jumping timestamp fix covers the clock-source faults behind most of these symptoms. Pairing IMU orientation with camera time is its own exercise — the IMU-camera time synchronization guide covers the clock arithmetic — and two depth cameras on one robot raise the stakes again, which the multi-camera sync and validation guide handles.

The four mistakes worth checking first:

• two publishers on the odometry-to-base transform, so the pose alternates every cycle;
• IMU stamped at arrival instead of sample time, so yaw lags by the transport delay;
• visual odometry and integrated wheels both claiming absolute position, so the pose oscillates;
• a visual rate left at 10 Hz with full weight, which at 0.5 m/s hides 50 mm of motion per gap.

Is an EKF overkill for a differential drive robot?

Keep a complementary filter only while the robot navigates 2D on lidar alone; the moment a depth camera feeds visual odometry and a 3D map, an EKF earns its keep because three asynchronous sources with different uncertainties need per-source weighting.

A complementary filter blends encoder velocity with gyro rate, costs almost nothing, and SLAM Toolbox corrects its drift at every scan. The depth camera changes the problem: the sources disagree on different timescales — 100 Hz wheels, 50 Hz gyro, 30 Hz vision — and only a filter that holds a covariance per source stays smooth when one of them is late or wrong. On ROS 2 the standard package for that filter is robot_localization; its configuration takes the odometry, inertial and pose inputs described above, and the same blend done at microcontroller level is worked through in this encoder and IMU odometry correction walkthrough.

The shortcut — let RTAB-Map run its own internal odometry and skip the fuse — builds a working 3D map but leaves navigation and mapping on two different poses, and a visual dropout then hits the map directly; the RGB-D odometry dropout fix shows what that failure looks like.

Which depth camera works for visual odometry on a bare floor?

Choose an active depth camera, ToF or projected structured light, for mobile visual odometry, because both illuminate the floor themselves and keep tracking on matte, textureless surfaces where a passive stereo pair finds no features at all.

Active depth keeps tracking through dim aisles and texture-free concrete because the camera supplies its own illumination. The limits are optical: matte black surfaces absorb the infrared pattern, glossy floors mirror it into false returns, and direct sunlight saturates the sensor, which is why outdoor routes want shade or a different ranging principle altogether.

Mounting geometry decides how much floor the camera sees. A depth camera with a 60° vertical field, mounted level at 0.50 m, first meets the floor at 0.50 m / tan(30°) ≈ 0.87 m ahead; tilting the same camera 10° down moves that nearest point in to 0.50 m / tan(40°) ≈ 0.60 m. Keep the nearest visible floor ahead of the drive wheels — the pose that matters is where the robot is about to be.

For long indoor runs, that makes a ToF sensing camera the default pick. The MV-DT01WMU is a ToF sensing camera that fits this role.

At shorter indoor ranges, a projected pattern does the same job with a dense depth map. The MV-DPS200P-02 is a projected structured-light 3D camera, the close-range pattern-projection approach.

Laser profilers and line-scan 2.5D systems in the same 3D sensor category belong on conveyors, where the part moves past a fixed line; a mobile base wants a frame camera that watches a volume ahead of it.

How do I prove the fused pose is better?

Run three drives and judge return-to-start error: a 10 m out-and-back, a 360° spin in place and a 16 m square loop, with fused error inside 0.2 m on the loop while a 2% wheel scale error alone puts the endpoint 0.32 m off.

Record while you drive: wheels, IMU, visual odometry and fused output together, so any anomaly replays offline — the H5 recording workflow keeps depth and poses in one file. Then judge three endpoints against their starts:

• 10 m straight out-and-back: fused endpoint within 0.10 m.
• 360° spin in place: final yaw within 2°.
• 16 m square loop: fused endpoint within 0.2 m.

The 0.2 m target leaves real margin. A 32 s loop at 30 Hz holds 960 visual updates, and with a typical 1 mm per-frame error a random walk accumulates 1 mm × √960 ≈ 31 mm — an estimate that ignores unmodeled slip, but it sits well inside target. Wheel-only odometry fails the same test by scale: a 2% speed error over 16 m puts the endpoint 0.32 m off before a single slip. For a deeper accuracy method, the stereo reconstruction validation tests transfer directly to depth-camera odometry.

Pick the depth camera first, because its frame rate and field set the budget everything else obeys. The 3D camera finder filters by type and rate, the vision system configurator walks the rest of the chain, and the 3D topic page collects the worked guides around this one. Send floor type, speed and mounting height through the contact page and we will shortlist a camera and a rate budget for your base.