Skoro już zacząłem przygodę z filtrem Kalmana, dalszą konsekwencją jest zrobienie przynajmniej dwóch kluczowych filtrów: kątów orientacji, a następnie filtra pozycji w przestrzeni 3D. Obecnie kąty Eulera liczone są dwiema metodami. Pierwsza z nich to najprostszy z możliwych filtr komplementarny, łączący całki prędkości kątowych żyroskopów z kątami obliczonymi metodami trygonometrycznymi z wektorów przyspieszenia i magnetyzmu w proporcji 98% kąta z całki oraz 2% kąta z trygonometrii. Druga metoda liczenia kątów oparta jest na rachunku kwaternionów. W przeciwieństwie do pierwszej jest nieintuicyjna, pewnie dlatego, że w szkole nie uczono nas o kwaternionach, ale bardziej elastyczna, bez miejsc o nieoznaczonej pozycji i prostsza obliczeniowo.
Obie metody liczą kąty z danych, jakie zostaną im dostarczone. Jeżeli dane są złe, bo np. żyroskopy nie trzymają zera albo magnetometr jest źle skalibrowany, to wszystkie te błędy przenoszą się na obliczane kąty orientacji. W pewnym zakresie wady tej nie ma filtr Kalmana, który potrafi na bieżąco estymować niektóre błędy czujnika i usuwać je z pomiaru. W liczonym niedawno filtrze wysokości estymowany był błąd wskazania wysokości obu czujników ciśnienia. Dodatkowo filtr Kalmana potrafi przyjmować dane z wielu czujników, a w APL3 podstawowe czujniki są zdublowane, więc aż prosi się o implementację tej metody. Ze wszystkich wymienionych metod wymaga ona największej mocy obliczeniowej, ale tę mamy dostępną w dużych ilościach. Mając kilka wersji obliczonych kątów, można je ze sobą porównać, próbować ustalić, dlaczego się różnią i poprawić przyczynę problemu. Przykładowo dosyć długo miałem ewidentną różnicę między kątem liczonym filtrem komplementarnym a kątem liczonym kwaternionami. To było 0,8 stopnia w przechyleniu i 0,11 stopnia w pochyleniu. Okazało się, że jedna metoda liczyła z pierwszego zestawu czujników, a druga z drugiego i widocznie takie są błędy ręcznego montażu akcelerometrów.
Budując filtr, okazało się, że to nie jest problem liniowy i wymagany jest filtr rozszerzony, czyli EKF. O ile wydaje mi się, że rozumiem działanie filtra liniowego, to przy konstrukcji EKF natrafiam na ścianę w postaci jakobianów. Na obecnym etapie nie potrafię tego zrozumieć. Na szczęście jest wiele przykładów, jest sztuczna inteligencja, która potrafi sporo podpowiedzieć. Podpierając się w taki sposób, dałem się zaprowadzić do celu, który sprawia wrażenie, że działa. Filtr na obecnym etapie działa na jednym komplecie czujników i estymuje błędy prędkości kątowych żyroskopów. Nie potrafi jeszcze poprawić błędu położenia zera magnetometru. Próbowałem zrobić dodatkowy filtr dla samych magnetometrów, który obraca wektorem magnetycznym z prędkościami kątowymi odczytanymi z żyroskopów, a aktualizacja bieżącymi odczytami magnetometrów kasuje dryft prędkości żyroskopu. Jednak utknąłem na problemie polegającym na tym, że nie wiadomo, jaka część sygnału jest błędem położenia zera, a jaka właściwym pomiarem. Przy pomiarze wysokości były jeszcze powiązane pomiary prędkości i przyspieszenia, a w magnetyzmie trudno wyobrazić sobie pochodną pola magnetycznego, a tym bardziej ją zmierzyć, więc brakuje danych. Wydaje się, że tutaj jest problem niejednoznaczności. Zarzuciłem filtr magnetometrów i wróciłem do filtra kątów.
O ile nie ma większych problemów z liczeniem kątów pochylenia i przechylenia, a wykresy rysowane z danych telemetrycznych praktycznie się pokrywają, o tyle kąt odchylenia korygowany magnetometrem sprawia mnóstwo problemów. Wynikają one co najmniej z kalibracji samego magnetometru. Odświeżyłem stare metody kalibracji. Zastosowałem nową metodę dopasowania końca wektora do sfery, która chodziła mi po głowie od dosyć dawna. O ile już dawno temu potrafiłem dopasować dane do prostej, o tyle ze sferą nie potrafiłem sobie poradzić. Dostępne dzisiaj źródła podpowiedziały macierzową metodę obliczeń, która dosyć łatwo daje się dopasować do dowolnego wzoru. Podejrzewam, że taką metodą arkusze kalkulacyjne wykonują dopasowanie jednej z kilku typowych funkcji do zbioru danych. Metoda wydaje się działać, chociaż koryguje tylko położenie wektora magnetycznego względem środka sfery. Na początku wydawało się, że to powinno wystarczyć, bo do określenia kąta potrzebujemy znać proporcje składowych wektora magnetycznego, a nie jego bezwzględną wartość. Jednak wciąż występuje problem, prawdopodobnie spowodowany zniekształceniami soft iron, które selektywnie odkształcając pole magnetyczne, zmieniają proporcje między składowymi XYZ wektora, generując błędy wyliczenia kierunku magnetycznego. Objawia się to dziwnym zachowaniem estymowanego błędu żyroskopu R (mierzącego prędkość kątową wokół osi Z). Czasami błąd jest niewielki, czasami całkiem duży i jest to związane z początkową orientacją czujników względem kierunków świata. Na początku ten problem był jeszcze większy, bo nie ustawiałem kąta początkowego. Dla algorytmu kąt początkowy wskazywał na północ, a autopilot stojący na biurku był skierowany na wschód. Teraz, zaczynając pracę filtra ustawiam wstępne położenie i błąd się zmniejszył, ale wciąż jest nieakceptowalnie duży.
Na obecnym etapie potrzebna jest znacznie lepsza kalibracja magnetometru, uwzględniająca macierz kompensacji błędów soft iron. Dowodem na to, że źródłem problemów jest soft iron, jest to, że w trakcie obrotu magnetometru ze skalibrowanym przesunięciem zera zmienia się długość wektora magnetycznego. Na poniższym obrazku pokazane są składowe obu magnetometrów na wykresach o wspólnej skali. Ostatni wykres pokazuje długość wektora magnetycznego, która powinna być stała, a zmienia się stanowczo zbyt mocno.
Mam już jakieś wzory na kalibrację, można zacząć to implementować, trzeba tylko sobie przypomnieć, jak się liczy pierwiastek macierzowy. Nie można też wykluczyć, że mam gdzieś błąd w obliczeniach kąta lub kalibracji magnetometru. Taki złożony problem jest trudny do oceny, gdyż przyczyn błędów może być kilka i usunięcie jednej nadal nie powoduje poprawy sytuacji.
Zmagając się z tym problemem, wpadłem na pomysł, aby zrobić symulator AHRS, czyli algorytm, który będzie symulował idealne dane czujników. Jeżeli takie dane podane do algorytmu liczącego kąty Eulera dadzą właściwy kąt, to znaczy, że algorytm jest poprawny. Następnie można sztucznie wprowadzać błędy pomiarów, np. skalować prędkości kątowe, przesuwać poziom zera każdego czujnika itp. Takie narzędzie pozwoliłoby w znacznie szerszym stopniu przetestować algorytm, bo testy na rzeczywistych czujnikach nie są tak elastyczne i pokazują tylko niewielki fragment rzeczywistości, z jaką można się spotkać w trakcie eksploatacji BSP. Symulator widzę jako dwa wektory: grawitacyjny, początkowo skierowany pionowo w dół, oraz magnetyczny skierowany na północ i wstępnie obrócony o kąt inklinacji wokół osi X i o kąt deklinacji wokół osi Z. Interfejs użytkownika musiałby mieć możliwość wprowadzenia trzech prędkości kątowych. Ich całki dadzą wartości kątów, o jakie będą obracane oba wektory za pomocą macierzy obrotu. Składowe XYZ wektorów to będą symulowane czujniki akcelerometru i magnetometru.