[Inne] Filtr Kalmana - Teoria i Praktyka

[Pitlab]

Mnie też ten kod nie bardzo działa. Zaimplementowałem go w C i nadal jest mocno niestabilny. Początkowo myślałem że mam jakiś problem z przypisaniem zmiennych (zrobiłem zestaw zmiennych dla macierzy 3x3 a teraz pakuję w to dane dla 2x2), ale podczas testów uchwyciłem moment wykrzaczenia się filtra:

Te białe wykresy na górze to 4 wartości macierzy P. Tam widać jakieś strzały i to pewnie tutaj jest źródło problemu. Niebieskie wykresy to wysokość, zielone prędkość, czerwony przyspieszenie a na samym dole 4 elementy macierzy K. Widać że w pewnym momencie wartości zerują się ( w debugerze przybierają wartości NaN - Not a Number). Zresztą widać że część współczynników macierzy K ma wartości ujemne (konkretnie [1,1] i [0,1]). Czy to ma sens? Może też tak jak w przypadku macierzy P nie dopuszczać do powstania ujemnych wartości?

W tym przykładzie akurat mam ustawiony bardzo mały szum akcelerometrów i filtr mocno ufa predykcji a słabo pomiarowi. Przy okazji zdałem sobie sprawę że ogólnie filtr może być mało ciekawy w locie, bo gdy platforma drży, to filtr przez mocno zaszumione dane z akcelerometru będzie wnosił dodatkowy szum do wyników. Może się okazać że poleganie na akcelerometrze będzie tylko źródłem kłopotów, no ale to trzeba sprawdzić w locie.

Edit. Zabranianie ujemnych wartości w macierzy K nic nie daje, natomiast gdy zwiększyłem odchylenie standardowe szumu akcelerometru (szum procesu) do 1m/s^2 to filtr zachowuje się poprawnie. Zrobię zrobię jakieś narzędzie aby móc elastycznie zmieniać parametry filtra w czasie pracy i zobaczymy.

Edit2:

Zrobiłem możliwość zmiany parametrów szumu i przedstawiam pierwsze rezultaty.

Szum pomiaru wysokości mam ustawiony na 0,2m

Szum pomiaru prędkości na 0,3m

Zmieniałem szum pomiaru akcelerometru. Na pierwszym obrazku kolejno od lewej 3, 2, 1 i 0,5 m/s^2. Macierz P (na smaej górze) teraz jest stabilna. Współczynniki kalmana (na dole) są dodatnie i widać jak się zmieniają.

Dalej zmieniałem szum akcelerometru, kolejno 0,3, 0,2, 0,1, 0,05 i 0,01 m/s^2.

Tutaj współczynnik kalmana dla prędkości jest ujemny. Na razie filtr wydaje się być stabilny. Na dzisiaj kończę, pobawię się jeszcze jutro, bo ciągle parametry filtra nie są zadowalające. Obserwuję za duże opóźnienie estymaty wysokości. Widać to przy szybkich ruchach, prędkość w miarę nadąża, ale szybkie zmiany wysokości są odfiltrowywane (trochę zbyt mocno).

[GAndaLF]

Taka mała uwaga, że definicja dodatniej określoności nie mówi dosłownie, że wszystkie komórki są dodatnie, tylko że wartości własne są dodatnie. Jeśli wszystkie komórki są dodatnie to macierz jest dodatnio określona, ale samo pojęcie jest bardziej ogólne.

Przyjrzał bym się dokładnie macierzom kowariancji szumu. Zwykle dla szumu w równaniu stanu macierz kowariancji daje się diagonalną. Innym potencjalnym miejscem, gdzie mogą się pojawiać błędy jest odwracanie macierzy. Kończą mi się już pomysły co jeszcze może być powodem błędnego działania.

[ Dodano: 06-12-2012, 01:34 ]

Napisałem skrypt matlabowy od początku:

clear
clc

%wektor czasu
t = 5;
dt = 0.01;
T = 0:dt:t;

%model stanu
A = [1 dt; 0 1];
B = [dt^2/2; dt];
C = [1 0; 0 1];

%pobudzenie
a = 0.3;
U = a*ones(1, size(T,2));

%wektory wejscia i wyjscia
X = zeros(2, size(T,2));
Y = zeros(2, size(T,2));
%Y = zeros(1, size(T,2));

%warunki poczatkowe
x0 = [1;0];
X(:,1) = x0;

%wektory zaszumione
Xn = X;
Yn = Y;

%wektory z filtrem kalmana
Xk = X;
Yk = Y;

%parametry szumu
stddevx1 = 0.9;
stddevx2 = 0.9;
V = [stddevx1^2*dt 0; 0 stddevx2^2*dt];
stddevy1 = 0.9;
stddevy2 = 0.9;
W = [stddevy1^2/dt 0; 0 stddevy2^2/dt];
%W = stddevy1^2;

%zmienne do filtru kalmana
xpri = [0;0];
xpost = [0;0];
Ppri = V;
Ppost = V;

