Skocz do zawartości
Komentator

Filtr Kalmana od teorii do praktyki - #2

Pomocna odpowiedź

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

UWAGA, to tylko wstęp! Dalsza część artykułu dostępna jest na blogu.

Przeczytaj całość »

Poniżej znajdują się komentarze powiązane z tym wpisem.

Udostępnij ten post


Link to post
Share on other sites

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

dane.txt

Udostępnij ten post


Link to post
Share on other sites

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

Udostępnij ten post


Link to post
Share on other sites

Cześć !

W artykule wzór na wyznaczenie wartości macierzy kowariancji Ppost jest następujący :

Ppost = Ppri - K*S*K

ale w literaturze przedmiotu wzór jest inny tzn. :

Ppost = (I - K*C) * Ppri ;gdzie I jest macierzą jednostkową 2x2

i teraz mam pytanie do autora artykułu GandalF jak przekształcić wzór : Ppost = (I - KC)*Ppri do Ppost = Ppri - KSK?

Proszę o pomoc w tej sprawie, bo próbowałem to przekształcić i cały czas mi się coś nie zgadza 😞

Udostępnij ten post


Link to post
Share on other sites

Dołącz do dyskusji, napisz odpowiedź!

Jeśli masz już konto to zaloguj się teraz, aby opublikować wiadomość jako Ty. Możesz też napisać teraz i zarejestrować się później.
Uwaga: wgrywanie zdjęć i załączników dostępne jest po zalogowaniu!

Gość
Napisz odpowiedź...

×   Wklejony jako tekst z formatowaniem.   Przywróć formatowanie

  Dozwolonych jest tylko 75 emoji.

×   Twój link będzie automatycznie osadzony.   Wyświetlać jako link

×   Twoja poprzednia zawartość została przywrócona.   Wyczyść edytor

×   Nie możesz wkleić zdjęć bezpośrednio. Prześlij lub wstaw obrazy z adresu URL.


×
×
  • Utwórz nowe...