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:

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.

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:

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:

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

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

A nasze równania systemu przyjmują postać:

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:

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

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:

Gdzie:

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

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.

Gdzie:

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.

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.

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:

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:

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

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

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

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:

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.

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.

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:

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:

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:

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

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:

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

Natomiast faza korekcji:

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:

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.

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

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:

Co można też wyrazić jako:

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

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:

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:

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

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:

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

filtr, kalman, matlab, octave