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 w praktyce – 3 przykłady z kodami!

Teoria 25.06.2015 Elektryk0

filtrKalmanaW artykule, postaram się opisać w prosty sposób filtr Kalmana. Zostanie tutaj podana minimalna dawka teorii, którą musimy dysponować aby zrozumieć jak działa filtr oraz jak go stosować.

Mam nadzieję, że udało mi się przedstawić to zagadnienie w sposób łatwy do zrozumienia.

Napiszemy także kilka programów, które pokażą na przykładach zastosowanie filtru Kalmana w różnych sytuacjach życiowych. Programy będą napisane w Octave oraz w C++ z użyciem biblioteki OpenCV. Algorytm zgodnie z którym działa FK jest prosty w implementacji, dodatkowo nie wymaga złożonych obliczeń, a więc analiza zamieszczonych kodów nie powinna sprawić trudności.

Najważniejszą rzeczą przy implementacji FK jest opisanie poprawnymi równaniami naszego procesu i systemu pomiarowego. Postaram się na prostym przykładzie pokazać jak robi się to krok po kroku, dlatego na początek zaczniemy od przyswojenia niezbędnej teorii.

Filtr Kalmana – teoria

Aby możliwe było zastosowanie filtru Kalmana, nasz proces powinien dać się opisać za pomocą następujących równań linowych:

fk_1

Pierwsze zwane jest równaniem stanu lub modelem procesu, natomiast drugie to model pomiaru. W powyższych równaniach A, B, H są macierzami, wektor x nazywany jest stanem systemu, wektor u zawiera informacje wejściowe dla systemu, np. zadaną z góry prędkość poruszania się robota lub jego przyspieszenie itp., wektor z to mierzone wyjście systemu.

Natomiast w oraz v oznaczają szum (odchylenia standardowe). Przy czym w to szum procesu, natomiast v jest szumem pomiaru. Wszystkie informacje o stanie systemu w danej chwili k zawiera wektor x, jednak nie możemy tych informacji wydobyć bezpośrednio z x, możemy je uzyskać poprzez wektor z dokonując pomiaru, co wprowadza nam szum pomiarowy reprezentowany wektorem v.

Dlatego dane otrzymane z wektora z nie są godne zaufania, możemy z nich skorzystać w procesie wyznaczania bardziej wiarygodnych danych, ale branie ich w ciemno nie jest najlepszym rozwiązaniem.

Aby zrozumieć to lepiej wyobraźmy sobie łódkę płynącą po rzece, widzianą z lotu ptaka. Możemy przyjąć, że łódka nie porusza się po rzece na boki tylko płynie wzdłuż niej, równolegle do brzegu, a więc ruch zachodzi w jednym wymiarze, wzdłuż jednej osi.

Przypuśćmy, że chcemy estymować położenie łódki na wodzie, mając pomiar położenia łódki z GPS. Łódka porusza się w dodatku z pewnym znanym przyspieszeniem, będzie to nasze u. Z fizyki wiadomo, że prędkość łódki w chwili k wyrażą się równaniem:

fk_2

Gdzie u jest przyspieszeniem. Jednakowoż w rzeczywistym świecie, łódka płynąca po rzece napotka pewne opory, będą one związane ze zmieniającymi się prędkością i kierunkiem wiatru, siłą prądu w rzece, kolizjami z obiektami unoszącymi się na wodzie itp.

W wyniku tego rzeczywista prędkość będzie nieco się różnić od tej wyliczonej za pomocą powyższego równania, możemy powiedzieć, ze prędkość będzie zaszumiona. Lepszym pomysłem jest więc napisanie równania w postaci:

fk_3

Gdzie dodatkowy parametr oznacza szumy. Podobnie możemy zapisać równanie na położenie:

fk_4Gdzie ostatni parametr oznacza szum położenia. Natomiast wektor stanu przyjmie więc postać:

fk_5

A nasze równania systemu przyjmują postać:

fk_6

Otrzymaliśmy więc równania dla naszego problemu, jednowymiarowego ruchu łódki na wodzie z pomiarem jej pozycji z GPS. Z równań wynika że nasze macierze A ,B, H są następujące:

fk_7

Dlaczego macierz H jest taka? Otóż macierz ta określa przejście ze stanu do pomiaru. Z racji, że wielkością którą chcemy mierzyć w naszym przykładzie jest, położeniu łódki konstruujemy macierz H w ten sposób żeby w równaniu modelu pomiaru uzyskać na koniec pomiar tejże wielkości. Po pomnożeniu macierzy H oraz wektora stanu xk otrzymamy (położenie):

fk_8

Jak widzimy nasz system nie jest pozbawiony szumów, a żeby móc poprawnie go wyregulować trzeba się ich pozbyć. W tym momencie przychodzi nam z pomocą Filtr Kalmana.

Algorytm, zgodnie z którym działa Filtr Kalmana, dzielimy na dwie fazy (etapy). Pierwszy etap zwany jest predykcją, a drugi korekcją. Podczas predykcji bazując na poprzedniej wartości stanu x wyznaczana jest nowa wartość stanu x oraz jej kowariancja.

Wartości te wyznaczane są bez korzystania z informacji ze świata zewnętrznego, tzn. w przypadku naszej łódki bez odniesienia do pomiaru z czujnika GPS. Są one więc niejako przewidywane na podstawie równań stanu x za pomocą których opisaliśmy nasz system. W przypadku naszej łódki były by tu wykorzystane równania fizyczne opisujące jej położenie i prędkość w zależności od czasu.

Ten etap to niejako czysta matematyka, poparta wiedzą o wcześniejszym stanie procesu, równania które realizuje są następujące:

fk_9Gdzie:

fk_10oznacza kowariancją szumu procesu. Póxniej wytłumaczę w jaki sposób się ją wyznacza. Na razie możemy powiedzieć, że wnosi informacje na temat szumów z naszego procesu. Jeśli ktoś nie wie co oznacza symbol T to należy szukać pod hasłem tranpozycja macierzy.

Gdybyśmy zostawili algorytm w tym momencie i pozwolili mu działać, dostali byśmy w wyniku taki sam przebieg jaki mają funkcje fizyczne opisujące prędkość oraz położenie naszego robota.

Jednak my dysponując pomiarami z czujników możemy uzyskać dane bliższe rzeczywistości niż zgadywanie z zamkniętymi oczami na podstawie matematyki, ale bez informacji ze świata zewnętrznego. W tym momencie przychodzi nam z pomocą faza korekcji.

W fazie korekcji wyznaczamy pewną wielkość K, zwaną dalej wzmocnieniem Kalmana.

fk_11Gdzie:

fk_12

oznacza kowariancją szumu pomiaru. Dalej wytłumaczę w jaki sposób się ją wyznacza.

Prześledźmy teraz fazę korekcji. Na początek wyznaczana jest wartość wzmocnienia Kalmana K. Jeżeli zwrócimy uwagę na sposób w jaki jest wyznaczana, dojdziemy do wniosku, że im większe szumy pomiarowe, które są tu reprezentowane przez kowariancję R tym mniejsza wartość K, gdyż R znajduje się w mianowniku.

W następnym równaniu uaktualniany jest wektor stanu x, jego wartość jest korygowana przy użyciu pomiarów z czujników (z). Duże znaczenie przy korekcji ma wzmocnienie Kalmana K.

Przypominam raz jeszcze, że będzie ono tym mniejsze im większe sumy pomiarowe, w więc w przypadku dużych szumów, nowa wartość x nie będzie się wiele różniła od tej wyliczonej w fazie predykcji.

Wniosek z tego jest następujący, za pomocą K określamy jak bardzo można polegać na danych z czujników, jeżeli szumy są duże, a więc K małe, to informacje z pomiarów będą miały niewielki wpływ na ostateczną wartość x, jeżeli natomiast szumy w układzie są małe, a więc K duże, oznacza to, że dane z pomiarów są o wiele bardziej wiarygodne i będą miały znaczny wpływa na ostateczną wartość x.

Na tym polega właśnie nasza faza korekcji. Na koniec należy jeszcze skorygować macierz kowariancji P, co realizuje ostatnie z równań fazy korekcji.

Więc algorytm składa się z 5 prostych równań, które właściwie można zapisać w formie trzech. Po wykonaniu fazy korekcji wracamy z powrotem do fazy predykcji i tak w kółko w pętli. Należy jeszcze pamiętać o wstępnej inicjalizacji wartości dla zmiennych z fazy korekcji, gdyż przy pierwszym przebiegu wykonywana jest najpierw faza predykcji, która wykorzystuje zmienne z fazy korekcji.

Nie są one jeszcze wtedy znane, należy więc ustawić je na przypadkowe lub przemyślane wartości początkowe, często po prostu się je zeruje ale o tym później przy omawianiu przykładowych programów.

Przykład pierwszy – jednowymiarowy ruch łódki

Rozważmy więc jeszcze raz nasz przykład z łódką i policzmy dla niej tajemnicze kowariancję R i Q. Równania opisujące ten proces były następujące:

fk_6

Załóżmy więc, że nasza łódka porusza się ze stałym przyspieszeniem 1 [m/(s*s)] i występują szumy przyspieszenia o wartości 0.1 [m/(s*s)]. Pozycję naszej łódki mierzymy co 0.1 [s]. Dodatkowo wiemy, że niedokładność pomiaru GPS wynosi 0.5 [m]. Skoro pozycja łódki jest mierzona z czasem T = 0.1 możemy zapisać nasze równanie w postaci:

fk_13

Dodatkowo wiemy, że odchylenie standardowe pomiaru z GPS wynosi 0.5 m, co daje nam wartość kowariancji R równą:

fk_14

Należy także policzyć kowariancję Q. Wiemy, że na nasz wektor stanu x składają się prędkość i położenie łódki.

fk_5

Powinniśmy więc policzyć ile wynoszą szumy. W tym celu spójrzmy na równianie stanu:

fk_15

Wynika z niego, że położenie jest proporcjonalne do 0.005 razy przyspieszenie u. Natomiast przyspieszenie u wnosi nam szum 0.1 [m/(s*s)]. Odchylenie standardowe będzie więc wynosić:

s = (0.1 * 0.005) = 0.0005

Prędkość jest proporcjonalne do 0.1 razy przyspieszenie u. Daje to wartość odchylenia standardowego:

v = 0.1 * 0.1 = 0.01

Ostatecznie wartość Q można wyliczyć jako:

fk_16

Choć obliczanie wartość dla kowariancji Q i R może wydać się żmudne, my amatorzy w wielu przypadkach możemy je pominąć i wyznaczyć je metodami na oko oraz prób i błędów.

W ten sposób mamy wszystko co trzeba, aby napisać filtr Kalmana dla ruchu łódki w jednym wymiarze, z pomiarem pozycji GPS. Program, który to zilustruje napiszemy w GNU Octave. Będzie on realizował co następuje:

  • symulację jednowymiarowego ruchu
  • odczyt danych z czujników
  • usuwanie szumów
  • estymację położenia filtr’em Kalmana.

Wcześniej jeszcze podam kilka ważnych informacji na temat Octave, które przydadzą się szczególnie tym, którzy do czynienia z tym pakietem do tej pory nie mieli.

Czym jest Octave?

Otóż zgodnie z tym co mówi Wikipedia:

GNU Octave – środowisko obliczeń oraz język programowania przeznaczony dla prowadzania obliczeń numerycznych. Octave dostępny jest na większości systemów uniksowych. Rozprowadzany jest na zasadach licencji GNU GPL. Octave jest wolnym odpowiednikiem środowiska MATLAB.

Programy, które tu zamieszczam będą działać także w Matlabie.

Skąd wziąć Octave?

Najprostszym sposobem jest skorzystanie ze środowiska w wersji online. Nie będziemy musieli pobierać plików, ani męczyć się z instalacją, a w mgnieniu oka uruchomimy zamieszczone skrypty.

Jeżeli wybierzemy ten sposób wystarczy wejść na stronę Verbosus i dokonać procesu rejestracji. Wszystko jest całkowicie darmowe, a rejestracja sprowadza się do podania adresu e-mail i hasła.

Po zalogowaniu się, naszym oczom ukaże się następujący widok:

fk_17

Środowisko w wersji online.

W tym momencie należy stworzyć nowy plik .m wpisując jego nazwę w pole „.m Files” znajdujące się po lewej stronie. Plik powinien nazywać się „kalman_boat.m” następnie wklejamy do niego kod programu, który znajduje się poniżej i zapisujemy go przyciskiem „Save”:

Należy jeszcze utworzyć plik .m z poziomu, którego wywołamy utworzony skrypt. W tym celu tworzymy plik „main.m”. Przechodzimy do niego klikając go dwukrotnie i wpisujemy kod wywołujący wcześniejszy skrypt.

Zapisujemy i wciskamy „Generate”. Poniżej efekt działania programu:

fk_18

Efekt działania programu.

