Kalman Filter, EKF, Drones, and Robotics
A quadcopter hovering in place is doing more math than it looks. Its gyroscope says it's rolling at 0.4 degrees per second. It isn't. Its accelerometer says it's tilted six degrees. It isn't, the motors are just shaking it. Its GPS says it drifted a meter and a half. It didn't.
Every sensor on that drone lies a little, each in its own way. The only reason it stays level is a sixty-year-old algorithm that knows exactly how much to believe each of them.
I recently went through Shane Ross's three-part series Kalman Filter for Beginners from the Ross Dynamics Lab at Virginia Tech, which follows Phil Kim's book Kalman Filter for Beginners with MATLAB Examples. What I liked is the order. It doesn't open with matrices. It opens with an average, and every step after that is a one-line edit to the step before, until the Kalman filter shows up and feels obvious.
This post follows that order. There are five interactive labs along the way so you can turn the knobs yourself. Then it goes where the series stops: the Extended Kalman Filter, what actually runs inside a drone's flight controller and a robot's localization stack, and the place most ML people meet the filter without noticing, inside every object tracker in computer vision.
Every Sensor Lies
A sensor gives you the truth plus something you didn't ask for:
z_k = x_k + v_k (measurement = truth + noise)
The lecture's first example is a battery. The real voltage is a constant 14.4 V. The reading you get back has noise with a standard deviation of 4 V, which sounds absurd until you've read a cheap ADC through a long cable next to a motor. The second example is a sonar altimeter on a drone. Same deal, but now the truth also moves.
Those are the two enemies. Noise, which is random and averages out. And change, which is not random and does not average out. Every filter in this post is a different answer to the same question: how much of the past should I keep?
The Average Filter, Written So It Runs Forever
Start with the obvious thing. Average all the readings you've seen so far:
x̄_k = (x_1 + x_2 + ... + x_k) / k
Fine for a spreadsheet. Useless on a microcontroller, because every new sample makes you re-add the whole history. The fix is the trick the entire series is built on. Multiply both sides by k, write the same thing for k-1, subtract, and you get the running average as a function of the previous average and the newest sample:
x̄_k = ((k-1)/k) · x̄_{k-1} + (1/k) · x_k
Call α = (k-1)/k and it reads:
x̄_k = α · x̄_{k-1} + (1-α) · x_k
def average_filter(z):
avg = 0.0
for k, x in enumerate(z, start=1):
alpha = (k - 1) / k
avg = alpha * avg + (1 - alpha) * x
yield avg
No history, no buffer. Two numbers of state. On the 14.4 V battery it works beautifully, and on the
climbing drone it's a disaster. Look at what α does: at k=100 it's 0.99, at k=1000 it's 0.999. The
filter trusts every new reading less than the one before it. It's designed to converge to a
constant, so if the truth moves, the filter stays behind and never catches up.
Moving Average: Forget the Past on Purpose
If remembering everything is the problem, remember only the last n samples:
x̄_k = (x_{k-n+1} + ... + x_k) / n
Same trick makes it recursive. Add the newest, subtract the one that just left the window:
x̄_k = x̄_{k-1} + (x_k - x_{k-n}) / n
from collections import deque
def moving_average(z, n=20):
buf = deque([z[0]] * n, maxlen=n)
total = z[0] * n
for x in z:
total += x - buf[0]
buf.append(x)
yield total / n
Now the filter can follow a changing signal. The price is lag. With n = 20 and a 50 Hz sonar you're
always reporting where the drone was 0.2 seconds ago. Make n bigger and it's smoother and later.
Make it smaller and it's on time and jittery. There's no n that wins both.
The lecture points out the deeper flaw. Every sample in the window gets the same weight, 1/n.
The reading from 19 steps ago counts exactly as much as the one from right now. That makes no sense
for anything that moves.
Low-Pass Filter: Weight the Recent Stuff More
Take the average filter's equation and stop letting α grow. Freeze it:
x̄_k = α · x̄_{k-1} + (1-α) · x_k 0 < α < 1
Expand it once and you see why it's better. Substitute the previous estimate into itself:
x̄_k = α² · x̄_{k-2} + α(1-α) · x_{k-1} + (1-α) · x_k
The newest sample gets (1-α), the one before gets α(1-α), the one before that α²(1-α). Weights
decay exponentially. Recent data matters more, old data fades out on its own. If you've done any ML or
trading you know this as the exponential moving average. Signal people call it a first-order low-pass
filter, because it passes the slow content (the signal) and attenuates the fast content (the noise).
def low_pass(z, alpha=0.85):
prev = z[0]
for x in z:
prev = alpha * prev + (1 - alpha) * x
yield prev
Two lines of state and it beats the moving average on most real signals. But look at what we did.
α used to be computed from k. Now it's a knob. The lecture calls it a "free parameter", which is
an academic way of saying nobody knows what it should be. You pick it by staring at plots.
Stare at some plots.
Try the step signal. Every filter lags, and the ones that lag less are noisier. Try the altitude
signal with α = 0.98: silky smooth, and about a second late. Then α = 0.3: on time, and you can
barely tell it from the raw dots. That's the wall. One fixed α cannot be right when the drone is
hovering and also right when it's climbing.
The Kalman Filter Is a Low-Pass Filter That Picks Its Own α
This is the reveal in Part 2 of the series, and it's the sentence that made everything click for me.
The Kalman filter's estimate equation looks intimidating:
x̂_k = x̂⁻_k + K_k · (z_k − H · x̂⁻_k)
x̂⁻_k is the prediction (what the filter expected before seeing the measurement), z_k is the
measurement, and K_k is the Kalman gain. For a scalar with H = 1, distribute the gain:
x̂_k = (1 − K_k) · x̂⁻_k + K_k · z_k
Now put that next to the low-pass filter:
x̄_k = α · x̄_{k-1} + (1-α) · x_k
Same shape. α = 1 − K_k. The only structural difference is that the low-pass filter blends with
the previous estimate, while the Kalman filter blends with a prediction of where the estimate
should be now. When your model says "nothing changes" those are the same thing.
The real difference is that K_k isn't a knob. It's computed every step from two numbers: how
uncertain the filter currently is about its own estimate (P), and how noisy the sensor is (R).
K_k = P⁻_k / (P⁻_k + R)
If the filter is confident and the sensor is noisy, K goes small and it mostly ignores the reading.
If the filter is unsure and the sensor is good, K goes toward 1 and it mostly takes the reading.
It's the low-pass filter with the honesty to change its mind.
Set Q to zero and watch K decay toward zero. That's the average filter again: the model says the
voltage is constant, so the filter grows certain and stops listening. Give Q a small value and K
settles on a floor instead. The filter now expects the truth to drift, so it never fully stops
listening. The whole tuning story is in that one slider.
The Four Steps
The full algorithm is a loop. The lecture draws it as a box, so here's the box.
Predict, then correct. The prediction step only touches A and Q. The estimation step only
touches H and R. That separation is not cosmetic. It's why you can run the prediction at 400 Hz
off a gyro and only run the correction at 5 Hz when a GPS fix shows up. The filter doesn't need the
measurements to arrive on a schedule.
The symbols, once, so the rest of the post doesn't have to keep stopping:
| Symbol | What it is | Where it comes from |
|---|---|---|
x̂_k | Estimate of the state | The filter's output |
x̂⁻_k | Prediction of the state before the measurement | Step I |
P_k | Error covariance, how wrong the filter thinks it is | Steps I and IV |
K_k | Kalman gain, how much to trust the measurement | Step II |
z_k | The measurement | Your sensor |
A | State transition, how the state evolves one step | Physics |
H | State-to-measurement, what the sensor sees of the state | Sensor geometry |
Q | Process noise covariance, how wrong the model is | Trial and error |
R | Measurement noise covariance, how noisy the sensor is | The datasheet |
P deserves one more line. The lecture describes the true state as a bell curve centered at the
estimate with width P. A small P is a narrow bell, a confident filter. A large P is a wide bell.
The filter carries its own error bar along with its answer, which is the thing none of the earlier
filters could do.
The System Model Is the Part Everyone Skips
The Kalman filter only works on a linear model with additive noise:
x_{k+1} = A · x_k + w_k (state, with process noise w ~ N(0, Q))
z_k = H · x_k + v_k (measurement, with noise v ~ N(0, R))
Writing A, H, Q, R for your problem is the actual work. The four steps are the same code every
time. Here's the whole thing, with the lecture's exact numbers for the battery:
import numpy as np
class Kalman:
def __init__(self, A, H, Q, R, x0, P0):
self.A, self.H = np.atleast_2d(A), np.atleast_2d(H)
self.Q, self.R = np.atleast_2d(Q), np.atleast_2d(R)
self.x = np.atleast_1d(np.array(x0, dtype=float))
self.P = np.atleast_2d(np.array(P0, dtype=float))
def step(self, z):
A, H, Q, R = self.A, self.H, self.Q, self.R
# I. predict
xp = A @ self.x
Pp = A @ self.P @ A.T + Q
# II. gain
K = Pp @ H.T @ np.linalg.inv(H @ Pp @ H.T + R)
# III. estimate
self.x = xp + K @ (np.atleast_1d(z) - H @ xp)
# IV. error covariance
self.P = Pp - K @ H @ Pp
return self.x
# Battery: constant voltage (A = 1), read directly (H = 1),
# perfect model (Q = 0), sensor σ = 2 V (R = 4), first guess 14 V.
kf = Kalman(A=1, H=1, Q=0, R=4, x0=14, P0=6)
R = 4 because the lecture assumes the sensor is good to ±2 V, and variance is sigma squared. Q = 0 because a battery's
voltage genuinely doesn't change on this timescale. P0 = 6 because the lecturer wasn't sure about
the initial guess and wanted the filter to fix it fast. That's what the initial covariance is for.
Guess big if you don't know. The filter recovers.
Estimating something you never measured
The example that sold me on this was in Part 3. A sonar gives you the drone's altitude. You want
its vertical velocity. The naive approach is a finite difference, (z_k − z_{k-1}) / dt, and with
50 Hz noisy sonar that's dividing noise by 0.02. The result is garbage.
Instead, put velocity in the state and let physics connect the two:
dt = 0.02
kf = Kalman(
A=[[1, dt], # position += velocity · dt
[0, 1]], # velocity stays
H=[[1, 0]], # the sonar only sees position
Q=[[1, 0],
[0, 3]],
R=10,
x0=[0, 20], P0=5 * np.eye(2),
)
H = [1 0] says the sensor sees position and nothing else. The filter never receives a velocity.
It infers one, because the model says position can only change if velocity is nonzero, and it works
out how much velocity explains the measurements it did receive. Go back to Lab 01, pick the altitude
signal, and switch the Kalman model to "Position + velocity". Watch it stop lagging on the climb.
The same filter with the constant model can't do that, because its model doesn't know climbs exist.
That's the lesson. The Kalman filter isn't smarter than the low-pass filter because of the gain formula. It's smarter because you told it how the world moves.
Sensor Fusion: The Gyro and the Accelerometer Cover for Each Other
Part 3 ends with the demo that every drone runs some version of. An IMU has a gyroscope that measures how fast the drone is rotating, and an accelerometer that measures how hard it's being pushed, gravity included. Neither can tell you which way is up on its own.
Integrate the gyro and you get angle. Angular velocity times dt, added up, step after step. The
lecture does it properly with the full rotation kinematics, but the idea is just that.
It works for about ten seconds. Then it drifts. Any constant bias in the gyro, even a tiny one, gets integrated into an angle that grows forever. The lecture ran it on real data and the roll walked off by tens of degrees while the sensor was sitting on a bench.
The accelerometer has the opposite personality. If it's sitting still it measures gravity, and gravity tells you which way is down. A bit of trigonometry on the three axes turns that into roll and pitch directly.
No drift, ever. But it can't tell gravity apart from any other acceleration, so the moment the motors spin up it shakes, and yaw is invisible to it because rotating about the gravity vector doesn't change the gravity vector.
Fusion is the obvious move once you say it out loud. Use the gyro in the prediction step, because it's smooth and fast. Use the accelerometer in the correction step, because it's honest on average. The gyro carries the estimate between corrections; the accelerometer pulls it back every time it wanders.
This lab is one axis with a two-element state, angle and gyro bias. The bias is the interesting part. The filter never measures it. It notices that the gyro's integrated angle keeps disagreeing with the accelerometer in the same direction, and the only state that can explain a consistent disagreement is a bias. So it learns it. Push "Accel trust" to zero and you get the drifting gyro back. Push it to one and you get the shaky accelerometer back. Anywhere in between beats both.
The lecture's version is bigger: four states, because it uses a quaternion instead of Euler angles. That choice is the crack that leads to the next section.
Where the Linear Filter Cracks
Look at what the gyro integration actually does. How much the roll changes depends on sines and
tangents of the current roll and pitch. The matrix that steps the state forward is built from the
state itself. You can't write it as x_{k+1} = A x_k with a constant A, which is the one thing
the Kalman filter requires.
The lecture dodges this with a genuinely nice trick: switch to quaternions, a different way of
writing a rotation whose update rule happens to be linear for a given angular velocity. A still
changes every step because the gyro reading changes, but it no longer depends on the state, so the
linear filter is happy. Four states, and it runs on the real IMU data with no drift.
That trick doesn't always exist. A ground robot's position update is x += v·dt·cos θ. Nonlinear in
θ, and there's no change of variables that fixes it. The range to a beacon is a square root of
squared differences. Bearing is an arctangent. Aircraft dynamics, satellite orbits, a camera
projecting a 3D point into pixels. The world is mostly curved.
The Extended Kalman Filter
The EKF's answer is not clever. Keep the nonlinear functions where you can, and linearize where you must.
x_{k+1} = f(x_k, u_k) + w_k (nonlinear motion, with control input u)
z_k = h(x_k) + v_k (nonlinear measurement)
Predict the state by pushing it through the real f. Predict the measurement by pushing it through
the real h. But the covariance and the gain need matrices, so for those you use the Jacobians of
f and h, evaluated at the current estimate:
F_k = ∂f/∂x |_{x̂_k} H_k = ∂h/∂x |_{x̂⁻_k}
Then it's the same four steps with the letters swapped:
For a ground robot with state [x, y, θ], given its speed v and turn rate ω:
def f(x, u, dt):
px, py, th = x
v, w = u
return np.array([px + v*dt*np.cos(th),
py + v*dt*np.sin(th),
th + w*dt])
def F_jac(x, u, dt):
_, _, th = x
v, _ = u
return np.array([[1, 0, -v*dt*np.sin(th)],
[0, 1, v*dt*np.cos(th)],
[0, 0, 1]])
The measurement side works the same way. Write the real h, say the range and bearing to a known
beacon, and take its Jacobian for the gain.
The honest caveats. Linearization is an approximation, so the EKF is not optimal the way the linear filter is. If your initial guess is far from the truth, the Jacobian is evaluated at the wrong place and the filter can diverge and never come back. Angles wrap, so you have to normalize the bearing innovation to ±π or the filter will happily chase a 359-degree error. And you have to derive the Jacobians, which is tedious by hand and a solved problem with symbolic tools or autodiff. The Unscented Kalman Filter skips Jacobians entirely by pushing a handful of sample points through the real nonlinearity, and it handles strong curvature better at a few times the cost.
None of that stopped it from going to the Moon. Rudolf Kalman's 1960 paper was linear. Stanley Schmidt at NASA Ames read it, saw that the Apollo midcourse navigation problem was nonlinear, and did the obvious thing: linearize around the reference trajectory and run the filter on the deviations. That is the EKF, and it flew in the Apollo guidance computer with a few kilobytes of memory. The whole extension was a practical engineer refusing to let a linear assumption stop a working idea.
Two things to notice in this lab. First, dead reckoning error only grows. It never comes back,
because nothing ever tells it where it is. Second, the ellipse. That's P drawn as a shape. It swells
between fixes as the filter admits it's guessing, and collapses the instant a measurement lands.
Switch to beacons and turn the range noise up. Range alone can't pin down a position, but range plus
bearing to a known point can, and the EKF works that geometry out through the Jacobian without you
writing any triangulation code.
What Actually Runs on a Drone
The toy filters above have two to four states. A flight controller's has a couple dozen, and the extra ones are all there to model the ways sensors lie.
PX4's EKF2, the default estimator on most open-source flight controllers, carries 24 states. Attitude, velocity, and position are only a handful of them. Most of the rest are biases: how far off the gyro is, how far off the accelerometer is, what the magnetometer is picking up from the drone's own motors, how hard the wind is blowing. The filter doesn't just estimate where the drone is. It estimates how wrong each sensor is, continuously, and corrects for it.
The IMU drives the prediction step hundreds of times a second. Everything else corrects at whatever rate it shows up. That's the predict/correct separation from the four-steps diagram doing real work.
Three things the textbook version never mentions, and that matter more than the equations once you're flying:
Sensors arrive late. A GPS fix describes where the drone was a fraction of a second ago. Fuse it against the current prediction and you're correcting the wrong moment. Real estimators keep a short buffer of recent sensor data, apply each measurement at the time it was actually valid, and roll the result forward to now.
Measurements get a bouncer. Before a reading is fused, the filter checks how far it landed from
the prediction, relative to how far off it expected to be given P and R. Too far out and the
reading is rejected. That's how a single bad GPS fix under a bridge doesn't throw the drone into a
wall. It's also the best debugging signal you have: if one sensor keeps getting rejected, that
sensor or your R for it is wrong.
Rotations get special treatment. Angles don't add up like ordinary numbers, so the filter tracks a small correction to its current orientation instead of the orientation itself. That keeps the covariance well-behaved over millions of iterations.
ArduPilot's EKF3 adds one more idea worth stealing. If the board has more than one IMU, it runs a complete EKF per IMU in parallel and flies on whichever one's sensor data is most self-consistent. If one IMU starts lying, the vehicle switches lanes.
That's the difference between the lab above and a drone that stays level while a motor bearing is failing. Not a different algorithm. The same four steps, with more states, delay handling, gating, and years of tuning.
And on a Robot
Ground robots have the same shape of problem with different sensors. Wheel encoders are the gyro here: smooth, fast, and they drift because wheels slip. GPS, a camera, or a lidar map is the accelerometer: jumpy, occasionally absent, but anchored to the world.
In ROS the standard tool is
robot_localization, an EKF that fuses
whatever mix of wheel odometry, IMU, and position sources you hand it. The idea worth stealing from its docs is the two-frame setup. An
estimate built from wheels and IMU is smooth and continuous but drifts. One that includes GPS is
globally correct but jumps whenever a fix lands. Rather than choosing, the convention is to run two
EKF instances: one publishes the smooth, drifting odom estimate the controllers drive on, the other
adds GPS and publishes the jumpy but correct map estimate the planner navigates by. Continuous for
driving, correct for navigating.
Its process_noise_covariance parameter is Q, and the docs give the tuning rule in one line: the
larger Q is relative to a measurement's variance, the faster the filter converges to that
measurement. Same slider as Lab 02, fifteen dimensions wide.
Put the landmarks into the state alongside the robot and you have EKF-SLAM: the beacon lab above with the beacons unknown. The covariance grows with the square of the landmark count, which is why modern SLAM moved to graph optimization, but that's where it started.
It's in Computer Vision Too
Here's the version of the filter I'd been using for two years before I understood it.
In 2024 I built EmotionLens: RetinaFace finds faces in a webcam feed, a residual masking network classifies the emotion, and SORT tracks each face across frames so I could majority-vote the label instead of letting it flicker. I picked SORT because it was 300 lines and worked. I didn't look inside. The inside is a Kalman filter, the Hungarian algorithm, and nothing else.
Once you see it, it's everywhere in tracking-by-detection. SORT, DeepSORT,
ByteTrack, BoT-SORT, StrongSORT, OC-SORT. The trackers that
ship in Ultralytics' model.track() are ByteTrack and BoT-SORT. Every one of them keeps one Kalman
filter per tracked object. They differ in how they match detections to tracks; the filter underneath
is the same.
The mapping onto everything above is direct. The detector is the sensor, and it's a bad one in exactly
the ways this post has been about: the box jitters a few pixels every frame even when nothing moved,
and some frames it produces nothing at all. The state is the box plus its velocity, constant-velocity
model, same A as the sonar example with two more dimensions:
# SORT's state, slightly simplified: box center, size, and center velocity
x = [cx, cy, w, h, vx, vy]
F = [[1, 0, 0, 0, 1, 0], # cx += vx
[0, 1, 0, 0, 0, 1], # cy += vy
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]]
H = [[1, 0, 0, 0, 0, 0], # the detector reports a box,
[0, 1, 0, 0, 0, 0], # never a velocity
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0]]
Each frame the tracker predicts where every box should be now, matches the new detections to those
predictions, and updates. The two things the filter buys you are the two things a raw detector can't
do. First, when the detector drops a frame or the person walks behind a pillar, the track keeps going
on prediction alone, so the ID survives and the box doesn't vanish and reappear as a new person.
Second, the gate. DeepSORT computes the Mahalanobis distance between a detection and a track's
predicted position using the filter's own S = H P Hᵀ + R, and refuses any match outside the 95%
ellipse. That's the same innovation gate PX4 uses to reject a bad GPS fix, applied to pixels, and it's
most of the reason two people crossing paths don't swap IDs.
Widen the occluder and watch the gate grow while the track coasts. That growth is P accumulating
process noise with no measurement to shrink it, and it's honest: the longer you've been guessing, the
more room a re-detection is allowed to land in. Then push process noise up and the coast starts to
wander, because you've told the filter the velocity isn't trustworthy. OC-SORT's whole contribution
is fixing that drift for long occlusions by re-fitting the velocity once the object comes back.
Two things worth knowing. These trackers use the plain linear filter, not the EKF. A box sliding across an image plane is close enough to linear that the nonlinear machinery buys nothing. The EKF shows up in vision when the state lives in 3D and the camera projection is in the measurement equation: visual-inertial odometry, SLAM, the pose estimation inside AR headsets. And the newer end-to-end trackers (TrackFormer, MOTR) learn the motion and association inside the network, so there's no explicit filter. Plenty of production pipelines still put one on the output anyway, because a network's box jitters too.
If you're running YOLO with track mode, you're running a Kalman filter per object at 30 fps.
It's worth knowing what its Q and R mean before you tune them.
Tuning It Without Losing a Week
Everything in this post reduces to choosing Q and R, so here is how that goes in practice.
R is the easy one. The datasheet gives you a noise figure, and variance is sigma squared. If you
don't trust the datasheet, log the sensor sitting still for a minute and compute the variance
yourself. That number is R. Don't tune it by feel when you can measure it.
Q is a confession. It's you writing down how wrong your model is. A constant-velocity model on a
drone that accelerates needs a Q large enough to cover the acceleration you didn't model. Start
small, look at the innovations, and raise Q on whichever state keeps lagging behind. If the filter
follows every wiggle of the sensor, Q is too big. If it ignores real motion, Q is too small.
P0 doesn't matter much, as long as it's big. A large initial covariance tells the filter "I have no
idea", the gain starts near 1, and the first few measurements set the state. A small P0 with a wrong
x0 is the one combination that hurts, because the filter is confident and wrong at the same time.
And watch the innovations, always. A well-tuned filter's innovations look like white noise with the
variance the filter predicted. If they have a trend, your model is missing a state. If they have
spikes the gate should catch, tighten the gate. If they're consistently larger than predicted, your
R is a lie. The filter tells you what's wrong with it, if you plot the right thing.
Wrapping Up
Every filter before the Kalman filter had a number you had to pick. The moving average had n.
The low-pass filter had α. Both were guesses, and both were wrong the moment conditions changed.
The Kalman filter's contribution isn't the matrices. It's that it replaces your guess with a
computation, and carries an honest error bar along with the answer so the computation can keep
adapting. Once you've seen x̂ = (1 − K)·x̂⁻ + K·z next to the low-pass filter, the rest is
bookkeeping. The EKF is that same idea admitting the world is curved. PX4's 24 states are that same
idea admitting sensors have biases, arrive late, and occasionally lie outright. SORT is that same
idea with a neural network as the sensor.
I went into the series expecting a wall of linear algebra and came out with a two-line filter I now reach for whenever a signal is noisy. The math was never the hard part. Writing down how your world moves is, and no filter can do that for you.
Have questions or ran into something I didn't cover? Feel free to reach out.