Poprzednio wyprowadziłem model stanowy robota i przygotowałem symulację do testowania algorytmów określania pozycji. Przyszła więc kolej na ich implementację. W tym wpisie skupię się na Rozszerzonym Filtrze Kalmana. Znajdzie się miejsce na wstęp teoretyczny oraz analizę implementacji w środowisku symulacyjnym. Starałem się wszystko opisać jak najprościej. Jednak aby wszystko dobrze zrozumieć potrzebna będzie zaawansowana wiedza z matematyki. Dobrze również wiedzieć coś o teorii sterowania. Dla mnie była to okazja do przypomnienia sobie co nieco ze studiów i wyprowadzenia kilku wzorów na kartce.

Ograniczenia zwykłego filtru Kalmana

Filtr Kalmana(KF) wyznacza optymalne oszacowanie stanu dla układów liniowych i niezmiennych w czasie (czyli stacjonarnych) na podstawie pomiarów zakłóconych białym szumem. Otrzymana w ten sposób estymata stanu minimalizuje błąd średniokwadratowy, czyli jest optymalna statystycznie. Po więcej informacji na temat zwykłego Filtru Kalmana odsyłam do serii artykułów, jakie napisałem kiedyś na portalu Forbot.

W większości rzeczywistych zastosowań nie możemy zapewnić liniowości i stacjonarności układu, a zakłócenia nie zawsze są białym szumem. Powstały więc sposoby radzenia sobie z tymi niedogodnościami. Gdy mamy do czynienia z układem nieliniowym możemy posłużyć się Rozszerzonym Filtrem Kalmana (EKF – Extended Kalman Filter). W przeciwieństwie do zwykłego KF nie gwarantuje on optymalności, ale jest dobrą praktyczną metodą na uwzględnienie w oszacowaniu stanu wiedzy o procesie i o zakłóceniach.

Model nieliniowy

Układu nieliniowego nie opiszemy za pomocą standardowych równań stanu wykorzystujących macierze A, B, C, D. Zamiast tego musimy posłużyć się zestawem nieliniowych funkcji. Równania dla układu nieliniowego mają postać:

$\mathbf{x}(t+1) = \mathbf{f}[\mathbf{x}(t), \mathbf{u}(t)] + \mathbf{v}(t)$

$\mathbf{y}(t) = \mathbf{h}[\mathbf{x}(t), \mathbf{u}(t)] + \mathbf{w}(t)$

gdzie x jest wektorem stanu, u wektorem wejść, a y wyjść. Wektory v i w to szum procesowy i szum pomiarowy. Symbole f i h oznaczają zestawy nieliniowych funkcji opisujących zależność od stanu i od wejścia. Muszą one być różniczkowalne po wszystkich parametrach. Ta właściwość będzie nam potrzebna podczas linearyzacji.

Linearyzacja

Linearyzacja modelu to proces wyznaczenia liniowego układu opisanego macierzami A, B, C, D przybliżającego zachowanie badanego układu nieliniowego w otoczeniu zadanego punktu pracy ( $x_*, u_*$ ). Oznacza to, że im bardziej stan różni się od punktu pracy, tym bardziej wyniki dawane przez zlinearyzowany model różnią się od rzeczywistych.

Wykorzystywana w EKF metoda linearyzacji opiera się na rozwinięciu funkcji w szereg Taylora. Nie będę tutaj wyprowadzał wzorów. Pokażę jedynie kroki wymagane do wykonania linearyzacji. W tym celu należy wyznaczyć pochodne cząstkowe funkcji z modelu nieliniowego:

$\mathbf{A} = \frac{\delta \mathbf{f}}{\delta \mathbf{x}} \big \rvert_{x = x_*, u = u_*}$

$\mathbf{B} = \frac{\delta \mathbf{f}}{\delta \mathbf{u}} \big \rvert_{x = x_*, u = u_*}$

$\mathbf{C} = \frac{\delta \mathbf{h}}{\delta \mathbf{x}} \big \rvert_{x = x_*, u = u_*}$

$\mathbf{D} = \frac{\delta \mathbf{h}}{\delta \mathbf{u}} \big \rvert_{x = x_*, u = u_*}$

Czyli poszczególne pola macierzy to pochodne funkcje po kolejnych parametrach. W tak otrzymanej pochodnej podstawiamy pod wszystkie wystąpienia x i u wartości dla punktu pracy. Jeżeli zachodzi potrzeba wyznaczenia jakiś parametrów, wartości pochodnych przyrównujemy do zera i rozwiązujemy układ równań. Brzmi dosyć skomplikowanie, w dalszej części pokażę obliczenia na przykładzie modelu robota.