for i=1:size(T,2)
   X(:,i+1) = A*X(:,i) + B*U(:,i);
   Y(:,i) = C*X(:,i);

   Xn(:,i+1) = A*X(:,i) +  B*U(:,i) + [rand*stddevx1 - stddevx1/2; rand*stddevx2 - stddevx2/2];
   Yn(:,i) = C*X(:,i) + [rand*stddevy1 - stddevy1/2; rand*stddevy2 - stddevy2/2];

   %filtr Kalmana

   %aktualizacja pomiarow
   e = Yn(:,i) - C*xpri;
   S = C*Ppri*C'+W;
   K = Ppri*C'*S^-1;

   xpost = xpri+K*e;
   Ppost = Ppri-K*S*K';

   Xk(:,i) = xpost;
   Yk(:,i) = C*Xk(:,i);

   %aktualizacja czasu
   xpri = A*xpost+B*U(:,i);
   Ppri = A*Ppost*A' + V;
end

subplot(2,1,1);
plot(T, X(1,1:i), T, Xn(1,1:i), T, Xk(1,1:i));
legend('prawdziwa', 'mierzona', 'estymowana');
title('Wysokosc');
subplot(2,1,2);
plot(T, X(2,1:i), T, Xn(2,1:i), T, Xk(2,1:i));
legend('prawdziwa', 'mierzona', 'estymowana');
title('Predkosc');

Tutaj wyniki:

Specjalnie dałem inny punkt startowy, żeby zobaczyć jak filtr Kalmana dochodzi do poprawnej wartości. Moja implementacja wykorzystuje minimalnie inne wzory:

x(t + 1|t) = A(t)x(t|t) + B(t)u(t)

P(t + 1|t) = A(t)P(t|t)A'(t) + V(t)

e(t) = y(t) − C(t)x(t|t − 1)

S(t) = C(t)P(t|t − 1)C'(t) +W(t)

K(t) = P(t|t − 1)C'(t)S^−1(t)

x(t|t) = x(t|t − 1) +K(t)e(t)

P(t|t) = P(t|t − 1) −K(t)S(t)K'(t)

Praktycznie jedyna różnica jest w ostatnim równaniu. Obie implementacje są spotykane w literaturze, więc ta różnica nie może być zbyt znacząca. Wydaje mi się, że klucz do problemu leży w wariancjach szumów i ich dyskretyzacji.

[Pitlab]

Przeanalizowałem Twój kod i to jest to samo co w poprzedniej wersji, wiec już nie będę zmieniał swojego kodu C w kontrolerze. Na razie działa dobrze. Nie mam pojęcia dlaczego na początku się wykrzaczał.

Jestem już po dłuższej zabawie z dobieraniem parametrów szumu i powoli zaczynam czuć zależności między nimi. Filtr sam w sobie jeszcze działa licho, ale to jest spowodowane opóźnieniami jakie są w moim układzie pomiarowym, które przekłamują zależności fizyczne miedzy pomiarami. Musze go trochę zmodyfikować i być może kopter zacznie lepiej reagować na sterowanie PID-em. Po eksperymentach widać też pewne negatywne zjawiska uboczne, takie jak wpływ przeciążenia na wskazania wysokościomierza i przy okazji wariometru, bo oba pomiary korzystają z tego samego czujnika ciśnienia. Widać to na obrazku. Tak jak wcześniej niebieski to wysokość, zielony to prędkość, czerwony to przyspieszenie w osi Z. Na dole są elementy macierzy K czyli współczynniki kalmana. Telemetria odświeżana z częstotliwością 50 Hz (pomiary i filtr 200 Hz).

W pierwszej fazie ruchu gdzie przyspieszenie zaczyna rosnąć (na wykresie jest wyskalowane w g, ale do obliczeń wchodzi w m/s^2) to wysokość i prędkość zaczynają maleć a przecież też powinny rosnąć. To jest własnie wpływ przyspieszenia na membranę czujnika. Wniosek jest taki że czujnik powinien być w układzie montowany do góry nogami, wtedy wpływ przyspieszenia będzie "wspomagał" wskazania zamiast je tłumić.

Kolejna sprawa to opóźnienie na filtrach wariometru. W chwili gdy przyspieszenie jest największe, pomiar prędkości powinien przechodzić przez punkt przegięcia na narastającym zboczu a tutaj sygnał prędkości dopiero zaczyna rosnąć. Gdy przyspieszenie przestanie oddziaływać i wróci do 1g to prędkość powinna być maksymalna (nie jest) a wysokość powinna być w punkcie przegięcia na narastającym zboczu i tak jest.

Po prostu wariometr jako z natury mocno szumiący zbyt mocno obłożyłem filtrami, przez co spóźnia się i przesuwa w fazie wykres prędkości. Trzeba wyrzucić to filtrowanie. Dla filtra kalmana lepiej mieć zaszumiony pomiar w czasie rzeczywistym niż gładki przesunięty.

[GAndaLF]

Możemy spróbować zamodelować opóźnienie transportowe bawiąc się indeksami wektorów X i Y. Parametry rzeczywiste będą obliczane tak jak wcześniej. Parametry zaszumione będą obliczane na podstawie stanu z przed k iteracji, gdzie k to opóźnienie. Wynik przeprowadzimy przez filtr kalmana i otrzymamy opóźniony stan. Następnie zrobimy k iteracji obliczania równania stanu Xk(n+i) = A*Xk(n+i-1) + B*U(n+i-1) aby otrzymać estymatę aktualnego stanu. Tak więc filtr będzie wyznaczał aktualną estymatę k próbek temu i na jej podstawie będzie próbował wewnętrznie przybliżyć aktualny stan. Jeśli symulacje w matlabie się powiodą, spróbujesz przybliżyć opóźnienie k i zaimplementować filtr na obiekcie.

