Zaskakująco mało inżynierów oprogramowania i naukowców wydaje się to rozumieć, co mnie zasmuca, ponieważ jest to tak ogólne i potężne narzędzie do łączenia informacji w obecności niepewności. czasami jego zdolność do wydobywania dokładnych informacji wydaje się prawie magiczna – a jeśli wydaje się, że mówię to w nadmiernej ilości, to spójrz na ten wcześniej zamieszczony film, w którym demonstruję filtr Kalmana decydujący o orientacji ciała swobodnie pływającego, obserwując jego prędkość. Zupełnie schludnie!

Co to jest filtr Kalmana?

Filtr Kalmana można stosować wszędzie tam, gdzie nie ma pewności co do informacji o jakimś dynamicznym systemie i można świadomie odgadnąć, do czego ten system zmierza. Chociaż rzeczywistość jest nieporządna i zakłóca czysty ruch, o którym się domyślacie, filtr Kalmana często dobrze się sprawdza przy podejmowaniu decyzji o tym, co się faktycznie wydarzyło. I może kasować korelacje pomiędzy szalonymi zjawiskami, których po prostu być może nie pomyślelibyście wykorzystać!

Filtry Kalmana idealnie nadają się do systemów, które ciągle się zmieniają. Mają tę zaletę, że są lekkie w pamięci (nie muszą trzymać żadnej historii poza poprzednim stanem), i że są w mgnieniu oka, co sprawia, że są kompatybilne z problemami w czasie rzeczywistym i systemami wbudowanymi.

Matematyka implementacji filtra Kalmana wydaje się dość przerażająca i nieprzejrzysta w większości miejsc odkrytych w Google. To paskudny stan rzeczy, ponieważ filtr Kalmana jest naprawdę super prosty i prosty, aby wiedzieć, czy sprawdzasz go w odpowiedni sposób. Dzięki temu jest to świetny temat na artykuł, a ja planuję oświetlić go wieloma jasnymi, ładnymi zdjęciami i kolorami. Warunki wstępne są proste; wszystko co chciałbyś wiedzieć to podstawowe zrozumienie prawdopodobieństwa i matryc.

Zacznę od luźnego przykładu tego typu rzeczy, które może rozwiązać filtr Kalmana, ale jeśli chciałbyś od razu przejść do błyszczących obrazków i matematyki, z przyjemnością przeskoczymy do przodu.

Co możemy zrobić z filtrem Kalmana?

Zróbmy przykład z zabawki: Zbudowałeś robota dotykowego, który będzie włóczył się po lesie, więc robot musi dokładnie wiedzieć, gdzie jest, żeby móc nawigować.

Twój mały robot

Powiemy, że nasz robot posiada stan xk→, który jest po prostu krawędzią i prędkością:

xk→=(p⃗ , v⃗ )

Zauważ, że stan jest po prostu inwentaryzacją liczb dotyczących podstawowej konfiguracji Twojego systemu; może to być cokolwiek. W naszym przykładzie jest to pozycja i prędkość, ale mogą to być dane o ilości płynu w zbiorniku, temperaturze silnika samochodu, pozycji palca użytkownika na panelu dotykowym, lub dowolnej ilości rzeczy, które musisz śledzić.

Nasz robot posiada również czujnik GPS, który jest dokładny do około 10 metrów, co jest słodkie, ale musi znać swoje położenie dokładniej niż 10 metrów. W tych lasach jest wiele wąwozów i klifów, a jeśli robot pomyli się o kilka stóp, może spaść z klifu. Więc GPS sam w sobie nie jest w porządku .

O nie.

Możemy też wiedzieć coś o tym, jak robot się porusza: Zna polecenia wysyłane do silników kół i wie, że jeśli zmierza w jednym kierunku, a zamek błyskawiczny przeszkadza, to w następnej chwili prawdopodobnie będzie dalej w tym samym kierunku. Ale w rzeczywistości nie wie wszystkiego o swoim ruchu: będzie zderzony z wiatrem, koła mogą się trochę poślizgnąć lub przetoczyć po wyboistym terenie; dlatego też ilość obrotów kół nie będzie dokładnie odzwierciedlać tego, jak daleko robot faktycznie się posunął, a zatem przewidywania nie będą idealne.

