Frage zu einem Kalibrationsfilter

07/01/2008 - 13:38 von mario | Report spam
Hi Group,
ich lese gerade in dem Buch "Integrierte Navigationssysteme" von Jan
Wendel ISBN 978-3-486-58160-7.
Dort wird in dem Kapitel 10.4.1. ein Verfahren zur Kalibrierung von
Beschleunigungssensoren vorgestellt.
Die Sensoren sind orthogonal zueinander ausgerichtet. Jeder Sensor
misst die Beschleunigung in einer Achsrichtung.
Durch das verfahren sollen die Bias und Skalenfaktoren jedes Sensors
ermittelt werden. Die Idee des Verfahrens
ist, die Sensoren so zu Bewegen, das in jeden Sensor die
Schwerebeschleunigung einkoppeln kann. Eine Beschleunigung
der Sensoren soll dabei vermieden werden. Es soll ausgenutzt werden,
das die Quadratsumme der gemessenen Beschleunigungen
in den drei Raumrichtungen bei korrekt kalibrierten Sensoren gerade
dem Quadrat der Schwerebeschleunigung entspricht:

g^2 = ax^2 + by^2 + az^2

Die obige Formel wird umgeformt, sodass die Bias und Skalenfaktoren
enthalten sind ( Matlab Notation ):

g^2 = [ ax_m^2, -2*ax_m, ay_m^2, -2*ay_m, az_m^2, -2az_m, 1 ] *
[ sx^2, sx^2*bx, sy^2, sy^2*by, sz^2, sz^2*bz, sx^2*bx^2 +
sy^2*by^2 + sz^2* bz^2 ]'

mit:
ax_m, ay_m, az_m = Gemessene Beschleunigung
sx, sy, sz = Skalierungsfaktoren
bx, by, bz = Bias

Soweit konnte ich es nachvollziehen ;) Jetzt wirds schwieriger ich
zitiere mal aus dem Buch:

"Fasst man das Quadrat der Schwerebeschleunigung als Pseudo Messwert
und die die Beschleunigungsmessungen
enthaltene Matrix als Messmatrix auf und ergànzt noch einen
Messrauschterm so liegt mit (obiger Gleichung)
ein linears Messmodell in der bei einem Kalman Filter üblichen Form
vorDas Messmodell kann sehr einfach
gewàhlt werden, die Komponenten des Zustandsvektors können als
konstant angenommen werdenBezeichnet man die
Komponenten des zustanstandsvektors mit x1 bis x7, so kann anhand der
vom Kalman Filter gelieferten Zustandsschàtzung
über folgende Gleichungen auf die zu bestimmenden Skalenfaktoren
und Biase geschlossen werden."

sx = sqrt( x1 )
bx = x2 / x1
sy = sqrt( x3 )
by = x4 / x3
sz = sqrt( x5 )
bz = x6 / x5

Als zusàtzliche Referenz wird witerhin dieses Dokument angegeben:

http://waas.stanford.edu/~wwu/papers/gps/PDF/demozins201.pdf



Ganz blauàugig bin ich an die Sache drangegangen...:

In Kapitel 6 wird der Kalman Filter beschrieben, zitat:
"Der Zusammenhang zwischen Messwerten y_k und den Systemzustand x_k
wird durch ein Messmodell der Form:

y_k = H_k * x_k + v_k

beschrieben."

Demnach wàre:
g^2 = y_k
H_k = [ ax_m^2, -2*ax_m, ay_m^2, -2*ay_m, az_m^2, -2az_m, 1 ]
x_k = [ sx^2, sx^2*bx, sy^2, sy^2*by, sz^2, sz^2*bz, sx^2*bx^2 +
sy^2*by^2 + sz^2* bz^2 ]



Folgendes Matlab Script habe ich mir nun erstellt:

H = eye( 7 ); % Mein Messmodell ist eine
Einheitsmatrix.
P_k = eye( 7 ); % Meine Kovarianzmatrix wird als
Einheitsmatrix initialisiert.
I = eye( 7 ); % Wird auch gebraucht...
x_k = [ 0; 0; 0; 0; 0; 0; 0 ]; % Der Statevector, der die Terme fuer
Bias und Skalierungswerte enthalten sollte...

