Ta strona używa ciasteczek (plików cookies), dzięki którym może działać lepiej. Dowiedz się więcejRozumiem i akceptuję

Filtr Kalmana od teorii do praktyki – #2

Programowanie Teoria 30.12.2014 GandalF

kalman2Poprzednio przybliżyłem teorię związaną z filtrem Kalmana oraz na przykładzie pokazałem reakcję filtru na zmiany poszczególnych parametrów.

Teraz zajmę się czymś bardziej praktycznym. Na podstawie danych pomiarowych z akcelerometru i żyroskopu postaram się zaprojektować filtr Kalmana dla układu śledzącego orientację robota w przestrzeni.

Nawigacja serii artykułów:
« poprzednia częśćnastępna część »

Metoda pomiaru

Wartością, którą będę śledził jest orientacja robota wyrażona jako kąt na płaszczyźnie xy w trójwymiarowym, kartezjańskim układzie współrzędnych. Do dyspozycji mam żyroskop mierzący prędkość kątową wokół trzech osi x, y i z, a także akcelerometr mierzący przyspieszenie liniowe również w trzech osiach x, y i z.

Żaden z tych czujników nie mierzy bezpośrednio położenia kątowego. Dlatego będę musiał posłużyć się przekształceniami matematycznymi. Pomiar kąta położenia wyciągnę z pomiarów akcelerometru. Dane z żyroskopu posłużą natomiast do korekcji.

Zakładam, że jedyną siłą działającą na akcelerometr jest grawitacja. Wektor siły grawitacji rozkłada się na trzy składowe odpowiadające poszczególnym osiom układu współrzędnych. Można go również przedstawić za pomocą współrzędnych sferycznych r, \phi, \theta (patrz rysunek). Kąt \phi można obliczyć z wzoru:

\phi = \arctan(\frac{x}{y})

Wektor w układzie współrzędnych przedstawiony za pomocą współrzędnych prostokątnych i sferycznych.

Wektor w układzie współrzędnych przedstawiony za pomocą współrzędnych prostokątnych i sferycznych.

Cechą charakterystyczną żyroskopów jest tak zwany dryft. Jest to rodzaj błędu pomiarowego kumulującego się z czasem. Jeżeli zostawimy czujnik nieruchomo, mierzona wartość będzie stopniowo oddalać się od zera. Pomiar z żyroskopu \omega_{g} można więc zapisać jako suma rzeczywistej prędkości kątowej \omega i aktualnej wartości dryftu g:

\omega_g = \omega + g

Wyprowadzenie modelu stanowego

W modelu stanowym obiektu zastosujemy pewien trick.

Dryft żyroskopu potraktujemy jako jedną ze zmiennych stanu, którą będziemy estymować. Tak więc nasz model będzie składać się z dwóch zmiennych stanu – położenia kątowego robota i dryftu żyroskopu. Położenie kątowe w danej chwili czasu składa się z położenia w poprzedniej chwili czasu i wpływu prędkości kątowej w bieżącym cyklu próbkowania. Można to zapisać jako:

\phi(t) = \phi(t-1) + \omega dt \\ \omega(t) = \omega_g(t) - g(t-1) \\ \phi(t) = \phi(t-1) + \omega_g(t) dt - g(t-1) dt \\ g(t) = g(t-1)

Z tych równań otrzymujemy następujący model stanowy:

kalman_02_01

Przekształcenia danych pomiarowych

Plik data2.txt zawiera dane pomiarowe z żyroskopu i akcelerometru. Zostały one zebrane przez mikrokontroler STM32 przy pomocy programu opublikowanego w drugiej części artykułu o filtrach alfa-beta. Wartości są przedstawione w słowach bitowych.

Należy je przekształcić odpowiednio na \frac{^{\circ}}{s} i g:

kalman_02_02

Implementacja w Matlabie

Oto implementacja filtru w Matlabie:

Wynik symulacji został przedstawiony na wykresie. Poszczególne wariancje dobierałem metodą prób i błędów. Jak widać, filtr reaguje na skokowe zmiany z niewielkim opóźnieniem i ignoruje szpilki. Inne właściwości filtru można uzyskać zmieniając macierz kowariancji V.

Wyniki symulacji.

Wyniki symulacji.

Podsumowanie

W tej części cyklu przedstawiłem sposób projektowania filtru Kalmana dla rzeczywistego układu. Najpierw przeanalizowałem dane odczytywane z czujników i dokonałem przekształceń. Następnie wyprowadziłem model stanowy badanego obiektu. Ostatnią fazą była implementacja filtru.

Jak widać w kodzie, implementacja filtru zajmuje 7 linijek i składa się z działań na macierzach.

W kolejnej części zajmiemy się implementacją filtru na STM32.

Powiadomienia o nowych, darmowych artykułach!

Załączniki

data2.txt (plain, 11 KB) - Pobierz

Dane testowe do filtru Kalmana.

Komentarze