Czujnik GPS mówi nam coś o stanie, ale tylko pośrednio i z pewną dozą niepewności lub niedokładności. Nasza prognoza mówi nam coś o tym, jak robot porusza się, ale tylko pośrednio i z pewną niepewnością lub niedokładnością.

Ale jeśli wykorzystamy całą dostępną nam wiedzę, czy możemy uzyskać dużo lepszą odpowiedź, niż którakolwiek z nich sama by nam dała? W rzeczywistości rozwiązanie jest tak, i do tego właśnie służy filtr Kalmana.

Jak filtr Kalmana widzi twój problem

Spójrzmy na krajobraz, który próbujemy zinterpretować. Będziemy kontynuować stan łatwy, mając tylko pozycję i prędkość.

x⃗ =[pv]

Nie wiemy, jaka jest dana pozycja i prędkość; istnieje cały szereg możliwych kombinacji pozycji i prędkości, które mogą być prawdziwe, ale wiele z nich jest bardziej prawdopodobnych niż inne:

Filtr Kalmana zakłada, że obie zmienne (postion i prędkość, w naszym przypadku) są przypadkowe i rozłożone na Gaussian. Każda zmienna posiada średnią μ, czyli środek rozkładu losowego (i jego przypuszczalny stan), oraz wariancję σ2, czyli niepewność:

gauss_1

Na powyższym obrazku pozycja i prędkość są nieskorelowane, co sugeruje, że stan 1 zmiennej nie mówi nic o tym, co może być odwrotne.

Poniższy przykład pokazuje coś bardziej interesującego: Pozycja i prędkość są skorelowane. Prawdopodobieństwo zaobserwowania określonej pozycji zależy od tego, jaką masz prędkość:

gauss_3 Ta dość trudna sytuacja może się pojawić, jeśli, na przykład, oszacujemy pozycję zastępczą obsługiwaną przez starą. Jeśli nasza prędkość była wysoka, prawdopodobnie przenieśliśmy się dalej, więc nasza pozycja będzie bardziej odległa. Jeśli poruszamy się powoli, nie dotarliśmy tak daleko.

Ten rodzaj relacji jest w zasadzie ważny, aby pozostać na bieżąco, ponieważ daje nam to więcej informacji: Jeden pomiar mówi nam coś o tym, czym mogą być inni . I taki jest cel filtra Kalmana, chcielibyśmy wycisnąć maksymalną ilość informacji z naszych niepewnych pomiarów, jak tylko możemy!

Ta korelacja jest uchwycona przez coś, co nazywa się matrycą kowariancji. W skrócie, każdy element macierzy Σij jest to, że stopień korelacji między ith zmiennej stanu, a zatem jth zmiennej stanu. (Możesz być gotów zgadnąć, że matryca kowariancji jest symetryczna, co sugeruje, że nie ma znaczenia czy zamienisz i i i j). Macierze kowariancji s± często oznaczone jako “Σ”, więc nazywamy ich elementy “Σij”.

gauss_2

Opisanie sprawy za pomocą matryc

Modelujemy naszą wiedzę o stanie jako gaussian blob, więc chcielibyśmy mieć dwie dane w czasie k: Nazwiemy nasze największe oszacowanie x^k (średnia, gdzie indziej nazwana μ ), a jego kowariancja matrycą Pk.

x^kPk=[prędkość położenia]=[ΣppΣvpΣpvvv](1)

(Oczywiście używamy tu tylko pozycji i prędkości, ale warto pamiętać, że stan może zawierać dowolną ilość zmiennych i reprezentować co tylko chcemy).

Następnie chcielibyśmy dowiedzieć się, jak wyglądać w obecnym stanie (w czasie k-1) i przewidywać kolejne stany w czasie k. Pamiętajmy, że nie wiemy, który stan jest tym “prawdziwym”, ale nasza funkcja przewidywania nie ma znaczenia. Po prostu działa ona na wszystkie z nich i zapewnia nam dystrybucję zastępczą:

gauss_7

gauss_8 Bierze każdy punkt naszego oryginalnego oszacowania i przenosi go do przewidywanego miejsca wymiany, które jest miejscem, w którym system przesunąłby się, gdyby ten oryginalny oszacowanie było właściwe.

gauss_8

Zastosujmy to. Jak użylibyśmy macierzy do przewidywania pozycji i prędkości w kolejnym momencie w przyszłości? Użyjemy bardzo podstawowego wzoru kinematycznego:

pkvk=pk-1+Δt=vk-1vk-1

Innymi słowy:

x^k=[10Δt1]x^k-1=Fkx^k-1(2)(3)

Mamy teraz matrycę przewidywań, która zapewnia nam nasz następny stan, ale nadal nie potrafimy zaktualizować matrycy kowariancji.

W tym miejscu chcielibyśmy skorzystać z innej formuły. Jeśli pomnożymy każdy punkt podczas dystrybucji przez matrycę A, to co stanie się z jej matrycą kowariancji Σ?

Cóż, to jest proste. Zaoferuję ci tylko tożsamość:

Cov(x)Cov(Ax)=Σ=AΣAT(4)

Zatem połączenie (4) z równaniem (3):

x^kPk=Fkx^k-1=FkPk-1FTk(5)

Wpływ zewnętrzny

Ale nie udało nam się złapać wszystkiego. Mogą być pewne zmiany, które nie są związane z samym stanem – świat powierzchniowy może mieć wpływ na system.

Na przykład, jeśli stan modeluje ruch pociągu, operator pociągu może nacisnąć na przepustnicę, powodując przyspieszenie pociągu. Podobnie, w naszym przykładzie robota, oprogramowanie nawigacyjne może wydać polecenie pokazania kół lub zatrzymania. Jeśli wszyscy znamy tę dodatkową informację o tym, co dzieje się na świecie, możemy ją wpisać do wektora o nazwie uk→, zrobić coś z nią i dodać ją do naszej prognozy jako korektę.

Powiedzmy, że wszyscy znamy oczekiwane przyspieszenie a dzięki ustawieniu przepustnicy lub komendom sterującym. Z podstawowej kinematyki otrzymujemy:

pkvk=pk-1+Δt=vk-1+vk-1+12aΔt2aΔt

W formie matrycy:

x^k=Fkx^k-1+[Δt22Δt]a=Fkx^k-1+Bkuk→(6)

Bk nazywany jest matrycą sterowania, a uk→ wektorem sterowania. (W przypadku bardzo prostych systemów bez wpływu z zewnątrz, można je pominąć).

Dodajmy jeszcze jeden szczegół. Co się stanie jeśli nasza prognoza nie będzie w 100% dokładnym modelem tego co się dzieje?

Zewnętrzna niepewność

Wszystko jest w porządku, jeśli stan ewolucji wspiera swoje własne właściwości. Wszystko pozostaje w porządku, jeśli stan rozwija się wspomagany siłami zewnętrznymi, pożegnaj się, bo wszyscy wiemy czym są te siły zewnętrzne.

Ale co z siłami, o których nie wiemy? Jeśli na przykład śledzimy czworoosobowy pojazd, może on być buforowany przez wiatr. Jeśli śledzimy robota na kółkach, koła mogą się ślizgać lub wyboje na dnie mogą go spowolnić. Nie możemy śledzić tych rzeczy, a jeśli do tego dojdzie, nasza prognoza może się nie powieść, ponieważ nie wzięliśmy pod uwagę tych dodatkowych sił.

Możemy modelować niepewność związaną ze “światem” (tzn. tym, czego nie śledzimy), dodając po każdym kroku przewidywania nową niepewność:

gauss_9

Każdy stan w naszych pierwotnych szacunkach mógł przenieść się do różnych stanów. Ponieważ tak bardzo lubimy kleksy gaussowskie, powiemy, że każdy punkt w x^k-1 jest przenoszony gdzieś wewnątrz kleksa gaussowskiego z kowariancją Qk. Inaczej mówiąc, często traktujemy nieśledzone wpływy jako hałas z kowariancją Qk.

gauss_10aWytwarza to zastępczy kowariancję Gaussian blob, o specjalnej kowariancji (ale równoważnej średniej):

gauss_10a

Rozszerzoną kowariancję otrzymujemy przez proste dodanie Qk, co daje nam pełne wyrażenie dla kroku przewidywania:

x^kPk=Fkx^k-1+Bkuk→=FkPk-1FTk+Qk(7)

