Logo FORBOT.pl - darmowe kursy elektroniki, Arduino, Raspberry Pi
Prowadniki kablowe dla robotów zaprojektowane do ruchów w trzech osiach

Filtr Kalmana od teorii do praktyki – #2

Filtr Kalmana od teorii do praktyki – #2

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

« Poprzedni artykuł z seriiNastępny artykuł z serii »

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.

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

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.

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

« Poprzedni artykuł z seriiNastępny artykuł z serii »

akcelerometr, filtr, kalman, kalmana, matlab, żyroskop

Załączniki

data2.txt (plain, 11 KB)

Dane testowe do filtru Kalmana.

Komentarze

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

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

Dodaj komentarz