Elektronika modelarska

Filtr Kalmana jest ciekawym narzędziem pozwalającym filtrować dane znacznie lepiej niż innymi filtrami. W naszej branży aż się prosi o jego użycie, dlatego warto o nim porozmawiać i lepiej go poznać bo jest to potężne, ale niezbyt proste narzędzie.

W porównaniu do "zwykłych filtrów", które przepuszczają przez siebie dane nic o nich nie wiedząc, filtr Kalmana oparty na modelu danego zjawiska. Jednocześnie filtruje dane wchodzące do modelu mierząc ich parametry statystyczne (wariancję) a także na podstawie tych danych tworzy model i liczy błąd predykcji następnego stanu modelu. Następnie przyjmuje wagi nazywane współczynnikiem Kalmana "K", przez które przemnaża dane wchodzące do procesu w fazie tak zwanej korekcji, oraz przemnaża przez odwrotność współczynnika (1-K) dane szacowane następnego stanu modelu. W zależności do wartości współczynnika, następny stan modelu będzie polegał bardziej na pomiarach lub na predykcji. Jeżeli dane wchodzące do procesu będą obarczone dużym szumem (ich wariancja będzie wysoka) to model będzie polegał bardziej na wartościach z predykcji. I odwrotnie jeżeli dane będą czyste a wyliczany model będzie miał dużą wariancję, wtedy filtr będzie bardziej polegał na danych pomiarowych.

 Jak działa taki model? 

W komputerze pokładowym mam wdrożony filtr sprzęgający dane o wysokości z wysokościomierza barometrycznego, prędkości pionowej z wariometru oraz przyspieszeniu w osi Z z akcelerometru. Te dane są ze sobą mocno związane fizycznie. Przyspieszenie jest pochodną prędkości a prędkość jest pochodną drogi czyli wysokości. Znając jedną wartość, można wyliczyć pozostałe. 

Dane pochodzą z 3 różnych czujników, z których każdy szumi na swój sposób. Odfiltrowanie szumu każdego czujnika osobno wprowadza do pomiaru duże opóźnienia co fatalnie przekłada się na jakość regulacji na podstawie tak odszumionego czujnika. Zastosowanie filtru Kalmana pozwala porównać wskazania modelu fizycznego tworzonego ze wszystkich czujników z każdym czujnikiem osobno. Jeżeli dane modelu i czujnika są mocno skorelowane, to takim danym warto ufać i dla tej zmiennej współczynnik Kalmana będzie bliższy jedności. Jeżeli dane modelu i czujnika są słabo skorelowane to współczynnik Kalmana aktualizujący model pod koniec każdego cyklu będzie niski i model będzie słabo korygowany od tego czujnika.

 

 Najprostszy filtr Kalmana będzie wykonywał następujące operacje:

- na podstawie aktualnego stanu modelu i parametrowi zasilającemu model (w naszym przypadku przyspieszenie) obliczy przewidywane przyspieszenie, prędkość i drogę w następnej chwili

- obliczy wariancję przewidzianych wartości. Jest to wariancja naszego modelu. Ta pierwsza faza nazywa się predykcją.

W drugiej fazie zwanej korekcją zostaną uwzględnione dane pomiarowe.

- obliczy współczynniki Kalmana dla każdego parametru korygującego filtr. W naszym przypadku korygujemy go wysokością i prędkością.

- dołoży do modelu procesu korekcję od danych pomiarowych przemnożonych przez współczynnik Kalmana

- obliczy wariancję uaktualnionego procesu I to koniec pętli.

W kolejnych obiegach filtr będzie dostrajał się polegając bardziej na tym co mu wychodzi lepiej: albo na predykcji, albo na pomiarze. Mówiąc bardzo mądrze FK jest optymalnym estymatorem procesu.

 

Wiemy już że taki filtr wiedzący co filtruje jest lepszy od filtru nie mającego pojęcia o danych, ale to jeszcze nie koniec możliwości. Nasz model można rozbudowywać o dodatkowe czujniki lub upraszczać gdy nie dysponujemy niektórymi czujnikami, np wariometrem.

 