Kod skryptu kalman_boat.m jest prosty do zrozumienia. Należy jeszcze raz spojrzeć na równania naszego procesu aby zobaczyć, że macierze A, B, H są z nich wprost przepisane. Kod oblicza także automatycznie wartości dla macierzy R oraz Q zgodnie ze schematem przedstawionym powyżej. Potem następuje początkowa inicjalizacja zmiennych.

Następnie symulujemy środowisko tak by występowały w nim szumy i na koniec próbujemy eliminować te szumy filtrem Kalmana. Program wyświetla wykres pozycji łódki z trzema przebiegami, rzeczywistym, mierzonym GPS’em oraz estymowanym filtrem kalmana, dzięki czemu możemy zobaczyć o ile wiarygodniejsze dane uzyskujemy stosując filtr.

Srypt wywołujemy podając pięć parametrów:

Poniżej zamieszczam wykres wygenerowany przez program:

Filtr Kalmana w praktyce.

Filtr Kalmana w praktyce.

Przykład drugi – filtracja zaszumionego napięcia

Kolejny przykład prezentuje pomiar zaszumionego napięcia z wykorzystaniem filtru kalmana. Zakładamy że napięcie ma stałą wartość. Wówczas równania opisujące nasz system znacznie się uproszczą.

fk_20

Z racji że napięcie jest stałe, macierz przejścia z stanu poprzedniego do następnego A = 1. Nie mamy żadnej wielkości sterującej (wejściowej) zatem u = 0. Macierz H natomiast wynosi jeden, gdyż ma tylko jedną wielkość mierzoną i estymowaną, którą jest napięcie. Zatem nasze równania upraszczają się do postaci:

fk_21

Faza predykcji jest więc następująca:

fk_22

Natomiast faza korekcji:

fk_23

Wartości Q i R są przyjmowane na oko. Kod programu:

Należy utworzyć skrypt o nazwie „kalman_dc.m”. Poniżej efekt działania programu wywołanego w main.m z parametrami:

Efekt działania powyższego programu.

Efekt działania powyższego programu.

Przykład trzeci – śledzenie obiektu w przestrzeni 2D

Na koniec rozważymy jeszcze problem śledzenia obiektu w przestrzeni dwuwymiarowej. Problem ten rozważymy najpierw teoretycznie, a potem napiszemy stosowny program w języku C++ przy użyciu biblioteki OpenCV.

Jako czujniki wykorzystamy informacje o położeniu kursora myszy na ekranie.

Zacznijmy więc jeszcze raz od zapisania równań:

fk_25

Możemy wyobrazić sobie, że sytuacja jest podobna jak podczas przykładu z łódką, zamiast jednego wymiaru mamy 2, a zamiast GPS położenie kursora myszy. Możemy napisać więc 4 proste równania opisujące nasz proces:

fk_26

Co można też wyrazić jako:

fk_27

Z czego wynika że macierz A będzie mieć postać:

fk_28

Natomiast nasza macierz H powinna zapewniać pomiar z dwóch czujników: położenia x oraz y, a więc powinna wyglądać następująco:

fk_29

W tym przypadku nie mamy sterowania (macierz B), a więc znika nam z równań ta część. Powinniśmy się jeszcze zastanowić nad macierzami R i Q. Można ich wartości przyjąć „na oko”. Macierze te mogę mieć wszystkie elementy równe 0 prócz elementów znajdujących się na głównej przekątnej. Wartości Q zazwyczaj przyjmuje się bardzo małe, R nieco większe, przykładowo, możemy je ustalić następująco:

fk_30

Macierz R może przyjąć następujące wartości:

fk_31

Kod programu:

Jest dobrze skomentowany, myślę że nie powinno być większych problemów z jego zrozumieniem, tym bardziej, że w OpenCV algorytm jest niemal realizowanym automatycznie, a główne zadanie sprowadza się do opisania procesu odpowiednimi równaniami. Usuwając znaki komentarza w liniach typu jak poniżej, można zobaczyć jak wyglądają poszczególne macierze.

Efekt działania programu:

Działanie filtru Kalmana w praktyce.

Działanie filtru Kalmana w praktyce.

Filtr nie jest idealny, szczególnie widać to przy szybkich ruchach myszką, myślę jednak że do celów szkoleniowych jest wystarczający, szybkość reakcji można zwiększyć dodając np. do naszego równania stanu przyspieszenie jako kolejny parametr.

Podsumowanie

Mam nadzieję, że artykuł rozjaśnił nieco zawiłości filtru Kalmana. Jeśli są jakieś wątpliwości, coś nie jest wystarczająco dokładnie wytłumaczone proszę o informacje. Postaram się też w najbliższym czasie dorzucić kody w c++ do dwóch pierwszych przykładów.

Autor: Elektryk0
Data publikacji na forum: 01-10-2012

Powiadomienia o nowych, darmowych artykułach!

Komentarze

Elvis

17:30, 01.10.2012

#1

Artykuł bardzo dobry, właśnie przeliczam wszystko "na piechotę" - inaczej ciężko to wszystko zrozumieć.

Na razie mam uwagę do:

Chyba pomylone są indeksy w drugim wierszu - wcześniej z(k) obliczane jest z aktualnego stanu, a tutaj z poprzedniego.

Elektryk0
Autor wpisu

20:59, 01.10.2012

#2

Osobiście spotkałem się z obiema wersjami :)

W każdym razie dzięki za zwrócenie uwagi. A błędne fragmenty poprawiłem tak by trzymać się jednej wersji i nie robić zamieszanie. W najbliższym czasie postaram się jeszcze raz wszystko prześledzić i jak by co wprowadzę zmiany. Jeśli znajdziesz coś jeszcze to daj znać.

Marooned

23:24, 01.10.2012

#3

Chętnie bym poznał jeszcze łopatologicznie wytłumaczoną różnicę między opisaną tu wersją podstawową a jego rozszerzoną wersją, która jest stosowana w mocniejszych kontrolerach lotu dla kopterów i samolotów.

GAndaLF

1:41, 02.10.2012

#4

EKF to rodzaj filtru Kalmana dla układów nieliniowych, gdzie wykonywana jest linearyzacja wokół pewnego punktu pracy. Dlatego EKF dobrze działa tylko w pewnym zakresie w pobliżu tego punktu. A w kopterach jest wykorzystywany dlatego, że ruch w układzie kartezjańskim jest opisywany za pomocą funkcji trygonometrycznych (rzuty na osie x y z), które oczywiście są nieliniowe.

Marooned

1:35, 03.10.2012

#5

GAndaLF napisał/a:

A w kopterach jest wykorzystywany dlatego, że ruch w układzie kartezjańskim jest opisywany za pomocą funkcji trygonometrycznych (rzuty na osie x y z), które oczywiście są nieliniowe.

