Filtr Kalmana: Co to jest i Jak Działa?
- Szczegóły
O filtrze Kalmana słyszał zapewne każdy, kto miał do czynienia z elektroniką lub naukami pokrewnymi. Od czasu jego wynalezienia minęło już ponad 50 lat (algorytm ochrzczono imieniem jego autora: Rudolfa Emila Kalmana) i zdążył się już zadomowić na dobre wszędzie tam, gdzie zachodzi potrzeba sterowania obiektami w warunkach niepewności. Każdy automatyk powinien zatem przyswoić sobie chociaż minimum wiedzy potrzebnej do wykorzystania FK w praktycznych zastosowaniach.
Filtr Kalmana jest optymalnym estymatorem stanu układu dynamicznego. Należy tutaj jednak przywołać kilka założeń, które są niezbędne (choć czasem przemilczane), aby filtr działał jak należy.
Model Matematyczny i Założenia
Podstawowy model układu dla filtru Kalmana można przedstawić następująco:
- $x_k = A x_{k-1} + B u_{k-1} + w_{k-1}$
- $y_k = H x_k + v_k$
gdzie:
- $x$ - stan układu,
- $u$ - wejście,
- $y$ - wyjście,
- $k$ oznacza dyskretną chwilę czasu $k$.
Sam układ opisany jest macierzami:
Przeczytaj także: Definicja i pomiar filtracji kłębuszkowej
- $A$ - macierz przejścia,
- $B$ - macierz wejścia,
- $H$ - macierz wyjścia.
Wektor $w$ jest wektorem szumu procesowego o zerowej wartości oczekiwanej i znanej macierzy kowariancji $Q$: $w_k\sim N\left(0,Q_k\right)$. Wektor $v$ to wektor szumu pomiarowego o podobnych założeniach co $w$ oraz znanej macierzy kowariancji $R$: $v_k\sim N\left(0,R_k\right)$. Aby możliwe było stworzenie poprawnego FK konieczne jest sprowadzenie modelu procesu do powyższej postaci oraz wyznaczenie obydwu macierzy szumów.
Wzmocnienie Filtru Kalmana
Wzmocnienie filtru Kalmana ($K$) jest kluczowym elementem algorytmu, wpływającym na sposób, w jaki pomiary korygują estymatę stanu. Można je wyrazić wzorem:
- $K$ - wzmocnienie filtru Kalmana,
- $R$ - macierz kowariancji szumu pomiarowego.
Wzmocnienie filtru, zależne od $R$ jest informacją o tym, w jaki sposób informacja pochodząca z pomiarów wpływać ma na korekcję estymatu stanu. Dla dużej wartości $K$ (małego zakłócenia stanu w $R$) pomiary będą miały istotny wpływ na wartość $\hat{x}_{k|k}$.
Inicjalizacja Algorytmu
Do rozpoczęcia działania filtra konieczne jest ustalenie dwóch wielkości: $\hat{x}_{0|0}$ oraz $P_{0|0}$, które wykorzystywane są w pierwszej fazie predykcji algorytmu. Wektor $\hat{x}_{0|0}$ jest wartością oczekiwaną stanu w chwili 0, zaś macierz $P_{0|0}$ można przyjąć za równą wartości macierzy kowariancji szumu procesowego.
Macierz Q i R
W praktyce spotkać się można z nieco mniej skomplikowanym podejściem do strojenia FK. Macierze $Q$ i $R$ ustalane są arbitralnie jako macierze diagonalne z dobranymi doświadczalnie wartościami na głównych przekątnych.
Przeczytaj także: Webber AP8400 - wymiana filtrów
Przykłady Zastosowań Filtru Kalmana
Przykład 1: Odszumianie Danych
Prosty model, gdzie: $A=1$, $B=0$ i $H=1$. Stan jest wielkością skalarną stąd wektory $w_k$ i $v_k$ również przyjmują postać skalarną. Taka postać FK może posłużyć do odszumiania praktycznie dowolnych danych. Strojenie takiego filtru opiera się tylko na doborze wartości parametrów Q i R. Ten prosty model ma niewątpliwą zaletę w postaci łatwości implementacji i niewielkiego narzutu obliczeniowego, szczególnie istotnego chociażby w systemach mikroprocesorowych.
Przykład 2: Estymacja Stanu Ruchu Obiektu
Sztandarowym przykładem wykorzystania FK jest estymacja stanu ruchu obiektu. Dotyczy to zarówno ruchu liniowego jak i obrotowego, w jednym lub wielu wymiarach. Prócz odszumiania danych pochodzących z GPSu, żyroskopu czy akcelerometru, FK pozwala na fuzję danych z wielu czujników równocześnie w celu poprawy ostatecznej oceny stanu.
Model może wyglądać następująco:
- gdzie elementy wektora $x$ to kolejno droga, prędkość i przyspieszenie.
Wspomniany model nie posiada wejść. Załóżmy, że z użyciem odpowiednich urządzeń pomiarowych mierzone są przyspieszenie i prędkość pewnego ciała. Zadaniem FK jest fuzja obydwu sygnałów w celu odtworzenia położenia ciała oraz odszumienie zebranych pomiarów.
Przykład 3: Badanie Działania Układu RLC
Ostatnim przykładem, który jest moim zdaniem warty rozpatrzenia jest badanie działania układu RLC. Zadanie jest o tyle ciekawe, że w pierwszej kolejności zostanie wyznaczony model matematyczny w postaci liniowych równań różniczkowych, zostanie dokonana dyskretyzacja równań oraz kompozycja modelu do FK. Zakładamy, że mierzone jest napięcie na kondensatorze czwórnika RLC, które obarczone jest szumami pomiarowymi. Zadaniem FK będzie prócz filtracji napięcia, odtworzenie drugiego elementu wektora stanu - prądu cewki, a w zasadzie prądu w całym układzie.
Przeczytaj także: Optymalne rozcieńczenie bimbru
Aby możliwe było użycie powyższego modelu w filtrze Kalmana należy dokonać dyskretyzacji powyższych równań. Na chwilę obecną pomijane jest uwzględnienie zakłóceń stanu. Zostaną one ujęte dopiero w modelu dyskretnym.
Przykład 4: Ruch Ciała w 2D
Pomysł na stworzenie jakiegoś przykładu realizacji FK w przeglądarce zaczerpnąłem stąd: Forbot.pl - Filtr Kalmana w praktyce - 3 przykłady z kodami!, gdzie Autor prezentuje wykorzystanie FK do estymacji położenia kursora (implementacja w OpenCV). Postanowiłem odtworzyć ten pomysł z użyciem jQuery oraz obiektu canvas wprowadzonym wraz z HTML5. Przykład jest zarówno w sporym stopniu "interaktywny" jak i prosty na tyle, aby prezentował niezerową wartość edykacyjną. Przyjęty przeze mnie model jest uproszczony (taki jak w przykładzie 1) w stosunku do tego z artykułu, który dodatkowo uwzględnia prędkość kursora.
Adaptacyjny Filtr Kalmana (ADKF) w Odbiornikach GNSS
W odbiornikach systemów nawigacji satelitarnej GNSS (Global Navigation Satellite System) zwykle stosowane są algorytmy filtracji Kalmana. Z reguły stosuje się rozszerzone filtry Kalmana (EKF) zaprojektowane przy założeniu stałej dokładności pseudoodległości. W praktyce pseudoodległości mają zmienną dokładność i dodatkowo mogą być okazjonalnie obarczone błędami znacznie przekraczającymi typowe wartości (błędami anomalnymi). Z tego względu wskazane jest estymowanie w filtrze macierzy kowariancji błędów pomiarowych, zamiast zakładania jej stałej wartości.
W niniejszej pracy zaprezentowano, opracowany przez autora, adaptacyjny filtr Kalmana odbiornika GNSS (ADKF), w którym macierz ta jest na bieżąco estymowana, przez co filtr dostosowuje swoje wagi do zmieniającej się dokładności pomiarów. Podano model projektowy i algorytm filtru oraz przedstawiono wybrane wyniki jego badań.
Nawigacyjne odbiorniki GNSS (Global Navigation Satellite System) określają położenie metodą odległościową [3, 6, 12]. Wielkościami mierzonymi są różnice czasu pomiędzy momentami wysłania i odbioru sygnałów radiowych transmitowanych przez satelity nawigacyjne. Sygnały satelitów zawierają w tzw. Odbiornik GNSS wykorzystując własny zegar określa czas odbioru sygnałów.
Wykorzystanie Bibliotek Programowych
Inżynierowie firmy STMicroelectronics wraz z poszerzeniem oferty czujników przyspieszenia opracowali biblioteki programowe, realizujące funkcję filtru Kalmana, ułatwiające stosowanie MEMS-ów w praktycznych aplikacjach. Dostępna bezpłatnie biblioteka iNEMO Engine Lite umożliwia obsłużenie zarówno wieloosiowych czujników przyspieszenia jak i czujników geomagnetycznych, minimalizując wpływy zakłóceń odczytów wynikających z jakości otoczenia na uzyskane wyniki. Wersja Lite bibliotek jest udostępniana przez producenta w wersji źródłowej dla mikrokontrolerów STM32. Jest ona użyta m.in. w zestawie ewaluacyjnym iNEMO (STEVAL-MKI062V2). W wyniku obliczeń wykonywanych przez bibliotekę iNEMO Engine Lite programista uzyskuje „czyste” (po programowej filtracji Kalmana) informacje o wartości kwaternionów (obiekty matematyczne, które służą do reprezentowania rotacji i orientacji w przestrzeni 3D), wykrywane są rotacje i obliczane ich prędkości, obliczane jest także przyspieszenie liniowe z jakim porusza się czujnik.
tags: #filtracja #danych #filtr #Kalmana #co #to