% Einige Variablen zum Speichern des State Vectors:
x1 = [ ];
x2 = [ ];
x3 = [ ];
x4 = [ ];
x5 = [ ];
x6 = [ ];
x7 = [ ];


r = 0.015;
R = eye( 7 ) * r; % Measurement Noise: Willürlich gewàhlter Wert.

q = 0.01;
Q = eye( 7 ) * q; % Process Noise: Willkürlich gewàhlter Wert.


A = eye( 7 ); % Mein Messmodell ist auch eine Einheitsmatrix


% Die von den Beschleunigungssensoren gemessenen Beschleunigungen:
ax = 0;
ay = 0;
az = 0;

% Eine Loop über alle Messwerte:
for i = 1 : length( ax )
%

% Prediction

% Der für die Prediction erwartete Fehler:
P_k = A * P_k * A' + Q;

%

% Estimation

% Bestimmung des Kalman Gain.
K_k = P_k * H' * inv( H * P_k * H' + R );

% Die aktuellen Messwerte.
ax = bx( i );
ay = by( i );
az = bz( i );

% Den State Vector bestimmen.
z_k = [ ax^2, -2 * ax, ay^2, -2 * ay, az^2, -2 * az, 1 ]';
y_k = H * z_k;
x_k = x_k + K_k * ( y_k - H * x_k );

% Die Kovarianzmatrix K aktualisieren.
P_k = ( I - K_k * H ) * P_k;

%


% Den State Vector speichern.
x1 = [ x1 x_k( 1 ) ];
x2 = [ x2 x_k( 2 ) ];
x3 = [ x3 x_k( 3 ) ];
x4 = [ x4 x_k( 4 ) ];
x5 = [ x5 x_k( 5 ) ];
x6 = [ x6 x_k( 6 ) ];
end

% Skalenfaktoren und Bias Werte berechnen.
sx = sqrt( abs( x1 ) );
bias_x = x2 ./ x1;

sy = sqrt( abs( x3 ) );
bias_y = x4 ./ x3;

sz = sqrt( abs( x5 ) );
bias_z = x6 ./ x5;

% Kompensierung der Messwerte.
bxc = sx .* bx - bias_x;
byc = sy .* by - bias_y;
bzc = sz .* bz - bias_z;



Wenn ich das Script auf einen Satz von Testdaten loslasse, von dem ich
Bias und Skalierungsfaktoren kenne,
dann stabilisiert sich der State Vektor nach einigen Durchlàufen. Die
berechneten Skalierungs und Bias
Werte sind jedoch völliger Unsinn :(
Ich habe jetzt schon einige Tage über das Problem nachgedacht, komme
aber irgendwie auf keinen grünen
Zweig. Hier mal eine Sammlung von Punkten die mir nicht klar sind:

- Brauch ich den Predicton Schritt, es liegen ja in jedem Durchlauf
neue Messdaten vor.
- Ist die Messmatrix richtig ? Ist sie eine 7x7 Einheitsmatrix ?
- Wenn der State Vektor bestimmt wird, wird gerechnet: y_k - H * x_k
Hier muss ich doch die Differenz vom erwarteten Wert zum aktuellen
Wert bestimmen. Mein erwarteter Wert
ist aber die Schwerebeschleunigung, die ich völlig unterschlage ?!
Wie verheirate ich Statevektor und
Schewerebeschleunigung ?
- In dem Referenziertem PDF Dokument steht noch folgendes:
"Once the first step states are estimated, the scale factors and
hard iron biases are extracted from
the first step states algebraically by using the auxiliary
variables." Habe ich hier noch irgendwas vergessen ?!

Ihr seht ich habe mehr Fragen als Antworten :) Ich hoffe jemand in
dieser Group kann mir ein wenig auf die
Sprünge helfen.

Vielen Dank für eure Mühe.

Grüsse
Mario
 

Lesen sie die antworten

#1 mario
07/01/2008 - 19:58 | Warnen spam
Hat niemand eine Idee oder einen guten Vorschlag ?

Ähnliche fragen