Zapewne wykażę się ignorancją, ale geometrię miałem ostatnio z 15 lat temu. Możesz pokrótce wyjaśnić dlaczego rzuty na osie są nieliniowe? Czy miałeś na myśli np. obrót, który po rzuceniu na oś będzie sinusem? Jeśli tak, to wszystko jasne.

Wiem, że EKF wykorzystywany jest głównie do filtrowania sygnałów z GPSa. I na tym chwilowo kończy się moja wiedza w tym zakresie.

GAndaLF

23:58, 25.10.2012

#6

Chodzi o to, że obiekty mobilne mogą się poruszać w dowolnym kierunku, który prawie nigdy nie jest równoległy do którejś z osi układu odniesienia. Podczas modelowania przypisujesz takiemu ruchowi jakiś wektor prędkości, przyspieszenia, siły itp a następnie musisz go rozbić na składowe x, y, z, gdzie dwie składowe i wektor początkowy tworzą trójkąt prostokątny. Dzięki temu możesz obliczyć niezależnie zmianę w każdym kierunku. Na pewno na fizyce miałeś kiedyś zadania z równią pochyłą. Tam się w ten sposób rozkładało siłę grawitacji.

Marooned

8:25, 26.10.2012

#7

To jasne, że poruszanie się w przestrzeni 3D rzadko odbywa się po osiach XYZ, ale rzutowanie ruchu na te osie nadal jest liniowe tylko zeskalowane o cosinus kąta między osią rzutowania a osią ruchu. Ale jest to skala stała (przy ruchu prostoliniowym oczywiście, no ale taki rozważamy w krótkich odstępach czasu).

GAndaLF

15:53, 28.10.2012

#8

Podstawianie pod funkcje trygonometryczne konkretnych wartości odpowiadających aktualnym kątom to właśnie linearyzacja modelu dla zadanego punktu pracy. Czyli jeden z dodatkowych kroków który wykonujesz w EKFie. Przy czym tej linearyzacji nie możesz dokonać raz na początku symulacji, musisz uaktualniać równania stanu w każdej iteracji, żeby twój model dobrze odzwierciedlał rzeczywisty obiekt.

Pitlab

1:31, 02.12.2012

#9

Cześć

Na początek wielkie dzięki za świetny artykuł i wskazanie takiego narzędzia jak Octave.

Zaimplementowałem ten filtr w swoim komputerze pokładowym AutoPitLot do bardzo podobnego zastosowania a mianowicie do filtrowania wysokości. W komputerze mam pomiar przyspieszenia, wariometr mierzący prędkość pionową i pomiar wysokości barometrycznej. Pomiary działają, ale mam problemy (jeszcze przed zaimplementowaniem filtru Klamana) ze sterowaniem w pętli PID, prawdopodobnie wynikające z opóźnień generowanych na filtrach wygładzających pomiary. Sytuacja aż prosi się o rozsądny filtr nie wprowadzający dużych opóźnień.

Analogicznie do przykładu z łódką droga u mnie jest wysokością, prędkość jest prędkością pionową a przyspieszenie jest składową przyspieszenia w osi Z (już po transformacji układu współrzędnych z lokalnego do globalnego, tak że pochylenie i przechylenie nie zmieniają wskazań przyspieszenia).

W tym układzie predykcja "zasilana" jest przyspieszeniem a korekcja jest robiona od wysokości (przydało by się jeszcze od prędkości, ale w tym miejscu macierz w wyniku obliczeń zwija się do rozmiaru 1x1 i nie ma jak podać prędkości).

Na obrazku jest kilka wykresów. Znaczenie kolorów: niebieski - wysokość, zielony - prędkość, czerwony - przyspieszenie.

Na górnym wykresie jest odchylenie standardowe szumu (szum wysokości jest testowo lekko przefiltrowany, ale to szczegół)

Niżej dwa niebieskie wykresy to wysokość. Jasna, pastelowa linia to wartość mierzona, intensywny kolor to estymowana filtrem, konkretnie wartość X[0] (w łódce to było xhat).

Niżej zielone wykresy to pomiar (pastelowy) i filtrowany (intensywny zielony)

Niżej jest pomiar przyspieszenia czerwonym pastelowym.

Na samym dole są współczynniki kalmana K[0] odpowiadający wysokości na niebiesko i K[1] odpowadający prędkości na zielono.

Pokazane jest kilka ruchów między podłogą a szafą mającą 201cm. Filtr nadąża za danymi, ale dosyć wolno. Na tyle wolno że przy szybkich ruchach ogranicza amplitudę wskazań. Dla moich potrzeba jest to stanowczo za wolno.

Co ciekawe w przypadku prędkości wynik filtru wyprzedza w czasie pomiar. Być może jest to efekt filtrowania sprzętowego w obwodach wariometru.

Na początku miałem stałe parametry dotyczące szumu procesu i pomiaru. Kręcenie nimi daje pewne pole do popisu, ale w rzeczywistym układzie mocno narażonym na drgania chcę aby filtr sam dopasowywał się do warunków, stąd w tym przykładzie szum procesu i pomiaru są okresowo (co 1s) uaktualniane bieżącymi danymi statystycznymi o sygnałach. Te aktualizacje widać jako skoki zielonego współczynnika kalmana wysokości na samym dole.

Mam wątpliwości do zachowania się współczynnika kalmana K[0] oznaczającego proporcję danych estymowanych do mierzonych. O ile K[1] odpowiadający prędkości ładnie rośnie wraz ze wzrostem odchylenia standardowego, to K[0] odpowiadający wysokości oscyluje ciągle w okolicy 0,05.

Jeszcze mam wątpliwość co do wielkości macierzy H, jest to pozioma macierz [2x1].

W miejscu gdzie liczymy współczynnik kalmana wzorem:

K = P * H' * inv( H * P * H' + R);

są wykonane 3 operacje H * P * H' .

Rozmiar macierzy P to 2x2. Jeżeli pomnożymy macierze

[H0 H1] * [P0 P1;P2 P3] to wynik będzie macierzą [HP0 HP1] czyli zredukuje się do 2x1

Jeżeli teraz pomnożymy go przez H' czyli [H0; H1] (macierz pionową) to otrzymamy wynik jednoelementowy.

Próbowałem używać w octave macierzy H o rozmiarze 2x2, ale nie potrafię go ogarnąć i filtr wzbudza się okresowo. Może autor będzie w stanie to przeanalizować.

Po prostu nie wiem, czy to przypadkiem nie jest błąd.

Zależało by mi też na możliwości korekcji od prędkości ale we wzorze:

xhat = xhat + K*(z - H*xhat);