A jeżeli chodzi o mój kod z poprzedniego posta to ważny moment jest przy wyznaczaniu macierzy kowariancji. Wartości tej macierzy dzielę przez czas próbkowania dokonując w ten sposób dyskretyzacji. Ten krok jest ważny ponieważ rzeczywisty proces działa w czasie ciągłym a my go musimy dostosować do czasu próbkowania.

Udało mi się osiągnąć coś takiego:

clear
clc

%wektor czasu
t = 10;
dt = 0.01;
T = 0:dt:t;

%model stanu
A = [1 dt; 0 1];
B = [dt^2/2; dt];
C = [1 0; 0 1];

%pobudzenie
a = 0.3;
U = [a*ones(1, floor(size(T,2)/3)) 0 -2*a*ones(1, floor(2*size(T,2)/3)) ];

%wektory wejscia i wyjscia
X = zeros(2, size(T,2));
Y = zeros(2, size(T,2));
%Y = zeros(1, size(T,2));

%warunki poczatkowe
x0 = [0;0];
X(:,1) = x0;

%wektory zaszumione
Xn = X;
Yn = Y;

%wektory z filtrem kalmana
Xk = X;
Yk = Y;

Xk2 = X;
Yk2 = Y;

%parametry szumu
stddevx1 = 0.9;
stddevx2 = 0.9;
V = [stddevx1^2*dt 0; 0 stddevx2^2*dt];
stddevy1 = 0.9;
stddevy2 = 0.9;
W = [stddevy1^2/dt 0; 0 stddevy2^2/dt];
%W = stddevy1^2;

%zmienne do filtru kalmana
xpri = [0;0];
xpost = [0;0];
Ppri = V;
Ppost = V;

%opoznienie transportowe
k = 100;

%wektor mierzonych wartosci
Ym = [zeros(2, size(T,2)) zeros(2, k)];

%obliczenie wartosci do macierzy
for i=1:size(T,2)

   %wartosci rzeczywiste
   X(:,i+1) = A*X(:,i) + B*U(:,i);
   Y(:,i) = C*X(:,i);    

   %wartosci zaszumione
   Xn(:,i+1) = A*X(:,i) +  B*U(:,i) + [rand*stddevx1 - stddevx1/2; rand*stddevx2 - stddevx2/2];
   Yn(:,i) = C*X(:,i) + [rand*stddevy1 - stddevy1/2; rand*stddevy2 - stddevy2/2];

   Ym(:,i+k) = Yn(:,i);
end

for i=k:size(T,2)
       %aktualizacja pomiarow
       e = Ym(:,i) - C*xpri;
       S = C*Ppri*C'+W;
       K = Ppri*C'*S^-1;

       xpost = xpri+K*e;
       Ppost = Ppri-K*S*K';

       Xk(:,i-k+1) = xpost;        
       Yk(:,i-k+1) = C*Xk(:,i-k+1) + B*U(i-k+1);

       Xtemp1 = xpost;

       for j=1:k-1
           Xtemp = Xtemp1;
           Xtemp1 = A*Xtemp + B*U(i-k+j);
           Ytemp = C*Xtemp;                       
       end

       Xk2(:,i) = Xtemp;
       Yk2(:,i) = Ytemp;

       %aktualizacja czasu
       xpri = A*xpost+B*U(:,i);
       Ppri = A*Ppost*A' + V;
end

subplot(2,1,1);
plot(T(k:i-k), Y(1,(k:i-k)), T(k:i-k), Ym(1,(k:i-k)), T(k:i-k), Yk(1,(k:i-k)), T(k:i-k), Yk2(1,(k:i-k)));
legend('prawdziwa', 'mierzona', 'estymowana z opoznieniem', 'estymowana na biezaco');
title('Wysokosc');
subplot(2,1,2);
plot(T(k:i-k), Y(2,(k:i-k)), T(k:i-k), Ym(2,(k:i-k)), T(k:i-k), Yk(2,(k:i-k)), T(k:i-k), Yk2(2,(k:i-k)));
legend('prawdziwa', 'mierzona', 'estymowana z opoznieniem', 'estymowana na biezaco');
title('Predkosc');

Wykresy:

Na bieżąco wykonuję filtrację Kalmana dla chwili czasu cofniętej o wartość opóźnienia k. Później na podstawie estymaty a posteriori wyznaczam aktualny stan dokonując predykcji o k kroków. Jak widać, wartość estymowana na bieżąco staje się mniej dokładna w okolicach miejsc zmian sygnału sterującego. Prawdopodobnie można by to wyeliminować dodatkowymi modyfikacjami algorytmu.

[Polecacz 101]

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

[Pitlab]

Nie wiem czy chodzi nam o to samo, ale pomyślałem sobie żeby opóźnić sygnały wysokości i przyspieszenia, tak aby dostosować je do prędkości ale to chyba nie najlepszy pomysł, bo na podstawie tych sygnałów robione jest sterowanie kopterem a im większe opóźnienie tym trudniej sterować w zamkniętej pętli.

