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

The Extended Kalman Filter: Nonlinear Systems and Jacobian Linearization

The EKF's Jacobian linearization is a first-order approximation — understand exactly when it fails and you will know when to reach for a better filter.

⏱ ~30 min read 🧮 Derivation-heavy ⚠ Failure modes taught

🎯 By the end you'll understand

Why the standard Kalman filter breaks on nonlinear systems, and what "linearization at the current estimate" means geometrically. How to derive the Jacobian matrices F and H by differentiating the motion and observation models — directly applying the Jacobian concept from Stage 2 kinematics. The full EKF predict-update cycle with correct equations. Why linearization error grows with covariance width, and how to detect EKF divergence before it causes a navigation failure using the NEES statistic.

KEY INSIGHTLinearization is a bet on how far wrong you can be

The EKF does not eliminate nonlinearity. It bets that the nonlinear function is approximately linear within the current uncertainty ellipsoid. When that bet loses, the filter diverges — confidently and silently.

The Kalman filter from Lesson 4.3 is exact for linear Gaussian systems. Every physical robot has nonlinear dynamics: a differential-drive robot's heading θ appears inside a cosine in its motion model, and a range-bearing sensor's bearing output is an arctangent of position differences. You cannot plug these directly into the KF — the Gaussian distributions that were exact for linear transforms become distorted into non-Gaussian shapes after nonlinear propagation.

🔑 The core approximation

The EKF's answer is a first-order Taylor expansion: replace the nonlinear function with its tangent-plane approximation at the current estimate, then run the standard Kalman filter on that linearized version. The error introduced by this approximation is small when two conditions hold together: (1) the function is weakly nonlinear near the estimate, and (2) the uncertainty ellipsoid is small enough that the linear approximation covers it. When uncertainty is large — which happens during initialization, after sensor dropout, or with bearing-only measurements — the EKF's linearization can be catastrophically wrong, and the filter may diverge. Knowing when to expect this is as important as knowing how to implement the EKF.

CORE CONCEPTWhy the KF breaks on nonlinear systems

Recall the linear KF's predict step. Given a linear motion model xk = Fxk-1 + Buk + w, a Gaussian prior propagates to a Gaussian posterior exactly: if xk-1 ~ 𝒩(μ, Σ) and we apply a linear map F, then Fxk-1 ~ 𝒩(Fμ, FΣFT). Gaussian in, Gaussian out — no approximation needed.

Now consider the differential-drive robot. Its true motion model is:

Nonlinear motion model: x_{k} = f(x_{k-1}, u_k) + w_k x_k = x_{k-1} + v·Δt·cos(θ_{k-1}) y_k = y_{k-1} + v·Δt·sin(θ_{k-1}) θ_k = θ_{k-1} + ω·Δt v, ω = commanded linear and angular velocity; Δt = timestep; w_k ~ 𝒩(0,Q) = process noise ← cos(θ): nonlinear ← sin(θ): nonlinear ← linear in θ
The differential-drive motion model. The x and y update terms contain cos(θ) and sin(θ) — nonlinear functions of the heading state. The KF's FΣFT covariance propagation assumes linearity; applying it here would give the wrong answer.

Propagating a Gaussian through cos(θ) or sin(θ) produces a non-Gaussian distribution. For a heading uncertainty of ±5°, the distortion is small and you might get away with it. For a heading uncertainty of ±45° — say, after a kidnapped-robot recovery or a long sensor dropout — the approximation error is large enough to corrupt the covariance estimate severely. This is the regime where the EKF breaks.

📖 The same problem in observation models

The observation model h(x) maps robot state to what the sensor should measure. For a range sensor measuring the distance to a landmark: h(x) = √((x_L - x)² + (y_L - y)²). For a bearing sensor: h(x) = atan2(y_L - y, x_L - x). Both are nonlinear in the robot state. Bearing-only localization is particularly nonlinear because small changes in position can produce large changes in bearing when the landmark is nearby — and tiny changes in bearing mean huge uncertainty in position when the landmark is far away. This asymmetry is exactly why bearing-only localization is a hard problem that exposes EKF weaknesses starkly. It is the widget we will explore below.

