6. Kalmanovi Filtry

Rekurzivní Nejmenší Čtverce - RLS

Rekurzivní nejmenší čtverce (RLS) je metoda používaná pro odhadování parametrů v lineárních modelech. RLS se vyznačuje svou schopností aktualizovat odhady parametrů v reálném čase na základě nových dat. Tento přístup je zvláště užitečný pro časové řady, kde parametry modelu mohou být považovány za konstantní.

Lineární Model a RLS

Uvažujme lineární model:

$$y_t = H_t x + \varepsilon_t,$$

kde:

• $y_t$ je vektor pozorování v čase $t$,
• $H_t$ je matice kompatibilního rozměru,
• $x$ je neznámý parametr (vektor),
• $\varepsilon_t$ je náhodný šum s nulovou střední hodnotou a kovariancí $R$.

Vlastnosti Odhadu

RLS algoritmus má několik klíčových vlastností:

• Nevychýlenost: Pokud je chyba $\varepsilon_t$ nestranná (má nulovou střední hodnotu) a počáteční odhad $\hat{x}_0$ je roven střední hodnotě pravého parametru, pak jsou všechny následující odhady $\hat{x}_t$ nevychýlené.
• Konvergence: S vhodným nastavením zesílení $K_t$ konverguje odhad $\hat{x}_t$ k pravému parametru $x$.

Aplikace RLS

RLS je významný v mnoha aplikacích, kde je potřeba odhadovat parametry modelu v reálném čase nebo kde se data časem mění. Používá se například v systémech pro sledování, v adaptivním řízení nebo v ekonometrických modelech.

Lineární Kalmanův Filtr

Kalmanův filtr je efektivní lineární estimátor pro dynamické systémy a časové řady, který efektivně kombinuje informace z modelu systému a pozorování. Při splnění předpokladů linearity a Gaussovského šumu poskytuje nejlepší možný lineární odhad stavu v smyslu minimalizace střední kvadratické chyby. Jeho schopnost dynamicky aktualizovat odhady na základě nových dat ho činí vhodným pro širokou škálu aplikací, včetně navigace a zpracování signálů.

Model Stavu

Model dynamiky stavů a model pozorování v Kalmanově filtru se vyvíjejí podle těchto rovnic:

1. Model Dynamiky Stavů $$x_t = A_t x_{t-1} + B_t u_t + w_t, \quad w_t\sim \operatorname{iid}\ (0, Q_t),$$

   • $x_t$: Stav systému v čase $t$.
   • $A_t$: Matice dynamiky systému.
   • $B_t$: Matice řídícího vstupu.
   • $u_t$: Vektor externích vstupů v čase $t$.
   • $w_t$: Náhodný šum procesu s kovariancí $Q_t$.

2. Model Pozorování $$y_t = H_t x_t + \varepsilon_t, \quad \varepsilon_t \sim (0, R_t)$$

   • $y_t$: Pozorované hodnoty v čase $t$.
   • $H_t$: Matice pozorování.
   • $\varepsilon_t$: Náhodný měřicí šum s kovariancí $R$.

V tomto kontextu, $x_t$ reprezentuje aktuální stav systému, který je odhadován Kalmanovým filtrem na základě dynamiky systému a měření.

Algoritmus Kalmanova Filtru

High-Level Přehled

1. Inicializace: Nastavení počátečního stavu a kovarianční matice.
2. Iterační Proces:
   1. Predikce Stavů: Odhadnutí budoucího stavu na základě současného stavu a modelu dynamiky.
   2. Korekce Stavů: Aktualizace odhadu stavu pomocí nových pozorování.

Matice a Formulace

• Stavová Matice ($X$):
  • Predikovaný Stav ($x_t^-$): Reprezentuje odhad stavu systému před přijetím nového pozorování. Tento odhad je založen na předchozím odhadu stavu a modelu dynamiky systému.
  • Aktualizovaný Stav ($x_t^+$): Reprezentuje odhad stavu systému po korekci s novým pozorováním. Tento stav vzniká aktualizací predikovaného stavu na základě nových dat.
