Jeszcze za czasów rozwoju APL2 próbowałem zbudować filtr Kalmana i na podstawie kilku przykładów praktycznie udało mi się uruchomić filtr wysokości, wariometru i przyspieszenia w osi Z. Już nie pamiętam szczegółów, ale sprawiało to wrażenie działania, chociaż było dalekie od oczekiwań. Wtedy nie byłem w stanie zrozumieć, jak to działa, a bez zrozumienia trudno jest dostosować się do konkretnego zastosowania, więc tamte próby traktuję jako ćwiczenia praktyczne. Pomimo intensywnych poszukiwań nie udało mi się wtedy znaleźć opracowania, które by w sposób dla mnie zrozumiały tłumaczyło podstawy działania filtra.
W czasie tegorocznego urlopu, gdy przeczytałem już wszystkie zabrane na wypoczynek książki, przejrzałem sieć pod kątem literatury dotyczącej filtra i znalazłem świetne opracowanie Alexa Beckera „Kalman Filter from the Ground Up”. Pod linkiem znajduje się strona typu „landing page” reklamująca książkę, zawierająca kilka podstawowych rozdziałów. Polecam jej lekturę, bo genialnie opisuje zasadę działania, zaczynając od prostych przykładów, stopniowo wprowadzając kolejne zagadnienia i wyprowadzając wzory. Książka już do mnie jedzie, ale informacje znajdujące się na powyższej stronie były wystarczające do uruchomienia kolejnych kilku wersji liniowego filtra na wcześniej wspomnianych danych.
Obecnie pierwsze podejście do filtra zawierało taki sam zestaw danych: wysokość, prędkość pionową i przyspieszenie w osi Z, tworzących wektor stanu:
x = [h, v, a]
Wysokość i przyspieszenie potrafimy zmierzyć z ograniczoną dokładnością, prędkość możemy tylko wyliczyć. Filtr dodatkowo wykorzystuje zależność między elementami wektora stanu, tworząc model matematyczny wiążący je ze sobą. Model jest zawarty w macierzy przejścia F:
Pierwszy wiersz macierzy dotyczy wysokości i mówi, że wysokość zależy od pomiaru wysokości (hestymowane = hmierzone * 1), prędkości (hestymowane = vestymowane * dt) oraz przyspieszenia (hestymowane = amierzone * dt2 / 2). Dzięki temu mamy nie jedno, ale trzy źródła informacji o wysokości, które możemy uśrednić, aby uzyskać dokładniejszy wynik. Drugi wiersz opisuje źródła prędkości zależne od pomiaru prędkości (vestymowane = vestymowane * 1) i przyspieszenia (vestymowane = amierzone * dt), czyli są dwa źródła informacji o prędkości. Przyspieszenie zależy tylko od pomiaru.
Zamiast prostego uśredniania danych, do filtra podaje się parametry statystyczne pomagające określić jakość każdego składnika i na tej podstawie wyznaczana jest macierz K współczynników Kalmana, które są wagami określającymi proporcje, z jakimi każdy ze składników wchodzi do finalnej estymaty.
Parametry statystyczne zebrane są w 3 macierzach:
- R — macierz wariancji pomiaru. Wariancja to kwadrat odchylenia standardowego, więc wystarczy zarejestrować trochę danych, wrzucić do arkusza, obliczyć odchylenie standardowe, a następnie wariancję i wstawić ją do głównej przekątnej macierzy. Jeszcze nie wiem, jak obliczyć kowariancję między pomiarami, więc wstępnie zakładam, że jest zerowa, czyli pomiary są niezależne, ale to nie zawsze będzie prawdą.
- P — macierz wariancji i kowariancji predykcji. Jest jeszcze dla mnie zagadką, skąd wziąć te dane. Na razie pochodzą z sufitu.
- Q — macierz wariancji i kowariancji procesu. Nawiązuje ona do macierzy F, gdzie jedne dane wynikały z innych, więc wariancja też podlega podobnym relacjom.
W macierzy F podstawą było przyspieszenie tworzące prędkość:
v = a * dt
oraz tworzące wysokość:
h = a * dt2 / 2
W macierzy Q wariancji procesu podstawą jest pochodna przyspieszenia, czyli zryw, oznaczony jako j (ang. „jerk”). Można utworzyć wektor G wpływu zrywu na wektor stanu x:
G = [dt3 / 6, dt2 / 2, dt]
który po przemnożeniu przez własną transpozycję utworzy macierz wariancji i kowariancji procesu Q:
Q = σj2 * G * GT
która finalnie wygląda tak:
W praktyce sprowadza się to do znalezienia odpowiedniej wartości wariancji zrywu σj2 przed macierzą, bo parametry czasowe zawarte w macierzy są proste do policzenia i zależą tylko od czasu między kolejnymi iteracjami filtra.
Pierwsze wyniki
Stan filtra aktualizowałem danymi wysokości i przyspieszenia, bo tylko one pochodzą z fizycznych czujników, a prędkość miała być estymowana przez filtr. Gdy filtr był już uruchomiony na biurku, do dalszych testów wykorzystałem infrastrukturę megalitycznej budowli Wielkiej Lechii, gdzie firma ma swoją siedzibę. Przespacerowałem się schodami na parter, a potem, wykorzystując teleport potocznie zwany windą, przeniosłem się w czasie Δt na wysokość Δh ponownie pod drzwi firmy. Niedawno rozbudowany rejestrator widział to tak:
Wykres estymowanej wysokości na tle pomiaru wygląda całkiem nieźle jak na pierwsze podejście. Na końcu drogi windy jest widoczne wygasające przeregulowanie. Za chwilę wyjaśni się, skąd ono pochodzi.
Estymowana prędkość wygląda znacznie gorzej, ale gdy podzieliłem Δh przez Δt na odcinku o stałej prędkości ruchu windy, wyszła mi prędkość wynosząca 1,1 m/s. Wygląda na to, że filtr policzył prędkość w miarę dobrze, natomiast klasycznie liczony wariometr dwukrotnie przestrzeliwuje. Wariometr liczę, odejmując wskazania wysokości od tych samych wskazań przepuszczonych przez filtr NOI. Im większy filtr, tym większa różnica czasu między porównywanymi pomiarami i tym większe wskazanie w przypadku ruchu liniowego. Kiedyś na samym początku wariometr liczył dobrze, ale po kilku optymalizacjach i ulepszeniach wynik się rozjechał. Mając wzorzec, dobrałem podstawę filtra tak, aby wynik zgadzał się z rzeczywistością.
Natomiast na wykresie przyspieszenia widać, że estymacja nie działa. Zaznaczyłem na wykresie dwa charakterystyczne kształty, wynikające z rozpędzania i hamowania windy. Nie ma ich na wykresie z filtra. Można się domyślić dlaczego: ponieważ filtr bierze pod uwagę przyspieszenie kinematyczne wynikające z ruchu, a dostaje sumę przyspieszenia kinematycznego i dużo większego ziemskiego. Gdy sprawdziłem współczynnik Kalmana dla przyspieszenia, był on bardzo mały, bo pomiar znajdował się daleko od oczekiwań filtra. Należałoby odjąć od pomiaru stałe przyspieszenie ziemskie, ale lepszym pomysłem jest rozbudowanie wektora stanu o dodatkową pozycję przechowującą statyczną część przyspieszenia, która nie wynika z ruchu.
Rozbudowany filtr z 4-elementowym wektorem stanu
Nigdy tego nie ćwiczyłem, ale okazuje się, że tego typu problemy rozwiązuje się, dzieląc problematyczną zmienną na dwie części. U nas będzie to przyspieszenie kinematyczne i reszta oznaczona jako g, zawierająca grawitację i inne składniki takie jak niedokładność wyznaczenia zera i wpływ temperatury. Pomiar jest dzielony na te dwie części, a różnią się one parametrami statystycznymi. Część kinematyczna może podlegać szybkim zmianom, wiec ma niewielką pewność estymacji a część statyczna jest prawie znana, więc jej pewność jest o kilka rzędów wielkości wyższa. Gdybym miał to zrobić na piechotę, zrobiłbym mocny filtr i to, co by przez niego przeszło, było by częścią statyczną, a część kinematyczna tworzyłby pomiar z odjętą częścią statyczną. Tak więc obecnie wektor stanu wygląda następująco:
x = [h, v, a, g]
Macierz przejścia jest podobna do poprzedniej, ale składowa grawitacyjna oznaczona jako g nie wpływa na pozostałe składniki, stąd poza jedynką w głównej przekątnej są zera w kolumnie i wierszu odpowiadającym składnikowi grawitacyjnemu.
To, co było wcześniej w macierzy Q, to zostaje. Dochodzi nowy parametr σg , który nie zależy od czasu i jest dzielony przez σj występującą przed macierzą, więc finalnie jest to współczynnik definiujący wielkość filtrowania stałej składowej przyspieszenia.
Próby znalezienia wariancji statycznej części przyspieszenia σg2
Być może są znane metody wyznaczania wariancji części pomiaru, ale na razie jeszcze nic o nich nie wiem, więc przyjąłem wartość, jaka ukazała mi się na suficie i rozpocząłem cykliczne próby jej zmiany, obserwując zachowanie filtra. Wcześniejszy test ze spacerem po schodach okazał się czasochłonny i niemiarodajny, więc ograniczyłem się do zdecydowanie bardziej powtarzalnej podróży teleportem. Nie będę Was katował kolejnymi podejściami, przedstawię tylko dwa, ze zbyt małym i zbyt dużym współczynnikiem. Łatwo sobie wyobrazić, co powinno być pomiędzy nimi. Na początek zbyt duża σg2 = 1e-2.
Estymacja wysokości jest lepsza niż wcześniej. Przeregulowania na końcu ruchu windy są jeszcze widoczne, ale pamiętajmy, że na wysokość składa się najwięcej czynników, więc ona zawsze będzie najlepiej wyglądała.
Prędkość ma już trochę poprawioną dynamikę, ale jest jeszcze daleko do tego, co powinno być. Dane wyglądają na zbyt mocno filtrowane, przez co są spóźnione, ale przyczyna może być inna. Widać to na poniższych składowych przyspieszenia kinematycznego i statycznego.
Przyspieszenie kinematyczne doświadcza potężnego ubytku danych. Przez zbyt duży współczynnik na statycznej części filtra większość istotnych danych trafia do części statycznej, gdzie nie są wykorzystywane do korekty prędkości i wysokości. Część dynamiczna dostaje tylko lekki zarys wpływu pomiaru przyspieszenia, więc teraz można zrozumieć, dlaczego prędkość, która jest budowana na podstawie przyspieszenia, była tak słabo odwzorowana.
Ostatnia część danych statycznych, czyli to, co jest odrzucane z pomiaru przyspieszenia. Zbyt dużo danych zostało wyrzuconych, więc trzeba mocno zmniejszyć wartość wariancji.
Kolejna próba ze zbyt małą wartością wariancji
Tym razem wartość wariancji statycznej części przyspieszenia ustawiam na σg2 = 1e-10. Nie nastąpiło to od razu. Jest to efekt stopniowej zmiany wartości wariancji, gdzie widać, że została przekroczona granica dopuszczalnej wartości. Na początek wykres przyspieszenia statycznego. Wygląda on tak jak niżej.
Widać, że nie podkrada już informacji o kinematyce procesu. Pojawiła się natomiast jakaś szpila, prawdopodobnie wynik jakiegoś błędu, którą na tym etapie potraktujmy jako niezależne zdarzenie losowe.
Estymata kinematycznej części przyspieszenia tym razem już znacznie lepiej wpasowuje się w pomiar z ręcznie odjętym przyspieszeniem grawitacyjnym, po to aby łatwiej było je porównać. Widać wyraźne przesunięcie względem zera wynikające ze zbyt powolnej estymacji statycznej części przyspieszenia, stąd część statycznego przyspieszenia jest interpretowana jako dynamiczna. Na upartego można by przyjąć założenie, że filtr musi na starcie odczekać na ustabilizowanie się, ale trwa to trochę za długo. Wariancję trzeba zwiększyć przynajmniej o rząd wielkości.
Estymata prędkości ma teraz ładniejsze zbocza wypracowane z pozyskanej informacji o przyspieszeniu, natomiast analogicznie jak w przyspieszeniu pojawiło się powolne dochodzenie do finalnej wartości, co objawia się dużym przesunięciem prędkości względem zera. Po pewnym czasie prędkość ustaliłaby się na właściwym poziomie, ale trwa to zbyt długo.
Estymata wysokości nie ma już obserwowanego wcześniej przeregulowania na końcu ruchu windy, ale podobnie jak w przypadku prędkości i przyspieszenia pojawiło się powolne dochodzenie do stanu ustalonego. Szpilkę wynikającą z błędu najlepiej widać na skali wysokości. To nie jest zakłócenie pomiaru, jak przypuszczałem, tylko efekt działania filtra. Trudno powiedzieć, czy to błąd numeryczny, czy coś maże po pamięci. Znalezienie przyczyny to żmudna praca inżynierska, którą trzeba będzie wykonać.
Podsumowując, tak ustawiony filtr ma zbyt dużą pewność dotyczącą statycznej części przyspieszenia, przez co zmienia się ona zbyt wolno. Część statycznej części przyspieszenia dostaje się do kinematycznej części i powoduje, że filtr widzi większe przyspieszenie niż w rzeczywistości. Przez macierz przejścia przyspieszenie wchodzi na prędkość i wysokość, zwiększając ich wartości.
Stosując metodę Macajewa-Pierniczenki, dobrałem wartość σg2 = 7e-9. Kryterium doboru wartości oparłem na tym, aby w statycznej części przyspieszenia było widać śladowe ilości przyspieszenia kinematycznego. Utrata śladowych ilości informacji nie jest groźna, a taki filtr możliwie szybko dochodzi do stanu ustalonego.
Dodanie pomiaru prędkości
Tak ustawiony filtr przyzwoicie estymuje wysokość i przyspieszenie, ale prędkość balansuje między słabymi zboczami a pływającym poziomem zera. Wcześniej nie chciałem ufać pomiarowi prędkości jako niepochodzącemu bezpośrednio z czujnika, ale porównując estymatę prędkości z filtra Kalmana i prędkość liczoną jako pochodna wysokości, ta druga wartość wydaje się być lepsza. Ma dosyć dużo szumu i przesunięcie fazowe wynikające z filtrowania tego szumu, ale przynajmniej sama metoda różniczkowania zapewnia prawidłowy poziom zera, bo jak pamiętamy ze szkolnej matematyki, pochodna ze stałej: c' = 0. Dlaczego by nie dodać prędkości jako kolejny pomiar?
Wcześniej nie wspominałem o wektorze pomiaru z, który dla filtra we wcześniejszej postaci wyglądał następująco:
z = [h, a]
a teraz będzie rozbudowany do:
z = [h, v, a]
Z pomiarem wiąże się macierz obserwacji H, która decyduje, do jakich elementów wektora stanu x trafia pomiar. Do tej pory dla 4-elementowego wektora stanu i 2-elementowego wektora pomiaru macierz obserwacji wyglądała tak:
Pierwszy element wektora pomiaru, czyli wysokość, jest reprezentowany w macierzy obserwacji jako pierwszy wiersz. Wysokość trafia do pierwszego elementu wektora stanu, czyli estymaty wysokości. Drugi element pomiaru, czyli przyspieszenie, reprezentowany przez drugi wiersz macierzy H, trafia do składowej kinematycznej i statycznej przyspieszenia. Po rozszerzeniu wektora pomiaru do 3 elementów, czyli po dodaniu prędkości, macierz obserwacji wygląda tak:
W praktyce sprawa jest trochę bardziej złożona, bo w każdym obiegu pętli mam nowy pomiar przyspieszenia, ale wysokość oraz liczona z niej prędkość pojawiają się rzadziej, gdyż czujnik ciśnienia ma ODR (ang. Output Data Rate) na poziomie 240 Hz. Wymusza to rozbicie fazy aktualizacji na 2 funkcje: aktualizację danych pochodzących z czujnika ciśnienia, czyli wysokości i prędkości, oraz aktualizację danych z akcelerometru. Funkcje mają inne rozmiary macierzy, więc różnią się szczegółami implementacji i wymagają osobnych zmiennych, ale ogólna zasada pozostaje ta sama.
Nie wspominałem o tym wcześniej, ale w APL2 zrobiłem własną bibliotekę operacji na macierzach, bo nie było dostępnego niczego innego, jednak jest ona dosyć prymitywna. W APL3 zdecydowałem, że będę korzystał ze standardowej biblioteki CMSIS-DSP dostarczonej przez ARM i dostępnej w narzędziach Cube. Jest ona powszechnie znana i dosyć wygodna w obsłudze. Teraz, gdy zmienia mi się rozmiar danych, potrzebuję około godziny, aby zmodyfikować rozmiary zmiennych macierzy, a biblioteka już obie z tym radzi.
Tak więc po uruchomieniu nowej wersji filtra i teleportacji na parter i z powrotem, rejestrator zwrócił takie dane:
Przyspieszenie kinematyczne jak dosyć dobrze odwzorowne. Ma jeszcze lekkie zafalowanie poziomu zera, ale to jest zmienna pomocnicza, wiec to mnie nie boli. Ważne, że filtr poprawnie reaguje na właściwe zmiany przyspieszenia, a ignoruje wcale nie mniejszy szum, który byłby nie do usunięcia innymi metodami.
Prędkość też wygląda całkiem nieźle, wyraźnie lepiej niż wcześniej. Ma ładne zbocza i trzyma poziom zera. To jest ważne, bo ta prędkość będzie szła do PID-a sterującego wysokością, więc musi być poprawna. Pojawił się problem ze zbyt małą amplitudą prędkości, ale to zostało szybko naprawione.
Natomiast estymata wysokości się popsuła. Trochę się zastanawiałem, co się stało, ale po próbach okazało się, że aby mieć ładne wskazania wariometru, prędkość jest dosyć mocno filtrowana. Ponieważ wysokość zależy od prędkości, więc opóźnienie na wariometrze wygładziło i przesunęło w czasie wykres wysokości.
Może w takim razie nie ma potrzeby filtrować wskazań wariometru, a sam filtr różniczkujący zmniejszyć, ile tylko się da. Zobaczmy, co się stanie, gdy tak zrobimy:
Szum na pomiarze prędkości drastycznie wzrósł, ale nie wygląda, aby estymata mocno ucierpiała z tego powodu. Trochę się wygładziła, czyli w mniejszym stopniu polega na pomiarze, a w większym na predykcji.
Natomiast wysokość jest teraz estymowana idealnie. Trzeba będzie spróbować znaleźć granicę między rozmiarem filtra różniczkującego a występowaniem opóźnienia na wysokości i znaleźć złoty środek, ale to, co jest teraz, już mnie zadowala. Sterowanie wysokością w wielowirnikowcu powinno być teraz bardziej spójne i prawdopodobnie zmniejszy się pływanie w funkcji wysokości.
Gdyby ktoś chciał zerknąć do kodu APL3 na GitHubie, to funkcje znajdują się w katalogu CM4\Core\Src\, czyli tam, gdzie jest kod autopilota w plikach o nazwach:
- KalmanWysokosci4X2Z.c — dotyczy 4-elementowego wektora stanu i 2-elemetowego wektora pomiaru
- KalmanWysokosci4X3Z.c — dotyczy 4-elementowego wektora stanu i 3-elemetowego wektora pomiaru
Do zrobienia jest jeszcze zbadanie wpływu wariancji predykcji, która jest obecnie wzięta z sufitu. To nie jest jedna wartość, tylko cała macierz, więc eksperymentalne jej wyznaczanie jest zadaniem karkołomnym. Może lektura książki wyjaśni, skąd brać te dane? Przy okazji przyszło mi do głowy zrobienie symulatora filtra, gdzie filtr mógłby pracować na zarejestrowanym wcześniej zestawie danych pomiarowych. To byłaby odwrotność rejestratora, gdzie zamiast zapisywać dane na karcie SD, pobierałby dane z karty i podstawiał filtrowi. Takie narzędzie przyspieszyłoby wykonywanie eksperymentów, jednak wyłuskanie danych ze zbioru o płynnej strukturze — każdą zmienną można rejestrować lub nie — nie jest takie proste. Trzeba wyważyć, czy jest rzeczywista potrzeba tworzenia takiego narzędzia.
To jednak nie wszystko, co można zrobić filtrem Kalmana. Już teraz zapraszam do następnej części, gdzie wkrótce opiszę przełomową i długo wyczekiwaną funkcjonalność, którą udało się osiągnąć dzięki filtrowi.