Kalmanův filtr: jak z nepřesných měření odhadnout, kde auto je a jak rychle jede

Spojení dvou měření a Kalmanův zisk, filtr jako průběžný průměr, stavový model, předpověď a oprava, sledování auta podle GPS s odhadem neměřené rychlosti, ladění šumů Q a R, optimalita a nelineární rozšíření.

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

Série Základy AI Díl 41 z 83

Celá osnova série 83 z 83 témat hotovo

Co si odnesete

Jak se spojí dvě nepřesná měření a co je Kalmanův zisk.
Jak filtr střídá předpověď podle modelu a opravu podle měření — a odhadne i to, co nikdo neměří.
Proč na nastavení šumů Q a R tolik záleží a jak si filtr poradí s nelineárním světem.

Co se hodí znát předem: Skryté Markovovy modely: Viterbiho algoritmus a nepoctivé kasino, Přehled pravděpodobnostních rozdělení: Bernoulli, binomické, Poisson, rovnoměrné, normální a exponenciální

Loď Apollo 11 letěla k Měsíci s palubním počítačem, který měl méně paměti než dnešní grafická kalkulačka. Navigace přitom musela z nepřesných měření hvězd, radaru a setrvačných čidel průběžně odhadovat, kde loď je a jak rychle letí. Pomohla jí metoda, kterou v roce 1960 publikoval Rudolf Kálmán a kterou pro NASA dotáhl do praxe Stanley Schmidt. Kalmanův filtr je dnes v každém mobilu, který spojuje GPS s akcelerometrem, v každém dronu a v každém autě, které samo udrží jízdní pruh. Tímhle dílem končí modul o pravděpodobnostních modelech.

Navazuje na díly Skryté Markovovy modely a Přehled pravděpodobnostních rozdělení.

1. Dvě nepřesná měření jsou lepší než jedno

Začneme jednoduše. Vzdálenost k majáku změříme dvakrát: jednou přesným přístrojem — 100 m s nejistotou (směrodatnou odchylkou) 5 m —, podruhé hrubým — 110 m s nejistotou 10 m. Co je nejlepší odhad?

Věta — spojení dvou normálních odhadů

Nechť x ∈ ℝ je neznámá veličina, m₁, m₂ ∈ ℝ a σ₁², σ₂² > 0. Nechť první odhad říká x ~ N(m₁, σ₁²) (apriorní rozdělení) a druhý odhad je pozorování m₂ = x + ε, kde šum ε ~ N(0, σ₂²) je nezávislý na x (normální rozdělení viz díl Přehled pravděpodobnostních rozdělení). Označme

K = σ₁²/σ₁² + σ₂² ∈ (0, 1).

Pak spojený odhad — podmíněné (aposteriorní) rozdělení x při pozorování m₂ — je normální N(m, σ²) s

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

Ekvivalentně 1/σ² = 1/σ₁² + 1/σ₂²: přesnosti (převrácené rozptyly) se sčítají.

Důkaz. Podle Bayesovy věty je aposteriorní hustota úměrná součinu apriorní hustoty a věrohodnosti, tj. obou normálních hustot. V exponentu se sčítají dvě kvadratické funkce x:

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

Hustota s exponentem −1/2 ax² + bx + konst. (a > 0) je po doplnění na čtverec normální s rozptylem 1/a a střední hodnotou b/a. Odtud 1/σ² = 1/σ₁² + 1/σ₂², tedy

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

Laicky řečeno

Vezměte první odhad a posuňte ho směrem k druhému — tím víc, čím víc druhému věříte. Číslo K říká, jak velký kus rozdílu převzít. Výsledek je přesnější než každé z měření samo; i hrubé měření přidá informaci.

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

m₁, σ₁²: první odhad (v obrázku modrý) je výchozí bod — od něj se posouváme; jeho rozptyl v čitateli K říká: čím méně věříme prvnímu odhadu, tím dál od něj se smíme pohnout
m₂, σ₂²: druhý odhad (jantarový); do vzorce vstupuje jen rozdílem m₂ − m₁ — kdyby se oba shodovaly, není co opravovat. Jeho rozptyl ve jmenovateli K zisk zmenšuje: hrubému měření věříme málo
K: Kalmanův zisk, číslo mezi 0 a 1 — jaký podíl rozdílu převzít. Je to podíl nejistoty prvního odhadu na celkové nejistotě, takže přesnější odhad „táhne“ výsledek k sobě
m, σ²: spojený odhad (zelený); činitel 1 − K < 1 zaručuje, že nová nejistota je menší než ta první — každé měření, i hrubé, něco přidá

Posuň se od prvního odhadu k druhému o podíl daný tím, komu víc věříš.

Spojení dvou měření vzdálenosti přesné: 100 ± 5 mhrubé: 110 ± 10 mspojené: 102 ± 4,5 m

Tady K = 25/25 + 100 = 0,2 (barvy podle vzorce výše): převezmeme pětinu rozdílu a dostaneme 102 m s nejistotou √20 ≈ 4,5 m — méně než 5 m přesného přístroje. Tohle číslo K je Kalmanův zisk a celý filtr není nic jiného než tenhle krok, opakovaný v čase.

Příklad — spojení měření 100 ± 5 m a 110 ± 10 m krok za krokem

Barvy podle rozboru vzorce výše.

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

Zisk: K = 25/25 + 100 = 0,2.

Střední hodnota: m = 100 + 0,2 · (110 − 100) = 102 m — totéž jako vážený průměr 0,8 · 100 + 0,2 · 110.

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

Kontrola přes přesnosti: 1/25 + 1/100 = 0,04 + 0,01 = 0,05 = 1/20. ✓

2. Opakované měření jedné veličiny

Teplotu v místnosti měříme teploměrem s nejistotou 2 °C, padesátkrát po sobě, a po každém měření spojíme nový údaj s dosavadním odhadem. Po padesáti měřeních je odhad 19,895 °C — přesně aritmetický průměr všech měření — a nejistota 0,283 °C = 2/√50. Kalmanův filtr pro veličinu, která se nemění, je tedy obyčejný průběžný průměr. Zajímavý začne být, když se veličina v čase mění.

Věta — filtr pro neměnnou veličinu je průběžný průměr

Nechť σ² > 0 je rozptyl měřidla a z₁, z₂, … ∈ ℝ jsou měření neměnné veličiny. Filtr začne odhadem m₀ ∈ ℝ s rozptylem v₀ > 0 a pro k = 1, 2, … spojí dosavadní odhad N(m_k−1, v_k−1) s měřením z_k podle předchozí věty:

K_k = v_k−1/v_k−1 + σ², m_k = m_k−1 + K_k (z_k − m_k−1), v_k = (1 − K_k) v_k−1.

Pak v limitě v₀ → ∞ (počáteční odhad je tak nejistý, že ho první měření úplně přebije) platí pro každé k ≥ 1

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

Důkaz indukcí podle k. Pro k = 1 je K₁ = v₀/v₀ + σ² → 1, takže m₁ = m₀ + K₁(z₁ − m₀) → z₁ a v₁ = v₀σ²/v₀ + σ² → σ². Nechť k ≥ 2 a tvrzení platí pro k − 1, tj. v_k−1 = σ²/k − 1 a m_k−1 je průměr z₁, …, z_k−1. Pak

K_k = v_k−1/v_k−1 + σ² = 1/k − 1/1/k − 1 + 1 = 1/k, v_k = (1 − 1/k) · σ²/k − 1 = σ²/k,

m_k = m_k−1 + 1/k (z_k − m_k−1) = (k − 1) m_k−1 + z_k/k,

a to je průměr všech k měření, protože (k − 1) m_k−1 = z₁ + … + z_k−1. ∎

Laicky řečeno

Desáté měření má v průměru deseti čísel váhu jedna desetina — a přesně tolik mu dá i filtr. Čím víc toho už víme, tím méně nás jedno nové měření pohne. Nejistota klesá jako 1/√k: pro poloviční chybu potřebujete čtyřikrát víc měření.

Příklad — první tři měření teploty

Náš filtr začíná odhadem 0 °C s rozptylem 10⁶ (nejistota 1 000 °C — „nevím nic“), rozptyl měření je 2² = 4.

