Łączenie danych żyroskopu i akcelerometru

Buduję robota równoważącego używając systemu Nxt Lego Mindstorm. Używam dwóch czujników firmy HiTechnic, pierwszy to akcelerometr, a drugi Żyroskop. Udało mi się odfiltrować szum z obu czujników i kątów wyprowadzenia dla obu w zakresie od -90 do 90 stopni, przy czym 0 stopni jest idealnie zrównoważone.

Moim kolejnym wyzwaniem jest połączenie obu wartości czujnika, aby skorygować dryf żyroskopu w czasie. Poniżej przykładowy wykres I stworzony na podstawie rzeczywistych danych, aby zademonstrować dryf z żyroskopu:

Najczęściej stosowanym podejściem do łączenia tych czujników jest użycie filtra Kalmana. Jednak nie jestem ekspertem w rachunku i naprawdę nie rozumiem symboli matematycznych, ale rozumiem matematykę w kodzie źródłowym.

Używam RobotC (który jest jak każda inna pochodna C) i byłbym naprawdę wdzięczny, gdyby ktoś mógł podać mi przykłady jak wykonać to w C.

Dziękuję za pomoc!

WYNIKI ROZWIĄZANIA:

Dobra, kersny rozwiązał mój problem wprowadzając mnie do filtrów uzupełniających. To jest wykres ilustrujący moje wyniki:

wynik #1

wynik #2

Jak widać, filtr koryguje dryf żyroskopowy i łączy oba sygnały w jeden gładki sygnał.

Edit: ponieważ naprawiałem tak czy inaczej, pomyślałem, że fajnie będzie pokazać platformę, której użyłem do wygenerowania tych danych: