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

Symbol blokuPotřebná licence: MODEL
PIC

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 dxdt = 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 x0 a počáteční kovariancí P0. Volbou parametru solver uživatel vybírá numerickou metodu integrace příslušné vektorové diferenciální rovnice. Pro speciální případ volby solver = 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 nz > 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í nz = 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)

2024 © REX Controls s.r.o., www.rexygen.com