HOW IT WORKSThe Jacobians F and H: linearization at the current estimate

The EKF's solution is to linearize f and h around the current best estimate. You learned the Jacobian in Lesson 2.4 as the matrix of partial derivatives that maps velocity in joint space to velocity in task space. Here it serves a different purpose — it maps how uncertainty in state space propagates through a nonlinear function. The mathematical structure is identical; the application is different.

The first-order Taylor expansion of f(x, u) around the current estimate x̂ is:

📖 Taylor expansion of the motion model

f(x, u) ≈ f(x̂, u) + F·(x - x̂) + higher-order terms (ignored)
where F = ∂f/∂x |x=x̂ (the Jacobian of f evaluated at x̂)

The EKF keeps only the linear term, treating higher-order terms as additional "linearization noise." This is what makes it an approximation, not an exact filter.

For the differential-drive motion model f = [x + v·Δt·cos(θ), y + v·Δt·sin(θ), θ + ω·Δt]T, the Jacobian F (a 3×3 matrix for a 3-state robot) is computed by differentiating each output with respect to each state variable x, y, θ:

Motion Jacobian F = ∂f/∂x evaluated at current estimate x̂ = [x̂, ŷ, θ̂] ∂f_x/∂· ∂f_y/∂· ∂f_θ/∂· ∂/∂x ∂/∂y ∂/∂θ 1 0 -v·Δt·sin(θ̂) 0 1 +v·Δt·cos(θ̂) 0 0 1 ← These two entries are the only nonlinear terms: heading θ̂ appears inside sin/cos. They are re-evaluated at the current estimate every timestep. This is what makes the EKF adaptive. If v or Δt is small, these entries are small → the motion is nearly linear → linearization error is small.
The motion Jacobian F for a 3-state differential-drive robot. Compare with Lesson 2.4's geometric Jacobian: the structure is identical — partial derivatives arranged in a matrix — but here the function being differentiated is the discrete-time motion model, not the forward kinematics.

The observation Jacobian H is derived the same way. For bearing-only localization (measuring only the angle to a landmark, not the distance), h(x) = atan2(y_L - y, x_L - x). Differentiating with respect to [x, y, θ]:

Observation Jacobian H = ∂h/∂x (bearing-only to landmark at [x_L, y_L]) Let d² = (x_L - x̂)² + (y_L - ŷ)² (squared distance to landmark) H = (y_L - ŷ)/d² -(x_L - x̂)/d² 0 ∂h/∂x ∂h/∂y ∂h/∂θ ⚠ Both entries divide by d². When the landmark is close, d² is tiny → H entries blow up → linearization is highly inaccurate.
The observation Jacobian H for a bearing-only sensor. Note that H → ∞ as d → 0 (landmark approaches the robot). This is a geometric singularity: bearing alone cannot distinguish "slightly to the left at 1 m" from "far to the left at 100 m" when position is uncertain. The EKF's linearization fails hardest in this regime.

The physical intuition: H encodes how sensitive the measurement is to changes in state. When the landmark is far away, a 1 m lateral error in robot position changes the bearing by very little (small H entries → low sensitivity → the measurement provides weak position information). When the landmark is close, a 1 m lateral error changes the bearing substantially (large H entries → measurement is highly informative, but also where the nonlinearity is sharpest and linearization most dangerous).

DEFINITIONThe EKF predict and update equations

With F and H in hand, the EKF replaces the standard KF's constant matrices with locally-evaluated Jacobians. Everything else is structurally identical to Lesson 4.3's KF — the same Riccati-style covariance flow, the same Kalman gain, the same MMSE update:

Step Equation What it does
Predict — state x̂⁻ = f(x̂, u) Propagate estimate through the nonlinear model (not the linearized one — this is important)
Predict — covariance P⁻ = F P FT + Q Propagate uncertainty through the linearized model; Q adds process noise
Kalman gain K = P⁻ HT (H P⁻ HT + R)⁻¹ Optimal weighting of measurement vs. prediction; H is local Jacobian
Innovation ν = z - h(x̂⁻) Difference between actual measurement and predicted measurement via nonlinear h
Update — state x̂ = x̂⁻ + K ν Shift estimate toward measurement, weighted by Kalman gain
Update — covariance P = (I − K H) P⁻ Reduce uncertainty where the measurement was informative
💡 The critical asymmetry: nonlinear f and h for means, linearized F and H for covariances

Notice that the state mean is propagated through the full nonlinear f(x̂, u) in the predict step, and the innovation uses the full nonlinear h(x̂⁻). Only the covariance propagation uses the Jacobians F and H. This is the correct application of the first-order approximation: the mean uses the exact function, and the uncertainty propagation uses the linear approximation. An implementation mistake — using F·x̂ instead of f(x̂, u) for the mean prediction — is a common bug that causes the estimate to drift even when the covariance looks healthy.

Here is a minimal Python implementation of the EKF predict step for the 3-state differential-drive robot, following the exact equations above:

ekf_predict.pyEKF predict step · differential-drive
import numpy as np

def ekf_predict(x_hat, P, u, Q, dt):
    """
    EKF predict step for 3-state differential-drive robot.
    x_hat : [x, y, theta] — current state estimate (3,)
    P     : 3x3 covariance matrix
    u     : [v, omega]  — control inputs (linear vel, angular vel)
    Q     : 3x3 process noise covariance
    dt    : timestep (seconds)
    """
    x, y, theta = x_hat
    v, omega = u

    # --- 1. Propagate mean through the NONLINEAR model ---
    x_pred = np.array([
        x + v * dt * np.cos(theta),     # nonlinear: cos(theta)
        y + v * dt * np.sin(theta),     # nonlinear: sin(theta)
        theta + omega * dt
    ])

    # --- 2. Evaluate the motion Jacobian F at the current estimate ---
    F = np.array([
        [1, 0, -v * dt * np.sin(theta)],  # ∂f_x/∂[x,y,θ]
        [0, 1,  v * dt * np.cos(theta)],  # ∂f_y/∂[x,y,θ]
        [0, 0,  1                      ]   # ∂f_θ/∂[x,y,θ]
    ])

    # --- 3. Propagate covariance through the LINEARIZED model ---
    P_pred = F @ P @ F.T + Q   # linearization error enters here

    return x_pred, P_pred, F

Three things worth holding onto here. First, theta is evaluated at x_hat[2] — the current estimate — not at x_pred. This is the "linearization at the current estimate" operation. Second, the F matrix is recomputed every timestep. Unlike the linear KF where F is constant, the EKF must re-evaluate F at each new estimate — this is extra computation but necessary for accuracy. Third, the only approximation is in line 3 (P_pred). If Q is set correctly and F captures the true curvature at the operating point, the filter is consistent. If Q is too small and F is evaluated at a poor estimate (because the true state has diverged far from x_hat), the filter's covariance will underestimate the true uncertainty — the precondition for divergence.

SEE ITEKF nonlinearity stress test: bearing-only localization

Bearing-only localization is the hardest legitimate EKF use case: you observe only the angle to landmarks, never the distance. This makes the observation function highly nonlinear — and it means a wide initial covariance is especially dangerous. The question below is not "does the EKF work?" but "at what initial uncertainty does it start to fail, and how does NEES reveal that failure before you crash?"

⚙ Interactive · EKF bearing-only localization divergence test

Set the initial position uncertainty and bearing noise level. Watch the covariance ellipse, the innovation sequence, and the NEES statistic. The worst choice is high initial uncertainty with low bearing noise — the filter becomes overconfident in a bad linearization and diverges silently.

