Filtr Kalmana: jak z niedokładnych pomiarów oszacować, gdzie jest samochód i jak szybko jedzie

Łączenie dwóch pomiarów i wzmocnienie Kalmana, filtr jako średnia bieżąca, model w przestrzeni stanów, predykcja i korekcja, śledzenie samochodu według GPS z oszacowaniem niemierzonej prędkości, strojenie szumów Q i R, optymalność i rozszerzenia nieliniowe.

14 minut Matěj Žalmánek 2026-10-15

Seria Podstawy AI Część 41 z 52

1. Mapa sztucznej inteligencji: co jest czym i jak to do siebie pasuje
2. Krótka historia sztucznej inteligencji: od Turinga do modeli językowych
3. Dane, model, trening, inferencja: podstawowe pojęcia na jednym przykładzie
4. Jak czytać zapis matematyczny: symbole, indeksy, sumy i kwantyfikatory
5. Zbiory i relacje: klocki, z których składają się definicje w AI
6. Funkcje i odwzorowania: dlaczego każdy model AI jest funkcją
7. Logika i dowody: reguły, według których rozumuje AI
8. Grafy i drzewa: struktura, na której opierają się przeszukiwanie i sieci neuronowe
9. Algebra liniowa dla uczenia maszynowego: wektory, macierze i dlaczego AI głównie mnoży
10. Pochodne, gradient i reguła łańcuchowa: jak model ustala, w którą stronę się uczyć
11. Spadek gradientu: jak uczy się model
12. Czym jest prawdopodobieństwo: od rzutu kostką do aksjomatów Kołmogorowa
13. Prawdopodobieństwo warunkowe i twierdzenie Bayesa: jak zmienić ocenę, gdy dowiesz się czegoś nowego
14. Zmienna losowa i dystrybuanta: jedna funkcja, która mówi o rozkładzie wszystko
15. Funkcja gęstości i funkcja prawdopodobieństwa: dlaczego zmienna ciągła ma w punkcie prawdopodobieństwo zero
16. Wartość oczekiwana, wariancja i kowariancja: gdzie leży rozkład, jak jest szeroki i co z czym się wiąże
17. Przegląd rozkładów prawdopodobieństwa: Bernoulliego, dwumianowy, Poissona, jednostajny, normalny i wykładniczy
18. Prawo wielkich liczb i centralne twierdzenie graniczne: dlaczego średnie zachowują się tak przewidywalnie
19. Statystyka i estymacja parametrów: największa wiarygodność, MAP i przedziały ufności
20. Teoria informacji: entropia, entropia krzyżowa i dywergencja KL — skąd się biorą funkcje straty
21. Przeszukiwanie przestrzeni stanów: BFS, DFS, UCS i A* z animacjami
22. Gry i przeciwnik: minimax, przycinanie alfa-beta i Monte Carlo tree search
23. Spełnianie ograniczeń: backtracking, propagacja ograniczeń i AC-3 na sudoku i mapie czeskich krajów
24. Logika i systemy z bazą wiedzy: wnioskowanie w przód i wstecz, rezolucja i systemy ekspertowe
25. Planowanie: STRIPS, świat klocków, heurystyki z relaksacji i planowanie w agentach AI
26. Przeszukiwanie lokalne i symulowane wyżarzanie: komiwojażer przez czeskie miasta wojewódzkie
27. Algorytmy genetyczne: populacja, krzyżowanie, mutacja i selekcja na plecaku na wycieczkę
28. Strategie ewolucyjne i programowanie genetyczne: od reguły jednej piątej do prawa Keplera
29. Optymalizacja kolonią mrówek: jak ślad feromonowy znajduje krótką drogę
30. Optymalizacja rojem cząstek: bezwładność, własne doświadczenie i doświadczenie roju
31. Neuroewolucja: ewolucja wag i kształtu sieci neuronowych oraz algorytm NEAT
32. Zbiory rozmyte i funkcje przynależności: jak zapisać „trochę ciepło”
33. Logika rozmyta a prawdopodobieństwo: dlaczego „trochę ciepło” nie znaczy „prawdopodobnie ciepło”
34. Logika rozmyta: t-normy, implikacje i reguły JEŻELI–TO
35. Sterowanie rozmyte: Mamdani i Sugeno od wentylatora po wahadło na wózku
36. Klasteryzacja rozmyta: algorytm fuzzy c-means i punkty między skupieniami
37. Systemy neuro-rozmyte (ANFIS): reguły rozmyte, które uczą się z danych
38. Łańcuchy Markowa: macierz przejścia, rozkład stacjonarny i PageRank
39. Ukryte modele Markowa: algorytm Viterbiego i nieuczciwe kasyno
40. Sieci bayesowskie: dlaczego trawa jest mokra i co z tego wynika dla zraszacza
41. Filtr Kalmana: jak z niedokładnych pomiarów oszacować, gdzie jest samochód i jak szybko jedzie
42. Rodzaje uczenia maszynowego: nadzorowane, nienadzorowane, półnadzorowane i inne
43. Prompty, kontekst i RAG: jak dać modelowi właściwe informacje
44. Wariacyjne autoenkodery (VAE): nowe obrazki z ciągłej przestrzeni latentnej
45. Generatywne sieci przeciwstawne (GAN): fałszerz kontra detektyw
46. Modele dyfuzyjne: jak z czystego szumu powstaje obraz
47. Procesy decyzyjne Markowa: stany, akcje, nagrody i równania Bellmana
48. Q-learning i DQN: uczenie się wartości akcji od tabeli po sieć neuronową
49. Policy gradient i PPO: uczenie się bezpośrednio polityki
50. Jak działa agent AI: model językowy w pętli z narzędziami
51. Programowanie z agentami AI: zadanie, testy i kontrola
52. Ocena i bezpieczeństwo systemów AI: jak mierzyć i jak pilnować