• Matice Dynamiky Systému ($A$): Matice $A$ je známa jako matice stavového přechodu nebo dynamiky systému. Slouží k popisu, jak se stav systému vyvíjí v čase, nezávisle na jakýchkoli externích vlivech. V kontextu sledování polohy vozidla by matice $A$ mohla například reprezentovat, jak se poloha a rychlost vozidla mění v čase za předpokladu konstantní rychlosti nebo akcelerace.
• Matice Řídícího Vstupu ($B$): Matice $B$ zohledňuje vliv externích řídících vstupů nebo akcí na stav systému. V modelu vozidla by matice $B$ mohla představovat, jak akcelerace nebo jiné řídicí vstupy ovlivňují rychlost a následně polohu vozidla.
• Kovarianční Matice Odhadu ($P$): Vyjadřuje neurčitost (nebo jistotu) odhadu stavu. Rozlišují se dvě formy: $P_t^-$ představuje kovarianční matici odhadu před korekcí, založenou na predikci a modelu systému, a $P_t^+$, která je kovarianční matice odhadu po korekci, reflektující nejistotu odhadu po zahrnutí nových pozorování.
• Matice Zesílení Kalmanova Filtru ($K$): Určuje, jak moc nové pozorování ovlivní aktuální odhad stavu.

Inicializace

1. Nastavení Počátečního Odhadu Stavu $x_0$: Nastavuje se výchozí odhad stavu systému.

   • Tento krok zahrnuje definování počátečního odhadu stavu systému. Odhad může být založen na předchozích znalostech nebo může být nastaven jako nulový či libovolný vektor, pokud neexistují žádné předchozí informace.

2. Nastavení Počáteční Kovariance Odhadů $P_0^+$: Iniciální nastavení kovariance odhadů.

   • Kovarianční matice $P_0^+$ kvantifikuje počáteční neurčitost (nebo jistotu) související s počátečním odhadem stavu. Tato matice je obvykle nastavena na vysokou hodnotu, pokud je počáteční odhad nejistý.

Iterační Proces

Pro každý časový okamžik $t = 1, 2, \ldots$:

1. Predikce Stavů (Časový Update)

   • Odhad Stavu:
     • V tomto kroku se vytváří předpoklad o tom, jak se stav systému vyvinul od posledního měření. Používá se k tomu model systému, který zahrnuje předchozí odhad stavu $\hat{x}_{t-1}^+$ a případně externí vstupy $u_t$. $$\hat{x}_t^{-} = A_t \hat{x}_{t-1}^{+} + B_t u_t,$$
   • Kovarianční Matice Odhadu:
     • Aktualizuje se kovarianční matice odhadu, což reflektuje, jak neurčitost odhadu stavu roste s časem v důsledku vnitřního šumu systému a nejistoty modelu. $$P_t^{-} = A_t P_{t-1}^{+} A_t^\intercal + Q_t.$$

2. Korekce (Datový Update) Stavů Pozorováním $y_t$

   • Výpočet Zesílení Kalmanova Filtru $K_t$:
     • Zesílení Kalmanova filtru $K_t$ je klíčové pro optimalizaci odhadu. Zohledňuje současnou neurčitost odhadu (reflektovanou v $P_t^-$) a spolehlivost nového měření (reflektovanou v $R_t$).
     • Optimální zesílení:
       • Efektivita: Optimální zesílení $K_t$ zajišťuje, že Kalmanův filtr efektivně kombinuje informace z minulých odhadů a aktuálních pozorování.
       • Přesnost: Díky minimalizaci kovarianční matice odhadu $P_t$ je zajištěna vysoká přesnost odhadů stavů. $$K_t = P_{t}^- H_t^\intercal (H_t P_{t}^{-} H_t^\intercal + R_t)^{-1},$$
3. Aktualizace Odhadu Stavu:
   • Na základě nového měření $y_t$ a odhadu stavu před korekcí $\hat{x}_t^-$ se aktualizuje odhad stavu. Korekce odhadu je vedená rozdílem mezi skutečným pozorováním a predikovaným stavem, váženým zesílením $K_t$. $$\hat{x}_t^+ = \hat{x}_{t}^{-} + K_t(y_t - H_t \hat{x}_{t}^{-}),$$
4. Aktualizace Kovarianční Matice Odhadu:
   • Nakonec se aktualizuje kovarianční matice odhadu, aby odrážela změněnou neurčitost odhadu po zahrnutí nového měření. Tento krok snižuje neurčitost odhadu, což je důležité pro zvýšení přesnosti v následujících predikcích. $$P_t^{+} = (I - K_t H_t) P_{t}^{-} (I - K_t H_t)^\intercal + K_t R_t K_t^\intercal.$$