wynik mnożenia H*xhat jest jednoelementowy i można go odjąć tylko od jednego pomiaru z a chciałbym aby to była macierz 1x2 i można było korygować od dwóch parametrów.

GAndaLF

1:15, 04.12.2012

#10

Macierz H jest w artykule dosyć niezręcznie nazwana, ponieważ jest to po prostu macierz C modelu stanowego wyrażającego się wzorem:

x(t+1) = A*x(t) + B*u(t) (równanie stanu)

y(t) = C*x(t) + D*u(t) (równanie wyjścia)

Tak samo szumy procesowy i pomiarowy w innej nomenklaturze oznaczane są odwrotnie, ale to już szczegół:

-v to szum procesowy dodawany do równania stanu, jego wariancja jest często oznaczana jako V a nie Q

-w to szum pomiarowy dodawany do równania wyjścia, jego wariancja jest często oznaczana jako W a nie R

Jeżeli chcesz mieć korekcję od położenia i prędkości jednocześnie musisz zmodyfikować macierz H do postaci [1 0; 0 1]. Wtedy tak naprawdę y(t) = x(t). Tą modyfikację musisz uwzględnić we wszystkich równaniach filtru, żeby rozmiary macierzy się zgadzały.

W teorii wzmocnienie Kalmana powinno się ustalać na jakiś konkretnych wartościach wraz z upływem czasu. Dlatego K[0] zachowuje się poprawnie, a to właśnie prędkość działa niepoprawnie. Prawdopodobnie dzieje się tak dlatego, że w twoim aktualnym modelu stanowym A,B,C(H),D zmienna stanu odpowiedzialna za prędkość jest nieobserwowalna. Bardzo możliwe, że jak zmodyfikujesz macierz H tak, aby prędkość była również śledzona na wyjściu i uaktualnisz wszystkie równania, parametr K[1] również będzie się zmieniał w niewielkich granicach. Jednak moim zdaniem ta ingerencja w filtr i dodatkowa aktualizacja danych o szumach jest niepotrzebna i może być nawet szkodliwa.

W artykule nie zostało to wyraźnie powiedziane, ale filtr Kalmana opiera się na teorii sterowania i rachunku prawdopodobieństwa. Jest to obserwator stanu czyli układ, który na podstawie analizy sygnału wyjściowego y(t), czyli wyjścia, przybliża wewnętrzny stan obiektu czyli x(t). Przybliżeniem stanu jest właśnie ten x z daszkiem. W pierwszych iteracjach te wskazania są błędne co ma związek z inicjalizowanymi wartościami parametrów. Jednak obserwatory zmniejszają swój błąd w każdym kroku w sposób wykładniczy, aby po kilku krokach poprawnie estymować stan obiektu. Teoria związana z obserwatorami nie uwzględnia jednak wpływu szumów. Dlatego filtr Kalmana dodatkowo wykorzystuje informacje o niedokładnościach w postaci macierzy kowariancji dla szumu pomiarowego i procesowego i minimalizuje powstały w ten sposób błąd zgodnie z kryterium minimalnych kwadratów.

Filtr Kalmana jest więc obserwatorem stanu działającym w sposób optymalny (najlepiej i najszybciej niweluje błąd estymacji stanu) dla zadanych warunków. Jeżeli warunki (najczęściej wariancje) podane filtrowi nie zgadzają się z rzeczywistymi, istnieje wtedy inny filtr Kalmana, który działa bardziej optymalnie. Jednak jeżeli te wariancje nie są diametralnie różne od rzeczywistych w dalszym ciągu otrzymujemy estymator stanu o bardzo dobrych parametrach a dodatkowo mamy większą pewność, że jest odporny na szumy. Nie musisz więc aktualizować informacji o szumach ponieważ w ten sposób sprawiasz, że zmieniły się warunki i filtr znowu przez kilka kroków musi nadganiać rzeczywisty stan. Wystarczy, że na początku założysz bezpieczną wariancję, a algorytm sobie poradzi.

Pitlab

11:25, 04.12.2012

#11

GAndaLF napisał/a:

Macierz H jest w artykule dosyć niezręcznie nazwana, ponieważ jest to po prostu macierz C modelu stanowego

Dzięki za odpowiedź. Ja też do swoich potrzeb nazywam to macierzą C, ale w wikipedii w wersji angielskiej używane jest oznaczenia H. Różne szkoły mają różne nazwy.

GAndaLF napisał/a:

Jeżeli chcesz mieć korekcję od położenia i prędkości jednocześnie musisz zmodyfikować macierz H do postaci [1 0; 0 1]. Wtedy tak naprawdę y(t) = x(t). Tą modyfikację musisz uwzględnić we wszystkich równaniach filtru, żeby rozmiary macierzy się zgadzały.

Tak własnie robię, ale coś jest nie tak, bo na początku wygląda dobrze ale w okolicach 6 iteracji zaczyna rosnąć kowariancja (macierz P), rosną współczynniki kalmana i wyglądają tak jak na obrazku niżej:

Podobnie zmienia się estymata:

Zmodyfikowany kod bazujący na kodzie łódki wygląda tak jak niżej. Zmieniłem sposób wywołania bez użycia funkcji, zmieniłem kilka nazw, np. H na C, xhat na x. Dodałem też zakomentowanych poleceń wyświetlenia wartości danych, aby debugować algorytm.

duration = 6 %czas trwania symulacji [s]

step = 0.05 %krok symulacji [s]

measnoise = [0.5; 0.5]; %wspolczynnik szumu pomiaru

accnoise = 0.2; %wspolczynnik szumu przyspieszenia

T = step;

R = [0.5^2 0.15; 0.15 0.3^2]; %Wyznaczenie kowariancji szumu pomiaru

Q = accnoise^2 * [(T^2/2)^2 T*0.02; T*0.02 0.04]; %Kowariancja szumu procesu

A = [1 T; 0 1]; %Macierz przejscia stanu poprzedniego do obecnego

B = [T^2 /2; T]; %Macierz przejscia dla wejscia

C = [1 0; 0 1]; %Macierz przejscia dla mierzonej wielkosci z korekcją dwu wartosci

%Inicjalizacja

I = [ 1 0; 0 1 ]; %Macierz jednostkowa

P = Q; %Poczatkowa macierz kowariancji P

x = [0; 0]; %wektor stanu

xsym = x; %symulowana wartość "prawdziwa"

u = 0.3; %Przyspieszenie [m/s*s]

%Przygotowanie miejsca na dane

pos = []; %Prawdziwa pozycja

poshat = []; %Pozycja estymowana filtrem kalmana

posmeas = []; %Pozycja mierzona

speed = []; %prawdziwa prędkość

speedhat = []; %prędkość estymoana filtrem

speedmeas = []; %predkość mierzona

kpos = []; %współczynnik kalmana dla wysokości

kspeed = []; %wspólczynnik kalmana dla prędkości

%Rozpoczecie symulacji

for t = 0:T:duration,

%Symulacja naszego stystemu liniowego, dodajemy szum przyspieszenia

xsym = A * xsym + B * u + accnoise * [(T^2/2)*randn; T * randn];

%printf ("xsym [%4.4f %4.4f]\n", xsym);

%Symulacja zaszumionego pomiaru

z = C * xsym + measnoise * randn;

%printf ("z [%4.4f %4.4f]\n", z);

%Faza Predykcji

x = A * x + B * u;

%printf ("x [%4.4f %4.4f]\n", x);

P = A*P*A' + Q;

%Faza Korekcji

K = P * C' * inv( C * P * C' + R);

%printf ("Macierz C [%4.0f %4.0f %4.0f %4.0f]\n", C);

printf ("Macierz P [%4.4f %4.4f %4.4f %4.4f]\n", P);

%printf ("C*P [%4.4f %4.4f]\n", C*P);

%printf ("K [%4.4f %4.4f %4.4f %4.4f]\n", K);

x = x + K*(z - C*x);

%printf ("z-C*x [%4.4f %4.4f]\n", z-C*x);

%printf ("K*(z-C*x) [%4.4f %4.4f]\n", K*(z-C*x));

P = ( I - K*C)*P;

%printf ("\n");

%Uaktualnienie danych

pos = [pos; xsym(1)]; %Pozycja rzeczywista

posmeas = [posmeas; z(1)]; %Pozycja mierzona

poshat = [poshat; x(1)]; %Pozycja estymowana FK

speed = [speed; xsym(2)];

speedmeas = [speedmeas; z(2)];

speedhat = [speedhat; x(2)];

kpos = [kpos; K(1)];

kspeed = [kspeed; K(2)];

end;

close all;

t = 0:step:duration;

figure;

plot(t, pos, t, posmeas, t, poshat);

legend('prawdziwa', 'mierzona', 'estymowana');

title('Wysokosc');

plot(t, speed, t, speedmeas, t, speedhat);

title('Predkosc');

plot(t, kpos, t, kspeed);

legend('Wysokosc', 'Predkosc');

title('Wspolczynniki Kalmana');

Siedziałem nad nim jeszcze dzisiaj przez kilka godzin, porównywałem ze wzorami w wikipedii i nic rozsądnego nie wymyśliłem. Coś musi być jednak nie tak, bo nawet jeżeli zmienię macierz C z [1 0; 0 1]; na [1 0; 0 0]; to nie jest tak samo jak w przykładzie z łódką. Czy mógłbyś rzucić okiem na ten kod?

GAndaLF napisał/a:

W teorii wzmocnienie Kalmana powinno się ustalać na jakiś konkretnych wartościach wraz z upływem czasu. Dlatego K[0] zachowuje się poprawnie, a to właśnie prędkość działa niepoprawnie. Prawdopodobnie dzieje się tak dlatego, że w twoim aktualnym modelu stanowym A,B,C(H),D zmienna stanu odpowiedzialna za prędkość jest nieobserwowalna. Bardzo możliwe, że jak zmodyfikujesz macierz H tak, aby prędkość była również śledzona na wyjściu i uaktualnisz wszystkie równania, parametr K[1] również będzie się zmieniał w niewielkich granicach.

OK dziękuję za ukierunkowanie. Spróbuję przerobić mój kod C tak aby macierz C była 2x2, bo na razie próbowałem z macierzą 2x1. Wydaje mi się że zmienna od prędkości jest obserwowalna, bo całkiem nieźle pokrywa się z wynikami pomiaru.

GAndaLF napisał/a:

Jednak moim zdaniem ta ingerencja w filtr i dodatkowa aktualizacja danych o szumach jest niepotrzebna i może być nawet szkodliwa.

Jasne. U mnie parametry szumu mocno zależą od tego czy maszyna stoi czy leci. Drgania wnoszone przez silniki w trakcie lotu przenoszą się na membranę czujnika ciśnienie i mocno zaszumiają pomiar. Stąd wziął się pomysł aktualizacji parametrów szumu. Skoro filtr sobie z tym radzi to tym lepiej, bo mniej roboty z obliczeniami.

GAndaLF

21:21, 04.12.2012

#12

No faktycznie wczoraj się pomyliłem z tą obserwowalnością. Jak skonstruujemy macierz obserwowalności [CA; CAA] to wyjdzie [1 0; 1 T] i jej wyznacznik wynosi T.

Przeanalizowałem Twój problem i wiem już dlaczego filtr rozjeżdża się między 5 a 6 sekundą. Powodem jest niestabilność numeryczna. Macierz P zawsze musi być dodatnio określona i symetryczna dlatego, że jest macierzą kowariancji dla każdego kolejnego kroku. Jednak te wartości robią się z każdym krokiem coraz mniejsze i w końcu są tak małe, że ich obliczanie powoduje błędy numeryczne. Jeżeli zatrzymasz symulację po 5 sekundach, kiedy estymata już się rozjeżdża, ale jeszcze nie doszła do tak wielkich rozmiarów, zobaczysz, że po ostatnim kroku macierz P ma wszystkie wartości ujemne. Poza tym te wartości są bardzo małe. U mnie rzędu e-19.

W swoich implementacjach filtru Kalmana jeszcze nie miałem tego problemu, ale na wykładach nam mówili, że jednym ze sposobów pozbycia się czegoś takiego jest inna forma równania na kowariancje a posteriori:

P(t|t) = [I−K(t)C]P(t|t−1)[I−K(t)C]T+K(t)WKT(t)

Ta forma też jest podobno zawodna, ale pewniejsza od tej standardowej. Najpewniejszym, ale i najbardziej skomplikowanym sposobem sposobem pozbycia się problemu jest "Square root Kalman filter" czyli postać w której używa się pierwiastków kwadratowych macierzy. Możesz też gdzieś w pętli programu sprawdzać, czy macierz P jest po przekształceniu dodatnio określona i jeśli nie to najwyżej jej nie aktualizować. Zmodyfikowałem twój kod według ostatniego sposobu. Oto Wyniki:

clear

clc

duration = 10 %czas trwania symulacji [s]

step = 0.05 %krok symulacji [s]

measnoise = [0.5; 0.5]; %wspolczynnik szumu pomiaru

accnoise = 0.2; %wspolczynnik szumu przyspieszenia

