Sensor and localization failure detection

1. Engineering problem

Failures rarely announce themselves. A LiDAR may keep publishing slowly, a localiser may keep publishing a pose with a growing covariance, or a relocalisation may move the pose by metres. A monitor that checks staleness, rate, uncertainty and jumps turns these into explicit states the rest of the system can react to.

2. When is this needed?

• Designing fault handling.
• When failures are noticed only after a collision.
• Specifying diagnostics.

3. Selection and design decisions

Signals: staleness (age of the last message above a timeout), rate (below the expected rate), uncertainty (covariance above a limit), jump (a change between updates larger than the motion allows). ROS 2 gives built-in support for part of this: the deadline and liveliness QoS policies raise events when messages are late or a publisher stops asserting liveliness (QoS documentation read 2026-11-06). The diagnostics packages publish and aggregate statuses (diagnostic_updater, diagnostic_aggregator; README read 2026-11-06).

4. Advantages and disadvantages

• Tight thresholds catch faults early and cause false alarms.
• Debounce (require the condition for some time) reduces flicker and adds delay.
• Local checks in each node versus a central monitor: the central one sees combinations.

5. Requirements and constraints

• Every safety-relevant sensor has staleness, rate and plausibility checks.
• Thresholds come from healthy logs.
• Each error level has a defined response (slow, stop, alert).

6. Architecture

sensor/localiser outputs ─▶ monitors (stale, rate, covariance, jump) ─▶ status OK/WARN/ERROR ─▶ diagnostics ─▶ behaviour (slow, stop, alert)

7. Implementation

1. List the signals each consumer needs.
2. Measure healthy ranges for rate and covariance.
3. Implement the checks (below) with debounce.
4. Map each level to a response.
5. Test by disconnecting and degrading each sensor.

def health(now, last_stamp, last_rates, cov_trace, jump_m, timeout=0.5, min_rate=8.0, cov_limit=0.25, jump_limit=0.5):
    out = []
    if now - last_stamp > timeout: out.append(f"STALE: no data for {now - last_stamp:.2f} s")
    rate = sum(last_rates) / len(last_rates)
    if rate < min_rate: out.append(f"RATE: {rate:.1f} Hz below {min_rate} Hz")
    if cov_trace > cov_limit: out.append(f"COVARIANCE: trace {cov_trace:.2f} above {cov_limit}")
    if jump_m > jump_limit: out.append(f"JUMP: pose moved {jump_m:.2f} m in one update")
    return ("OK" if not out else ("ERROR" if any(o.startswith(("STALE", "JUMP")) for o in out) else "WARN"), out)
cases = {"healthy": (10.0, 9.9, [10, 10, 9.8], 0.05, 0.02), "slow lidar": (10.0, 9.95, [6, 7, 6.5], 0.06, 0.02), "uncertain pose": (10.0, 9.95, [10, 10, 10], 0.40, 0.03), "dead sensor": (10.0, 9.0, [10, 10, 0], 0.05, 0.02), "relocalisation jump": (10.0, 9.95, [10, 10, 10], 0.05, 1.8)}
for name, c in cases.items():
    status, why = health(*c)
    print(f"{name:20s} {status:5s} {why}")

Printed (checked by the site’s tests):

healthy              OK    []
slow lidar           WARN  ['RATE: 6.5 Hz below 8.0 Hz']
uncertain pose       WARN  ['COVARIANCE: trace 0.40 above 0.25']
dead sensor          ERROR ['STALE: no data for 1.00 s', 'RATE: 6.7 Hz below 8.0 Hz']
relocalisation jump  ERROR ['JUMP: pose moved 1.80 m in one update']

Reading it: a healthy case is OK; a lidar at 6.5 Hz is a warning; a pose covariance trace of 0.40 against a 0.25 limit is a warning; a sensor silent for 1.0 s is an error (also flagged for its low rate); and a 1.8 m pose jump is an error. Thresholds are illustrative.

8. Real examples

• AMR: lidar stale for 0.5 s stops the robot; covariance above the limit reduces the speed limit.

9. Failure scenarios

• Only checking that a topic exists, not that it is fresh.
• Alarms with no defined response.
• Thresholds copied from another robot.

10. Troubleshooting

1. Plot the signals against failures in logs.
2. Check for flicker and add debounce.
3. Check each response in a test.

11. Validation

• The monitor prints the lines shown (executed by the site tests).
• Each injected fault is detected within the required time and produces the defined response.

12. Engineering checklist

• Staleness, rate, uncertainty, jump checks
• Thresholds from healthy data
• Debounce
• Responses defined
• Fault injection tested

13. Further references

• ROS 2 Jazzy, About Quality of Service settings: docs.ros.org, read 2026-11-06 (deadline and liveliness policies and their QoS events).
• ROS diagnostics README: ros/diagnostics, read 2026-11-06 (diagnostic_updater publishes, diagnostic_aggregator aggregates, rqt_robot_monitor visualises).

Related on FixTheRobot