An Interactive Guide through the Kalman Algorithm

01 · Noise

A sensor that never sits still

An aircraft holds exactly 1,000 metres. Its altimeter does not agree, and it disagrees differently every time you ask.

Every sensor adds noise. Electronics hum, air pressure flutters, a GPS signal bounces off a building. The reading you get is the true value plus an error you cannot predict. If you plotted enough of those errors in a histogram, most sensors would give you the familiar bell curve, a normal orGaussian distribution. Small errors are common and large errors are rare.

One number describes how wide that bell is, the standard deviation σ. About two readings in three land within one σ of the truth, and about 95% land within two σ. Drag the noise slider and watch the dots spread out.

To get noise like this in code, you need two things. The first is a random number generator that you can seed, so every reader of this page sees the same noise and every test gets the same numbers. The second is a way to turn uniform random numbers into bell-shaped ones, which the Box-Muller transform does in two lines.

random.tsTypeScript

export function seeded(seed: number): Random {
  let state = seed >>> 0;
  return () => {
    state = (state + 0x6d2b79f5) >>> 0;
    let t = state;
    t = Math.imul(t ^ (t >>> 15), t | 1);
    t ^= t + Math.imul(t ^ (t >>> 7), t | 61);
    return ((t ^ (t >>> 14)) >>> 0) / 4294967296;
  };
}

export function noise(random: Random, sigma: number): number {
  const u = 1 - random();
  const v = random();
  return sigma * Math.sqrt(-2 * Math.log(u)) * Math.cos(2 * Math.PI * v);
}

Everything that follows is a way to look at a stream of numbers like these and work out where the aircraft really is.

02 · Averaging

Averaging works until the aircraft moves

The first fix every programmer reaches for is an average. It works well until the aircraft starts to climb.

A plain average of every reading so far converges on the truth while the aircraft holds altitude. Once it climbs, the old readings drag the answer down. The usual fix is an exponential moving average. It keeps one running value and nudges it towards each new reading by a fixed fraction α.

\hat{x}_{n} = \hat{x}_{n-1} + \alpha\left(z_n - \hat{x}_{n-1}\right)

Read it from right to left. $z_n - \hat{x}_{n-1}$ is how surprised we are by the new reading $z_n$ . We move our estimate $\hat{x}$ a fraction α of the way towards it. The hat on the x is the convention for "our estimate of", as opposed to the true value.

gaussian.tsTypeScript

export function smooth(previous: number, reading: number, alpha: number): number {
  return previous + alpha * (reading - previous);
}

This shape, old value plus a factor times the surprise, is the heart of the Kalman filter. The problem with the moving average is that you pick α once, by hand, and it never changes. A small α trusts the past and lags behind. A large α trusts each reading and jitters.

The right α depends on two things the moving average knows nothing about. One is how noisy the sensor is. The other is how much the aircraft could have moved since the last reading. The Kalman filter tracks both, and computes a fresh α at every step. That α is called the Kalman gain. The next three chapters are about how it is computed.

03 · Uncertainty

Uncertainty is a number you can carry

The filter never stores just "the altitude is 1,004 m". It stores "the altitude is 1,004 m, give or take 3".

That one change is most of the trick. If the estimate carries its own uncertainty, then a new reading is simply a second opinion with its own uncertainty, and you can combine the two opinions properly. Both opinions are bell curves, so each is described by two numbers, a mean and a variance. The variance is σ², the standard deviation squared. Variances are easier to compute with than standard deviations, so the filter works in variances.

gaussian.tsTypeScript

export interface Gaussian {
  mean: number;
  variance: number;
}

Multiply two bell curves together and you get a third bell curve. That product answers the question "where is the aircraft, given both opinions?" Its mean and variance work out to:

\mu = \frac{\sigma_2^2\,\mu_1 + \sigma_1^2\,\mu_2}{\sigma_1^2 + \sigma_2^2} \qquad \sigma^2 = \frac{\sigma_1^2\,\sigma_2^2}{\sigma_1^2 + \sigma_2^2}

Each mean is weighted by the other variance, so the more certain opinion pulls harder. The combined variance is smaller than either input. Two opinions together always know more than either one alone. Try it.

Now rewrite that formula in the shape of the moving average. Call the variance of our belief $p$ and the variance of the sensor $r$ . Then, in the notation of kalmanfilter.net:

K_n = \frac{p_{n,n-1}}{p_{n,n-1} + r_n} \qquad \hat{x}_{n,n} = \hat{x}_{n,n-1} + K_n\left(z_n - \hat{x}_{n,n-1}\right) \qquad p_{n,n} = \left(1 - K_n\right)p_{n,n-1}

The subscripts read as "at step n, given what we knew at step n − 1". So $\hat{x}_{n,n-1}$ is the estimate for step n before its reading arrived, and $\hat{x}_{n,n}$ is the estimate after. The middle equation is the moving average again, with α replaced by $K_n$ . The gain is our uncertainty divided by the total uncertainty. If we are unsure and the sensor is precise, K is near 1 and we jump to the reading. If we are sure and the sensor is poor, K is near 0 and we barely move.

gaussian.tsTypeScript

export function update(prior: Gaussian, measurement: Gaussian): Gaussian {
  const gain = prior.variance / (prior.variance + measurement.variance);
  return {
    mean: prior.mean + gain * (measurement.mean - prior.mean),
    variance: (1 - gain) * prior.variance
  };
}

That function is the complete update step of a one-dimensional Kalman filter. It is four lines long.

04 · Predict and update

Predict, then correct

Between two readings the aircraft moves. The filter has to move its belief along with it, and admit that it is now less sure.

Say the autopilot holds a climb of 5 m/s and readings arrive once a second. Before the next reading arrives, the best guess is "5 m higher than last time". That guess is the motion model. It is not perfect, because turbulence pushes the aircraft around in ways the autopilot did not ask for. So the prediction also adds some uncertainty, the process noise $q$ .

\hat{x}_{n+1,n} = \hat{x}_{n,n} + v\,\Delta t \qquad p_{n+1,n} = p_{n,n} + q

Predicting is adding two bell curves, the belief and the motion. Means add and variances add. Every prediction makes the bell wider and every update makes it narrower. Step through it below. Watch the orange bell widen on predict and narrow on update, while the black line marks the truth the filter cannot see.

gaussian.tsTypeScript

export function predict(belief: Gaussian, motion: Gaussian): Gaussian {
  return {
    mean: belief.mean + motion.mean,
    variance: belief.variance + motion.variance
  };
}

Put the two steps in a loop and you have a working Kalman filter. This is the whole algorithm in one dimension.

gaussian.tsTypeScript

export function filterAltitude(
  readings: number[],
  climbRates: number[],
  dt: number,
  turbulence: number,
  altimeter: number,
  start: Gaussian
): Step[] {
  const steps: Step[] = [];
  let belief = start;
  for (let k = 0; k < readings.length; k++) {
    const predicted = predict(belief, { mean: climbRates[k] * dt, variance: turbulence ** 2 });
    belief = update(predicted, { mean: readings[k], variance: altimeter ** 2 });
    steps.push({ predicted, estimate: belief });
  }
  return steps;
}

After a few steps the variance stops changing. The widening from each predict and the narrowing from each update balance out, and the gain settles at a constant value. For a filter with a fixed q and r, the gain is the ideal α for the moving average, worked out from the noise instead of guessed.

05 · Tuning Q and R

Tuning with Q and R

The filter is only as good as what you tell it about the world. You tell it two things, and both are easy to get wrong.

$R$ is how noisy you believe the sensor is. You can usually look it up in a datasheet or measure it by logging a sensor that is sitting still. $Q$ is how much you believe the world deviates from your motion model between steps. Nobody publishes a datasheet for turbulence, so Q is where most tuning effort goes. In one dimension they are the lowercase q and r from before. In the matrix version they become matrices, hence the capitals.

The chart below shows the error of every reading and of the filter over two minutes, and the 2σ band the filter claims for itself. A healthy filter is both accurate and honest. Its error is small, and it stays inside its own band about 95% of the time. Try the presets, then drag the sliders.

The overconfident filter is the dangerous one. It believes its model so much that it ignores readings. Its band settles at ±1.3 m while it is regularly 4 or 5 m off, and nothing downstream can tell that apart from a filter that really is that good. A filter that is too nervous is merely noisy. When in doubt, err on the side of a larger Q.