Každý krok je spojení dvou odhadů z první kapitoly: dosavadní odhad hraje roli prvního, měření druhého (barvy podle rozboru vzorce pro spojení výše).

1. měření 23,098 °C: K₁ = 10⁶/10⁶ + 4 ≈ 1, odhad 23,098 °C, rozptyl 4,00.

2. měření 21,967 °C: K₂ = 4/4 + 4 = 1/2, odhad 23,098 + 1/2 · (21,967 − 23,098) = 22,533 °C, rozptyl 2,00.

3. měření 16,630 °C: K₃ = 2/2 + 4 = 1/3, odhad 22,533 + 1/3 · (16,630 − 22,533) = 20,565 °C, rozptyl 1,33 = 4/3.

Kontrola: 23,098 + 21,967 + 16,630/3 = 20,565 °C — obyčejný průměr. Po padesáti měřeních je rozptyl 4/50 = 0,08 a nejistota √0,08 ≈ 0,283 °C.

3. Stavový model

Definice — lineární stavový model

Nechť n, m ≥ 1 jsou rozměry stavu a měření a

F ∈ ℝ^n×n je přechodová matice,
H ∈ ℝ^m×n je matice měření,
Q ∈ ℝ^n×n je symetrická pozitivně semidefinitní matice — kovariance procesního šumu,
R ∈ ℝ^m×m je symetrická pozitivně definitní matice — kovariance šumu měření,
x̂₀ ∈ ℝⁿ a symetrická pozitivně semidefinitní P₀ ∈ ℝ^n×n popisují počáteční stav x₀ ~ N(x̂₀, P₀).

Lineární stavový model je posloupnost náhodných vektorů — skrytých stavů x_k ∈ ℝⁿ a měření z_k ∈ ℝ^m, k = 1, 2, … — pro které platí

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),

kde x₀, w₁, w₂, …, v₁, v₂, … jsou navzájem nezávislé. (Vícerozměrné normální rozdělení N(μ, Σ) a kovarianční matici definujeme níže.) Stav se tedy vyvíjí podle F s náhodnými nárazy w_k a měření ho zachycuje přes H se šumem v_k.

Laicky řečeno

Je to skrytý Markovův model z minulého dílu, jen stav není „poctivá nebo falešná kostka“, ale vektor čísel — třeba poloha a rychlost. Fyzika říká, jak se stav zhruba vyvíjí (F), náhodné nárazy a nepřesnost modelu shrnuje Q. Měřidlo vidí jen část stavu (H) a plete se o R.

Náš příklad: auto jede po silnici zhruba stálou rychlostí 15 m/s, ale řidič občas přibrzdí nebo přidá — náhodné zrychlení se směrodatnou odchylkou 0,5 m/s². Stav je (poloha, rychlost), krok 1 s, takže F = [[1, 1], [0, 1]]: nová poloha je stará plus rychlost. GPS měří jen polohu, s nejistotou 8 m: H = [1, 0], R = 64.

Definice — kovarianční matice a vícerozměrné normální rozdělení

Nechť n ≥ 1 a X = (X₁, …, X_n)^T je náhodný vektor, jehož složky jsou náhodné veličiny s konečným rozptylem. Jeho střední hodnota je vektor μ = E[X] = (E[X₁], …, E[X_n])^T ∈ ℝⁿ (střední hodnota a kovariance viz díl Střední hodnota, rozptyl a kovariance). Jeho kovarianční matice je Σ ∈ ℝ^n×n,

Σ = E[(X − μ)(X − μ)^T], tj. Σ_ij = cov(X_i, X_j) = E[(X_i − μ_i)(X_j − μ_j)] pro i, j = 1, …, n.

Na diagonále jsou rozptyly složek (Σ_ii = var X_i), mimo ni kovariance. Matice je symetrická (Σ = Σ^T) a pozitivně semidefinitní: pro každé a ∈ ℝⁿ je a^TΣa = var(a^TX) ≥ 0.

Nechť μ ∈ ℝⁿ a Σ ∈ ℝ^n×n je symetrická pozitivně definitní matice. Náhodný vektor X má vícerozměrné normální rozdělení, píšeme X ~ N(μ, Σ), právě když má hustotu