3 m
Low (1 m) = well-initialized; High (10 m) = kidnapped robot
3°
Low = very accurate sensor; High = noisy or wind-disturbed bearing
More landmarks = more geometry constraints = better observability
● true ● estimated - - cov ellipse
Filter state
Position error
—
Cov trace √(tr P)
—
Innovation |ν|
—
NEES consistency check
Normalized Estimation Error Squared (NEES) = εT P-1 ε. For a consistent 2D filter, NEES should lie in the χ²(2) 95% band [0.05, 7.38]. Outside → filter is inconsistent.
— awaiting run
χ²(2) 95% band: [0.05, 7.38]. NEES >> 7.4 = overconfident / diverged. NEES << 0.05 = underconfident / Q too large.
Set parameters above and click Run EKF to begin. Try σ₀=10 m, σ_z=1° to see divergence onset.
NEES over time (50 steps)
Red band = 95% χ²(2) upper bound (7.38). Bars above it indicate filter inconsistency / divergence.

The key lesson the widget teaches: when σ₀ is large and σ_z is small, the filter becomes overconfident. The small measurement noise tells the EKF the sensor is highly accurate, so the filter strongly weights the bearing measurements. But with a large initial uncertainty, the linearization of atan2 is poor — the actual relationship between position error and bearing error is highly nonlinear. The EKF uses a linear approximation that underestimates how much the bearing can change for a given position shift. The result: the filter's covariance shrinks (it thinks it has localized), but the true position error remains large. NEES climbs above 7.4 and stays there — divergence has occurred. Adding more landmarks provides geometric constraints that help, but cannot fully rescue a severely misinitialized EKF.

IN PRACTICEEKF for 2D pose estimation with odometry + bearing

Putting together the equations above, here is the complete EKF update step for bearing-only observations. The motion model is differential-drive from above; the observation model is bearing to a known landmark:

ekf_update.pyEKF bearing update · robot localization
import numpy as np

def ekf_bearing_update(x_hat, P, bearing_meas, landmark, R):
    """
    EKF update step for a single bearing measurement.
    x_hat    : [x, y, theta] — predicted state estimate
    P        : 3x3 predicted covariance
    bearing_meas : scalar — measured bearing to landmark (rad)
    landmark : [x_L, y_L] — known landmark position
    R        : scalar — bearing measurement noise variance (rad²)
    Returns  : updated (x_hat, P, nees_contribution)
    """
    x, y, theta = x_hat
    x_L, y_L = landmark

    # --- 1. Predicted measurement via NONLINEAR h ---
    dx = x_L - x
    dy = y_L - y
    d_sq = dx**2 + dy**2
    d = np.sqrt(d_sq)

        z_pred = np.arctan2(dy, dx)   # nonlinear h(x̂) — not H·x̂

    # --- 2. Innovation (wrap to [-π, π]) ---
    nu = bearing_meas - z_pred
    nu = np.arctan2(np.sin(nu), np.cos(nu))  # angle wrap

    # --- 3. Observation Jacobian H at current estimate ---
    H = np.array([[dy / d_sq, -dx / d_sq, 0]])  # shape (1, 3)

    # --- 4. Innovation covariance S and Kalman gain K ---
    S = H @ P @ H.T + R     # scalar: expected innovation variance
    K = P @ H.T / S          # shape (3, 1): Kalman gain

    # --- 5. Update state and covariance ---
    x_upd = x_hat + (K * nu).flatten()
    x_upd[2] = np.arctan2(np.sin(x_upd[2]), np.cos(x_upd[2]))  # wrap theta
    P_upd = (np.eye(3) - K @ H) @ P

    # --- 6. NEES requires ground truth, not available inside update ---
    # Compute NEES externally after comparing x_upd to the true state.
    # e.g.: err = x_true[:2] - x_upd[:2]; nees = err @ solve(P_upd[:2,:2], err)

    return x_upd, P_upd

The highlighted line is load-bearing: z_pred = np.arctan2(dy, dx) is the full nonlinear h evaluated at x̂, not the linearized approximation H·x̂. Many EKF implementation bugs arise from using H·x̂ here instead — this is wrong, and it introduces a systematic bias into the innovation that corrupts the update. The innovation ν must compare like with like: a real bearing measurement vs. what you'd expect to measure from the current estimated position via the actual nonlinear sensor model.

💡 Angle wrapping is not optional

The innovation ν = bearing_measured − bearing_predicted must be wrapped to [−π, π]. Without the arctan2(sin(ν), cos(ν)) wrap, a robot at −175° estimated heading and a true heading of +175° computes a 350° innovation instead of the correct −10° one. The Kalman gain then applies a 350° correction, launching the estimate into a completely wrong region. This is one of the most common EKF implementation bugs in bearing-based localization, and it is entirely invisible in unit tests that don't straddle the ±π boundary.

FOLLOWBOTFull EKF on FollowBot: state vector, sensors, NEES test

The stage milestone for Stage 4 calls for a full EKF on FollowBot with a 5-state vector [x, y, θ, v, ω], fusing wheel odometry, IMU, and LIDAR scan-matching updates, with filter consistency validated via NEES on recorded data. Here is how the architecture unfolds.

State vector x (5×1) x — map-frame position (m) y — map-frame position (m) θ — heading (rad) v — linear velocity (m/s) ω — angular velocity (rad/s) Wheel odometry Δs, Δθ → v, ω update 50 Hz · motion model predict IMU (6-axis) gyro → ω obs update 200 Hz · heading correction LIDAR scan-match [x, y, θ] obs update 10 Hz · pose correction EKF (5×5) Predict: odom at 50 Hz Update: IMU at 200 Hz Update: LIDAR at 10 Hz Output: x̂, P every cycle NEES Consistency Monitor ε = x̂ - x_true (from bag) NEES = εᵀ P⁻¹ ε Alert if > χ²(5, 0.95) = 11.07
FollowBot's EKF architecture. The 5-state vector adds v and ω to the 3-state pose vector, allowing the filter to smooth velocity estimates and predict motion between slow LIDAR updates. Three sensor streams operate at different rates; the EKF handles asynchronous updates by running a predict step to the measurement timestamp before each update. NEES is computed offline against ground-truth pose from a bag file.

A critical implementation detail with asynchronous sensors: when a LIDAR update arrives at time tL and the last predict was at tk, you must run a partial predict step from tk to tL before applying the update. If you apply the update with a stale prediction (using tk's state), you are comparing a 100 ms old estimate to a current measurement — the innovation will be dominated by the motion since tk, not by the measurement error. This is one of the reasons the robot_localization package, which FollowBot uses, implements a state buffer and retroactive prediction rather than a simple fixed-rate update.

🔑 NEES for a 5-state filter

The NEES statistic for a consistent n-state filter follows a χ²(n) distribution. For n=5, the 95% upper bound is 11.07 (not 7.38 as in the 2D widget — the degrees of freedom differ). When FollowBot's NEES exceeds 11.07 for more than a few consecutive steps, the filter's covariance P is underestimating the true uncertainty — the classic sign of: (1) Q too small, (2) R too small, (3) a sensor is failing silently, or (4) the motion model has unmodeled nonlinearity (wheel slip on a rug). Each of these requires a different fix; NEES tells you which category you're in.

Here is the ROS 2 node structure for FollowBot's EKF, using robot_localization with a custom NEES validator node:

ekf_params.yamlrobot_localization · FollowBot EKF config
# EKF node config for FollowBot (robot_localization package)
ekf_filter_node:
  ros__parameters:
    frequency: 50.0        # predict rate (Hz) — matches odometry
    sensor_timeout: 0.1   # s; drop sensor if silent > 100 ms
    two_d_mode: true       # 2D: x, y, yaw only (ignore z, roll, pitch)
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    # Odometry: fuse x, y, yaw velocity (not absolute pose)
    odom0: /odom           # topic: nav_msgs/Odometry
    odom0_config: [false, false, false,   # x, y, z absolute
                   false, false, false,   # roll, pitch, yaw absolute
                   true,  true,  false,   # vx, vy, vz <-- fuse these
                   false, false, true]    # ax, ay, vyaw <-- fuse vyaw

    # IMU: fuse angular velocity only (gyro; not accel in 2D mode)
    imu0: /imu/data
    imu0_config: [false, false, false,
                  false, false, false,
                  false, false, false,
                  false, false, true]   # fuse yaw rate from gyro
    imu0_remove_gravitational_acceleration: true

    # Process noise Q — start large, then tune down with NEES
    process_noise_covariance: [
      0.05, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
      0,    0.05, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,
      0,    0,    0.06, 0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,    0
    ]   # truncated — see full 15×15 in the repo ekf_params.yaml
💡 The LIDAR scan-matching update is not built into robot_localization

The robot_localization package fuses odometry and IMU out of the box. LIDAR scan-matching (using slam_toolbox in localization-only mode) publishes its pose estimate as a nav_msgs/Odometry topic on /scan_match_odom. You configure a second odometry input (odom1) in the YAML, fusing only absolute position and heading from the scan matcher while relying on the kinematic model for velocities. This layering is standard in production ROS 2 navigation stacks — the EKF acts as the sensor fusion hub, not the localization algorithm itself.

WEAKNESSWhere the EKF bites: three failure modes that matter

✗ Large initial uncertainty + highly nonlinear observation = silent divergence

The widget demonstrates this precisely. With a 10 m initial uncertainty and a highly informative but highly nonlinear bearing sensor, the EKF's linearization of atan2 is inaccurate over the full uncertainty ellipsoid. The filter applies the Kalman gain as if the linearization were correct, collapses the covariance (thinks it's localized), but the true position remains far from the estimate. Once diverged in this way, the EKF typically cannot self-recover: the small covariance causes low Kalman gain, meaning future measurements barely influence the estimate, and the filter is locked into a wrong region of state space. The fix is not better tuning — it is using an Unscented Kalman Filter (Lesson 4.6) or particle filter (Lesson 4.7) for the initialization phase, then handing off to the EKF once the uncertainty is small.

✗ Bearing-only sensors with a single landmark are fundamentally unobservable

One bearing measurement to one landmark provides one scalar constraint on a 3-dimensional state space (x, y, θ). The system is unobservable — infinitely many (x, y, θ) combinations produce the same bearing. The EKF will not crash; it will quietly fail to reduce position uncertainty in the directions that bearing doesn't constrain. With two non-collinear landmarks, the intersection of bearing rays constrains position. With three or more, the system is overdetermined and filter consistency improves markedly. Before designing a bearing-only localization system, check observability formally using the observability Gramian — if it's rank-deficient, no filter algorithm will save you.

✗ Incorrect Jacobian due to heading representation

A subtle implementation trap: the Jacobian F derived above assumes the heading state θ is represented as an unbounded real number (radians, possibly accumulating past ±π). If your state vector wraps θ to [−π, π] inside the predict step — a common "fix" for the wraparound issue — the Jacobian computed from the pre-wrap state and applied to the post-wrap covariance is inconsistent. The partial derivative ∂f_θ/∂θ = 1 holds globally only for the unwrapped representation. For wrapped θ, there's a discontinuity at ±π that the linear approximation cannot capture, and the covariance update will be wrong near the ±π boundary. The correct fix is to wrap θ only when computing the innovation ν, never inside f itself. This is exactly what the ekf_predict code above does — and what the ekf_update angle-wrap note addresses.

KEY INSIGHTHow this connects

  • 4.3 · The Kalman Filter: Derivation and Geometric Intuition — the linear KF from which the EKF extends: exact predict-update cycle, MMSE derivation, geometric picture of uncertainty ellipsoids. The EKF keeps all of this structure and adds Jacobian re-evaluation at each step.
  • 4.4 · Kalman Filter Tuning: Q, R, and the Art of Honest Uncertainty — Q and R tuning is equally load-bearing for the EKF, but with an additional complication: poor Q (too small) widens the regime where the EKF diverges from linearization error, because the covariance collapses faster than the true uncertainty. NEES is the EKF practitioner's primary Q-tuning instrument.
  • 2.4 · The Jacobian: Velocity Kinematics and the Spine of Robotics — the Jacobian concept introduced for kinematics (J mapping joint velocity to end-effector velocity) is reused here for a different purpose: F and H map state uncertainty through nonlinear dynamics and observation functions. Same mathematical tool, different physical interpretation.
  • 4.6 · The Unscented Kalman Filter: When the Jacobian Is Not Enough → — the direct response to EKF failure modes: the UKF replaces Jacobian evaluation with deterministic sigma-point sampling, providing better nonlinearity handling without the need to derive or compute Jacobians. Use the EKF when nonlinearity is mild; reach for the UKF when it's not.

The EKF is the workhorse filter of robotics — it powers robot_localization, most production SLAM back-ends, and thousands of deployed robots. But it has a clearly defined failure regime: high nonlinearity combined with large uncertainty. Knowing where that regime is, and how NEES reveals when you've entered it, is the difference between a roboticist who trusts their filter blindly and one who validates it before trusting the navigation stack with a physical robot.

GLOSSARYKey terms

Extended Kalman Filter (EKF)
A Kalman filter variant for nonlinear systems that linearizes the motion model f(x,u) and observation model h(x) via first-order Taylor expansion (Jacobians) at the current state estimate. Exact for linear Gaussian systems; approximate otherwise.
Motion Jacobian F
The matrix of partial derivatives ∂f/∂x evaluated at the current estimate x̂. Used in the EKF predict step to propagate covariance: P⁻ = F P FT + Q. Must be recomputed every timestep because it depends on the current state.
Observation Jacobian H
The matrix of partial derivatives ∂h/∂x evaluated at the predicted estimate x̂⁻. Used to compute the Kalman gain K and the innovation covariance S. For bearing sensors, entries in H diverge as the landmark approaches the robot.
Innovation ν
The difference between the actual measurement z and the predicted measurement h(x̂⁻) — computed via the full nonlinear h, not the linearized approximation H·x̂⁻. A large sustained innovation (relative to the innovation covariance S) signals filter inconsistency or model mismatch.
Linearization error
The error introduced by replacing f(x,u) and h(x) with their first-order Taylor expansions. Grows quadratically with: (1) the distance from the estimate to the true state, and (2) the width of the uncertainty ellipsoid. EKF divergence occurs when linearization error dominates the noise terms Q and R.
NEES (Normalized Estimation Error Squared)
The statistic εT P-1 ε where ε is the state estimation error and P is the filter's covariance. For a consistent filter, NEES follows a χ²(n) distribution. NEES consistently above the χ² upper bound indicates the filter is overconfident (P too small); below the lower bound indicates underconfidence (P too large).
Bearing-only localization
A localization problem in which sensors provide only the angle to landmarks, not their range. Highly nonlinear (atan2) and poorly observable with fewer than 2 non-collinear landmarks. A canonical benchmark for EKF nonlinearity stress — exactly where the UKF and particle filter outperform the EKF.
Observability
The property of a dynamical system that all states can be inferred from a sequence of measurements. In the Kalman filter context, a system is observable if the observability Gramian (or the matrix stacked from H, HF, HF², …) has full rank. A bearing-only system with one landmark is not observable in position.

ACTIVE RECALLCheck your understanding

Answer in your own words first, then reveal.

1Why does the standard Kalman filter fail for a differential-drive robot, and what specific property of the motion model causes this?›