The gain chart shows the other half of the story. It starts high, because the filter knows nothing and trusts the first readings. It then settles to the constant value from the last chapter. Lower Q gives a lower settled gain, which means more smoothing and more lag.

06 · Hidden state

Estimating what you never measure

So far the filter was told the climb rate. Take that away, and the filter works out the climb rate on its own from nothing but altitude readings.

Real aircraft do not hand the filter their exact climb rate. So make vertical speed part of what the filter estimates. The state is now two numbers, altitude $h$ and vertical speed $\dot{h}$ . The motion model says the new altitude is the old altitude plus speed times time, and the speed stays the same unless something changes it. Written as a matrix, that model is $F$ , the state transition matrix.

x = \begin{bmatrix} h \\ \dot{h} \end{bmatrix} \qquad F = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \qquad H = \begin{bmatrix} 1 & 0 \end{bmatrix}

$H$ is the observation matrix. It says what the sensor sees of the state, which here is the altitude and not the speed. The single variance p becomes a 2×2covariance matrix $P$ . The diagonal holds the variance of the altitude and the variance of the speed. The two off-diagonal entries hold how the two errors move together, and those are what make the filter work.

After a predict, an error in speed has turned into an error in altitude. The filter records that link in P. When the next altitude reading surprises it, the filter knows part of that surprise came from a wrong speed, and it corrects the speed too. That is how a value no sensor reports ends up estimated.

TypeScript has no matrices, so we need a few helpers. Arrays of rows keep the code readable, which matters more here than speed.

matrix.tsTypeScript

export type Matrix = number[][];

export function multiply(a: Matrix, b: Matrix): Matrix {
  return a.map((row) => b[0].map((_, j) => row.reduce((sum, value, k) => sum + value * b[k][j], 0)));
}

export function add(a: Matrix, b: Matrix): Matrix {
  return a.map((row, i) => row.map((value, j) => value + b[i][j]));
}

export function subtract(a: Matrix, b: Matrix): Matrix {
  return a.map((row, i) => row.map((value, j) => value - b[i][j]));
}

export function transpose(a: Matrix): Matrix {
  return a[0].map((_, j) => a.map((row) => row[j]));
}

export function identity(size: number): Matrix {
  return Array.from({ length: size }, (_, i) => Array.from({ length: size }, (_, j) => (i === j ? 1 : 0)));
}

Now the predict and update steps from the one-dimensional filter, written once for any number of states. Put the two versions side by side and every line has a partner. Addition stays addition. Multiplying by F replaces "add v times Δt". Dividing by p + r becomes multiplying by the inverse of $S$ , the uncertainty of the predicted reading.

filter.tsTypeScript

export interface Belief {
  x: Matrix;
  P: Matrix;
}

export interface Model {
  F: Matrix;
  Q: Matrix;
  G?: Matrix;
}

export interface Sensor {
  H: Matrix;
  R: Matrix;
}

filter.tsTypeScript

export function predict({ x, P }: Belief, { F, Q, G }: Model, u?: Matrix): Belief {
  const moved = multiply(F, x);
  return {
    x: G && u ? add(moved, multiply(G, u)) : moved,
    P: add(multiply(multiply(F, P), transpose(F)), Q)
  };
}

filter.tsTypeScript

export function update({ x, P }: Belief, { H, R }: Sensor, z: Matrix): Belief {
  const innovation = subtract(z, multiply(H, x));
  const S = add(multiply(multiply(H, P), transpose(H)), R);
  const K = multiply(multiply(P, transpose(H)), invert(S));
  return {
    x: add(x, multiply(K, innovation)),
    P: multiply(subtract(identity(x.length), multiply(K, H)), P)
  };
}

Scrub through the climb at 10 to 15 seconds. Right after the climb starts, the estimated speed lags behind, because the filter needs a few surprising readings before it believes the aircraft really changed what it was doing. Raise the assumed acceleration and it reacts sooner, at the cost of a noisier speed estimate. It is the same trade-off as α, now made for you from the noise numbers.

filter.tsTypeScript

export function constantVelocity(dt: number, acceleration: number): Model {
  const q = acceleration ** 2;
  return {
    F: [
      [1, dt],
      [0, 1]
    ],
    Q: [
      [(q * dt ** 3) / 3, (q * dt ** 2) / 2],
      [(q * dt ** 2) / 2, q * dt]
    ]
  };
}