Pełny program serii Gotowe: 52 z 83 tematów

Statek Apollo 11 leciał na Księżyc z komputerem pokładowym, który miał mniej pamięci niż dzisiejszy kalkulator graficzny. Nawigacja musiała przy tym na bieżąco szacować z niedokładnych pomiarów gwiazd, radaru i czujników inercyjnych, gdzie statek jest i jak szybko leci. Pomogła jej metoda, którą w 1960 roku opublikował Rudolf Kálmán, a którą dla NASA dopracował w praktyce Stanley Schmidt. Filtr Kalmana jest dziś w każdym telefonie, który łączy GPS z akcelerometrem, w każdym dronie i w każdym samochodzie, który sam utrzymuje pas ruchu. Tą częścią kończy się moduł o modelach probabilistycznych.

Nawiązuje do części Ukryte modele Markowa i Przegląd rozkładów prawdopodobieństwa.

1. Dwa niedokładne pomiary są lepsze niż jeden

Zacznijmy prosto. Odległość do latarni morskiej mierzymy dwa razy: raz dokładnym przyrządem — 100 m z niepewnością (odchyleniem standardowym) 5 m —, drugi raz zgrubnym — 110 m z niepewnością 10 m. Jakie jest najlepsze oszacowanie?

Twierdzenie — połączenie dwóch normalnych oszacowań

Jeśli dwa niezależne oszacowania to N(m₁, σ₁²) i N(m₂, σ₂²), to połączone oszacowanie jest normalne z

m = m₁ + K (m₂ − m₁),   σ² = (1 − K) σ₁²,   gdzie K = σ₁² / (σ₁² + σ₂²).

Równoważnie 1/σ² = 1/σ₁² + 1/σ₂²: precyzje się sumują.

Dowód. Połączone oszacowanie ma gęstość proporcjonalną do iloczynu obu gęstości (twierdzenie Bayesa z pierwszym oszacowaniem jako rozkładem a priori i drugim jako wiarygodnością). W wykładniku sumują się dwie funkcje kwadratowe:

−(x − m₁)² / (2σ₁²) − (x − m₂)² / (2σ₂²) = −½ (1/σ₁² + 1/σ₂²) x² + (m₁/σ₁² + m₂/σ₂²) x + const.

Gęstość z wykładnikiem −½ ax² + bx jest po uzupełnieniu do kwadratu normalna z wariancją 1/a i wartością oczekiwaną b/a. Stąd 1/σ² = 1/σ₁² + 1/σ₂², czyli σ² = σ₁²σ₂² / (σ₁² + σ₂²) = (1 − K) σ₁², oraz m = σ² (m₁/σ₁² + m₂/σ₂²) = (1 − K) m₁ + K m₂ = m₁ + K (m₂ − m₁). ∎

m = m₁ + K (m₂ − m₁),   K = σ₁² / (σ₁² + σ₂²),   σ² = (1 − K) σ₁²

m₁, σ₁²

pierwsze oszacowanie (na rysunku niebieskie) jest punktem wyjścia — od niego się przesuwamy; jego wariancja w liczniku K mówi: im mniej ufamy pierwszemu oszacowaniu, tym dalej możemy się od niego oddalić

m₂, σ₂²

drugie oszacowanie (bursztynowe); do wzoru wchodzi tylko przez różnicę m₂ − m₁ — gdyby oba się zgadzały, nie byłoby czego poprawiać. Jego wariancja w mianowniku K zmniejsza wzmocnienie: zgrubnemu pomiarowi ufamy mało

K

wzmocnienie Kalmana, liczba od 0 do 1 — jaką część różnicy przejąć. To udział niepewności pierwszego oszacowania w niepewności całkowitej, więc dokładniejsze oszacowanie „ciągnie“ wynik do siebie

m, σ²

połączone oszacowanie (zielone); czynnik 1 − K < 1 gwarantuje, że nowa niepewność jest mniejsza niż pierwsza — każdy pomiar, nawet zgrubny, coś dodaje

Tutaj K = 25 / (25 + 100) = 0,2: przejmujemy jedną piątą różnicy i dostajemy 102 m z niepewnością √20 ≈ 4,5 m — mniejszą niż 5 m dokładnego przyrządu. Ta liczba K to wzmocnienie Kalmana, a cały filtr to nic innego jak ten krok, powtarzany w czasie.

Przykład — połączenie pomiarów 100 ± 5 m i 110 ± 10 m krok po kroku

Wariancje: σ₁² = 5² = 25, σ₂² = 10² = 100.

Wzmocnienie: K = 25 / (25 + 100) = 0,2.

Wartość oczekiwana: m = 100 + 0,2 · (110 − 100) = 102 m — to samo co średnia ważona 0,8 · 100 + 0,2 · 110.

Wariancja: σ² = (1 − 0,2) · 25 = 20,   σ = √20 ≈ 4,47 m.

Sprawdzenie przez precyzje: 1/25 + 1/100 = 0,04 + 0,01 = 0,05 = 1/20. ✓

2. Powtarzany pomiar jednej wielkości

Temperaturę w pokoju mierzymy termometrem z niepewnością 2 °C, pięćdziesiąt razy z rzędu, i po każdym pomiarze łączymy nowy odczyt z dotychczasowym oszacowaniem. Po pięćdziesięciu pomiarach oszacowanie wynosi 19,895 °C — dokładnie średnią arytmetyczną wszystkich pomiarów — a niepewność 0,283 °C = 2/√50. Filtr Kalmana dla wielkości, która się nie zmienia, jest więc zwykłą średnią bieżącą. Ciekawy staje się dopiero wtedy, gdy wielkość zmienia się w czasie.

Twierdzenie — filtr dla niezmiennej wielkości to średnia bieżąca

Mierzymy stałą przyrządem o wariancji σ² i zaczynamy od oszacowania tak niepewnego, że pierwszy pomiar całkowicie je przesłania (v₀ → ∞). Wtedy po k pomiarach zachodzi

K_k = 1/k,   m_k = m_k−1 + (1/k) (z_k − m_k−1) = (z₁ + … + z_k) / k,   v_k = σ²/k.

Dowód indukcyjny. Dla k = 1 jest K₁ = v₀ / (v₀ + σ²) → 1, więc m₁ = z₁ i v₁ = σ². Jeśli v_k−1 = σ²/(k − 1), to K_k = v_k−1 / (v_k−1 + σ²) = (1/(k − 1)) / (1/(k − 1) + 1) = 1/k oraz v_k = (1 − 1/k) · σ²/(k − 1) = σ²/k. Jeśli m_k−1 jest średnią pierwszych k − 1 pomiarów, to m_k = ((k − 1) m_k−1 + z_k) / k jest średnią wszystkich k. ∎

Przykład — pierwsze trzy pomiary temperatury

Nasz filtr zaczyna od oszacowania 0 °C z wariancją 10⁶ (niepewność 1 000 °C — „nic nie wiem“), wariancja pomiaru wynosi 2² = 4.

1. pomiar 23,098 °C: K₁ = 10⁶ / (10⁶ + 4) ≈ 1, oszacowanie 23,098 °C, wariancja 4,00.

2. pomiar 21,967 °C: K₂ = 4 / (4 + 4) = 1/2, oszacowanie 23,098 + ½ · (21,967 − 23,098) = 22,533 °C, wariancja 2,00.

3. pomiar 16,630 °C: K₃ = 2 / (2 + 4) = 1/3, oszacowanie 22,533 + ⅓ · (16,630 − 22,533) = 20,565 °C, wariancja 1,33 = 4/3.

Sprawdzenie: (23,098 + 21,967 + 16,630) / 3 = 20,565 °C — zwykła średnia. Po pięćdziesięciu pomiarach wariancja wynosi 4/50 = 0,08, a niepewność √0,08 ≈ 0,283 °C.

3. Model w przestrzeni stanów

Definicja — liniowy model w przestrzeni stanów

Ukryty stan x_k ∈ ℝⁿ ewoluuje, a pomiar z_k rejestruje go z szumem:

x_k = F x_k−1 + w_k,   w_k ~ N(0, Q),    z_k = H x_k + v_k,   v_k ~ N(0, R).

F to macierz przejścia, H macierz pomiaru, Q kowariancja szumu procesu, a R kowariancja szumu pomiaru.

Nasz przykład: samochód jedzie drogą z mniej więcej stałą prędkością 15 m/s, ale kierowca czasem przyhamuje albo przyspieszy — losowe przyspieszenie z odchyleniem standardowym 0,5 m/s². Stan to (położenie, prędkość), krok 1 s, więc F = [[1, 1], [0, 1]]: nowe położenie to stare plus prędkość. GPS mierzy tylko położenie, z niepewnością 8 m: H = [1, 0], R = 64.

Definicja — macierz kowariancji i wielowymiarowy rozkład normalny

Dla wektora losowego X ∈ ℝⁿ z wartością oczekiwaną μ = E[X] macierz kowariancji to Σ = E[(X − μ)(X − μ)^T], tj. Σ_ij = cov(X_i, X_j). Na przekątnej są wariancje składowych, poza nią kowariancje; macierz jest symetryczna i dodatnio półokreślona.

Zapis X ~ N(μ, Σ) oznacza wielowymiarowy rozkład normalny z gęstością proporcjonalną do exp(−½ (x − μ)^T Σ⁻¹ (x − μ)). Dla odwzorowania liniowego zachodzi AX + c ~ N(Aμ + c, A Σ A^T), a suma niezależnych wektorów normalnych jest normalna z sumą kowariancji.

Przykład — skąd się bierze macierz Q

Podczas kroku o długości Δt na samochód działa stałe losowe przyspieszenie a ~ N(0, q²). Ze wzorów na ruch jednostajnie przyspieszony:

p_k = p_k−1 + Δt · v_k−1 + ½ Δt² a,   v_k = v_k−1 + Δt · a.

Bez szumu to F = [[1, Δt], [0, 1]]. Szum to w = g a z wektorem g = (½ Δt², Δt), a zgodnie z regułą AΣA^T powyżej

Q = q² g g^T = q² [[Δt⁴/4, Δt³/2], [Δt³/2, Δt²]].

Dla Δt = 1 s i q = 0,5 m/s²: Q = 0,25 · [[0,25; 0,5], [0,5; 1]] = [[0,0625; 0,125], [0,125; 0,25]]. W ciągu jednej sekundy losowe przyspieszenie dodaje więc niepewność √0,0625 = 0,25 m w położeniu i 0,5 m/s w prędkości — a oba błędy mają ten sam znak, dlatego kowariancja jest dodatnia. Dokładnie ta macierz jest w kodzie poniżej.

4. Predykcja i korekcja

Algorytm — filtr Kalmana

Oszacowanie to rozkład normalny z wartością oczekiwaną x̂ i kowariancją P. W każdym kroku:

Predykcja: x̂⁻ = F x̂,   P⁻ = F P F^T + Q.

Korekcja: K = P⁻ H^T (H P⁻ H^T + R)⁻¹,   x̂ = x̂⁻ + K (z − H x̂⁻),   P = (I − K H) P⁻.

Kolory w rozbiorach poniżej odpowiadają rysunkom: predykcja (wcześniej „pierwsze oszacowanie“) na niebiesko, pomiar na bursztynowo, wynik na zielono, wzmocnienie na fioletowo.

x̂⁻ = F x̂,   P⁻ = F P F^T + Q

x̂⁻, P⁻

predykcja na ten krok, zanim przyjdzie pomiar — pełni rolę „pierwszego oszacowania“ z połączenia dwóch pomiarów

F … F^T

model przesuwa oszacowanie do przodu; niepewność przenosi się regułą AΣA^T (w samochodzie: niepewna prędkość po sekundzie przekłada się na niepewność położenia). Nie wystarczy FP, bo kowariancja jest wielkością kwadratową — mnoży się z lewej i z prawej

Q

model nie jest doskonały (kierowca przyhamuje), więc niepewność dodaje się w każdym kroku — bez Q macierz P spadłaby do zera, a filtr przestałby słuchać pomiarów

K = P⁻ H^T (H P⁻ H^T + R)⁻¹

H P⁻ H^T

niepewność predykcji przeliczona na język przyrządu pomiarowego (w samochodzie tylko wariancja położenia) — odpowiada σ₁²

R

wariancja pomiaru, odpowiada σ₂²; suma w nawiasie to wariancja innowacji S — o ile pomiar i predykcja mogą się rozjechać czysto przypadkiem

P⁻ H^T

kowariancja całego stanu z mierzoną wielkością; dzięki niej niezerowe wzmocnienie dostaje też prędkość, której nikt nie mierzy — błąd położenia i prędkości są ze sobą powiązane

K

wzmocnienie Kalmana, po jednej liczbie dla każdej składowej stanu; odwrotność zamiast dzielenia tylko dlatego, że pomiar może być wektorem

x̂ = x̂⁻ + K (z − H x̂⁻),   P = (I − K H) P⁻

x̂⁻, P⁻

predykcja — punkt wyjścia, od którego się przesuwamy

z

nowy pomiar (na rysunku bursztynowe punkty GPS); jedyne miejsce, którym dane wchodzą do oszacowania

H

przekłada predykcję stanu na to, co powinien pokazać przyrząd pomiarowy; pomiaru i stanu nie da się odjąć bezpośrednio, bo GPS widzi tylko położenie, nie prędkość

K

jaką część innowacji z − Hx̂⁻ przejąć do każdej składowej stanu

x̂, P

poprawione oszacowanie (na rysunku zielone); czynnik I − KH zmniejsza niepewność — tym bardziej, im większą część pomiaru przejęliśmy

