Kursy • Poradniki • Inspirujące DIY • Forum
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 »
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, ϕ, θ (patrz rysunek). Kąt ϕ można obliczyć z wzoru:
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 ωg można więc zapisać jako suma rzeczywistej prędkości kątowej ω i aktualnej wartości dryftu g:
W modelu stanowym obiektu zastosujemy pewien trik.
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:
Z tych równań otrzymujemy następujący model stanowy:
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:
Oto implementacja filtru w Matlabie:
%Wczytywanie danych z pliku
datatmp = importdata('data2.txt');
data = datatmp.data;
gyro_data = data(:,1:3);
acc_data = data(:,4:6);
%Skalowanie danych
gyro_data = gyro_data*250/32768;
acc_data = acc_data*4/65535;
%Wektor czasu
dt = 0.1;
t = dt:dt:size(data, 1)*dt;
%Model w przestrzeni stanu
A = [1 -dt; 0 1];
B = [dt; 0];
C = [1 0];
%Szum pomiarowy i procesowy
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;
% Wartosci poczatkowe
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;
u = gyro_data(i,3);
Y(i) = acc_angle;
if i == 1;
% Pierwszy pomiar sluzy nam jako wartosc poczatkowa dla filtru
xpost = [acc_angle; 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
plot(t, Y, 'b', t, Yf, 'r', 'LineWidth', 1.5)
title('Filtr Kalmana')
xlabel('Czas')
ylabel('Sygnal mierzony')
legend('Wartosc mierzona', 'Wartosc estymowana')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.
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.
« Poprzedni artykuł z seriiNastępny artykuł z serii »
Przeczytaj powiązane artykuły oraz aktualnie popularne wpisy lub losuj inny artykuł »
akcelerometr, filtr, kalman, kalmana, matlab, żyroskop
Dołącz do 30 tysięcy osób, które otrzymują powiadomienia o nowych artykułach! Zapisz się, a otrzymasz PDF-y ze ściągami (m.in. na temat mocy, tranzystorów, diod i schematów) oraz listę inspirujących DIY z Arduino i RPi.
Trwa ładowanie komentarzy...