Kalmanov filter: ako z nepresných meraní odhadnúť, kde auto je a ako rýchlo ide

Spojenie dvoch meraní a Kalmanov zisk, filter ako priebežný priemer, stavový model, predpoveď a oprava, sledovanie auta podľa GPS s odhadom nemeranej rýchlosti, ladenie šumov Q a R, optimalita a nelineárne rozšírenia.

14 minút Matěj Žalmánek 2026-10-15

Séria Základy AI Diel 41 z 52

Celá osnova série Hotovo: 52 z 83 tém

Čo si odnesiete

Ako sa spoja dve nepresné merania a čo je Kalmanov zisk.
Ako filter strieda predpoveď podľa modelu a opravu podľa merania — a odhadne aj to, čo nikto nemeria.
Prečo na nastavení šumov Q a R toľko záleží a ako si filter poradí s nelineárnym svetom.

Čo sa hodí vedieť vopred: Skryté Markovove modely: Viterbiho algoritmus a nepoctivé kasíno, Prehľad pravdepodobnostných rozdelení: Bernoulliho, binomické, Poissonovo, rovnomerné, normálne a exponenciálne

Loď Apollo 11 letela k Mesiacu s palubným počítačom, ktorý mal menej pamäte než dnešná grafická kalkulačka. Navigácia pritom musela z nepresných meraní hviezd, radaru a zotrvačných snímačov priebežne odhadovať, kde loď je a ako rýchlo letí. Pomohla jej metóda, ktorú v roku 1960 publikoval Rudolf Kálmán a ktorú pre NASA dotiahol do praxe Stanley Schmidt. Kalmanov filter je dnes v každom mobile, ktorý spája GPS s akcelerometrom, v každom dróne a v každom aute, ktoré samo udrží jazdný pruh. Týmto dielom sa končí modul o pravdepodobnostných modeloch.

Nadväzuje na diely Skryté Markovove modely a Prehľad pravdepodobnostných rozdelení.

1. Dve nepresné merania sú lepšie ako jedno

Začneme jednoducho. Vzdialenosť k majáku zmeriame dvakrát: raz presným prístrojom — 100 m s neistotou (smerodajnou odchýlkou) 5 m —, druhý raz hrubým — 110 m s neistotou 10 m. Aký je najlepší odhad?

Veta — spojenie dvoch normálnych odhadov

Nech x ∈ ℝ je neznáma veličina, m₁, m₂ ∈ ℝ a σ₁², σ₂² > 0. Nech prvý odhad hovorí x ~ N(m₁, σ₁²) (apriórne rozdelenie) a druhý odhad je pozorovanie m₂ = x + ε, kde šum ε ~ N(0, σ₂²) je nezávislý od x (normálne rozdelenie pozri diel Prehľad pravdepodobnostných rozdelení). Označme

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

Potom spojený odhad — podmienené (aposteriórne) rozdelenie x pri pozorovaní m₂ — je normálne N(m, σ²) s

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

Ekvivalentne 1/σ² = 1/σ₁² + 1/σ₂²: presnosti (prevrátené rozptyly) sa sčítajú.

Dôkaz. Podľa Bayesovej vety je aposteriórna hustota úmerná súčinu apriórnej hustoty a vierohodnosti, t. j. oboch normálnych hustôt. V exponente sa sčítajú dve kvadratické funkcie x:

−(x − m₁)²/2σ₁² − (x − m₂)²/2σ₂² = −1/2 (1/σ₁² + 1/σ₂²) x² + (m₁/σ₁² + m₂/σ₂²) x + konšt.

Hustota s exponentom −1/2 ax² + bx + konšt. (a > 0) je po doplnení na štvorec normálna s rozptylom 1/a a strednou hodnotou b/a. Odtiaľ 1/σ² = 1/σ₁² + 1/σ₂², teda

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

Laicky povedané

Vezmite prvý odhad a posuňte ho smerom k druhému — tým viac, čím viac druhému veríte. Číslo K hovorí, aký veľký kus rozdielu prevziať. Výsledok je presnejší než každé z meraní samo; aj hrubé meranie pridá informáciu.

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

m₁, σ₁²: prvý odhad (na obrázku modrý) je východiskový bod — od neho sa posúvame; jeho rozptyl v čitateli K hovorí: čím menej veríme prvému odhadu, tým ďalej od neho sa smieme pohnúť
m₂, σ₂²: druhý odhad (jantárový); do vzorca vstupuje len rozdielom m₂ − m₁ — keby sa oba zhodovali, nie je čo opravovať. Jeho rozptyl v menovateli K zisk zmenšuje: hrubému meraniu veríme málo
K: Kalmanov zisk, číslo medzi 0 a 1 — aký podiel rozdielu prevziať. Je to podiel neistoty prvého odhadu na celkovej neistote, takže presnejší odhad „ťahá“ výsledok k sebe
m, σ²: spojený odhad (zelený); činiteľ 1 − K < 1 zaručuje, že nová neistota je menšia než tá prvá — každé meranie, aj hrubé, niečo pridá

Posuňte sa od prvého odhadu k druhému o podiel daný tým, komu viac veríte.

Spojenie dvoch meraní vzdialenosti presné: 100 ± 5 mhrubé: 110 ± 10 mspojené: 102 ± 4,5 m

Tu K = 25/25 + 100 = 0,2 (farby podľa vzorca vyššie): prevezmeme pätinu rozdielu a dostaneme 102 m s neistotou √20 ≈ 4,5 m — menej než 5 m presného prístroja. Toto číslo K je Kalmanov zisk a celý filter nie je nič iné než tento krok, opakovaný v čase.

Príklad — spojenie meraní 100 ± 5 m a 110 ± 10 m krok za krokom

Farby podľa rozboru vzorca vyššie.

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

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

Stredná hodnota: m = 100 + 0,2 · (110 − 100) = 102 m — to isté ako vážený priemer 0,8 · 100 + 0,2 · 110.

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

Kontrola cez presnosti: 1/25 + 1/100 = 0,04 + 0,01 = 0,05 = 1/20. ✓

2. Opakované meranie jednej veličiny

Teplotu v miestnosti meriame teplomerom s neistotou 2 °C, päťdesiatkrát po sebe, a po každom meraní spojíme nový údaj s doterajším odhadom. Po päťdesiatich meraniach je odhad 19,895 °C — presne aritmetický priemer všetkých meraní — a neistota 0,283 °C = 2/√50. Kalmanov filter pre veličinu, ktorá sa nemení, je teda obyčajný priebežný priemer. Zaujímavý začne byť, keď sa veličina v čase mení.

Veta — filter pre nemennú veličinu je priebežný priemer

Nech σ² > 0 je rozptyl meradla a z₁, z₂, … ∈ ℝ sú merania nemennej veličiny. Filter začne odhadom m₀ ∈ ℝ s rozptylom v₀ > 0 a pre k = 1, 2, … spojí doterajší odhad N(m_k−1, v_k−1) s meraním z_k podľa predchádzajúcej vety:

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.

Potom v limite v₀ → ∞ (počiatočný odhad je taký neistý, že ho prvé meranie úplne prebije) platí pre každé k ≥ 1

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

Dôkaz indukciou podľa k. Pre k = 1 je K₁ = v₀/v₀ + σ² → 1, takže m₁ = m₀ + K₁(z₁ − m₀) → z₁ a v₁ = v₀σ²/v₀ + σ² → σ². Nech k ≥ 2 a tvrdenie platí pre k − 1, t. j. v_k−1 = σ²/k − 1 a m_k−1 je priemer z₁, …, z_k−1. Potom

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 priemer všetkých k meraní, pretože (k − 1) m_k−1 = z₁ + … + z_k−1. ∎

Laicky povedané

Desiate meranie má v priemere desiatich čísel váhu jedna desatina — a presne toľko mu dá aj filter. Čím viac toho už vieme, tým menej nami jedno nové meranie pohne. Neistota klesá ako 1/√k: pre polovičnú chybu potrebujete štyrikrát viac meraní.

