Spełniło się moje marzenie sprzed nastu lat, kiedy chciałem uzyskać lepszy pomiar, uśredniając dane z dwóch czujników. W APL2 istotnym problemem było utrzymanie wysokości. Kopter oscylował wokół zadanej wysokości i nie udawało się tego poprawić regulacją PID ani obróbką sygnałów z czujników. Zrobiłem nawet specjalny moduł ze sprzętowym wariometrem, aby regulator prędkości pionowej miał dobre dane. Próbowałem umieszczać czujniki ciśnienia po obu stronach płytki tak, aby mechaniczne oddziaływanie przyspieszenia w osi Z działało na membrany czujników w przeciwnych kierunkach, licząc na to że uśredniony sygnał będzie mniej zależny od wpływu przyspieszenia. Zacząłem nawet robić niepublikowany na stronie duży moduł, zajmujący 2 gniazda APL, zawierający 2 wariometry i 4 czujniki ciśnienia, po 2 z obu stron obwodu drukowanego. Oprócz tego moduł zawierał komplet czujników inercyjnych i mikrokontroler Cortex-M4, którego jedynym zadaniem było liczenie AHRS aby odciążyć kontroler główny. Nie wystarczyło mi czasu, aby go skończyć, ale wszystkie te próby były tylko półśrodkami. Uśrednienie wskazań dwóch czujników ciśnienia poprawiało sytuację w trudnym do oceny stopniu, ale raczej niewielkim. Wymagało to porządnej kalibracji wszystkich czujników przed taką fuzją. Dużo pomogło dołożenie czujnika ultradźwiękowego, dającego znacznie dokładniejszy pomiar, ale jego zasięg wynosił zaledwie około 1 m. Mimo to najtrudniejsze etapy lotu, czyli start i przyziemienie, miały dzięki temu czujnikowi solidne wsparcie, które powodowało, że wskazanie wysokości z czujników barometrycznych uzyskiwało kalibrację czujnikiem, którego wskazania nie były fałszowane poduszką powietrzną tworzącą się tuż przy ziemi na początkowym etapie lotu oraz przy lądowaniu. Podczas lądowania po dłuższym locie widać było w logach dociąganie wysokości, która w trakcie lotu zdążyła odpłynąć wraz z naturalnym dryftem ciśnienia.
Moim marzeniem było znaleźć taki algorytm, który połączy dane z wielu czujników, aby uzyskać lepszy pomiar. Właśnie to się udało dzięki filtrowi Kalmana. Obecnie używany moduł inercyjno-barometryczny ma w tym celu zdublowane wszystkie czujniki, a dodatkowo czujniki są różnych typów, więc ich błędy pomiaru są inne. Takie zróżnicowanie pozwala praktycznie ocenić jakość czujnika w zastosowaniu do pomiaru wysokości bezzałogowca, bo dane katalogowe nie powiedzą wszystkiego.
Rozbudowałem opracowany przed chwilą filtr Kalmana o 4 elementach wektora stanu x do 5, dodając dryft drugiego akcelerometru oraz rozbudowując wektor pomiaru z 3 do 6 elementów. Dosyć szybko okazało się, że wektor pomiaru z nie będzie miał 6 elementów, ale będą 4 wektory: dwa 2-elementowe, zawierające wysokość i prędkość dla czujników ciśnienia oraz dwa 1-elementowe dla przyspieszenia. Dzięki temu można wykonywać aktualizację każdego czujnika niezależnie, można je włączać lub wyłączać w trakcie pracy filtra.
Obecnie w module pomiarowym mam uszkodzony czujnik ciśnienia BMP581. Ogólnie jestem zawiedziony tym czujnikiem. Bardzo trudno się go lutuje, trzeba było poprawiać wiele razy, przez co długo jest grzany i ma kontakt z topnikiem. Otwory dostarczające powietrze są bardzo małe, więc łatwo o ich zatkanie resztkami topnika. To jest prawdopodobna przyczyna błędów jego działania. Na początku czujniki działały prawidłowo ale z czasem w obu modułach mają wadliwe wskazania. W pracy robiłem autopilot na tym czujniku i tam na zupełnie innym oprogramowaniu też jest z nim problem. Na początek do filtra włączam więc tylko jeden sprawny czujnik ciśnienia. Są za to dwa akcelerometry. W działaniu na testowym obiekcie wygląda to tak:
Wysokość jest estymowana bardzo ładnie. Nie mam żadnych zastrzeżeń. Sąsiad, który ze mną jechał na 8 piętro, też był pod wrażeniem.
Prędkość nie jest idealna, ale akceptowalna. Jeszcze nie próbowałem optymalizować wielkości filtra różniczkującego, więc jest jeszcze pole manewru do polepszenia pomiaru.
Przyspieszenie z obu czujników ma zauważalne przesuniecie. Czujnik 1 z czerwonym wykresem wskazuje troszkę wyżej. To może być wpływ kiedyś robionej kalibracji a w miedzy czasie moduły były zamieniane miedzy dwoma egzemplarzami APL3, więc możliwe jest że moduł pracuje z wartością kalibracji dla innego modułu, która psuje pomiar. Przy stosowaniu filtra Kalmana kalibracja poziomu zera w zasadzie nie jest potrzebna, więc bezpiecznie będzie można ją wyrzucić. Pozostanie tylko termostatowanie czujników.
Statyczna część przyspieszenia. Tak jak pisałem wcześniej zostawiam tutaj śladowe ilości przyspieszenia kinematycznego. Być może troszkę za dużo, ale to już są drobne korekty do trymowania w przyszłości. Widać że filtr stabilizuje się dosyć szybko. Prędkość logowania to obecnie ok. 66 Hz, więc stabilizuje się po ok. 20s.
Praca z wadliwym czujnikiem
Teraz kluczowa chwila. Jak zachowa się filtr, gdy dołożę do niego czujnik, który wskazuje wysokość różniącą się o 250 m i posiada tak duży dryft, że prawie nie widać ruchu windą?
Okazuje się że pomiar z wadliwego czujnika został włączony do estymaty, ale jego błąd został mocno stłumiony powodując przesuniecie estymaty o 5 m. To odpowiada stłumieniu błędu o ok. 50 razy. Bardzo mi się to podoba. Nawet uszkodzenie czujnika w locie nie powinno spowodować katastrofy. Dodatkowo konstrukcja filtra pozwala na wyłączenie wadliwego czujnika poprzez zaniechanie aktualizacji filtra. Można będzie pokusić się o algorytm, który wyłączy czujnik, gdy skojarzony z nim współczynnik wzmocnienia Kalmana spadnie poniżej pewnego progu. Można wtedy zasygnalizować wystąpienie błędu i zaświecić na panelu operatora symboliczną czerwoną kontrolkę.
Analogicznie wariometr. Co prawda z samej zasady działania różniczkowania nie jest podatny na dryft, ale widać kilka wyraźnych odchyleń wariometru 2 od przeciętnego poziomu, które nie mają swojego odzwierciedlenia w estymacie. Widać też błąd w liczeniu wariometru 2 — przy opadającym wskazaniu wysokości, wskazania prędkości powinny być ujemne. Już to zostało poprawione. Pojawiła się też jakaś szpilka zakłóceń w estymacie prędkości, bo wariometr 1 wygenerował szpilkę sięgajacą wartości -540 m/s. Czujniki nie powinny generować takich zakłóceń, ale filtr radzi sobie nawet z tym. Wykresów przyspieszenia już nie załaczam, bo nie ma tam nic więcej ponad to, co już było pokazane.
Kupiłem moduł z czujnikiem BMP585. Wkrótce go podłączę i oprogramuję. Wtedy zobaczymy, jak filtr pracuje z dwoma prawidłowo działającymi czujnikami. W dalszych planach jest włączenie do filtra wysokości z dwóch odbiorników GNSS oraz z dalmierza laserowego. Mam kupione scalone dalmierze VL53L1CX oraz VL53L5CX. Ich zasięg to pojedyncze metry, ale chciałbym, aby pełniły taką funkcję taką jak sonar w APL2 oraz pomagały w rozdzieleniu wysokości AGL od MSL. Odbiorniki GNSS będą korygowały wskazania wysokości MSL, a korektorem wysokości AGL i wspomaganiem lądowania będzie dalmierz. Swoją drogą bardzo jestem ciekaw, jak będą wpływały na wartość estymaty wysokości wskazania odbiorników GNSS wystawione na zakłócenia.
Gdyby ktoś chciał obejrzeć kod w repozytorium, to plik zawierający funkcje tego filtra nazywa się KalmanWysokosci5X6Z.c.
Po kilku dniach przyszedł czujnik, oprogramowałem go, ręcznie wyrównałem offset ciśnienia, tak że oba czujniki na wykresie telemetrii szły łeb w łeb. W filtrze podmieniłem dane z czujnika BMP581 na dane z BMP585 i wykonałem standardowy przejazd windą.
Po ponownym uruchomieniu czujniki rozjechały się, więc trzeba zrobić mechanizm usuwania biasu z obu wysokości MSL, analogicznie jak od pomiaru przyspieszenia odejmowana jest grawitacja. Obecnie czujnik ustawiony jest na uśrednianie 8 próbek. Daje bardzo ładny wykres, stabilniejszy niż czujnik 1, ale filtr nie chce go słuchać i podąża za wysokością MSL1. Podejrzewam, że przyczyną może być to uśrednianie. Do macierzy R filtra wpisałem takie same parametry statystyczne dla obu czujników, więc to, że sygnał odbiega od deklarowanych parametrów, też może być przyczyną odrzucenia. Współczynnik Kalmana z komórki odpowiedzialnej za włączanie MSL1 do wysokości to okolice 2,5e-3, a współczynnik dla MSL3 to 4,6e-5, czyli filtr ok. 200 razy bardziej preferuje MSL1.
Wykresy wariometrów. W międzyczasie trochę zwiększyłem filtr do liczenia wariometru z MSL1, więc przebieg jest mniej zaszumiony niż wcześniej. Estymata prędkości wcześniej była dosyć gładka, teraz jest lepiej skorelowana z pomiarem. Wariometr liczony z MSL3 analogicznie jak wysokość jest bardzo ładny, ale filtr słucha wariometru 1, a nie 3.
Nie dawało mi spokoju, dlaczego filtr nie lubi nowego czujnika, więc w pierwszym podejściu, jeszcze nie zmieniając parametrów pracy samego czujnika, zmierzyłem wariancję danych ze wszystkich czujników, ze statycznego fragmentu wykresu i zaktualizowałem wszystkie 4 macierze R. Zmieniłem też sposób inicjalizacji filtra. Zamiast inicjować go pierwszym czujnikiem, inicjalizuję go średnią z obu. Okazało się, że to był strzał w dziesiątkę. Estymowana wysokość znajduje się pomiędzy pomiarami, bliżej gładszego MSL3. Współczynniki Kalmana to odpowiednio 5,2e-4 dla MSL1 i 6,5e-3 dla MSL3, co potwierdza, że teraz bardziej lubi MSL3, ale tylko o rząd wielkości bardziej niż MSL1.
Estymata prędkości też polubiła wariometr 3. Wstrzeliwuje się w jego środek i nie widać aby chciała uciec z obwiedni wyznaczonej przez szum tego czujnika a wcześniej chętnie uciekała.
