Kursy • Poradniki • Inspirujące DIY • Forum

Transformator – budowa,
zasada działania i zastosowanie

3 podstawowe rodzaje
akumulatorów, które warto znać

Co ciekawego w DIY?
10 projektów naszych czytelników #33

Piezoelektryki – czym
są materiały piezoelektryczne?

Jak czytać oznaczenia
baterii? Norma IEC 60086-1 w praktyce

Jak działa płyta
indukcyjna? Czym są prądy wirowe?

Czym jest moc?

Poznaj podstawy

Raspberry Pi

Od podstaw

Kursy Artykuły

DIY Forum

Filtr Kalmana od teorii do praktyki – #2

Programowanie GandalF

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 serii Nastę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.

Ż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:

Wyprowadzenie modelu stanowego

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:

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:

Implementacja w Matlabie

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

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

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.

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

Załączniki

data2.txt (plain, 11 KB)

Dane testowe do filtru Kalmana.

Artykuł był ciekawy?

Dołącz do 20 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 na bazie Arduino i Raspberry Pi.