Príklad — prvé tri merania teploty

Náš filter začína odhadom 0 °C s rozptylom 10⁶ (neistota 1 000 °C — „neviem nič“), rozptyl merania je 2² = 4.

Každý krok je spojenie dvoch odhadov z prvej kapitoly: doterajší odhad hrá úlohu prvého, meranie druhého (farby podľa rozboru vzorca pre spojenie vyššie).

1. meranie 23,098 °C: K₁ = 10⁶/10⁶ + 4 ≈ 1, odhad 23,098 °C, rozptyl 4,00.

2. meranie 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. meranie 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čajný priemer. Po päťdesiatich meraniach je rozptyl 4/50 = 0,08 a neistota √0,08 ≈ 0,283 °C.

3. Stavový model

Definícia — lineárny stavový model

Nech n, m ≥ 1 sú rozmery stavu a merania a

F ∈ ℝ^n×n je prechodová matica,
H ∈ ℝ^m×n je matica merania,
Q ∈ ℝ^n×n je symetrická pozitívne semidefinitná matica — kovariancia procesného šumu,
R ∈ ℝ^m×m je symetrická pozitívne definitná matica — kovariancia šumu merania,
x̂₀ ∈ ℝⁿ a symetrická pozitívne semidefinitná P₀ ∈ ℝ^n×n opisujú počiatočný stav x₀ ~ N(x̂₀, P₀).

Lineárny stavový model je postupnosť náhodných vektorov — skrytých stavov x_k ∈ ℝⁿ a meraní z_k ∈ ℝ^m, k = 1, 2, … — pre ktoré 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₂, … sú navzájom nezávislé. (Viacrozmerné normálne rozdelenie N(μ, Σ) a kovariančnú maticu definujeme nižšie.) Stav sa teda vyvíja podľa F s náhodnými nárazmi w_k a meranie ho zachytáva cez H so šumom v_k.

Laicky povedané

Je to skrytý Markovov model z minulého dielu, len stav nie je „poctivá alebo falošná kocka“, ale vektor čísel — napríklad poloha a rýchlosť. Fyzika hovorí, ako sa stav zhruba vyvíja (F), náhodné nárazy a nepresnosť modelu zhŕňa Q. Meradlo vidí len časť stavu (H) a mýli sa o R.

Náš príklad: auto ide po ceste zhruba stálou rýchlosťou 15 m/s, ale vodič občas pribrzdí alebo pridá — náhodné zrýchlenie so smerodajnou odchýlkou 0,5 m/s². Stav je (poloha, rýchlosť), krok 1 s, takže F = [[1, 1], [0, 1]]: nová poloha je stará plus rýchlosť. GPS meria len polohu, s neistotou 8 m: H = [1, 0], R = 64.

Definícia — kovariančná matica a viacrozmerné normálne rozdelenie

Nech n ≥ 1 a X = (X₁, …, X_n)^T je náhodný vektor, ktorého zložky sú náhodné premenné s konečným rozptylom. Jeho stredná hodnota je vektor μ = E[X] = (E[X₁], …, E[X_n])^T ∈ ℝⁿ (strednú hodnotu a kovarianciu pozri diel Stredná hodnota, rozptyl a kovariancia). Jeho kovariančná matica je Σ ∈ ℝ^n×n,

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

Na diagonále sú rozptyly zložiek (Σ_ii = var X_i), mimo nej kovariancie. Matica je symetrická (Σ = Σ^T) a pozitívne semidefinitná: pre každé a ∈ ℝⁿ je a^TΣa = var(a^TX) ≥ 0.

Nech μ ∈ ℝⁿ a Σ ∈ ℝ^n×n je symetrická pozitívne definitná matica. Náhodný vektor X má viacrozmerné normálne rozdelenie, píšeme X ~ N(μ, Σ), práve vtedy, keď má hustotu

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

Potom E[X] = μ a kovariančná matica X je Σ. Diel používa dve vlastnosti:

Ak X ~ N(μ, Σ), A ∈ ℝ^p×n a c ∈ ℝ^p, potom AX + c ~ N(Aμ + c, A Σ A^T) (ak je AΣA^T singulárna, ide o tzv. degenerované normálne rozdelenie bez hustoty).
Ak sú X ~ N(μ₁, Σ₁) a Y ~ N(μ₂, Σ₂) nezávislé vektory v ℝⁿ, potom X + Y ~ N(μ₁ + μ₂, Σ₁ + Σ₂).

Laicky povedané

Kovariančná matica je „neistota pre viac čísel naraz“. Na diagonále stojí, nakoľko si nie sme istí polohou a nakoľko rýchlosťou. Mimo diagonály je, či sa chyby držia spolu: kladná kovariancia polohy a rýchlosti znamená „keď sme auto odhadli príliš vpredu, najskôr sme mu pripísali aj príliš veľkú rýchlosť“.

Príklad — odkiaľ sa vezme matica Q

Počas kroku dĺžky Δt pôsobí na auto stále náhodné zrýchlenie a ~ N(0, q²). Zo vzorcov pre rovnomerne zrýchlený 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 vektorom g = (½ Δt², Δt), a podľa pravidla AΣA^T vyššie

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

Pre Δ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 teda náhodné zrýchlenie pridá neistotu √0,0625 = 0,25 m v polohe a 0,5 m/s v rýchlosti — a obe chyby majú rovnaké znamienko, preto je kovariancia kladná. Presne táto matica je v kóde nižšie.

4. Predpoveď a oprava

Algoritmus — Kalmanov filter

Vstup: matice F, H, Q, R lineárneho stavového modelu (definícia vyššie), počiatočný odhad x̂₀ ∈ ℝⁿ s kovarianciou P₀ ∈ ℝ^n×n (symetrická, pozitívne semidefinitná) a merania z₁, …, z_T ∈ ℝ^m.

Výstup: pre každé k = 1, …, T odhad x̂_k ∈ ℝⁿ a jeho kovariancia P_k ∈ ℝ^n×n — stredná hodnota a kovariancia normálneho rozdelenia, ktorým filter opisuje stav x_k po meraniach z₁, …, z_k. I je jednotková matica n × n.

Kroky: pre k = 1, 2, …, T postupne:

