Skocz do zawartości

[Inne] Filtr Kalmana - Teoria i Praktyka


Elektryk0

Pomocna odpowiedź

Artykuł bardzo dobry, właśnie przeliczam wszystko "na piechotę" - inaczej ciężko to wszystko zrozumieć.

Na razie mam uwagę do: IMG_5068c5d346e541217.png

Chyba pomylone są indeksy w drugim wierszu - wcześniej z(k) obliczane jest z aktualnego stanu, a tutaj z poprzedniego.

Link do komentarza
Share on other sites

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ć.

Link do komentarza
Share on other sites

Zarejestruj się lub zaloguj, aby ukryć tę reklamę.
Zarejestruj się lub zaloguj, aby ukryć tę reklamę.

jlcpcb.jpg

jlcpcb.jpg

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

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.

Link do komentarza
Share on other sites

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.

Link do komentarza
Share on other sites

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.

Link do komentarza
Share on other sites

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).

Link do komentarza
Share on other sites

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.

Link do komentarza
Share on other sites

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.

Link do komentarza
Share on other sites

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.

Link do komentarza
Share on other sites

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.

Link do komentarza
Share on other sites

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'); 

  • Lubię! 1
Link do komentarza
Share on other sites

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ą 🙂

Link do komentarza
Share on other sites

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.

Link do komentarza
Share on other sites

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!

Anonim
Dołącz do dyskusji! Kliknij i zacznij pisać...

×   Wklejony jako tekst z formatowaniem.   Przywróć formatowanie

  Dozwolonych jest tylko 75 emoji.

×   Twój link będzie automatycznie osadzony.   Wyświetlać jako link

×   Twoja poprzednia zawartość została przywrócona.   Wyczyść edytor

×   Nie możesz wkleić zdjęć bezpośrednio. Prześlij lub wstaw obrazy z adresu URL.

×
×
  • Utwórz nowe...

Ważne informacje

Ta strona używa ciasteczek (cookies), dzięki którym może działać lepiej. Więcej na ten temat znajdziesz w Polityce Prywatności.