Autopilot może być wyposażony w kilka magnetometrów. Dwa magnetometry są umieszczone na module inercyjnym IMU I2P, jeden może być umieszczony na zewnętrznym module GNSS oraz potencjalnie jeden w IMU kamery. Tego w kamerze jeszcze fizycznie nie ma, więc spróbujmy skalibrować 3 dostępne czujniki. Obecnie dostępne są trzy procedury obsługi magnetometrów różniące się sposobem wykonania i rodzajem zasilających je danych. Zostaną one omówione poniżej, ale jako wstęp powiedzmy kilka słów o samym pomiarze.
Procedura pomiaru magnetometrem
Typowy czujnik ma dostępny zestaw poleceń umożliwiających wykonanie pomiaru, odczyt statusu czujnika mówiącego o gotowości danych oraz odczyt zmierzonych danych. Praktycznie wszystkie czujniki magnetometrów komunikują się przez dosyć wolną magistralę I2C. Jeden a czujników IIS2MDS potrafi komunikować się po SPI, ale w nietypowej konfiguracji, więc ten sposób odpada. Czujniki wewnętrzne na module I2P mają dosyć krótką magistralę o stałej długości i pracują na prędkości 100 kHz, natomiast czujniki zewnętrzne ze względu na możliwość zastosowania dłuższego przewodu oraz podatność na zewnętrzne zakłócenia pracują na 25 kHz. Wysłanie najkrótszego polecenia składającego się z adresu czujnika i kodu polecenia (z bitem START lub STOP i ACK zawierają po 10 bitów) trwa 200us dla wewnętrznej magistrali i 800us dla zewnętrznej. To trwa bardzo dużo czasu, więc w autopilocie, który jest krytyczny czasowo, nie można sobie pozwolić na uruchomienie wszystkich poleceń jednym ciągiem. Operacja musi zostać podzielona na poszczególne elementy i realizowana sekwencyjnie. Samo wysłanie polecenia zajmuje procesorowi nanosekundy, po czym sprzętowo sterowana transmisja jest realizowana automatycznie, aż do jej zakończenia, a procesor w tym czasie może realizować inne zadania.
Najprostsza sekwencja pomiarowa wygląda tak:
- Wysłanie polecenia rozpoczynającego pomiar.
- Wysłanie polecenia odczytu statusu.
- Odczyt statusu.
- Sprawdzenie statusu. W przypadku braku gotowości powrót do punktu 2, zwykle jest gotowość, więc następuje wysłanie polecenia odczytu danych.
- Odczyt danych pomiarowych.
Ostatnio nauczyłem się jak można automatycznie połączyć punkty 2 i 3 oraz 4 i 5, bo to są transmisje z powtórzonym START-em, więc po zakończeniu jednej można automatycznie, jeszcze w tym samym obiegu pętli głównej wysłać drugą transmisję. Taka sekwencja zajmuje 3 obiegi pętli głównej. Niektóre czujniki mają tryb pracy ciągłej, więc może odpaść punkt rozpoczynania pomiaru, więc sekwencja skraca się do dwu punktów, a uruchamiana z częstotliwością 200 Hz zwraca wyniki pomiaru z częstotliwością 100 Hz.
Czujniki firmy Memsic i STM mają funkcjonalność, która pozwala na przemagnesowanie struktury pomiarowej. Każdy z czujników osi XYZ ma cewkę, przez którą może przepuścić prąd powodujący powstanie pola magnetycznego przemagnesowującego czujnik. Prąd można przepuścić w jedną lub drugą stronę, przemagnesowując każdy z czujników danej osi w dwie strony. Jeżeli przemagnesujemy czujnik i wykonamy pomiar, to zarejestrujemy natężenie zewnętrznego pola magnetycznego oraz natężenie pola własnego czujnika.
H pomiaru+ = H ziemi + H czujnika+
Jeżeli przemagnesujemy czujnik w drugą stronę i wykonany kolejny pomiar to zarejestrujemy:
H pomiaru- = H ziemi + H czujnika-
Jeżeli dodamy te pomiary do siebie, to przeciwnie skierowane pola czujnika zrównoważą się.
H ziemi = (H pomiaru+ + H pomiaru-) / 2
W ten sposób czujnik staje się odporny ma wstępne namagnesowanie. Wadą takiego rozwiązania jest znacznie bardziej skomplikowana procedura obsługi, co przekłada się na znacznie dłuższą sekwencję pomiarową. Zanim opiszę tę sekwencję należy jeszcze dodać, że aby przepuścić odpowiednio duży prąd przemagnesowujący przez cewkę o niezerowej rezystancji, potrzebne jest odpowiednio wysokie napięcie. Czujnik wytwarza je pompą ładunkową, zbierając energię w zewnętrznym kondensatorze. Naładowanie tego kondensatora trochę trwa. Tak wiec sekwencja pomiarowa z przemagnesowaniem struktury wygląda tak:
- Wysłanie polecenia ładowania kondensatora i ustawienie licznika czasu ładowania na katalogowe 50ms (10 obiegów pętli głównej).
- Dekrementacja licznika czasu ładowania. Jeżeli upłynął już wymagany czas, można przejść do kolejnego punktu.
- Wysłanie polecenia SET powodującego przemagnesowanie struktury w jedną stronę.
- Wysłanie polecenie wykonania pomiaru.
- Wysłanie połączonych ramek polecenia odczytu statusu i rozpoczęcie odbieranie statusu.
- Gdy odebrany status informuje, że pomiar nie jest jeszcze gotowy to wracamy poprzedniego punktu. Gdy jest gotowy, następuje wysłanie połączonych ramek polecenia odczytu danych i odebranie danych pomiarowych.
- to samo, co w punkcie 1
- to samo, co w punkcie 2
- Wysłanie polecenia RESET powodującego przemagnesowanie struktury w przeciwną stronę
- to samo, co w punkcie 4
- to samo, co w punkcie 5
- to samo, co w punkcie 6
Ten pomiar zajmuje aż 12 etapów, a punkty 2 i 8 zajmują po 10 obiegów pętli głównej, więc łącznie jest to 30 obiegów pętli głównej. Jednak cząstkowy wynik jest już odczytywany w punkcie 6 (oraz 12) co daje częstotliwość aktualizacji pomiaru 13,(3) Hz. Na etapie implementacji pomiaru trudno jest stwierdzić, czy tak pomiar ma sens, czyli czy utrata prędkości zostanie zrekompensowana zwyżką jakości pomiaru.
Czujnik IIS2MDC też ma funkcjonalność przemagnesowywania swojej struktury. Nazywa się to "offset cancellation" i jest robione automatycznie przez sam czujnik, więc nie wymaga takiego nakładu pracy procesora. Dodatkowo ten czujnik ma możliwość pracy w trybie ciągłym, więc nie ma potrzeby wyzwalania pomiaru za każdym razem, jedynie odczytywana jest wartość statusu oraz danych, przez co cykl pracy skraca się do dwóch cykli, pozwalając osiągnąć częstotliwość aktualizacji pomiaru 100 Hz.
Po pewnym czasie udało mi się zmodyfikować sekwencję przemagnesowywania czujnika MMC34160, tak aby przemagnesowanie nie było wykonywane co każdy pomiar, ale okresowo, co 200 pomiarów. Wymagało po policzenia i zapamiętania wartości H czujnika+ i H czujnika-. Do obliczenia pola magnetycznego ziemi używam wzorów:
H ziemi = H pomiaru+ - H czujnika+
H ziemi = H pomiaru- + H czujnika-
Sekwencja pomiaru jest przedstawiona na zielonym wykresie. Są to serie po 200 cykli wysłania polecenia pomiaru, odczytu statusu i odczytu pomiaru. Pomiędzy nimi są polecenia ładowania kondensatora, oczekiwania na naładowanie oraz polecenia SET i RESET. W tle wykresu wyniki pomiaru dla 3 osi. Dla takiej procedury częstotliwość aktualizacji wzrasta do 66,(6) Hz z okresowymi spowolnieniami do 13,(3) Hz w czasie przemagnesowania.
Na tym etapie warto zastanowić się nad szumem pomiaru. Tak uzyskany pomiar jest odniesiony do pola własnego czujnika: H czujnika+ lub H czujnika-. Wartości te uzyskiwane są z pomiarów, czyli też są zaszumione. Wynik dodawania lub odejmowania dwóch zaszumionych pomiarów daje w efekcie podwójną wartość szumu, więc warto było by pozbyć się chociaż jego części. W trakcie lotu, wartość pomiaru zmienia się wraz z położeniem czujnika w przestrzeni 3D. Filtrowanie pomiaru opóźnia nam odpowiedź, więc nie wchodzi w rachubę, natomiast pole własne czujnika nie powinno się zmieniać, więc jego wyliczoną wartość można spróbować filtrować. Wartości H czujnika+ lub H czujnika- mają tą samą wartość bezwzględną, różnią się tylko znakiem, więc można sprowadzić je do modułu wartości, a znak uwzględniać w obliczeniach danej sekwencji pomiarowej. Wypróbowałem kilka wartości filtrów IIR starając się dobrać je tak, aby szum pola własnego czujnika nie przekraczał połowy bitu rozdzielczości czujnika. Tutaj pojawia się szereg spostrzeżeń.
Przede wszystkim okres przemagnesowywania nie powinien być zbyt długi, ponieważ nowy pomiar pola czujnika uzyskujemy po przemagnesowaniu jego struktury, więc im rzadziej to robimy, tym dłużej trzeba czekać na ustabilizowanie się filtra. Pierwotnie definiowałem przemagnesowanie co 200 cykli, ale wróciłem do 50 cykli. Dla filtra 1/16 (15 wartości poprzednich + jedna nowa dzielone przez 16) czas ustalania się wartości to około 6 minut. Stanowczo za długo. Przyjąłem filtr 1/8. Jego odpowiedź widać na poniższym wykresie. Zielony wykres, to jest przebieg sekwencji pomiarowej, oś X jest wyrażona w sekundach. Pozostałe wykresy to natężenie pola przemagnesowania czujnika. Wartości pola monotonicznie nasycają się do około 180 sekund, potem widać normalną pracę filtra.
Stosowanie mniejszych wartości filtra skraca czas ustabilizowania się odpowiedzi filtra, ale słabiej filtruje szum.
Na poniższym wykresie jest pokazany rozkład szumu dla ustabilizowanego filtra 1/8. Dla zwiększenia przejrzystości wyłączyłem pokazywanie sekwencji pomiarowej. Wartości są wyrażone w jednostkach natywnych czujnika, jeszcze przed przeliczeniem na Tesle. Wartość 1 na skali Y, to jest 1 bit rozdzielczości pomiaru. Widać, że w zaledwie kilku przypadkach zmiana filtrowanego natężenia pola czujnika jest większa niż połowa bitu.
Warto porównać tak uzyskany pomiar z innymi. Dobrze to widać na wykresie do testowania magnetometru przy obrocie 2D. Tam jest rysowana linia między kolejnymi pomiarami. Jeżeli czujnik nie porusza się, to wykres widzi tylko jego szum, rysując chmurę linii na końcu wektora początkowego. Wykres czujnika MMC34160 jest fioletowy. Ma minimalnie większy szum od czujnika żółtego HMC5883, który ma bezpośredni pomiar bez przemagnesowywania. Wykres błękitny pochodzi od IIS2MDC, który ma przemagnesowanie sprzętowe i prawdopodobnie nie ma filtrowania pola czujnika, więc wartość szumu jest około wyraźnie większa od tego, co udało się osiągnąć. Wartość szumu należy traktować jako proporcję kąta uzyskanego przez linie idące od środka okręgu do skrajnych wartości chmury punktu. Nie mam narzędzia to pomiaru kąta szumu, ale szacuję jago wartość na 2° dla czujników żółtego i fioletowego, i ok. 5° dla czujnika błękitnego.
Istotny jest jeszcze rozkład szumu w czasie. Szum na wykresie kumulował się przez około 5 minut. Trudno to pokazać na statycznym obrazie, ale dla błękitnego IIS2MDC szum przyrasta najszybciej. Szum pozostałych czujników dobrze oddaje angielska nazwa "random walk", czyli powolne dryfowanie.
Procedura kalibracji przesunięcia zera i skalowania długości wektora magnetycznego
Jest to podstawowa procedura kalibracyjna. Jest dostępna w menu kalibracji magnetometrów pod nazwą "Kal Magn x", gdzie x to numer czujnika. Czujnik 1 (IIS2MDC), oraz 2 (MMC34160) to czujniki wbudowane w IMU, czujnik 3 (HMC5883L) jest umieszczony na zewnętrznym module GNSS.
Po uruchomieniu pokazuje się ekran z instrukcją przeprowadzenia kalibracji. Należy ustawić się z bezzałogowcem tak, aby kalibrowana oś (pierwsza jest X) była umieszczona wzdłuż linii wschód - zachód, czyli była w zerze pola magnetycznego. Obracając wokół niej, uzyskujemy pełną zmianę wskazań na pozostałych 2 osiach. Procedura potrzebuje znaleźć ekstrema, czyli minima i maksima. Znalezienie nowego ekstremum jest potwierdzane komunikatem głosowym złożonym z dwu wyrazów. Pierwszym wyrazem jest rodzaj ekstremum "Min" lub "Max", drugim wyrazem jest nazwa osi. W pierwszym etapie kalibracji jest lawina komunikatów, która później się ucisza. Jeżeli znajdziemy nowe ekstremum warto przeszukać jego okolice, czy nie będzie czegoś jeszcze większego. Gdy nie ma już nic nowego, wtedy należy przejść do kalibracji kolejnej osi, naciskając przycisk "Nastepna Os".
Obrót czujników dookoła każdej z osi jest wizualizowany na wykresie biegunowym standardowymi kolorami RGB. Kolor czerwony (R) rysuje wykres YZ podczas obrotu wokół osi X, kolor zielony (G) rysuje wykres XZ podczas obrotu wokół Y, kolor niebieski (B) rysuje wykres XY. Wykresy są automatycznie skalowane, tak aby mieściły się w oknie. Im większe będą różnice między osiami, tym wykresy będą mniejsze. Skalowanie dzieje się automatycznie wraz z postępem pomiaru, więc pierwsze pomiary rysują wykres większy, który dosyć szybko się zmniejsza. Gdyby to było rysowane na dużym komputerze, to należało by przechować w pamięci wszystkie pomiary i przerysowywać wszystko po zmianie współczynnika skalowania. To było by eleganckie rozwiązanie, ale na mikrokontrolerze jest zbyt kosztowne, więc dane nie są przechowywane, tylko rysowane na bieżąco, więc znajdowanie kolejnych ekstremów powoduje, że wykresy stają się coraz mniejsze, bo te wcześniej narysowane nie mogą być już zmniejszone. Na poniższym obrazku widać, że wykres R jest dużo większy niż pozostałe, ponieważ w czasie jego rysowania nie były znane ekstrema w osi X. Pierwszy przebieg wykresu G też jest większy, aż zacieśni się do nominalnego rozmiaru. Wykres B zaczynamy rysować, znając już wszystkie ekstrema, wiec jego średnica się nie zmienia. Czasami pojawi się tam nowe ekstremum, ale to jest już drobna korekta, niezauważalna na wykresie w tej skali. Problem skalowania jest wyłącznie wizualny. Właściwa kalibracja jest wykonywana po zebraniu wszystkich danych.
Po zebraniu kompletu danych dla każdej osi, obliczane są współczynniki przesunięcia zera:
fPrzesMagn = (Max + Min) / 2
Liczony jest też współczynnik skalowania, tak aby wektor mierzony przez wszystkie osie magnetometru miał tą samą długość:
fSkaloMagn = NOMINALNE_MAGN / (Max + Min)
Wartość wyrażona stałą NOMINALNE_MAGN przedstawia nominalne natężenie ziemskiego pola magnetycznego i jest podane w Teslach. Przyjęto wartość 46 uT. Znajomość długości wektora nie jest istotna dla odczytu kursu magnetycznego, ale będzie potrzebna do oceny wartości zakłóceń magnetycznych w procedurze aktualizacji danych dla AHRS. Jeżeli długość wektora będzie zbytnio obiegała od normy, to taki pomiar nie będzie wykorzystywany do korekty pozycji modelu wektora magnetycznego.
Obliczone współczynniki kalibracyjne są zapisywane w pamięci FRAM i pobierane przy starcie urządzenia.
Procedura sprawdzenia jakości kalibracji
Procedura wykorzystuje ten sam algorytm co kalibracja z tą różnicą, że jest zasilana danymi po kalibracji. Dla dobrze skalibrowanych czujników i braku zakłóceń, wykresy poszczególnych osi powinny pokrywać się w środku obszaru wykresu. O ile na etapie kalibracji wykresy były skalowane dynamicznie i okręgi zmniejszały się w miarę wykrywania kolejnych ekstremów, to na tym etapie współczynnik skalowania jest stały i dobrany, tak aby szerokość wykresu odpowiadała nominalnemu natężeniu pola magnetycznego. Wykres okręgów w 3 płaszczyznach powinien w pełni zawierać się w kwadratowym obszarze wykresu.
Procedura porównywania czujników
Magnetometr służy do pomiaru kursu magnetycznego. Wyznacza się go jako arkus tangens składowych X i Y magnetometru, więc płaszczyzna XY jest najważniejsza dla uzyskania finalnego wyniku. O ile okręgi rysowane w poprzedniej procedurze miały promień równy długości wektora, ponieważ oś obrotu zawsze leżała w kierunku wschód - zachód, czyli w zerze i rejestrowana była pełna długość wektora, to w locie pomiar wykonywany jest dla pionowej osi obrotu. Wektor magnetyczny jest wtedy widziany jako nachylony pod kątem inklinacji wynoszącym dla centralnej Polski ok. 68°, więc na wykresie pojawia się jego składowa o wartości wynoszącej kosinus kąta inklinacji.
Ponieważ kąt kursu to atan2(X,Y), więc zależy nam na tym, aby dodatnia i ujemna część osi X i Y były symetryczne. Na wykresie objawia się tym, że okrąg jest wycentrowany na wykresie. Jeżeli nie będzie wycentrowany to rozkład kątów zwracanych przez atan2 będzie niesymetryczny. Niesymetryczność zależy od dokładności kalibracji, oraz od pochylenia i przechylenia.
Żeby lepiej to zrozumieć, wyobraźmy sobie, że obracając nasze urządzenie, widzimy wektor magnetyczny we wszystkich możliwych kierunkach. Gdybyśmy zrobili wizualizację 3D, to na wyświetlaczu została by narysowana sfera. Jeżeli teraz zamiast dowolnych obrotów w przestrzeni będziemy wykonywali płaski obrót, to wektor umieszczony pod kątem inklinacji narysuje nam okrąg o promieniu równym:
cos(kąt_inklinacji) ≈ 0,37 promienia sfery.
Jeżeli sferę wyobrazimy sobie jako piłkę nożną, a wykresem z płaskiego obrotu jest jedna z czarnych łatek, to przekręcając piłkę do przodu wykres będzie przesuwał do góry, przekręcając w prawo, będzie się przesuwał w prawo itd. Dodatkowo przekręcają naszą "piłkę" okrąg wykres oprócz przesunięcia, będzie stawał się coraz bardziej eliptyczny.
Tak więc aby poprawnie wyznaczyć kąt kursu magnetycznego należy korygować pochylenie i przechylenie, a jeszcze wcześniej mieć pewność, że sama procedura kalibracji daje nam dobry materiał wejściowy. Do oceny tego materiału zostało zbudowane kolejne narzędzie przedstawiające wykres wektora magnetycznego w płaskim obrocie. W odróżnieniu od poprzednich wykresów tutaj mamy zaznaczone kierunki magnetyczne N, E, S i W. Trochę inaczej niż na mapie, ale wydaje mi się, że logicznie. Jeżeli skierujemy urządzenie osią X, czyli kierunkiem lotu na północ, to magnetometr zarejestruje najsilniejszy sygnał w dodatniej części osi X prostokątnego układu współrzędnych. Dlatego N leży na dodatniej części osi X. Obracając na wschód, maksimum sygnały magnetycznego będzie na lewym skrzydle, czyli ujemnej części osi Y, dlatego wschód jest na dole, itd.
Ponieważ w obecnej chwili APL jest wyposażony w 3 magnetometry, więc rysuję wykresy dla ich wszystkich kolorami CMY. Na powyższym obrazku widać kilka problemów. Jest pewna eliptyczność zewnętrznego magnetometru 3. To sygnalizuje, że czujnik jest wstępnie namagnesowany, a nie mamy narzędzi do jego przemagnesowania (są dostępne tyko dla czujników 1 i 2). Najlepiej wycentrowany jest magnetometr 1, ale on ma z nich wszystkich największy szum. Na wykresie jest okrąg wzorcowy o promieniu wyznaczonym ze znormalizowanej długości wektora i bieżącej inklinacji. Wszystkie 3 wykresy są trochę większe. Dla atan2(x, y) rozmiar nie ma znaczenia, ale jest to informacja, że długość wektora jest zakłócona. W tym przypadku jest to spowodowane tym, że pomiary wykonywane są w żelbetowym budynku, gdyż takie swego czasu na warszawskiej Woli budowała Wielka Lechia. Pomiary przeprowadzone na otwartej przestrzeni pokazane na poniższym obrazku, praktycznie leżą na okręgu wzorcowym. Wykres jest trochę bardziej nierówny, gdyż na otwartej przestrzeni trudniej jest o dobrze wypoziomowaną płaszczyznę, na której można wykonać pomiar, ale średnia odległość od promienia wzorcowego jest wyraźnie mniejsza niż po kalibracji w budynku.
Na tym etapie można już wstępnie ocenić, czy rozbudowana procedura pomiarowa czujnika 2 przynosi jakieś rezultaty. Wykres nie jest wycentrowany, ale to wpływ pochylenia i przechylenia, więc sprawa niezależna. Być może trzeba to będzie osobno kompensować dla każdego czujnika. Natomiast pomiar jest gładki, co może być efektem uśredniania pomiarów H+ i H-. Kolistość wykresu, czyli obecność problemów typu hard i soft iron, w tej skali nie budzi zastrzeżeń. Warto było by wrzucić pomiar na duży ekran komputera, ale takie oprogramowanie jeszcze nie powstało. Utrata prędkości pomiaru nie jest problemem w przypadku AHRS, gdyż wektor pola magnetycznego jest aktualizowany w każdym obiegu pętli głównej przez obrót o kąt będący całką z odczytu żyroskopów. Okresowo, gdy pojawi się nowy pomiar, położenie wektora jest korygowane danymi pomiarowymi. Tak więc nawet jeżeli UAV wykonuje szybki manewr, to w każdej chwili znamy dokładne położenie wektora magnetycznego, z którego można będzie odczytać to, co będzie potrzebne algorytmom stabilizacji lotu. Tak więc warto poświęcić prędkość odczytu, jeżeli zyskujemy na jakości. Dodam tylko, że wykresy prezentowane powyżej nie pochodzą z AHRS, tylko z żywych danych pomiarowych, prosto z czujników.
Do pełni szczęścia brakuje jeszcze kompensacji pochylenia i przechylenia oraz kompensacji odchylenia o kąt deklinacji, tak aby północ magnetyczna mogła pokryć się z północą geograficzną. Tutaj jest trochę sprzecznych zależności. Aby dokończyć AHRS potrzeba dobrze działającego magnetometru, ażeby dokończyć magnetometr potrzebne są dobrze działające kąty z AHRS.
Kompensacja pochylenia i przechylenia
Znalazłem wolną chwilę i zacząłem robić kompensację. Chodzi o to, że aby odczytać poprawną informację o kursie, magnetometr musi leżeć poziomo. Nasz bezzałogowiec w locie nie będzie zawsze w takiej pozycji, więc magnetometr byłby mało użyteczny, ale skoro korzystając z kwaternionów potrafimy obracać wektorem w przestrzeni 3D i AHRS mówi nam, o jaki kąt jesteśmy obróceni, to spróbujmy odwrócić masz magnetometr o te same kąty w przeciwną stronę, tak aby wektor magnetyczny dla celów odczytu pozostał w płaszczyźnie ziemi. Korzystając z kursów Mateusza Kowalskiego na YT wiemy, że obrót w przeciwną stronę można uzyskać zmieniając kolejność operacji mnożenia kwaternionów. Sprawdziłem i daje to taki sam efekt, jak zmiana znaku kąta, o jaki obracamy. Wiemy też, że kwaternion obraca się o podwójną wartość kąta, więc jeżeli obrócimy go o połowę wymaganego kąta to powinno być OK. Na obrazku poniżej jest wykres składowej X kwaternionu magnetometru fQmag[1] (wykres żółty) i składowej X kwaternionu fQMagKompens[1] po kompensacji pochylenia (wykres czerwony). Wykres jest zarejestrowany podczas zmiany kąta pochylenia o ok. +-20° od poziomu. Widać dwie fazy procesu. Podczas zmiany kąta wykres kompensowany zmienia się w przeciwną stronę, ale w fazie ustalonej wykres stabilizuje się na stałej wartości, takiej, jak przed obrotem. To sugeruje, że kompensacja działa. Wykresy nie pokrywają się, ale trzeba pamiętać, że to narzędzie rysuje wykresy, gdzie zakres jest ustalany automatycznie, więc zera też są przesunięte o taką samą wartość.
Wykres przedstawia kompensacje o jeden z kątów, o pochylenie. Wiemy też, że obroty można składać, czyli tworzę kwaternion obrotu o ujemne pochylenie i kwaternion obrotu o ujemne przechylenie. Mnożę je przez siebie, uzyskując kwaternion obrotu o oba te kąty i nim obracam wektor magnetyczny.
Dla zmiany przechylenia praktycznie nie widać zmiany dla składowej X magnetometru, więc przełączyłem się na podgląd składowej Y, czyli wartości w kwaternionie pod indeksem 2. Poniższy wykres przedstawia kompensację obu kątów: pochylenia i przechylenia. Wygląda na to, że złożenie obrotu wprowadza jakąś zmianę, ponieważ tym razem musiałem kompensować o dodatni kąt pochylenia, gdyż ujemny zamiast kompensować, wzmacniał wpływ przechylenia. Dla przechylenia jest analogicznie jak wcześniej. Są dwie fazy: zmiany i ustalona. Widać też, że składowa Y nie reaguje na pochylenie. Wcześniej składowa X nie reagowała na przechylenie.
Dobrze było by zrozumieć, dlaczego mamy fazę nieustaloną, bo przecież lot będzie jedną, wielką fazą nieustaloną. Wygląda to tak, jakby obracanie wektora żyroskopami było robione o niewłaściwy (podwójny?) kąt i dopiero faza korekcji AHRS ściąga wykres do właściwej wartości. Sprawdziłem jak proces zachowa się gdy podam mu korektę od żyroskopów z kątem zmniejszonym o połowę oraz z kątem o ujemnym znaku, ale to nie jest to. To musi być problem obliczeń AHRS.
Ten problem można było by obejść, rozbudowując filtr komplementarny. Obecnie filtr reaguje na długość wektora magnetometru. Jeżeli dołożyć mu dynamikę zmiany kąta, tak aby pozwalał na kompensację kąta z żyroskopu tylko w fazie ustalonej to problem byłby rozwiązany.