Predpoveď: x̂_k⁻ = F x̂_k−1, P_k⁻ = F P_k−1 F^T + Q.
Inovácia a jej kovariancia: y_k = z_k − H x̂_k⁻, S_k = H P_k⁻ H^T + R (je pozitívne definitná, pretože R je, a teda 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 spracovaní posledného merania z_T (pri priebežnom behu pokračuje s každým novým meraním). V rozboroch nižšie index k vynechávame: x̂, P vpravo sú hodnoty z minulého kroku, vľavo nové.

Laicky povedané

Predpoveď: „podľa fyziky by auto teraz malo byť tu“ — a pretože fyzika nie je dokonalá, neistota trochu vzrastie. Oprava: príde meranie; rozdiel medzi meraním a predpoveďou (inovácia) sa prevezme len z časti K, podľa toho, komu viac veríme. Je to spojenie dvoch meraní z prvej kapitoly — jedným „meraním“ je predpoveď z minula, druhým GPS. Kovariancia P si navyše pamätá, ako spolu súvisí chyba polohy a rýchlosti, a vďaka tomu filter opravuje aj rýchlosť, ktorú nikto nemeria.

Farby v rozboroch nižšie sedia s obrázkami: predpoveď (predtým „prvý odhad“) modrou, meranie jantárovou, výsledok zelenou, zisk fialovou.

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

x̂⁻, P⁻: predpoveď na tento krok, skôr než príde meranie — hrá úlohu „prvého odhadu“ zo spojenia dvoch meraní
F … F^T: model posunie odhad dopredu; neistota sa prenáša pravidlom AΣA^T (pri aute: neistá rýchlosť sa za sekundu premietne do neistoty polohy). Nestačí FP, pretože kovariancia je kvadratická veličina — násobí sa zľava aj sprava
Q: model nie je dokonalý (vodič pribrzdí), takže sa neistota pripočítava v každom kroku — bez Q by P klesla k nule a filter by prestal meranie počúvať

Predpoveď: kam sa stav podľa fyziky dostane a o koľko sa tým zväčší neistota.

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

H P⁻ H^T: neistota predpovede prepočítaná do reči meradla (pri aute len rozptyl polohy) — zodpovedá σ₁²
R: rozptyl merania, zodpovedá σ₂²; súčet v zátvorke je rozptyl inovácie S — o koľko sa meranie a predpoveď môžu rozchádzať čisto náhodou
P⁻ H^T: kovariancia celého stavu s meranou veličinou; vďaka nej dostane nenulový zisk aj rýchlosť, ktorú nikto nemeria — chyba polohy a rýchlosti spolu súvisia
K: Kalmanov zisk, pre každú zložku stavu jedno číslo; inverzia namiesto delenia len preto, že meranie môže byť vektor

V jednej dimenzii s H = 1 je P⁻H^T = HP⁻H^T = σ₁² a vzorec je presne K = σ₁²/σ₁² + σ₂² z prvej kapitoly.

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

x̂⁻, P⁻: predpoveď — východiskový bod, od ktorého sa posúvame
z: nové meranie (na obrázku jantárové body GPS); jediné miesto, kadiaľ do odhadu vstupujú dáta
H: prevedie predpoveď stavu na to, čo by meradlo malo ukázať; meranie a stav sa nedajú odčítať priamo, pretože GPS vidí len polohu, nie rýchlosť
K: aký podiel inovácie z − Hx̂⁻ prevziať do každej zložky stavu
x̂, P: opravený odhad (na obrázku zelený); činiteľ I − KH neistotu zmenší — tým viac, čím väčší diel merania sme prevzali

Oprava: to isté „posuňte sa k meraniu o podiel K“, len pre vektory.

import numpy as np

F = np.array([[1, 1], [0, 1]])                    # poloha += rýchlosť
H = np.array([[1, 0]])                            # meriame len polohu
Q = 0.5**2 * np.array([[1/4, 1/2], [1/2, 1]])     # náhodné zrýchlenie
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             # predpoveď
        K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)
        x = x + K @ (z - H @ x)                   # oprava podľa inovácie
        P = (np.eye(2) - K @ H) @ P
        yield x, P

Príklad — prvé dva kroky filtra na dátach auta

Štart. Prvé meranie GPS je z₁ = 8,44 m. Filter začne polohou 8,44 m, rýchlosťou 0 a veľkou neistotou P₀ = [[100; 0], [0; 100]] (10 m a 10 m/s). Oprava tým istým meraním polohu nezmení (inovácia je 0), len zmenší jej rozptyl (farby podľa rozborov vyššie): K = 100/100 + 64 = 0,610, P₁₁ = (1 − 0,610) · 100 = 39,02.

Predpoveď na 2. sekundu. x̂⁻ = (8,44 + 0; 0) = (8,44; 0,00). Kovariancia FPF^T pripočíta k rozptylu polohy rozptyl rýchlosti (poloha += rýchlosť) a k tomu sa pripočíta 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. Meranie z₂ = 23,23 m, inovácia 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 dvoch meraniach filter „vie“, že sa auto hýbe, hoci rýchlosť nikto nemeral: odhadol 7,3 m/s (skutočnosť 13,9 m/s). Zisk pre rýchlosť je nenulový len vďaka kovariancii 100,13 mimo diagonály. Prečo len polovica? Merania sú zašumené o 8 m, takže filter rozdiel 15 m ešte neberie úplne vážne — počas ďalších sekúnd sa rýchlosť doladí.

5. Filter sleduje auto

Minúta jazdy, jedno meranie GPS za sekundu. Za ten čas auto prejde necelý kilometer, takže by sa šum v grafe polohy stratil. Kreslíme preto, o koľko je auto vpredu alebo pozadu oproti jazde presne 15 m/s:

