ProjekinAja. Robotics Stage 4 · State Estimation Lesson 4.9
● Stage 4 · Lesson 4.9

Multi-Sensor Fusion on FollowBot: Architecture and Covariance Design

The multi-sensor EKF is only as good as its weakest covariance matrix — and most practitioners set those covariances by guessing.

⏱ ~28 min read 🧠 Architecture 🤖 FollowBot milestone

🎯 By the end you'll understand

How to design the EKF state vector for FollowBot with correct physical dimensions. How the differential-drive process model is linearized into the F Jacobian. How to write observation models for wheel odometry, IMU angular rate, and LIDAR scan-matching — with their R covariance matrices set from first principles, not guessing. How asynchronous sensor updates are handled correctly without time-ordering assumptions. How chi-squared gating filters outliers before they corrupt your filter. The dual-EKF architecture that separates robot localization from person tracking. And the silent failure mode that costs you days: wrong frame IDs silently corrupting EKF inputs.

KEY INSIGHTCovariance is not a knob you tune — it is a physical claim

When you set R = diag(0.01, 0.01, 0.005) for your LIDAR observation model, you are not choosing a filter gain. You are asserting that your scan-matcher makes errors of roughly 10 cm (1σ) in position and 4° in heading. If that assertion is wrong by a factor of 10, your filter will either ignore LIDAR updates as too uncertain or, worse, trust them too much and diverge from reality.

Multi-sensor fusion is usually taught as "run the EKF predict step, then call the update step for each sensor." That is structurally correct but operationally incomplete. The filter equations are just matrix algebra — they produce the right answer only if the covariance matrices Q (process noise) and R (measurement noise) reflect the actual noise statistics of your system. This lesson builds FollowBot's full multi-sensor EKF from the ground up, deriving each matrix from physical first principles rather than tuning by feel.

🔑 Hold onto this

A covariance matrix is a probability statement about your sensor. R for your wheel odometry encodes how much the encoder ticks deviate from true wheel rotation. Q for your process model encodes how much the robot's motion deviates from the kinematic model prediction over one time step. Both are empirically measurable from logged data — and both should be verified using filter consistency tests (NEES, NIS) on recorded sequences, not just visually inspected in RViz. Covariance design is an engineering task with a ground truth, not a hyperparameter search.

CORE CONCEPTDesigning the EKF state vector for FollowBot

Before writing a single matrix, you must decide what your filter estimates. Every state variable has a cost: it adds a row and column to every covariance matrix and a term to every Jacobian. Add only what the sensors can observe and what the planner consumes. For FollowBot's localization EKF:

📖 FollowBot localization state vector

x = [x, y, θ, v, ω]ᵀ — a 5-element column vector. Here x and y are the robot's position in the 2D map frame (meters), θ is the heading (radians, measured from the x-axis), v is the linear velocity (m/s), and ω is the angular rate (rad/s). The choice to include v and ω is deliberate: they are directly observed by the IMU and wheel encoders, and including them makes the prediction step more accurate when sensor updates are sparse.

Why not include accelerations? FollowBot's IMU provides angular rate directly (gyroscope); linear acceleration is integrated into velocity by a separate IMU preintegration layer (Lesson 4.8) before entering the localization EKF. Adding raw accelerations to the state vector would require modeling IMU biases as additional state dimensions — appropriate for a full INS but unnecessary for a 2D ground robot where gravity is constant and the accelerometer data is dominated by centripetal terms. Keep the state minimal until the filter diverges and you understand why.

The initial state covariance P₀ encodes your uncertainty about the robot's starting pose. If the robot is placed at a known location in the map:

ekf_init.pystate vector + P₀ initialization
import numpy as np

# State: [x, y, theta, v, omega]
x0 = np.array([0.0, 0.0, 0.0, 0.0, 0.0])

# P0: initial covariance — reflect true starting uncertainty
# Position known to ~5 cm (1σ); heading known to ~2° (1σ)
# Velocity starts at zero — known exactly
P0 = np.diag([
    0.05**2,   # x: (5 cm)²
    0.05**2,   # y: (5 cm)²
    np.deg2rad(2.0)**2,  # theta: (2°)²
    0.01**2,   # v: (1 cm/s)² — essentially known
    np.deg2rad(0.5)**2   # omega: (0.5°/s)²
])
# Do NOT initialize P0 = zeros — the filter can never
# grow uncertainty from zero without being told by Q
⚠ Never initialize P₀ = 0

A zero initial covariance tells the filter: "I know the starting state perfectly." The Kalman gain for subsequent measurements will be computed relative to that claim, and the filter will initially ignore all sensor corrections. P₀ must reflect genuine initial uncertainty — even if you know the starting position well, a few centimeters of position uncertainty and a degree of heading uncertainty are physically honest and numerically important.

HOW IT WORKSProcess model, observation models, and their Jacobians

Process model. The differential-drive kinematic model predicts how the state evolves over one time step dt given control inputs (v_cmd, ω_cmd). The nonlinear prediction function f(x, u) is:

Prediction x̂_{k+1|k} = f(x̂_k, u_k) x_{k+1} = x_k + v·cos(θ)·dt y_{k+1} = y_k + v·sin(θ)·dt θ_{k+1} = θ_k + ω·dt v_{k+1} = v_k (first-order hold) ω_{k+1} = ω_k (first-order hold) Jacobian F = ∂f/∂x (5×5) [1 0 −v·sin(θ)·dt cos(θ)·dt 0 ] [0 1 v·cos(θ)·dt sin(θ)·dt 0 ] F = [0 0 1 0 dt ] [0 0 0 1 0 ] [0 0 0 0 1 ] evaluated at current (v, θ) — recomputed every step
The EKF predict step for FollowBot's 5-state localization EKF. The process noise Q is added in the covariance predict: P = F·P·Fᵀ + Q. Note that F must be recomputed at every time step because it depends on the current estimated (v, θ).

The process noise Q encodes how much the robot deviates from this ideal kinematic model. For FollowBot running at up to 1.2 m/s with dt = 0.05 s (20 Hz prediction rate), empirically validated values are:

ekf_noise.pyprocess noise Q — set from physical understanding
# Q: process noise covariance — models unmodeled accelerations,
# slip, and control latency over dt = 0.05 s
Q = np.diag([
    0.005**2,              # x: slip ~5mm per step
    0.005**2,              # y: slip ~5mm per step
    np.deg2rad(0.1)**2,   # theta: ~0.1°/step heading error
    0.02**2,               # v: ~2 cm/s per step velocity noise
    np.deg2rad(0.3)**2    # omega: ~0.3°/s per step rate noise
])
# These numbers come from logged encoder data vs. ground truth,
# NOT from intuition. Verify using NEES consistency test.

Observation models. Each sensor provides a different measurement vector z and maps from state to measurement via a nonlinear h(x) with Jacobian H = ∂h/∂x. For FollowBot's three sensors:

Sensor Measurement z h(x) mapping R matrix (diagonal) Rate
Wheel odometry [v_odom, ω_odom] h = [v, ω] — extracts velocity states directly diag(0.01², 0.005²) 100 Hz
IMU (gyroscope) [ω_gyro] h = [ω] — scalar angular rate diag((0.003 rad/s)²) 200 Hz
LIDAR scan-match [x_scan, y_scan, θ_scan] h = [x, y, θ] — direct pose observation diag(0.05², 0.05², 0.02²) 10 Hz

The scan-matching R entries deserve explanation. A well-tuned ICP or NDT matcher on FollowBot's RPLIDAR A2 in a structured indoor environment achieves roughly 5 cm position error and 1° heading error (1σ). Setting R = diag(0.05², 0.05², (0.017 rad)²) quantifies this claim. In open or featureless corridors — long straight walls — scan-matching heading uncertainty grows substantially; an adaptive R that inflates the θ variance in low-feature environments is worth implementing once the baseline works.

CORE CONCEPTHandling asynchronous measurements correctly

FollowBot's three sensors publish at three different rates: wheel odometry at 100 Hz, IMU at 200 Hz, LIDAR at 10 Hz. The EKF must handle these without assuming any ordering or simultaneity. The standard approach for a ROS 2 implementation is the time-stamped update pattern: each incoming measurement carries a header.stamp, and the EKF update for that measurement is applied using the state propagated to exactly that timestamp.

In practice this means the EKF node maintains a circular buffer of recent state predictions. When a LIDAR scan-match arrives at time t_L, the node:

1

Identify the nearest predict snapshot

Find the buffered state x̂(t) and covariance P(t) closest to t_L from below (the predict step that immediately preceded this measurement). Never extrapolate forward from a future predict — that violates causality.

2

Propagate to measurement time

If the stored snapshot is at t_prev and the measurement is at t_L, propagate the EKF from t_prev to t_L using the predict equations with dt = t_L - t_prev. This produces x̂(t_L) and P(t_L).

3

Apply the measurement update

Compute the innovation z - h(x̂), Kalman gain K, and update x̂ and P. This corrected state is now the starting point for all subsequent predict steps that were buffered after t_L (re-propagation, sometimes called "smoothing-back"). In production implementations with fast sensors, the re-propagation is typically skipped for simplicity — the LIDAR update is applied to the current state with a short extrapolation, which introduces a small but bounded error.

4

Validate the header.stamp — always

Before storing any measurement in the buffer, verify that its header.frame_id matches the frame the observation model expects. A LIDAR pose in laser frame fed to an EKF expecting map frame measurements is the silent corruption failure described in the lesson brief. The validator should log an error and drop the measurement if the frame is wrong — not silently transform or ignore the mismatch.

💡 Use robot_localization, but understand what it does

The ROS 2 robot_localization package implements exactly this time-stamped asynchronous EKF. It is well-tested and handles most of the above details correctly. However, its covariance parameters — the diagonal elements of each sensor's R matrix, passed in the YAML configuration — are still your responsibility. The package cannot deduce what your sensor's noise properties are. Using the defaults from the documentation without measurement is one of the most common sources of poor EKF performance in practice.

HOW IT WORKSChi-squared outlier gating

Even a correctly configured EKF is vulnerable to occasional large measurement errors: a scan-matcher that locks onto a person's legs instead of walls, an encoder that skips ticks during wheel slip, an IMU spike from a cable bump. Without outlier rejection, a single bad measurement can corrupt the state estimate for seconds. Chi-squared gating is the standard defense.

The innovation v_k = z_k - h(x̂_{k|k-1}) is the difference between the actual measurement and the predicted measurement. Under the filter's own noise assumptions, this innovation should be zero-mean Gaussian with covariance S_k = H·P·Hᵀ + R. The normalized squared innovation:

📖 Chi-squared gate

γ = vᵀ S⁻¹ v — the Mahalanobis distance squared. If the measurement is consistent with the filter's noise model, γ is chi-squared distributed with m degrees of freedom, where m is the measurement dimension. For a 3D LIDAR pose update (m=3), the 99.9% gate threshold is χ²(3, 0.999) ≈ 16.3. Any measurement with γ > 16.3 is rejected as an outlier. This is not discarding data carelessly — it is enforcing the statistical claim implicit in your R matrix.

chi2_gate.pyinnovation gating — called before every EKF update
from scipy.stats import chi2

# Gate threshold for m-dimensional measurement at 99.9% confidence
def gate_threshold(m, confidence=0.999):
    return chi2.ppf(confidence, df=m)

def chi2_gate(innovation, S, threshold):
    # innovation: (m,) array; S: (m,m) innovation covariance
    gamma = float(innovation @ np.linalg.solve(S, innovation))
    if gamma > threshold:
        return False, gamma  # REJECT
    return True, gamma    # ACCEPT

# In the LIDAR update callback:
# S = H @ P @ H.T + R_lidar
# v = z_lidar - h(x_hat)  # innovation
# accepted, gamma = chi2_gate(v, S, gate_threshold(3))
# if accepted: apply EKF update; else: log rejection + skip
⚠ Gating too aggressively causes filter starvation

If your R matrix underestimates sensor noise (too small), the gate threshold in normalized measurement space is effectively tighter than your data supports, and legitimate measurements will be rejected. The filter covariance will grow unchecked because it never receives corrections. Monitor the accept/reject ratio in flight: if more than 5% of LIDAR updates are being gated, your R matrix is likely wrong — not your sensor. This is diagnosed by reducing R's diagonal values and observing whether the rejection rate drops.

SEE ITFusion debugger: figure-eight trajectory

FollowBot is driving a figure-eight at 0.6 m/s. Three sensors are fusing their measurements into the EKF every predict cycle. Toggle sensors on and off, inject a LIDAR outlier, and observe how the filter covariance ellipse grows and shrinks. The worst choice — disabling LIDAR entirely — reveals how quickly odometry-only dead-reckoning becomes unreliable even on a short trajectory.

⚙ Interactive · Multi-sensor EKF Fusion Debugger

Toggle sensors. Inject a LIDAR outlier to watch chi-squared gating. Observe which sensor dominates the covariance ellipse in each trajectory phase.

Cycle0 Pos uncertainty σ– Heading σ–
Active sensorsOdom + IMU + LIDAR Last LIDAR gate– Dominant sensor–
Gate log will appear here…

Notice the asymmetry: LIDAR suppresses the covariance ellipse dramatically — even though it only updates at 10 Hz — because it provides direct pose observations with relatively small uncertainty. When you mask LIDAR, the ellipse grows along the heading direction first, because heading error compounds into position error through the kinematic integration. IMU stabilizes heading but cannot correct position. This three-way interaction is what makes covariance design non-trivial: the dominant sensor shifts depending on the phase of the trajectory.

IN PRACTICEMeasuring covariances rather than guessing them

The right way to set R for wheel odometry is to log 100 straight-line runs of known distance on the deployment surface, compute the deviation of odometry-integrated velocity from measured ground-truth velocity, and fit a Gaussian. That variance is your R diagonal entry. The same procedure applies to LIDAR scan-matching: log scan-matcher outputs against a motion-capture system or high-quality external reference, compute the position and heading errors, and fit the covariance.

For Q, a slightly different approach: log the difference between IMU-integrated pose changes and encoder-integrated pose changes over many short intervals. The residual captures the combined effect of wheel slip, control latency, and kinematic model error. Fit that distribution to get Q.

If ground-truth instrumentation is unavailable, the NEES test (Normalized Estimation Error Squared, introduced in Lesson 4.3) provides an indirect check. On a recorded sequence where you have a good reference (another high-quality sensor or a map-locked position), compute:

📖 NEES filter consistency check

NEES_k = ẽᵀ_k P⁻¹_k ẽ_k where ẽ_k = x_true_k − x̂_k is the state error. If the filter is consistent, NEES should be chi-squared distributed with n degrees of freedom (n=5 for our state), so the expected value is 5.0. NEES >> 5 means the filter is overconfident (Q, R too small). NEES << 5 means the filter is too uncertain (Q, R too large). Running this test on a 30-minute FollowBot session and checking that NEES stays in the 95% chi-squared confidence band [chi2.ppf(0.025, n), chi2.ppf(0.975, n)] = [0.83, 12.83] for n=5 is the validation gating step before declaring the EKF ready for field use. (As a rule of thumb, [n×0.5, n×2] = [2.5, 10] is a practical informal check, but the statistical threshold comes from the chi-squared CDF.)

nees_check.pyNEES consistency validation on recorded bag
import numpy as np
from scipy.stats import chi2

def compute_nees(x_true_seq, x_hat_seq, P_seq):
    # x_true_seq: (N, 5); x_hat_seq: (N, 5); P_seq: (N, 5, 5)
    n = x_true_seq.shape[1]
    nees_vals = []
    for i in range(len(x_true_seq)):
        e = x_true_seq[i] - x_hat_seq[i]
        # wrap heading error to [-pi, pi]
        e[2] = (e[2] + np.pi) % (2*np.pi) - np.pi
        P = P_seq[i]
        nees = float(e @ np.linalg.solve(P, e))
        nees_vals.append(nees)
    avg_nees = np.mean(nees_vals)
    lo = chi2.ppf(0.025, df=n)
    hi = chi2.ppf(0.975, df=n)
    consistent = lo < avg_nees < hi
    return avg_nees, consistent, (lo, hi)

# Expect avg_nees ≈ 5.0 for a 5-state filter
# avg_nees = 18.3 → Q and/or R are too small → filter overconfident
# avg_nees = 1.1  → Q and/or R are too large → filter too conservative

HOW IT WORKSThe dual-EKF architecture for FollowBot

FollowBot needs two distinct estimation problems solved simultaneously: where is the robot (localization), and where is the tracked person (tracking). These are typically solved by two separate EKF instances rather than one large joint state vector — for three reasons:

1

Different update rates and sensors

The localization EKF fuses odometry (100 Hz), IMU (200 Hz), and LIDAR scan-matching (10 Hz). The tracking EKF fuses LIDAR person cluster position (10 Hz) and RGB-D skeleton detections (30 Hz). Combining them into one 8+ state filter couples their covariances unnecessarily and makes Q/R tuning much harder.

2

The person model is independent of the robot kinematic model

The localization EKF uses a differential-drive process model. The person tracking EKF uses a constant-velocity or constant-acceleration model for the tracked person. Mixing these into one prediction function is mathematically awkward and physically incorrect.

3

Fault isolation

If the person tracking EKF diverges (the person runs out of LIDAR range), it can be re-initialized or replaced with a particle filter without touching the localization EKF. Coupled states propagate failures across subsystems.

The two EKFs communicate through the TF tree (introduced in Lesson 1.5): the localization EKF publishes the map → odom transform, and the tracking EKF expresses the person's position in the map frame using that transform. Any error in the published transform immediately corrupts the person's apparent map-frame position — which is why the frame validation step from the async section is critical at this interface.

Localization EKF state: [x, y, θ, v, ω] Odometry 100 Hz | IMU 200 Hz LIDAR scan-match 10 Hz publishes: map → odom TF transform TF2 Tree map → odom odom → base_link base_link → laser lookup Tracking EKF state: [px, py, pvx, pvy] LIDAR person cluster 10 Hz RGB-D skeleton 30 Hz publishes: /person/pose (map frame)
The dual-EKF architecture. The localization EKF owns the map→odom transform. The tracking EKF reads that transform from TF2 to express person observations in map frame. Frame ID validation at each EKF input is the critical error-prevention step — a person cluster published in laser frame and consumed as map frame will silently rotate the apparent person position by the current robot heading.

FOLLOWBOTCompleting the Stage 4 milestone: full EKF operational

This lesson completes the Stage 4 milestone for FollowBot. By the end of this stage, your EKF localization node should meet these concrete specifications:

RequirementTargetHow to verify
State vector [x, y, θ, v, ω] — 5 elements Inspect published /ekf/state topic
Position error RMS < 0.10 m over a 100 m run in the test map Compare against map-matched ground truth or motion-capture reference
Heading error RMS < 2° over 100 m Same reference; wrap differences to [-π, π]
LIDAR update rate ≥ 8 Hz (accepting ≥ 80% of 10 Hz scans) Monitor /ekf/gate_log rejection rate
NEES consistency Average NEES in 95% chi-squared band [0.83, 12.83] (n=5) on 30-min recorded session Run nees_check.py on recorded bag with reference trajectory
Sensor dropout behavior LIDAR loss: position σ < 0.25 m after 5 s; recovery on LIDAR return < 2 s Inject LIDAR topic pause in simulation, measure covariance trajectory
Frame validation EKF logs ERROR and drops measurement on wrong frame_id; never silently transforms Publish a mock LIDAR pose with frame_id=laser when map expected; confirm drop

The YAML configuration for robot_localization that wires these three sensors together, with the covariance diagonal entries derived in this lesson, looks like this:

followbot_ekf.yamlrobot_localization EKF config — Stage 4 milestone
ekf_node:
  ros__parameters:
    frequency: 50.0        # predict rate Hz — matches odometry rate
    sensor_timeout: 0.1   # 100 ms — warn if sensor silent
    two_d_mode: true       # 2D robot: ignore z, roll, pitch

    # Fused state in map frame
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: map

    odom0: /odom
    odom0_config: [false,false,false,false,false,false,true,false,false,false,false,true,false,false,false]
    # fuses: vx (index 6) and vyaw (index 11) from odometry
    odom0_noise_covariance: [0.0001, 0, ..., 0.000025]  # diag(0.01², 0.005²)

    imu0: /imu/data
    imu0_config: [false,false,false,false,false,false,false,false,false,false,false,true,false,false,false]
    # fuses: vyaw only — gyro angular rate
    imu0_noise_covariance: [9e-6]  # (0.003 rad/s)²

    pose0: /scan_match/pose
    pose0_config: [true,true,false,false,false,true,false,...]
    # fuses: x, y, yaw from LIDAR scan-matcher
    pose0_noise_covariance: [0.0025,0,0,0,0.0025,0,0,0,0.0004]
    # diag(0.05², 0.05², 0.02²) — measured from scan-match error characterization

    process_noise_covariance: [0.000025,0,0,...]
    # Q diagonal: [x, y, theta, v, omega, ...] — measured from slip data
🔑 FollowBot milestone achieved when

The nees_check.py script, run on a 30-minute indoor session with a reference trajectory, returns an average NEES inside the 95% chi-squared band [0.83, 12.83] for n=5, and the position error RMS over any 100-meter segment stays below 0.1 m. These two numbers, together, prove the filter is both accurate and consistent — not just one or the other. A filter can have low position error (by luck or conservative tuning) while being statistically inconsistent (NEES far from n). The milestone requires both.

WEAKNESSWhere multi-sensor EKF design bites practitioners

⚠ Wrong frame IDs corrupt the EKF silently

This is the failure mode flagged in the lesson brief, and it is genuinely common. A scan-matcher that publishes its output in the laser frame (sensor-fixed) instead of the map frame (world-fixed) will produce a pose measurement that rotates with the robot's heading. When that measurement enters the localization EKF as a map-frame observation, the EKF's innovation is systematically wrong by a rotation equal to the current heading. The filter will appear to track but will produce a heading-dependent position bias that looks exactly like a poor Q setting. This cannot be diagnosed by tuning covariances — it requires checking ros2 topic echo /scan_match/pose --field header.frame_id and verifying the transform chain in tf2_tools view_frames. Always add a frame_id assertion at every EKF measurement input.

⚠ Conflating sensor availability with sensor reliability

A sensor that is publishing is not necessarily publishing useful data. Wheel odometry integrates faithfully while the wheels are in contact with the floor. During a sharp deceleration on a smooth tile floor, all four wheels can momentarily lose traction and the encoder reports motion that did not happen. The encoder is publishing; the data is wrong. The EKF has no internal mechanism to detect this — it will happily accept the anomalous velocity measurement and update the state accordingly. IMU-odometry cross-validation is the correct guard: if the gyroscope and the encoder angular rate disagree by more than 3× their combined uncertainty, flag the odometry measurement for gating review. This is not automatic in robot_localization; you must implement it as a pre-processing node.

⚠ Reusing Q and R across deployment environments

Q and R are not hardware properties — they are system properties in a deployment context. The wheel odometry noise on carpet is measurably different from the same odometry on polished concrete. The LIDAR scan-matching noise in an office with textured walls is different from the same hardware in a long featureless corridor. A filter tuned in simulation and deployed on real hardware without re-characterizing Q and R will likely be inconsistent from day one. This is not a calibration failure — it is a modeling failure. The covariances are site-specific and should be validated on each new deployment environment, not once at commissioning.

KEY INSIGHTHow this connects

This lesson is the capstone of Stage 4. Every filter technique introduced across the stage — Bayes filtering, the Kalman filter, the EKF, the particle filter, IMU preintegration — converges here into a single operational system for FollowBot. Stage 5 adds the perception layer: camera geometry, feature extraction, and SLAM. The localization EKF built here becomes the prior that the SLAM loop corrects when loop closures are detected.

GLOSSARYKey terms

Process noise covariance (Q)
The matrix encoding how much the true state deviates from the kinematic model prediction over one time step. Physical sources: wheel slip, control latency, model mismatch. Set from logged deviation data, not by feel. Too small → filter overconfident, rejects real corrections. Too large → filter ignores the model, covariance grows unnecessarily.
Measurement noise covariance (R)
The matrix encoding the uncertainty of a single sensor measurement. One R matrix per sensor type. Entries are the variance (σ²) of that sensor's error in each measured dimension, measured empirically against a ground-truth reference on the deployment platform.
Innovation
The difference between an actual measurement and the filter's predicted measurement: v = z − h(x̂). The innovation should be zero-mean and Gaussian if the filter is consistent. Large systematic innovations indicate model error or frame ID mismatch.
Chi-squared gating
Outlier rejection applied before the EKF update step. The Mahalanobis distance γ = vᵀ S⁻¹ v is compared to a chi-squared threshold with m degrees of freedom (m = measurement dimension). Measurements with γ above the threshold are rejected as inconsistent with the filter's noise model.
NEES (Normalized Estimation Error Squared)
A filter consistency metric. The expected value of NEES equals the state dimension n if the filter's covariance correctly reflects true uncertainty. NEES ≫ n means overconfidence (Q, R too small). NEES ≪ n means conservatism (Q, R too large). The statistically correct 95% band for FollowBot's 5-state EKF is [chi2.ppf(0.025,5), chi2.ppf(0.975,5)] = [0.83, 12.83], computed by nees_check.py.
Asynchronous measurement update
Applying a sensor's measurement to the EKF at the exact sensor timestamp, using a buffered predict snapshot interpolated to that time. Required when sensors run at different rates. Avoids the assumption that all sensors update simultaneously.
Dual-EKF architecture
The design pattern of running two separate EKF instances — one for robot localization (state: pose + velocity) and one for target tracking (state: target pose + velocity) — connected through the TF2 transform tree rather than sharing a joint state vector. Improves modularity, fault isolation, and tuning clarity.
Frame ID validation
The practice of explicitly checking that every incoming measurement's header.frame_id matches the frame expected by the observation model before the measurement enters the EKF. Prevents the silent corruption failure where a sensor-frame measurement is processed as a world-frame measurement.

