Filtr Alfa – Beta od teorii do praktyki – #2

Programowanie GandalF

W poprzednim artykule przedstawiłem teorię dotyczącą filtrów alfa-beta. Wykonałem również symulacje w Matlabie demonstrujące wpływ nastaw na zachowanie filtru. Teraz przyszła pora na implementację filtru na mikrokontrolerze.

W projekcie wykorzystałem płytkę STM32F4 Discovery i moduł MiniIMU v2 od Pololu.

Udostępnij

« Poprzedni artykuł z serii

Praktyka

Zaimplementowana aplikacja wypisuje przez interfejs USART aktualne odczyty z żyroskopu we wszystkich trzech osiach oraz oszacowania prędkości kątowej wykonane za pomocą filtru alfa-beta. Aplikacja wykorzystuje system FreeRTOS na którym działają cztery wątki:

1. wątek odczytu danych z czujnika po I2C
2. wątek obliczający aktualne oszacowania filtru
3. wątek wypisujący dane przez interfejs USART
4. wątek diagnostyczny odpowiadający za inicjalizację pozostałych wątków i migający diodą w celu sprawdzenia czy aplikacja działa poprawnie.

Pomiary z czujnika wykonywane są co 100 ms. Do tej częstotliwości zostały dopasowane częstotliwości działania filtru i loggera wypisującego dane na USART.

Implementacja

Implementacja filtru alfa-beta jest bardzo prosta i składa się z 4 linijek kodu na każdą oś:

x_pri.x = x_post.x + dt * v_post.x;
x_pri.y = x_post.y + dt * v_post.y;
x_pri.z = x_post.z + dt * v_post.z;
v_pri.x = v_post.x;
v_pri.y = v_post.y;
v_pri.z = v_post.z;

x_post.x = x_pri.x + alfa.x * (sensor_get_x() - x_pri.x);
x_post.y = x_pri.y + alfa.y * (sensor_get_y() - x_pri.y);
x_post.z = x_pri.z + alfa.z * (sensor_get_z() - x_pri.z);
v_post.x = v_pri.x + beta.x * (sensor_get_x() - x_pri.x) / dt;
v_post.y = v_pri.y + beta.y * (sensor_get_y() - x_pri.y) / dt;
v_post.z = v_pri.z + beta.z * (sensor_get_z() - x_pri.z) / dt;

Dane z powyższej aplikacji zebrałem za pomocą programu RS Data Logger, a następnie przekonwertowałem na dane czytelne dla Matlaba. Wykorzystałem do tego prosty program napisanym w C (main.c w załączniku). W kolejnym kroku wygenerowałem wykresy wartości mierzonej i estymowanej dla wszystkich trzech osi:

Oś X

Oś y

Oś z

Analiza sygnałów

Sygnał z żyroskopu charakteryzuje się niewielkim zaszumieniem, a zmiany sygnału są dosyć gwałtowne. W takim wypadku dobrze zastosować dużą wartość parametru alfa, co oznacza upodobnienie sygnału odfiltrowanego do oryginalnego. Filtracja jest bardziej przydatna dla sygnałów o większym zaszumieniu.

Zobaczmy więc, jak filtr działa dla akcelerometru. Poniżej pomiary dla trzech osi dla parametru alfa=0.2 i beta=0.05:

Oś x

Oś y

Oś z

Podsumowanie

Tym razem widzimy pewne przeregulowania przy gwałtownych zmianach wartości, za to filtr dobrze radzi sobie z szumem.

Jego implementacja nie jest skomplikowana i zajmuje 4 linijki kodu. Strojenia dokonujemy za pomocą dwóch parametrów, gdzie najważniejszy jest parametr alfa.

« Poprzedni artykuł z serii

Załączniki

• Filtr alpha-beta
• Filtr alpha - beta, akcelerometr
• main.c

To nie koniec, sprawdź również

Przeczytaj powiązane artykuły oraz aktualnie popularne wpisy lub losuj inny artykuł »

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

Jak szybko płynie prąd? Prędkość ślimaka czy światła?

Widzieliście już najmniejszy moduł DIY z ESP32 i OLED?

Niezwykła stacja pogodowa DIY z dwoma ekranami e-ink

OpenMV – przetwarzanie obrazu na STM32

Intel Edison, czyli Internet Rzeczy dla każdego(?)

Git dla samotnych – czy to ma sens? Jak zacząć?

Jak zaimplementować regulator PID dla silnika z enkoderem?

alpha-beta, filtr, FreeRTOS, stm32