import numpy as np

F = np.array([[1, 1], [0, 1]])                    # położenie += prędkość
H = np.array([[1, 0]])                            # mierzymy tylko położenie
Q = 0.5**2 * np.array([[1/4, 1/2], [1/2, 1]])     # losowe przyspieszenie
R = np.array([[8**2]])                            # GPS ± 8 m

def kalman(mereni, x, P):
    for z in mereni:
        x, P = F @ x, F @ P @ F.T + Q             # predykcja
        K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)
        x = x + K @ (z - H @ x)                   # korekcja według innowacji
        P = (np.eye(2) - K @ H) @ P
        yield x, P

Przykład — pierwsze dwa kroki filtru na danych samochodu

Start. Pierwszy pomiar GPS to z₁ = 8,44 m. Filtr zaczyna od położenia 8,44 m, prędkości 0 i dużej niepewności P₀ = [[100; 0], [0; 100]] (10 m i 10 m/s). Korekcja tym samym pomiarem nie zmienia położenia (innowacja wynosi 0), tylko zmniejsza jego wariancję: K = 100 / (100 + 64) = 0,610, P₁₁ = (1 − 0,610) · 100 = 39,02.

Predykcja na 2. sekundę. x̂⁻ = (8,44 + 0; 0) = (8,44; 0,00). Kowariancja FPF^T dodaje do wariancji położenia wariancję prędkości (położenie += prędkość):

P⁻ = [[39,02 + 100 + 0,0625; 100 + 0,125], [100 + 0,125; 100 + 0,25]] = [[139,09; 100,13], [100,13; 100,25]].

Korekcja. Pomiar z₂ = 23,23 m, innowacja y = 23,23 − 8,44 = 14,78 m, jej wariancja S = 139,09 + 64 = 203,09.

K = (139,09; 100,13) / 203,09 = (0,685; 0,493),

x̂ = (8,44 + 0,685 · 14,78; 0 + 0,493 · 14,78) = (18,57 m; 7,29 m/s).

Po dwóch pomiarach filtr „wie“, że samochód się porusza, choć prędkości nikt nie mierzył: oszacował 7,3 m/s (w rzeczywistości 13,9 m/s). Wzmocnienie dla prędkości jest niezerowe tylko dzięki kowariancji 100,13 poza przekątną. Dlaczego tylko połowa? Pomiary są zaszumione o 8 m, więc filtr nie traktuje jeszcze różnicy 15 m całkiem poważnie — w kolejnych sekundach prędkość się dostroi.

5. Filtr śledzi samochód

Minuta jazdy, jeden pomiar GPS na sekundę. W tym czasie samochód przejeżdża niecały kilometr, więc szum zgubiłby się na wykresie położenia. Rysujemy więc, o ile samochód jest z przodu lub z tyłu w porównaniu z jazdą dokładnie 15 m/s:

Pomiary GPS odbiegają od rzeczywistego położenia średnio o 8,3 m (od dziesiątej sekundy, kiedy filtr się rozpędzi). Oszacowanie filtru tylko o 4,9 m — o 41 % mniej. A prędkość, której żaden czujnik nie mierzy, szacuje z błędem 0,8 m/s: filtr wyprowadza ją z tego, jak zmienia się położenie. Na początku prędkości nie zna (zaczyna od zera), ale po kilku sekundach ją łapie. Pas ±2σ to uczciwa niepewność filtru — rzeczywiste położenie leży w nim w 95 % kroków, dokładnie tak, jak powinno.

Definicja — średni błąd (RMSE) i pas ±2σ

„Średni błąd“ w tej części to pierwiastek błędu średniokwadratowego (RMSE) po krokach k = 10, …, 59 (N = 50):

RMSE = √( (1/N) ∑_k (x̂_k − x_k)² ),