rmdiy

19:34, 25.05.2015

#1

Witam.

Próbuję zrobić taki system :

Jak widać, kąty Pitch, Roll są przeliczane z danych Acc, trafiają do KF razem z Gyro gdzie są estymowane. Estymowane wartości Pitch, Roll trafiają do "Tilt Compensation".

Oto wzory modułu tilt :

W praniu wygląda to tak:

Może ktoś coś poradzić, na temat kąta Yaw.

W zał. przesyłam plik do matlaba oraz dane w pliku tekstowym.

clc;

% get data from file

% datatmp = importdata('data2.txt');

% data = datatmp.data;

gyro_data = data(:,1:3);

acc_data = data(:,4:6);

mag_data=data(:,7:9);

%data

gyro_data = gyro_data*2000/32768;

acc_data = acc_data*2/65535;

mag_data = mag_data*4/65535;

% time vector

dt = 0.012;

t = dt:dt:size(data, 1)*dt;

%state space model

A = [1 -dt; 0 1];

B = [dt; 0];

C = [1 0];

%measure and proces noise

std_dev_v = 1;

std_dev_w = 2;

V = [std_dev_v*std_dev_v*dt 0; 0 std_dev_v*std_dev_v*dt];

W = std_dev_w*std_dev_w;

% t(0)

x0 = [0; 0];

P0 = [1 0; 0 1];

xpri = x0;

Ppri = P0;

xpost = x0;

Ppost = P0;

% Wektory wynikow

Y = zeros(1, size(t,2));

Yf = Y;

for i = 1:size(data, 1);

%Obliczenie aktualnego kata na podstawie danych z akcelerometru

% acc_angle = atan(acc_data(i,1)./acc_data(i,2))*180/pi;

Pitch = atan(acc_data(i,1)./(acc_data(i,1).^2 + acc_data(i,3).^2))*180/pi;

u = gyro_data(i,1);

Y(i) = Pitch;

if i == 1;

% Pierwszy pomiar sluzy nam jako wartosc poczatkowa dla filtru

xpost = [Pitch; 0];

else

% aktualizacja czasu

xpri = A*xpost + B*u;

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

end

%Zapis estymaty do wektora wynikow

Yf(i) = xpost(1);

end

% Wektory wynikow

Y1 = zeros(1, size(t,2));

Yf1 = Y1;

for i = 1:size(data, 1);

%Obliczenie aktualnego kata na podstawie danych z akcelerometru

% acc_angle = atan(acc_data(i,1)./acc_data(i,2))*180/pi;

Roll = atan(acc_data(i,2)./(acc_data(i,2).^2 + acc_data(i,3).^2))*180/pi;

u = gyro_data(i,2);

Y1(i) = Roll;

if i == 1;

% Pierwszy pomiar sluzy nam jako wartosc poczatkowa dla filtru

xpost = [Roll; 0];

else

% aktualizacja czasu

xpri = A*xpost + B*u;

Ppri = A*Ppost*A' + V;

% aktualizacja pomiarow

eps = Y1(i) - C*xpri;

S = C*Ppri*C' + W;

K = Ppri*C'*S^(-1);

xpost = xpri + K*eps;

Ppost = Ppri - K*S*K';

end

%Zapis estymaty do wektora wynikow

Yf1(i) = xpost(1);

end

XH = zeros(1, size(t,2));

YH = XH;

Yaw = XH;

%B = Yf(i)

%L=Yf1(i)

for i = 1:size(data, 1);

XH(i) = mag_data(i,1)*cos(Yf(i)) + mag_data(i,2)*sin(Yf(i))*sin(Yf1(i)) + mag_data(i,3)*sin(Yf(i))*cos(Yf1(i));

YH(i) = mag_data(i,2)*cos(Yf1(i)) + mag_data(i,3)*sin(Yf1(i));

Yaw(i) = atan(-YH(i)/XH(i)) *180/pi;

end

subplot(3,1,1)

plot(t, Y, 'b', t, Yf, 'r', 'LineWidth', 1.5)

title('Pitch')

xlabel('Czas')

ylabel('Pitch [deg]')

legend('ACC', 'KF out')

subplot(3,1,2)

plot(t, Y1, 'b', t, Yf1, 'r', 'LineWidth', 1.5)

title('Roll')

xlabel('Czas')

ylabel('Roll [deg]')

legend('Acc', 'KF out')

subplot(3,1,3)

plot(t, Yaw)

title('Yaw')

xlabel('Czas')

ylabel('Yaw [deg]')

simba92

22:46, 22.09.2017

#2

Jaki byłby model stanowy dla filtru kalmana w celu wyliczenia kąta Yaw z wykorzystaniem magnetometru + żyroskopu ??? ktoś coś wie ???

Zobacz powyższe komentarze na forum

FORBOT Damian Szymański © 2006 - 2017 Zakaz kopiowania treści oraz grafik bez zgody autora. vPRsLH.