EKF for 2D Navigation¶

EKF for our 5-State System¶

To see how the Extended Kalman Filter (EKF) handles different types of information, let’s apply it to our 5-state robot using two very different sensors.

As established, our state vector $\mathbf{x}$ tracks the robot in the World Frame, while our input vector $\mathbf{u}$ captures the "driving" forces from the onboard sensors in the Body Frame:$$\mathbf{x} = \begin{bmatrix} p_1 \\ p_2 \\ v_1 \\ v_2 \\ \theta \end{bmatrix}, \quad \mathbf{u} = \begin{bmatrix} a_1^B \\ a_2^B \\ \omega \end{bmatrix}$$While the robot moves, we receive "correction" measurements ($\mathbf{z}$) from two external sensors: a Magnetometer and a Distance Beacon.

1. The Magnetometer: A Linear Measurement¶

A calibrated magnetometer provides the robot's absolute heading $\theta$ relative to the World North.

The Measurement Function $h(\mathbf{x})$: The sensor simply "sees" the fifth element of our state.$$h_{mag}(\mathbf{x}) = \theta$$The Jacobian $\mathbf{J}_{H, mag}$: Since the relationship is a simple direct observation, the derivative of $\theta$ with respect to itself is 1, and 0 for all others. This is a linear measurement. $$\mathbf{J}_{H, mag} = \begin{bmatrix} 0 & 0 & 0 & 0 & 1 \end{bmatrix}$$

2. The Distance Beacon: A Nonlinear Measurement¶

Imagine a radio beacon located exactly at the World origin $(0,0)$. The sensor measures the total distance $d$ to that origin.

The Measurement Function $h(\mathbf{x})$: The distance is calculated using the Pythagorean theorem based on the current position.$$h_{dist}(\mathbf{x}) = \sqrt{p_1^2 + p_2^2}$$

This relationship is nonlinear. The sensor does not see $p_1$ or $p_2$ directly; it sees a combination of them. To use this in our EKF, we must calculate the Jacobian $\mathbf{J}_{H, dist}$ by taking the partial derivatives:$$\frac{\partial h}{\partial p_1} = \frac{p_1}{\sqrt{p_1^2 + p_2^2}}, \quad \frac{\partial h}{\partial p_2} = \frac{p_2}{\sqrt{p_1^2 + p_2^2}}$$ The Jacobian $\mathbf{J}_{H, dist}$: $$\mathbf{J}_{H, dist} = \begin{bmatrix} \frac{p_1}{\sqrt{p_1^2 + p_2^2}} & \frac{p_2}{\sqrt{p_1^2 + p_2^2}} & 0 & 0 & 0 \end{bmatrix}$$

Summary of the EKF Correction¶

When these sensors provide data, the EKF uses these Jacobians to update the state.

1. Magnetometer Update: The filter sees $\mathbf{J}_{H, mag}$ and realizes the measurement only provides information about $\theta$. It "corrects" the heading directly.
2. Distance Update: The filter looks at $\mathbf{J}_{H, dist}$. If the robot is currently at $(10, 0)$, the Jacobian becomes $[1, 0, 0, 0, 0]$. The EKF "understands" that at this specific location, a change in distance only helps correct the $p_1$ coordinate. If the robot moves to $(0, 10)$, the Jacobian automatically shifts the correction to $p_2$.

This demonstrates the power of the EKF: by linearizing the distance formula at the robot's current estimated position, the filter can extract useful information from a sensor that only knows "how far" but not "what direction."

EKF Implementation for 2D Navigation¶

The Extended Kalman Filter works in two distinct phases:

1. Prediction (where we use our physics model and IMU)
2. Updates (where we use our external sensors).

1. Prediction¶

The nonlinear transition $\mathbf{f}$ for the state vector prediction:

The state is updated by integrating the body-frame accelerations, which are rotated into the world frame via the current orientation $\theta$:

$$\mathbf{\hat{x}}_k^- = \begin{bmatrix} p_{1,k-1} + v_{1,k-1} \Delta t + \frac{1}{2}\Delta t^2 (a_1^B \cos\theta_{k-1} - a_2^B \sin\theta_{k-1}) \\ p_{2,k-1} + v_{2,k-1} \Delta t + \frac{1}{2}\Delta t^2 (a_1^B \sin\theta_{k-1} + a_2^B \cos\theta_{k-1}) \\ v_{1,k-1} + \Delta t (a_1^B \cos\theta_{k-1} - a_2^B \sin\theta_{k-1}) \\ v_{2,k-1} + \Delta t (a_1^B \sin\theta_{k-1} + a_2^B \cos\theta_{k-1}) \\ \theta_{k-1} + \omega \Delta t \end{bmatrix}$$

The Covariance Propagation: We propagate the uncertainty $\mathbf{P}$ by linearizing the motion. This step "warps" our existing uncertainty and injects new noise from the IMU:

$$\mathbf{P}_k^- = \mathbf{J}_F \mathbf{P}_{k-1}^+ \mathbf{J}_F^\top + \mathbf{Q}_k$$

Where

• $\mathbf{Q}_k = \mathbf{J}_G \mathbf{Q}_u \mathbf{J}_G^\top$

Recap: Process Noise ($\mathbf{Q}_k$) — in practice¶

The construction of $\mathbf{Q}_k = \mathbf{J}_G\,\mathbf{Q}_u\,\mathbf{J}_G^\top$ (the control Jacobian $\mathbf{J}_G$, the input-noise matrix $\mathbf{Q}_u$, and the isotropic simplification) was derived in detail in the 2D state-system notebook. Under the usual isotropic accelerometer assumption ($\sigma_{a1}=\sigma_{a2}=\sigma_a$) the heading cancels, and $\mathbf{Q}_k$ reduces to the constant, heading-free matrix we hard-code in the implementation below:

$$\mathbf{Q} = \begin{bmatrix} \frac{1}{4}\Delta t^4 \sigma_a^2 & 0 & \frac{1}{2}\Delta t^3 \sigma_a^2 & 0 & 0 \\ 0 & \frac{1}{4}\Delta t^4 \sigma_a^2 & 0 & \frac{1}{2}\Delta t^3 \sigma_a^2 & 0 \\ \frac{1}{2}\Delta t^3 \sigma_a^2 & 0 & \Delta t^2 \sigma_a^2 & 0 & 0 \\ 0 & \frac{1}{2}\Delta t^3 \sigma_a^2 & 0 & \Delta t^2 \sigma_a^2 & 0 \\ 0 & 0 & 0 & 0 & \Delta t^2 \sigma_\omega^2 \end{bmatrix}$$

As always, $\sigma_a$ and $\sigma_\omega$ are the primary tuning knobs: larger values tell the filter to trust the IMU less and lean more on the external sensors (Distance Beacon, Magnetometer).

Update Phase (Correction)¶

In a real-world EKF, measurements from different sensors rarely arrive at the same time or frequency. We handle this using Sequential Updates: we perform a specific correction step whenever a sensor provides new data.

The Magnetometer Update (Linear)¶

The magnetometer provides an absolute heading $z_{mag} = \theta$. Since the sensor observes a state variable directly, the relationship is linear.

• The Innovation ($\mathbf{y}_k$): The difference between the measured angle and our predicted angle.$$\mathbf{y}_k = \text{normalize}(z_{mag} - \hat{\theta}_k^-)$$Note: We must wrap the result to $[-\pi, \pi]$ to avoid mathematical errors at the $\pm 180^\circ$ ($\pm\pi$) boundary — without wrapping, a small true error across the seam (e.g. $+170^\circ$ vs. $-170^\circ$) would appear to the filter as a jump of nearly $360^\circ$.
• The Measurement Jacobian ($\mathbf{J}_{H,mag}$): A simple selection matrix that picks $\theta$ out of the state vector.

$$\mathbf{J}_{H,mag} = \begin{bmatrix} 0 & 0 & 0 & 0 & 1 \end{bmatrix}$$

• The Kalman Gain ($\mathbf{K}_k$):

$$\mathbf{K}_k = \mathbf{P}_k^- \mathbf{J}_{H,mag}^\top (\mathbf{J}_{H,mag} \mathbf{P}_k^- \mathbf{J}_{H,mag}^\top + R_{mag})^{-1}$$

• Correction:
  • State: $\hat{\mathbf{x}}_k^+ = \hat{\mathbf{x}}_k^- + \mathbf{K}_k \mathbf{y}_k$
  • Covariance: $\mathbf{P}_k^+ = (\mathbf{I} - \mathbf{K}_k \mathbf{J}_{H,mag}) \mathbf{P}_k^-$

The Distance Beacon (Nonlinear)¶

The beacon at the origin $(0,0)$ provides a distance measurement $z_{dist}$. The relationship to the state is nonlinear ($\sqrt{p_1^2 + p_2^2}$), requiring a fresh Jacobian calculation at every update.

• The Innovation ($\mathbf{y}_k$): The difference between the measured distance and the distance calculated from our current position estimate.$$\mathbf{y}_k = z_{dist} - \sqrt{(\hat{p}_1^-)^2 + (\hat{p}_2^-)^2}$$
• The Measurement Jacobian ($\mathbf{J}_{H,dist}$): This represents the direction (gradient) of the distance change relative to our position.$$\mathbf{J}_{H,dist} = \begin{bmatrix} \frac{p_1}{\sqrt{p_1^2 + p_2^2}} & \frac{p_2}{\sqrt{p_1^2 + p_2^2}} & 0 & 0 & 0 \end{bmatrix}$$
• The Kalman Gain ($\mathbf{K}_k$):$$\mathbf{K}_k = \mathbf{P}_k^- \mathbf{J}_{H,dist}^\top (\mathbf{J}_{H,dist} \mathbf{P}_k^- \mathbf{J}_{H,dist}^\top + R_{dist})^{-1}$$
• Correction:
  • State: $\hat{\mathbf{x}}_k^+ = \hat{\mathbf{x}}_k^- + \mathbf{K}_k \mathbf{y}_k$
  • Covariance: $\mathbf{P}_k^+ = (\mathbf{I} - \mathbf{K}_k \mathbf{J}_{H,dist}) \mathbf{P}_k^-$
    Insight: Notice how $\mathbf{J}_{H,dist}$ changes as the robot moves. If the robot is at $(p_1=10, p_2=0)$, the Jacobian becomes $[1, 0, 0, 0, 0]$. This tells the filter that, at this specific location, the distance measurement is only useful for correcting the $p_1$ (X) coordinate. If the robot moves to $(0, 10)$, the Jacobian automatically shifts the correction weight to $p_2$ (Y).

The "EKF Killer": Systematic Sensor Bias¶

In our previous iterations, we handled zero-mean Gaussian Noise. This is "friendly" noise because, like static on a radio, it averages out to zero over time. However, real-world Micro-Electro-Mechanical Systems (MEMS) sensors suffer from Systematic Bias: a persistent, non-zero offset that "pulls" the estimate away from reality.

The Accelerometer: The Quadratic Explosion¶

For inertial sensors, bias is catastrophic because of dead-reckoning integration. When a standard 5-state EKF receives a biased acceleration signal while the robot is actually stationary, it is forced to interpret the error as motion:

1. Velocity Drift: $v_{err} \approx \int b_a \, dt = b_a t$ (The robot "gains speed" linearly while sitting still). $b_a$ is the bias in acceleration measurement.
2. Position Divergence: $p_{err} \approx \int v_{err} \, dt = \frac{1}{2} b_a t^2$ (The estimate "explodes" quadratically away from the origin).

The Gyroscope: The "Spinning" Error¶

In the next simulation, we also introduce Gyroscope Bias ($b_\omega$). While it might seem less dangerous than an accelerometer error because it only affects the angle, its downstream impact is arguably worse:

• Heading Drift: A constant $b_\omega$ causes the estimated heading ($\theta$) to drift linearly over time ($\theta_{err} \approx b_\omega t$).
• The "EKF Killer" (Coordinate Misalignment): Even if the accelerometer were perfect, a heading error causes the EKF to rotate the body-frame acceleration into the wrong world-frame coordinates.
• Result: In reality the robot is driving straight forward, but the biased gyro makes the filter believe it is continuously rotating. It therefore keeps rotating the (correct) body-frame "forward" acceleration into a steadily wrong world-frame direction, so the estimated path curves away from the true straight line. This heading-induced drift corrupts the position estimate even while the distance sensor keeps providing updates.

A Note on Magnetometer Bias in Practice¶

While we do not model Magnetometer bias in our current simulations, it is a significant factor in real-world robotics (often called Hard-Iron Distortion). In professional applications, it is handled in one of two ways:

1. Offline Calibration: The robot is spun in circles (the "figure-eight" dance) before the mission. The resulting magnetic data is used to calculate a fixed offset that is subtracted from all future readings.
2. State Augmentation: Just as we added $b_{a}$ and $b_\omega$ (see below), an additional state can be added to the EKF to track magnetic heading offsets.

The 5-State Failure¶

Because the bias drifts due to thermal and mechanical changes, a one-time startup calibration is insufficient; the filter must adapt to the bias as it evolves. A standard 5-state EKF is "blind" to these systematic offsets. It treats every $0.1 \, \text{m/s}^2$ or $0.01 \, \text{rad/s}$ as a real physical force or rotation. Because it lacks internal states to "label" a signal as a bias, it has no choice but to corrupt the velocity and position estimates to satisfy the incoming data. This leads to the quadratic divergence and curved-path errors we observe in biased systems.

In the following simulation we add a bias for the acceleration and angular velocity measurements:

The Evolution to 8-State EKF: Defeating Sensor Bias¶

The Solution: State Augmentation¶

Professional-grade navigation filters use State Augmentation. We treat the unknown biases as "hidden" states that the filter must estimate alongside position and velocity.$$\mathbf{x}_{8-state} = [p_1, p_2, v_1, v_2, \theta, b_{a1}, b_{a2}, b_\omega]^\top$$

The EKF "learns" these biases through a process of Attribution:

• The Discrepancy: If the IMU says we are accelerating at $1.0 \, \text{m/s}^2$ but the Distance Beacon consistently reports positions that imply an acceleration of only $0.9 \, \text{m/s}^2$, the EKF calculates a persistent Innovation (the error between prediction and reality).
• The Correlation: Using the Augmented Jacobian, the EKF understands the causal link: Bias $\to$ Velocity $\to$ Position. It calculates that the most mathematically likely cause for this persistent error is not a random "glitch," but a constant offset in the sensor.
• Self-Calibration: It shifts a portion of the correction into the bias states ($b_{a1}, b_{a2}, b_\omega$). Over time, the filter "subtracts" the estimated bias from the raw IMU data in real-time, effectively "cleaning" the sensor signal before it is integrated.

Recap: Numerical Robustness: The Joseph-Form¶

As the state vector grows to 8 dimensions, the filter becomes more sensitive to floating-point rounding, which can make the standard covariance update $\mathbf{P}_k^+ = (\mathbf{I} - \mathbf{K}_k \mathbf{J}_H)\mathbf{P}_k^-$ lose symmetry or positive-definiteness (a negative variance crashes the filter). We therefore use the algebraically equivalent Joseph form

$$\mathbf P_k^+ = (\mathbf I - \mathbf K_k \mathbf J_H) \mathbf P_{k}^- (\mathbf I - \mathbf K_k \mathbf J_H)^\top + \mathbf K_k \mathbf R_k \mathbf K_k^\top,$$

whose symmetric "sandwich + positive term" structure keeps $\mathbf{P}$ symmetric positive-definite over long runs. It is derived and motivated in detail in the 2D state-system notebook; here we simply adopt it in the 8-state _apply_update.

The 8-State Discrete-Time Dynamics¶

To find the Jacobian, we first look at the state equations where $a_1^B, a_2^B, \omega$ are the raw, noisy, biased measurements from the sensors:

1. $p_{1,k} = p_{1,k-1} + v_{1,k-1}\Delta t + \frac{1}{2}\Delta t^2 \left( (a_1^B - b_{a1,k-1})\cos\theta_{k-1} - (a_2^B - b_{a2,k-1})\sin\theta_{k-1} \right)$
2. $p_{2,k} = p_{2,k-1} + v_{2,k-1}\Delta t + \frac{1}{2}\Delta t^2 \left( (a_1^B - b_{a1,k-1})\sin\theta_{k-1} + (a_2^B - b_{a2,k-1})\cos\theta_{k-1} \right)$
3. $v_{1,k} = v_{1,k-1} + \Delta t \left( (a_1^B - b_{a1,k-1})\cos\theta_{k-1} - (a_2^B - b_{a2,k-1})\sin\theta_{k-1} \right)$
4. $v_{2,k} = v_{2,k-1} + \Delta t \left( (a_1^B - b_{a1,k-1})\sin\theta_{k-1} + (a_2^B - b_{a2,k-1})\cos\theta_{k-1} \right)$
5. $\theta_k = \theta_{k-1} + (\omega - b_{\omega,k-1})\Delta t$
6. $b_{a1,k} = b_{a1,k-1}$
7. $b_{a2,k} = b_{a2,k-1}$
8. $b_{\omega,k} = b_{\omega,k-1}$

Note: How the bias is subtracted from the raw measurements.

The 8-State Jacobian ($\mathbf{J}_F$)¶

To keep the matrix readable, let:

• $c = \cos\theta$ and $s = \sin\theta$
• $\tilde{a}_1 = a_1^B - b_{a1}$ (Bias-corrected acceleration)
• $\tilde{a}_2 = a_2^B - b_{a2}$

$$\mathbf{J}_F = \begin{bmatrix} 1 & 0 & \Delta t & 0 & \frac{1}{2}\Delta t^2 (-\tilde{a}_1 s - \tilde{a}_2 c) & -\frac{1}{2}\Delta t^2 c & \frac{1}{2}\Delta t^2 s & 0 \\ 0 & 1 & 0 & \Delta t & \frac{1}{2}\Delta t^2 (\tilde{a}_1 c - \tilde{a}_2 s) & -\frac{1}{2}\Delta t^2 s & -\frac{1}{2}\Delta t^2 c & 0 \\ 0 & 0 & 1 & 0 & \Delta t (-\tilde{a}_1 s - \tilde{a}_2 c) & -\Delta t c & \Delta t s & 0 \\ 0 & 0 & 0 & 1 & \Delta t (\tilde{a}_1 c - \tilde{a}_2 s) & -\Delta t s & -\Delta t c & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & -\Delta t \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}$$

Key Improvements in this 8-State Version¶

1. The Identity Diagonal (Rows 6-8): Because the bias has a constant mean (our random-walk model adds only zero-mean noise, which is handled separately in $\mathbf{Q}$ and does not enter $\mathbf{J}_F$), the derivative of $b_k$ with respect to $b_{k-1}$ is 1 (see Modelling the bias: the random walk below).
2. The Bias Sensitivity Columns (6 & 7): These entries ($-\Delta t c$, etc.) are the "Information Bridge." They tell the EKF: "If my position prediction is wrong, it might be because I have the wrong value for $b_{a1}$ or $b_{a2}$."
3. The Gyro-Bias Sensitivity (Entry 5,8): The $-\Delta t$ at row 5, column 8 is how the EKF learns the gyroscope offset. It links the heading error directly to the gyro bias state.
4. Bias-Corrected Acceleration: Notice that in the $\theta$-derivatives (column 5), we now use $\tilde{a}$ (raw acceleration minus current bias estimate). This makes the Jacobian more accurate as the filter converges.