Dzisiaj w wolnej chwili wyprułem filtr z wariometru. We wzmacniaczu różniczkującym w gałęzi sprzężenia zwrotnego miałem kondensator 47nF. Zmieniłem na 22nF, potem na 10nF - tutaj już zaczynało trochę szumieć, potem jeszcze na 1nF, ale już było trochę za dużo, więc zostałem na 2,2nF. Wyrzuciłem też filtr RC 13k*1uF za wzmacniaczem i teraz jest spory szum, ale zależności fazowe się zgadzają. Tak to wygląda na wykresie:

Odpowiedź filtra jest poprawna zarówno dla wolnych jak i szybkich sygnałów. Przydało by się jeszcze jakieś "koło zamachowe", które swoją bezwładnością trochę wygładziło by sygnały wyjściowe, bo z tego co rozumiem szum w jakiejkolwiek części danych wchodzących do filtra, czy to w ramach predykcji czy korekcji przenosi się na pozostałe elementy.

Kiedyś w regulatorze PID miałem zaimplementowany ogranicznik szybkości narastania sygnału. Wiadomo że sterowany obiekt ma jakaś konkretną bezwładność (masę) i ograniczoną siłę mechanizmów wykonawczych (tutaj ciąg śmigieł). Z tego można wyliczyć jak duże przyspieszenie może wygenerować i wszystkie zbyt szybko zmieniające się przyspieszenia przycinać. Taki filtr nie przesuwa w czasie podstawowego sygnału tylko jego składową o wysokiej częstotliwości.

Jeszcze inny pomysł, ale nie wiem czy rozsądny to dorobienie jeszcze jednego członu filtra, trzeciej pochodnej, czyli zrywu. Coś mi się obiło o uszy że kolejny człon w jakiś sposób polepsza działanie filtra ale nie bardzo mogę znaleźć rozsądne wytłumaczenie dlaczego tak się dzieje. Może chodzi o to że jest to kolejne powiązanie ze sobą wszystkich zależności fizycznych? Wadą tego rozwiązania jest wyraźnie większe obciążenie obliczeniowe. Wcześniejszy filtr 3x3 zajmował ok. 1100us. Obecny 2x2 zaledwie 218us

A jeżeli chodzi o mój kod z poprzedniego posta to ważny moment jest przy wyznaczaniu macierzy kowariancji. Wartości tej macierzy dzielę przez czas próbkowania dokonując w ten sposób dyskretyzacji. Ten krok jest ważny ponieważ rzeczywisty proces działa w czasie ciągłym a my go musimy dostosować do czasu próbkowania.

Przyglądam się kodowi nie widzę tego dzielenia.

Na początku macierz kowariancji była liczona równaniami:

P = A*P*A' + Q; - predykcja

P = ( I - K*C)*P; - korekcja

potem

Ppri = A*Ppost*A' + V; -predykcja - dokładnie to samo, tylko inne oznaczenia

S = C*Ppri*C'+W;

Ppost = Ppri-K*S*K'; - korekcja - inny wzór ale nie widzę jawnego dzielenia

[GAndaLF]

W moim pomyśle chodzi o to, że jeśli program otrzymuje pomiary z czujników w chwili t. To są one opóźnione i tak naprawdę pokazują co działo się w czasie t-k. Przy czym dla każdego czujnika to opóźnienie może być inne. Dlatego chciałem zapisywać historię pomiarów, składać wektory wyjścia z odpowiadającymi opóźnieniami i dla nich liczyć Kalmana. Otrzymane x będzie estymacją stanu sprzed k chwil. Dlatego na jego bazie obliczam równania stanu tyle razy, aby nadgonić to opóźnienie i otrzymać estymatę rzeczywistej wartości w aktualnym momencie. W wyniku tego otrzymuję x(t|t-k) czyli estymację wektora stanu w czasie t na bazie informacji do chwili t-k.

Rzeczywiście wywalenie filtrów powinno pomóc i teraz takie zabiegi matematyczne pewnie nie będą potrzebne. Próbowałeś podzielić macierze kowariancji Q i R przez czas próbkowania dt i sprawdzić jak wtedy wyglądają pomiary? Bo teraz mam wrażenie jakby filtr za mocno zawierzał pomiarom, a wykres filtru powinien być gładszy.

Jeżeli chodzi o dodawanie nowych czujników to kalman zawsze poprawi swoje właściwości mając dostęp do większej liczby pomiarów. I nie musi to być wcale czujnik mierzący inną wartość fizyczną. Dla filtru to jest tylko powiększona macierz C i dodatkowa wariancja. Sam sobie wybierze w jakim stopniu korzystać z nowego czujnika i w najgorszym wypadku będzie tak jak przed dodaniem, nigdy gorzej. Oczywiście im więcej czujników tym korzyść z dodania nowego mniejsza. Tyle jeśli chodzi o teorię. Jeżeli twierdzisz, że dodanie nowego czujnika wydłuży obliczenia 5 razy to ja bym dał sobie z tym spokój, te czujniki które masz powinny spokojnie wystarczyć.

Te ograniczniki sygnału w PIDach o których mówisz to jest praktycznie standard. Większego sygnału sterującego i tak się nie wygeneruje, a można tylko zepsuć sprzęt. Tak samo w członie całkującym daje się ograniczenie na sumę uchybu, żeby nie przepełnić zmiennej.

[Pitlab]