ACTIVE RECALLCheck your understanding

Answer in your own words first, then reveal.

1Why is R not a tuning knob? What does it represent physically, and how should it be set?›

R is the measurement noise covariance — a probability statement about how much a sensor's output deviates from the true underlying quantity it measures. Setting R = diag(0.01, 0.01) for LIDAR position measurements asserts that the scan-matcher makes errors with standard deviation ~10 cm (since sqrt(0.01) = 0.1). This is a physical claim about scanner performance in your deployment environment, not a filter gain. It should be measured by logging scan-matcher output against a ground-truth reference (motion capture, high-accuracy external localization) over many runs and fitting the resulting error distribution. Tuning R by watching the filter behavior in RViz produces a filter that looks good without being statistically correct — it will fail the NEES test and may degrade under sensor dropout conditions not captured in the visual inspection.

2FollowBot's EKF has a 5% LIDAR update rejection rate. What are the two most likely causes, and how do you distinguish them?›

The two most likely causes are: (1) the scan-matcher is producing genuinely bad poses (e.g., in featureless corridors, during rapid rotation, or when dynamic objects are present in the scan), and (2) the R matrix for LIDAR is set too small, making the gate threshold tight and rejecting legitimate measurements as outliers. To distinguish: log the rejected innovations and compare their magnitude to the expected sensor error. If rejected innovations are consistently large (10–50 cm) even in known good environments, cause 1 is likely — the scan-matcher itself is unreliable. If rejected innovations are moderate (2–5 cm) and the rejection rate drops when R is inflated, cause 2 is likely — R is underestimating true sensor noise. The distinction matters because fixing cause 1 requires improving the scan-matcher or adding a fallback, while fixing cause 2 requires re-characterizing the sensor from logged data.

3Why does heading error compound into position error during LIDAR dropout, but not the reverse?›

The differential-drive kinematic model integrates velocity in the robot's heading direction. If the heading estimate is wrong by δθ, then at velocity v, the position integrates in a direction offset by δθ — producing a growing positional error of roughly v·t·sin(δθ) over time t. A positional error, on the other hand, does not feed back into the heading estimate in the process model: the kinematic equations for θ_{k+1} = θ_k + ω·dt contain no position terms. So heading error is the primary driver of position growth. This is why IMU gyroscope integration — which observes ω directly — helps stabilize position even without LIDAR, while position-only GPS would not help heading during a dropout. The IMU angular rate measurement keeps the heading covariance from growing too fast, which indirectly bounds position drift.

4You publish a LIDAR scan-match pose with header.frame_id = "laser" when the EKF expects "map" frame. Describe the observable symptom and the correct diagnosis path.›

The scan-match pose is expressed in the sensor-fixed laser frame, which rotates with the robot heading. When consumed by the EKF as a map-frame observation, the apparent position of the scan match rotates by the robot's current heading θ relative to where the true pose is. The EKF's innovation is offset by approximately [x·(cos(θ)−1), x·sin(θ)] — a heading-dependent position bias. Observable symptom: the robot's estimated pose in RViz circles or spirals as it turns, even when stationary. The systematic pattern — errors correlated with heading — is the key diagnostic signal. To diagnose: run ros2 topic echo /scan_match/pose --field header.frame_id and compare against what the EKF expects. Check the TF tree with ros2 run tf2_tools view_frames. This cannot be fixed by adjusting Q or R — it requires correcting the scan-matcher's output frame or adding an explicit TF lookup before publishing.

Stage 4 complete — next: camera geometry and the SLAM loop →