T = step;

R = [0.5^2 0.15; 0.15 0.3^2]; %Wyznaczenie kowariancji szumu pomiaru

Q = accnoise^2 * [(T^2/2)^2 T*0.02; T*0.02 0.04]; %Kowariancja szumu procesu

A = [1 T; 0 1]; %Macierz przejscia stanu poprzedniego do obecnego

B = [T^2 /2; T]; %Macierz przejscia dla wejscia

C = [1 0; 0 1]; %Macierz przejscia dla mierzonej wielkosci z korekcją dwu wartosci

%Inicjalizacja

I = [ 1 0; 0 1 ]; %Macierz jednostkowa

P = Q; %Poczatkowa macierz kowariancji P

x = [0; 0]; %wektor stanu

xsym = x; %symulowana wartość "prawdziwa"

u = 0.3; %Przyspieszenie [m/s*s]

%Przygotowanie miejsca na dane

pos = []; %Prawdziwa pozycja

poshat = []; %Pozycja estymowana filtrem kalmana

posmeas = []; %Pozycja mierzona

speed = []; %prawdziwa prędkość

speedhat = []; %prędkość estymoana filtrem

speedmeas = []; %predkość mierzona

kpos = []; %współczynnik kalmana dla wysokości

kspeed = []; %wspólczynnik kalmana dla prędkości

%Rozpoczecie symulacji

for t = 0:T:duration,

%Symulacja naszego stystemu liniowego, dodajemy szum przyspieszenia

xsym = A * xsym + B * u + accnoise * [(T^2/2)*randn; T * randn];

%printf ("xsym [%4.4f %4.4f]\n", xsym);

%Symulacja zaszumionego pomiaru

z = C * xsym + measnoise * randn;

%printf ("z [%4.4f %4.4f]\n", z);

%Faza Predykcji

x = A * x + B * u;

%printf ("x [%4.4f %4.4f]\n", x);

P = A*P*A' + Q;

%Faza Korekcji

K = P * C' * inv( C * P * C' + R);

%printf ("Macierz C [%4.0f %4.0f %4.0f %4.0f]\n", C);

%printf ("Macierz P [%4.4f %4.4f %4.4f %4.4f]\n", P);

%printf ("C*P [%4.4f %4.4f]\n", C*P);

%printf ("K [%4.4f %4.4f %4.4f %4.4f]\n", K);

x = x + K*(z - C*x);

%printf ("z-C*x [%4.4f %4.4f]\n", z-C*x);

%printf ("K*(z-C*x) [%4.4f %4.4f]\n", K*(z-C*x));

Pn = P - K*(C*P*C'+R)*K';

if eig(Pn)>=0

P = Pn;

end

%printf ("\n");

%Uaktualnienie danych

pos = [pos; xsym(1)]; %Pozycja rzeczywista

posmeas = [posmeas; z(1)]; %Pozycja mierzona

poshat = [poshat; x(1)]; %Pozycja estymowana FK

speed = [speed; xsym(2)];

speedmeas = [speedmeas; z(2)];

speedhat = [speedhat; x(2)];

kpos = [kpos; K(1)];

kspeed = [kspeed; K(2)];

end;

close all;

t = 0:step:duration;

figure;

subplot(3,1,1);

plot(t, pos, t, posmeas, t, poshat);

legend('prawdziwa', 'mierzona', 'estymowana');

title('Wysokosc');

subplot(3,1,2);

plot(t, speed, t, speedmeas, t, speedhat);

title('Predkosc');

subplot(3,1,3);

plot(t, kpos, t, kspeed);

legend('Wysokosc', 'Predkosc');

title('Wspolczynniki Kalmana');

Pitlab

21:56, 04.12.2012

#13

GAndaLF napisał/a:

Przeanalizowałem Twój problem i wiem już dlaczego filtr rozjeżdża się między 5 a 6 sekundą. Powodem jest niestabilność numeryczna. [...]

W swoich implementacjach filtru Kalmana jeszcze nie miałem tego problemu, ale na wykładach nam mówili, że jednym ze sposobów pozbycia się czegoś takiego jest inna forma równania na kowariancje a posteriori:

P(t|t) = [I−K(t)C]P(t|t−1)[I−K(t)C]T+K(t)WKT(t)

Ta forma też jest podobno zawodna, ale pewniejsza od tej standardowej. Najpewniejszym, ale i najbardziej skomplikowanym sposobem sposobem pozbycia się problemu jest "Square root Kalman filter" czyli postać w której używa się pierwiastków kwadratowych macierzy. Możesz też gdzieś w pętli programu sprawdzać, czy macierz P jest po przekształceniu dodatnio określona i jeśli nie to najwyżej jej nie aktualizować.

Jesteś wielki! Bardzo dziękuję.

Testowałem kilka różnych rzeczy, zauważyłem że ze zmianą parametrów szumu problem następuje w innym miejscu, ale o dodatnim określeniu macierzy nigdy nie słyszałem i mógłbym tak walić głową w mur bez skutku.

Mam 5 innych pilnych rzeczy do zrobienia, ale chyba usiądę i zakoduję to w C aby przetestować w rzeczywistej aplikacji. Już mnie paluszki swędzą :-)

GAndaLF

20:36, 05.12.2012

#14

Dzisiaj jeszcze pomyślałem trochę nad tym twoim kodem i chyba jednak dalej coś jest nie tak. Wariancja wyrażona macierzą P nie powinna maleć do 0. To by oznaczało, że proces z wzrostem czasu przestaje być losowy, a to jest bez sensu. W innej mojej symulacji większość jej wartości jest wyraźnie różna od 0.

Jak zmieniłem początkowe wartości wariancji szumów na np. [0.1 0; 0 0.1] to filtr dąży do wartości mierzonej, a nie rzeczywistej. To by oznaczało, że dalej gdzieś w kodzie jest błąd.

Jak będę miał więcej czasu to spróbuję napisać program od 0 i porównamy wyniki.

Pitlab

21:38, 05.12.2012

#15

Mnie też ten kod nie bardzo działa. Zaimplementowałem go w C i nadal jest mocno niestabilny. Początkowo myślałem że mam jakiś problem z przypisaniem zmiennych (zrobiłem zestaw zmiennych dla macierzy 3x3 a teraz pakuję w to dane dla 2x2), ale podczas testów uchwyciłem moment wykrzaczenia się filtra:

Te białe wykresy na górze to 4 wartości macierzy P. Tam widać jakieś strzały i to pewnie tutaj jest źródło problemu. Niebieskie wykresy to wysokość, zielone prędkość, czerwony przyspieszenie a na samym dole 4 elementy macierzy K. Widać że w pewnym momencie wartości zerują się ( w debugerze przybierają wartości NaN - Not a Number). Zresztą widać że część współczynników macierzy K ma wartości ujemne (konkretnie [1,1] i [0,1]). Czy to ma sens? Może też tak jak w przypadku macierzy P nie dopuszczać do powstania ujemnych wartości?

W tym przykładzie akurat mam ustawiony bardzo mały szum akcelerometrów i filtr mocno ufa predykcji a słabo pomiarowi. Przy okazji zdałem sobie sprawę że ogólnie filtr może być mało ciekawy w locie, bo gdy platforma drży, to filtr przez mocno zaszumione dane z akcelerometru będzie wnosił dodatkowy szum do wyników. Może się okazać że poleganie na akcelerometrze będzie tylko źródłem kłopotów, no ale to trzeba sprawdzić w locie.

Edit. Zabranianie ujemnych wartości w macierzy K nic nie daje, natomiast gdy zwiększyłem odchylenie standardowe szumu akcelerometru (szum procesu) do 1m/s^2 to filtr zachowuje się poprawnie. Zrobię zrobię jakieś narzędzie aby móc elastycznie zmieniać parametry filtra w czasie pracy i zobaczymy.

Edit2:

Zrobiłem możliwość zmiany parametrów szumu i przedstawiam pierwsze rezultaty.

Szum pomiaru wysokości mam ustawiony na 0,2m

Szum pomiaru prędkości na 0,3m

Zmieniałem szum pomiaru akcelerometru. Na pierwszym obrazku kolejno od lewej 3, 2, 1 i 0,5 m/s^2. Macierz P (na smaej górze) teraz jest stabilna. Współczynniki kalmana (na dole) są dodatnie i widać jak się zmieniają.

Dalej zmieniałem szum akcelerometru, kolejno 0,3, 0,2, 0,1, 0,05 i 0,01 m/s^2.

Tutaj współczynnik kalmana dla prędkości jest ujemny. Na razie filtr wydaje się być stabilny. Na dzisiaj kończę, pobawię się jeszcze jutro, bo ciągle parametry filtra nie są zadowalające. Obserwuję za duże opóźnienie estymaty wysokości. Widać to przy szybkich ruchach, prędkość w miarę nadąża, ale szybkie zmiany wysokości są odfiltrowywane (trochę zbyt mocno).

GAndaLF

0:01, 06.12.2012

#16

Taka mała uwaga, że definicja dodatniej określoności nie mówi dosłownie, że wszystkie komórki są dodatnie, tylko że wartości własne są dodatnie. Jeśli wszystkie komórki są dodatnie to macierz jest dodatnio określona, ale samo pojęcie jest bardziej ogólne.

Przyjrzał bym się dokładnie macierzom kowariancji szumu. Zwykle dla szumu w równaniu stanu macierz kowariancji daje się diagonalną. Innym potencjalnym miejscem, gdzie mogą się pojawiać błędy jest odwracanie macierzy. Kończą mi się już pomysły co jeszcze może być powodem błędnego działania.

[ Dodano: 06-12-2012, 01:34 ]

Napisałem skrypt matlabowy od początku:

clear

clc

%wektor czasu

t = 5;

dt = 0.01;

T = 0:dt:t;

%model stanu

A = [1 dt; 0 1];

B = [dt^2/2; dt];

C = [1 0; 0 1];

%pobudzenie

a = 0.3;

U = a*ones(1, size(T,2));

%wektory wejscia i wyjscia

X = zeros(2, size(T,2));

Y = zeros(2, size(T,2));

%Y = zeros(1, size(T,2));

%warunki poczatkowe

x0 = [1;0];

X(:,1) = x0;

%wektory zaszumione

Xn = X;

Yn = Y;

%wektory z filtrem kalmana

Xk = X;

Yk = Y;

%parametry szumu

stddevx1 = 0.9;

stddevx2 = 0.9;

V = [stddevx1^2*dt 0; 0 stddevx2^2*dt];

stddevy1 = 0.9;

stddevy2 = 0.9;

W = [stddevy1^2/dt 0; 0 stddevy2^2/dt];

%W = stddevy1^2;

%zmienne do filtru kalmana

xpri = [0;0];

xpost = [0;0];

Ppri = V;

Ppost = V;

for i=1:size(T,2)

X(:,i+1) = A*X(:,i) + B*U(:,i);

Y(:,i) = C*X(:,i);

Xn(:,i+1) = A*X(:,i) + B*U(:,i) + [rand*stddevx1 - stddevx1/2; rand*stddevx2 - stddevx2/2];

Yn(:,i) = C*X(:,i) + [rand*stddevy1 - stddevy1/2; rand*stddevy2 - stddevy2/2];

%filtr Kalmana

%aktualizacja pomiarow

e = Yn(:,i) - C*xpri;

S = C*Ppri*C'+W;

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

xpost = xpri+K*e;

Ppost = Ppri-K*S*K';

Xk(:,i) = xpost;

Yk(:,i) = C*Xk(:,i);

%aktualizacja czasu

xpri = A*xpost+B*U(:,i);

Ppri = A*Ppost*A' + V;

end

subplot(2,1,1);

plot(T, X(1,1:i), T, Xn(1,1:i), T, Xk(1,1:i));

legend('prawdziwa', 'mierzona', 'estymowana');

title('Wysokosc');

subplot(2,1,2);

plot(T, X(2,1:i), T, Xn(2,1:i), T, Xk(2,1:i));

legend('prawdziwa', 'mierzona', 'estymowana');

title('Predkosc');

Tutaj wyniki:

Specjalnie dałem inny punkt startowy, żeby zobaczyć jak filtr Kalmana dochodzi do poprawnej wartości. Moja implementacja wykorzystuje minimalnie inne wzory:

x(t + 1|t) = A(t)x(t|t) + B(t)u(t)

P(t + 1|t) = A(t)P(t|t)A'(t) + V(t)

e(t) = y(t) − C(t)x(t|t − 1)

S(t) = C(t)P(t|t − 1)C'(t) +W(t)

K(t) = P(t|t − 1)C'(t)S^−1(t)

x(t|t) = x(t|t − 1) +K(t)e(t)

P(t|t) = P(t|t − 1) −K(t)S(t)K'(t)

Praktycznie jedyna różnica jest w ostatnim równaniu. Obie implementacje są spotykane w literaturze, więc ta różnica nie może być zbyt znacząca. Wydaje mi się, że klucz do problemu leży w wariancjach szumów i ich dyskretyzacji.

Zobacz wszystkie komentarze (32) na forum

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