Filtr Kalmana od teorii do praktyki - #2

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

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

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 😞

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay