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, ϕ, θ (patrz rysunek). Kąt ϕ można obliczyć z wzoru:

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 ωg można więc zapisać jako suma rzeczywistej prędkości kątowej ω i aktualnej wartości dryftu 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:

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:

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 »

Załączniki

data2.txt (plain, 11 KB)

Dane testowe do filtru Kalmana.

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

Trwa ładowanie komentarzy...