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.
- Magnetometer Update: The filter sees $\mathbf{J}_{H, mag}$ and realizes the measurement only provides information about $\theta$. It "corrects" the heading directly.
- 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:
- Prediction (where we use our physics model and IMU)
- 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$
Computing the Process Noise ($\mathbf{Q}_k$) in Practice¶
In the EKF, the process noise $\mathbf{Q}_k$ isn't a static value; it is the result of mapping raw sensor noise into the world-frame state. There are two primary ways to compute the term $\mathbf{J}_G \mathbf{Q}_u \mathbf{J}_G^\top$.
Method A: The Jacobian Mapping (Exact)
We define $\mathbf{Q}_u$ as the diagonal covariance of our raw IMU inputs (e.g. the "datasheet" values or calculated from a controlled experiment):
$$\mathbf{Q}_u = \text{diag}(\sigma_{a1}^2, \sigma_{a2}^2, \sigma_\omega^2)$$
We then multiply this by the Control Jacobian $\mathbf{J}_G$ (the partial derivatives of $\mathbf{f}$ with respect to the inputs $a_1^B, a_2^B, \omega$). This mathematically "rotates" the accelerometer noise into the world frame based on the robot's current $\theta$.
Method B: The Discrete Approximation (Recommended)
In practice, the most robust way to implement $\mathbf{Q}_k$ is using a discrete White Noise Model.
- Isotropic assumption: It assumes noise is equal in all directions. This makes it more stable than Method A (Jacobian Mapping), particularly if the $\theta$ estimate is temporarily poor or jumping.
- Integration Approximation: It approximates the integration of noise over the interval $\Delta t$. Assuming constant acceleration noise $\sigma_a^2$ and gyroscope noise $\sigma_\omega^2$, the block-diagonal matrix becomes:
$$\mathbf{Q}_k = \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}$$
Methods A and B are the same matrix (for isotropic accelerometer noise). This is not a coincidence. Put $\mathbf{Q}_u = \mathrm{diag}(\sigma_a^2, \sigma_a^2, \sigma_\omega^2)$ (equal noise on both accelerometer axes) and carry out Method A explicitly — the heading $\theta$ cancels completely and you recover exactly the matrix above:
$$\mathbf{J}_G\,\mathbf{Q}_u\,\mathbf{J}_G^\top = \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}$$
The $\cos\theta/\sin\theta$ terms drop out because an isotropic noise disk is rotation-invariant ($\mathbf{R}\,\sigma_a^2\mathbf{I}\,\mathbf{R}^\top = \sigma_a^2\mathbf{I}$). So Method B is just Method A under the isotropy assumption — it skips the (now redundant) rotation.
Notice too that each translational $(p,v)$ block,
$$\begin{bmatrix} \tfrac{1}{4}\Delta t^4 & \tfrac{1}{2}\Delta t^3 \\ \tfrac{1}{2}\Delta t^3 & \Delta t^2 \end{bmatrix}\sigma_a^2,$$
is exactly the 1D process-noise matrix $\mathbf{Q} = \mathbf{G}\,\sigma_a^2\,\mathbf{G}^\top$ from process-noise.ipynb, applied independently to the X and Y axes — with the gyro term $\Delta t^2\sigma_\omega^2$ appended for the heading state.
Why this structure matters:
- The Velocity-Position Link: Because $p \approx v\Delta t + \frac{1}{2}a\Delta t^2$, any uncertainty in acceleration impacts velocity by $\Delta t$ and position by $\Delta t^2$. When squaring these for a covariance matrix, we get the $\Delta t^2, \Delta t^3,$ and $\Delta t^4$ terms. A noisy accelerometer creates uncertainty in velocity that "leaks" into an even larger uncertainty in position over time.
- The Cross-Terms (Off-Diagonals): These represent the cross-correlation created by integration. Since position is the mathematical integral of velocity, their errors are tethered—if your velocity estimate is overshooting, your position estimate likely is too.
- Tuning: In a real-world deployment, $\sigma_a$ and $\sigma_\omega$ serve as your primary "tuning knobs." Increasing these values tells the filter to trust your IMU less and rely more heavily on external corrections, such as the Distance Beacon or 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 $0/360^\circ$ boundary.
- 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).
import numpy as np
from ipynb.fs.defs.elipse_data_generation import generate_ellipse_data, generate_measurements
from ipynb.fs.defs.plot_two_d_navigation import plot_ekf_results, plot_bias_estimation
# Fix the RNG seed so the synthetic data, measurements and plots are reproducible
# across runs. Re-run the data-generation cells top-to-bottom for identical results.
np.random.seed(0)
class RobotEKF:
def __init__(self, x0, P0, sigma_a, sigma_w):
"""
Optimal EKF for 2D Navigation.
x0: Initial state [p1, p2, v1, v2, theta]
P0: Initial covariance
sigma_a: Accelerometer noise standard deviation (m/s^2)
sigma_w: Gyroscope noise standard deviation (rad/s)
"""
self.x = x0.astype(float)
self.P = P0.astype(float)
self.sigma_a = sigma_a
self.sigma_w = sigma_w
def normalize_angle(self, angle):
"""Wraps angle to [-pi, pi]"""
return (angle + np.pi) % (2 * np.pi) - np.pi
def predict(self, u, dt):
"""Nonlinear Prediction with Rigorous Noise Mapping"""
a1_b, a2_b, omega = u
p1, p2, v1, v2, theta = self.x
cos_t = np.cos(theta)
sin_t = np.sin(theta)
# 1. State Prediction (Nonlinear f)
# Rotate body-frame accelerations to world frame
a1_w = a1_b * cos_t - a2_b * sin_t
a2_w = a1_b * sin_t + a2_b * cos_t
self.x[0] = p1 + v1 * dt + 0.5 * a1_w * dt**2
self.x[1] = p2 + v2 * dt + 0.5 * a2_w * dt**2
self.x[2] = v1 + a1_w * dt
self.x[3] = v2 + a2_w * dt
self.x[4] = self.normalize_angle(theta + omega * dt)
# 2. State Jacobian (J_F = df/dx)
J_F = np.eye(5)
J_F[0, 2], J_F[1, 3] = dt, dt
# Derivatives with respect to theta
J_F[0, 4] = 0.5 * dt**2 * (-a1_b * sin_t - a2_b * cos_t)
J_F[1, 4] = 0.5 * dt**2 * ( a1_b * cos_t - a2_b * sin_t)
J_F[2, 4] = dt * (-a1_b * sin_t - a2_b * cos_t)
J_F[3, 4] = dt * ( a1_b * cos_t - a2_b * sin_t)
self.J_F = J_F # stored so it can be inspected / unit-tested
# 3. Process Noise (Q) - Discrete White Noise Model
# This captures the correlation between velocity and position uncertainty
s_a2 = self.sigma_a**2
s_w2 = self.sigma_w**2
# We build the 2D blocks for X and Y, then add rotation
Q = np.zeros((5, 5))
# X-Y Translation blocks
for i in [0, 1]: # 0 for p1/v1, 1 for p2/v2
p_idx, v_idx = i, i + 2
Q[p_idx, p_idx] = 0.25 * dt**4 * s_a2
Q[v_idx, v_idx] = dt**2 * s_a2
Q[p_idx, v_idx] = Q[v_idx, p_idx] = 0.5 * dt**3 * s_a2
# Rotation block
Q[4, 4] = dt**2 * s_w2
# 4. Uncertainty Propagation
self.P = J_F @ self.P @ J_F.T + Q
def update_magnetometer(self, z_theta, std_mag):
"""Linear Update for Absolute Heading"""
J_H = np.array([[0, 0, 0, 0, 1]])
R = np.array([[std_mag**2]])
# Innovation must be normalized
y = self.normalize_angle(z_theta - self.x[4])
self._apply_update(y, J_H, R)
def update_distance(self, z_dist, std_dist):
"""Nonlinear Update for Distance to Origin (0,0)"""
p1, p2 = self.x[0], self.x[1]
dist_pred = np.sqrt(p1**2 + p2**2)
# Safety check for origin singularity
if dist_pred < 1e-6:
return
J_H = np.array([[p1/dist_pred, p2/dist_pred, 0, 0, 0]])
R = np.array([[std_dist**2]])
y = z_dist - dist_pred
self._apply_update(y, J_H, R)
def _apply_update(self, y, J_H, R):
"""Centralized Kalman Correction Step"""
# Innovation Covariance
S = J_H @ self.P @ J_H.T + R
# Kalman Gain
K = self.P @ J_H.T @ np.linalg.inv(S)
# State Update (force 1D array to match x)
self.x = self.x + (K @ np.atleast_1d(y)).flatten()
# Keep the heading state wrapped to [-pi, pi] after the correction
self.x[4] = self.normalize_angle(self.x[4])
# Covariance Update (standard form; see the Joseph form in the 8-state filter)
self.P = (np.eye(5) - K @ J_H) @ self.P
# --- Sanity check: verify the analytic state Jacobian J_f against finite differences ---
# A hand-derived Jacobian is easy to get wrong. We numerically differentiate the
# prediction's state-transition and confirm it matches the analytic J_f stored above.
def finite_diff_jac(fn, z, eps=1e-6):
"""Central-difference Jacobian of a vector function fn at point z."""
z = np.asarray(z, float)
f0 = fn(z)
J = np.zeros((f0.size, z.size))
for i in range(z.size):
d = np.zeros_like(z); d[i] = eps
J[:, i] = (fn(z + d) - fn(z - d)) / (2 * eps)
return J
def _mean_after_predict_5(x, u, dt):
e = RobotEKF(np.array(x, float), np.eye(5), sigma_a=0.1, sigma_w=0.1)
e.predict(np.asarray(u, float), dt)
return e.x.copy()
_x = np.array([1.0, -2.0, 0.5, 0.3, np.radians(40)])
_u = np.array([0.4, -0.25, 0.15])
_dt = 0.05
_ekf = RobotEKF(_x.copy(), np.eye(5), 0.1, 0.1)
_ekf.predict(_u, _dt)
_err = np.max(np.abs(_ekf.J_F - finite_diff_jac(lambda z: _mean_after_predict_5(z, _u, _dt), _x)))
assert _err < 1e-5, f"5-state J_f disagrees with finite differences (max err {_err:.2e})"
print(f"5-state J_f matches finite differences (max abs err {_err:.2e}).")
def run_ekf_simulation(t, u_data, measurements,
x0, P0,
sigma_a, sigma_w,
std_mag, std_dist):
"""
Runs the EKF loop over a set of IMU data and sparse measurements.
Args:
t: Time vector
u_data: (N, 3) array of [a1_b, a2_b, omega]
measurements: List of dicts from generate_measurements()
x0, P0: Initial state and covariance
sigma_a, sigma_w: Process noise standard deviations
std_mag, std_dist: Measurement noise standard deviations
Returns:
x_estimates: (N, 5) array of state estimates
P_estimates: (N, 5, 5) array of covariance matrices
"""
n_steps = len(t)
dt = t[1] - t[0] if n_steps > 1 else 0.01
# 1. Initialize the EKF object (Best of Both version)
ekf = RobotEKF(x0, P0, sigma_a, sigma_w)
# 2. Storage for results
x_estimates = np.zeros((n_steps, 5))
P_estimates = np.zeros((n_steps, 5, 5))
meas_idx = 0
num_meas = len(measurements)
# 3. Main Loop
for i in range(n_steps):
# --- PREDICTION STEP ---
# High-frequency: Every time step we integrate the IMU
ekf.predict(u_data[i], dt)
# --- UPDATE STEP ---
# Sparse/Asynchronous: Only update when a sensor "event" occurs
if meas_idx < num_meas and measurements[meas_idx]['t_idx'] == i:
packet = measurements[meas_idx]
# Update Magnetometer (Linear heading update)
if 'mag' in packet:
ekf.update_magnetometer(packet['mag'], std_mag)
# Update Distance Beacon (Nonlinear radial update)
if 'dist' in packet:
ekf.update_distance(packet['dist'], std_dist)
meas_idx += 1
# 4. Save Current State (use .copy() to avoid pointer issues)
x_estimates[i] = ekf.x.copy()
P_estimates[i] = ekf.P.copy()
return x_estimates, P_estimates
# EFK noise
sigma_a_efk=0.2
sigma_w_efk=0.07
std_mag_efk=0.07
std_dist_efk=0.5
# measurements freq and noise
freq_mag=2
freq_dist=3
std_mag_true=0.07
std_dist_true=0.5
# --- 1. Generate Synthetic Data ---
# Ground truth trajectory and noisy IMU inputs
t, x_gt, u_noisy = generate_ellipse_data(
duration=10.0,
dt=0.01,
a=5.5,
b=3.0
)
# Sparse, noisy sensor measurements (Magnetometer and Distance Beacon)
measurements = generate_measurements(
x_gt,
freq_mag=freq_mag,
freq_dist=freq_dist,
std_mag=std_mag_true,
std_dist=std_dist_true
)
# --- 2. Run Simulation ---
# Initialize with Ground Truth at t=0
x0 = x_gt[0]
P0 = np.eye(5) * 0.1
x_estimates, P_estimates = run_ekf_simulation(
t,
u_noisy,
measurements,
x0,
P0,
sigma_a_efk,
sigma_w_efk,
std_mag_efk,
std_dist_efk
)
# --- 3. Compare Results ---
# This will now display the trajectory, error plots, and uncertainty bounds
plot_ekf_results(t, x_gt, x_estimates, P_estimates)
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:
- 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.
- 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: The robot thinks it is accelerating "Forward," but it is actually accelerating "Forward and slightly Left." This creates a lateral drift that ruins position accuracy even if the distance sensor is 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:
- 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.
- 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:
def apply_sensor_biases(u_noisy, acc_bias, omega_bias):
"""
Simulates systematic sensor errors (biases).
NOTE: These are 'Fixed-Sensor' biases. Because they are
added in the Body-Frame, they represent offsets that rotate WITH
the robot.
"""
u_biased = np.copy(u_noisy)
# Apply constant offsets
u_biased[:, 0] += acc_bias[0]
u_biased[:, 1] += acc_bias[1]
u_biased[:, 2] += omega_bias
return u_biased
# true sensor bias
acc_bias=[-.6, .62]
omega_bias = 0.55
t, x_gt, u_noisy = generate_ellipse_data()
measurements = generate_measurements(x_gt)
u_biased = apply_sensor_biases(u_noisy, acc_bias, omega_bias) # Inject the EKF killer!
# Recompute the initial state/covariance from the (re)generated ground truth so they
# never go stale if the generation parameters above are changed.
x0 = x_gt[0]
P0 = np.eye(5) * 0.1
x_estimates, P_estimates = run_ekf_simulation(
t,
u_biased,
measurements,
x0,
P0,
sigma_a_efk, # Tuning: slightly higher than ground truth noise
sigma_w_efk, # Tuning: slightly higher than ground truth noise
std_mag_efk,
std_dist_efk
)
# Compare Results
plot_ekf_results(t, x_gt, x_estimates, P_estimates)
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.
Numerical Robustness: The Joseph-Form¶
As the state vector grows to 8 dimensions, the EKF becomes more sensitive to numerical instabilities. The standard covariance update ($\mathbf P_k^+ = (\mathbf I - \mathbf K_k \mathbf H_k)\mathbf P^-_k$) is prone to asymmetry and "loss of positive-definiteness" due to floating-point rounding errors. If the diagonal of $P$ (the variance) ever becomes negative, the filter crashes.
To prevent this, we implement the Joseph-Form Covariance Update:
$$\mathbf P_k^+ = (\mathbf I - \mathbf K_k \mathbf H_k) \mathbf P_{k}^- (\mathbf I - \mathbf K_k \mathbf H_k)^\top + \mathbf K_k \mathbf R_k \mathbf K_k^\top$$
Mathematically, this is identical to the standard covariance update.
Why it is superior:
- Guaranteed Symmetry: It is structured as a "sandwich" form ($\mathbf A \mathbf P \mathbf A^\top + \mathbf B$). Mathematically, any matrix in the form $\mathbf X \mathbf Y \mathbf X^\top$ is symmetric if $\mathbf Y$ is symmetric. This prevents the EKF from "leaning" to one side due to rounding errors.
- Positive Definiteness: The $\mathbf K \mathbf R \mathbf K^\top$ term acts as a "safety floor." Since the measurement noise $\mathbf R$ is always positive, this term ensures that the uncertainty in the filter never collapses below a physical reality.
- Stability: This form is the benchmark for long-running aerospace and robotics navigation filters where numerical stability over thousands of iterations is non-negotiable.
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:
- $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)$
- $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)$
- $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)$
- $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)$
- $\theta_k = \theta_{k-1} + (\omega - b_{\omega,k-1})\Delta t$
- $b_{a1,k} = b_{a1,k-1}$
- $b_{a2,k} = b_{a2,k-1}$
- $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¶
- The Identity Diagonal (Rows 6-8): Because we assume the bias is constant (or changes very slowly as a random walk), the derivative of $b_k$ with respect to $b_{k-1}$ is 1.
- 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}$."
- 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.
- 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.
class RobotEKF8State:
def __init__(self, x0, P0, sigma_a, sigma_w, sigma_ba, sigma_bw):
"""
State x: [p1, p2, v1, v2, theta, ba1, ba2, bw]
sigma_a/w: IMU measurement noise
sigma_ba/bw: Bias stability (Random Walk)
"""
# Ensure x0 is 8-dim
if len(x0) == 5:
self.x = np.hstack([x0, [0.0, 0.0, 0.0]]).astype(float)
else:
self.x = x0.astype(float)
self.P = P0.astype(float)
# Noise parameters
self.s_a = sigma_a
self.s_w = sigma_w
self.s_ba = sigma_ba
self.s_bw = sigma_bw
def normalize_angle(self, angle):
return (angle + np.pi) % (2 * np.pi) - np.pi
def predict(self, u_raw, dt):
p1, p2, v1, v2, theta, ba1, ba2, bw = self.x
a1_raw, a2_raw, w_raw = u_raw
# 1. Correct measurements using current bias estimates
a1_c = a1_raw - ba1
a2_c = a2_raw - ba2
w_c = w_raw - bw
cos_t, sin_t = np.cos(theta), np.sin(theta)
# Accelerations rotated to world frame
a1_w = a1_c * cos_t - a2_c * sin_t
a2_w = a1_c * sin_t + a2_c * cos_t
# 2. Nonlinear State Transition
self.x[0] += v1 * dt + 0.5 * a1_w * dt**2
self.x[1] += v2 * dt + 0.5 * a2_w * dt**2
self.x[2] += a1_w * dt
self.x[3] += a2_w * dt
self.x[4] = self.normalize_angle(theta + w_c * dt)
# 3. Rigorous Jacobian J_F (8x8)
J_F = np.eye(8)
J_F[0, 2], J_F[1, 3] = dt, dt
J_F[0, 4] = 0.5 * dt**2 * (-a1_c * sin_t - a2_c * cos_t)
J_F[1, 4] = 0.5 * dt**2 * ( a1_c * cos_t - a2_c * sin_t)
J_F[2, 4] = dt * (-a1_c * sin_t - a2_c * cos_t)
J_F[3, 4] = dt * ( a1_c * cos_t - a2_c * sin_t)
J_F[0, 5], J_F[0, 6] = -0.5 * dt**2 * cos_t, 0.5 * dt**2 * sin_t
J_F[1, 5], J_F[1, 6] = -0.5 * dt**2 * sin_t, -0.5 * dt**2 * cos_t
J_F[2, 5], J_F[2, 6] = -dt * cos_t, dt * sin_t
J_F[3, 5], J_F[3, 6] = -dt * sin_t, -dt * cos_t
J_F[4, 7] = -dt
self.J_F = J_F # stored so it can be inspected / unit-tested
# 4. Rigorous Process Noise Q (8x8)
# Fix: Separate the time-power variables clearly
dt2 = dt**2
dt3_05 = 0.5 * dt**3
dt4_025 = 0.25 * dt**4
Q = np.zeros((8, 8))
# Translation block: Position/Velocity correlations
for i in [0, 1]:
Q[i, i] = dt4_025 * self.s_a**2
Q[i+2, i+2] = dt2 * self.s_a**2
Q[i, i+2] = Q[i+2, i] = dt3_05 * self.s_a**2
Q[4, 4] = dt**2 * self.s_w**2
# Bias Random Walk
Q[5, 5] = Q[6, 6] = dt * self.s_ba**2
Q[7, 7] = dt * self.s_bw**2
self.P = J_F @ self.P @ J_F.T + Q
def _apply_update(self, y, J_H, R):
"""Universal Joseph-Form Update (Ensures P stays positive-definite)"""
S = J_H @ self.P @ J_H.T + R
K = self.P @ J_H.T @ np.linalg.inv(S)
# State Update
self.x = self.x + (K @ np.atleast_1d(y)).flatten()
# Keep the heading state wrapped to [-pi, pi] after the correction
self.x[4] = self.normalize_angle(self.x[4])
# Joseph Form Covariance Update: P = (I-KH)P(I-KH)' + KRK'
I_KH = np.eye(8) - K @ J_H
self.P = I_KH @ self.P @ I_KH.T + K @ R @ K.T
def update_magnetometer(self, z_theta, std_mag):
J_H = np.zeros((1, 8))
J_H[0, 4] = 1
y = self.normalize_angle(z_theta - self.x[4])
self._apply_update(y, J_H, np.array([[std_mag**2]]))
def update_distance(self, z_dist, std_dist):
p1, p2 = self.x[0], self.x[1]
dist_pred = np.sqrt(p1**2 + p2**2)
if dist_pred < 1e-6: return
J_H = np.zeros((1, 8))
J_H[0, 0], J_H[0, 1] = p1/dist_pred, p2/dist_pred
y = z_dist - dist_pred
self._apply_update(y, J_H, np.array([[std_dist**2]]))
# --- Sanity check: 8-state state Jacobian and the nonlinear distance Jacobian ---
# Reuses finite_diff_jac defined for the 5-state filter above.
def _mean_after_predict_8(x, u, dt):
e = RobotEKF8State(np.array(x, float), np.eye(8), 0.1, 0.1, 0.01, 0.01)
e.predict(np.asarray(u, float), dt)
return e.x.copy()
_x8 = np.array([1.0, -2.0, 0.5, 0.3, np.radians(40), 0.1, -0.05, 0.02])
_u8 = np.array([0.4, -0.25, 0.15])
_dt = 0.05
_ekf8 = RobotEKF8State(_x8.copy(), np.eye(8), 0.1, 0.1, 0.01, 0.01)
_ekf8.predict(_u8, _dt)
_err_f = np.max(np.abs(_ekf8.J_F - finite_diff_jac(lambda z: _mean_after_predict_8(z, _u8, _dt), _x8)))
assert _err_f < 1e-5, f"8-state J_f disagrees with finite differences (max err {_err_f:.2e})"
# Distance measurement Jacobian J_h at a sample position (3, 4) -> distance 5
_p = np.array([3.0, 4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
_d = np.hypot(_p[0], _p[1])
_J_H = np.array([[_p[0]/_d, _p[1]/_d, 0, 0, 0, 0, 0, 0]])
_J_H_num = finite_diff_jac(lambda z: np.array([np.hypot(z[0], z[1])]), _p)
_err_h = np.max(np.abs(_J_H - _J_H_num))
assert _err_h < 1e-5, f"distance J_h disagrees with finite differences (max err {_err_h:.2e})"
print(f"8-state J_f matches finite differences (max abs err {_err_f:.2e}).")
print(f"distance J_h matches finite differences (max abs err {_err_h:.2e}).")
def run_ekf_simulation_bias_states(t, u_data, measurements, x0_5state, P0_5state,
P0_bias,
sigma_a, sigma_w, sigma_ba, sigma_bw,
std_mag, std_dist,
calibration=False, calib_duration=5.0):
"""
Runs a professional 8-state EKF that estimates [p1, p2, v1, v2, theta, ba1, ba2, bw].
Args:
sigma_a, sigma_w: IMU measurement noise (std dev)
sigma_ba, sigma_bw: Bias "Random Walk" stability (std dev)
std_mag, std_dist: Sensor noise (std dev)
calibration: if True, apply a Zero-Velocity Update (ZUPT) while stationary
calib_duration: length of the stationary ZUPT window in seconds
(must match the calibration period prepended to the data)
"""
n_steps = len(t)
dt = t[1] - t[0] if n_steps > 1 else 0.01
# 1. Augment Initial State: [x, y, vx, vy, theta, ba1, ba2, bw]
x0_8 = np.zeros(8)
x0_8[:5] = x0_5state
# Biases (ba1, ba2, bw) initialized to 0.0
# 2. Augment Initial Covariance
P0_8 = np.eye(8)
P0_8[:5, :5] = P0_5state
# Setting initial bias uncertainty higher allows the filter to learn biases faster
P0_8[5:, 5:] = P0_bias
# 3. Initialize the Professional Class
# Note: We pass standard deviations; the class computes Q internally
ekf = RobotEKF8State(x0_8, P0_8, sigma_a, sigma_w, sigma_ba, sigma_bw)
x_estimates = np.zeros((n_steps, 8))
P_estimates = np.zeros((n_steps, 8, 8))
meas_idx = 0
num_meas = len(measurements)
# 4. Main Loop
for i in range(n_steps):
# --- PREDICTION ---
ekf.predict(u_data[i], dt)
# --- UPDATE ---
if meas_idx < num_meas and measurements[meas_idx]['t_idx'] == i:
packet = measurements[meas_idx]
if 'mag' in packet:
ekf.update_magnetometer(packet['mag'], std_mag)
if 'dist' in packet:
ekf.update_distance(packet['dist'], std_dist)
meas_idx += 1
if calibration and t[i] < calib_duration:
# "Virtual" Measurement: "I promise you, velocity is zero"
z_v = np.array([0.0, 0.0])
J_H_v = np.zeros((2, 8))
J_H_v[0, 2] = 1 # v1
J_H_v[1, 3] = 1 # v2
R_v = np.eye(2) * 0.000001 # Very high confidence
ekf._apply_update(z_v - ekf.x[2:4], J_H_v, R_v)
# 5. Store Results
x_estimates[i] = ekf.x.copy()
P_estimates[i] = ekf.P.copy()
return x_estimates, P_estimates
# 1. Run the Augmented 8-State Simulation
# We pass the biased IMU data and the sparse measurements
x_est_8, P_est_8 = run_ekf_simulation_bias_states(
t,
u_biased, # The data with acc_bias and omega_bias
measurements,
x0_5state = x_gt[0], # Starting position/heading from GT
P0_5state = np.eye(5) * 0.0001,
P0_bias = np.eye(3) * 1.,
sigma_a = sigma_a_efk, # Expected accelerometer noise std
sigma_w = sigma_w_efk, # Expected gyro noise std
sigma_ba = 0.01, # Stability of acc bias (how fast it changes)
sigma_bw = 0.01, # Stability of gyro bias
std_mag = std_mag_efk, # Magnetometer noise std
std_dist = std_dist_efk # Distance beacon noise std
)
# 2. Compare Trajectory & Errors
# We slice [:, :5] because the plotting function only expects [p1, p2, v1, v2, theta]
plot_ekf_results(
t,
x_gt,
x_est = x_est_8[:, :5],
P_est = P_est_8[:, :5, :5]
)
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.
# Final Visualization of the Bias States
plot_bias_estimation(
t,
x_est_8,
P_est_8, # Including P allows for the uncertainty shading
true_acc_bias=acc_bias,
true_omega_bias=omega_bias
)
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: Zero Jacobians and Geometry "Blind Spots"¶
During a stationary phase, we encounter a "Dead Zone" in the filter's mathematics:
- The Velocity Link: If the robot isn't moving, the velocity is zero. In the prediction step, if we mathematically force velocity and position to stay at zero, we "cut the wire" in the Jacobian ($J_f$). The filter no longer sees how a change in bias would lead to a change in position.
- 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 ($H$) becomes zero. Mathematically, the derivative of distance at the origin is undefined ($0/0$). The filter sees a distance error but has no idea which direction to "pull" the states to fix it.
Potential Remedies¶
In professional navigation, there are three common ways to break this "Dead Zone":
Motion "Nudging": Moving the robot in a small circle or "jiggle" to make the biases observable through change.
State Resets: Letting the EKF drift wildly for 5 seconds to learn the bias, then manually "snapping" the position and velocity back to zero.
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:
- 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.
- 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}$."
- The Innovation: The EKF now has a contradiction: the IMU says "we are moving," but the ZUPT says "we are still."
- The Correction: Because we use the 8-state Jacobian, the EKF looks at this contradiction and realizes the only way to satisfy both the IMU and the ZUPT is to adjust the Bias States.
def patch_data_with_calibration(t_orig, x_gt_orig,
u_biased_orig,
measurements_orig,
std_a,
std_w,
acc_bias,
omega_bias,
std_mag_true,
std_dist_true,
calib_duration=5.0
):
"""
Prepends a stationary period to existing BIASED data.
"""
dt = t_orig[1] - t_orig[0]
n_cal = int(calib_duration / dt)
t_cal = np.arange(0, calib_duration, dt)
# Create stationary Ground Truth
start_state = x_gt_orig[0].copy()
x_gt_cal = np.tile(start_state, (n_cal, 1))
x_gt_cal[:, 2:4] = 0.0 # Velocity is zero
u_biased_cal = np.zeros((n_cal, 3))
# Add noise + the EXACT same bias used in u_biased_orig
u_biased_cal[:, 0] = np.random.normal(0, std_a, n_cal) + acc_bias[0]
u_biased_cal[:, 1] = np.random.normal(0, std_a, n_cal) + acc_bias[1]
u_biased_cal[:, 2] = np.random.normal(0, std_w, n_cal) + omega_bias
# Concatenate
t_patched = np.concatenate([t_cal, t_orig + calib_duration])
x_gt_patched = np.vstack([x_gt_cal, x_gt_orig])
u_biased_total = np.vstack([u_biased_cal, u_biased_orig])
# 4. Patch Measurements
patched_measurements = []
# Shift motion measurements
for m in measurements_orig:
m_new = m.copy()
m_new['t_idx'] += n_cal
patched_measurements.append(m_new)
# Add stationary measurements
d_start = np.sqrt(start_state[0]**2 + start_state[1]**2)
for i in range(0, n_cal, int(0.2/dt)):
patched_measurements.append({
't_idx': i,
'dist': d_start + np.random.normal(0, std_dist_true),
'mag': start_state[4] + np.random.normal(0, std_mag_true)
})
patched_measurements = sorted(patched_measurements, key=lambda x: x['t_idx'])
return t_patched, x_gt_patched, u_biased_total, patched_measurements
# true sensor / process noise
noise_std_acc_true=0.2
noise_std_omega_true=0.07
t_cal, x_gt_cal, u_biased_cal, measurements_cal = patch_data_with_calibration(
t, x_gt, u_biased, measurements,
noise_std_acc_true, # Maps to std_a
noise_std_omega_true, # Maps to std_w
acc_bias=acc_bias,
omega_bias=omega_bias, # Added missing comma
std_mag_true=std_mag_true, # Added missing argument
std_dist_true=std_dist_true, # Added missing argument
calib_duration=5.0
)
x_est_8_cal, P_est_8_cal = run_ekf_simulation_bias_states(
t_cal,
u_biased_cal, # The data with acc_bias and omega_bias
measurements_cal,
x0_5state = x_gt[0], # Starting position/heading from GT
P0_5state = np.eye(5) * 0.001,
P0_bias = np.eye(3) * 1.,
sigma_a = sigma_a_efk, # Expected accelerometer noise std
sigma_w = sigma_w_efk, # Expected gyro noise std
sigma_ba = 0.01, # Stability of acc bias (how fast it changes)
sigma_bw = 0.01, # Stability of gyro bias
std_mag = std_mag_efk, # Magnetometer noise std
std_dist = std_dist_efk, # Distance beacon noise std
calibration = True,
calib_duration = 5.0 # Must match calib_duration used in patch_data_with_calibration
)
# 2. Compare Trajectory & Errors
# We slice [:, :5] because the plotting function only expects [p1, p2, v1, v2, theta]
plot_ekf_results(
t_cal,
x_gt_cal,
x_est = x_est_8_cal[:, :5],
P_est = P_est_8_cal[:, :5, :5]
)
# Final Visualization of the Bias States
plot_bias_estimation(
t_cal,
x_est_8_cal,
P_est_8_cal, # Including P allows for the uncertainty shading
true_acc_bias=acc_bias,
true_omega_bias=omega_bias
)
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.
# --- Quantitative comparison: position RMSE of the three strategies ---
# RMSE here is the root-mean-square of the Euclidean position error over the run.
def position_rmse(x_est, x_gt):
err = x_est[:, :2] - x_gt[:, :2]
return np.sqrt(np.mean(np.sum(err**2, axis=1)))
print("Position RMSE (Euclidean, metres) on the biased dataset")
print(f" 5-State EKF (no bias estimation) : {position_rmse(x_estimates, x_gt):.3f}")
print(f" 8-State EKF (augmented) : {position_rmse(x_est_8, x_gt):.3f}")
# Score the ZUPT run only over the moving phase (after the stationary window),
# so all three numbers describe accuracy during actual navigation.
_move = t_cal >= 5.0
print(f" 8-State EKF + ZUPT (motion only) : "
f"{position_rmse(x_est_8_cal[_move], x_gt_cal[_move]):.3f}")
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.
| Feature | 5-State EKF (Basic) | 8-State EKF (Augmented) | 8-State EKF + ZUPT |
|---|---|---|---|
| State Vector | $[p_1, p_2, v_1, v_2, \theta]$ | $[p_1, p_2, v_1, v_2, \theta, b_{a1}, b_{a2}, b_\omega]$ | $[p_1, p_2, v_1, v_2, \theta, b_{a1}, b_{a2}, b_\omega]$ |
| Noise Handling | Zero-mean Gaussian only | Systematic Bias + Gaussian | Systematic Bias + Gaussian |
| Bias Estimation | None (Assumes $b=0$) | Estimated during motion | Estimated while stationary |
| Startup Behavior | Immediate drift | Initial "discovery" drift | Clean, calibrated start |
| Position Error | Quadratic Explosion ($t^2$) | Recovers after convergence | Minimum / Optimal |
| Numerical Method | Standard Form | Joseph-Form (Robust) | Joseph-Form (Robust) |
| Observability | N/A | Requires motion to see bias | Forces observability via ZUPT |