Sledovanie auta: hore odchýlka polohy, dole rýchlosť skutočnosť meranie GPS odhad filtra ± 2σ

Merania GPS sa od skutočnej polohy odchyľujú v priemere o 8,3 m (od desiatej sekundy, keď sa filter rozbehne). Odhad filtra len o 4,9 m — o 41 % menej. A rýchlosť, ktorú žiadny snímač nemeria, odhadne s chybou 0,8 m/s: filter ju odvodí z toho, ako sa mení poloha. Na začiatku rýchlosť nepozná (začína od nuly), za pár sekúnd sa ale chytí. Pás ±2σ je poctivá neistota filtra — skutočná poloha leží vnútri v 95 % krokov, presne ako má.

Definícia — priemerná chyba (RMSE) a pás ±2σ

Nech 𝒦 je konečná neprázdna množina krokov, N = |𝒦|, a pre každé k ∈ 𝒦 je p_k ∈ ℝ skutočná poloha a p̂_k ∈ ℝ jej odhad (prvá zložka odhadu filtra x̂_k, alebo samotné meranie). Odmocnina zo strednej kvadratickej chyby (RMSE) je číslo

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

„Priemerná chyba“ v tomto diele je RMSE cez kroky 𝒦 = {10, …, 59}, teda N = 50. Štvorce trestajú veľké chyby viac než malé a odmocnina vracia výsledok do metrov. Pre samotnú GPS by RMSE malo vyjsť okolo σ = 8 m — a vyšlo 8,3 m. Pre rýchlosť sa RMSE definuje rovnako s druhou zložkou stavu.

Nech P_k ∈ ℝ^2×2 je kovariancia odhadu filtra v kroku k a P_k,11 jej prvok vľavo hore (rozptyl polohy). Pás ±2σ v kroku k je interval

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

Pre X ~ N(μ, σ²) je P(|X − μ| ≤ 2σ) ≈ 0,9545; ak má filter neistotu P_k vypočítanú správne, musí skutočnosť padnúť do pásu asi v 95 % krokov.

Kalmanov zisk sa po pár krokoch ustáli na K ≈ (0,30; 0,05): z každej inovácie prevezme filter 30 % do polohy a 5 % (na meter inovácie 0,05 m/s) do rýchlosti. Cez 50 simulovaných jázd je chyba filtra v priemere 55 % chyby samotnej GPS.

Veta — ustálený zisk

Vo vzorcoch pre P⁻, K a P nie sú merania z — kovariancia aj zisk sa dajú vypočítať vopred. Spojením predpovede a opravy vznikne rekurencia (diskrétna Riccatiho rovnica)

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 pozitívne definitná a Q = GG^T pre nejaké G ∈ ℝ^n×r sú pevné (nezávislé od k) a platí

dvojica (F, H) je pozorovateľná: matica zložená pod seba z H, HF, …, HFⁿ⁻¹ má hodnosť n (z radu meraní sa dá zistiť celý stav);
dvojica (F, G) je riaditeľná: matica zložená vedľa seba z G, FG, …, Fⁿ⁻¹G má hodnosť n (šum sa časom dostane do všetkých zložiek stavu).

Potom pre každú symetrickú pozitívne semidefinitnú počiatočnú maticu postupnosť P⁻_k konverguje k matici P_∞, ktorá nezávisí od počiatočnej matice a je jediným symetrickým pozitívne semidefinitným pevným bodom rekurencie; zisk K_k konverguje k ustálenej hodnote K_∞ = P_∞H^T (HP_∞H^T + R)⁻¹. Pri aute obe podmienky platia: [H; HF] = [[1, 0], [1, 1]] aj [g, Fg] = [[0,5; 1,5], [1; 1]] majú hodnosť 2.

Pri aute: po 60 krokoch je K = (0,297; 0,052) a P = [[19,04; 3,35], [3,35; 1,29]], teda filter si verí na √19,04 ≈ 4,4 m v polohe a √1,29 ≈ 1,1 m/s v rýchlosti.

Laicky povedané

