4.4 Filtr Kalmana · Matematyka i fizyka od podstaw

0 Prolog: droga na Księżyc

Jesienią 1960 r. Rudolf Kálmán odwiedził NASA Ames i opowiedział o swojej nowej metodzie filtracji. Słuchał go Stanley Schmidt, który szukał sposobu na nawigację statku Apollo: komputer pokładowy miał łączyć model ruchu w polu grawitacyjnym Ziemi i Księżyca z rzadkimi, niedokładnymi pomiarami gwiazd i horyzontu. Metoda Kálmána była liniowa, a ruch statku nie. Zespół Schmidta zastosował ją więc do modelu zlinearyzowanego wokół bieżącej estymaty. Tak powstał rozszerzony filtr Kalmana (lekcja 4.5 Rozszerzony filtr Kalmana i fuzja czujników w jednej osi), który poleciał na Księżyc.

Dziś filtr Kalmana jest w każdym telefonie (GPS i czujniki ruchu), samolocie, dronie, robocie i systemie śledzenia rakiet. Ta lekcja pokazuje, że w środku nie ma magii, tylko średnia ważona niepewnościami, liczona w kółko.

Pytanie na start. Barometr drona myli się średnio o 30 cm, a akcelerometr ma mały stały błąd (0,1 m/s²), przez co po minucie całkowania pozycja ucieka o 180 m. Czy połączenie dwóch tak słabych czujników może dać estymatę wysokości lepszą niż 10 cm? Odpowiedź w części 4.

1 Łączenie dwóch niepewnych informacji

Dwa pomiary tej samej wielkości: $z_1$ z niepewnością (odchyleniem standardowym) $\sigma_1$ i $z_2$ z $\sigma_2$ . Najlepsza (o najmniejszej wariancji) estymata liniowa to średnia ważona odwrotnościami wariancji (lekcja 5.2 Statystyka pomiarów i dopasowania):

\hat x = \frac{\sigma_2^2\,z_1 + \sigma_1^2\,z_2}{\sigma_1^2 + \sigma_2^2},\qquad \sigma^2 = \frac{\sigma_1^2\sigma_2^2}{\sigma_1^2 + \sigma_2^2} .

Zapiszmy to inaczej: zaczynamy od $z_1$ i korygujemy o część różnicy:

\hat x = z_1 + K\,(z_2 - z_1),\qquad K = \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2},\qquad \sigma^2 = (1 - K)\,\sigma_1^2 .

To już jest cały filtr Kalmana. $z_1$ to przewidywanie modelu z niepewnością $P$ , $z_2$ to nowy pomiar z niepewnością $R$ , a $K = \frac{P}{P + R}$ to wzmocnienie Kalmana. Gdy model jest pewny ( $P \ll R$ ), $K \approx 0$ i pomiar prawie nic nie zmienia. Gdy pomiar jest pewny ( $R \ll P$ ), $K \approx 1$ i estymata przeskakuje na pomiar. Wynik jest zawsze pewniejszy od każdej informacji osobno.

Zadania · część 1

3 zadania · ok. 15 min

Dwa termometry: 21,0 °C ( $\sigma = 2$ °C) i 22,0 °C ( $\sigma = 1$ °C). Podaj estymatę i jej niepewność.

Odpowiedź
Wagi $1/4$ i $1$ , czyli $0{,}2$ i $0{,}8$ : $\hat x = 21{,}8$ °C, $\sigma = \sqrt{4\cdot 1/5} \approx 0{,}89$ °C.
Pokaż, że dwie postacie (średnia ważona i „przewidywanie + $K\cdot$ różnica”) są równoważne.

Odpowiedź
$z_1 + \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2}(z_2 - z_1) = \frac{(\sigma_1^2 + \sigma_2^2)z_1 - \sigma_1^2z_1 + \sigma_1^2z_2}{\sigma_1^2 + \sigma_2^2} = \frac{\sigma_2^2z_1 + \sigma_1^2z_2}{\sigma_1^2 + \sigma_2^2}$ ✓.
Ile identycznych pomiarów o $\sigma = 0{,}1$ V trzeba połączyć, żeby niepewność spadła do 0,01 V?