W moim pomyśle chodzi o to, że jeśli program otrzymuje pomiary z czujników w chwili t. To są one opóźnione i tak naprawdę pokazują co działo się w czasie t-k. Przy czym dla każdego czujnika to opóźnienie może być inne. Dlatego chciałem zapisywać historię pomiarów, składać wektory wyjścia z odpowiadającymi opóźnieniami i dla nich liczyć Kalmana. Otrzymane x będzie estymacją stanu sprzed k chwil. Dlatego na jego bazie obliczam równania stanu tyle razy, aby nadgonić to opóźnienie i otrzymać estymatę rzeczywistej wartości w aktualnym momencie. W wyniku tego otrzymuję x(t|t-k) czyli estymację wektora stanu w czasie t na bazie informacji do chwili t-k.

Jasne, teraz rozumiem. To jest dobre rozwiązanie, bo można polegać na przefiltrowanych sygnałach nie tracąc właściwości dynamicznych. Wymaga tylko wiarygodnego pomiaru do sterowania a z tym może być problem.

Jeszcze nie próbowałem uruchomić tego w locie, ale o ile przyspieszenie w ruchu ręcznym wygląda super, to przy pracujących silnikach robi się sieczka i boję się ze na tym mogę polec. Już dzisiaj padam, ale spróbuję jutro wgrać kod na sterownik w kopterze i zdjąć pomiary może jeszcze nie w locie ale przy pracujących silnikach.

Próbowałeś podzielić macierze kowariancji Q i R przez czas próbkowania dt i sprawdzić jak wtedy wyglądają pomiary? Bo teraz mam wrażenie jakby filtr za mocno zawierzał pomiarom, a wykres filtru powinien być gładszy.

Macierze mam zdefiniowane tak:

//ustaw parametry szumu
   sh = 0.08;   //przyjmuję szum pomiaru wysokości h=0,08 m, 
   sv = 0.3;   //szum pomiaru prędkości v=0,3 m/s 
   sa = 1.4;   //szum przyspieszenia a=0,5 m/s^2

   //proces jest opisany jako
   // |dt^2/2|*AccZ 
   // | dt   |       
   //odchylenie standardowe obliczeń wysokości wynosi h=dt^2/2*sa
   //odchylenie standardowe obliczeń prędkości wynosi v=dt*sa

   //macierz kowariancji procesu
   //Q = E(wk * wkT); gdzie wkT to transponowana macierz wk = szum procesu
   //Q = E[[hv]*[h]] = [h^2  hv]
   //     [     [v]]   [vh  v^2]
   //Q[0][0] = (dt*dt*sa/2)*(dt*dt*sa/2);
   //Q[0][1] = (dt*dt*sa/2)*(dt*sa);
   //Q[1][0] = (dt*sa)*(dt*dt*sa/2);
   //Q[1][1] = (dt*sa)*(dt*sa);
   //po uproszczeniu mamy
   Q[0][0] = dt*dt*dt*dt*sa*sa/4;
   Q[0][1] = dt*dt*dt*sa*sa/2;
   Q[1][0] = dt*dt*dt*sa*sa/2;
   Q[1][1] = dt*dt*sa*sa;

   //R - macierz kowariancji pomiaru 
   //R = E(Y * YT); gdzie YT to transponowana macierz Y = szum pomiaru
   //R = E[[h v]*[h]] = [h^2  hv]
   //     [      [v]]   [vh  v^2]
   R[0][0] = sh*sh;    
   R[0][1] = sh*sv;
   R[1][0] = sv*sh;
   R[1][1] = sv*sv;

Ten wykres był zdjęty dla trochę innych wartości szumu. Mogę je zmieniać programowo i jak pamiętam to szum prędkości miałem ustawiony bardzo duży, coś ok 3m/s (domyślnie jest 0,3). Dla mniejszych wartości szumu prędkości miałem bardzo postrzępiony wykres wysokości.

Jeżeli chodzi o dodawanie nowych czujników to kalman zawsze poprawi swoje właściwości mając dostęp do większej liczby pomiarów. I nie musi to być wcale czujnik mierzący inną wartość fizyczną. Dla filtru to jest tylko powiększona macierz C i dodatkowa wariancja. Sam sobie wybierze w jakim stopniu korzystać z nowego czujnika i w najgorszym wypadku będzie tak jak przed dodaniem, nigdy gorzej. Oczywiście im więcej czujników tym korzyść z dodania nowego mniejsza. Tyle jeśli chodzi o teorię. Jeżeli twierdzisz, że dodanie nowego czujnika wydłuży obliczenia 5 razy to ja bym dał sobie z tym spokój, te czujniki które masz powinny spokojnie wystarczyć.

Przede wszystkim chodzi o to że większa macierz o n elementów wymaga n razy więcej obliczeń typu dodawanie i mnożenie przez skalar i chyba n^2 obliczeń jeżeli chodzi o mnożenie a tam jest cała masa mnożeń.

Jeżeli chodzi o czujniki to mogę bez problemu dołożyć jeszcze jeden cyfrowy czujnik ciśnienia i wyciągnąć z niego wysokość (wariometr mogę uzyskać tylko z czujnika analogowego), jednak nie oczekuję jakichś specjalnie dużych korzyści.

Te ograniczniki sygnału w PIDach o których mówisz to jest praktycznie standard. Większego sygnału sterującego i tak się nie wygeneruje, a można tylko zepsuć sprzęt. Tak samo w członie całkującym daje się ograniczenie na sumę uchybu, żeby nie przepełnić zmiennej.