f(x) = 1/√(2π)ⁿ det Σ exp(−1/2 (x − μ)^T Σ⁻¹ (x − μ)), x ∈ ℝⁿ.

Pak E[X] = μ a kovarianční matice X je Σ. Díl používá dvě vlastnosti:

Je-li X ~ N(μ, Σ), A ∈ ℝ^p×n a c ∈ ℝ^p, pak AX + c ~ N(Aμ + c, A Σ A^T) (je-li AΣA^T singulární, jde o tzv. degenerované normální rozdělení bez hustoty).
Jsou-li X ~ N(μ₁, Σ₁) a Y ~ N(μ₂, Σ₂) nezávislé vektory v ℝⁿ, pak X + Y ~ N(μ₁ + μ₂, Σ₁ + Σ₂).

Laicky řečeno

Kovarianční matice je „nejistota pro víc čísel najednou“. Na diagonále stojí, jak moc si nejsme jistí polohou a jak moc rychlostí. Mimo diagonálu je, jestli se chyby drží spolu: kladná kovariance polohy a rychlosti znamená „když jsme auto odhadli moc vpředu, nejspíš jsme mu přisoudili i moc velkou rychlost“.

Příklad — odkud se vezme matice Q

Během kroku délky Δt působí na auto stálé náhodné zrychlení a ~ N(0, q²). Ze vzorců pro rovnoměrně zrychlený pohyb:

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

Bez šumu je to F = [[1, Δt], [0, 1]]. Šum je w = g a s vektorem g = (½ Δt², Δt), a podle pravidla AΣA^T výše

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

Pro Δt = 1 s a q = 0,5 m/s²: Q = 0,25 · [[0,25; 0,5], [0,5; 1]] = [[0,0625; 0,125], [0,125; 0,25]]. Za jednu sekundu tedy náhodné zrychlení přidá nejistotu √0,0625 = 0,25 m v poloze a 0,5 m/s v rychlosti — a obě chyby mají stejné znaménko, proto je kovariance kladná. Přesně tahle matice je v kódu níže.

4. Předpověď a oprava

Algoritmus — Kalmanův filtr

Vstup: matice F, H, Q, R lineárního stavového modelu (definice výše), počáteční odhad x̂₀ ∈ ℝⁿ s kovariancí P₀ ∈ ℝ^n×n (symetrická, pozitivně semidefinitní) a měření z₁, …, z_T ∈ ℝ^m.

Výstup: pro každé k = 1, …, T odhad x̂_k ∈ ℝⁿ a jeho kovariance P_k ∈ ℝ^n×n — střední hodnota a kovariance normálního rozdělení, kterým filtr popisuje stav x_k po měřeních z₁, …, z_k. I je jednotková matice n × n.

Kroky: pro k = 1, 2, …, T postupně:

Předpověď: x̂_k⁻ = F x̂_k−1, P_k⁻ = F P_k−1 F^T + Q.
Inovace a její kovariance: y_k = z_k − H x̂_k⁻, S_k = H P_k⁻ H^T + R (je pozitivně definitní, protože R je, a tedy invertibilní).
Zisk: K_k = P_k⁻ H^T S_k⁻¹ ∈ ℝ^n×m.
Oprava: x̂_k = x̂_k⁻ + K_k y_k, P_k = (I − K_k H) P_k⁻.

Algoritmus skončí po zpracování posledního měření z_T (při průběžném běhu pokračuje s každým novým měřením). V rozborech níže index k vynecháváme: x̂, P vpravo jsou hodnoty z minulého kroku, vlevo nové.

Laicky řečeno

Předpověď: „podle fyziky by auto teď mělo být tady“ — a protože fyzika není dokonalá, nejistota trochu vzroste. Oprava: přijde měření; rozdíl mezi měřením a předpovědí (inovace) se převezme jen z části K, podle toho, komu víc věříme. Je to spojení dvou měření z první kapitoly — jedním „měřením“ je předpověď z minula, druhým GPS. Kovariance P si navíc pamatuje, jak spolu souvisí chyba polohy a rychlosti, a díky tomu filtr opravuje i rychlost, kterou nikdo neměří.

Barvy v rozborech níže sedí s obrázky: předpověď (dřív „první odhad“) modře, měření jantarově, výsledek zeleně, zisk fialově.

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