Vlastnosti Kalmanova filtru

• Jsou-li obě šumové složky normálně rozdělené, centrované v nule, nekorelované a bílé, potom je Kalmanův filtr optimální lineární filtr.
• Pokud není zajištěna normalita, je Kalmanův filtr stále nejlepší lineární filtr. To jest, může existovat lepší (např. nelineární) filtr, ale KF je stále nejlepší lineární.
• Pokud jsou šumy korelované nebo barevné, existují různé modifikace.
• Je-li proces nelineární, lze použít rozšířený KF nebo jiné varianty filtru.

Rozšířený Kalmanův filtr - EKF

Rozšířený Kalmanův Filtr (EKF) je varianta Kalmanova filtru pro nelineární systémy. EKF lineárně aproximuje nelineární funkce pomocí prvního řádu Taylorovy řady, což umožňuje použití standardního lineárního Kalmanova filtru.

Model Stavu

1. Nelineární Model Dynamiky Stavů

   • Nelineární vztah stavu a času: $$x_t = f(x_{t-1}, u_t, w_t),$$ kde $f$ je nelineární funkce stavů, $u_t$ jsou externí vstupy, a $w_t$ je procesní šum.

2. Nelineární Model Pozorování

   • Nelineární vztah mezi stavem a pozorováním: $$y_t = h(x_t, \varepsilon_t),$$ kde $h$ je nelineární funkce pozorování a $\varepsilon_t$ je měřicí šum.

Algoritmus Rozšířeného Kalmanova Filtru

1. Inicializace: Nastavení počátečního odhadu stavu $x_0$ a kovarianční matice $P_0^+$.

2. Iterační Proces:

   • Predikce Stavů: Odhad budoucího stavu na základě nelineárního modelu dynamiky stavů.
   • Linearizace: Použití prvního řádu Taylorovy řady pro aproximaci nelineárních funkcí $f$ a $h$, což vede k výpočtu "lineárních ekvivalentů" matic $A_t$ a $H_t$.
   • Korekce Stavů: Aktualizace odhadu stavu pomocí nových pozorování s využitím linearizovaných funkcí.

Rozdíly oproti Lineárnímu Kalmanovu Filtru

• Nelineární Modely: Zatímco lineární Kalmanův filtr předpokládá lineární vztahy mezi stavy a pozorováními, EKF umožňuje pracovat s nelineárními vztahy.
• Linearizace: EKF používá linearizaci nelineárních funkcí pro zjednodušení výpočtů.
• Aproximace: Kvůli linearizaci je EKF pouze aproximací, a jeho přesnost závisí na míře nelinearity systému a na kvalitě aproximace.

Rozšířený Kalmanův filtr je vhodný pro širokou škálu aplikací, kde je potřeba zpracovávat nelineární dynamiku, jako je navigace vozidel, sledování objektů a mnoho dalších oblastí.

Vlastnosti EKF

• Odhad EKF obecně není optimální. Optimalita je zajištěna pouze tehdy, pokud jsou stavové funkce lineární - pak se vše ale zredukuje na lineární stavový model a lineární KF.
• EKF může divergovat, zvl. pokud jsou iniciální stavy nastaveny hodně špatně.
• kovarianční matice odhadů je obvykle podhodnocena, čímž může být filtr inkonzistentní.
• Pokud není model dostatečně dobře znám nebo je nepřesný, je lepší stejně použít Monte Carlo metody.
• Obecně má ale EKF natolik dobré vlastnosti, že je standardem v celé řadě problémů, včetně navigace a GPS.
• V případě potřeby navíc existují vylepšené verze, např. s Taylorovým polynomem druhého řádu. Bohužel s tím ale roste náročnost.

Unscented Kalman filter - UKF

Unscented Kalmanův Filtr je rozšířením tradičního Kalmanova filtru pro nelineární systémy. Místo linearizace nelineárních funkcí využívá unscented transformaci (UT) k efektivnímu odhadu stavů.

Unscented Transformace (UT)

Unscented transformace je klíčová v UKF. Místo lineární aproximace nelineárních funkcí přistupuje k reprezentaci rozdělení pravděpodobnosti stavové proměnné pomocí sady sigma-bodů. Tyto body jsou navrženy tak, aby zachovávaly klíčové statistické vlastnosti (střední hodnotu a kovarianci) původního rozdělení.

