Filtr Kalmana

Filtr Kalmana – algorytm rekurencyjnego wyznaczania minimalno-wariancyjnej estymaty wektora stanu modelu liniowego dyskretnego układu dynamicznego na podstawie pomiarów wyjścia oraz wejścia tego układu. Przyjmuje się założenie, że zarówno pomiar, jak i proces przetwarzania wewnątrz układu jest obarczony błędem o rozkładzie gaussowskim.

Wstęp

Teoretycznie filtr Kalmana jest estymatorem tego co nazywa się problemem liniowo-kwadratowym, czyli problemem estymacji natychmiastowego stanu liniowego układu dynamicznego, który narażony jest na perturbacje białego szumu (zob. regulator liniowo-kwadratowy-Gaussa) – przez użycie pomiarów liniowo związanych ze stanem, ale zakłóconych przez biały szum. Wynikowy estymator jest statystycznie optymalny w odniesieniu do dowolnej funkcji estymowanego błędu.

Równania filtru Kalmana nie zmieniają swojej postaci w przypadku, gdy układ jest niestacjonarny. Jest to jedna z jego podstawowych zalet w stosunku do filtru Wienera ^{potrzebne źródło}.

Funkcja będąca filtrem Kalmana posiada dwa parametry wejściowe – są to wejście układu oraz jego wyjście. Na tej podstawie oblicza się estymaty zmiennych stanu (zob. też obserwator Luenbergera).

Równania liniowego dyskretnego modelu stanowego

 Osobny artykuł: Równanie stanu (teoria układów dynamicznych).

Filtr Kalmana jest optymalnym obserwatorem stanu układu (czyli układem, który estymuje jego stan), który modelowany jest z użyciem równań stanu:

${\displaystyle \mathbf {x} (t+1)=\mathbf {Ax} (t)+\mathbf {Bu} (t)+\mathbf {v} (t),}$

${\displaystyle \mathbf {y} (t)=\mathbf {C} \mathbf {x} (t)+\mathbf {w} (t),}$

gdzie:

${\displaystyle t=\dots ,-1,0,1,2,\dots }$ dyskretna chwila czasu,

${\displaystyle \mathbf {x} (t)}$ – chwilowa wartość wektora stanu,

${\displaystyle \mathbf {A} }$ – macierz systemowa układu (macierz przejścia),

${\displaystyle \mathbf {B} }$ – macierz wejścia,

${\displaystyle \mathbf {C} }$ – macierz wyjścia,

${\displaystyle \mathbf {v} (t)}$ – wektor szumu przetwarzania,

${\displaystyle \mathbf {w} (t)}$ – wektor szumu pomiarowego,

${\displaystyle \mathbf {y} (t)}$ – wektor pomiarowy.

Równania filtru

Równania filtru Kalmana dzielą się na dwie kategorie: równania aktualizacji czasu i równania aktualizacji pomiarów.

Równania aktualizacji czasu

Równania aktualizacji czasu dokonują predykcji stanu układu na chwilę ${\displaystyle t}$ na podstawie znajomości estymaty stanu na chwilę ${\displaystyle t-1}$

${\displaystyle {\hat {\mathbf {x} }}(t|t-1)=\mathbf {A{\hat {x}}} (t-1|t-1)+\mathbf {B{\hat {u}}} (t-1),}$

${\displaystyle \mathbf {P} (t|t-1)=\mathbf {AP} (t-1|t-1)\mathbf {A} ^{T}+\mathbf {Q} .}$

Wektory

${\displaystyle {\hat {\mathbf {x} }}(t|t-1),}$

${\displaystyle {\hat {\mathbf {x} }}(t-1|t-1)}$

stanowią estymaty a priori (przed pomiarem) i a posteriori (po pomiarze) wektora stanu.

Macierze

${\displaystyle \mathbf {P} (t|t-1)=E\left[{\tilde {\mathbf {x} }}(t|t-1){\tilde {\mathbf {x} }}^{T}(t|t-1)\right],}$

${\displaystyle \mathbf {P} (t-1|t-1)=E\left[{\tilde {\mathbf {x} }}(t-1|t-1){\tilde {\mathbf {x} }}^{T}(t-1|t-1)\right],}$