07 · Sensor fusion

Two sensors, two clocks

Sensor fusion sounds like a separate subject. It is the update step, called once per reading, with each sensor's own H and R.

Our aircraft now has a barometric altimeter that reports ten times a second with σ 3 m, and a GPS that reports once a second with σ 8 m. They do not take turns politely. So the filter treats time as a stream of events. For each event it predicts forward to the event's timestamp, then updates with whichever sensor produced it. A clock event with no reading only predicts, which lets us draw the filter's belief at a steady rate even when nothing arrives.

In code, that becomes a discriminated union of events and a lookup table from sensor to its H and R. Adding a third sensor means adding a row to the table, not another branch.

vertical.tsTypeScript

export type AltitudeSensor = 'baro' | 'gps';

export type AltitudeEvent =
  | { kind: AltitudeSensor; t: number; altitude: number }
  | { kind: 'clock'; t: number };

vertical.tsTypeScript

export function trackAltitude(
  events: AltitudeEvent[],
  sensors: Record<AltitudeSensor, Sensor>,
  acceleration: number
): VerticalStep[] {
  const steps: VerticalStep[] = [];
  let belief: Belief = { x: column(1000, 0), P: diagonal(100, 25) };
  let t = events[0].t;
  for (const event of events) {
    const predicted = predict(belief, constantVelocity(event.t - t, acceleration));
    belief = event.kind === 'clock' ? predicted : update(predicted, sensors[event.kind], [[event.altitude]]);
    t = event.t;
    steps.push({ t, predicted, estimate: belief, event });
  }
  return steps;
}

Turn the barometer off and the filter leans on GPS alone, so its band widens between each 1 Hz fix. Turn GPS off and little changes, because the barometer is both faster and more precise here. Lose both and the band balloons, because the filter is predicting with no correction. It still knows how fast it was climbing, so it keeps a good guess for a while, and it says honestly how unsure it is becoming. When the readings return, the gain jumps and the band snaps back.

08 · The general form

The general form, line by line

These are the five equations that most explanations open with. By now each one should read like a line of code you have already seen.

The notation follows the summary on kalmanfilter.net. The only new symbol is $G$ , the control matrix. It feeds a known input $u$ into the prediction, the way the autopilot's climb rate did in chapter 4. Other texts call it $B$ .

Predict the state
Move the estimate forward with the motion model and any known input.
$\hat{x}_{n+1,n} = F\hat{x}_{n,n} + Gu_n$
```
x: G && u ? add(moved, multiply(G, u)) : moved
```
Predict the uncertainty
Carry the uncertainty along with the state, then add what the model cannot explain.
$P_{n+1,n} = FP_{n,n}F^{T} + Q$
```
P: add(multiply(multiply(F, P), transpose(F)), Q)
```
Kalman gain
Our uncertainty, divided by our uncertainty plus the sensor's. The matrix version of p / (p + r).
$K_n = P_{n,n-1}H^{T}\left(HP_{n,n-1}H^{T} + R_n\right)^{-1}$
```
const S = add(multiply(multiply(H, P), transpose(H)), R);
const K = multiply(multiply(P, transpose(H)), invert(S));
```
Update the state
Move towards the reading by the gain times the surprise.
$\hat{x}_{n,n} = \hat{x}_{n,n-1} + K_n\left(z_n - H\hat{x}_{n,n-1}\right)$
```
const innovation = subtract(z, multiply(H, x));
x: add(x, multiply(K, innovation))
```
Update the uncertainty
Shrink the uncertainty by as much as the reading was trusted.
$P_{n,n} = \left(I - K_nH\right)P_{n,n-1}$
```
P: multiply(subtract(identity(x.length), multiply(K, H)), P)
```

kalmanfilter.net writes the last equation in a longer form, the Joseph form, which adds a term to stay numerically safe when K is not quite optimal. With 64-bit floats and well-behaved matrices like ours, the short form above works, and it is the one most code uses. If you ever see P lose its symmetry or grow a negative variance, switch to the Joseph form first.

The only operation the helpers above did not cover is the inverse of S. For the filters in this guide S is never larger than 2×2, but a small Gauss-Jordan elimination handles any size.

matrix.tsTypeScript