Możemy sobie wyobrazić kolejne udoskonalanie filtra przez dokładanie dalszych, lub dalsze rozbudowywanie modelu uwzględniając dalsze pochodne drogi (cokolwiek by to znaczyło w sensie fizycznym). Istotne jest aby kolejne człony filtra (pomiary lub człony modelu) miały inne parametry niż obecne. Rozbudowywania filtra jednak wpływa na ilość obliczeń do wykonania.

 

Takich procesów dających się modelować i częściowo mierzyć jest wiele. Kolejny przykład to pomiar kąta pochylenia latającej platformy za pomocą inklinometru lub 3 osiowego akcelerometru. Model uwzględnia kąt pochylenia oraz zmianę prędkości tego kąta w czasie czyli prędkość kątową, którą to można zmierzyć żyroskopem. Nie bardzo można zmierzyć przyspieszenia kątowego, ale spokojnie można go modelować jeżeli model bez tego przyspieszenia będzie miał zbyt słabe parametry dynamiczne. Tak jak wcześniej żyroskop ma dryft w czasie a akcelerometr szumi. Po spleceniu filtrem uzyskuje się o wiele lepsze dane. Czystsze i bez dryftu.

 

 

Tyle tytułem wstępu. Teraz coś o sposobie liczenia.

Sami widzicie ile parametrów razem się splata. Teraz z tego trzeba liczyć wariancję i to nie tylko z pojedynczych danych pomiaru i modelu, ale każdy z każdym (o ile to ma fizyczny sens). Dlatego operacje wykonuje się nie na pojedynczych danych ale na wektorach i macierzach. Tak wygląda graf obliczeń zaczerpnięty z wikipedii:

Diagram stanów filtru Kalmana

Dane tworzące model, czyli droga, prędkość i przyspieszenie z pierwszego przykładu tworzą tzw wektor stanu oznaczany x. W przykładzie z pomiarem prędkości wektor stanu będzie miał 3 pozycje. Jest to zasobnik do przechowywania naszego modelu.

 

x = [S, v, a]

gdzie S-droga, v-prędkość, a-przyspieszenie

 

Macierz A nazywana macierzą przejścia ma rozmiar [n_stanów * n_stanów]. Zawiera ona informację o tym jak poszczególne elementy wektora stanu wpływają na siebie. W powyższym przykładzie było by tak:

 

S v a

S|1 dt dt^2|

v|0 1 dt |

a|0 0 1 |

gdzie: dt-czas jaki upływa w trakcie jednej iteracji filtra.

Czyli jedynki w głównej przekątnej macierzy mówią że parametr wpływa na samego siebie z mnożnikiem 1. To jest oczywiste. Dodam tylko że macierz mająca tylko jedynki na głównej przekątnej nazywana jest macierzą jednostkową i mnożenie innej macierzy przez macierz jednostkową jest równoważne możeniu przez 1.

dt w drugiej kolumnie pierwszego wiersza oznacza że prędkość = droga * czas. Wyrażenie dt^2 w trzeciej kolumnie pierwszego wiersza oznacza że przyspieszenie = droga * czas^2. dt w trzeciej kolumnie drugiego wiersza oznacza że przyspieszenie = prędkość * czas.

Zakładając że mamy komplet pomiarów, to taka prędkość będzie zależała zarówno od drogi jak i przyspieszenia. Taki filtr lepiej wiązał by parametry modelu ze sobą. Kolejny (opcjonalny) element filtra: macierz wejściowa B o rozmiarze [n_stanów * m_wejść] Poprzez tą macierz wchodzi do układu wektor u zawierający tzw. zmienne sterujące. Zmienne te wpływają na model już w fazie predykcji a nie jak normalne zmienne pomiarowe w fazie korekcji.

Czym się różni zmienna sterująca od zmiennej pomiarowej?

Tym że zmienna pomiarowa charakteryzuje się tylko szumem a nie posiada dryftu. Jest to wartość na której można polegać. W praktyce nie ma danych idealnych, ale wysokość uzyskana z barometru na pewno będzie charakteryzowała się mniejszym dryftem niż wyliczona przez podwójne całkowanie z przyspieszenia.  W naszym przykładzie z pomiarem wysokości, prędkości i przyspieszenia modelu pomiar przyspieszenia jest obciążony największym dryftem w związku z tym jest on zmienną sterującą. . Wysokość i prędkość są danymi pomiarowymi i od nich wykonujemy korekcję wyliczonego modelu.

 