Modelling the bias: the random walk¶

How should the filter propagate the three bias states $b_{a1}, b_{a2}, b_\omega$? We do not know a deterministic law for how a sensor bias evolves, so we use the simplest honest model — a random walk:

$$\mathbf{b}_k = \mathbf{b}_{k-1} + \mathbf{w}^b_k,\qquad \mathbf{w}^b_k \sim \mathcal{N}\!\big(\mathbf{0},\,\mathbf{Q}^b\big).$$

This is "constant plus a small random nudge each step." It splits cleanly into the two halves of the EKF prediction:

• Mean (deterministic part) — held constant. Since $\mathbb{E}[\mathbf{w}^b_k]=\mathbf{0}$, the predicted bias is just $\hat{\mathbf{b}}_k^- = \hat{\mathbf{b}}_{k-1}$. That is why the bias rows of the state update are left unchanged and the corresponding rows of $\mathbf{J}_F$ are the identity (cell above, point 1): $\partial b_k/\partial b_{k-1}=1$.
• Covariance (stochastic part) — slowly inflating. The nudge $\mathbf{w}^b_k$ adds variance every step, so the filter's uncertainty about the bias grows between corrections. This is injected through the bias block of the process-noise matrix $\mathbf{Q}$:

$$\mathbf{Q}[5,5]=\mathbf{Q}[6,6]=\sigma_{b_a}^2\,\Delta t,\qquad \mathbf{Q}[7,7]=\sigma_{b_\omega}^2\,\Delta t.$$

Why the $\Delta t$ factor? sigma_ba, sigma_bw are continuous-time random-walk strengths (units of bias-change per $\sqrt{\text{second}}$). Discretising a continuous random walk over a step of length $\Delta t$ accumulates a variance proportional to $\Delta t$ — so a longer step lets the bias wander further. (Contrast the position/velocity block, which comes from integrating acceleration noise and therefore scales as $\Delta t^4,\Delta t^3,\Delta t^2$.)

Constant vs. drifting — and what $\sigma_b$ really controls. The "Attribution" story above treats the bias as a constant the filter discovers. The random walk is the small, deliberate relaxation of that assumption:

• Set $\sigma_b = 0$ and the bias is exactly constant — but then $\mathbf{Q}^b=\mathbf{0}$, the bias covariance only ever shrinks, and once the filter is confident it locks its bias estimate and can no longer follow a real, slowly drifting bias (thermal/mechanical changes, cf. the motivation earlier).
• Set $\sigma_b$ too large and the filter believes the bias jumps around, so it never settles and stays noisy.

So $\sigma_{b_a},\sigma_{b_\omega}$ are exactly the knob that keeps bias estimation adaptive — small enough to average out noise, large enough to track genuine drift. They are not readable from a static record (a constant bias has zero drift to measure); they are a modelling/tuning choice, set by consistency checking (NEES) in the tuning exercises.

The Result¶

With these improvements, the EKF can start with zero knowledge of the sensor's flaws. While the initial position estimate may suffer from slight drift or "jitter" as the filter determines the correct bias values, the system quickly converges. Within a few seconds of motion, the EKF accurately tracks the robot's path while simultaneously identifying exactly how much the IMU is "lying" to it. This "self-healing" capability allows the robot to maintain precision even as sensor performance shifts over time.

The Challenge of Initialization: The "Cold Start" Problem¶

While the 8-state EKF is capable of learning biases during motion, it faces a significant challenge at startup. If the robot begins its mission immediately, the "discovery phase" mentioned earlier results in a permanent initial position error. To solve this, we use a Stationary Calibration Phase, but this introduces a new mathematical hurdle: Observability.

The Problem: Geometry "Blind Spots" and Clamping¶

During a stationary phase, two distinct things can rob the filter of information:

• The Clamping Trap (a self-inflicted "Dead Zone"): A naive way to handle standing still is to hard-force the position and velocity states back to zero on every step. This is what "cuts the wire" in the Jacobian ($\mathbf{J}_F$): by overwriting the states we also destroy the bias$\to$velocity$\to$position correlations the filter had built up, so it can no longer attribute the drift to a bias. Note that standing still by itself does not break observability — the bias$\to$velocity/position entries of $\mathbf{J}_F$ depend only on $\Delta t$ and $\theta$, not on the velocity value, so they stay alive at rest. That is exactly why the ZUPT below works instead of clamping.
• The Geometry Singularity: If the robot starts at the origin $(0,0)$ and our only reference is a distance beacon at $(0,0)$, the measurement Jacobian $\mathbf{J}_H = [\,p_1/r,\; p_2/r,\; 0,\dots]$ becomes undefined at the origin (the $0/0$ indeterminate form, since $r=\sqrt{p_1^2+p_2^2}\to 0$). The filter sees a distance error but has no idea which direction to "pull" the states to fix it. (In the code this is guarded by skipping the distance update when $r<10^{-6}$.)

Potential Remedies¶

In professional navigation, there are three common ways to break this "Dead Zone":

1. Motion "Nudging": Moving the robot in a small circle or "jiggle" to make the biases observable through change.

2. State Resets: Letting the EKF drift wildly for 5 seconds to learn the bias, then manually "snapping" the position and velocity back to zero.

3. Virtual Constraints (ZUPT): Using "fake" measurements to tell the math what the physical reality is.

Our Solution: The Zero Velocity Update (ZUPT)¶

We implement the Zero Velocity Update (ZUPT). This is the industry-standard method for calibrating inertial systems while stationary. Instead of forcing the states to zero in the prediction step, we let the EKF physics run normally but apply a virtual sensor update.

How the ZUPT works in our EKF:

1. The Prediction: During the first 5 seconds, the EKF integrates the raw (biased) IMU data. It predicts that the robot should be moving slightly because of the sensor bias.
2. The Virtual Measurement: We tell the EKF: "I am a virtual sensor, and I promise you the velocity is exactly $0.0 \, \text{m/s}$."
3. The Innovation: The EKF now has a contradiction: the IMU says "we are moving," but the ZUPT says "we are still."
4. The Correction: Using the 8-state Jacobian, the EKF resolves this contradiction by adjusting the accelerometer bias states $b_{a1}, b_{a2}$ — a constant accelerometer offset is the only explanation consistent with both "the IMU integrates to a velocity" and "the velocity is truly zero."

What about the gyro bias? A zero-velocity constraint carries no heading information, so ZUPT on its own cannot observe the gyro bias $b_\omega$. In our setup this bias still gets calibrated because the magnetometer is also active during the stationary window (see patch_data_with_calibration): it anchors the absolute heading $\theta$, and a persistent heading drift is what makes $b_\omega$ observable. So the clean start comes from ZUPT (accelerometer biases) and the magnetometer (heading $\to$ gyro bias) working together.

The Result of ZUPT Calibration¶

By the time the 5-second calibration window closes, the EKF has already "dialed in" the accelerometer and gyroscope offsets. When the robot finally begins its mission, it starts with:

• Clean Data: The biases are already subtracted from the raw IMU stream.
• Stable Covariance: The uncertainty ($P$ matrix) for the biases has shrunk, meaning the filter is confident in its calibration.
• Zero Initial Drift: The robot begins its trajectory from a "perfect" $(0,0)$ state without the quadratic explosion that plagues uncalibrated systems.

Comparison: EKF Navigation Strategies¶

The cell above prints the concrete position RMSE for each strategy on the biased dataset, quantifying the qualitative "Position Error" row below: the 5-state filter diverges, the 8-state filter recovers once the bias converges, and adding ZUPT calibration gives the lowest error from the very first moving step.

* The covariance-update form (standard vs. Joseph) is an independent numerical-stability choice, not a property of the state dimension — any of these filters could use either. We use the standard form in the 5-state demo and the Joseph form in the 8-state class purely as an implementation choice; the Joseph form is simply the safer default for long runs because it preserves symmetry and positive-definiteness.