export function invert(a: Matrix): Matrix {
  const n = a.length;
  const m = a.map((row, i) => [...row, ...identity(n)[i]]);
  for (let col = 0; col < n; col++) {
    let pivot = col;
    for (let row = col + 1; row < n; row++) {
      if (Math.abs(m[row][col]) > Math.abs(m[pivot][col])) pivot = row;
    }
    [m[col], m[pivot]] = [m[pivot], m[col]];
    const scale = m[col][col];
    m[col] = m[col].map((value) => value / scale);
    for (let row = 0; row < n; row++) {
      if (row === col) continue;
      const factor = m[row][col];
      m[row] = m[row].map((value, j) => value - factor * m[col][j]);
    }
  }
  return m.map((row) => row.slice(n));
}

09 · Flying the aircraft

Flying the whole aircraft

Three dimensions, three attitude angles, three kinds of sensor, and time as the fourth axis. The finale uses no new algorithm. It runs six of the filters you already know, side by side.

A four-and-a-half minute flight, filtered

True path and aircraft
GPS fix
Filter estimate
2σ position

The 3D view loads when you get here

Aircraft drawn 4× actual size · drag to orbit outside chase view

Time: –
Last GPS fix: –
Altitude: –
Bank: –
Pitch: –
Heading: –
Position error: –
Gyro bias, roll · pitch · yaw: –

0:00

GPS outage, 2:08 to 2:40 Estimate gyro bias

Sensor noise

Horizontal position error against the filter's own 2σ. Click to jump.

Bank angle, estimate against truth. Click to jump.

The aircraft takes off heading west, makes a climbing left turn, cruises, then descends through a right turn towards the approach. Three sensors report on it, each at its own rate:

A GPS receiver at 1 Hz. It gives latitude, longitude, and altitude, plus a velocity measured from the Doppler shift of the satellite signals.
A barometric altimeter at 10 Hz.
Three gyroscopes at 50 Hz, one each for roll, pitch, and yaw. Each has a bias, a small constant error, of up to 0.6°/s.

Press play, switch cameras, and drag the timeline. The orange aircraft is the filter's estimate. The faint black one is the truth that the filter never sees. The orange shell around the estimate is its 2σ position uncertainty at true size, so it is too small to see until GPS goes away.

One event stream, one reducer

Every reading becomes a SensorEvent, sorted by time. GPS arrives as latitude and longitude, because that is what GPS produces. It is converted to metres east, north, and up from a reference point, and that conversion is the only place in the code that knows about degrees. A flat-earth projection is good to a few metres over the ten kilometres this flight covers, which is about the size of the GPS noise. The simulator uses the same projection, so here it adds no error at all.

sensors.tsTypeScript

export type SensorEvent =
  | { kind: 'imu'; t: number; gyro: Rates }
  | { kind: 'baro'; t: number; altitude: number }
  | { kind: 'gps'; t: number; fix: Geodetic; velocity: Enu };

geo.tsTypeScript

export function toEnu(fix: Geodetic, reference: Geodetic = origin): Enu {
  return {
    east: (fix.lon - reference.lon) * radians * earthRadius * Math.cos(reference.lat * radians),
    north: (fix.lat - reference.lat) * radians * earthRadius,
    up: fix.alt - reference.alt
  };
}

The filter's whole memory is one plain object. Three position filters, one per axis, each tracking a position and a velocity. Three attitude filters, each tracking an angle and the bias of its gyro. Plus the last gyro reading, which one of the measurements below needs.

aircraft.tsTypeScript

export interface AircraftBelief {
  t: number;
  position: Record<Axis, Belief>;
  attitude: Record<Angle, Belief>;
  gyro: Rates;
  lastImu: number;
}

Position is chapter 6, three times

Each axis runs the constant velocity filter from chapter 6. GPS updates all three axes at once with both position and velocity, so $H$ is the 2×2 identity and the update uses the full matrix form. The barometer updates only the up axis, exactly as in chapter 7. Because the axes are independent, the 3D uncertainty is an ellipsoid aligned with the axes, and the scene can draw it as a sphere stretched by 2σ in each direction.

Turn on the GPS outage. It falls on the straight cruise leg, and the shell grows to hundreds of metres while the estimate carries on in a straight line. The real error stays inside the shell, so the filter is uncertain but honest. A constant velocity filter can only ever fly straight, so an outage in a turn would go badly. Real navigation systems fill that gap with accelerometers, which is a topic for a later article.

