Outlier rejection and localization jumps

1. Engineering problem

A single bad measurement (a GNSS multipath fix, a wrong loop closure, a wheel-slip velocity) can drag the pose by metres. A gate compares each measurement with the filter’s prediction and its uncertainty, and rejects the improbable ones.

2. When is this needed?

• Pose jumps in the fused estimate.
• When GNSS or LiDAR pose measurements contain glitches.
• After enabling a new sensor.

3. Selection and design decisions

The robot_localization documentation (read 2026-11-06) provides per-sensor rejection thresholds (for example odomN_pose_rejection_threshold, odomN_twist_rejection_threshold, poseN_rejection_threshold, twistN_rejection_threshold, imuN_pose_rejection_threshold, imuN_angular_velocity_rejection_threshold and imuN_linear_acceleration_rejection_threshold), “expressed as Mahalanobis distances, to control how far away from the current vehicle state a sensor measurement is permitted to be”; each defaults to the maximum double value, which means no gating. In one dimension the Mahalanobis distance is |innovation| ÷ the standard deviation of the innovation.

4. Advantages and disadvantages

• Tight gates reject glitches and valid data; after a long outage the prediction is uncertain and gates open up automatically.
• Gating after a long dropout can reject a correct measurement that disagrees with a drifted prediction: allow a reset path.
• Use covariances that reflect reality, or the gate is wrong.

5. Requirements and constraints

• Covariances are consistent so distances are meaningful.
• A recovery path exists when many consecutive measurements are rejected.
• Rejections are logged and counted.

6. Architecture

prediction + covariance ─▶ innovation = z − ẑ ─▶ Mahalanobis distance ─▶ accept (update) | reject (count and log)

7. Implementation

1. Check that covariances are realistic.
2. Set thresholds per sensor from logged data (start near 3).
3. Log every rejection with the distance and the sensor.
4. Look at sequences of rejections: a run means the prediction or the sensor is wrong.
5. Add a reset or relaxation after a run of rejections.

import math
def gate(innovation, s_innov, threshold):
    d = abs(innovation) / s_innov                  # 1-D Mahalanobis distance
    return d, ("accept" if d <= threshold else "reject")
s_innov = math.sqrt(0.02 + 0.04)                   # predicted variance + measurement variance
for z, pred in ((1.02, 1.00), (1.30, 1.00), (3.50, 1.00)):
    d, verdict = gate(z - pred, s_innov, 3.0)
    print(f"measurement {z}, prediction {pred}: Mahalanobis {d:.2f} -> {verdict}")
print("for a Gaussian, 3 sigma keeps", round(math.erf(3 / math.sqrt(2)) * 100, 2), "% of valid measurements")

Printed (checked by the site’s tests):

measurement 1.02, prediction 1.0: Mahalanobis 0.08 -> accept
measurement 1.3, prediction 1.0: Mahalanobis 1.22 -> accept
measurement 3.5, prediction 1.0: Mahalanobis 10.21 -> reject
for a Gaussian, 3 sigma keeps 99.73 % of valid measurements

Reading it: with an innovation standard deviation of about 0.245 m, a 0.02 m innovation has distance 0.08 (accepted), a 0.30 m innovation has distance 1.22 (accepted) and a 2.50 m innovation has distance 10.21 (rejected) with the 3σ threshold; a 3σ gate keeps 99.73 % of valid Gaussian measurements. The numbers are illustrative.

8. Real examples

• GNSS multipath near a building: a position jump of metres is rejected, and the filter continues on odometry and IMU.

9. Failure scenarios

• Gating with an over-optimistic covariance, so valid data is rejected.
• No gate, so one glitch moves the pose.
• A reject run that never recovers after a real change.

10. Troubleshooting

1. Plot distances over time and the threshold.
2. Compare rejected measurements with the truth when available.
3. Check covariance realism if rejections are frequent.

11. Validation

• The example prints the lines shown (executed by the site tests).
• On logs, glitches are rejected and valid measurements are accepted at the expected rate.

12. Engineering checklist

• Covariances realistic
• Thresholds per sensor
• Rejections logged
• Recovery path
• Tested on logs with known glitches

13. Further references

• robot_localization documentation (ros2 branch): state_estimation_nodes.rst and params/ekf.yaml, read 2026-11-06.

Related on FixTheRobot