Trochę źle się wyraziłem. Nie chodzi mi o ogranicznik wartości tylko ogranicznik prędkości zmiany tej wartości. To się chyba nazywa "slope limiter".

[GAndaLF]

Mógł byś udostępnić kilka plików z danymi pomiarowymi? To się nimi jeszcze pobawię w matlabie

[Pitlab]

Mógł byś udostępnić kilka plików z danymi pomiarowymi? To się nimi jeszcze pobawię w matlabie

Jeżeli chcesz dane z rzeczywistych lotów (jeszcze przed badaniami FK) to mam tego sporo. Tak na szybko z opublikowanych w tym półroczu widzę:

http://www.pitlab.pl/autopitlot/log/Hexa4s_121013_55.zip

http://www.pitlab.pl/autopitlot/log/20121014.zip

http://www.pitlab.pl/autopitlot/log/20120929-30.zip

http://www.pitlab.pl/autopitlot/log/20120913_47.zip

http://www.pitlab.pl/autopitlot/log/20120912_hexa.zip

http://www.pitlab.pl/autopitlot/log/20120830_PID_rownolegly.zip

http://www.pitlab.pl/autopitlot/log/Hexa4s_120714_20.zip

Mogą się różnić zestawem danych, ale takie podstawowe dane jak wysokość (Alti), prędkość pionowa (Vario) i przyspieszenie w osi Z (AcelZ) powinny być wszędzie.

Jeszcze dwie uwagi.

- Loguję tylko przyspieszenie bezpośrednio z akcelerometru 3-osiowego a do FK biorę jego składową pionową po transformacji układu lokalnego do globalnego. Na upartego dane do transformacji są (Theta i Phi). To są dane z raczej stabilnych lotów, wiec być może dane z samej osi Z będą wystarczająco dokładne.

- Dopiero podczas badań z FK odkryłem błąd kalibracji wariometru. Wszystkie dane są zaniżone. Po naprawie tego błędu jeszcze nie latałem, wiec nie mam poprawnych danych. Wystarczy wszystkie dane z wariometru przemnożyć przez jakiś współczynnik. Już znalazłem. Wcześniej w celu uzyskania jednostki m/s mnożyłem napięcie napięcie z wariometru przez 4,1111 a po poprawce dzielę przez 0.01597, czyli wychodzi mi że trzeba stare dane pomnożyć przez 15,23.

Gdybyś chciał dane z ostatnich testów robionych w domu z reki, to mam tylko dane z telemetrii, wszystko co trzeba + stany macierzy X, P i K:

http://www.pitlab.pl/autopitlot/log/20121206_Kalman.zip

Miałem dzisiaj przedmuchać kod na kopterze, ale przywaliła mnie pilna dokumentacja. Spróbuję dokończyć to w najbliższych dniach.

[Bobby]

Jako, że mam raczej mierną wiedzę, ale temat wydaje się ciekawy, to go sobie czytuję i zaintrygowała mnie jedna rzecz - dlaczego czujnik cyfrowy nie nadaje się na vario?

Wybaczcie za taki mały offtopic.

[Pitlab]

dlaczego czujnik cyfrowy nie nadaje się na vario?

Nie nadaje się do wario w moim module, bo to jest wariometr na wzmacniaczu operacyjnym i wymaga sygnału analogowego. Wariometr można zrobić z dowolnego czujnika ciśnienia, tylko jego jakość będzie zależała od rozdzielczości pomiaru wysokości. Jeżeli czujnik ma rozdzielczość rzędu 1m to wariometr zrobiony z niego będzie lichy. Są już czujniki o rozdzielczości kilku cm np MS5611 i z czyś takim można już wiele zrobić.

[Bobby]

Ok, czyli jak rozumiem po prostu skrót myślowy z Twojej strony - wszystko jasne 🙂

[GAndaLF]

Moje wyniki dla pomiarów z wario 1nF:

%wyczyszczenie danych i ekranu
clear
clc

%import danych
data = importdata('20121206 Kalman/wario 1nF.log', '\t');
data = data.data;
altitude = data(:,1)';
velocity = data(:,2)';
acceleration = data(:,3)';

%usuwanie offsetu
velocity = velocity - mode(velocity);
acceleration = acceleration - mode(acceleration);

%skalowanie
%velocity = velocity/10;

%wektor czasu
dt = 0.02;
T = 0:size(altitude, 2)-1;
T = T*dt;

%model stanu
A = [1 dt; 0 1];
B = [dt^2/2; dt];
C = [1 0; 0 1];

%inicjalizacja danych
U = acceleration;
Y = [altitude; velocity];
x0 = [altitude(1); velocity(1)];

Xk = zeros(size(Y));
Yk = zeros(size(Y));

% %parametry szumu
% stddevx1 = 0.1;
% stddevx2 = 0.2;
% V = [stddevx1^2 0; 0 stddevx2^2];
% stddevy1 = 0.1;
% stddevy2 = 0.1;
% W = [stddevy1^2 0; 0 stddevy2^2];
%
% %dyskretyzacja
% V = V*dt;

sh = 0.08;
sv = 0.3;
sa = 1.4;
V = [dt*dt*sa/2 0; 0 dt*sa];
W = [sh 0; 0 sv];

