Popularny post Elektryk0 Napisano Październik 1, 2012 Popularny post Udostępnij Napisano Październik 1, 2012 W artykule, postaram się opisać w prosty sposób filtr kalmana. Zostanie tutaj podana minimalna dawka teorii, którą musimy dysponować aby zrozumieć jak działa filtr oraz jak poprawnie go stosować. Mam nadzieję, że uda mi się przedstawić to zagadnienie w sposób łatwy do zrozumienia. Napiszemy także kilka programów, które pokażą na przykładach zastosowanie filtru kalmana w różnych sytuacjach życiowych. Programy będą napisane w Octave oraz w C++ z użyciem biblioteki OpenCV. Algorytm zgodnie z którym działa FK jest prosty w implementacji, dodatkowo nie wymaga złożonych obliczeń, a więc analiza zamieszczonych kodów nie powinna sprawić trudności. Najważniejszą rzeczą przy implementacji FK jest opisanie poprawnymi równaniami naszego procesu i systemu pomiarowego. Postaram się na prostym przykładzie pokazać jak robi się to krok po kroku, dlatego na początek zaczniemy od przyswojenia niezbędnej teorii. 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. 3 Cytuj Link do komentarza Share on other sites More sharing options...
Elvis Październik 1, 2012 Udostępnij Październik 1, 2012 Artykuł bardzo dobry, właśnie przeliczam wszystko "na piechotę" - inaczej ciężko to wszystko zrozumieć. Na razie mam uwagę do: Chyba pomylone są indeksy w drugim wierszu - wcześniej z(k) obliczane jest z aktualnego stanu, a tutaj z poprzedniego. Cytuj Link do komentarza Share on other sites More sharing options...
Elektryk0 Październik 1, 2012 Autor tematu Udostępnij Październik 1, 2012 Osobiście spotkałem się z obiema wersjami 🙂 W każdym razie dzięki za zwrócenie uwagi. A błędne fragmenty poprawiłem tak by trzymać się jednej wersji i nie robić zamieszanie. W najbliższym czasie postaram się jeszcze raz wszystko prześledzić i jak by co wprowadzę zmiany. Jeśli znajdziesz coś jeszcze to daj znać. Cytuj Link do komentarza Share on other sites More sharing options...
Marooned Październik 1, 2012 Udostępnij Październik 1, 2012 Chętnie bym poznał jeszcze łopatologicznie wytłumaczoną różnicę między opisaną tu wersją podstawową a jego rozszerzoną wersją, która jest stosowana w mocniejszych kontrolerach lotu dla kopterów i samolotów. Cytuj Link do komentarza Share on other sites More sharing options...
Polecacz 101 Zarejestruj się lub zaloguj, aby ukryć tę reklamę. Zarejestruj się lub zaloguj, aby ukryć tę reklamę. Produkcja i montaż PCB - wybierz sprawdzone PCBWay! • Darmowe płytki dla studentów i projektów non-profit • Tylko 5$ za 10 prototypów PCB w 24 godziny • Usługa projektowania PCB na zlecenie • Montaż PCB od 30$ + bezpłatna dostawa i szablony • Darmowe narzędzie do podglądu plików Gerber Zobacz również » Film z fabryki PCBWay
GAndaLF Październik 1, 2012 Udostępnij Październik 1, 2012 EKF to rodzaj filtru Kalmana dla układów nieliniowych, gdzie wykonywana jest linearyzacja wokół pewnego punktu pracy. Dlatego EKF dobrze działa tylko w pewnym zakresie w pobliżu tego punktu. A w kopterach jest wykorzystywany dlatego, że ruch w układzie kartezjańskim jest opisywany za pomocą funkcji trygonometrycznych (rzuty na osie x y z), które oczywiście są nieliniowe. Cytuj Link do komentarza Share on other sites More sharing options...
Marooned Październik 2, 2012 Udostępnij Październik 2, 2012 A w kopterach jest wykorzystywany dlatego, że ruch w układzie kartezjańskim jest opisywany za pomocą funkcji trygonometrycznych (rzuty na osie x y z), które oczywiście są nieliniowe.Zapewne wykażę się ignorancją, ale geometrię miałem ostatnio z 15 lat temu. Możesz pokrótce wyjaśnić dlaczego rzuty na osie są nieliniowe? Czy miałeś na myśli np. obrót, który po rzuceniu na oś będzie sinusem? Jeśli tak, to wszystko jasne. Wiem, że EKF wykorzystywany jest głównie do filtrowania sygnałów z GPSa. I na tym chwilowo kończy się moja wiedza w tym zakresie. Cytuj Link do komentarza Share on other sites More sharing options...
GAndaLF Październik 25, 2012 Udostępnij Październik 25, 2012 Chodzi o to, że obiekty mobilne mogą się poruszać w dowolnym kierunku, który prawie nigdy nie jest równoległy do którejś z osi układu odniesienia. Podczas modelowania przypisujesz takiemu ruchowi jakiś wektor prędkości, przyspieszenia, siły itp a następnie musisz go rozbić na składowe x, y, z, gdzie dwie składowe i wektor początkowy tworzą trójkąt prostokątny. Dzięki temu możesz obliczyć niezależnie zmianę w każdym kierunku. Na pewno na fizyce miałeś kiedyś zadania z równią pochyłą. Tam się w ten sposób rozkładało siłę grawitacji. Cytuj Link do komentarza Share on other sites More sharing options...
Marooned Październik 26, 2012 Udostępnij Październik 26, 2012 To jasne, że poruszanie się w przestrzeni 3D rzadko odbywa się po osiach XYZ, ale rzutowanie ruchu na te osie nadal jest liniowe tylko zeskalowane o cosinus kąta między osią rzutowania a osią ruchu. Ale jest to skala stała (przy ruchu prostoliniowym oczywiście, no ale taki rozważamy w krótkich odstępach czasu). Cytuj Link do komentarza Share on other sites More sharing options...
GAndaLF Październik 28, 2012 Udostępnij Październik 28, 2012 Podstawianie pod funkcje trygonometryczne konkretnych wartości odpowiadających aktualnym kątom to właśnie linearyzacja modelu dla zadanego punktu pracy. Czyli jeden z dodatkowych kroków który wykonujesz w EKFie. Przy czym tej linearyzacji nie możesz dokonać raz na początku symulacji, musisz uaktualniać równania stanu w każdej iteracji, żeby twój model dobrze odzwierciedlał rzeczywisty obiekt. Cytuj Link do komentarza Share on other sites More sharing options...
Pitlab Grudzień 2, 2012 Udostępnij Grudzień 2, 2012 Cześć Na początek wielkie dzięki za świetny artykuł i wskazanie takiego narzędzia jak Octave. Zaimplementowałem ten filtr w swoim komputerze pokładowym AutoPitLot do bardzo podobnego zastosowania a mianowicie do filtrowania wysokości. W komputerze mam pomiar przyspieszenia, wariometr mierzący prędkość pionową i pomiar wysokości barometrycznej. Pomiary działają, ale mam problemy (jeszcze przed zaimplementowaniem filtru Klamana) ze sterowaniem w pętli PID, prawdopodobnie wynikające z opóźnień generowanych na filtrach wygładzających pomiary. Sytuacja aż prosi się o rozsądny filtr nie wprowadzający dużych opóźnień. Analogicznie do przykładu z łódką droga u mnie jest wysokością, prędkość jest prędkością pionową a przyspieszenie jest składową przyspieszenia w osi Z (już po transformacji układu współrzędnych z lokalnego do globalnego, tak że pochylenie i przechylenie nie zmieniają wskazań przyspieszenia). W tym układzie predykcja "zasilana" jest przyspieszeniem a korekcja jest robiona od wysokości (przydało by się jeszcze od prędkości, ale w tym miejscu macierz w wyniku obliczeń zwija się do rozmiaru 1x1 i nie ma jak podać prędkości). Na obrazku jest kilka wykresów. Znaczenie kolorów: niebieski - wysokość, zielony - prędkość, czerwony - przyspieszenie. Na górnym wykresie jest odchylenie standardowe szumu (szum wysokości jest testowo lekko przefiltrowany, ale to szczegół) Niżej dwa niebieskie wykresy to wysokość. Jasna, pastelowa linia to wartość mierzona, intensywny kolor to estymowana filtrem, konkretnie wartość X[0] (w łódce to było xhat). Niżej zielone wykresy to pomiar (pastelowy) i filtrowany (intensywny zielony) Niżej jest pomiar przyspieszenia czerwonym pastelowym. Na samym dole są współczynniki kalmana K[0] odpowiadający wysokości na niebiesko i K[1] odpowadający prędkości na zielono. Pokazane jest kilka ruchów między podłogą a szafą mającą 201cm. Filtr nadąża za danymi, ale dosyć wolno. Na tyle wolno że przy szybkich ruchach ogranicza amplitudę wskazań. Dla moich potrzeba jest to stanowczo za wolno. Co ciekawe w przypadku prędkości wynik filtru wyprzedza w czasie pomiar. Być może jest to efekt filtrowania sprzętowego w obwodach wariometru. Na początku miałem stałe parametry dotyczące szumu procesu i pomiaru. Kręcenie nimi daje pewne pole do popisu, ale w rzeczywistym układzie mocno narażonym na drgania chcę aby filtr sam dopasowywał się do warunków, stąd w tym przykładzie szum procesu i pomiaru są okresowo (co 1s) uaktualniane bieżącymi danymi statystycznymi o sygnałach. Te aktualizacje widać jako skoki zielonego współczynnika kalmana wysokości na samym dole. Mam wątpliwości do zachowania się współczynnika kalmana K[0] oznaczającego proporcję danych estymowanych do mierzonych. O ile K[1] odpowiadający prędkości ładnie rośnie wraz ze wzrostem odchylenia standardowego, to K[0] odpowiadający wysokości oscyluje ciągle w okolicy 0,05. Jeszcze mam wątpliwość co do wielkości macierzy H, jest to pozioma macierz [2x1]. W miejscu gdzie liczymy współczynnik kalmana wzorem: K = P * H' * inv( H * P * H' + R); są wykonane 3 operacje H * P * H' . Rozmiar macierzy P to 2x2. Jeżeli pomnożymy macierze [H0 H1] * [P0 P1;P2 P3] to wynik będzie macierzą [HP0 HP1] czyli zredukuje się do 2x1 Jeżeli teraz pomnożymy go przez H' czyli [H0; H1] (macierz pionową) to otrzymamy wynik jednoelementowy. Próbowałem używać w octave macierzy H o rozmiarze 2x2, ale nie potrafię go ogarnąć i filtr wzbudza się okresowo. Może autor będzie w stanie to przeanalizować. Po prostu nie wiem, czy to przypadkiem nie jest błąd. Zależało by mi też na możliwości korekcji od prędkości ale we wzorze: xhat = xhat + K*(z - H*xhat); wynik mnożenia H*xhat jest jednoelementowy i można go odjąć tylko od jednego pomiaru z a chciałbym aby to była macierz 1x2 i można było korygować od dwóch parametrów. Cytuj Link do komentarza Share on other sites More sharing options...
GAndaLF Grudzień 4, 2012 Udostępnij Grudzień 4, 2012 Macierz H jest w artykule dosyć niezręcznie nazwana, ponieważ jest to po prostu macierz C modelu stanowego wyrażającego się wzorem: x(t+1) = A*x(t) + B*u(t) (równanie stanu) y(t) = C*x(t) + D*u(t) (równanie wyjścia) Tak samo szumy procesowy i pomiarowy w innej nomenklaturze oznaczane są odwrotnie, ale to już szczegół: -v to szum procesowy dodawany do równania stanu, jego wariancja jest często oznaczana jako V a nie Q -w to szum pomiarowy dodawany do równania wyjścia, jego wariancja jest często oznaczana jako W a nie R Jeżeli chcesz mieć korekcję od położenia i prędkości jednocześnie musisz zmodyfikować macierz H do postaci [1 0; 0 1]. Wtedy tak naprawdę y(t) = x(t). Tą modyfikację musisz uwzględnić we wszystkich równaniach filtru, żeby rozmiary macierzy się zgadzały. W teorii wzmocnienie Kalmana powinno się ustalać na jakiś konkretnych wartościach wraz z upływem czasu. Dlatego K[0] zachowuje się poprawnie, a to właśnie prędkość działa niepoprawnie. Prawdopodobnie dzieje się tak dlatego, że w twoim aktualnym modelu stanowym A,B,C(H),D zmienna stanu odpowiedzialna za prędkość jest nieobserwowalna. Bardzo możliwe, że jak zmodyfikujesz macierz H tak, aby prędkość była również śledzona na wyjściu i uaktualnisz wszystkie równania, parametr K[1] również będzie się zmieniał w niewielkich granicach. Jednak moim zdaniem ta ingerencja w filtr i dodatkowa aktualizacja danych o szumach jest niepotrzebna i może być nawet szkodliwa. W artykule nie zostało to wyraźnie powiedziane, ale filtr Kalmana opiera się na teorii sterowania i rachunku prawdopodobieństwa. Jest to obserwator stanu czyli układ, który na podstawie analizy sygnału wyjściowego y(t), czyli wyjścia, przybliża wewnętrzny stan obiektu czyli x(t). Przybliżeniem stanu jest właśnie ten x z daszkiem. W pierwszych iteracjach te wskazania są błędne co ma związek z inicjalizowanymi wartościami parametrów. Jednak obserwatory zmniejszają swój błąd w każdym kroku w sposób wykładniczy, aby po kilku krokach poprawnie estymować stan obiektu. Teoria związana z obserwatorami nie uwzględnia jednak wpływu szumów. Dlatego filtr Kalmana dodatkowo wykorzystuje informacje o niedokładnościach w postaci macierzy kowariancji dla szumu pomiarowego i procesowego i minimalizuje powstały w ten sposób błąd zgodnie z kryterium minimalnych kwadratów. Filtr Kalmana jest więc obserwatorem stanu działającym w sposób optymalny (najlepiej i najszybciej niweluje błąd estymacji stanu) dla zadanych warunków. Jeżeli warunki (najczęściej wariancje) podane filtrowi nie zgadzają się z rzeczywistymi, istnieje wtedy inny filtr Kalmana, który działa bardziej optymalnie. Jednak jeżeli te wariancje nie są diametralnie różne od rzeczywistych w dalszym ciągu otrzymujemy estymator stanu o bardzo dobrych parametrach a dodatkowo mamy większą pewność, że jest odporny na szumy. Nie musisz więc aktualizować informacji o szumach ponieważ w ten sposób sprawiasz, że zmieniły się warunki i filtr znowu przez kilka kroków musi nadganiać rzeczywisty stan. Wystarczy, że na początku założysz bezpieczną wariancję, a algorytm sobie poradzi. Cytuj Link do komentarza Share on other sites More sharing options...
Pitlab Grudzień 4, 2012 Udostępnij Grudzień 4, 2012 Macierz H jest w artykule dosyć niezręcznie nazwana, ponieważ jest to po prostu macierz C modelu stanowego Dzięki za odpowiedź. Ja też do swoich potrzeb nazywam to macierzą C, ale w wikipedii w wersji angielskiej używane jest oznaczenia H. Różne szkoły mają różne nazwy. Jeżeli chcesz mieć korekcję od położenia i prędkości jednocześnie musisz zmodyfikować macierz H do postaci [1 0; 0 1]. Wtedy tak naprawdę y(t) = x(t). Tą modyfikację musisz uwzględnić we wszystkich równaniach filtru, żeby rozmiary macierzy się zgadzały. Tak własnie robię, ale coś jest nie tak, bo na początku wygląda dobrze ale w okolicach 6 iteracji zaczyna rosnąć kowariancja (macierz P), rosną współczynniki kalmana i wyglądają tak jak na obrazku niżej: Podobnie zmienia się estymata: Zmodyfikowany kod bazujący na kodzie łódki wygląda tak jak niżej. Zmieniłem sposób wywołania bez użycia funkcji, zmieniłem kilka nazw, np. H na C, xhat na x. Dodałem też zakomentowanych poleceń wyświetlenia wartości danych, aby debugować algorytm. duration = 6 %czas trwania symulacji [s] step = 0.05 %krok symulacji [s] measnoise = [0.5; 0.5]; %wspolczynnik szumu pomiaru accnoise = 0.2; %wspolczynnik szumu przyspieszenia T = step; R = [0.5^2 0.15; 0.15 0.3^2]; %Wyznaczenie kowariancji szumu pomiaru Q = accnoise^2 * [(T^2/2)^2 T*0.02; T*0.02 0.04]; %Kowariancja szumu procesu A = [1 T; 0 1]; %Macierz przejscia stanu poprzedniego do obecnego B = [T^2 /2; T]; %Macierz przejscia dla wejscia C = [1 0; 0 1]; %Macierz przejscia dla mierzonej wielkosci z korekcją dwu wartosci %Inicjalizacja I = [ 1 0; 0 1 ]; %Macierz jednostkowa P = Q; %Poczatkowa macierz kowariancji P x = [0; 0]; %wektor stanu xsym = x; %symulowana wartość "prawdziwa" u = 0.3; %Przyspieszenie [m/s*s] %Przygotowanie miejsca na dane pos = []; %Prawdziwa pozycja poshat = []; %Pozycja estymowana filtrem kalmana posmeas = []; %Pozycja mierzona speed = []; %prawdziwa prędkość speedhat = []; %prędkość estymoana filtrem speedmeas = []; %predkość mierzona kpos = []; %współczynnik kalmana dla wysokości kspeed = []; %wspólczynnik kalmana dla prędkości %Rozpoczecie symulacji for t = 0:T:duration, %Symulacja naszego stystemu liniowego, dodajemy szum przyspieszenia xsym = A * xsym + B * u + accnoise * [(T^2/2)*randn; T * randn]; %printf ("xsym [%4.4f %4.4f]\n", xsym); %Symulacja zaszumionego pomiaru z = C * xsym + measnoise * randn; %printf ("z [%4.4f %4.4f]\n", z); %Faza Predykcji x = A * x + B * u; %printf ("x [%4.4f %4.4f]\n", x); P = A*P*A' + Q; %Faza Korekcji K = P * C' * inv( C * P * C' + R); %printf ("Macierz C [%4.0f %4.0f %4.0f %4.0f]\n", C); printf ("Macierz P [%4.4f %4.4f %4.4f %4.4f]\n", P); %printf ("C*P [%4.4f %4.4f]\n", C*P); %printf ("K [%4.4f %4.4f %4.4f %4.4f]\n", K); x = x + K*(z - C*x); %printf ("z-C*x [%4.4f %4.4f]\n", z-C*x); %printf ("K*(z-C*x) [%4.4f %4.4f]\n", K*(z-C*x)); P = ( I - K*C)*P; %printf ("\n"); %Uaktualnienie danych pos = [pos; xsym(1)]; %Pozycja rzeczywista posmeas = [posmeas; z(1)]; %Pozycja mierzona poshat = [poshat; x(1)]; %Pozycja estymowana FK speed = [speed; xsym(2)]; speedmeas = [speedmeas; z(2)]; speedhat = [speedhat; x(2)]; kpos = [kpos; K(1)]; kspeed = [kspeed; K(2)]; end; close all; t = 0:step:duration; figure; plot(t, pos, t, posmeas, t, poshat); legend('prawdziwa', 'mierzona', 'estymowana'); title('Wysokosc'); plot(t, speed, t, speedmeas, t, speedhat); title('Predkosc'); plot(t, kpos, t, kspeed); legend('Wysokosc', 'Predkosc'); title('Wspolczynniki Kalmana'); Siedziałem nad nim jeszcze dzisiaj przez kilka godzin, porównywałem ze wzorami w wikipedii i nic rozsądnego nie wymyśliłem. Coś musi być jednak nie tak, bo nawet jeżeli zmienię macierz C z [1 0; 0 1]; na [1 0; 0 0]; to nie jest tak samo jak w przykładzie z łódką. Czy mógłbyś rzucić okiem na ten kod? W teorii wzmocnienie Kalmana powinno się ustalać na jakiś konkretnych wartościach wraz z upływem czasu. Dlatego K[0] zachowuje się poprawnie, a to właśnie prędkość działa niepoprawnie. Prawdopodobnie dzieje się tak dlatego, że w twoim aktualnym modelu stanowym A,B,C(H),D zmienna stanu odpowiedzialna za prędkość jest nieobserwowalna. Bardzo możliwe, że jak zmodyfikujesz macierz H tak, aby prędkość była również śledzona na wyjściu i uaktualnisz wszystkie równania, parametr K[1] również będzie się zmieniał w niewielkich granicach. OK dziękuję za ukierunkowanie. Spróbuję przerobić mój kod C tak aby macierz C była 2x2, bo na razie próbowałem z macierzą 2x1. Wydaje mi się że zmienna od prędkości jest obserwowalna, bo całkiem nieźle pokrywa się z wynikami pomiaru. Jednak moim zdaniem ta ingerencja w filtr i dodatkowa aktualizacja danych o szumach jest niepotrzebna i może być nawet szkodliwa. Jasne. U mnie parametry szumu mocno zależą od tego czy maszyna stoi czy leci. Drgania wnoszone przez silniki w trakcie lotu przenoszą się na membranę czujnika ciśnienie i mocno zaszumiają pomiar. Stąd wziął się pomysł aktualizacji parametrów szumu. Skoro filtr sobie z tym radzi to tym lepiej, bo mniej roboty z obliczeniami. Cytuj Link do komentarza Share on other sites More sharing options...
GAndaLF Grudzień 4, 2012 Udostępnij Grudzień 4, 2012 No faktycznie wczoraj się pomyliłem z tą obserwowalnością. Jak skonstruujemy macierz obserwowalności [CA; CAA] to wyjdzie [1 0; 1 T] i jej wyznacznik wynosi T. Przeanalizowałem Twój problem i wiem już dlaczego filtr rozjeżdża się między 5 a 6 sekundą. Powodem jest niestabilność numeryczna. Macierz P zawsze musi być dodatnio określona i symetryczna dlatego, że jest macierzą kowariancji dla każdego kolejnego kroku. Jednak te wartości robią się z każdym krokiem coraz mniejsze i w końcu są tak małe, że ich obliczanie powoduje błędy numeryczne. Jeżeli zatrzymasz symulację po 5 sekundach, kiedy estymata już się rozjeżdża, ale jeszcze nie doszła do tak wielkich rozmiarów, zobaczysz, że po ostatnim kroku macierz P ma wszystkie wartości ujemne. Poza tym te wartości są bardzo małe. U mnie rzędu e-19. W swoich implementacjach filtru Kalmana jeszcze nie miałem tego problemu, ale na wykładach nam mówili, że jednym ze sposobów pozbycia się czegoś takiego jest inna forma równania na kowariancje a posteriori: P(t|t) = [i−K(t)C]P(t|t−1)[i−K(t)C]T+K(t)WKT(t) Ta forma też jest podobno zawodna, ale pewniejsza od tej standardowej. Najpewniejszym, ale i najbardziej skomplikowanym sposobem sposobem pozbycia się problemu jest "Square root Kalman filter" czyli postać w której używa się pierwiastków kwadratowych macierzy. Możesz też gdzieś w pętli programu sprawdzać, czy macierz P jest po przekształceniu dodatnio określona i jeśli nie to najwyżej jej nie aktualizować. Zmodyfikowałem twój kod według ostatniego sposobu. Oto Wyniki: clear clc duration = 10 %czas trwania symulacji [s] step = 0.05 %krok symulacji [s] measnoise = [0.5; 0.5]; %wspolczynnik szumu pomiaru accnoise = 0.2; %wspolczynnik szumu przyspieszenia T = step; R = [0.5^2 0.15; 0.15 0.3^2]; %Wyznaczenie kowariancji szumu pomiaru Q = accnoise^2 * [(T^2/2)^2 T*0.02; T*0.02 0.04]; %Kowariancja szumu procesu A = [1 T; 0 1]; %Macierz przejscia stanu poprzedniego do obecnego B = [T^2 /2; T]; %Macierz przejscia dla wejscia C = [1 0; 0 1]; %Macierz przejscia dla mierzonej wielkosci z korekcją dwu wartosci %Inicjalizacja I = [ 1 0; 0 1 ]; %Macierz jednostkowa P = Q; %Poczatkowa macierz kowariancji P x = [0; 0]; %wektor stanu xsym = x; %symulowana wartość "prawdziwa" u = 0.3; %Przyspieszenie [m/s*s] %Przygotowanie miejsca na dane pos = []; %Prawdziwa pozycja poshat = []; %Pozycja estymowana filtrem kalmana posmeas = []; %Pozycja mierzona speed = []; %prawdziwa prędkość speedhat = []; %prędkość estymoana filtrem speedmeas = []; %predkość mierzona kpos = []; %współczynnik kalmana dla wysokości kspeed = []; %wspólczynnik kalmana dla prędkości %Rozpoczecie symulacji for t = 0:T:duration, %Symulacja naszego stystemu liniowego, dodajemy szum przyspieszenia xsym = A * xsym + B * u + accnoise * [(T^2/2)*randn; T * randn]; %printf ("xsym [%4.4f %4.4f]\n", xsym); %Symulacja zaszumionego pomiaru z = C * xsym + measnoise * randn; %printf ("z [%4.4f %4.4f]\n", z); %Faza Predykcji x = A * x + B * u; %printf ("x [%4.4f %4.4f]\n", x); P = A*P*A' + Q; %Faza Korekcji K = P * C' * inv( C * P * C' + R); %printf ("Macierz C [%4.0f %4.0f %4.0f %4.0f]\n", C); %printf ("Macierz P [%4.4f %4.4f %4.4f %4.4f]\n", P); %printf ("C*P [%4.4f %4.4f]\n", C*P); %printf ("K [%4.4f %4.4f %4.4f %4.4f]\n", K); x = x + K*(z - C*x); %printf ("z-C*x [%4.4f %4.4f]\n", z-C*x); %printf ("K*(z-C*x) [%4.4f %4.4f]\n", K*(z-C*x)); Pn = P - K*(C*P*C'+R)*K'; if eig(Pn)>=0 P = Pn; end %printf ("\n"); %Uaktualnienie danych pos = [pos; xsym(1)]; %Pozycja rzeczywista posmeas = [posmeas; z(1)]; %Pozycja mierzona poshat = [poshat; x(1)]; %Pozycja estymowana FK speed = [speed; xsym(2)]; speedmeas = [speedmeas; z(2)]; speedhat = [speedhat; x(2)]; kpos = [kpos; K(1)]; kspeed = [kspeed; K(2)]; end; close all; t = 0:step:duration; figure; subplot(3,1,1); plot(t, pos, t, posmeas, t, poshat); legend('prawdziwa', 'mierzona', 'estymowana'); title('Wysokosc'); subplot(3,1,2); plot(t, speed, t, speedmeas, t, speedhat); title('Predkosc'); subplot(3,1,3); plot(t, kpos, t, kspeed); legend('Wysokosc', 'Predkosc'); title('Wspolczynniki Kalmana'); 1 Cytuj Link do komentarza Share on other sites More sharing options...
Pitlab Grudzień 4, 2012 Udostępnij Grudzień 4, 2012 Przeanalizowałem Twój problem i wiem już dlaczego filtr rozjeżdża się między 5 a 6 sekundą. Powodem jest niestabilność numeryczna. [...]W swoich implementacjach filtru Kalmana jeszcze nie miałem tego problemu, ale na wykładach nam mówili, że jednym ze sposobów pozbycia się czegoś takiego jest inna forma równania na kowariancje a posteriori: P(t|t) = [i−K(t)C]P(t|t−1)[i−K(t)C]T+K(t)WKT(t) Ta forma też jest podobno zawodna, ale pewniejsza od tej standardowej. Najpewniejszym, ale i najbardziej skomplikowanym sposobem sposobem pozbycia się problemu jest "Square root Kalman filter" czyli postać w której używa się pierwiastków kwadratowych macierzy. Możesz też gdzieś w pętli programu sprawdzać, czy macierz P jest po przekształceniu dodatnio określona i jeśli nie to najwyżej jej nie aktualizować. Jesteś wielki! Bardzo dziękuję. Testowałem kilka różnych rzeczy, zauważyłem że ze zmianą parametrów szumu problem następuje w innym miejscu, ale o dodatnim określeniu macierzy nigdy nie słyszałem i mógłbym tak walić głową w mur bez skutku. Mam 5 innych pilnych rzeczy do zrobienia, ale chyba usiądę i zakoduję to w C aby przetestować w rzeczywistej aplikacji. Już mnie paluszki swędzą 🙂 Cytuj Link do komentarza Share on other sites More sharing options...
GAndaLF Grudzień 5, 2012 Udostępnij Grudzień 5, 2012 Dzisiaj jeszcze pomyślałem trochę nad tym twoim kodem i chyba jednak dalej coś jest nie tak. Wariancja wyrażona macierzą P nie powinna maleć do 0. To by oznaczało, że proces z wzrostem czasu przestaje być losowy, a to jest bez sensu. W innej mojej symulacji większość jej wartości jest wyraźnie różna od 0. Jak zmieniłem początkowe wartości wariancji szumów na np. [0.1 0; 0 0.1] to filtr dąży do wartości mierzonej, a nie rzeczywistej. To by oznaczało, że dalej gdzieś w kodzie jest błąd. Jak będę miał więcej czasu to spróbuję napisać program od 0 i porównamy wyniki. Cytuj Link do komentarza Share on other sites More sharing options...
Pomocna odpowiedź
Dołącz do dyskusji, napisz odpowiedź!
Jeśli masz już konto to zaloguj się teraz, aby opublikować wiadomość jako Ty. Możesz też napisać teraz i zarejestrować się później.
Uwaga: wgrywanie zdjęć i załączników dostępne jest po zalogowaniu!