Fusing Sensors You Cannot Fully Trust

Picture a yard robot with a habit of teleporting. Every afternoon around 3 p.m., its position estimate jumps about four metres toward the warehouse wall, holds for a few seconds, then snaps back. The filter is fine. The code is fine. The sun has moved, and the GPS receiver is now getting a strong reflection off the building's metal cladding.

That is sensor fusion in practice. The maths is settled and elegant. The hard part is that every sensor on the robot is wrong some of the time, in its own particular way, and the filter only knows what you told it about those ways.

Each sensor lies differently

The reason we fuse sensors at all is that their errors are mostly uncorrelated. One drifts slowly, another is noisy but unbiased, another is precise but fails completely in certain conditions. Combine them well and you get an estimate better than any single input.

Sensor	What it is good at	How it fails
IMU (gyro + accelerometer)	High rate (100 Hz to 1 kHz), short-term orientation and angular rate, no external dependency	Bias drift with temperature and time; integrated position diverges within seconds
Wheel odometry	Cheap, high rate, good relative motion on firm ground	Wheel slip on wet floors, gravel, ramps; tyre wear changes effective radius
GNSS / GPS	Absolute position with bounded error outdoors	Multipath near buildings and containers, dropouts under canopies, sudden jumps when satellites change
2D/3D LiDAR	Precise ranges, works in the dark, strong for scan matching	Rain, fog, dust and steam scatter returns; featureless corridors defeat scan matching; glass and mirrors
Camera	Rich features, cheap, good for loop closure and semantics	Lighting changes, motion blur, low texture, lens contamination

None of these rows is exotic. Every one of those failures will happen on a deployed fleet within the first month.

The Kalman filter in one paragraph

R. E. Kalman published "A New Approach to Linear Filtering and Prediction Problems" in the ASME Journal of Basic Engineering in 1960. The idea: keep a best estimate of the state and an estimate of how uncertain you are (the covariance). Each cycle, predict forward using a motion model, which grows the uncertainty. When a measurement arrives, update by blending prediction and measurement, weighted by their relative uncertainties. The blend weight is the Kalman gain.

For linear systems with Gaussian noise, this is the optimal estimator. Robots are not linear, which is where the variants come in.

Here is a complete 1-D filter, estimating a stationary position from noisy readings, with one addition that matters more in the field than in textbooks: an innovation gate.

def kalman_1d(measurements, x0, p0, q, r, gate=9.0):
    """x0/p0: initial estimate and variance. q: process noise. r: measurement noise.
    gate: reject measurements whose squared normalised innovation exceeds this (9.0 = 3 sigma)."""
    x, p = x0, p0
    history = []
    for z in measurements:
        # Predict: constant-position model, uncertainty grows by q.
        p = p + q

        # Innovation: how far is the measurement from what we expected?
        y = z - x
        s = p + r                     # innovation variance
        if y * y / s > gate:          # outlier: skip the update, keep predicting
            history.append((x, p, False))
            continue

        # Update
        k = p / s                     # Kalman gain
        x = x + k * y
        p = (1 - k) * p
        history.append((x, p, True))
    return history

That gate is the line that catches the 3 p.m. teleport. A four-metre jump on a receiver you have told the filter is good to half a metre is wildly improbable, so the filter refuses it. The catch is obvious once you say it: if the gate is too tight and the robot genuinely moves fast, or the estimate has drifted, the filter rejects the truth and diverges. Choosing the gate is a judgment call, not a derivation.

EKF and UKF, intuitively

The Extended Kalman Filter handles nonlinear motion and measurement models by linearising them around the current estimate with a Jacobian. It is cheap and works well when the nonlinearity is mild over one time step, which for a ground robot at 30 Hz it usually is.

The Unscented Kalman Filter, introduced by Julier and Uhlmann in 1997, skips the Jacobian. It pushes a small set of carefully chosen sample points ("sigma points") through the true nonlinear function and recovers mean and covariance from where they land. It copes better with strong nonlinearities, at a modest compute cost.

In practice the choice between them matters less than how you set the noise covariances. Thrun, Burgard and Fox's Probabilistic Robotics (MIT Press, 2005) remains the best single reference for both, and for the particle filters you reach for when the belief is not even roughly Gaussian.

robot_localization in ROS 2

