Filtr Kalmana od teorii do praktyki – #1 – Matlab

Filtr Kalmana od teorii do praktyki – #1 – Matlab

W niniejszej serii artykułów mam zamiar zająć się filtrami Kalmana. Zacznę od niezbędnej dawki teorii, by przejść do przykładów praktycznych.

Najpierw na podstawie rzeczywistych danych pomiarowych z czujnika zaprojektujemy filtr w Matlabie, a potem przeniesiemy implementację na mikrokontroler STM32.

Następny artykuł z serii »

Filtr Kalmana (KF) jest obserwatorem stanu minimalizującym średniokwadratowy błąd estymacji. Oznacza to, że algorytm na podstawie pomiarów wejścia i wyjścia obiektu szacuje jego wewnętrzny stan, a oszacowanie stanu jest statystycznie optymalne.

Oryginalnie filtr Kalmana jest przeznaczony do pracy z układami liniowymi, jednak po niewielkich modyfikacjach jest możliwa również praca z układami niestacjonarnymi (których model zmienia się w czasie) i nieliniowymi. KF dla układów nieliniowych nazywany jest Rozszerzonym Filtrem Kalmana (EKF - Extended Kalman Filter).

Założenia wstępne

Rozpatrywany proces można przedstawić za pomocą dyskretnoczasowego modelu w przestrzeni stanu:

x(t + 1) = Ax(t) + Bu(t) + v(t)

y(t) = Cx(t) + w(t)

x(t) oznacza stan w chwili czasu t = {0, 1, . . . }, y(t) wyjście układu, A macierz stanu, B macierz wejścia, C macierz wyjścia. Symbole v(t) i w(t) oznaczają szum procesowy (szum tworzący) i szum pomiarowy. Są to wzajemnie niezależne realizacje białego szumu gaussowskiego o zerowej wartości oczekiwanej i znanych macierzach kowariancji V i W. Odwzorowują one niedoskonałość przyjętego modelu oraz niedokładność aparatury pomiarowej.

v(t) \sim N(0,V)\\w(t) \sim N(0,W)

Stan początkowy x(0) jest realizacja gaussowskiej zmiennej losowej niezależnej od v(t) i w(t) o znanej wartości oczekiwanej x_{0} i macierzy kowariancji P_{0}:

x(0) \sim N(x_{0},P_{0})

Równania filtru Kalmana

Algorytm składa się z dwóch faz. Pierwsza nazywana jest aktualizacją czasu i polega na wyliczeniu jednokrokowej predykcji stanu czyli estymaty a priori \hat{x}(t + 1|t) i jej macierzy kowariancji P(t + 1|t):

kalman_01_01

Potem następuje zwiększenie czasu t \leftarrow t + 1 i algorytm przechodzi do drugiej fazy, czyli aktualizacji pomiarów:

kalman_01_02

Wartość \epsilon(t) jest różnicą pomiędzy najnowszym pomiarem y(t), a wartością spodziewaną na podstawie aktualnego oszacowania stanu. Wartość ta nazywana jest innowacją, ponieważ przynosi nowe dane o procesie. Macierz K(t) nazywana jest wzmocnieniem Kalmana. Decyduje ona, jaki wpływ na estymatę stanu a posteriori \hat{x}(t|t) ma nowy pomiar, a jaki estymata a priori \hat{x}(t|t-1).

Filtr Kalmana - przykład w Matlabie

To tyle jeśli chodzi o teorię. Aby pokazać działanie filtru w praktyce przygotowałem prosty przykład w Matlabie. Jest to pomiar zaszumionego napięcia.

Pomiary są wykonywane z odchyleniem standardowym 10. Przez pierwszą połowę symulacji wartość oczekiwana napięcia wynosi 0V. Następnie następuje skok wartości oczekiwanej do 100V. Dzięki temu będzie można zaobserwować jak filtr radzi sobie przy braku zmian wartości zadanej i przy skokowej zmianie o dużą wartość. Oto kod programu:

Mamy do czynienia z układem o jednej zmiennej stanu, dlatego macierze A, C, V i W upraszczają się do zwykłych liczb. Podobnie ma się sytuacja z macierzami P, K i S. Wariancję szumu pomiarowego ustawiłem jako kwadrat odchylenia standardowego czujnika.

Wariancję szumu procesowego, wartość początkową oszacowania stanu oraz jego wariancję wybiera się podczas strojenia filtru. Przeanalizujemy teraz wpływ zmian tych wartości na wyniki symulacji.

Pierwszy wykres przedstawia wyniki symulacji dla V=5*stddev*dt, P_0 = 1, x_0 = 0. Można zauważyć, że w początkowej fazie oszacowanie stanu waha się o około 5V od wartości zadanej. Po skokowej zmianie sygnału filtr potrzebuje około 1.5s aby nadążyć za zmianą.

p1_1

V=5*stddev*dt, P0 = 1, x_0 = 0

Drugi wykres przedstawia wyniki symulacji dla zwiększonej wartości wariancji szumu procesowego. Tym razem odpowiedź na skokową zmianę jest dużo szybsza, ale dokładność filtracji w stanie ustalonym jest zdecydowanie mniejsza.

V=50*stddev*dt, P_0 = 1, x_0 = 0

V=50*stddev*dt, P_0 = 1, x_0 = 0

Skutki zmniejszenia wartości wariancji szumu procesowego przedstawiono na poniższym wykresie. Tym razem wahania sygnału w stanie ustalonym są bardzo niewielkie kosztem zwiększonego czasu ustalania po skokowej zmianie.

V=1*stddev*dt, P_0 = 1, x_0 = 0

V=1*stddev*dt, P_0 = 1, x_0 = 0

Kolejny wykres przedstawia wpływ dużej wartości początkowej wariancji oszacowania stanu. W początkowej fazie symulacji filtr dużo bardziej wierzy pomiarom niż aktualnemu oszacowaniu stanu, dlatego zmiana wartości jest gwałtowna mimo małej wariancji szumu procesowego.

Po kilku pierwszych iteracjach algorytmu wariancja oszacowania stanu maleje i sygnał znów wykazuje niewielkie zmiany:

V=1*stddev*dt, P_0 = 10000, x_0 = 30

V=1*stddev*dt, P_0 = 10000, x_0 = 30

Ostatni wykres przedstawia symulację dla niskiej początkowej wartości oszacowania stanu. Tym razem filtr bardziej bazuje na aktualnym oszacowaniu stanu niż na pomiarach, dlatego zmiana sygnału nie jest tak gwałtowna mimo dużej wariancji szumu procesowego.

Podobnie jak w poprzednim przypadku zmiana wartości początkowych ma znaczenie tylko w pierwszych kilku iteracjach filtru.

V=50*stddev*dt, P_0 = 0.00001, x_0 = 30

V=50*stddev*dt, P_0 = 0.00001, x_0 = 30

Podsumowanie

W tej części artykułu zaprezentowałem podstawy teoretyczne filtru Kalmana oraz omówiłem jego strojenie. W kolejnej części zajmę się projektowaniem filtru w Matlabie dla danych pomiarowych zebranych z rzeczywistych czujników - akcelerometru i żyroskopu.

Następny artykuł z serii »

akcelerometr, czujniki, filtr, kalman, matlab, żyroskop