Filtr Kalmana od teorii do praktyki – #2

Filtr Kalmana od teorii do praktyki – #2

Poprzednio przybliżyłem teorię związaną z filtrem Kalmana oraz na przykładzie pokazałem reakcję filtru na zmiany poszczególnych parametrów.

Teraz zajmę się czymś bardziej praktycznym. Na podstawie danych pomiarowych z akcelerometru i żyroskopu postaram się zaprojektować filtr Kalmana dla układu śledzącego orientację robota w przestrzeni.

« Poprzedni artykuł z seriiNastępny artykuł z serii »

Metoda pomiaru

Wartością, którą będę śledził jest orientacja robota wyrażona jako kąt na płaszczyźnie xy w trójwymiarowym, kartezjańskim układzie współrzędnych. Do dyspozycji mam żyroskop mierzący prędkość kątową wokół trzech osi x, y i z, a także akcelerometr mierzący przyspieszenie liniowe również w trzech osiach x, y i z.

Zakładam, że jedyną siłą działającą na akcelerometr jest grawitacja. Wektor siły grawitacji rozkłada się na trzy składowe odpowiadające poszczególnym osiom układu współrzędnych. Można go również przedstawić za pomocą współrzędnych sferycznych r, \phi, \theta (patrz rysunek). Kąt \phi można obliczyć z wzoru:

\phi = \arctan(\frac{x}{y})

Wektor w układzie współrzędnych przedstawiony za pomocą współrzędnych prostokątnych i sferycznych.

Wektor w układzie współrzędnych przedstawiony za pomocą współrzędnych prostokątnych i sferycznych.

Cechą charakterystyczną żyroskopów jest tak zwany dryft. Jest to rodzaj błędu pomiarowego kumulującego się z czasem. Jeżeli zostawimy czujnik nieruchomo, mierzona wartość będzie stopniowo oddalać się od zera. Pomiar z żyroskopu \omega_{g} można więc zapisać jako suma rzeczywistej prędkości kątowej \omega i aktualnej wartości dryftu g:

\omega_g = \omega + g

Wyprowadzenie modelu stanowego

Dryft żyroskopu potraktujemy jako jedną ze zmiennych stanu, którą będziemy estymować. Tak więc nasz model będzie składać się z dwóch zmiennych stanu - położenia kątowego robota i dryftu żyroskopu. Położenie kątowe w danej chwili czasu składa się z położenia w poprzedniej chwili czasu i wpływu prędkości kątowej w bieżącym cyklu próbkowania. Można to zapisać jako:

\phi(t) = \phi(t-1) + \omega dt \\ \omega(t) = \omega_g(t) - g(t-1) \\ \phi(t) = \phi(t-1) + \omega_g(t) dt - g(t-1) dt \\ g(t) = g(t-1)

Z tych równań otrzymujemy następujący model stanowy:

kalman_02_01

Przekształcenia danych pomiarowych

Plik data2.txt zawiera dane pomiarowe z żyroskopu i akcelerometru. Zostały one zebrane przez mikrokontroler STM32 przy pomocy programu opublikowanego w drugiej części artykułu o filtrach alfa-beta. Wartości są przedstawione w słowach bitowych.

Należy je przekształcić odpowiednio na \frac{^{\circ}}{s} i g:

kalman_02_02

Implementacja w Matlabie

Oto implementacja filtru w Matlabie:

Wynik symulacji został przedstawiony na wykresie. Poszczególne wariancje dobierałem metodą prób i błędów. Jak widać, filtr reaguje na skokowe zmiany z niewielkim opóźnieniem i ignoruje szpilki. Inne właściwości filtru można uzyskać zmieniając macierz kowariancji V.

Wyniki symulacji.

Wyniki symulacji.

Podsumowanie

W tej części cyklu przedstawiłem sposób projektowania filtru Kalmana dla rzeczywistego układu. Najpierw przeanalizowałem dane odczytywane z czujników i dokonałem przekształceń. Następnie wyprowadziłem model stanowy badanego obiektu. Ostatnią fazą była implementacja filtru.

W kolejnej części zajmiemy się implementacją filtru na STM32.

« Poprzedni artykuł z seriiNastępny artykuł z serii »

akcelerometr, filtr, kalman, kalmana, matlab, żyroskop

Załączniki

data2.txt (plain, 11 KB)

Dane testowe do filtru Kalmana.

Komentarze

Dodaj komentarz