Odpowiedź
Niepewność średniej $\sigma/\sqrt N$ : $N = 100$ . Każde zmniejszenie niepewności o połowę wymaga czterokrotnie więcej pomiarów.

2 Filtr Kalmana dla jednej liczby

Klasyczny przykład z wprowadzenia Welcha i Bishopa: mierzymy stałe napięcie (−0,377 V) woltomierzem o szumie $\sigma = 0{,}1$ V. Model: wartość się nie zmienia, a jeśli już, to o mały losowy przyrost o wariancji $Q$ na krok. Filtr powtarza dwa kroki:Welch–Bishop 3

Cykl filtru Kalmana (jeden stan)

Predykcja: $\hat x_k^- = \hat x_{k-1}$ , $\quad P_k^- = P_{k-1} + Q$ .

Korekta: $K_k = \frac{P_k^-}{P_k^- + R}$ , $\quad\hat x_k = \hat x_k^- + K_k(z_k - \hat x_k^-)$ , $\quad P_k = (1 - K_k)P_k^-$ .

Przy $Q = 0$ wzmocnienie maleje jak $1/k$ i estymata to dokładnie średnia ze wszystkich dotychczasowych pomiarów (zadanie 2.2). Przy $Q > 0$ wzmocnienie zatrzymuje się na stałym poziomie $K_\infty$ : filtr pamięta mniej więcej ostatnie $1/K_\infty$ pomiarów i może śledzić powolne zmiany.

Zadania · część 2

3 zadania · ok. 25 min

Start $\hat x_0 = 0$ , $P_0 = 1$ , $R = 0{,}01$ , $Q = 0$ , pierwszy pomiar $z_1 = -0{,}3$ . Policz $K_1$ , $\hat x_1$ , $P_1$ .

Odpowiedź
$K_1 = 1/1{,}01 = 0{,}990$ ; $\hat x_1 = -0{,}297$ ; $P_1 = 0{,}0099$ . Pierwszy pomiar prawie całkowicie zastępuje niepewny start.
Pokaż, że przy $Q = 0$ i $P_0 \to \infty$ jest $K_k = 1/k$ i estymata to średnia arytmetyczna.

Odpowiedź
Indukcja: jeśli $P_{k-1} = R/(k - 1)$ , to $K_k = \frac{R/(k-1)}{R/(k-1) + R} = \frac1k$ i $P_k = (1 - \frac1k)\frac{R}{k-1} = \frac Rk$ . Z $K_k = 1/k$ : $\hat x_k = \frac{k-1}{k}\hat x_{k-1} + \frac1kz_k$ , czyli średnia bieżąca.
Stan ustalony: $P^- = P + Q$ , $P = (1 - K)P^-$ . Pokaż, że $P^-$ spełnia $(P^-)^2 - QP^- - QR = 0$ , i policz $K_\infty$ dla $R = 0{,}01$ , $Q = 10^{-5}$ oraz $Q = 10^{-3}$ .

Odpowiedź
$P^- = Q + \frac{RP^-}{P^- + R}$ ; mnożymy przez $P^- + R$ : $(P^-)^2 = QP^- + QR$ . $Q = 10^{-5}$ : $P^- = 3{,}21\cdot 10^{-4}$ , $K_\infty = 0{,}031$ (pamięć ok. 30 pomiarów). $Q = 10^{-3}$ : $K_\infty = 0{,}27$ (pamięć ok. 4 pomiarów).

3 Wersja macierzowa

Dla stanu wektorowego model w czasie dyskretnym (lekcja 3.4 Sterowanie cyfrowe: próbkowanie, transformata Z i regulator w kodzie):

\mathbf x_{k} = F\mathbf x_{k-1} + G\mathbf u_{k-1} + \mathbf w_{k-1},\qquad \mathbf z_k = H\mathbf x_k + \mathbf v_k,

gdzie $\mathbf w$ (zakłócenia, błędy modelu) ma macierz kowariancji $Q$ , a $\mathbf v$ (szum pomiaru) macierz $R$ . Niepewność estymaty opisuje macierz kowariancji $P$ : na przekątnej wariancje poszczególnych składowych, poza nią ich korelacje.

Filtr Kalmana