Innymi słowy, nowy najlepszy szacunek może być prognozą składającą się z poprzedniego najlepszego szacunku, plus korekta o znane wpływy zewnętrzne.

Nowa niepewność jest przewidywana na podstawie starej niepewności, z pewną dodatkową niepewnością dotyczącą środowiska.

gauss_10b

W porządku, więc to jest wystarczająco proste. Mamy rozmyte szacunki gdzie nasz system może być, podane przez x^k i Pk. Co się stanie, gdy dostaniemy trochę danych z naszych czujników?

Udoskonalenie szacunku za pomocą pomiarów

Możemy mieć kilka czujników, które dadzą nam informacje o stanie naszego systemu. Dla nonce nie ma znaczenia, co mierzą; być może jeden odczytuje pozycję, a więc i prędkość. Każdy czujnik mówi nam coś pośredniego o stanie, czyli inaczej mówiąc, czujniki obsługują stan i wytwarzają grupę odczytów.

gauss_12

gauss_12

gauss_12Notice that the units and scale of the reading will not be an equivalent because the units and scale of the state we’re keeping track of. you would possibly be ready to guess where this is often going: Zamodelujemy czujniki z matrycą, Hk.

Dowiemy się, jaki jest rozkład odczytów czujników, który chcemy ustalić w zwykły sposób:

μ⃗ expectedΣexpected=Hkx^k=HkPkHTk(8)

Filtry Kalmana świetnie nadają się do obsługi szumów z czujników. Innymi słowy, nasze czujniki są mało wiarygodne, a każdy stan w naszych pierwotnych oszacowaniach może zakończyć się różnymi odczytami czujników.gauss_12

gauss_12

Z każdego odczytu, który obserwujemy, domyślamy się, że nasz system był w danym stanie. Ale ponieważ istnieje niepewność, niektóre stany są bardziej prawdopodobne, że niektóre z nich stworzyły odczyt, który widzieliśmy:gauss_11

gauss_11

Nazwiemy kowariancję tej niepewności (tj. szumu czujnika) Rk. Rozkład charakteryzuje się średnią odpowiednią do obserwowanego przez nas odczytu, którą nazwiemy zk→.

Tak więc teraz mamy dwie kropelki Gaussian: Jeden otaczający średnią z naszych przekształconych przewidywań, drugi otaczający konkretny odczyt czujnika, który otrzymaliśmy w liczbie

gauss_4

Musimy spróbować pogodzić nasze przypuszczenia co do odczytów, które widzieliśmy obsługiwane przewidywany stan (różowy) ze specjalnym zgadywaniem obsługiwanych naszych czujników (zielony), które faktycznie zaobserwowaliśmy.

Jaki jest więc nasz nowy prawdopodobny stan? Dla każdego możliwego odczytu (z1,z2), mamy dwa powiązane prawdopodobieństwa: (1) Prawdopodobieństwo, że nasz odczyt czujnika zk→ może być (błędnym) pomiarem (z1,z2), oraz (2) prawdopodobieństwo, że nasza poprzednia estymata myśli (z1,z2) jest tym, że odczyt, który zawsze powinniśmy zobaczyć.

Jeśli mamy dwa prawdopodobieństwa i chcemy zrozumieć perspektywę, że oba są prawdziwe, po prostu pomnożymy je razem. Tak więc, bierzemy 2 kleksy gausowskie i mnożymy je:

gauss_5

Pozostaje nam tylko nakładanie się, region, w którym obie plamy są jasne/ prawdopodobne. I jest to o tony dokładniejsze niż którekolwiek z naszych poprzednich szacunków. Średnia z tego rozkładu jest taka, że konfiguracja, która oba szacunki są przypuszczalnie , a więc jest najprostszym zgadywaniem konfiguracji prawdy, biorąc pod uwagę całą naszą wiedzę .

Hmm. Wygląda to na kolejny kleks Gaussian.

gauss_6

Jak się wydaje, po pomnożeniu dwóch kleksów Gaussian z oddzielnymi środkami i matrycami kowariancji, otrzymujesz zastępczy kleks Gaussian z własną średnią i matrycą kowariancji! Może zobaczysz, dokąd to często zmierza: Potrzebna jest formuła, która będzie namawiać do nowych parametrów ze starych!