Getting started
1
Introduction
draft
Every sensor lies. The trick is knowing how.
A small drone hangs in the air above a field. Its job is simple: stay ten metres up. A controller spins the motors faster when the drone is too low and slower when it is too high, and you could write that controller in an afternoon. But it needs one number, many times a second: how high are we right now? Nothing on board measures that number directly. The drone has three sensors, each with its own answer, and no two answers agree.
This book builds the thing that listens to all three and decides what to believe. It’s called a Kalman filter. We’ll build it one small program at a time, starting from a drone that knows nothing, until every line of the textbook equations is something we needed and put there ourselves.
The cast
The drone and its three sensors have their own colours, in pictures and in text alike:
D the drone, which must hold ten metres
A Acc, the accelerometer: quick, smooth, and slowly wrong
B Baro, the barometer: always roughly right, never exactly
E Echo, the sonar: precise, but only near the ground
Each sensor gives a reading, many times a second. Each is wrong in its own way, and the way matters more than the amount.
Baro feels air pressure, which falls by about one hectopascal for every eight metres you climb. Its readings jump around by a few decimetres from one moment to the next, and the propellers’ downwash adds its own gusts. Worse, the weather moves the pressure too: on a day when a front comes through, the whole sky can shift by a metre’s worth of pressure in ten minutes. Baro is never far off, but never on the dot, and it wanders.
Acc feels acceleration, a hundred times a second, with very little noise. But acceleration isn’t height. To get height we integrate twice: acceleration into speed, speed into height. Any tiny bias gets integrated twice too. An error of one hundredth of a metre per second squared, far below anything you’d feel, grows to ½ · 0.01 · 60² = 18 metres after one minute. Acc is excellent about the next tenth of a second and hopeless about the next minute.
Echo sends a click of sound downwards and times the echo off the ground. Its readings are good to a centimetre, better than either of the others. But beyond about four metres the echo is too faint, and Echo hears nothing at all. When the drone tilts, Echo measures the slanted path instead of the vertical one, and reads a little long.
So we have three witnesses: one fast but drifting, one honest but noisy, one precise but short-sighted. And we want the true height, which no one on board ever sees.
Why one sensor isn’t enough
The obvious move is to pick the best sensor and trust it. Every choice goes wrong, and each goes wrong differently:
- Trust Baro, and the controller chases its noise. Each reading that jumps up by thirty centimetres makes the motors slow down; the next, which jumps back, speeds them up again. The drone shivers, and on a stormy day it slowly climbs or sinks with the weather while believing it is perfectly still.
- Trust Acc, and everything is smooth for a few seconds. Then the double integration drifts, and within a minute the controller is sure the drone is at ten metres while it is really lying in the grass, or climbing out of sight.
- Trust Echo, and the drone is blind for most of its flight: at ten metres, Echo never hears the ground.
Each sensor fails where another is strong. Acc is good over a tenth of a second, where Baro is noisy. Baro is good over a minute, where Acc has drifted. Echo is good near the ground, where we most need precision, for landing. The answer must be some mix of them.
Why averaging isn’t enough
Then mix them. Average Baro’s last hundred readings, and the noise mostly cancels. But an average looks backwards: it describes where the drone was over the last second, not where it is now. When a gust drops the drone by half a metre, the average admits it only gradually, and the controller reacts late. That lag is exactly what a controller can’t afford.
And how much of each sensor should go into the mix? Equal parts would be silly: Echo, when it hears anything, is thirty times more precise than Baro. Fixed weights fail too, because the right weight changes over time. Just after take-off, near the ground, Echo should dominate. At ten metres, it has nothing to say. After a minute of hovering, our own estimate, built from many readings, may be better than any single new reading from Baro; a reading that disagrees should move us only a little. But after a gust, when we know we’ve been knocked about, a new reading should move us a lot.
So the right amount to trust a reading depends on how much we already know. That’s the idea this book is built on. An estimate, our best guess of the true height, is not enough on its own. We’ll carry a second number beside it: how unsure we are of that guess. Every reading will shrink that doubt, and every moment that passes without one will grow it again.
Key concept
An estimate is a guess and a doubt
The filter we’ll build never holds just a height. It holds a guess of the height and how unsure it is of that guess, and every decision it makes, how far to move towards a new reading, comes from comparing its own doubt with the reading’s.
What we’ll build
We’ll begin with the simplest possible version of the problem and add one difficulty at a time:
- One sensor, Baro, and a drone that holds still. How should we combine readings of something that doesn’t change? An average, yes, but one we can update after every reading without keeping the old ones.
- Two sensors that disagree. How much should each count?
- A drone that moves. Our estimate must move with it, and our doubt must grow while it does.
- Acc, and knowing more than one thing at once: height, speed, and Acc’s own bias, all estimated together. This is where the doubts become a matrix, and where we arrive at the filter Kalman published in 1960.
- Tuning, and catching a filter that has started to lie to itself.
- Looking back after the flight: once the drone has landed, the whole flight’s readings can improve every estimate along the way.
- Echo’s slanted path, and sensors whose readings aren’t straight lines of the state. We’ll bend the filter to fit, three different ways, the last of which gives up on doubts as single numbers altogether.
That list is a direction, not a plan: each chapter is shaped by what turned out hard in the one before it.
How this book works
Each chapter follows the same path. We start from a flight that goes wrong, look at exactly why, and fix it in the smallest way that works. Then we fly a thousand flights, and look for the next thing that goes wrong.
The flights are simulated. A small program in Zig plays the field, the wind, the drone and its sensors, and our filter listens to the sensors as it would in the air. The code is real: every snippet in the book belongs to that one program, and every run quoted in the book is its real output, checked every time the book is built. When a chapter changes earlier code, you’ll see the old lines struck through, right above the new ones.
Key concept
Deterministic simulation
A simulation where everything that could vary, every gust of wind and every bit of sensor noise, comes from one number: the seed. The same seed always gives the same flight, on every machine, so a flight that went wrong can be replayed and studied as often as we like.
For the curious · safe to skip
Why simulate the drone at all?
A real drone would be more honest, and one day we should fly one. But a real flight has a flaw that no amount of care can fix: it never tells you the true height. You can only compare one sensor’s story with another’s. In a simulation the program knows the true height at every instant, so we can grade every estimate the filter makes, flight after flight, and say not just “this looks smoother” but “this is wrong by 4 centimetres on average, where the last version was wrong by 11”.
Who this book is for
You know how to program, and you have an engineering background: state-space models, a PID loop or two, some linear algebra and probability. You may well have seen the Kalman filter’s equations, perhaps even used them, and found that they work without quite knowing why. Where does the gain come from? Why does that covariance shrink? What are Q and R really, and why does everyone tune them by hand?
This book answers those questions by building the filter, one small step at a time, rather than by deriving it. The maths arrives as we need it, worked on small numbers first, and is always checked against a running program.
The road ahead
The next chapter builds the smallest flight we can: a drone that hovers, perfectly still, ten metres up, and Baro alone, reading its height. We’ll take Baro at its word, and watch the drone shiver.
Challenges
- Your phone has an accelerometer and, very likely, a barometer. Which one would you use to tell which floor of a building you’re on? Which one to count your steps? Why not the other way round?
- Acc’s bias was one hundredth of a metre per second squared, and its height drifted by 18 metres in a minute. How long does it take to drift by one metre? If the bias were ten times smaller, how much longer would we have?
- Baro says 3.2 metres, and is usually within 30 centimetres of the truth. Echo says 2.9 metres, and is usually within one centimetre. Before reading on, write down what you’d believe, and why. Then ask the same question when they say 3.2 and 2.1. Keep your answers; we’ll check them in a few chapters.
Design note: where this road leads
The idea of weighing noisy readings by how much we trust them is old: Gauss and Legendre fitted planetary orbits with least squares around 1800. What changed in 1960 was doing it recursively, one reading at a time, for a state that moves. Rudolf Kálmán’s paper “A New Approach to Linear Filtering and Prediction Problems” did exactly that, and within a few years Stanley Schmidt’s group at NASA Ames had stretched it to the nonlinear equations of spaceflight; that stretched version, the extended Kalman filter, helped guide Apollo to the Moon.
Those are the book’s destinations, and a few more beyond them: the Rauch–Tung–Striebel smoother of 1965, which revisits a finished flight backwards; the unscented Kalman filter of Julier and Uhlmann (1997), which handles nonlinear sensors by pushing a handful of carefully chosen points through them instead of linearising; and particle filters (Gordon, Salmond and Smith’s “bootstrap filter”, 1993), which give up on the bell curve altogether and represent a belief as a cloud of guesses.
We won’t start from any of these papers. We’ll start from a drone that shivers, and rediscover each idea because a flight went wrong without it. The papers are where we’ll check our answers.