%zmienne do filtru kalmana
xpri = x0;
xpost = x0;
Ppri = V;
Ppost = V;

for i=1:size(T,2)
   e = Y(:,i) - C*xpri;
   S = C*Ppri*C'+W;
   K = Ppri*C'*S^-1;

   xpost = xpri+K*e;
   Ppost = Ppri-K*S*K';

   Xk(:,i) = xpost;
   Yk(:,i) = C*Xk(:,i) + B*U(i);

   %aktualizacja czasu
   xpri = A*xpost+B*U(:,i);
   Ppri = A*Ppost*A' + V;
end

subplot(3,1,1)
plot(T, altitude, T, Xk(1,:));
legend('mierzona', 'estymowana');
title('Wysokosc');
axis([13 37 -2 4]);
subplot(3,1,2)
plot(T, velocity, T, Xk(2,:));
legend('mierzona', 'estymowana');
title('Predkosc');
axis([13 37 -20 20]);
subplot(3,1,3)
plot(T, acceleration);
axis([13 37 -2 4]);
title('Przyspieszenie - sygnal sterujacy');

figure
subplot(3,1,1)
plot(T, altitude, T, Xk(1,:));
legend('mierzona', 'estymowana');
title('Wysokosc');
axis([30 35 -2 4]);
subplot(3,1,2)
plot(T, velocity, T, Xk(2,:));
legend('mierzona', 'estymowana');
title('Predkosc');
axis([30 35 -20 20]);
subplot(3,1,3)
plot(T, acceleration);
axis([30 35 -2 4]);
title('Przyspieszenie - sygnal sterujacy');

Tutaj wykresy według Twoich macierzy kowariancji. Usunąłem wartości spoza głównej przekątnej, żeby nie było błędów numerycznych:

I zbliżenie na moment dużych zmian wartości zadanej:

Zmodyfikowałem wcześniejsze wartości do postaci:

sh = 0.08;
sv = 0.3;
sa = 1.4;
V = [dt*dt*sa/2 0; 0 dt*sa];
W = [sh 0; 0 sv];

Widać, że opóźnienie jest mniejsze:

Wypróbowałem też wariancję dobieraną na oko:

%parametry szumu
stddevx1 = 0.1;
stddevx2 = 0.2;
V = [stddevx1^2 0; 0 stddevx2^2];
stddevy1 = 0.1;
stddevy2 = 0.1;
W = [stddevy1^2 0; 0 stddevy2^2];

%dyskretyzacja
V = V*dt

Wyniki są jeszcze lepsze:

Nie wiem jakie wartości tych opóźnień między sygnałem a filtrem były by satysfakcjonujące. Jednak z tych symulacji wynika, że zabawy z wariancją mogą dać całkiem niezłe efekty.

[Pitlab]

Moje wyniki dla pomiarów z wario 1nF:

%import danych
data = importdata('20121206 Kalman/wario 1nF.log', '\t');

Świetna robota!

Możliwość analizy rzeczywistych danych w programie jest wielkim przełomem w pracy nad algorytmami, tylko ze wstydem muszę przyznać że nie potrafię wczytać danych. Próbowałem podawać pełną ścieżkę do pliku na swoim komputerze, ale program wyrzuca błąd dla funkcji importdata. Uruchamiam to na stronie: http://www.verbosus.com/octavus.html.

Przejrzałem dokumentacje octave ale tam nie ma takiego słowa kluczowego. Czy to jest Twoja własna funkcja?

Prosiłbym o kilka słów komentarza jak się tym posługiwać.

Zmodyfikowałem wcześniejsze wartości do postaci:

sh = 0.08;
sv = 0.3;
sa = 1.4;
V = [dt*dt*sa/2 0; 0 dt*sa];
W = [sh 0; 0 sv];

Widać, że opóźnienie jest mniejsze

Czyli jest znacznie mniej mnożenia przez krok czasu dt niż w moim przykładzie. To jest mała wartość, u mnie w okolicy 0,005s, więc macierz kowariancji finalnie przybiera większe wartości.

Wypróbowałem też wariancję dobieraną na oko:

%parametry szumu
stddevx1 = 0.1;
stddevx2 = 0.2;
V = [stddevx1^2 0; 0 stddevx2^2];
stddevy1 = 0.1;
stddevy2 = 0.1;
W = [stddevy1^2 0; 0 stddevy2^2];

%dyskretyzacja
V = V*dt

Wyniki są jeszcze lepsze

Tutaj nie ma dt^2, więc wartości macierzy kowariancji są większe. W swoich testach też zaobserwowałem że zdefiniowanie większego szumu daje lepsze wyniki, tylko u mnie potrafiło się wykrzaczyć przy zbyt dużym szumie. Spróbuję jeszcze tak jak Ty wyrzucić elementy macierzy poza główna przekątną.

Dodane po dłuższej chwili.

Ze wstydu ze osoby trzecie więcej pracują nad moim problemem niż ja sam, wgrałem kod (ten po dzisiejszych poprawkach z okrojonymi macierzami kowariancji procesu i pomiaru) na kopter i przedmuchałem go w biurze.

Jako punkt odniesienia pomiar z wyłączonymi silnikami

Następnie z włączonymi silnikami pracującymi na prędkości znamionowej, takiej gdzie ciąg śmigieł równoważy masę koptera.

