EFK 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}$$The Relationship: This 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. Predition

The Nonlinear Transition ($\mathbf{f}$): 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 + v_1 \Delta t + \frac{1}{2}\Delta t^2 (a_1^B \cos\theta - a_2^B \sin\theta) \\ p_2 + v_2 \Delta t + \frac{1}{2}\Delta t^2 (a_1^B \sin\theta + a_2^B \cos\theta) \\ v_1 + \Delta t (a_1^B \cos\theta - a_2^B \sin\theta) \\ v_2 + \Delta t (a_1^B \sin\theta + a_2^B \cos\theta) \\ \theta + \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}_u \mathbf{Q}_u \mathbf{J}_u^\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}_u \mathbf{Q}_u \mathbf{J}_u^\top$.

Method A: The Jacobian Mapping (Exact)

We define $\mathbf{Q}_u$ as the diagonal covariance of our raw IMU inputs (the "datasheet" values):

$$\mathbf{Q}_u = \text{diag}(\sigma_{a1}^2, \sigma_{a2}^2, \sigma_\omega^2)$$

We then multiply this by the Control Jacobian $\mathbf{J}_u$ (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. This approximates the integration of noise over the interval $\Delta t$. If we assume 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}$$

Why this structure matters:

• The $\Delta t^4$ and $\Delta t^3$ terms: These represent the physical coupling between velocity and position. If the accelerometer is noisy, it creates a large uncertainty in velocity ($\Delta t^2$), which "leaks" into an even larger uncertainty in position over time.
• The Cross-Terms (Off-Diagonals): These correlations tell the filter that if our velocity estimate is wrong, our position estimate is likely wrong in the same direction.
• Tuning: In a real robot, you often treat $\sigma_a$ and $\sigma_\omega$ as "tuning knobs." Increasing them tells the filter to rely less on the IMU integration and more on external measurements (like the Distance Beacon).

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

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 (Jf = df/dx)
        Jf = np.eye(5)
        Jf[0, 2], Jf[1, 3] = dt, dt
        # Derivatives with respect to theta
        Jf[0, 4] = 0.5 * dt**2 * (-a1_b * sin_t - a2_b * cos_t)
        Jf[1, 4] = 0.5 * dt**2 * ( a1_b * cos_t - a2_b * sin_t)
        Jf[2, 4] = dt * (-a1_b * sin_t - a2_b * cos_t)
        Jf[3, 4] = dt * ( a1_b * cos_t - a2_b * sin_t)

        # 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 = Jf @ self.P @ Jf.T + Q

    def update_magnetometer(self, z_theta, std_mag):
        """Linear Update for Absolute Heading"""
        Jh = 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, Jh, 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 
            
        Jh = 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, Jh, R)

    def _apply_update(self, y, Jh, R):
        """Centralized Kalman Correction Step"""
        # Innovation Covariance
        S = Jh @ self.P @ Jh.T + R
        # Kalman Gain
        K = self.P @ Jh.T @ np.linalg.inv(S)
        
        # State Update (force 1D array to match x)
        self.x = self.x + (K @ np.atleast_1d(y)).flatten()
        # Covariance Update (Joseph Form or Simple)
        self.P = (np.eye(5) - K @ Jh) @ self.P
    

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:

1. Velocity Drift: $v_{err} \approx \int b_a \, dt = b_a t$ (The robot "gains speed" linearly while sitting still).
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 this 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:

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

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.

def apply_sensor_biases(u_noisy, acc_bias, omega_bias):
    """
    Simulates systematic sensor errors (biases).
    
    NOTE: These are 'Hard-Iron' / 'Fixed-Sensor' biases. Because they are 
    added in the Body-Frame, they represent offsets that rotate WITH 
    the robot (e.g., internal motor interference or MEMS manufacturing defects).
    """
    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! 

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 ($P = (I - KH)P$) 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:

$$P_k = (I - K_k H_k) P_{k|k-1} (I - K_k H_k)^\top + K_k R_k K_k^\top$$