to macierze kowariancji, odpowiednio: a priori i a posteriori, wektorów

${\displaystyle {\tilde {\mathbf {x} }}(t|t-1)=\mathbf {x} (t)-\mathbf {\hat {x}} (t|t-1),}$

${\displaystyle {\tilde {\mathbf {x} }}(t-1|t-1)=\mathbf {x} (t-1)-\mathbf {\hat {x}} (t-1|t-1).}$

Wektory te stanowią różnicę pomiędzy rzeczywistą wartością wektora stanu a jego estymatą i są miarą błędu oceny wektora stanu.

Macierz ${\displaystyle \mathbf {Q} }$ to macierz kowariancji szumu przetwarzania.

Równania aktualizacji pomiarów

Równania z drugiej kategorii aktualizują wyliczoną wcześniej predykcję stanu, na podstawie pomiaru:

${\displaystyle {\hat {\mathbf {x} }}(t|t)=\mathbf {\hat {x}} (t|t-1)+\mathbf {K} (t)\mathbf {e} (t),}$

${\displaystyle \mathbf {e} (t)=\mathbf {y} (t)-\mathbf {C} \mathbf {\hat {x}} (t|t-1),}$

${\displaystyle \mathbf {K} (t)=\mathbf {P} (t|t-1)\mathbf {C} ^{T}\mathbf {S} ^{-1}(t),}$

${\displaystyle \mathbf {S} (t)=\mathbf {C} \mathbf {P} (t|t-1)\mathbf {C} ^{T}+\mathbf {R} ,}$

${\displaystyle \mathbf {P} (t|t)=\mathbf {P} (t|t-1)-\mathbf {K} (t)\mathbf {S} (t)\mathbf {K} ^{T}(t).}$

Macierz ${\displaystyle \mathbf {K} (t)}$ nosi nazwę wzmocnienia Kalmana. Wartość wzmocnienia Kalmana decyduje o tym czy bardziej „ufamy” naszej ocenie stanu (a priori), czy informacji zawartej w pomiarach. Zerowe wzmocnienie oznacza, że zupełnie ignorujemy pomiary – są obarczone nieskończenie wielkim błędem (wariancja szumu pomiarowego jest nieskończona). Wzmocnienie równe ${\displaystyle \left(\mathbf {C} ^{T}\right)^{-1}(t)}$ (jeżeli macierz ${\displaystyle \mathbf {C} ^{T}(t)}$ jest kwadratowa) powoduje, że estymata a posteriori wektora stanu opierać się będzie wyłącznie na pomiarze wyjścia układu.

Wektor ${\displaystyle \mathbf {e} (t)}$ to wektor innowacyjny. Nazwany został tak dlatego, że jego wartość niesie ze sobą nową (opartą na najnowszych pomiarach) informację o procesie.

Macierz ${\displaystyle \mathbf {R} }$ to macierz kowariancji szumu pomiarowego.

Zastosowania

Filtr Kalmana znalazł zastosowanie w ogromnej liczbie dziedzin techniki, do których należą m.in.: automatyka, robotyka, elektronika, teoria sterowania, przetwarzanie sygnałów, inżynieria dźwięku i obrazu, rzeczywistość rozszerzona i wiele innych ^{potrzebne źródło}.

Wśród aplikacji wymienić można sterowanie złożonymi systemami dynamicznymi, takimi jak ciągłe procesy produkcyjne, samoloty, statki wodne i kosmiczne ^{potrzebne źródło}. Dla takich aplikacji często nie jest możliwe lub pożądane by mierzyć każdą zmienną, którą chce się sterować, a filtr Kalmana dostarcza środków do „wydedukowania” brakującej informacji z pomiarów pośrednich. Filtr Kalmana może także być użyty do predykcji (prognozowania) możliwego przyszłego kursu systemów dynamicznych, takich jak przepływ rzek podczas powodzi, trajektorie ciał niebieskich czy ceny towarów będących przedmiotem handlu ^{potrzebne źródło}.

Do rzeczywistych zastosowań filtru Kalmana należą:

• śledzenie obiektów na podstawie pomiarów z radaru,
• jednoczesna lokalizacja i budowanie mapy (SLAM),
• autopilot,
• usuwanie szumu z pomiarów czujników^[1],
• usuwanie szumu z nagrań mowy.