The standard Kalman filter's predict step uses the relation P⁻ = F P FT + Q, which is only exact when the motion model is linear. For a differential-drive robot, the x and y position updates contain cos(θ) and sin(θ) — nonlinear functions of the heading state. Propagating a Gaussian distribution through a nonlinear function produces a non-Gaussian result: the true posterior over (x, y) after a curved motion is banana-shaped, not elliptical. The KF's Gaussian assumption forces it to approximate this with an ellipse, introducing covariance estimation error that grows with heading uncertainty. For small heading uncertainty (±5°), the approximation is adequate. For large heading uncertainty, it can cause the filter to significantly underestimate or misalign the uncertainty ellipsoid, leading to incorrect Kalman gains in the update step.

2What is the specific mathematical relationship between the Jacobian in Lesson 2.4 (velocity kinematics) and the Jacobians F and H in the EKF?›

Both are the same mathematical object — a matrix of first-order partial derivatives — used for different physical purposes. In Lesson 2.4, the geometric Jacobian J maps joint velocity q̇ to end-effector velocity ẋ: ẋ = J q̇. The Jacobian linearizes the forward kinematics function at a given configuration. In the EKF, F = ∂f/∂x linearizes the discrete-time motion model f(x,u) at the current state estimate, and H = ∂h/∂x linearizes the observation model h(x) at the predicted estimate. In both cases, the Jacobian is a first-order local approximation that becomes inaccurate far from the evaluation point — the core limitation shared by velocity kinematics (singularities, manipulability) and the EKF (linearization error, divergence).

3In the EKF update step, the state mean uses the full nonlinear h(x̂⁻) for the innovation, but the covariance uses the Jacobian H. Why this asymmetry, and what bug arises from using H·x̂⁻ instead of h(x̂⁻)?›

The asymmetry is intentional and correct. The state mean (a single point) can be propagated through the exact nonlinear function h without approximation — this is cheap and gives the most accurate predicted measurement. The covariance (a distribution) needs to be propagated through a linear function to remain Gaussian-representable; so the Jacobian H (the best linear approximation at x̂⁻) is used. Using H·x̂⁻ for the innovation instead of h(x̂⁻) introduces a systematic bias: the predicted measurement would be the linearized approximation evaluated at x̂⁻, which differs from the actual expected measurement h(x̂⁻) by the linearization error term. This bias appears as a persistent non-zero mean innovation, which corrupts the update — the filter "sees" a measurement offset that doesn't exist and shifts the state estimate accordingly. For a bearing sensor near a close landmark, this bias can be several degrees — large enough to cause navigation divergence.

4FollowBot's NEES has been consistently above 11.07 (the χ²(5, 0.95) threshold) for the past 30 seconds. List three distinct hypotheses for the root cause, and for each hypothesis, describe one diagnostic action that would confirm or rule it out.›

Hypothesis 1: Q is too small. The process noise matrix underestimates the true motion uncertainty (e.g., wheel slip on a rug adds unmodeled noise). Diagnostic: increase Q by a factor of 5 and re-run NEES. If NEES drops back below 11.07, Q was the culprit. Supporting evidence: NEES rises gradually after turns or accelerations, then settles — timing with motion events is the fingerprint.

Hypothesis 2: A sensor is failing silently. The IMU's gyro is biased (common after thermal drift), or the LIDAR scan-matcher is returning incorrect poses due to a featureless corridor. Diagnostic: plot each sensor's innovation ν over time. A biased IMU will show a nonzero mean ν (should be zero-mean if honest). A failing scan-matcher will show large erratic innovations that correlate with NEES spikes.

Hypothesis 3: Significant unmodeled nonlinearity (EKF linearization error). The robot is executing sharp turns at high speed, pushing the motion model into a regime where the cos(θ) and sin(θ) linearization is inaccurate. Diagnostic: log the Frobenius norm of the higher-order Hessian terms (approximated numerically by comparing f at x̂ ± σ). If these are comparable to the Q diagonal values, the linearization error is significant. Alternative: switch to a UKF (Lesson 4.6) and compare NEES — if UKF NEES is within bounds while EKF NEES is not, linearization error is confirmed.

Next: the Unscented Kalman Filter — when the Jacobian is not enough →