Kalman Filter: The Tour • 🍄🍄🍄

-->

-->

Back<br>TABLE OF CONTENTS<br>Introduction<br>The Problem: Sensor-Model Tension<br>Why This Matters for Control Systems<br>The Solution: Kalman Filtering The Two-Step Cycle<br>The Kalman Gain: Optimal Weighting

Implementation Details State Representation<br>The Matrices

Results Quantitative Performance<br>Qualitative Observations

Applications<br>Next Steps<br>Conclusion<br>References<br>18 January 2026 / 8 min read

Kalman Filter: The Tour<br>filters ,<br>signal-processing ,<br>math ,<br>drones ,<br>defense

Introduction

GPS measurements in real-world drone navigation have 2-5m of error. If you base your control directly on raw GPS measurements, the controls will not be smooth. The drone’s controller ends up fighting measurement noise instead of actual disturbances.

Kalman filter can address this problem. The filter fuses noisy GPS position measurements with clean IMU acceleration data to produce smooth state estimates. The result: 62.8% reduction in position error compared to raw GPS measurements.

The Problem: Sensor-Model Tension

The fundamental challenge in state estimation is the tension between two imperfect sources of information.

Sensors are inherently imperfect. Measurements can be noisy, biased, incomplete, or disrupted by environmental factors. They capture what’s happening now but don’t predict future state or account for underlying dynamics. GPS, for example, provides position measurements but with significant noise (±3m standard deviation). Computing velocity from GPS using numerical derivatives amplifies this noise dramatically.

Physics models are mathematical representations based on physical laws. They are deterministic or probabilistic frameworks that integrate prior knowledge. Their limitation is that they’re abstractions - often simplified to make computation feasible. They may ignore real-world complexities like wind, drag, or sensor biases. Over time, model predictions can drift from reality.

The fundamental tension arises from this dichotomy. Sensors provide ground truth obstructed by uncertainty, while physics models offer coherent predictions that can diverge from reality due to idealizations.

Consider a concrete example from my implementation: GPS says “you’re at 10.3m altitude” (noisy but real), while the physics model says “based on last position and IMU acceleration, you should be at 9.8m” (smooth but possibly drifting). Which do you trust? Trust GPS too much and you get jerky control. Trust the model too much and you drift away from reality.

The outcome is a trade-off in trust between these two sources.

Why This Matters for Control Systems

I previously implemented PID controllers for waypoint navigation. When you feed noisy measurements into a PID controller:

The derivative term (Kd) amplifies noise. If GPS jumps ±3m, the velocity estimate oscillates wildly, causing the drone to fight phantom movements.

The integral term (Ki) accumulates these noisy errors over time.

The controller is essentially fighting measurement noise instead of actual disturbances like wind.

Without filtering, your PID controller cannot distinguish between real state changes and sensor noise. This is unacceptable for stable autonomous flight.

The Solution: Kalman Filtering

The Kalman filter solves this problem by computing the optimal balance between prediction and measurement automatically. It’s a two-step recursive algorithm that runs continuously during flight.

The Two-Step Cycle

1. Predict Step (runs at 100 Hz using IMU acceleration)

The filter uses a physics model to predict where the drone should be at the next timestep:

x_p = A × x + B × u

P_p = A × P × A^T + Q

Where:

x_p is the predicted state [position, velocity]

A is the state transition matrix (constant velocity model)

B is the control input matrix (acceleration from IMU)

u is the control input (acceleration)

P_p is the predicted covariance (uncertainty)

Q is the process noise (model uncertainty)

Uncertainty grows during this step because our physics model is imperfect. We add process noise Q to account for unmodeled dynamics.

2. Correct Step (runs at 10 Hz when GPS measurement arrives)

When a GPS measurement arrives, the filter compares it with the prediction and updates its belief:

y = z - H × x_p # Innovation (measurement - prediction)

S = H × P_p × H^T + R # Innovation covariance

K = P_p × H^T × S^(-1) # Kalman gain

x = x_p + K × y # Update state

P = P_p - K × H × P_p # Update covariance

Where:

z is the GPS measurement

H is the measurement matrix (we measure position, not velocity)

K is the Kalman gain (optimal weight)

R is the measurement noise covariance

Uncertainty shrinks during this step because the measurement provides new information.

The Kalman Gain: Optimal Weighting

The Kalman gain K is the most important concept in the filter. It automatically computes the optimal trust balance between prediction and measurement.

When K is close to 1, our certainty about the measurement grows. We trust the...