x̂⁻, P⁻: předpověď na tento krok, než přijde měření — hraje roli „prvního odhadu“ ze spojení dvou měření
F … F^T: model posune odhad dopředu; nejistota se přenáší pravidlem AΣA^T (u auta: nejistá rychlost se za sekundu promítne do nejistoty polohy). Nestačí FP, protože kovariance je kvadratická veličina — násobí se zleva i zprava
Q: model není dokonalý (řidič přibrzdí), takže se nejistota přičítá v každém kroku — bez Q by P klesla k nule a filtr by přestal měření poslouchat

Předpověď: kam podle fyziky stav dojde a o kolik se tím zvětší nejistota.

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

H P⁻ H^T: nejistota předpovědi přepočtená do řeči měřidla (u auta jen rozptyl polohy) — odpovídá σ₁²
R: rozptyl měření, odpovídá σ₂²; součet v závorce je rozptyl inovace S — o kolik se měření a předpověď mohou rozcházet čistě náhodou
P⁻ H^T: kovariance celého stavu s měřenou veličinou; díky ní dostane nenulový zisk i rychlost, kterou nikdo neměří — chyba polohy a rychlosti spolu souvisí
K: Kalmanův zisk, pro každou složku stavu jedno číslo; inverze místo dělení jen proto, že měření může být vektor

V jedné dimenzi s H = 1 je P⁻H^T = HP⁻H^T = σ₁² a vzorec je přesně K = σ₁²/σ₁² + σ₂² z první kapitoly.

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

x̂⁻, P⁻: předpověď — výchozí bod, od kterého se posouváme
z: nové měření (v obrázku jantarové body GPS); jediné místo, kudy do odhadu vstupují data
H: převede předpověď stavu na to, co by měřidlo mělo ukázat; měření a stav se nedají odečíst přímo, protože GPS vidí jen polohu, ne rychlost
K: jaký podíl inovace z − Hx̂⁻ převzít do každé složky stavu
x̂, P: opravený odhad (v obrázku zelený); činitel I − KH nejistotu zmenší — tím víc, čím větší díl měření jsme převzali

Oprava: totéž „posuň se k měření o podíl K“, jen pro vektory.

import numpy as np

F = np.array([[1, 1], [0, 1]])                    # poloha += rychlost
H = np.array([[1, 0]])                            # měříme jen polohu
Q = 0.5**2 * np.array([[1/4, 1/2], [1/2, 1]])     # náhodné zrychlení
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             # předpověď
        K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)
        x = x + K @ (z - H @ x)                   # oprava podle inovace
        P = (np.eye(2) - K @ H) @ P
        yield x, P

Příklad — první dva kroky filtru na datech auta

Start. První měření GPS je z₁ = 8,44 m. Filtr začne polohou 8,44 m, rychlostí 0 a velkou nejistotou P₀ = [[100; 0], [0; 100]] (10 m a 10 m/s). Oprava týmž měřením polohu nezmění (inovace je 0), jen zmenší její rozptyl (barvy podle rozborů výše): K = 100/100 + 64 = 0,610, P₁₁ = (1 − 0,610) · 100 = 39,02.

Předpověď na 2. sekundu. x̂⁻ = (8,44 + 0; 0) = (8,44; 0,00). Kovariance FPF^T přičte k rozptylu polohy rozptyl rychlosti (poloha += rychlost) a k tomu se přičte Q:

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

Oprava. Měření z₂ = 23,23 m, inovace y = 23,23 − 8,44 = 14,78 m, její rozptyl 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 dvou měřeních filtr „ví“, že se auto hýbe, přestože rychlost nikdo neměřil: odhadl 7,3 m/s (skutečnost 13,9 m/s). Zisk pro rychlost je nenulový jen díky kovarianci 100,13 mimo diagonálu. Proč jen polovina? Měření jsou zašuměná o 8 m, takže filtr rozdíl 15 m ještě nebere úplně vážně — za další sekundy se rychlost doladí.

5. Filtr sleduje auto

Minuta jízdy, jedno měření GPS za sekundu. Za tu dobu auto ujede necelý kilometr, takže by se šum v grafu polohy ztratil. Kreslíme proto, o kolik je auto napřed nebo pozadu oproti jízdě přesně 15 m/s:

Sledování auta: nahoře odchylka polohy, dole rychlost skutečnost měření GPS odhad filtru ± 2σ

Měření GPS se od skutečné polohy odchylují v průměru o 8,3 m (od desáté sekundy, kdy se filtr rozběhne). Odhad filtru jen o 4,9 m — o 41 % méně. A rychlost, kterou žádné čidlo neměří, odhadne s chybou 0,8 m/s: filtr ji odvodí z toho, jak se mění poloha. Na začátku rychlost nezná (začíná od nuly), za pár sekund se ale chytí. Pás ±2σ je filtrova poctivá nejistota — skutečná poloha leží uvnitř v 95 % kroků, přesně jak má.

Definice — průměrná chyba (RMSE) a pás ±2σ

Nechť 𝒦 je konečná neprázdná množina kroků, N = |𝒦|, a pro každé k ∈ 𝒦 je p_k ∈ ℝ skutečná poloha a p̂_k ∈ ℝ její odhad (první složka odhadu filtru x̂_k, nebo samotné měření). Odmocnina ze střední kvadratické chyby (RMSE) je číslo

RMSE = √1/N ∑_{k ∈ 𝒦} (p̂_k − p_k)².

„Průměrná chyba“ v tomto dílu je RMSE přes kroky 𝒦 = {10, …, 59}, tedy N = 50. Čtverce trestají velké chyby víc než malé a odmocnina vrací výsledek do metrů. Pro samotnou GPS by RMSE mělo vyjít kolem σ = 8 m — a vyšlo 8,3 m. Pro rychlost se RMSE definuje stejně s druhou složkou stavu.

Nechť P_k ∈ ℝ^2×2 je kovariance odhadu filtru v kroku k a P_k,11 její prvek vlevo nahoře (rozptyl polohy). Pás ±2σ v kroku k je interval

[p̂_k − 2√P_k,11, p̂_k + 2√P_k,11].

Pro X ~ N(μ, σ²) je P(|X − μ| ≤ 2σ) ≈ 0,9545; má-li filtr nejistotu P_k spočítanou správně, musí skutečnost padnout do pásu asi v 95 % kroků.

Kalmanův zisk se po pár krocích ustálí na K ≈ (0,30; 0,05): z každé inovace převezme filtr 30 % do polohy a 5 % (na metr inovace 0,05 m/s) do rychlosti. Přes 50 simulovaných jízd je chyba filtru v průměru 55 % chyby samotné GPS.

Věta — ustálený zisk

Ve vzorcích pro P⁻, K a P nejsou měření z — kovariance i zisk se dají spočítat předem. Spojením předpovědi a opravy vznikne rekurence (diskrétní Riccatiho rovnice)

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

Nechť F ∈ ℝ^n×n, H ∈ ℝ^m×n, R pozitivně definitní a Q = GG^T pro nějaké G ∈ ℝ^n×r jsou pevné (nezávislé na k) a platí

dvojice (F, H) je pozorovatelná: matice složená pod sebe z H, HF, …, HFⁿ⁻¹ má hodnost n (z řady měření se dá zjistit celý stav);
dvojice (F, G) je řiditelná: matice složená vedle sebe z G, FG, …, Fⁿ⁻¹G má hodnost n (šum se časem dostane do všech složek stavu).

Pak pro každou symetrickou pozitivně semidefinitní počáteční matici posloupnost P⁻_k konverguje k matici P_∞, která nezávisí na počáteční matici a je jediným symetrickým pozitivně semidefinitním pevným bodem rekurence; zisk K_k konverguje k ustálené hodnotě K_∞ = P_∞H^T (HP_∞H^T + R)⁻¹. U auta obě podmínky platí: [H; HF] = [[1, 0], [1, 1]] i [g, Fg] = [[0,5; 1,5], [1; 1]] mají hodnost 2.

U auta: po 60 krocích je K = (0,297; 0,052) a P = [[19,04; 3,35], [3,35; 1,29]], tedy filtr si věří na √19,04 ≈ 4,4 m v poloze a √1,29 ≈ 1,1 m/s v rychlosti.

Laicky řečeno

Na začátku filtr neví skoro nic a hltá každé měření (zisk 0,61, pak 0,68). Jak se odhad zpřesňuje, zisk klesá — až se ustálí tam, kde nejistota přidaná za krok šumem Q je stejně velká jako ta, kterou ubere měření. Dál už se nic nemění, a proto se v jednoduchých zařízeních zisk často spočítá jednou předem a filtr pak jen násobí pevnými čísly.

6. Komu věřit: Q a R

Filtr potřebuje znát Q a R. GPS výrobce obvykle udává, ale procesní šum se odhaduje hůř. Co když filtru dáme jiné hodnoty, než jaké mají data?

Procesní šum, kterému filtr věří (m/s²)	0,01	0,1	0,5 (pravda)	2	10
chyba polohy (m)	9,29	5,88	4,86	5,67	7,08
chyba rychlosti (m/s)	1,19	0,86	0,79	1,53	5,19

Když filtr věří, že auto jede skoro přesně stálou rychlostí (malé Q), drží se svého modelu a na skutečné zrychlení reaguje pozdě — chyba polohy je dokonce větší než u samotné GPS. Když naopak čeká divoké změny (velké Q), nevěří modelu a kopíruje zašuměná měření. Nejlépe dopadne se skutečnou hodnotou 0,5. Totéž platí pro R: s nejistotou GPS 2, 8 a 30 m vyjde chyba polohy 5,66; 4,86 a 5,39 m.

7. Proč zrovna takhle

Věta — optimalita Kalmanova filtru

Nechť x_k, z_k splňují lineární stavový model (definice výše) s počátečním stavem x₀ ~ N(x̂₀, P₀) a nechť x̂_k, P_k jsou výstupy Kalmanova filtru spuštěného se stejnými F, H, Q, R, x̂₀, P₀. Pak pro každé k ≥ 1:

podmíněné (aposteriorní) rozdělení stavu x_k při měřeních z₁, …, z_k je přesně N(x̂_k, P_k);
zejména x̂_k = E[x_k | z₁, …, z_k], a ze všech odhadů g(z₁, …, z_k) má nejmenší střední kvadratickou chybu E[‖x_k − g(z₁, …, z_k)‖²].

Vypustíme-li předpoklad normality (x₀, w_k, v_k jsou jen nekorelované se stejnými středními hodnotami a kovariancemi), je x̂_k nejlepší ve smyslu střední kvadratické chyby mezi odhady, které jsou afinní (lineární plus konstanta) funkcí z₁, …, z_k.

Laicky řečeno

Kalmanův filtr je Bayesova věta aplikovaná znovu a znovu. Předpověď je apriorní rozdělení, měření je věrohodnost, oprava je aposteriorní rozdělení. Normální rozdělení má tu vzácnou vlastnost, že součin normálních je zase normální — a tak stačí pamatovat si střední hodnotu a kovarianci místo celé funkce. Je to dopředný algoritmus ze skrytých Markovových modelů pro spojitý stav.

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: předpověď (apriorní rozdělení): minulý stav neznáme přesně, tak průměrujeme přechod přes všechny možné minulé stavy, vážené tím, čemu jsme minule věřili. Integrál je spojitá obdoba součtu ∑_i v dopředném algoritmu HMM. Pro lineární model a normální rozdělení vyjde N(Fx̂, FPF^T + Q)
p(z_k | x_k): věrohodnost nového měření — jak dobře by které místo vysvětlilo, co GPS ukázala; z modelu je to N(z_k; Hx_k, R)
p(x_k | z_1:k): aposteriorní rozdělení po všech měřeních; „∝“ proto, že normalizační konstantu p(z_k | z_1:k−1) nepotřebujeme — součin dvou normálních hustot dopočítá důkaz věty ze začátku dílu, a vyjde z něj přesně krok opravy

Bayesova věta v čase: předpověď krát věrohodnost dává nový odhad, který je předpovědí pro příští krok.

8. Když svět není lineární

