Extended Kalman Filter, Orientierung mit Quaternionen

WICHTIG: Der Betrieb von goMatlab.de wird privat finanziert fortgesetzt. - Mehr Infos...

Partner:

Forum

Option

[Erweitert]

• Diese Seite per Mail weiterempfehlen

Gehe zu:

DrTurkelten

Gast


	Beiträge: ---

	Anmeldedatum: ---

	Wohnort: ---

	Version: ---

Verfasst am: 08.09.2015, 16:28 Titel: Extended Kalman Filter, Orientierung mit Quaternionen

Hallo!

Ich arbeite gerade an einen Extended Kalman Filter (EKF). Das gesamte Projekt sieht so aus, dass ich von einem MEMS-Chip mit Gyroscopen und Acceleratoren Daten bekomme, diese an Matlab schicke und dann mit Hilfe eines EKF und Quaternionen die Orientierung des Chips bzw. irgendwann eine kurze Navigation ermögliche.

Allerdings stecke ich gerade ein bisschen in der Sackgasse, habs nicht verstanden oder sehe den Wald vor lauter Bäumen nicht mehr Rolling Eyes

Um mich langsam an den EKF ranzutasten, simuliere ich erstmal nur die Daten von einem Gyroskop in X Richtung. Er zeigt mir also die Rollgeschwindigkeit. Beschleunigungsmesser, lasse ich erstmal komplett weg. Ziel es eine Sekunde lang mit 45 Grad/s zu drehen. Also muss roll = 45 sein und yaw und pitch = 0;

Die allgemeine Frage: Welche Zustände nehme ich für den Zustandsvektor?
Ich habe Beispiele gesehen, in den alle Zustände, also Messwerte, Berechnungswerte und Ausgangswerte übernommen werden z.B.
[1] http://campar.in.tum.de/Chair/KalmanFilter
ab 'Extended Kalman Filter for position and orientation tracking'

aber auch Beispiele ohne die Messwerte

http://www.goddardconsulting.ca/sim.....lman-filter-tracking.html

Mein Code (ein Teil davon) sieht folgendermaßen aus:

(lieber erstmal ausblenden Wink

)

Code:

%%%%%%%%%%%%%%%%%%%%%%%%
%% EKF initialization %%
%%%%%%%%%%%%%%%%%%%%%%%%

% model x_k = f(x_k-1, u, w)
% xprio: state prediction, before correction
% xpost: state correction, after prediction
% x(1 - 4): Quaternion current attitude, q.rot, q.x, q.y, q.z
% u(1 - 4): Quaternion delta rotation, q.rot, q.x, q.y, q.z
%
% x(1 - 4) * u(1 - 4), quaternion multiplication :
% http://de.mathworks.com/help/aerobl.....ernionmultiplication.html
%
% q.rot = (q1.rot*q2.rot - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z);
% q.x = (q1.rot*q2.x + q1.x*q2.rot + q1.y*q2.z - q1.z*q2.y);
% q.y = (q1.rot*q2.y - q1.x*q2.z + q1.y*q2.rot + q1.z*q2.x);
% q.z = (q1.rot*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.rot);

f = @(x,u) [ x(1)*u(1) - x(2)*u(2) - x(3)*u(3) - x(4)*u(4) ; % qrot
x(1)*u(2) + x(2)*u(1) + x(3)*u(4) - x(4)*u(3) ; % qx
x(1)*u(3) - x(2)*u(4) + x(3)*u(1) + x(4)*u(2) ; % qy
x(1)*u(4) + x(2)*u(3) - x(3)*u(2) + x(4)*u(1) ; % qz
x(5) % gx
x(6) % gy
x(7) ]; % gz

% measurement model
% h(x)
h = @(x) [ atan2(2*x(2)*x(1)-2*x(3)*x(4) , 1 - 2*x(2)*x(2) - 2*x(4)*x(4) ) % delta gx
asin( 2*(x(2)*x(3) + x(4)*x(1) ) ); % delta gy
atan2(2*x(3)*x(1)-2*x(2)*x(4) , 1 - 2*x(3)*x(3) - 2*x(4)*x(4) ) ]; % delta gz

