State Estimation and the Kalman Filter

1. Hidden Truth Behind Noisy Signals

A mobile robot traverses a 1D corridor. Its true physical position $x_t \in \mathbb{R}$ is a hidden variable governed by Newtonian mechanics. The onboard flight computer cannot directly access $x_t$; it receives only noisy wheel tick counts and jittery distance measurements.

The state vector captures the minimal set of quantities needed to predict future behavior:

$$x_t = \begin{bmatrix} p_t \\ v_t \end{bmatrix}$$

Because real sensors deliver corrupted signals $z_t = h(x_t) + v_t$, raw readings cannot be trusted directly. Passing unfiltered sensor values straight into a motor controller causes erratic commands, excessive mechanical wear, and catastrophic collisions.

2. Representing Belief with Gaussians

The Kalman filter expresses its belief over the hidden state as a normal probability density function $\mathcal{N}(\mu, \sigma^2)$. The mean $\hat{x}$ denotes the most likely position, while the variance $\sigma^2$ (or covariance matrix $\mathbf{P}$) captures the degree of certainty.

$$p(x) = \frac{1}{\sqrt{2\pi \sigma^2}} \exp\left( -\frac{(x - \hat{x})^2}{2\sigma^2} \right)$$

At time $t=0$, prior calibration establishes our initial state belief:

$$\hat{x}_0 = 2.00\,\text{m}, \quad \sigma_0^2 = 0.25\,\text{m}^2 \quad (\sigma_0 = 0.50\,\text{m})$$

The width of the bell curve visually conveys robot confidence. A narrow peak reflects high confidence, whereas a flat, spread distribution indicates severe uncertainty.

3. The Motion Prediction Step

When motors engage, the robot commands a forward displacement $u = 1.00\,\text{m}$. In an ideal world, the robot would land precisely at $3.00\,\text{m}$. In reality, wheel slip and gear backlash inject process noise $w \sim \mathcal{N}(0, \sigma_w^2)$ with variance $\sigma_w^2 = 0.15\,\text{m}^2$.

The prediction equations propagate belief forward in time:

$$\hat{x}_1^- = \hat{x}_0 + u = 2.00 + 1.00 = 3.00\,\text{m}$$ $$\sigma_1^{-2} = \sigma_0^2 + \sigma_w^2 = 0.25 + 0.15 = 0.40\,\text{m}^2$$

Moving always increases uncertainty in the absence of external landmarks. The prior bell curve shifts rightward to $\hat{x}_1^- = 3.00\,\text{m}$ while flattening out to standard deviation $\sigma_1^- \approx 0.63\,\text{m}$.

4. The Incoming Noisy Measurement

At time $t=1$, an onboard ultrasonic sensor pings a wall to measure position. Due to acoustic reverberation and electronic noise, the sensor observation model contains measurement noise $v \sim \mathcal{N}(0, \sigma_v^2)$ with variance $\sigma_v^2 = 0.60\,\text{m}^2$:

$$z_1 = 3.60\,\text{m}, \quad \sigma_v^2 = 0.60\,\text{m}^2 \quad (\sigma_v \approx 0.77\,\text{m})$$

The robot now possesses two contradictory representations of reality: a motion prediction claiming $3.00\,\text{m}$ ($\sigma^2 = 0.40$) and an external sensor report claiming $3.60\,\text{m}$ ($\sigma^2 = 0.60$).

5. The Measurement Residual and Innovation

The discrepancy between what the sensor observed and what the motion model predicted is termed the measurement residual or innovation $y$:

$$y_1 = z_1 - \hat{x}_1^- = 3.60 - 3.00 = +0.60\,\text{m}$$

The innovation variance $S$ combines the uncertainty from both domains:

$$S_1 = \sigma_1^{-2} + \sigma_v^2 = 0.40 + 0.60 = 1.00\,\text{m}^2$$

A positive residual indicates that the robot is further along the track than dead reckoning suggested. The filter must determine how much of this $0.60\,\text{m}$ gap is genuine motion versus spurious sensor jitter.

6. The Optimal Kalman Gain

The Kalman gain $K \in [0, 1]$ calculates the optimal mathematical weighting between prediction trust and sensor trust:

$$K = \frac{\sigma_1^{-2}}{\sigma_1^{-2} + \sigma_v^2} = \frac{0.40}{0.40 + 0.60} = \frac{0.40}{1.00} = 0.40$$