Predykcja: $\hat{\mathbf x}_k^- = F\hat{\mathbf x}_{k-1} + G\mathbf u_{k-1}$ , $\quad P_k^- = FP_{k-1}F^T + Q$ .

Korekta: $K_k = P_k^-H^T\left(HP_k^-H^T + R\right)^{-1}$ , $\quad\hat{\mathbf x}_k = \hat{\mathbf x}_k^- + K_k\left(\mathbf z_k - H\hat{\mathbf x}_k^-\right)$ , $\quad P_k = (I - K_kH)P_k^-$ .

Porównaj z obserwatorem z lekcji 4.2: to ten sam schemat „model plus korekta innowacją $\mathbf z - H\hat{\mathbf x}$ ”. Różnica: $K_k$ nie jest dobierane ręcznie, tylko liczone z niepewności. W stanie ustalonym filtr Kalmana to obserwator z optymalnym $L$ , wyznaczonym z równania Riccatiego dualnego do tego z LQR (lekcja 4.3 LQR: koszt zamiast biegunów).

Zadania · część 3

3 zadania · ok. 25 min

Pokaż, że dla jednego stanu ( $F = H = 1$ ) wzory macierzowe dają wzory z części 2.

Odpowiedź
$P^- = P + Q$ , $K = P^-/(P^- + R)$ , $\hat x = \hat x^- + K(z - \hat x^-)$ , $P = (1 - K)P^-$ ✓.
Stan $[z, v]$ , mierzymy tylko $z$ . Po korekcie zmieniła się też estymata $v$ . Dlaczego, skoro $v$ nie mierzymy?

Odpowiedź
$K = P^-H^T/(\dots)$ to pierwsza kolumna $P^-$ podzielona przez liczbę. Jej druga składowa $P_{zv}^-$ to korelacja błędów wysokości i prędkości, która powstaje w predykcji (błąd prędkości przekłada się na błąd wysokości). Pomiar wysokości mówi więc coś o prędkości.
Co się dzieje z $P$ , gdy przez dłuższy czas nie ma pomiarów (np. dron w tunelu bez barometru)?

Odpowiedź
Tylko predykcje: $P \leftarrow FPF^T + Q$ . Niepewność rośnie: dla wysokości z akcelerometru wariancja rośnie jak $t^3$ od szumu i jak $t^4$ od nieznanego biasu (odchylenie standardowe jak $t^{1{,}5}$ i $t^2$ ). Po powrocie pomiaru $K$ jest duże i estymata szybko „wskakuje” na pomiar.

4 Dron: barometr i akcelerometr

Dwa czujniki o przeciwnych wadach:

akcelerometr (100 Hz): mierzy przyspieszenie z szumem 0,2 m/s² i stałym błędem (biasem). Dwukrotne całkowanie daje wysokość dokładną na krótką metę, ale bias 0,1 m/s² po minucie to $\frac12\cdot 0{,}1\cdot 60^2 = 180$ m błędu;
barometr (10 Hz): mierzy wysokość bez dryfu, ale z szumem 0,3 m.

Filtr: stan $[z, v]$ , akcelerometr jako wejście predykcji ( $z \leftarrow z + vT + \frac12a_mT^2$ , $v \leftarrow v + a_mT$ ), barometr jako pomiar. Szum akcelerometru wchodzi do $Q$ : $Q = \sigma_a^2\begin{bmatrix}T^4/4 & T^3/2\\ T^3/2 & T^2\end{bmatrix}$ . Bias jest wolny, a nie biały, więc dokładamy go jako trzeci stan $b$ z $\dot b = 0$ : filtr sam go oszacuje, bo bias zostawia charakterystyczny ślad w różnicy między całkowaniem a barometrem.

Odpowiedź na pytanie z prologu. Symulacja (60 s, ruch sinusoidalny): sam barometr ±0,3 m, samo całkowanie ucieka o ok. 185 m, filtr z biasem w modelu ma błąd wysokości ok. 7,5 cm (RMS), a bias szacuje z dokładnością do 0,002 m/s². Filtr bez biasu w modelu ma błąd ok. 48 cm, a twierdzi, że ok. 8 cm: błędny model daje filtr zbyt pewny siebie.