gdzie x_k to rzeczywiste położenie, a x̂_k oszacowanie (albo sam pomiar). Kwadraty karzą duże błędy bardziej niż małe, a pierwiastek przywraca wynik do metrów. Dla samego GPS RMSE powinien wyjść około σ = 8 m — i wyszedł 8,3 m.

Pas ±2σ to przedział x̂_k ± 2√P_k,11. Dla rozkładu normalnego w przedziale μ ± 2σ leży prawdopodobieństwo 0,9545; jeśli filtr ma niepewność P policzoną poprawnie, rzeczywistość musi trafić do pasa w mniej więcej 95 % kroków.

Wzmocnienie Kalmana po kilku krokach ustala się na K ≈ (0,30; 0,05): z każdej innowacji filtr przejmuje 30 % do położenia i 5 % (na metr innowacji 0,05 m/s) do prędkości. W 50 symulowanych przejazdach błąd filtru wynosi średnio 55 % błędu samego GPS.

Twierdzenie — ustalone wzmocnienie

We wzorach na P⁻, K i P nie ma pomiarów z — kowariancję i wzmocnienie da się policzyć z góry. Połączenie predykcji i korekcji daje rekurencję (dyskretne równanie Riccatiego)

P⁻_k+1 = F (P⁻_k − P⁻_kH^T (HP⁻_kH^T + R)⁻¹ HP⁻_k) F^T + Q.

Dla stałych F, H, Q, R spełniających zwykłe warunki (układ jest obserwowalny — z serii pomiarów da się ustalić cały stan — a szum Q działa na wszystkie jego składowe) ciąg zbiega do jedynego punktu stałego niezależnie od początkowego P₀; wzmocnienie zbiega wtedy do wartości ustalonej.

W samochodzie: po 60 krokach K = (0,297; 0,052), a P = [[19,04; 3,35], [3,35; 1,29]], czyli filtr ufa sobie z dokładnością √19,04 ≈ 4,4 m w położeniu i √1,29 ≈ 1,1 m/s w prędkości.

6. Komu ufać: Q i R

Filtr musi znać Q i R. Dla GPS producent zwykle je podaje, ale szum procesu szacuje się trudniej. Co, jeśli dasz filtrowi inne wartości niż te, które mają dane?

Kiedy filtr wierzy, że samochód jedzie prawie dokładnie ze stałą prędkością (małe Q), trzyma się swojego modelu i na rzeczywiste przyspieszenie reaguje z opóźnieniem — błąd położenia jest nawet większy niż dla samego GPS. Kiedy natomiast spodziewa się gwałtownych zmian (duże Q), nie ufa modelowi i kopiuje zaszumione pomiary. Najlepiej wypada z rzeczywistą wartością 0,5. To samo dotyczy R: przy niepewności GPS 2, 8 i 30 m błąd położenia wychodzi 5,66; 4,86 i 5,39 m.

7. Dlaczego właśnie tak

Twierdzenie — optymalność filtru Kalmana

Jeśli model jest liniowy, a szumy normalne, rozkład N(x̂_k, P_k) jest dokładnie rozkładem a posteriori stanu P(x_k | z₁, …, z_k). Bez założenia normalności filtr Kalmana jest najlepszym liniowym estymatorem w sensie błędu średniokwadratowego.

p(x_k | z_1:k) ∝ p(z_k | x_k) · ∫ p(x_k | x_k−1) p(x_k−1 | z_1:k−1) dx_k−1

∫ p(x_k | x_k−1) p(x_k−1 | z_1:k−1) dx_k−1

predykcja (rozkład a priori): poprzedniego stanu nie znamy dokładnie, więc uśredniamy przejście po wszystkich możliwych poprzednich stanach, ważonych tym, w co wierzyliśmy ostatnio. Całka to ciągły odpowiednik sumy ∑_i w algorytmie forward dla HMM. Dla modelu liniowego i rozkładu normalnego wychodzi N(Fx̂, FPF^T + Q)

p(z_k | x_k)

wiarygodność nowego pomiaru — jak dobrze każde miejsce wyjaśniałoby to, co pokazał GPS; z modelu to N(z_k; Hx_k, R)