Rys historyczny

 Osobny artykuł: Historia automatyki.

Algorytm filtru Kalmana został zaproponowany w pracy Rudolfa Emila Kalmana A new approach to linear filtering and prediction problems, opublikowanej na łamach czasopisma Journal of Basic Engineering w 1960.

Założenie, dotyczące stacjonarności podstawowego sygnału użytecznego i szumu, było założeniem zasadniczym w teorii Wienera i Kołmogorowa. Dopiero na przełomie lat 50. i 60. XX w. rozwinięto nową teorię, która nie wymagała spełnienia założenia o stacjonarności. Opracowanie tej teorii było niejako wymuszone faktem, że często w praktyce miano do czynienia z sygnałami niestacjonarnymi i nie można było tego w żaden sposób ignorować. Teoria ta wkrótce przybrała nazwę teorii filtru Kalmana.

W 1960 Rudolf Kalman i jego współpracownicy pracujący w Stanach Zjednoczonych, opublikowali trzy najistotniejsze artykuły. W trzecim z artykułów Kalman opisał filtrację optymalną i teorię estymacji przedstawiając równania dyskretnego filtru Kalmana. Odpowiednik filtru Kalmana dla układów ciągłych opracowany został w 1961 (Kalman and Bucy 1961).

Początkowo, związek pomiędzy teorią Wienera i Kołmogorowa oraz Kalmana wydawał się wątpliwy, ponieważ wcześniejszą teorię opracowano w dziedzinie częstotliwości, a późniejszą – w dziedzinie czasu. Istnieje jednakże między nimi dość podstawowy związek, wynikający chociażby z faktu, że procesy stacjonarne są szczególnym przypadkiem niestacjonarnych. W chwili obecnej można łatwo wykazać, że teoria Wienera i Kołmogorowa jest szczególnym przypadkiem teorii Kalmana. Filtr Kalmana stanowi naturalne rozszerzenie filtru Wienera dla niestacjonarnych układów stochastycznych.

Obie teorie rozwinęły się, by sprostać potrzebom chwili, wynikającym z rozwoju techniki. Warto nadmienić, że możliwości realizacji obu rodzajów filtrów były dostosowane do istniejących warunków technologicznych. Filtry Wienera realizowano przy użyciu wzmacniaczy oraz elementów niezmiennych w czasie, jak oporniki i kondensatory, podczas gdy filtry Kalmana buduje się z zastosowaniem cyfrowych układów scalonych.

Z końcem listopada 1958, niedługo przed przejściem do Research Institute for Advanced Study w Baltimore, Kalman wracał pociągiem do Baltimore po wizycie na Uniwersytecie w Princeton. Około godziny 22.00 pociąg zatrzymał się na prawie godzinę tuż przed Baltimore. Było późno, był zmęczony i bolała go głowa. Gdy tak przez godzinę tkwił „uwięziony” w pociągu, przyszedł mu do głowy pomysł: Dlaczego by nie zastosować koncepcji zmiennych stanu do problemu filtracji Wienera? Był już zbyt zmęczony by rozwijać ten pomysł jeszcze tego wieczoru, jednak pamiętał o nim. Zapoznał się z książką Michel Loève’a na temat teorii prawdopodobieństwa, co pozwoliło mu zrealizować swoje zamierzenia. Miało to kluczowe znaczenie dla wyprowadzenia filtru Kalmana. Z dodatkowym założeniem o skończonej wymiarowości mógł wyprowadzić filtr Wienera, co dało w efekcie to, co nazywane jest dziś filtrem Kalmana. Przejście na postać zmiennych stanu uprościło matematyczne podstawy wyprowadzenia.

