[Inne] Filtr Kalmana - Teoria i Praktyka

[Pitlab]

Fajnie się ogląda jak rzeczy wypracowane w symulacjach tak dobrze działają w rzeczywistych warunkach.

Dzisiaj wreszcie wgrałem kod na komputer w quadrokopterze i wylatałem jeden pakiet. Różnica w działaniu jest wyraźna. Przedtem gdy polegałem na danych z czujników filtrowanych dolnoprzepustowo, nie mogłem sobie poradzić z utrzymaniem wysokości. Oscylacje rzędu 2-3m to był normalka. Po pracy śmigieł słychać było jak pracuje człon różniczkujący szarpiąc silnikami. Teraz regulatory PID nie są jeszcze dostrojone do nowych warunków, ale na dzień dobry w ogóle nie słychać jazgotu śmigieł, tylko jednostajny szum. Sygnał wejściowy jest dużo gładszy, to i sygnał wyjściowy z członu różniczkującego też jest gładszy i nie katuje silników ciagłymi zmianami prędkości obrotwej.

Na początek podpiąłem 2 potencjometry w aparaturze pod współczynniki Kp członów P regulatora wysokości i regulatora prędkości pionowej - taki sposób regulacji on-line. Na obydwu podniosłem wzmocnienie dwukrotnie i nadal zachowuje się stabilnie. To akurat jest trochę podejrzane, bo niedawno rozszerzałem możliwości sterowania z jednego kanału na 2 i być może coś skopałem. Sprawa do weryfikacji. Tak wygląda fragment wykresu regulacji wysokości. Na górze pomarańczowa linia to wartość zadana (drążek aparatury), ciemnomiebieskie to wysokość z kalmana, jasnoniebieskie to wysokość z czujnika. Wszystko wyskalowane w metrach

Na dole żółta linie to wyjście regulatora PID a linie w kolorach R, G, B to składowe P, I oraz D

Jest wyraźny postęp chociaż jeszcze nie ma się czym chwalić dopóki nie nastroję regulatorów. Na razie podstawowy szum spadł od ok. 1 m, ale jeszcze praktycznie nie działa człon różniczkujący (niebieska linia na dolnym wykresie). Trzeba podkręcić czas różniczkowania, wtedy kopter będzie sztywniej trzymał się wartości zadanej. Na logach z koptera niewiele widać, bo miałem ustawione logowanie na 15 Hz. To jednak dosyć mało aby widzieć problemy z szumem. Teraz zmniejszyłem ilość logowanych danych i podniosłem częstotliwość do 25 Hz to w następnym locie będzie widać ciut więcej.

To że sygnał z czujników mocno pokrywa się z sygnałem z kalmana to w dużej mierze kwestia działania samego logera, który uśrednia dane. Dane wchodzą na 200 Hz a wychodzą na 15 Hz czyli na wykresie są uśrednione 13,3 raza. Chyba wyrzucę to uśrednianie bo na obecnym etapie rozwoju więcej psuje niż pomaga a zajmuje sporo czasu procesora.

Wiem już dlaczego zalecane jest dawanie jakiś empirycznych wartości do macierzy opisujących szum. Można próbować wyznaczać ją teoretycznie:

http://pl.wikipedia.org/wiki/Dyskretyzacja_%28matematyka%29#Dyskretyzacja_modelu_uk.C5.82adu_liniowego_w_przestrzeni_stan.C3.B3w

Obliczenia dla modelu z trzema zmiennymi. Wyznaczamy macierz V = [sh 0 0; 0 sv 0; 0 0 sa] odpowiadającą wariancji w czasie ciągłym. Wyznaczamy macierz A w czasie ciągłym [0 1 0; 0 0 1; 0 0 0]. Dalej trzeba bawić się z tymi równaniami całkowymi, co może być dosyć problematyczne, a i tak otrzymane tak wartości będą prawdziwe tylko jeżeli na układ nie działają żadne dodatkowe zakłócenia. Tak więc sposób z własnymi wariancjami jest dużo prostszy.

Ja wymiękam przy takich obliczeniach

Dzisiaj przed lotami trochę posiedziałem i zmieniałem parametry szumu poszczególnych czujników aby znaleźć zadowalające wartości współczynników kalmana. Z danych zbliżonych do rzeczywistych współczynnik kalmana dla korekcji wysokości wychodził dużo niższy niż dla prędkości. algorytm chciał bardziej polegać na sygnale z wariometru niż z wysokościomierza. Zrobiłem serię pomiarów aby znaleźć zależności między parametrami szumu a finalnymi współczynnikami kalmana. Wyszła mi taka tabelka:

Modyfikowałem dotąd aż uzyskałem współczynnik dla wysokości (K00) trochę większy niż dla prędkości (K11). Proporcje między nimi są już zachowane. Proporcje między korekcją a predykcją zmieniam modyfikując szum akcelerometru. Widać że wartości szumu nie mają nic wspólnego z warunkami rzeczywistymi, są kompletnie nieintuicyjne (0,01 i 2,0). Tak wiec ten model nie jest rzeczywisty ale działa i w sumie tyle wystarczy.

Zrobiłem te zmiany dla 3 zmiennych stanu. Teraz model matematyczny bardziej odpowiada rzeczywistości. Mamy pomiary pozycji, prędkości i przyspieszenia, brakuje informacji o sygnale sterującym i wszystkie 3 pomiary wymagają filtracji.

O rety! Ja już gonię na oparach wolnego czasu a tu do implementacji filtr na macierzy 3x3

Spróbuję to zakodować, ale potrzebuję kilku dni, bo już teraz mam trochę poprawek do uwzględnienia a żeby móc to porównać warto dopiąć na ostatni guzik.

[Treker (Damian Szymański)]

Artykuł został przeniesiony na blog: Filtr Kalmana w praktyce – 3 przykłady z kodami!

[Komzar]

Witam.

Sam troszkę zaczynam bawić się Kalmanem ale napotkałem na mały problem z którym nie umiem sobie poradzić. Może ktoś z Was będzie w stanie podesłać jakiś adekwatny link.

Też chcę estymować położenie łodzi/statku ale do dyspozycji mam kilka pomiarów położenia, powiedzmy 4 GPSy. Nie bardzo wiem jak je wszystkie wpleść w te macierze.

Istotne jest, że nie mogę wziąć ich średniej i traktować ich jako jeden pomiar bo potrzebuje niezależnej kowariancji błędu z macierzy P dla każdego z pomiarów.

Z góry dziękuje za podpowiedź.