p(x_k | z_1:k)

rozkład a posteriori po wszystkich pomiarach; „∝“ dlatego, że stałej normalizującej p(z_k | z_1:k−1) nie potrzebujemy — iloczyn dwóch gęstości normalnych dolicza dowód twierdzenia z początku części, i wychodzi z niego dokładnie krok korekcji

8. Kiedy świat nie jest liniowy

• Rozszerzony filtr Kalmana (EKF) w każdym kroku linearyzuje model nieliniowy za pomocą pochodnej — tak robiła też nawigacja Apolla.
• Bezśladowy filtr Kalmana (UKF, ang. unscented) zamiast pochodnych przepuszcza przez model kilka starannie wybranych punktów i z nich liczy nową wartość oczekiwaną i kowariancję.
• Filtr cząsteczkowy całkowicie rezygnuje z rozkładu normalnego: oszacowanie reprezentuje tysiącami próbek („cząstek“), które poruszają się według modelu i są przeważane według pomiarów. Poradzi sobie nawet z oszacowaniem o wielu wierzchołkach, na przykład „robot jest albo w kuchni, albo w salonie“.
• Gdzie wszędzie: łączenie GPS, akcelerometru i żyroskopu w telefonach, nawigacja samolotów, dronów i rakiet, śledzenie obiektów radarem, szacowanie położenia samochodów autonomicznych, a nawet wygładzanie ekonomicznych szeregów czasowych.

Definicja — rozszerzony filtr Kalmana (EKF)

Model jest nieliniowy: x_k = f(x_k−1) + w_k, z_k = h(x_k) + v_k. EKF liczy wartość oczekiwaną funkcjami nieliniowymi, a kowariancję ich macierzami Jacobiego (macierzami pochodnych cząstkowych) w bieżącym oszacowaniu:

F_k = ∂f/∂x |_x̂k−1,   H_k = ∂h/∂x |_x̂⁻k,

x̂⁻ = f(x̂),   P⁻ = F_k P F_k^T + Q,   x̂ = x̂⁻ + K (z − h(x̂⁻)),   K i P jak wcześniej z H_k zamiast H.

Przykład: radar mierzy odległość h(p_x, p_y) = √(p_x² + p_y²); wtedy H_k = (p_x/d, p_y/d) z d = √(p_x² + p_y²) — wektor jednostkowy w kierunku celu. Dla celu w (300, 400) m jest d = 500 m i H_k = (0,6; 0,8). Wynik nie jest już dokładnym rozkładem a posteriori, tylko jego przybliżeniem; im bardziej model jest zakrzywiony w zakresie niepewności, tym gorszym.

9. Podsumowanie

• Dwa niezależne normalne pomiary łączy się średnią ważoną; precyzje się sumują, a waga K to wzmocnienie Kalmana.
• Filtr Kalmana powtarza predykcję według modelu i korekcję według pomiaru; dla niezmiennej wielkości to średnia bieżąca.
• W samochodzie zmniejszył błąd położenia z 8,3 m (GPS) do 4,9 m i oszacował też niemierzoną prędkość z błędem 0,8 m/s; pas ±2σ objął rzeczywistość w 95 % kroków.
• Wynik zależy od Q i R: zbyt duże zaufanie do modelu reaguje z opóźnieniem, zbyt małe kopiuje szum.
• Dla modelu liniowego z szumem normalnym filtr jest dokładną aktualizacją bayesowską; z nieliniowym światem radzą sobie EKF, UKF i filtry cząsteczkowe.

Tą częścią kończy się moduł o modelach probabilistycznych. Chcesz to wypróbować w praktyce? Poproś agenta AI, żeby napisał filtr Kalmana dla danych z akcelerometru twojego telefonu, i sprawdź, jak dobrze oszacuje z nich, o ile przesunąłeś telefon.

Masz inne zdanie? Napisz na support@coden-app.com.