EKF – Rozšířený (nelineární) Kalmanův filtr

Symbol blokuPotřebná licence: MODEL

Popis funkce
Funkční blok implementuje algoritmus nelineárního rekonstruktoru stavu známého jako Rozšířený Kalmanův filtr. Cílem je poskytnout odhad přímo neměřitelných stavových veličin nelineárního dynamického systému popsaného stavovou a výstupní rovnicí ve tvaru $d x ∕ d t = f (x, u) + w (t), y = h (x, u) + v (t)$ pro případ spojitého času a $x (k + 1) = f (x (k), u (k)) + w (k), y (k) = h (x (k), u (k)) + v (k)$ pro případ diskrétního systému. Veličiny $w, v$ označují náhodný šum modelu a pozorování. Předpokládá se, že jde o náhodné procesy s nulovou střední hodnotou a Gaussovým rozdělením hustoty pravděpodobnosti definované kovariancemi $Q$ and $R$ , které se zadávají jako parametr bloku. Rozšířený Kalmanův filtr je nelineární verze algoritmu Kalmanova filtru pracující na principu linearizace stavové a výstupní rovnice v okolí aktuálního pracovního bodu. Jde o algoritmus typu prediktor-korektor, který střídá fázi predikce stavu v otevřené smyčce s využitím modelu a korekce (filtrace) na základě přímo měřených pozorování. Vektor výstupů-pozorování může být dodáván asynchronně vůči periodickému běhu algoritmu filtrace v libovolných okamžicích spouštění bloku.

Krok predikce je vykonáván každou periodu běhu bloku a řeší stavovou rovnici technikami numerické integrace, počínaje zadaným stavem $x 0$ a počáteční kovariancí $P 0$ . Volbou parametru $s o l v e r$ uživatel vybírá numerickou metodu integrace příslušné vektorové diferenciální rovnice. Pro speciální případ volby $s o l v e r = 1$ algoritmus přechází na diskrétní variantu modelu a numerická integrace se redukuje na pouhé vyhodnocení pravé strany rekurze definované stavovou diferenční rovnicí $x (k + 1) = f (x (k), u (k))$ . Kromě vektoru stavu je v čase propagována také příslušná kovarianční matice $P$ , která uchovává informaci o neurčitosti odhadu. Více detailů o jednotlivých numerických metodách lze nalézt v dokumentaci k bloku NSSM.

Krok filtrace je proveden vždy když je v daném okamžiku spuštění bloku na vstup přivedena hodnota $n z > 0$ . Toto signalizuje dostupný vektor měření na vstupu $z$ , který je následně použit pro opravu odhadu stavu a jeho kovariance. Je možné kombinovat více pravých stran výstupní rovnice pomocí kooperujícího bloku REXLANG. Toto může být užitečné v aplikacích s větším počtem senzorů, které dodávají data s různou periodou vzorkování nebo nepravidelně oproti periodickému spouštění bloku. Pro nastavení $n z = 0$ signalizuje uživatelský algoritmus bloku nedostupnost měření a v dané periodě je provedena extrapolace stavu pozorovaného systému na základě modelu.

Kalmanův filtr obecně není optimální rekonstruktor stavu ve smyslu minimalizace střední kvadratické chyby odhadu. Nicméně, v praktických úlohách s dostatečně hladkou nelineární dynamikou systému poskytuje odhady stavu v rozumné kvalitě a je považován za de facto standard v oblasti nelineární filtrace. V případě zadání lineární pravé strany stavové a výstupní rovnice přechází algoritmus odhadu na standardní Kalmanův filtr, který je již optimální pro danou stochastickou formulaci problému odhadu stavu lineárního systému.

Vstupy

funcRef	Odkaz na spolupracující blok REXLANG	Reference
u	Vektor vstupů modelu	Reference
z	Vektor výstupů (měření) modelu	Reference
nz	Číslo sady měřených výstupů $↓$ 1	Long (I32)
Qk	Kovarianční matice stavového šumu	Reference
Rk	Kovarianční matice výstupního šumu	Reference
RST	Reset bloku	Bool
HLD	Pozastavení	Bool
x0	Vektor počáteční hodnoty stavu modelu	Reference
P0	Počáteční hodnota kovarianční matice modelu	Reference

Parametry

nmax	Rezervovaná paměť pro výstupní matici (celkový počet prvků) $↓$ 5 $↑$ 10000 $⊙$ 20	Long (I32)
solver	Metoda numerické integrace $⊙$ 2	Long (I32)
	1 .... diskrétní model 2 .... eulerova metoda (1. řád) 3 .... metoda Adams-Bashforth 2. řádu 4 .... metoda Adams-Bashforth 3. řádu 5 .... metoda Adams-Bashforth 4. řádu 6 .... metoda Adams-Bashforth 5. řádu 7 .... Runge-Kutta 4. řádu 8 .... implicitní Euler 9 .... implicitní Euler (více iterací) 10 ... implicitní Adams-Moulton 2. řádu 11 ... implicitní Adams-Moulton 2. řádu (více iterací) 12 ... implicitní Adams-Moulton 3. řádu 13 ... implicitní Adams-Moulton 3. řádu (více iterací) 14 ... implicitní RadauIIA 2. řádu 15 ... implicitní RadauIIA 2. řádu (více iterací) 16 ... implicitní RadauIIA 3. řádu 17 ... implicitní RadauIIA 3. řádu (více iterací)

Výstupy

x	Vektor stavu modelu	Reference
P	Kovarianční matice stavu modelu	Reference
trP	Stopa kovarianční matice	Reference
cmd	Číslo příkazu navázaného bloku REXLANG	Long (I32)
f	Odkaz na vektor nastavovaný navázaným blokem REXLANG	Reference
df	Odkaz na matici nastavovanou navázaným blokem REXLANG	Reference
err	Kód chyby (0 bez chyby, jinak popis v system logu)	Long (I32)

[Předchozí] [Na začátek] [Výše] [Další]