Filtr Kalmana od teorii do praktyki - #1

[Komentator]

W niniejszej serii artykułów mam zamiar zająć się filtrami Kalmana. Zacznę od niezbędnej dawki teorii, by przejść do przykładów praktycznych.Najpierw na podstawie rzeczywistych danych pomiarowych z czujnika zaprojektujemy filtr w Matlabie, a potem przeniesiemy implementację na mikrokontroler STM32.

UWAGA, to tylko wstęp! Dalsza część artykułu dostępna jest na blogu.

Przeczytaj całość »

Poniżej znajdują się komentarze powiązane z tym wpisem.

[Bobby]

Jeśli ktoś ma problem w zrozumieniu dlaczego filtr Kalmana działa, polecam ten tekst: http://lirec.iiar.pwr.edu.pl/~konarch/download/algorytmy/kalman.pdf

[Treker (Damian Szymański)]

Bobby, dzięki za materiał uzupełniający

Pozostałych zachęcam do komentowania czy artykuł jest dla Was ciekawy i zrozumiały. Jest to dość trudny tekst i chciałbym wiedzieć, czy warto zajmować się takimi tematami w przyszłości.

[cyber31]

warto, warto dalej zgłębiać temat tym bardziej że robisz to w sposób merytoryczny

[Treker (Damian Szymański)]

cyber31, akurat nie ja jestem autorem - tekst pisał GAndalF - tak jak jest podane przy tekście Kolejna część pojawi się w ciągu najbliższych 2 tygodni.

[admunt1]

Polećcie coś do przeczytania gdzie znajdę odpowiedź jak dobrać macierze A, C, V i W - w zależności od ZMIENNYCH STANU ???. Czytałem w wielu publikacjach, że jest zapis macierzowy o określonym rozmiarze i zadanych wartościach ale brakuje mi informacji jak się je dobiera w zależności zmiennych stanu OBIEKTU który generuje oscylacje.

[mog123]

A i C określa Ci twój system - jednym słowem musisz nauczyć się modelować systemy za pomocą state-space

a V i W to zakłocenia procesowe i pomiarowe. Zarówno jedne jak i drugie nie muszą zawsze być obecne. No i ich rozmiary również wynikają z tego gdzie co i jak się zakłóca.

[admunt1]

Jak mogą wyglądać macierze A, C, V, W dla nieliniowego filtru Kalmana ?

Przykładowe równania, generujące zakłócenia przesyłam w załączniku poczty.

Pozdrawiam

Adam

[maniu]

admunt1

Jak mogą wyglądać macierze A, C, V, W dla nieliniowego filtru Kalmana ?

Nie sprowadzisz modelu do postaci równania stanu, jeżeli model jest nieliniowy. Cały myk polega na tym, aby zlinearyzować go wokół jakiegoś punktu pracy.

[GAndaLF]

Dla modelu nieliniowego stosujesz EKF. Macierz A tworzysz z pochodnych kolejnych równań stanu przez kolejne zmienne stanu, czyli dostajesz [df_1/dx_1 ... df_1/dx_n; ... df_n/dx_1 ... df_n/dx_n]. Macierz B tworzysz z pochodnych kolejnych równań stanu przez kolejne wejścia i dostajesz [df_1/du_1 ... df_1/du_n; ... df_n/du_1 ... df_n/du_n], a macierz C tworzysz z pochodnych równań wyjścia przez kolejne zmienne stanu, czyli [dg_1/dx_1 ... dg_1/dx_n; ... dg_n/dx_1 ... dg_n/dx_n]. Nowe wartości poszczególnych macierzy liczysz w każdej iteracji algorytmu i pod x_1 - x_6 podstawiasz aktualne estymacje zmiennych stanu.

[admunt1]

Co zmieni się w programie jeśli zmienią się rozmiary macierzy ?

Przykładowo w kolejności wiersz/kolumna: A(6x6); C(3x6); P0(6x6); x0(6x1)

Mam na uwadze linijki kodu jak:

% aktualizacja czas
xpri = A*xpost;
Ppri = A*Ppost*A' + V;

% aktualizacja pomiarow
eps = Y(i) - C*xpri;
S = C*Ppri*C' + W;
K = Ppri*C'*S^(-1);
xpost = xpri + K*eps;
Ppost = Ppri - K*S*K';

Co trzeba zmienić w xpri, xpost, Ppri itp. ???

[GAndaLF]

http://pl.wikipedia.org/wiki/Mno%C5%BCenie_macierzy

Musisz dobrać rozmiary poszczególnych macierzy tak, żeby ilość wierszy w pierwszej macierzy odpowiadała ilości kolumn w drugiej i vice versa. Jeżeli nie pasują ci rozmiary, użyj debuga i w miejscu gdzie program się wywala, wypisz na konsolę zawartość poszczególnych macierzy z mnożenia. W ten sposób sprawdzisz jak musisz pozmieniać rozmiary.

[admunt1]

Z pomocą GAndaLFa wypróbowałem filtr Kalmana dla różnych układów generujących szum, oscylacje, zakłócenia o charakterze nieliniowym i nieokresowym.

Posłużyłem się następującym równaniem, generującym oscylacje:

Model przedstawia zmodyfikowane równanie Vander Pola, używane do opisu drgań lampy elektronowej.

Widzimy , że jest to układ bez sygnału wymuszającego tzn. B=0, wzbudzany niezerowymi warunkami początkowymi tzn. x(0), y(0), z(0) ≠0.

Wektor równania stanu zapiszemy jako:

Równanie stanu przyjmuje postać:

Macierz stanu A(6x6) układu dla nieliniowego filtru Kalmana składa się z pochodnych kolejnych równań stanu. Dla uproszczenia przyjęto zerowy punkt pracy tzn. x1(0)=x2(0)=x3(0)=x4(0)=x5(0)=x6(0)=0;

Macierz stanu A ma postać:

Macierz wymuszeń B(6x1), ze względu na brak sygnału wymuszającego przyjmuje zerowe wartości:

Wektor równania wyjść, ma postać:

Zatem równanie wyjść, opiszemy jak:

Macierz wyjścia C układu dla nieliniowego filtru Kalmana – równania składa się z pochodnych kolejnych równań wyjść:

Macierz przenoszenia przyjęto D=0

Przyjęto warunki początkowe o następujących wartościach:

Dla odchylenia standardowego 10, przyjęto doświadczalnie kolejno macierze kowariancji szumów i procesu:

Poniżej zamieszczam wynik estymacji nieliniowego filtra Kalmana – przebiegi czasowe x(t), y(t), z(t):

[Imbryk]

Cześć, mam pytanie odnośnie przykładu 1. We wzorze aktualizacji czasu jest xpri = A*xpost; Dlaczego nie korzystamy tam z macierzy B*u zgodnie ze wzorem xpri = A*xpost + B*u?

[kamolix321]

Przeanalizowałem program i nie wiem czy czegoś nie rozumiem czy może jest błąd w programie. Swoje wątpliwości mam po przeczytaniu tego artykułu na stronie MIT. Najlepiej je ilustruje obrazek przedstawiony poniżej.

Dotyczą one dwóch poniższych linijek kodu.

S = C*Ppri*C' + W;

Czy w tym miejscu zamiast W nie powinno być V?

Ppri = A*Ppost*A' + V;

W tym zaś zamiast V nie powinno być W?