Kalman zaprezentował swoje nowe wyniki podczas rozmów z kilkoma uczelniami i laboratoriami badawczymi zanim opublikowano je drukiem. Jego idee spotkały się jednak z pewnym sceptycyzmem kolegów, co zmusiło Kalmana do wybrania, do publikacji, czasopisma naukowego z zakresu inżynierii mechanicznej (zamiast ewentualnego czasopisma z zakresu inżynierii elektrycznej) bo, jak wyraził się podczas sympozjum w Los Angeles 17 kwietnia 1991, „Gdy ktoś boi się stąpać po zapadłym gruncie obwarowanym partykularnymi interesami najlepiej jeśli przejdzie bokiem”. Jego drugi artykuł, który przedstawiał przypadek dla czasu ciągłego, został raz odrzucony bo – jak wyraził się jeden z osądzających – jeden z kroków w dowodzie nie może prawdopodobnie być zgodny z prawdą (w istocie jednak był zgodny z prawdą). Kalman uparcie prezentował swój filtr i w innych miejscach spotykał się z coraz większą, natychmiastową akceptacją. Niedługo potem jego idea stała się podstawą szeregu tematów badawczych na wielu uczelniach i przedmiotem prac doktorskich z zakresu inżynierii elektrycznej, jakie powstawały w następnych latach.

Kalman spotkał się z ciepłym przyjęciem audytorium podczas prezentacji jego filtru w 1960 w centrum badawczym NASA (Ames Research Center of NASA) w Mountain View w Kalifornii, gdzie przybył z wizytą do Stanley F. Schmidta. Kalman opisał swoje ostatnie wyniki i Schmidt docenił jego potencjalne możliwości zastosowania do problemów, jakie badano w Ames – do estymacji trajektorii i problemów sterowania w projekcie Apollo (planowanej misji na księżyc). Schmidt od razu rozpoczął pracę nad tym, co stało się prawdopodobnie pierwszą pełną implementacją filtru Kalmana. Wkrótce stał się pomysłodawcą filtru, który dziś znany jest pod nazwą rozszerzonego filtru Kalmana. Był on od tamtej pory zawsze używany do większości nieliniowych aplikacji filtru Kalmana dla czasu rzeczywistego. Pełen entuzjazmu po swoim sukcesie z filtrem Kalmana, zabrał się za nawracanie innych, którzy zajmowali się podobnymi pracami. W początkach 1961 Schmidt opisał swoje wyniki Richardowi H. Battinowi z MIT Instrumentation Laboratory (później zmieniło ono nazwę na Charles Stark Draper Laboratory). Battin używał już metod zmiennych stanu do projektowania i implementacji astronautycznych systemów naprowadzania i dzięki niemu filtr Kalmana, odpowiednio zaprojektowany i opracowany w Instrumentation Laboratory, stał się jedną z części systemu naprowadzania, jaki montowano na statkach kosmicznych w programie Apollo. W połowie lat 60., dzięki staraniom Schmidta, filtr Kalmana stał się częścią systemu nawigacji w samolocie transportowym C5A. Filtr Kalmana zastosowano do problemu łączenia danych z radaru z danymi sensorów bezwładnościowych, tak by uzyskać ogólną estymatę trajektorii samolotu oraz do problemu odrzucania danych, związanego z detekcją błędów egzogennych w danych pomiarowych. Od tego czasu filtr Kalmana stał się integralną częścią prawie każdego projektu pokładowego systemu estymacji trajektorii i sterowania nią.

Wiele z tego, co osiągnięto od czasu wprowadzenia filtru Kalmana, nie byłoby możliwe bez niego. W szczególności była to jedna z technologii, która umożliwiła nadejście epoki lotów kosmicznych. Precyzyjna i wydajna nawigacja statków kosmicznych w Układzie Słonecznym nie mogłaby być realizowana bez filtru Kalmana. Zasadnicze wykorzystanie filtracji Kalmana ma miejsce w systemach sterowania, przy nawigacji wszelkiego rodzaju pojazdów i przy predykcyjnym projektowaniu estymacji i sterowaniu systemów. Takie techniczne działania stały się możliwe właśnie dzięki filtrowi Kalmana.

Przypisy

Bibliografia

• Mohinder S. Grewal, Angus P. Andrews, Kalman Filtering: Theory and Practice Using Matlab, John Wiley and Sons, 2001, ISBN 0-471-39254-5.

Linki zewnętrzne

• A new approach to linear filtering and prediction problems – transkrypcja oryginalnego artykułu Rudolfa Kalmana, opublikowanego w Journal of Basic Engineering w 1960, w którym przedstawiony został po raz pierwszy opis filtru Kalmana (ang.)
• Kalman Filter Explained Simply – proste wyjaśnienie działania filtru Kalmana (ang.)