Te 4 pierwsze elementy opisuje zależność będąca pierwszym równaniem filtru Kalmana (faza predykcji)

 

(1) x = A*x + B*u + wk

 

gdzie: wk to szum procesu

 

Drugie i ostatnie równanie fazy predykcji wygląda następująco:

 

(2) P_ = A*P*A^T + Q

 

gdzie: P_ to macierz kowariancji estymowanego błędu o rozmiarach [n_stanów * n_stanów]. Przechowuje wyliczany zestaw wariancji, który będzie potrzebny do wyliczenia współczynnika Kalmana K. Po prostu to jest taki zasobnik na dane statystyczne. Minus przy liczbie oznacza "a priori", czyli to że macierz zawiera informacje pochodzące z przeszłości. Po aktualizacji w drugiej fazie filtra zgubi minus zamieni się na "posteriori", czyli będzie oparte o wszystkie dane (wcześniejsze + aktualne z fazy korekcji).

Q to macierz kowariancji procesu o rozmiarze [n_stanów * n_stanów]. Zawiera informacje o wartości wariancji poszczególnych składników modelu. Tą macierz inicjuje się przed rozpoczęciem pracy filtra. Inicjacja polega na wypełnieniu pól na głównej przekątnej macierzy wartościami wariancji procesu. Przykładowo jeżeli wynik procesu jest iloczynem dwu zmiennych pomiarowych o znanej wariancji to wariancja tego procesu będzie iloczynem wariancji zmiennych.

 

Faza korekcji zawiera trzy równania:

 

(3) K = P_*C^T*(C*P_*C^T + R)^-1

 

Tutaj liczony jest jest współczynnik Kalmana K Ze zmiennych dochodzi R będące macierzą kowariancji pomiarów. Podobnie jak Q będące macierzą kowariancji procesu musi być zainicjowane przed startem wartościami wariancji danych pomiarowych ułożonych na głównej przekątnej. Rozmiar tej macierzy wynosi [n_stanów * n_stanów].

W następnym równaniu dokonujemy właściwej korekcji z uwzględnieniem współczynnika Kalmana i danych wejściowych:

 

(4) x = x_ + K*(y - C*x)

 

gdzie  y jest wektorem pomiarowym o rozmiarze [n_stanów]. Tutaj do filtra wchodzą dane w fazie korekcji. W naszym przykładzie było by to: [ S v a ]. Kompletnie nie wiem dlaczego na grafie strzałka jest skierowana do wektora y a nie odwrotnie. Może coś źle rozumiem - proszę o korektę. Podobnie średnio rozumiem rolę macierzy wyjściowej C. Jej rozmiar to [r_wyjść * n_stanów]. Ponieważ C jest związane z y, nie jestem pewien czy dobrze podałem rozmiar wektora pomiarowego y. Być może rozmiar y to [r_wyjść] a nie [n_stanów].

W sporej części przykładów C jest macierzą jednostkową co wybitnie wpływa na szybkość obliczeń, gdyż mnożenia przez macierz jednostkową można pominąć. W ostatnim kroku liczymy wariancję korekcji

 

(5) P = (I-K*C)*P_

 

gdzie I to macierz jednostkowa (z 1 na głównej przekątnej) I to tyle obliczeń.

 

Wyjaśnię jeszcze kilka "ptaszków", które dla osób nie liczących macierzy na co dzień nie są oczywiste. Wszystkie operatory w równaniach FK odnoszą się do operacji na macierzach.

dodawanie macierzy, mnożenie przez skalar i inną macierz

transpozycja (^T)

odwracanie macierzy (^-1)

 

Na koniec kilka pozycji literatury do znalezienia w sieci.

"Filtr Kalmana - zastosowania w prostych układach sensorycznych." Jan Kedzierski, Koło Naukowe Robotyków KoNaR. Chyba najbardziej kompletne polskojęzyczne opracowanie, jednak mam wrażenie że jest pisane dla matematyków stąd nie nadążam z analizą przykładów.

- Na Elektrodzie jest wątek o FK w którym podany jest link do projektu w VC. Daje się skompilować i działa. Liczy metodą brutalnej siły (wszystko z wszystkim) przez co trwa koszmarnie długo, ale działa i jest niezłym przykładem do szczegółowej analizy.

Lataj bezpiecznie swoim UAV