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 – #3 – Testy na STM32

Programowanie Teoria 27.01.2015 GandalF

kalman3W poprzedniej części wyprowadziłem model stanowy dla systemu określającego położenie kątowe robota na podstawie danych z akcelerometru i żyroskopu. Zaprojektowałem filtr Kalmana dla tego modelu i sprawdziłem jego działanie z rzeczywistymi danymi pomiarowymi.

Teraz zajmę się przeniesieniem algorytmu na środowisko docelowe, czyli na STM32.

Nawigacja serii artykułów:
« poprzednia część

Do implementacji filtru wykorzystałem płytkę STM32F4 Discovery i moduł MiniIMU-9v2 zawierający trzyosiowy żyroskop, akcelerometr i magnetometr. W programie będę korzystał z dwóch pierwszych czujników.

Struktura programu

Szkielet programu jest podobny do tego wykorzystanego w implementacji filtru alfa-beta. Na STMie działa system FreeRTOS, na którym uruchomiono następujące wątki:

  • wątek diagnostyczny – nadrzędny wątek z którego uruchamiane są wszystkie pozostałe.
  • wątek czujnika – co 100ms sczytuje po I2C pomiary z akcelerometru i żyroskopu.
  • wątek filtru – co 100ms wykonuje iterację Filtru Kalmana z uwzględnieniem aktualnych odczytów z czujników.
  • logger – wysyła dane pomiarowe z czujników i aktualną estymatę obliczoną z KF przez USART.

Obliczenia macierzowe na STM32

W Matlabie implementacja Filtru Kalmana jest bardzo prosta ze względu na jego przystosowanie do obliczeń macierzowych. Mamy tam gotowe funkcje macierzowe do transpozycji, mnożenia, odwracania itp. Podczas implementacji na mikrokontrolerze nie mamy tego luksusu. Mamy tutaj dwa rozwiązania. Możemy wykorzystać gotową bibliotekę do obliczeń macierzowych albo napisać swoją. Ja zdecydowałem się na drugie rozwiązanie.

Moja biblioteka obliczeń macierzowych jest bardzo prosta. Implementacja znajduje się w folderze /code/matrix (pliki w załączniku). Każda macierz jest reprezentowana jako tablica typu float. Nie wykorzystuję żadnych dodatkowych parametrów określających ilość wierszy i kolumn. W programie wykonuję dodawanie i mnożenie na macierzach 2×2, 2×1, 1×2 i 1×1. Dla każdej kombinacji działania i rozmiarów macierzy jest oddzielna funkcja np. do pomnożenia macierzy 2×2 i 2×1 wykorzystuję funkcję matrix\_2x2\_mul\_2x1.

Pierwsze dwa argumenty to macierze, które przez siebie mnożę, trzeci argument to zmienna w której przechowujemy wynik działania.

Przy bardziej rozbudowanych macierzach i bardziej skomplikowanych obliczeniach takie podejście będzie niepraktyczne i wtedy lepiej zastosować gotową bibliotekę.

Implementacja filtru Kalmana

Obliczenia macierzowe zostały wykorzystane w pliku /code/filter/filter.c do implementacji Filtru Kalmana. Jak widać na każde równanie z głównego wzoru przypada kilka działań macierzowych. Należy stworzyć dodatkowe zmienne przechowujące wyniki pośrednie.

Wyniki

Wynik działania algorytmu przedstawia wykres. Wariancje zostały dobrane tak samo jak we wcześniejszym skrypcie matlabowym. Jak widać przefiltrowany sygnał wykazuje się niewielkim opóźnieniem i nie reaguje na gwałtowne chwilowe zmiany wartości.

Wynik działania algorytmu.

Wynik działania algorytmu.

Należy zauważyć, że wykorzystywane przeze mnie czujniki są bardzo wysokiej jakości. Żyroskop praktycznie nie ma dryftu, a szumy akcelerometru są bardzo niewielkie. W takim wypadku implementacja filtru niewiele daje. Praktycznie jedyną jego zaletą jest ochrona przed błędami grubymi, które mogą się trafić w jakiś pojedynczych odczytach.

« Poprzednia część

Powiadomienia o nowych, darmowych artykułach!

Załączniki

data3.txt (plain, 9 KB) - Pobierz

Dane odczytane z STM32.

Kalman_C.zip (zip, 286 KB) - Pobierz

Kompletny projekt pozwalający na uruchomienie filtru Kalmana na STM32.

Komentarze

r_bot

13:41, 04.03.2015

#1

Mam kilka uwag.

1. Czy bezpieczna jest ta linia? y[0] = atan(acc_x/acc_y)*180/M_PI Co jeśli acc_y będzie równe 0?

2. Nigdzie nie widzę użycia dwuelementowej tablicy CP? W tej linijce nie ma błędu? matrix_1x2_mul_2x1(C, C, CPCT); Z nazwy funkcji wynika, że mnożymy macierz 1x2 i macierz 2x1 a jako parametry są dwie takie same tablice C? Nie powinno być C*CP?

3. Czy nie lepiej byłoby skorzystać z tablic dwuwymiarowych, kod od razu był by dużo czytelniejszy.

GAndaLF

9:19, 05.03.2015

#2

1. Faktycznie lepiej zabezpieczyć się przed dzieleniem przez 0. Można dodać warunki, że dla acc_x ujemnego i acc_y = 0, y[0] = -90, dla acc_x dodatniego i acc_y = 0, y[0] = 90 i dla acc_x = 0, acc_y = 0, y = 0.

2. Powinno być CP * C - macierz CP jako pierwsza, chociaż w tym przypadku zamieniona kolejność da taki sam wynik.

3. Jak komuś wygodniej to oczywiście może użyć tablic dwuwymiarowych. Trzeba tylko pamiętać, żeby dodać transpozycje dla macierzy, w których jeden z wymiarów jest równy 1. W aktualnym kodzie korzystam z faktu, że macierz Nx1 i 1xN jest przechowywana w ten sam sposób.

r_bot

15:35, 05.03.2015

#3

Mam jeszcze jedno pytanie, dlaczego dane w kolumnie est_angle w pliku data3.txt są aż tak duże? Wrzuciłem ten filtr do Visual Studio i dla danych wejściowych acc_x, acc_y, gyro_z dane wyjściowe są 100 razy mniejsze niż te w kolumnie est_angle.

GAndaLF

15:50, 05.03.2015

#4

Odpowiedź znajdziesz w pliku logger.c - wynik przed wysłaniem przez usart jest mnożony przez 100, żeby uwzględnić 2 miejsca po przecinku.

Daniel_Wit

15:55, 18.05.2015

#5

Tutaj podobny projekt, warto również zajrzeć. https://github.com/TKJElectronics/KalmanFilter

moniu30

11:04, 03.08.2017

#6

Witam mam serdeczna prośbę czy mógłby ktoś krok po korku ze screenami kolejnych działań pokazać jak uruchomić ten project w eclipse.

Otworzenie, zaimportowanie i kompilacja.

Ni jak mi nie wychodzi ciągle jakieś błedy

"The selection cannot be launched, and there are n recent launches"

będe wdzieczny za każda pomoc

ewentualnie kontakt do przesłania instrukcji moniu30@wp.pl

Zobacz powyższe komentarze na forum

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