states = 7; % amount of states used in the state-vector
% x or rather f in EKF

Q = eye(states)*.00001; % std of system noise
R = eye(3)*.001; % std of measurement noise
% dimension of R: amount of measured states

% prio: prediction, before correction
% post: correction, after prediction

Pprio = eye(states); % P covariances
Ppost = eye(states);

I = eye(states); % identity matrix

% initial states
xprio = [ 1
0
0
0
0
0
0 ];

xpost = [ 1
0
0
0
0
0
0 ];

% Quaternions
qCurrent = Quaternion(1, 0, 0, 0); % Quaternion represent current attitude
qDelta = Quaternion(1, 0, 0, 0); % Quaternion represent attitude change
% per sample
%%%%%%%%%%%%%%%%
%% simulation %%
%%%%%%%%%%%%%%%%

t = 0:dt:1; % time vector for simulation values;
N = length(t); % amount of values for simulation

% gx, simulation data
gx(1:N) = 45*DEG2RAD; %[rad/s] % roll
gy(1:N) = 0; %[DEG/s] % pitch
gz(1:N) = 0; %[DEG/s] % yaw

% noise, simulation data
w = Q*randn(states,N); % system noise (gaussian)
v = R*randn(3,N); % measurement noise (gaussian)

%%%%%%%%%%%%%%%%%
%% go! EKF go! %%
%%%%%%%%%%%%%%%%%

for k=2:N
%%%%%%%%%%%%%%%%
% measurements %
%%%%%%%%%%%%%%%%
z(:,k) = [ gx(k)
gy(k)
gz(k) ] + v(:,k);
z(:,k) = z(:,k)*dt;

%%% condition %%%
qDelta = Quaternion.qFromEuler(xpost(5),xpost(6),xpost(7));
qDelta = qDelta.normalize();
u(1) = qDelta.rot;
u(2) = qDelta.x;
u(3) = qDelta.y;
u(4) = qDelta.z;

%%%%%%%%%%%%%%
% prediction %
%%%%%%%%%%%%%%
xprio = f(xpost,u) + w(:,k); % u: Quaternion qDelta

% normalize qCurrent
qCurrent = Quaternion(xprio(1), xprio(2), xprio(3), xprio(4));
qCurrent = qCurrent.normalize();
xprio(1) = qCurrent.rot;
xprio(2) = qCurrent.x;
xprio(3) = qCurrent.y;
xprio(4) = qCurrent.z;
% qCurrent is now the new attitude

% Jacobian F
JF =[ u(1), -u(2), -u(3), -u(4), 0, 0, 0
u(2), u(1), u(4), -u(3), 0, 0, 0
u(3), -u(4), u(1), u(2), 0, 0, 0
u(4), u(3), u(2), u(1), 0, 0, 0
0, 0, 0, 0, 1, 0, 0
0, 0, 0, 0, 0, 1, 0
0, 0, 0, 0, 0, 0, 1 ];
%% % Jacobian H
% u: q.rot q.x q.y q.z (Current)
u(1) = xprio(1);
u(2) = xprio(2);
u(3) = xprio(3);
u(4) = xprio(4);

JH11 = (2*u(2))/((u(3)*u(2)^2*u(4) - 2*u(1)*u(2) + 2*u(4)^2)^2 + 1) ;
JH12 = (2*u(1) - 2*u(2)*u(3)*u(4))/((u(3)*u(2)^2*u(4) - 2*u(1)*u(2) + 2*u(4)^2)^2 + 1) ;
JH13 = -(u(2)^2*u(4))/((u(3)*u(2)^2*u(4) - 2*u(1)*u(2) + 2*u(4)^2)^2 + 1) ;
JH14 = -(u(3)*u(2)^2 + 4*u(4))/((u(3)*u(2)^2*u(4) - 2*u(1)*u(2) + 2*u(4)^2)^2 + 1);

JH21 = (2*u(4))/(1 - (2*u(1)*u(4) + 2*u(2)*u(3))^2)^(1/2);
JH22 = (2*u(3))/(1 - (2*u(1)*u(4) + 2*u(2)*u(3))^2)^(1/2);
JH23 = (2*u(2))/(1 - (2*u(1)*u(4) + 2*u(2)*u(3))^2)^(1/2);
JH24 = (2*u(1))/(1 - (2*u(1)*u(4) + 2*u(2)*u(3))^2)^(1/2);