Sigma-body

Sigma-body jsou specifické body vybrané tak, aby představovali diskrétní aproximaci hustoty stavové veličiny. Pro stavový vektor $x \in \mathbb{R}^n$ s průměrem $\bar{x}$ a kovarianční maticí $P$, generujeme $2n$ sigma-bodů podle vzorce:

$$\begin{alignat}{2} x^{(i)} &= \bar{x} + \tilde{x}^{(i)}, \quad &&i=1,\ldots,2n, \\ \tilde{x}^{(i)} &= \left( \sqrt{nP} \right)_{i}^\intercal, \quad &&i=1,\ldots,n, \\ \tilde{x}^{(n+i)} &= -\left( \sqrt{nP} \right)_{i}^\intercal, \quad &&i=1,\ldots,n. \end{alignat}$$

kde $\sqrt{nP}$ je maticová odmocnina matice $nP$, a $\left( \sqrt{nP} \right)_i$ značí $i$-tý řádek této matice.

Algoritmus UKF

Inicializace

Zahrnuje nastavení počátečního odhadu stavu $x_0$ a kovarianční matice $P_0^+$.

Iterační Proces

Pro každý časový krok $t = 1, 2, \ldots$ UKF postupuje podle následujících kroků:

1. Inicializace Sigma-bodů

   • Generují se sigma-body z aktuálního odhadu stavu a kovarianční matice. Tyto body reprezentují možné stavy systému. $$\begin{align} \hat{x}_{t-1}^{(i)} &= \hat{x}_{t-1}^+ + \tilde{x}^{(i)}, \quad i=1,\ldots,2n, \\ \tilde{x}^{(i)} &= \left( \sqrt{nP_{t-1}^+} \right)_{i}^\intercal, \quad i=1,\ldots,n, \\ \tilde{x}^{(n+i)} &= -\left( \sqrt{nP_{t-1}^+} \right)_{i}^\intercal, \quad i=1,\ldots,n. \end{align}$$

2. Predikce Stavů

   • Každý sigma-bod je transformován pomocí modelu systému $f_t$. Výsledkem je sada predikovaných sigma-bodů. $$\hat{x}_t^{(i)} = f_t\left(\hat{x}_{t-1}^{(i)}, u_t\right)$$
   • Výpočet průměru a kovariance predikovaných bodů dává predikovaný stav $\hat{x}_t^-$ a kovarianční matici $P_t^-$. $$\begin{align} \tilde{x}^{(i)} &= \left( \sqrt{nP_{t-1}^+} \right)_{i}^\intercal, \quad i=1,\ldots,n, \\ \tilde{x}^{(n+i)} &= -\left( \sqrt{nP_{t-1}^+} \right)_{i}^\intercal, \quad i=1,\ldots,n. \end{align}$$

3. Korekce Stavů Pozorováním $y_t$

   • Volitelná reinicializace sigma-bodů pro korekci
     • Vygenerování nové generace sigma-bodů na základě predikovaného stavu a kovarianční matice pro použití v korekčním kroku.
   • Korekce stavů pozorováním $y_t$
     • Transformace sigma-bodů pomocí funkce pozorování, výpočet předpovědi pozorování a kovariančních matic, a aktualizace odhadu stavu. $$\begin{align} \hat{y}_t^{(i)} &= h_t\left(\hat{x}_t^{(i)}\right), \\ \hat{y}_t &= \frac{1}{2n} \sum_{i=1}^{2n} \hat{y}_t^{(i)}, \\ P_y &= \frac{1}{2n}\sum_{i=1}^{2n} \left( \hat{y}_t^{(i)} - \hat{y}_t \right) \left( \hat{y}_t^{(i)} - \hat{y}_t \right)^\intercal + R_t, \\ P_{xy} &= \frac{1}{2n}\sum_{i=1}^{2n} \left( \hat{x}_t^{(i)} - \hat{x}_t^- \right) \left( \hat{y}_t^{(i)} - \hat{y}_t \right)^\intercal, \\ K_t &= P_{xy}P_y^{-1}, \\ \hat{x}_t^+ &= \hat{x}_{t}^{-} + K_t\left[y_t - \hat{y}_t\right], \\ P_t^{+} &= P_t^- - K_t P_y K_t^\intercal. \end{align}$$