Rozšířený Kalmanův filtr (EKF) nelineární model v každém kroku linearizuje derivací — tak to dělala i navigace Apolla.
Unscented Kalmanův filtr (UKF) místo derivací prožene modelem pár pečlivě vybraných bodů a z nich spočítá novou střední hodnotu a kovarianci.
Částicový filtr se vzdá normálního rozdělení úplně: odhad reprezentuje tisíci vzorků („částic“), které se pohybují podle modelu a převažují podle měření. Zvládne i odhad s více vrcholy, třeba „robot je buď v kuchyni, nebo v obýváku“.
Kde všude: spojování GPS, akcelerometru a gyroskopu v mobilech, navigace letadel, dronů a raket, sledování objektů radarem, odhad polohy autonomních aut, a dokonce vyhlazování ekonomických časových řad.

Definice — rozšířený Kalmanův filtr (EKF)

Nechť f : ℝⁿ → ℝⁿ a h : ℝⁿ → ℝ^m jsou diferencovatelné funkce (derivace viz díl Derivace, gradient a řetízkové pravidlo), Q, R jako v lineárním modelu a model je nelineární:

x_k = f(x_k−1) + w_k, z_k = h(x_k) + v_k, w_k ~ N(0, Q), v_k ~ N(0, R), všechny šumy nezávislé.

Jacobiho matice funkce f v bodě a ∈ ℝⁿ je matice parciálních derivací J_f(a) ∈ ℝ^n×n, (J_f(a))_ij = ∂f_i/∂x_j(a); obdobně J_h(a) ∈ ℝ^m×n. Rozšířený Kalmanův filtr má stejný vstup a výstup jako Kalmanův filtr (s f, h místo F, H) a pro k = 1, 2, …, T provede:

Předpověď: F_k = J_f(x̂_k−1), x̂_k⁻ = f(x̂_k−1), P_k⁻ = F_k P_k−1 F_k^T + Q.
Zisk: H_k = J_h(x̂_k⁻), K_k = P_k⁻ H_k^T (H_k P_k⁻ H_k^T + R)⁻¹.
Oprava: x̂_k = x̂_k⁻ + K_k (z_k − h(x̂_k⁻)), P_k = (I − K_k H_k) P_k⁻.

Střední hodnotu tedy EKF počítá nelineárními funkcemi, kovarianci jejich linearizací v aktuálním odhadu.

Příklad: radar měří vzdálenost h(p_x, p_y) = √p_x² + p_y²; pak H_k = (p_x/d, p_y/d) s d = √p_x² + p_y² — jednotkový vektor směrem k cíli. Pro cíl v (300, 400) m je d = 500 m a H_k = (0,6; 0,8). Výsledek už není přesné aposteriorní rozdělení, jen jeho přiblížení; čím víc je model v rozsahu nejistoty zakřivený, tím horší.

9. Shrnutí

Dvě nezávislá normální měření se spojí váženým průměrem; přesnosti se sčítají a váha K je Kalmanův zisk.
Kalmanův filtr opakuje předpověď podle modelu a opravu podle měření; pro neměnnou veličinu je to průběžný průměr.
U auta snížil chybu polohy z 8,3 m (GPS) na 4,9 m a odhadl i neměřenou rychlost s chybou 0,8 m/s; pás ±2σ pokryl skutečnost v 95 % kroků.
Výsledek záleží na Q a R: příliš velká důvěra v model reaguje pozdě, příliš malá kopíruje šum.
Pro lineární model s normálním šumem je filtr přesná Bayesova aktualizace; nelineární svět zvládají EKF, UKF a částicové filtry.

Tímhle dílem končí modul o pravděpodobnostních modelech. Chcete si to osahat? Nechte AI agenta napsat Kalmanův filtr pro data z akcelerometru vlastního telefonu a zkuste, jak dobře z nich odhadne, o kolik jste telefon posunuli.

Máte na to jiný názor? Napište na support@coden-app.com.

Kalmanův filtr: jak z nepřesných měření odhadnout, kde auto je a jak rychle jede

1. Dvě nepřesná měření jsou lepší než jedno

2. Opakované měření jedné veličiny

3. Stavový model

4. Předpověď a oprava

5. Filtr sleduje auto

6. Komu věřit: Q a R

7. Proč zrovna takhle

8. Když svět není lineární

9. Shrnutí

Agent na vašem počítači, ovládaný z telefonu