The gain acts as a dynamic blending knob:

• If sensors are exceptionally noisy ($\sigma_v^2 \to \infty$), $K \to 0$: the filter ignores sensor updates and follows dead reckoning.
• If sensors are virtually noiseless ($\sigma_v^2 \to 0$), $K \to 1$: the filter discards historical predictions and snaps directly to measurements.
• With $K = 0.40$, the filter attributes $40\%$ of the residual to true robot displacement and $60\%$ to measurement noise.

7. The Measurement Update Step

The posterior mean $\hat{x}_1$ reconciles prediction with observation by scaling the innovation through the Kalman gain:

$$\hat{x}_1 = \hat{x}_1^- + K \cdot y_1 = 3.00 + 0.40 \times (+0.60) = 3.24\,\text{m}$$

The updated belief settles at $3.24\,\text{m}$. Rather than naively averaging $3.00\,\text{m}$ and $3.60\,\text{m}$ to $3.30\,\text{m}$, the filter places greater weight on the prediction because its variance ($0.40\,\text{m}^2$) was smaller than the sensor noise ($0.60\,\text{m}^2$).

8. Variance Reduction and Shrinking Uncertainty

Fusing two independent probabilistic estimates guarantees that posterior uncertainty is strictly lower than either source individually:

$$\sigma_1^2 = (1 - K) \cdot \sigma_1^{-2} = (1 - 0.40) \times 0.40 = 0.24\,\text{m}^2$$

The resulting standard deviation shrinks to $\sigma_1 = \sqrt{0.24} \approx 0.49\,\text{m}$. Notice that $0.24\,\text{m}^2$ is smaller than the predicted variance ($0.40\,\text{m}^2$) and drastically smaller than the raw sensor variance ($0.60\,\text{m}^2$). Combining imperfect predictions with noisy observations yields higher certainty than either source could attain on its own.

9. Tracking State Over Extended Horizons

The Kalman filter operates as an infinite recursive loop:

1. Predict: Apply control inputs and add process covariance ($P^- = A P A^\top + Q$).
2. Measure: Receive raw sensor observations $z_t$.
3. Update: Compute gain $K$, correct state estimate $\hat{x}$, and contract covariance $P$.
4. Recurse: Set $\hat{x}_{t+1} \leftarrow \hat{x}_t$ and repeat at 100 Hz.

Over multiple time steps, the filter smoothly tracks the true vehicle trajectory, rejecting transient noise spikes while faithfully following authentic accelerations and turns.

10. Where It Breaks

Standard linear Kalman filtering guarantees optimality only under rigid assumptions: linear kinematics and zero-mean Gaussian noise. Real physical systems break these assumptions regularly:

1. Sensor Glitches and Non-Gaussian Outliers: If a specular reflection causes the rangefinder to report $0.50\,\text{m}$ instead of $3.60\,\text{m}$, the quadratic penalty in Gaussian estimation jerks the state estimate violently off track. Robust M-estimators or particle filters are required to reject non-Gaussian outliers.
2. Filter Divergence from Covariance Collapse: If process noise $Q$ is underestimated, the covariance matrix $P$ shrinks toward zero. The filter becomes overconfident ("smug"), driving $K \to 0$ and completely ignoring new sensor data while the true robot drifts away.
3. Kinematic Non-linearities: Real robots rotate with sines and cosines. Linear Kalman filters cannot propagate non-linear functions directly, requiring the Extended Kalman Filter (EKF) via first-order Taylor expansion or the Unscented Kalman Filter (UKF) via deterministic sigma points.

The Quick Version

• State estimation deduces unmeasurable physical variables from noisy, indirect sensor observations.
• Robot belief is tracked as a Gaussian distribution with mean and covariance.
• Motor commands advance state predictions while increasing uncertainty via process noise.
• Sensors deliver corrupted observations characterized by measurement noise variance.
• Innovation measures the discrepancy between expected sensor readings and actual observations.
• Kalman gain balances prediction uncertainty against measurement noise for optimal blending.
• State updates shift the estimate toward the observation proportional to gain.
• Fusing prediction and measurement strictly contracts posterior uncertainty below both sources.
• Recursive predict-update loops maintain bounded tracking error across extended operating horizons.
• Non-linear kinematics and non-Gaussian sensor glitches trigger catastrophic filter divergence.