Kursy • Poradniki • Inspirujące DIY • Forum

Premiera: Raspberry Pi
Debug Probe

Historia tranzystora w skrócie.
Wynalezienie czy odkrycie?

3 podstawowe rodzaje
akumulatorów, które warto znać

Co ciekawego w DIY?
10 projektów naszych czytelników #33

Piezoelektryki – czym
są materiały piezoelektryczne?

Jak unikać błędów

podczas lutowania?

NE555

Kurs elektroniki

Kursy Artykuły

DIY Forum

Filtr Kalmana od teorii do praktyki – #3 – Testy na STM32

ProgramowanieSTM32 GandalF

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

« Poprzedni artykuł z serii

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 2x2, 2x1, 1x2 i 1x1. Dla każdej kombinacji działania i rozmiarów macierzy jest oddzielna funkcja np. do pomnożenia macierzy 2x2 i 2x1 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.

      /* x(t+1|t) = Ax(t|t) + Bu(t) */
      u[0] = sensor_gyro_get_z()*250/32768;
      matrix_2x2_mul_2x1(A, x_post, Ax);
      matrix_2x1_mul_1x1(B, u, Bu);
      matrix_2x1_add_2x1(Ax, Bu, x_pri);

      /* P(t+1|t) = AP(t|t)A^T + V */
      matrix_2x2_mul_2x2(A, P_post, AP);
      matrix_2x2_trans(A, AT);
      matrix_2x2_mul_2x2(AP, AT, APAT);
      matrix_2x2_add_2x2(APAT, V, P_pri);

      /* eps(t) = y(t) - Cx(t|t-1) */
      acc_x = sensor_acc_get_x();
      acc_y = sensor_acc_get_y();
      y[0] = atan(acc_x/acc_y)*180/M_PI;
      matrix_1x2_mul_2x1(C, x_pri, Cx);
      eps[0] = y[0] - Cx[0];

      /* S(t) = CP(t|t-1)C^T + W */
      matrix_1x2_mul_2x2(C, P_pri, CP);
      matrix_1x2_mul_2x1(C, C, CPCT);
      S[0] = CPCT[0] + W[0];

      /* K(t) = P(t|t-1)C^TS(t)^-1 */
      matrix_2x2_mul_2x1(P_pri, C, PCT);
      S1[0] = 1/S[0];
      matrix_2x1_mul_1x1(PCT, S1, K);

      /* x(t|t) = x(t|t-1) + K(t)eps(t) */
      matrix_2x1_mul_1x1(K, eps, Keps);
      matrix_2x1_add_2x1(x_pri, Keps, x_post);

      /* P(t|t) = P(t|t-1) - K(t)S(t)K(t)^T */
      matrix_2x1_mul_1x1(K, S, KS);
      matrix_2x1_mul_1x2(KS, K, KSKT);
      matrix_2x2_sub_2x2(P_pri, KSKT, P_post);

/* x(t+1|t) = Ax(t|t) + Bu(t) */

u[0] = sensor_gyro_get_z()*250/32768;

matrix_2x2_mul_2x1(A, x_post, Ax);

matrix_2x1_mul_1x1(B, u, Bu);

matrix_2x1_add_2x1(Ax, Bu, x_pri);

/* P(t+1|t) = AP(t|t)A^T + V */

matrix_2x2_mul_2x2(A, P_post, AP);

matrix_2x2_trans(A, AT);

matrix_2x2_mul_2x2(AP, AT, APAT);

matrix_2x2_add_2x2(APAT, V, P_pri);

/* eps(t) = y(t) - Cx(t|t-1) */

acc_x = sensor_acc_get_x();

acc_y = sensor_acc_get_y();

y[0] = atan(acc_x/acc_y)*180/M_PI;

matrix_1x2_mul_2x1(C, x_pri, Cx);

eps[0] = y[0] - Cx[0];

/* S(t) = CP(t|t-1)C^T + W */

matrix_1x2_mul_2x2(C, P_pri, CP);

matrix_1x2_mul_2x1(C, C, CPCT);

S[0] = CPCT[0] + W[0];

/* K(t) = P(t|t-1)C^TS(t)^-1 */

matrix_2x2_mul_2x1(P_pri, C, PCT);

S1[0] = 1/S[0];

matrix_2x1_mul_1x1(PCT, S1, K);

/* x(t|t) = x(t|t-1) + K(t)eps(t) */

matrix_2x1_mul_1x1(K, eps, Keps);

matrix_2x1_add_2x1(x_pri, Keps, x_post);

/* P(t|t) = P(t|t-1) - K(t)S(t)K(t)^T */

matrix_2x1_mul_1x1(K, S, KS);

matrix_2x1_mul_1x2(KS, K, KSKT);

matrix_2x2_sub_2x2(P_pri, KSKT, P_post);

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.

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.

« Poprzedni artykuł z serii

Załączniki

data3.txt (plain, 9 KB)

Dane odczytane z STM32.

Kalman_C.zip (zip, 286 KB)

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

Artykuł był ciekawy?

Dołącz do 20 tysięcy osób, które otrzymują powiadomienia o nowych artykułach! Zapisz się, a otrzymasz PDF-y ze ściągami (m.in. na temat mocy, tranzystorów, diod i schematów) oraz listę inspirujących DIY na bazie Arduino i Raspberry Pi.