Most ROS 2 ground robots use the robot_localization package rather than hand-rolling a filter. It provides ekf_localization_node, ukf_localization_node, and navsat_transform_node for GPS. Both estimators track a 15-dimensional state (position, orientation, their velocities, and linear acceleration) and accept nav_msgs/Odometry, sensor_msgs/Imu, and pose or twist messages with covariance. You choose, per sensor, which state variables each input is allowed to touch. The design is described in Moore and Stouch's IAS-13 paper (2014).

Two pieces of its documentation are worth taping to the monitor.

First, REP 105 defines the frames. The odom frame is continuous but drifts without bound. The map frame does not drift but may jump. Local planners need continuity; global planners need drift-free positions.

Second, the package docs warn that a position estimate that includes GPS is likely unfit for navigation modules because GPS jumps. Their suggested pattern is two filter instances: one fusing only continuous sources (wheel odometry, IMU) into odom, and a second fusing everything, including GPS, into map.

That one design decision prevents a whole class of "robot swerves for no reason" bugs.

Time and space: the boring failures

Most fusion bugs are not filter bugs. They are bookkeeping bugs.

Time synchronisation

A LiDAR scan timestamped when the driver published it rather than when the laser fired can be 50 to 100 ms late. At 2 m/s that is 10 to 20 cm of error, injected confidently into your filter. Use hardware triggering or PTP where you can, and check that every driver stamps acquisition time. robot_localization can handle lagged data (smooth_lagged_data with a history_length window), but only if the timestamps are honest.

Extrinsic calibration

The filter needs to know where each sensor sits relative to base_link. An IMU mounted 2 degrees off level will read a constant gravity component as forward acceleration. A LiDAR whose yaw is off by 1 degree will place a wall 10 m away about 17 cm to the side. Measure extrinsics after every mechanical change, not just at build time, and verify them with a known target.

The failure modes that need a human

Here are the situations where experienced engineers earn their salary.

GPS multipath near structures. The receiver still reports a fix, often with an optimistic covariance. The filter has no way to know the satellite geometry is lying unless you inflate covariance near known reflective structures or gate aggressively. Someone has to walk the site and know where those structures are.

IMU bias drift. Gyro bias changes with temperature. A robot that leaves a 20 °C warehouse for a 2 °C loading dock will see its heading slowly rotate. Estimating bias as a state helps; so does a policy of re-zeroing at known stationary moments.

LiDAR in rain, fog and dust. Kutila et al. (2018) tested two 905 nm automotive LiDARs in a fog chamber and saw about a 50% reduction in target detection performance as visibility dropped. In a cement plant or a foundry, airborne dust does the same thing every day. Scan matching degrades quietly, not loudly.

Wheel slip. Odometry reports confident forward motion while the robot is spinning its wheels on a wet ramp. Cross-checking odometry against IMU acceleration catches some of this.

Where judgment lives

A filter is a statement of belief about your sensors, written as covariance matrices. The algorithm will faithfully act on whatever you wrote, including the parts you guessed.

Deciding that this GPS cannot be trusted within 15 m of the north wall, that the LiDAR should be down-weighted when the dust extractor is off, or that a robot with a recently replaced wheel needs its odometry covariance raised until someone recalibrates: those are judgments that come from standing on site and watching the data. Better AI will help propose those numbers. Someone still has to look at a teleporting robot at 3 p.m. and ask what the sun is doing.

Sources

1. A New Approach to Linear Filtering and Prediction Problems (R. E. Kalman, 1960) — the original Kalman filter paper, ASME Journal of Basic Engineering 82(1):35–45.
2. Probabilistic Robotics (Thrun, Burgard, Fox; MIT Press, 2005) — standard reference on Kalman, EKF, UKF and particle filters for robots.
3. A New Extension of the Kalman Filter to Nonlinear Systems (Julier & Uhlmann, 1997) — the unscented transform and UKF, SPIE vol. 3068.
4. robot_localization documentation — EKF/UKF/navsat nodes, 15-D state, supported message types, Moore & Stouch (IAS-13, 2014) citation.
5. robot_localization: Integrating GPS Data — warning about GPS jumps and the two-instance odom/map pattern.
6. REP 105: Coordinate Frames for Mobile Platforms — odom is continuous but drifts; map is drift-free but can jump.
7. Automotive LiDAR performance verification in fog and rain (Kutila et al., IEEE ITSC 2018) — about 50% reduction in detection performance for 905 nm LiDARs in fog.