Attitude is chapter 6 with a gyro

A gyro measures how fast an angle changes, not the angle itself. Integrating it gives you the angle, plus the bias times the elapsed time, so even a 0.5°/s bias puts you 30° off within a minute. The bias is the hidden state this time. Each attitude filter tracks the angle θ and the bias b. The gyro reading ω is the known input u, which goes through the control matrix G from the general form.

\theta_{n+1} = \theta_n + (\omega_n - b_n)\,\Delta t \quad\Longrightarrow\quad F = \begin{bmatrix} 1 & -\Delta t \\ 0 & 1 \end{bmatrix} \quad G = \begin{bmatrix} \Delta t \\ 0 \end{bmatrix} \quad u_n = \omega_n

aircraft.tsTypeScript

export function gyroModel(dt: number, gyroNoise: number, estimateBias: boolean): Model {
  return {
    F: [
      [1, -dt],
      [0, 1]
    ],
    G: [[dt], [0]],
    Q: [
      [(gyroNoise * dt) ** 2, 0],
      [0, estimateBias ? biasDrift ** 2 * dt : 0]
    ]
  };
}

To learn the bias, the filter needs something that measures the angles directly, even roughly. The aircraft's motion provides it. Heading is the direction of the GPS velocity. Pitch is the climb angle plus a typical angle of attack. Bank comes from the physics of a coordinated turn, where turn rate, airspeed, and bank angle are tied together:

\tan\varphi = \frac{V\,\dot{\psi}}{g} \qquad \gamma = \arctan\frac{v_\text{up}}{v_\text{ground}} \qquad \psi = \operatorname{atan2}\left(v_\text{east},\, v_\text{north}\right)

aircraft.tsTypeScript

export function attitudeFromMotion(velocity: Enu, yawRate: number): Rates {
  const groundSpeed = Math.hypot(velocity.east, velocity.north);
  const airspeed = Math.hypot(groundSpeed, velocity.up);
  return {
    roll: Math.atan((airspeed * yawRate) / gravity),
    pitch: Math.atan2(velocity.up, groundSpeed) + nominalAngleOfAttack,
    yaw: Math.atan2(velocity.east, velocity.north)
  };
}

Untick estimate gyro bias to see why the hidden bias state matters. The filter now believes its gyros are perfect. Its uncertainty stays tiny, it gives the GPS-derived angles almost no weight, and the attitude drifts tens of degrees away from the truth while the filter remains confident. That is the overconfident filter from chapter 5 again, this time caused by a model that is missing a state rather than by a bad Q.

The step function

Here is everything joined up. Every event moves the position filters forward in time. Then the kind of event decides which update runs. A pure function from the old belief and one event to the new belief means the whole flight is a fold over the event list. The 3D view stores a frame for every 50 ms, and the timeline just picks one. Scrubbing backwards is an array lookup.

aircraft.tsTypeScript

export function step(belief: AircraftBelief, event: SensorEvent, tuning: Tuning): AircraftBelief {
  const dt = event.t - belief.t;
  const position = map(belief.position, axes, (axis, key) => predict(axis, constantVelocity(dt, acceleration[key])));
  const moved = { ...belief, t: event.t, position };
  const { baro, gps, attitude } = sensors(tuning.noiseScale);

  switch (event.kind) {
    case 'imu': {
      const dtImu = event.t - belief.lastImu;
      const model = gyroModel(dtImu, sensorSpecs.imu.gyro * tuning.noiseScale, tuning.estimateBias);
      return {
        ...moved,
        attitude: map(belief.attitude, angles, (angle, key) => predict(angle, model, [[event.gyro[key]]])),
        gyro: event.gyro,
        lastImu: event.t
      };
    }
    case 'baro':
      return { ...moved, position: { ...position, up: update(position.up, baro, [[event.altitude]]) } };
    case 'gps': {
      const point = toEnu(event.fix);
      const fixed = map(position, axes, (axis, key) =>
        update(axis, gps[key], column(point[key], event.velocity[key]))
      );
      const velocity = map(fixed, axes, rate);
      const yawRate = belief.gyro.yaw - rate(belief.attitude.yaw);
      const measured = attitudeFromMotion(velocity, yawRate);
      return {
        ...moved,
        position: fixed,
        attitude: map(belief.attitude, angles, (angle, key) => {
          const z = value(angle) + wrap(measured[key] - value(angle));
          return update(angle, attitude[key], [[z]]);
        })
      };
    }
  }
}

aircraft.tsTypeScript

export function fly(truth: TruthSample[], events: SensorEvent[], tuning: Tuning): Frame[] {
  const frames: Frame[] = [];
  let belief = initialBelief(tuning.estimateBias);
  let next = 0;
  for (const event of events) {
    belief = step(belief, event, tuning);
    if (event.kind !== 'imu' || event.t < next - 1e-9) continue;
    const sample = truth[Math.round(event.t * truthRate)];
    frames.push({
      t: event.t,
      leg: sample.leg,
      truth: sample,
      estimate: poseOf(belief, value),
      sigma: poseOf(belief, sigma),
      bias: map(belief.attitude, angles, rate)
    });
    next += 1 / frameRate;
  }
  return frames;
}

The truth comes from a short simulator that flies this plan with a coordinated-turn model, small gusts, and an angle of attack that grows as the aircraft slows down. That last detail is why the pitch estimate is off by up to a degree. The pitch measurement assumes one fixed angle of attack, and the filter cannot do better than the measurement it is given.

flight.tsTypeScript

export interface Leg {
  name: string;
  until: number;
  bank: number;
  verticalSpeed: number;
  airspeed: number;
}

export const flightPlan: Leg[] = [
  { name: 'Climb out', until: 35, bank: 0, verticalSpeed: 6, airspeed: 75 },
  { name: 'Climbing left turn', until: 95, bank: -25, verticalSpeed: 5, airspeed: 80 },
  { name: 'Climb', until: 125, bank: 0, verticalSpeed: 5, airspeed: 85 },
  { name: 'Cruise', until: 165, bank: 0, verticalSpeed: 0, airspeed: 90 },
  { name: 'Descending right turn', until: 225, bank: 25, verticalSpeed: -4, airspeed: 85 },
  { name: 'Approach', until: 270, bank: 0, verticalSpeed: -3.5, airspeed: 75 }
];

Side note · quaternions and the extended Kalman filter

This aircraft cheats in two places, and both are on purpose. Its gyros report how fast roll, pitch, and heading change. Real strapdown gyros measure rotation about the aircraft's own axes, and turning that into angle rates is a rotation that depends on the current attitude. Roll, pitch, and heading also break down near vertical flight, a problem called gimbal lock. Real attitude filters therefore store orientation as a quaternion, four numbers that rotate smoothly in every direction.

Both fixes make the model nonlinear, and a nonlinear model cannot be written as a matrix F. Theextended Kalman filter handles that by linearising the model around the current estimate at every step, using its Jacobian, the matrix of partial derivatives. It then runs the same five equations you have seen here. Quaternions and the extended Kalman filter are the subject of a follow-up article.

10 · Beyond linear

Where the linear filter stops

The filter on this page is optimal when the motion is linear and the noise is Gaussian. Real systems are neither, and a family of variants deals with that.

The extended Kalman filter linearises a nonlinear model at every step. It is the standard in inertial navigation and the natural next step from this page.
The unscented Kalman filter skips the derivatives. It pushes a handful of carefully chosen sample points through the nonlinear model and measures where they land. It is often more accurate than the extended filter and easier to get right.
The error-state filter runs the gyros and accelerometers open-loop at full rate and uses a Kalman filter only to estimate the errors in that integration. Many inertial navigation systems work this way.
Particle filters drop the bell curve altogether and represent the belief as thousands of weighted guesses. They handle beliefs with two peaks, such as "the aircraft is over one of these two runways", which no Gaussian can represent.

All of them keep the loop you have built here. Predict with a model, compare with a reading, and move towards the reading in proportion to how much you trust it.

A sensor that never sits still

Averaging works until the aircraft moves

Uncertainty is a number you can carry

Predict, then correct

Tuning with Q and R

Estimating what you never measure

Two sensors, two clocks

The general form, line by line

Predict the state

Predict the uncertainty

Kalman gain

Update the state

Update the uncertainty

Flying the whole aircraft

One event stream, one reducer

Position is chapter 6, three times

Attitude is chapter 6 with a gyro

The step function

Where the linear filter stops

Further reading