Zadania · część 4

3 zadania · ok. 25 min

Napisz $F$ , $G$ , $H$ dla stanu $[z, v]$ z akcelerometrem jako wejściem i barometrem jako pomiarem, $T = 0{,}01$ s.

Odpowiedź
$F = \begin{bmatrix}1 & 0{,}01\\ 0 & 1\end{bmatrix}$ , $G = \begin{bmatrix}5\cdot 10^{-5}\\ 0{,}01\end{bmatrix}$ , $H = [1\ 0]$ . Pomiar barometru przychodzi co 10 kroków: korekta tylko wtedy.
Jak zmienią się $F$ , $G$ , $H$ po dodaniu biasu $b$ (akcelerometr mierzy $a + b$ )?

Odpowiedź
Prawdziwe przyspieszenie to $a_m - b$ : $F = \begin{bmatrix}1 & T & -T^2/2\\ 0 & 1 & -T\\ 0 & 0 & 1\end{bmatrix}$ , $G = [T^2/2,\ T,\ 0]^T$ , $H = [1\ 0\ 0]$ .
Dlaczego estymowanie biasu wymaga, żeby dron w ogóle był obserwowany przez barometr? Odpowiedz przez obserwowalność.

Odpowiedź
Bez barometru $H$ jest puste i nic nie jest obserwowalne. Z barometrem rozszerzony układ ma $W_o$ rzędu 3 (jak zadanie z rozszerzeniem o siłę w lekcji 4.2): stały bias daje parabolę w wysokości, którą barometr widzi. Gdyby akcelerometr był jedynym czujnikiem, biasu nie dałoby się odróżnić od prawdziwego przyspieszenia.

5 Metoda i przykłady rozwiązane

Metoda: projekt filtru Kalmana

Model: stan, $F$ , $G$ , $H$ (dyskretyzacja z lekcji 3.4); czujniki szybkie jako wejście, wolne i dokładne jako pomiar.
Szumy: $R$ z karty katalogowej albo z nagrania nieruchomego czujnika; $Q$ z szumu wejść i niepewności modelu.
Błędy niebiałe (bias, dryf) jako dodatkowe stany.
Obserwowalność rozszerzonego stanu.
Sprawdzenie spójności: rzeczywisty błąd (w symulacji) i innowacje mają być zgodne z $P$ i $HP^-H^T + R$ .

Przykład 5.1 Woltomierz: trzy wartości R

50 pomiarów napięcia −0,37727 V z szumem $\sigma = 0{,}1$ V, $Q = 10^{-5}$ . Porównaj filtry z $R = 0{,}01$ (prawdziwe), 1 i $10^{-4}$ .

$R = 0{,}01$ : $K_{50} = 0{,}034$ , estymata zbliżona do średniej z ostatnich kilkudziesięciu pomiarów.
$R = 1$ : $K_{50} = 0{,}020$ , filtr ufa sobie bardziej, ale przy stałej wartości to niewiele szkodzi.
$R = 10^{-4}$ : $K_{50} = 0{,}27$ , estymata skacze z każdym pomiarem (w serii z pliku weryfikacyjnego błąd końcowy 0,044 V, dwa razy więcej niż przy prawdziwym $R$ ).

Wynik: zaniżone $R$ (filtr nadmiernie wierzy pomiarom) psuje estymatę bardziej niż zawyżone.

Sprawdzenie: niepewność średniej z 50 pomiarów to $0{,}1/\sqrt{50} = 0{,}014$ V, a deklarowane $\sqrt{P_{50}} = 0{,}018$ V przy prawdziwym $R$ ✓ (dodatnie $Q$ zmniejsza efektywną liczbę pomiarów).

Przykład 5.2 Fuzja w dronie

Akcelerometr 100 Hz ( $\sigma_a = 0{,}2$ m/s², bias 0,1 m/s²), barometr 10 Hz ( $\sigma_b = 0{,}3$ m). Porównaj sposoby estymacji wysokości po 60 s.

