Ta strona używa ciasteczek (plików cookies), dzięki którym może działać lepiej. Dowiedz się więcejRozumiem i akceptuję

Filtr Kalmana od teorii do praktyki – #1 – Matlab

Programowanie Teoria 01.12.2014 GandalF

kalman2W 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.

Nawigacja serii artykułów:
następna część »

Do pełnego zrozumienia artykułu wskazana jest
podstawowa wiedza z teorii sterowania.

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).

Filtr Kalmana został wynaleziony w 1960 roku i od tego czasu znalazł zastosowanie w wielu gałęziach techniki takich jak loty w kosmos, robotyka, procesy chemiczne, a nawet meteorologia czy ekonomia.

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ępna część »

Powiadomienia o nowych, darmowych artykułach!

Komentarze

Bobby

11:55, 01.12.2014

#1

Jeśli ktoś ma problem w zrozumieniu dlaczego filtr Kalmana działa, polecam ten tekst: http://lirec.iiar.pwr.edu.pl/~konarch/download/algorytmy/kalman.pdf

Treker
Administrator

21:25, 01.12.2014

#2

Bobby, dzięki za materiał uzupełniający :)

Pozostałych zachęcam do komentowania czy artykuł jest dla Was ciekawy i zrozumiały. Jest to dość trudny tekst i chciałbym wiedzieć, czy warto zajmować się takimi tematami w przyszłości.

cyber31

13:19, 13.12.2014

#3

warto, warto dalej zgłębiać temat tym bardziej że robisz to w sposób merytoryczny

Treker
Administrator

13:22, 13.12.2014

#4

cyber31, akurat nie ja jestem autorem - tekst pisał GAndalF - tak jak jest podane przy tekście :) Kolejna część pojawi się w ciągu najbliższych 2 tygodni.

admunt1

10:03, 23.01.2015

#5

Polećcie coś do przeczytania gdzie znajdę odpowiedź jak dobrać macierze A, C, V i W - w zależności od ZMIENNYCH STANU ???. Czytałem w wielu publikacjach, że jest zapis macierzowy o określonym rozmiarze i zadanych wartościach ale brakuje mi informacji jak się je dobiera w zależności zmiennych stanu OBIEKTU który generuje oscylacje.

mog123

10:44, 23.01.2015

#6

A i C określa Ci twój system - jednym słowem musisz nauczyć się modelować systemy za pomocą state-space

a V i W to zakłocenia procesowe i pomiarowe. Zarówno jedne jak i drugie nie muszą zawsze być obecne. No i ich rozmiary również wynikają z tego gdzie co i jak się zakłóca.

admunt1

23:57, 29.01.2015

#7

Jak mogą wyglądać macierze A, C, V, W dla nieliniowego filtru Kalmana ?

Przykładowe równania, generujące zakłócenia przesyłam w załączniku poczty.

Pozdrawiam

Adam

maniu

14:53, 06.02.2015

#8

admunt1

admunt1 napisał/a:

Jak mogą wyglądać macierze A, C, V, W dla nieliniowego filtru Kalmana ?

Nie sprowadzisz modelu do postaci równania stanu, jeżeli model jest nieliniowy. Cały myk polega na tym, aby zlinearyzować go wokół jakiegoś punktu pracy.

GAndaLF

22:24, 06.02.2015

#9

Dla modelu nieliniowego stosujesz EKF. Macierz A tworzysz z pochodnych kolejnych równań stanu przez kolejne zmienne stanu, czyli dostajesz [df_1/dx_1 ... df_1/dx_n; ... df_n/dx_1 ... df_n/dx_n]. Macierz B tworzysz z pochodnych kolejnych równań stanu przez kolejne wejścia i dostajesz [df_1/du_1 ... df_1/du_n; ... df_n/du_1 ... df_n/du_n], a macierz C tworzysz z pochodnych równań wyjścia przez kolejne zmienne stanu, czyli [dg_1/dx_1 ... dg_1/dx_n; ... dg_n/dx_1 ... dg_n/dx_n]. Nowe wartości poszczególnych macierzy liczysz w każdej iteracji algorytmu i pod x_1 - x_6 podstawiasz aktualne estymacje zmiennych stanu.

admunt1

10:57, 23.02.2015

#10

Co zmieni się w programie jeśli zmienią się rozmiary macierzy ?

Przykładowo w kolejności wiersz/kolumna: A(6x6); C(3x6); P0(6x6); x0(6x1)

Mam na uwadze linijki kodu jak:

% aktualizacja czas

xpri = A*xpost;

Ppri = A*Ppost*A' + V;

% aktualizacja pomiarow

eps = Y(i) - C*xpri;

S = C*Ppri*C' + W;

K = Ppri*C'*S^(-1);

xpost = xpri + K*eps;

Ppost = Ppri - K*S*K';

Co trzeba zmienić w xpri, xpost, Ppri itp. ???

GAndaLF

10:09, 02.03.2015

#11

http://pl.wikipedia.org/wiki/Mno%C5%BCenie_macierzy

Musisz dobrać rozmiary poszczególnych macierzy tak, żeby ilość wierszy w pierwszej macierzy odpowiadała ilości kolumn w drugiej i vice versa. Jeżeli nie pasują ci rozmiary, użyj debuga i w miejscu gdzie program się wywala, wypisz na konsolę zawartość poszczególnych macierzy z mnożenia. W ten sposób sprawdzisz jak musisz pozmieniać rozmiary.

admunt1

15:18, 07.04.2015

#12

Z pomocą GAndaLFa wypróbowałem filtr Kalmana dla różnych układów generujących szum, oscylacje, zakłócenia o charakterze nieliniowym i nieokresowym.

Posłużyłem się następującym równaniem, generującym oscylacje:

Model przedstawia zmodyfikowane równanie Vander Pola, używane do opisu drgań lampy elektronowej.

Widzimy , że jest to układ bez sygnału wymuszającego tzn. B=0, wzbudzany niezerowymi warunkami początkowymi tzn. x(0), y(0), z(0) ≠0.

Wektor równania stanu zapiszemy jako:

Równanie stanu przyjmuje postać:

Macierz stanu A(6x6) układu dla nieliniowego filtru Kalmana składa się z pochodnych kolejnych równań stanu. Dla uproszczenia przyjęto zerowy punkt pracy tzn. x1(0)=x2(0)=x3(0)=x4(0)=x5(0)=x6(0)=0;

Macierz stanu A ma postać:

Macierz wymuszeń B(6x1), ze względu na brak sygnału wymuszającego przyjmuje zerowe wartości:

Wektor równania wyjść, ma postać:

Zatem równanie wyjść, opiszemy jak:

Macierz wyjścia C układu dla nieliniowego filtru Kalmana – równania składa się z pochodnych kolejnych równań wyjść:

Macierz przenoszenia przyjęto D=0

Przyjęto warunki początkowe o następujących wartościach:

Dla odchylenia standardowego 10, przyjęto doświadczalnie kolejno macierze kowariancji szumów i procesu:

Poniżej zamieszczam wynik estymacji nieliniowego filtra Kalmana – przebiegi czasowe x(t), y(t), z(t):

Imbryk

12:12, 09.04.2016

#13

Cześć, mam pytanie odnośnie przykładu 1. We wzorze aktualizacji czasu jest xpri = A*xpost; Dlaczego nie korzystamy tam z macierzy B*u zgodnie ze wzorem xpri = A*xpost + B*u?

Zobacz powyższe komentarze na forum

FORBOT Damian Szymański © 2006 - 2017 Zakaz kopiowania treści oraz grafik bez zgody autora. vPRsLH.