Kalman Filter

Wikipedia · Kalman filter · CC BY-SA 4.0

The Kalman filter estimates the state of a linear system from noisy measurements. Each step has two phases: predict (use the model to forecast the next state) and update (correct the forecast using the measurement). It is optimal in the least-squares sense for Gaussian noise.

Prediction and update

Predict: propagate the state estimate forward using the system model. The uncertainty grows. Update: incorporate the new measurement. The Kalman gain weights how much to trust the measurement versus the prediction. If the measurement is precise (low noise), the gain is high and you trust the sensor. If the model is precise (low process noise), the gain is low and you trust the prediction.

The Kalman gain

K = P_predicted * H' * (H * P_predicted * H' + R)^(-1). Here P is the error covariance, H is the measurement matrix, and R is the measurement noise covariance. When R is large (noisy sensor), K is small and the filter mostly trusts the model. When P is large (uncertain model), K is large and the filter mostly trusts the measurement. Over time, P shrinks and the estimate converges.

Scheme

; Scalar Kalman filter: estimate a constant from noisy measurements
; True value: 42.0
; Measurement noise variance R = 25.0

(define true-value 42.0)
(define R 25.0)  ; measurement noise variance
(define Q 0.01)  ; process noise variance (small, nearly constant)

(define (kalman x-est P measurement)
  (let* (;; Predict (constant model: x-predicted = x-est)
         (x-pred x-est)
         (P-pred (+ P Q))
         ;; Update
         (K (/ P-pred (+ P-pred R)))
         (innovation (- measurement x-pred))
         (x-new (+ x-pred (* K innovation)))
         (P-new (* (- 1.0 K) P-pred)))
    (list x-new P-new K)))

; Simulate noisy measurements around true value
(define measurements
  (list 47 38 51 35 44 39 48 36 43 41 45 40 42 38 44))

(define (run-filter measurements x-est P step)
  (if (null? measurements)
    (begin (display "Final estimate: ") (display x-est)
           (display " (true: ") (display true-value) (display ")") (newline))
    (let* ((z (car measurements))
           (result (kalman x-est P z))
           (new-x (car result))
           (new-P (car (cdr result)))
           (K (car (cdr (cdr result)))))
      (display "  z=") (display z)
      (display "  est=") (display new-x)
      (display "  K=") (display K) (newline)
      (run-filter (cdr measurements) new-x new-P (+ step 1)))))

(display "Kalman filter (true=42):") (newline)
(run-filter measurements 50.0 100.0 0)

; Scalar Kalman filter: estimate a constant from noisy measurements
; True value: 42.0
; Measurement noise variance R = 25.0

(define true-value 42.0)
(define R 25.0)  ; measurement noise variance
(define Q 0.01)  ; process noise variance (small, nearly constant)

(define (kalman x-est P measurement)
  (let* (;; Predict (constant model: x-predicted = x-est)
         (x-pred x-est)
         (P-pred (+ P Q))
         ;; Update
         (K (/ P-pred (+ P-pred R)))
         (innovation (- measurement x-pred))
         (x-new (+ x-pred (* K innovation)))
         (P-new (* (- 1.0 K) P-pred)))
    (list x-new P-new K)))

; Simulate noisy measurements around true value
(define measurements
  (list 47 38 51 35 44 39 48 36 43 41 45 40 42 38 44))

(define (run-filter measurements x-est P step)
  (if (null? measurements)
    (begin (display "Final estimate: ") (display x-est)
           (display " (true: ") (display true-value) (display ")") (newline))
    (let* ((z (car measurements))
           (result (kalman x-est P z))
           (new-x (car result))
           (new-P (car (cdr result)))
           (K (car (cdr (cdr result)))))
      (display "  z=") (display z)
      (display "  est=") (display new-x)
      (display "  K=") (display K) (newline)
      (run-filter (cdr measurements) new-x new-P (+ step 1)))))

(display "Kalman filter (true=42):") (newline)
(run-filter measurements 50.0 100.0 0)

Neighbors

📊 OpenIntro Ch. 5 — Inference — the Kalman update is Bayesian inference with Gaussian priors
🎰 Grinstead Ch. 6 — Expected Value — the Kalman filter minimizes expected squared error
🎰 Probability Ch.4 — the Kalman filter is Bayesian updating for Gaussian models
📐 Linear Algebra Ch.5 — eigenvalues of the covariance matrix determine filter gain
🤖 ML Ch.7 — Gaussian processes are infinite-dimensional Kalman filters

← Optimal Control by june.kim Lyapunov Stability →