Sam barometr: błąd ok. 0,3 m w każdej chwili, bez dryfu.
Samo całkowanie akcelerometru: ok. 185 m po 60 s (bias), plus błąd od szumu.
Kalman bez biasu: ok. 0,48 m RMS, deklaruje 0,08 m.
Kalman z biasem: ok. 0,075 m RMS, deklaruje ok. 0,08 m; bias oszacowany z błędem ok. 0,002 m/s².

Wynik: fuzja jest 4 razy dokładniejsza niż barometr, a filtr uczciwie mówi, ile wie.

Sprawdzenie: bez biasu w akcelerometrze filtr dwustanowy osiąga 0,069 m, prawie tyle co trzystanowy z biasem ✓.

6 Błędne intuicje, których trzeba się oduczyć

„Filtr Kalmana to filtr dolnoprzepustowy.”: Jest estymatorem stanu: odtwarza też wielkości niemierzone (prędkość, bias) i zmienia wzmocnienie w czasie, gdy zmienia się niepewność.
„Większe $R$ zawsze daje gładszą, a więc lepszą estymatę.”: Gładszą, ale spóźnioną. Optimum jest przy prawdziwych wartościach szumów.
„Mała macierz $P$ oznacza, że estymata jest dobra.”: $P$ jest dobre tylko tak, jak model. Filtr bez biasu w modelu deklarował 8 cm, a mylił się o 48 cm.
„Filtr Kalmana wymaga szumu gaussowskiego.”: Wśród estymatorów liniowych jest optymalny dla dowolnego szumu o znanych wariancjach; przy szumie gaussowskim jest optymalny wśród wszystkich estymatorów.
„Akcelerometr nadaje się tylko do pomiaru drgań.”: W fuzji to główne źródło informacji o szybkich zmianach; wolne czujniki tylko pilnują dryfu.

7 Zadania do przemyślenia

Te zadania wymagają więcej myślenia niż liczenia.

Zadania do przemyślenia

5 zadań · ok. 1,5 godziny

GPS i nawigacja inercjalna. Samolot ma platformę inercjalną (akcelerometry i żyroskopy, 200 Hz) i GPS (1 Hz, ±5 m). Opisz filtr podobny do drona z części 4. Co się dzieje, gdy GPS zniknie na minutę?
Rozwiązanie
1. Inercja jako wejście predykcji, GPS jako pomiar; w stanie pozycja, prędkość, orientacja i biasy czujników. Biasy oszacowane, dopóki GPS działa.
2. Bez GPS: same predykcje, $P$ rośnie. Ponieważ biasy są już oszacowane, dryf jest dużo wolniejszy niż w surowej inercji. Po powrocie GPS filtr szybko się koryguje.
Odrzucanie dzikich pomiarów. Barometr czasem zwraca bzdurę (podmuch od śmigła na czujniku). Jak filtr może to wykryć?
Rozwiązanie
1. Innowacja $\nu = z - H\hat{\mathbf x}^-$ ma wariancję $S = HP^-H^T + R$ . Jeśli $\nu^2/S$ jest bardzo duże (np. powyżej 9, czyli 3σ), pomiar jest podejrzany.
2. Takiego pomiaru się nie używa albo zwiększa się dla niego $R$ . To bramkowanie innowacji (ang. innovation gating), standard w śledzeniu celów.
Śledzenie z modelem prędkości. Radar mierzy co sekundę położenie samolotu. Model „stała prędkość” z zakłóceniem przyspieszenia w $Q$ . Co się dzieje, gdy samolot nagle skręca? Jak dobrać $Q$ ?
Rozwiązanie
1. Przy małym $Q$ filtr wierzy w prosty lot i przez kilka sekund „wylatuje” na zewnątrz zakrętu. Przy dużym $Q$ szybko nadąża, ale estymata w locie prostym jest bardziej zaszumiona.
2. Rozwiązania: $Q$ z maksymalnego spodziewanego przyspieszenia albo kilka modeli naraz (prosty lot, zakręt) z przełączaniem według innowacji (filtry IMM).
Dualność z LQR. W stanie ustalonym $P^-$ spełnia równanie Riccatiego. Jakie podstawienie przekształca równanie LQR ( $A$ , $B$ , $Q$ , $R$ ) w równanie filtru ( $A$ , $H$ , $Q_w$ , $R_v$ )?
Rozwiązanie
1. $A \to A^T$ , $B \to H^T$ , $Q \to Q_w$ , $R \to R_v$ . Wzmocnienie filtru to transponowane wzmocnienie „LQR dla układu dualnego”.
2. Intuicja: koszt stanu w LQR odpowiada niepewności modelu w filtrze, koszt sterowania odpowiada szumowi pomiaru. Drogie sterowanie to wolny regulator, szum pomiaru to wolny filtr.
LQG. Połączenie LQR z filtrem Kalmana nazywa się LQG. Co mówi o nim zasada separacji z lekcji 4.2? Czego nie mówi?
Rozwiązanie
1. Mówi, że oba można projektować osobno, a bieguny całości to bieguny LQR i filtru. Dla szumów gaussowskich LQG jest wręcz optymalny dla kosztu oczekiwanego.
2. Nie mówi nic o odporności na błędy modelu. Doyle pokazał w 1978 r., że LQG może mieć dowolnie małe zapasy stabilności, mimo że LQR miał gwarantowane 60°. Lekcja 5.2 LQG i odporność.