Why it is superior:

• Guaranteed Symmetry: It is structured as a "sandwich" form ($APA^\top + B$). Mathematically, any matrix in the form $XYX^\top$ is symmetric if $Y$ is symmetric. This prevents the EKF from "leaning" to one side due to rounding errors.
• Positive Definiteness: The $K R K^\top$ term acts as a "safety floor." Since the measurement noise $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:

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})\cos\theta - (a_2^B - b_{a2})\sin\theta \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})\sin\theta + (a_2^B - b_{a2})\cos\theta \right)$
3. $v_1[k] = v_1[k-1] + \Delta t \left( (a_1^B - b_{a1})\cos\theta - (a_2^B - b_{a2})\sin\theta \right)$
4. $v_2[k] = v_2[k-1] + \Delta t \left( (a_1^B - b_{a1})\sin\theta + (a_2^B - b_{a2})\cos\theta \right)$
5. $\theta[k] = \theta[k-1] + (\omega - b_\omega)\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]$

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 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.
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.

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 Jf (8x8)
        Jf = np.eye(8)
        Jf[0, 2], Jf[1, 3] = dt, dt
        Jf[0, 4] = 0.5 * dt**2 * (-a1_c * sin_t - a2_c * cos_t)
        Jf[1, 4] = 0.5 * dt**2 * ( a1_c * cos_t - a2_c * sin_t)
        Jf[2, 4] = dt * (-a1_c * sin_t - a2_c * cos_t)
        Jf[3, 4] = dt * ( a1_c * cos_t - a2_c * sin_t)
        
        Jf[0, 5], Jf[0, 6] = -0.5 * dt**2 * cos_t,  0.5 * dt**2 * sin_t
        Jf[1, 5], Jf[1, 6] = -0.5 * dt**2 * sin_t, -0.5 * dt**2 * cos_t
        Jf[2, 5], Jf[2, 6] = -dt * cos_t,  dt * sin_t
        Jf[3, 5], Jf[3, 6] = -dt * sin_t, -dt * cos_t
        Jf[4, 7] = -dt
        
        # 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 = Jf @ self.P @ Jf.T + Q

    def _apply_update(self, y, H, R):
        """Universal Joseph-Form Update (Ensures P stays positive-definite)"""
        S = H @ self.P @ H.T + R
        K = self.P @ H.T @ np.linalg.inv(S)
        
        # State Update
        self.x = self.x + (K @ np.atleast_1d(y)).flatten()
        
        # Joseph Form Covariance Update: P = (I-KH)P(I-KH)' + KRK'
        I_KH = np.eye(8) - K @ H
        self.P = I_KH @ self.P @ I_KH.T + K @ R @ K.T

    def update_magnetometer(self, z_theta, std_mag):
        H = np.zeros((1, 8))
        H[0, 4] = 1 
        y = self.normalize_angle(z_theta - self.x[4])
        self._apply_update(y, 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
        
        H = np.zeros((1, 8))
        H[0, 0], H[0, 1] = p1/dist_pred, p2/dist_pred
        y = z_dist - dist_pred
        self._apply_update(y, H, np.array([[std_dist**2]]))

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):
    """
    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)
    """
    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 t[i] < 5.0 and calibration:
            # "Virtual" Measurement: "I promise you, velocity is zero"
            z_v = np.array([0.0, 0.0])
            H_v = np.zeros((2, 8))
            H_v[0, 2] = 1 # v1
            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], H_v, R_v)
    
        # 5. Store Results
        x_estimates[i] = ekf.x.copy()
        P_estimates[i] = ekf.P.copy()
        
    return x_estimates, P_estimates

The Result

With these improvements, the EKF can start with zero knowledge of the sensor's flaws and, within a few seconds of motion, accurately track the robot's path while simultaneously identifying exactly how much the IMU is "lying" to it.

# 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":

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: 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
)

# 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.

Comparison: EKF Navigation Strategies

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