Rozszerzony Filtr Kalmana

Równania EKF opierają się na tych z klasycznego Filtru Kalmana (można je znaleźć w podlinkowanym wyżej artykule), jednak zawierają kilka modyfikacji:

$\hat{\mathbf{x}}(t+1|t) = f[\hat(\mathbf{x})(t|t), \mathbf{u}(t)]$

$\mathbf{P}(t+1|t) = \mathbf{A}(t|t) \mathbf{P}(t|t) \mathbf{A}^T(t|t) + \mathbf{V}$

W tym momencie następuje aktualizacja czasu $t \rightarrow t + 1$

$\mathbf{\varepsilon}(t) = \mathbf{y}(t) - \mathbf{h}[\hat{\mathbf{x}}(t|t-1), \mathbf{u}(t)]$

$\mathbf{S}(t) = \mathbf{C}(t|t-1) \mathbf{P}(t|t-1) \mathbf{C} (t|t-1) + \mathbf{W}$

$\mathbf{K}(t) = \mathbf{P}(t|t-1) \mathbf{C}^T(t|t-1) \mathbf{S}^{-1}(t)$

$\hat{\mathbf{x}}(t|t) = \hat{\mathbf{x}}(t|t-1) + \mathbf{K}(t) \varepsilon(t)$

$\mathbf{P}(t|t) = \mathbf{P}(t|t-1) - \mathbf{K}(t) \mathbf{S}(t) \mathbf{K}^T(t)$

W równaniach 1 i 3 wykorzystane zostały funkcje f i h z modelu w przestrzeni stanu. Zamiast stałych macierzy A i C zastosowano ich zlinearyzowane odpowiedniki. Macierz A w punkcie pracy $(\hat{\mathbf[x]}(t|t), u(t))$ – czyli z użyciem estymaty a posteriori, a macierz C w $(\hat{\mathbf[x]}(t|t-1), u(t))$ z użyciem estymaty a priori.

Implementacja KF na liczbach zmiennoprzecinkowych i związane z tym niedokładności mogą negatywnie wpłynąć na stabilność numeryczną. Problematyczne jest tutaj ostatnie równanie, w którym występuje odejmowanie macierzy. Może ono spowodować, że macierz P przestanie być dodatnio określona. Prostym mechanizmem obronnym jest tzw. Formuła Josepha:

$\mathbf{P}(t|t) =[\mathbf{I} - \mathbf{K}(t)\mathbf{C}(t|t-1)] \mathbf{P}(t|t-1)[\mathbf{I} - \mathbf{K}(t) \mathbf{C}(t|t-1)]^T + \mathbf{K}(t) \mathbf{W} \mathbf{K}^T(t)$

Polepsza ona stabilność numeryczną, ale nie gwarantuje pełnej odporności na tego typu błędy. Aby uzyskać pełne zabezpieczenie należy wykorzystać Pierwiastkowy Filtr Kalmana (Square Root Kalman Filter).

Linearyzacja w praktyce

Równania ruchu robota wyprowadziłem w poprzednim wpisie. Teraz zajmiemy się ich linearyzacją wokół punktu pracy z zadanymi wartościami $x, y, \alpha$ i nieznanymi $v, \omega$ .

Na początek musimy obliczyć pochodne cząstkowe. Dla pierwszego równania niezerowe będą pochodne $x_1, x_3$ oraz $x_4$ :

$\frac{\delta f_1}{\delta x_1} = 1$

$\frac{\delta f_1}{\delta x_3} = - dt v sin(\alpha)$

$\frac{\delta f_1}{\delta x_4} = dt cos(\alpha)$

Analogicznie dla drugiego:

$\frac{\delta f_2}{\delta x_2} = 1$

$\frac{\delta f_2}{\delta x_3} = dt v cos(\alpha)$

$\frac{\delta f_2}{\delta x_4} = dt sin(\alpha)$

Kiedy przyrównamy środkowe równania do 0, wyjdzie nam, że v również jest równe 0. Dla pozostałych równań wyznaczanie pochodnych jest już proste. Po podstawieniu pod v zera otrzymujemy zlinearyzowaną macierz A:

$A = \begin{bmatrix} 1 & 0 & 0 & dt cos(x3) & 0 \\ 0 & 1 & 0 & dt sin(x3) & 0 \\ 0 & 0 & 1 & 0 & dt \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix}$