8 Sprawdź się

Odpowiedz w myślach, zanim rozwiniesz odpowiedź.

1 Jak połączyć dwa pomiary o różnych wariancjach?

Średnia ważona odwrotnościami wariancji; wariancja wyniku $\sigma_1^2\sigma_2^2/(\sigma_1^2 + \sigma_2^2)$ .

2 Ile wynosi wzmocnienie Kalmana dla jednego stanu?

$K = P^-/(P^- + R)$ .

3 Jakie są dwa kroki filtru?

Predykcja (model, $P$ rośnie o $Q$ ) i korekta (pomiar, $P$ maleje).

4 Co oznacza

Q = 0

Model idealny: $K \to 0$ , filtr przestaje słuchać pomiarów.

5 Co to macierz kowariancji

P

Niepewność estymaty: wariancje składowych i ich korelacje.

6 Jak uwzględnić bias czujnika?

Jako dodatkowy stan o wolnej dynamice ( $\dot b = 0$ plus mały szum).

7 Jaki jest związek filtru Kalmana z obserwatorem?

W stanie ustalonym to obserwator z optymalnym $L$ z równania Riccatiego.

9 Karta wzorów i co dalej

Filtr Kalmana

$\hat{\mathbf x}^- = F\hat{\mathbf x} + G\mathbf u$ , $\;P^- = FPF^T + Q$

$K = P^-H^T(HP^-H^T + R)^{-1}$

$\hat{\mathbf x} = \hat{\mathbf x}^- + K(\mathbf z - H\hat{\mathbf x}^-)$ , $\;P = (I - KH)P^-$

1D: $K = P^-/(P^- + R)$ ; stan ustalony $(P^-)^2 - QP^- - QR = 0$

Dron

$F = \begin{bmatrix}1 & T\\ 0 & 1\end{bmatrix}$ , $G = \begin{bmatrix}T^2/2\\ T\end{bmatrix}$ , $H = [1\ 0]$

$Q = \sigma_a^2\begin{bmatrix}T^4/4 & T^3/2\\ T^3/2 & T^2\end{bmatrix}$ , $R = \sigma_b^2$

dryf od biasu: $\frac12bt^2$

bramkowanie: $\nu^2/S > 9$ → odrzuć

Do przeczytania

Obowiązkowo: G. Welch, G. Bishop, An Introduction to the Kalman Filter, rozdz. 1 i 3 (krótki, wolny dostęp; przykład z woltomierzem jest stamtąd).
Dla chętnych: A. Tewari, Modern Control Design with MATLAB and SIMULINK, rozdz. 7 (Kalman i LQG); S. Brunton, Control Bootcamp, wykłady o filtrze Kalmana.

Filtr Kalmana wymaga modelu liniowego. Orientacja drona już taka nie jest: akcelerometr mierzy $g\sin\varphi$ , a nie $g\varphi$ . Następna lekcja pokazuje, jak Schmidt i jego zespół rozwiązali ten problem dla Apollo: linearyzują model wokół bieżącej estymaty. Na przykładzie jednej osi zobaczymy też prostszego kuzyna, filtr komplementarny.