Na začiatku filter nevie skoro nič a hltá každé meranie (zisk 0,61, potom 0,68). Ako sa odhad spresňuje, zisk klesá — až sa ustáli tam, kde neistota pridaná za krok šumom Q je rovnako veľká ako tá, ktorú uberie meranie. Ďalej sa už nič nemení, a preto sa v jednoduchých zariadeniach zisk často vypočíta raz vopred a filter potom len násobí pevnými číslami.

6. Komu veriť: Q a R

Filter potrebuje poznať Q a R. GPS výrobca zvyčajne udáva, ale procesný šum sa odhaduje ťažšie. Čo ak filtru dáme iné hodnoty, než aké majú dáta?

Procesný šum, ktorému filter verí (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 rýchlosti (m/s)	1,19	0,86	0,79	1,53	5,19

Keď filter verí, že auto ide skoro presne stálou rýchlosťou (malé Q), drží sa svojho modelu a na skutočné zrýchlenie reaguje neskoro — chyba polohy je dokonca väčšia než pri samotnej GPS. Keď naopak čaká divoké zmeny (veľké Q), neverí modelu a kopíruje zašumené merania. Najlepšie dopadne so skutočnou hodnotou 0,5. To isté platí pre R: s neistotou GPS 2, 8 a 30 m vyjde chyba polohy 5,66; 4,86 a 5,39 m.

7. Prečo práve takto

Veta — optimalita Kalmanovho filtra

Nech x_k, z_k spĺňajú lineárny stavový model (definícia vyššie) s počiatočným stavom x₀ ~ N(x̂₀, P₀) a nech x̂_k, P_k sú výstupy Kalmanovho filtra spusteného s rovnakými F, H, Q, R, x̂₀, P₀. Potom pre každé k ≥ 1:

podmienené (aposteriórne) rozdelenie stavu x_k pri meraniach z₁, …, z_k je presne N(x̂_k, P_k);
najmä x̂_k = E[x_k | z₁, …, z_k], a zo všetkých odhadov g(z₁, …, z_k) má najmenšiu strednú kvadratickú chybu E[‖x_k − g(z₁, …, z_k)‖²].

Ak vypustíme predpoklad normality (x₀, w_k, v_k sú len nekorelované s rovnakými strednými hodnotami a kovarianciami), je x̂_k najlepší v zmysle strednej kvadratickej chyby medzi odhadmi, ktoré sú afinnou (lineárnou plus konštanta) funkciou z₁, …, z_k.

Laicky povedané

Kalmanov filter je Bayesova veta aplikovaná znova a znova. Predpoveď je apriórne rozdelenie, meranie je vierohodnosť, oprava je aposteriórne rozdelenie. Normálne rozdelenie má tú vzácnu vlastnosť, že súčin normálnych je opäť normálny — a tak stačí pamätať si strednú hodnotu a kovarianciu namiesto celej funkcie. Je to dopredný algoritmus zo skrytých Markovových modelov pre 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: predpoveď (apriórne rozdelenie): minulý stav nepoznáme presne, tak priemerujeme prechod cez všetky možné minulé stavy, vážené tým, čomu sme minule verili. Integrál je spojitá obdoba súčtu ∑_i v doprednom algoritme HMM. Pre lineárny model a normálne rozdelenie vyjde N(Fx̂, FPF^T + Q)
p(z_k | x_k): vierohodnosť nového merania — ako dobre by ktoré miesto vysvetlilo, čo GPS ukázala; z modelu je to N(z_k; Hx_k, R)
p(x_k | z_1:k): aposteriórne rozdelenie po všetkých meraniach; „∝“ preto, že normalizačnú konštantu p(z_k | z_1:k−1) nepotrebujeme — súčin dvoch normálnych hustôt dopočíta dôkaz vety zo začiatku dielu, a vyjde z neho presne krok opravy

Bayesova veta v čase: predpoveď krát vierohodnosť dáva nový odhad, ktorý je predpoveďou pre ďalší krok.

8. Keď svet nie je lineárny

Rozšírený Kalmanov filter (EKF) nelineárny model v každom kroku linearizuje deriváciou — tak to robila aj navigácia Apolla.
Unscented Kalmanov filter (UKF) namiesto derivácií preženie modelom pár starostlivo vybraných bodov a z nich vypočíta novú strednú hodnotu a kovarianciu.
Časticový filter sa vzdá normálneho rozdelenia úplne: odhad reprezentuje tisíckami vzoriek („častíc“), ktoré sa pohybujú podľa modelu a prevažujú podľa merania. Zvládne aj odhad s viacerými vrcholmi, napríklad „robot je buď v kuchyni, alebo v obývačke“.
Kde všade: spájanie GPS, akcelerometra a gyroskopu v mobiloch, navigácia lietadiel, dronov a rakiet, sledovanie objektov radarom, odhad polohy autonómnych áut, a dokonca vyhladzovanie ekonomických časových radov.

Definícia — rozšírený Kalmanov filter (EKF)

Nech f : ℝⁿ → ℝⁿ a h : ℝⁿ → ℝ^m sú diferencovateľné funkcie (derivácie pozri diel Derivácie, gradient a reťazové pravidlo), Q, R ako v lineárnom modeli a model je nelineárny:

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šetky šumy nezávislé.

Jacobiho matica funkcie f v bode a ∈ ℝⁿ je matica parciálnych derivácií J_f(a) ∈ ℝ^n×n, (J_f(a))_ij = ∂f_i/∂x_j(a); obdobne J_h(a) ∈ ℝ^m×n. Rozšírený Kalmanov filter má rovnaký vstup a výstup ako Kalmanov filter (s f, h namiesto F, H) a pre k = 1, 2, …, T vykoná:

Predpoveď: 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⁻.

Strednú hodnotu teda EKF počíta nelineárnymi funkciami, kovarianciu ich linearizáciou v aktuálnom odhade.

Príklad: radar meria vzdialenosť h(p_x, p_y) = √p_x² + p_y²; potom H_k = (p_x/d, p_y/d) s d = √p_x² + p_y² — jednotkový vektor smerom k cieľu. Pre cieľ v (300, 400) m je d = 500 m a H_k = (0,6; 0,8). Výsledok už nie je presné aposteriórne rozdelenie, len jeho priblíženie; čím viac je model v rozsahu neistoty zakrivený, tým horšie.

9. Zhrnutie

Dve nezávislé normálne merania sa spoja váženým priemerom; presnosti sa sčítajú a váha K je Kalmanov zisk.
Kalmanov filter opakuje predpoveď podľa modelu a opravu podľa merania; pre nemennú veličinu je to priebežný priemer.
Pri aute znížil chybu polohy z 8,3 m (GPS) na 4,9 m a odhadol aj nemeranú rýchlosť s chybou 0,8 m/s; pás ±2σ pokryl skutočnosť v 95 % krokov.
Výsledok závisí od Q a R: príliš veľká dôvera v model reaguje neskoro, príliš malá kopíruje šum.
Pre lineárny model s normálnym šumom je filter presná Bayesova aktualizácia; nelineárny svet zvládajú EKF, UKF a časticové filtre.

Týmto dielom sa končí modul o pravdepodobnostných modeloch. Chcete si to vyskúšať na vlastnej koži? Nechajte AI agenta napísať Kalmanov filter pre dáta z akcelerometra vlastného telefónu a vyskúšajte, ako dobre z nich odhadne, o koľko ste telefón posunuli.

Máte na to iný názor? Napíšte na support@coden-app.com.

Kalmanov filter: ako z nepresných meraní odhadnúť, kde auto je a ako rýchlo ide

1. Dve nepresné merania sú lepšie ako jedno

2. Opakované meranie jednej veličiny

3. Stavový model

4. Predpoveď a oprava

5. Filter sleduje auto

6. Komu veriť: Q a R

7. Prečo práve takto

8. Keď svet nie je lineárny

9. Zhrnutie

Agent na vašom počítači, ovládaný z telefónu