Implementacja EKF

Kod symulacji w MATLABie można znaleźć na moim GitHubie. Symuluję w niej ruch robota z zadanymi profilami prędkości postępowej i obrotowej, odczyty z enkoderów i żyroskopu oraz implementuję EKF. Wyniki są prezentowane na wykresach. Estymata EKF poszczególnych zmiennych stanu w czasie wygląda następująco (niebieska linia to rzeczywista wartość, czerwona to EKF):

Oszacowanie pozycji w przestrzeni 2D:

Od razu nasuwa się kilka obserwacji. Po pierwsze oszacowanie pozycji stopniowo coraz bardziej odbiega od rzeczywistości. Jest to jak najbardziej zrozumiałe zachowanie i takim kumulującym się błędem charakteryzują się wszystkie pomiary oparte na odometrii (więcej na wikipedii pod hasłami Odometry i Dead reckoning). Analizując ten problem od strony teorii sterowania, bez problemu zauważymy, że badany układ nie jest obserwowalny. Oznacza to, że dane pomiarowe nie zapewniają możliwości poznania pełnego stanu obiektu. W naszym przypadku mamy pomiary prędkości, ale czujniki nie dają nam żadnych informacji o pozycji. Sytuacja się zmieni, kiedy będę uwzględniał informacje o ścianach labiryntu.

Kolejną obserwacją jest fakt, że największy wpływ na niedokładność oszacowania ma obrót. Wynika to z faktu, że to $\alpha$ jest odpowiedzialny za nieliniowość funkcji dla x i y – wprowadza tam funkcje trygonometryczne. Jak wspomniałem wcześniej – EKF dokonuje linearyzacji dla punktu pracy wyznaczonego przez aktualną estymatę stanu. Im mocniej ta estymata odbiega od rzeczywistości, tym większy błąd.

Zachowaniem EKF można manipulować modyfikując parametry takie jak warunki początowe $\mathbf{x}_0, \mathbf{P}_0$ oraz macierzami kowariancji szumu procesowego i pomiarowego – $\mathbf{V}, \mathbf{W}$ . Możemy w ten sposób sugerować, czy filtr ma bardziej polegać na danych z czujników, czy wewnętrznym obliczeniom stanu.

Dalsze plany

Implementacja EKF w środowisku symulacyjnym spełniła swoje założenia. Miałem okazję zobaczyć wpływ nieliniowości i nieobserwowalności układu na błędy estymacji. Mam referencyjną implementację, którą mogę teraz przenieść na mikrokontroler. W najbliższym czasie będę chciał sprawdzić EKF w środowisku symulacyjnym na zestawie rzeczywistych danych pobranych z robota. Poza tym chcę wykonać docelową implementację na robocie.

Kolejnym punktem programu jest zamontowanie i kalibracja czujników ścian. Będę musiał również pomyśleć nad sposobem integracji danych z czujników ścian z EKF. Nie wiem jeszcze, czy będę chciał podawać informacje z każdego czujnika oddzielnie, czy obrabiać je wstępnie i przekazywać dalej jedynie oszacowanie pozycji. Poza tym problemem może być fakt, że nie zawsze ściana zostanie wykryta. Może też różnić się czas próbkowania tych danych.

Opcjonalnym zadaniem będzie sprawdzenie w praktyce algorytmów innych niż EKF. Mam tu głównie na myśli UKF (Unscented Kalman Filter), który powinien dawać lepsze oszacowania dla układów silnie nieliniowych przy porównywalnej złożoności obliczeniowej. Może poeksperymentuję również z pierwiastkowymi wersjami EKF lub UKF.

Szukaj

EKF – Rozszerzony filtr Kalmana

Ograniczenia zwykłego filtru Kalmana

Model nieliniowy

Linearyzacja

Rozszerzony Filtr Kalmana

Linearyzacja w praktyce

Implementacja EKF

Dalsze plany

Podobne

1 Comment

4 Pingbacks

Dodaj komentarz Anuluj pisanie odpowiedzi

EKF – Rozszerzony filtr Kalmana

Ograniczenia zwykłego filtru Kalmana

Model nieliniowy

Linearyzacja

Rozszerzony Filtr Kalmana

Linearyzacja w praktyce

Implementacja EKF

Dalsze plany

Podziel się:

Podobne

1 Comment

4 Pingbacks

Dodaj komentarz Anuluj pisanie odpowiedzi