JH31 = (2*u(3))/((2*u(3)^2 - 2*u(1)*u(3) + 2*u(4)^2 + 2*u(2)*u(4))^2 + 1);
JH32 = -(2*u(4))/((2*u(3)^2 - 2*u(1)*u(3) + 2*u(4)^2 + 2*u(2)*u(4))^2 + 1);
JH33 = (2*u(1) - 4*u(3))/((2*u(3)^2 - 2*u(1)*u(3) + 2*u(4)^2 + 2*u(2)*u(4))^2 + 1);
JH34 = -(2*u(2) + 4*u(4))/((2*u(3)^2 - 2*u(1)*u(3) + 2*u(4)^2 + 2*u(2)*u(4))^2 + 1);

JH = [ JH11, JH12, JH13, JH14, 0, 0, 0
JH21, JH22, JH23, JH24, 0, 0, 0
JH31, JH32, JH33, JH34, 0, 0, 0 ];

Pprio = JF*Ppost*JF' + Q;

%%%%%%%%%%%%%%
% correction %
%%%%%%%%%%%%%%

S = JH*Pprio*JH' + R;
K = Pprio*JH' / S; % Kalman Gain

y = z(:,k) - h(xprio);

xpost = xprio + K*y;
Ppost = (I-K*JH)*Pprio;

% normalize qCurrent
qCurrent = Quaternion(xpost(1), xpost(2), xpost(3), xpost(4));
qCurrent = qCurrent.normalize();
xpost(1) = qCurrent.rot;
xpost(2) = qCurrent.x;
xpost(3) = qCurrent.y;
xpost(4) = qCurrent.z;

ang = Quaternion.eulerFromQ(qCurrent); %=[DEG]

%%% data collecting %%%
angles(1,k) = ang(1);
angles(2,k) = ang(2);
angles(3,k) = ang(3);
gx_(k) = xpost(5);
end

Funktion ohne Link?

Die Messfunktion h(x) entspricht den Gleichungen aus

[2] http://www.euclideanspace.com/maths.....rsions/quaternionToEuler/
(ganz oben)

Die Gleichungen unter Jacobian H, sind deren Ableitungen.

Irgendwo im Aufbau des Filters liegt ein Fehler. Zum Beispiel werden die Messwerte gx, gy, gz nie aktualisiert. Ich vermute, dass meine Messfunktion falsch ist. Obwohl in Beispiel [2] die Messwerte mithilfe ihrer nichtlinearen Zusammenhänge zu den Zuständen beschrieben werden.

Quaternion.m

Beschreibung:

Quaternion Klasse

Download

Dateiname:

Quaternion.m

Dateigröße:

4.15 KB

Heruntergeladen:

555 mal

EKF_Quaternion_goMatlab.m

Beschreibung:

Download

Dateiname:

EKF_Quaternion_goMatlab.m

Dateigröße:

6.56 KB

Heruntergeladen:

605 mal


Einstellungen und Berechtigungen
Beiträge der letzten Zeit anzeigen:	Du kannst Beiträge in dieses Forum schreiben. Du kannst auf Beiträge in diesem Forum antworten. Du kannst deine Beiträge in diesem Forum nicht bearbeiten. Du kannst deine Beiträge in diesem Forum nicht löschen. Du kannst an Umfragen in diesem Forum nicht mitmachen. Du kannst Dateien in diesem Forum posten Du kannst Dateien in diesem Forum herunterladen

Impressum | Nutzungsbedingungen | Datenschutz | FAQ |

RSS

Hosted by:

Copyright © 2007 - 2024 goMatlab.de | Dies ist keine offizielle Website der Firma The Mathworks

MATLAB, Simulink, Stateflow, Handle Graphics, Real-Time Workshop, SimBiology, SimHydraulics, SimEvents, and xPC TargetBox are registered trademarks and The MathWorks, the L-shaped membrane logo, and Embedded MATLAB are trademarks of The MathWorks, Inc.