Kalman Filter Explained

GPS drifts. IMU accumulates error. But combined with a Kalman filter, they give you smooth, accurate position estimates. The Kalman filter is the optimal linear estimator — it mathematically minimises mean-square error by fusing a model prediction with noisy measurements.

1. The Estimation Problem

You want to know the true state of a system — say, a drone's position and velocity. You have two imperfect sources of information:

Model prediction: "Given the last state and my control inputs (thrust, etc.), where should it be?" — accurate short-term but drifts due to unmodeled forces.
Measurement: GPS gives position directly — but it's noisy (±3 m) and slow (1–10 Hz).

The Kalman filter combines both optimally. It weights each source inversely by its uncertainty: trust the measurement more when the model is uncertain; trust the model more when the sensor is noisy.

2. State Space Model

A linear dynamic system is described by two equations:

State transition and measurement model x_k = A·x_{k-1} + B·u_k + w_k (process model) y_k = C·x_k + v_k (measurement model) x — state vector (position, velocity, …) A — state transition matrix B — control input matrix; u — control input C — observation matrix (maps state → measurement) w ~ N(0, Q) — process noise; v ~ N(0, R) — measurement noise

For a 1D constant-velocity model with position x and velocity v:
A = [[1, dt],[0, 1]]; C = [1, 0] (we measure position only).

3. Predict–Update Cycle

Each timestep runs two phases:

🔵 Predict (time update)

Project state forward using the model.

x̂⁻ₖ = A·x̂_{k-1} + B·u_k
P⁻ₖ = A·P_{k-1}·Aᵀ + Q

P — error covariance matrix
Q — process noise covariance

🟢 Update (measurement update)

Correct the prediction with measurement.

K = P⁻·Cᵀ·(C·P⁻·Cᵀ + R)⁻¹
x̂ₖ = x̂⁻ₖ + K·(yₖ − C·x̂⁻ₖ)
Pₖ = (I − K·C)·P⁻ₖ

K — Kalman gain

The term (y_k − C·x̂⁻_k) is called the innovation or residual — how much the measurement differs from the prediction. The Kalman gain K scales how much of this residual to use.

4. Kalman Gain Intuition

The Kalman gain K ∈ [0, 1] represents trust in the measurement:

K → 1: trust the measurement completely (P⁻ huge, R tiny — model very uncertain, sensor very accurate)
K → 0: trust the model prediction (P⁻ tiny, R huge — model accurate, sensor very noisy)

For the scalar 1D case: K = P⁻ / (P⁻ + R). If your model uncertainty P⁻ = 9 m² and GPS variance R = 4 m²: K = 9/(9+4) ≈ 0.69 — lean toward the GPS.

Steady-state Kalman filter: If A, C, Q, R are constant, the covariance P converges to a steady-state value after a few iterations. You can precompute the steady-state K offline — very efficient for embedded systems.

5. Extended and Unscented KF

Extended Kalman Filter (EKF)

When the state transition or measurement model is nonlinear — e.g. a robot turning with a sinusoidal position update — the standard KF breaks. The EKF linearises nonlinear functions via Jacobian matrices at each step:

EKF linearisation x_k = f(x_{k-1}, u_k) + w_k (nonlinear process) y_k = h(x_k) + v_k (nonlinear measurement) F = ∂f/∂x |_{x̂} (Jacobian of f evaluated at x̂) H = ∂h/∂x |_{x̂} (Jacobian of h)

EKF replaces A with F and C with H in the standard equations. It's the most popular filter for robotics (ROS robot_localization, autonomous vehicles SLAM).

Unscented Kalman Filter (UKF)

EKF can diverge for highly nonlinear systems because the Jacobian is only a first-order approximation. The UKF uses deterministic sigma points (2n+1 carefully chosen state samples) to propagate the distribution through the nonlinear function — achieving third-order accuracy without computing any Jacobians.

6. Applications

GPS + IMU fusion: GPS provides absolute position at 10 Hz; IMU accelerometer at 1000 Hz. Kalman filter fuses them: smooth, 1000 Hz position with GPS drift correction.
Tracking radars: Apollo guidance computer used a Kalman filter to navigate to the Moon (1960s). Rudolf Kálmán derived it in 1960.
Financial time series: Estimating hidden volatility from noisy price data.
Computer vision: Tracking bounding boxes across frames (SORT, DeepSORT use Kalman filter as the motion predictor).
Audio noise reduction: Kalman filter estimates clean speech from noisy microphone signal.

7. JavaScript Implementation (1D Constant Velocity)

// 1D Kalman filter — track position from noisy measurements
class KalmanFilter1D {
  constructor({ processNoise = 0.1, measurementNoise = 4 } = {}) {
    this.Q = processNoise;       // process noise variance
    this.R = measurementNoise;   // measurement noise variance
    this.x = 0;   // state estimate (position)
    this.P = 1;   // error covariance
    this.dt = 1;  // time step
  }

  predict(velocity = 0) {
    // State transition: pos += vel * dt
    this.x = this.x + velocity * this.dt;
    // Covariance grows with process noise
    this.P = this.P + this.Q;
  }

  update(measurement) {
    const K = this.P / (this.P + this.R);   // Kalman gain
    this.x = this.x + K * (measurement - this.x);  // update
    this.P = (1 - K) * this.P;             // reduce uncertainty
    return this.x;
  }
}

// Usage: fuse noisy GPS with velocity prediction
const kf = new KalmanFilter1D({ processNoise: 0.5, measurementNoise: 9 });
const gpsReadings = [10.2, 10.9, 12.1, 13.8, 15.3];  // noisy

for (const gps of gpsReadings) {
  kf.predict(/*velocity=*/ 1.5);
  const estimate = kf.update(gps);
  console.log(`GPS: ${gps.toFixed(1)}, Estimate: ${estimate.toFixed(2)}`);
}

2D/multidimensional: For position + velocity in 2D, x is a 4-element vector [px, py, vx, vy], A is a 4×4 matrix, and you need matrix multiplication. Libraries like math.js or a custom micro-matrix implementation handle this cleanly.