Byłem przekonany że będę miał znacznie większy szum na akcelerometrach, ale widocznie po przejściu przez obliczenia IMU przyspieszenia też się filtrują. To dobrze, bo nie ma dużego problemu z szumem procesu. Być może jest to efekt filtrowania drgań spowodowany trzymaniem koptera w ręku. Starałem się trzymać za elastycznie mocowany pakiet napędowy, ale mimo wszystko drgania mogły być częściowo stłumione. Pełną prawdę poznam w rzeczywistym locie.

Jest za to spory szum wysokości i wariometru. To mogę jeszcze próbować zmniejszyć filtrem mechanicznym z gąbki założonej na czujnik.

Po uproszczeniu macierzy kowariancji zmieniły się dwie rzeczy:

- Współczynniki kalmana są teraz stałe. Wcześniej gdy wyświetlałem je na wykresie to widać że się zmieniały a teraz stoją w miejscu (teraz wyrzuciłem je z wykresu aby lepiej było widać dane pomiarowe). To pewnie efekt zer poza główną przekątną w macierzy kowariancji

- Teraz estymata wysokości jest znacznie gładsza i o to właśnie chodzi.

Wygląda na to że kod jest gotowy do oblatania. Odłożę to na jutro, bo dzisiaj jestem mocno zajęty i jeszcze nie wydobrzałem po ostatniej chorobie. Swoją drogą jutro ma być cieplej.

[GAndaLF]

Te wykresy z działania filtru wyglądają już naprawdę nieźle. Fajnie się ogląda jak rzeczy wypracowane w symulacjach tak dobrze działają w rzeczywistych warunkach.

U siebie mam zainstalowanego matlaba na komputerze i nie muszę wykorzystywać wersji online. Ale to jest przywilej bycia studentem, bo za nieakademickiego matlaba trzeba słono zapłacić. Poszukaj w dokumentacji octave funkcji do wczytywania plików, a nie dokładnie tego słowa kluczowego. Niektóre elementy się różnią między matlabem, a octavem. Ja tą funkcję znalazłem w matlabowym helpie razem z przykładami użycia.

Jeszcze można rozszerzyć model tak, aby przyspieszenie było trzecią zmienną stanu i nie było sygnału sterującego U. Natomiast szum V opisywał by maksymalną możliwą zmianę przyspieszenia między momentami próbkowania. W ten sposób odczyt przyspieszenia też miał byś filtrowany. Spróbuję jeszcze powalczyć z tym problemem.

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.

[ Dodano: 10-12-2012, 20:57 ]

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.

%wyczyszczenie danych i ekranu
clear
clc

%import danych
data = importdata('20121206 Kalman/wario 1nF.log', '\t');
data = data.data;
altitude = data(:,1)';
velocity = data(:,2)';
acceleration = data(:,3)';

%usuwanie offsetu
velocity = velocity - mode(velocity);
acceleration = acceleration - mode(acceleration);

%skalowanie
%velocity = velocity/10;

%wektor czasu
dt = 0.02;
T = 0:size(altitude, 2)-1;
T = T*dt;

%model stanu
A = [1 dt dt*dt/2; 0 1 dt; 0 0 1];
C = [1 0 0; 0 1 0; 0 0 1];

%inicjalizacja danych
Y = [altitude; velocity; acceleration];
x0 = [altitude(1); velocity(1); acceleration(1)];

Xk = zeros(size(Y));
Yk = zeros(size(Y));

%parametry szumu
stddevx1 = 0.1;
stddevx2 = 0.1;
stddevx3 = 0.3;
V = [stddevx1^2 0 0; 0 stddevx2^2 0; 0 0 stddevx3^2];
stddevy1 = 0.05;
stddevy2 = 0.05;
stddevy3 = 0.05;
W = [stddevy1^2 0 0; 0 stddevy2^2 0; 0 0 stddevy3^2];
V = V*dt;

%zmienne do filtru kalmana
xpri = x0;
xpost = x0;
Ppri = V;
Ppost = V;

for i=1:size(T,2)
   e = Y(:,i) - C*xpri;
   S = C*Ppri*C'+W;
   K = Ppri*C'*S^-1;

   xpost = xpri+K*e;
   Ppost = Ppri-K*S*K';

   Xk(:,i) = xpost;
   Yk(:,i) = C*Xk(:,i);

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

subplot(3,1,1)
plot(T, altitude, T, Xk(1,:));
legend('mierzona', 'estymowana');
title('Wysokosc');
axis([13 37 -2 4]);
subplot(3,1,2)
plot(T, velocity, T, Xk(2,:));
legend('mierzona', 'estymowana');
title('Predkosc');
axis([13 37 -20 20]);
subplot(3,1,3)
plot(T, acceleration, T, Xk(3,:));
axis([13 37 -2 4]);
legend('mierzona', 'estymowana');
title('Przyspieszenie');

figure
subplot(3,1,1)
plot(T, altitude, T, Xk(1,:));
legend('mierzona', 'estymowana');
title('Wysokosc');
axis([30 35 -2 4]);
subplot(3,1,2)
plot(T, velocity, T, Xk(2,:));
legend('mierzona', 'estymowana');
title('Predkosc');
axis([30 35 -20 20]);
subplot(3,1,3)
plot(T, acceleration, T, Xk(3,:));
axis([30 35 -2 4]);
legend('mierzona', 'estymowana');
title('Przyspieszenie');