www.mikrocontroller.net

Forum: Digitale Signalverarbeitung / DSP Frage zu einem Kalibrationsfilter Optionen


Autor: Mario (Gast)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Hi zusammen,
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
vor....Das Messmodell kann sehr einfach
gewählt werden, die Komponenten des Zustandsvektors können als
konstant angenommen werden....Bezeichnet 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

Autor: Detlef _a (detlef_a)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Hi,

>>- Ist die Messmatrix richtig ? Ist sie eine 7x7 Einheitsmatrix ?
Du hast die Messmatrix doch hingeschrieben, das sollte sein:
H_k = [ ax_m^2, -2*ax_m, ay_m^2, -2*ay_m, az_m^2, -2az_m, 1 ]
also ein 7*1 Vektor

>>- 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 ?

y_k ist m.E. Dein Pseudomeßwert, also g^2. Wenn x_k richtig geschätzt 
ist,
sollte y_k - H * x_k=0 sein.

>>- In dem Referenziertem PDF Dokument steht noch folgendes:
habe ich noch nicht ernsthaft reingekuckt, sollte aber noch Erhellendes 
zu finden sein.

Hab mal bißchen gebastelt ohne Erfolg:
Kalmanfilter berechnet Zustandsgrößen x eines Systems x(k+1)=A*x(k), 
y=H*x, aus y. Die Iteration ist schlicht:

xd=A*x; %Prediction
x=xd+kcal*(y-H*x); %Correction

kcal sind die Kalman-Verstärkungen, die werden für jeden step neu 
berechnet mit dem Eingangsrauschen des Systems und dem Meßrauschen. Das 
muß allerdings auch mit kleinen, festen kcal funktionieren, dann wirds 
nen Beobachter. Ist nicht mehr optimal, aber als Test ganz gut.

Das habe ich für das vorliegende Beispiel mal implementiert. 
Funktioniert nicht, warum weiß ich nicht, muß ich nochmal ausgeschafen 
überlegen. Scheint mir aber ne gute Idee zu sein. Hast Du das zitierte 
Buch als PDF verfügbar, würde ich wohl mal gerne reinkucken?

gute Nacht
Detlef


clear

a=[1 2 3]';  %der Beschleunigungvektor,naja, beliebig
a=a/norm(a); %g^2 soll mal 1 sein

sg=[4 5 6]'; %die 'wahren' Skalierungsfaktoren, die suche ich
bg=[7 8 9]'; %die 'wahren' Offsets, die suche ich

am=a./sg+bg; %das sind meine umskalierten/biased Messwerte

H=[(am.^2)' ;-2*am'];
H=[H(:) ; 1]'; % Das ist die Meßmatrix

x=zeros(7,1);  % der Zustandsvektor
n=1000;
xall=zeros(7,n);
kcal=1e-6*ones(7,1);
for(k=1:n)
  x=x+kcal*(1-H*x);
  xall(:,k)=x;
end

return

Autor: Mario (Gast)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Hallo Detlef,
danke für deine Antwort. Wenn ich 1-H*x rechne, dann kommt da nur ein 
Skalar bei raus. Wenn ich das mit dem Kalman Gain multiplziere und dann 
vom x subrahiere, korrigiere ich doch jedes Element von x um den 
gleichen Wert ? Ist da irgendwo der Fehler ?
Das Buch habe ich leider nicht als pdf. Aber die paar Seiten zu dem 
Thema kann ich dir einscannen und mailen falls es dir hilft ?

Autor: Detlef _a (detlef_a)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Mario wrote:
> Hallo Detlef,
> danke für deine Antwort. Wenn ich 1-H*x rechne, dann kommt da nur ein
> Skalar bei raus. Wenn ich das mit dem Kalman Gain multiplziere und dann
> vom x subrahiere, korrigiere ich doch jedes Element von x um den
> gleichen Wert ?

So ist es, das war auch genau mein Problem, die Elemente von x blieben 
gleich. Das ist nen System mit ner zeitvarianten Meßmatrix, vielleicht 
liegt da ein Verständnisproblem. Bin noch nicht dazu gekommen, mir das 
paper anzusehen.

> Das Buch habe ich leider nicht als pdf. Aber die paar Seiten zu dem
> Thema kann ich dir einscannen und mailen falls es dir hilft ?
gerne, danke.

gute Nacht
Detlef

Autor: Mario Hubner (mariohubner)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Hallo Detlef,
ich hoffe du wirst noch Zeit findne dir das genauer anzusehen, ich habe 
irgendwie keine Idee mehr :(. Das Kapitel habe ich dir eingescannt, wenn 
du mir deine mail Addresse gibst, schick ich es dir zu.

grüsse
mario

Autor: Detlef _a (detlef_a)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Hi,

jetzt gehts. Zwei Irrtümer waren noch vorhanden:
- Für ne zeitvariante Meßmatrix gibts keine konstanten 
Kalmanverstärkungen
- der Beschleunigungsvektor darf nicht konstant sein, d.h. um die 
Verstärkungs-/Biasfehler der Beschleunigungssensoren rausrechnen zu 
können muß Du die Sensoren im Schwerefeld drehen, logisch. Ansonsten ist 
das System nicht beobachtbar. Die Einlassung stammt aus dem scan.

Ein Beispiel in Matlab ist unten angehängt. Die vorgegeben Fehler werden 
auch korrekt bestimmt. Die Startwerte der Zustandsvariablen müssen recht 
sorgfältig gewählt werden. Liegt der Startwert zu sehr daneben, wird ne 
andere, ebenfalls gültige Lösung gefunden.

Das ganze ist ja im Prinzip die Nullstellensuche in einer quadratischen, 
mehrdimensionalen Funktion. Der Kalmanansatz liefert Dir wahrscheinlich 
nur ne numerische Iteration zur Lösung davon. Oder so, was weiß ich.

Danke für die schöne Frage und die tolle Idee.

Cheers
Detlef

clear

sg=[4 5 6]'; %die 'wahren' Skalierungsfaktoren, die suche ich
bg=[7 8 9]'; %die 'wahren' Offsets, die suche ich

x=[ sg(1).^2 sg(1)*bg(1) sg(2).^2 sg(2)*bg(2) sg(3).^2 sg(3)*bg(3)....
        (bg.^2)'*(sg.^2)]';
x=x+10*(rand(size(x))-0.5);
%x=ones (7,1);  % der Zustandsvektor
%x=zeros(7,1);  % der Zustandsvektor

n=1000;
xall=zeros(7,n);
Kall=zeros(7,n);

%Kalman stuff
% großes  R treibt die K langsamer auf Ihren kleineren Endwert
% kleines R treibt die K schneller auf Ihren größeren  Endwert
%kleines Q macht den stationären Endfehler kleiner
%großes  Q macht den Schätzer schneller
%großes  Q treibt die K schneller auf ihren stionären
%Endwert, der mit Q wächst
R = 1000;
P = 100*eye(7);
Q=0.0;
R=0;
A=eye(7);
for(k=1:n)
  %a=[1 2 3]';  %der Beschleunigungvektor,naja, beliebig
  a=rand(3,1)-0.5; %der Beschleunigungvektor, variabel, das Ding dreht 
im Raum
  a=a/norm(a); %g^2 soll mal 1 sein
  am=a./sg+bg; %das sind meine umskalierten/biased Messwerte
  C=[(am.^2)' ;-2*am'];
  C=[C(:) ; 1]'; % Das ist die Meßmatrix

  PS = A*P*A'+Q;
  K = PS*C'*inv(C*PS*C'+R);
  P = (eye(7)-K*C);
  P = P*PS*P'+K*R*K';

  x=x+K*(1-C*x);
  xall(:,k)=x;
  Kall(:,k)=K;
end

sqrt(x(1:2:5))
x(2:2:6)./x(1:2:5)
plot(xall(1:6,:)','.')
return

Autor: Mario Hubner (mariohubner)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Hi Detlef,
vielen Dank für deinen Support, das klappt prima ! Allerdings hab ich 
noch immer ein Problem. Der Filter schätzt nur richtig wenn die 
Startbedingungen gut gewählt sind. Bei dem Bias ist das kein Problem, 
den kann man ja nach Augenmaß gut ablesen. Bei den Skalierungsfaktoren 
sieht das jedoch anders aus. Werden diese Startwerte falsch gewählt, so 
findet der Filter wie erwartet eine Lösung bei der gilt 1 = C * x

Da, wie du auch geschrieben hast, viele Lösungen möglich sind ist das 
auch logisch, jedoch sind aufgrund des "falschen" Skalierungsfaktoren 
die einzelnen Sensoren noch immer nich richtig abgeglichen. Oder mache 
ich da noch was falsch ?

Autor: Detlef _a (detlef_a)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Mario Hubner wrote:

> Da, wie du auch geschrieben hast, viele Lösungen möglich sind ist das
> auch logisch, jedoch sind aufgrund des "falschen" Skalierungsfaktoren
> die einzelnen Sensoren noch immer nich richtig abgeglichen. Oder mache
> ich da noch was falsch ?

Genau, sie sind nicht richtig abgeglichen. Sie erfüllen 1 = C * x für 
die Meßwerte, die bei den 'zufälligen' Orientierungen der ersten Werte 
des Gravitationsvektors rauskommen. Kuck Dir die Zustandsgrößen an: die 
springen paarmal und sind dann konstant. Der Grund liegt darin, daß 
R=Q=0 ist, d.h. das System sieht kein 'Eingangsrauschen' und die 
Meßwerte sind auch rauschfrei angenommen. Deswegen ist das Kalman Filter 
nach kurzer Zeit der Meinung, daß es die Zustandsgrößen kennt und macht 
zu: K -> 0. Du kannst es mit Q>0,R>0 'offenhalten', dann zieht es sich 
womöglich noch zu einer anderen Lösung hin. Mit Q und R bißchen zu 
spielen ist ne gute Idee.

Ich würde physikalisch rangehen: Jeweils einen einzigen 
Beschleunigungsmesser parallel und orthogonal zum Schwerefeld messen 
lassen, dann gibts einen groben  Wert für den Proportionalitätsfaktor. 
Mit dem dann das Kalmanfilter starten. Das ist alles gutmütig, bei 
meinem Beispiel fange ich auch mit Faktor 2 falschen Werten an, trotzdem 
gehts.

schönen Sonntag noch
Cheers
Detlef

Autor: Mario Hubner (mariohubner)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Detlef _a wrote:
> Du kannst es mit Q>0,R>0 'offenhalten', dann zieht es sich
> womöglich noch zu einer anderen Lösung hin. Mit Q und R bißchen zu
> spielen ist ne gute Idee.

Ja, das verstehe ich. Ich habe auch mit den Q und R experimentiert aber 
ohne Erfolg. Die Startparameter für die Bias Werte sind auch relativ 
unkritsch. Grosse Probleme machen nur die Skalierungsfaktoren. Auch in 
deinem Beispiel sind die ja schon sehr gut gewählt. Wenn man da nur ein 
wenig dran schraubt kommt was völlig anderes raus.

> Ich würde physikalisch rangehen: Jeweils einen einzigen
> Beschleunigungsmesser parallel und orthogonal zum Schwerefeld messen
> lassen, dann gibts einen groben  Wert für den Proportionalitätsfaktor.
> Mit dem dann das Kalmanfilter starten.

Verstehe ich noch nicht :) Wenn ich die orthogonal ausgerichteten 
Sensoren auf parallel zum Schwerefeld ausrichte, dann kann ich den bias 
ablesen aber doch nicht Skalierungsfehler ?!

Autor: Detlef _a (detlef_a)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Mario Hubner wrote:

>
> Verstehe ich noch nicht :) Wenn ich die orthogonal ausgerichteten
> Sensoren auf parallel zum Schwerefeld ausrichte, dann kann ich den bias
> ablesen aber doch nicht Skalierungsfehler ?!

Dann verstehe ich noch was nicht. a_m=s*a+b , also Meßwerte a_m, 
Skalierungsfaktor s, wahre Beschleunigung a und Offset b, s und b 
unbekannt. Jetzt nimmst Du Dir einen der drei Beschleunigungssensoren 
vor und richtest den parallel zum Schwerefeld aus, sodaß die vollen 
9.81m/sec^2 auf den Sensor wirken. Meßwert a_m1 ablesen, sei mal 5. Dann 
orthogonal dazu ins Schwerefeld hängen, Meßwert a_m2 ablesen, sei mal 
drei. Dann hast Du zwei Gleichungen für zwei Unbekannte: 5=s*9.81+b, 
3=s*0+b. -> b=3, s=2/9.81. Das für alle drei Beschleunigungssensoren tun 
und damit das Filter starten!!??

Cheers
Detlef

Autor: Mario Hubner (mariohubner)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Ok der Punkt ist es, diese Prozedur für alle drei Sensoren zu machen. 
Ich hatte es so verstanden das du es nur für einen machen willst. Wenn 
man es aber für alle drei Sensoren machen muss und die Sensoren in 
gezielte Lagen bewegen muss, dann verspielt man ja schon wieder den 
Vorteil des Filters ?!

Autor: Detlef _a (detlef_a)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Mario Hubner wrote:
> Wenn
> man es aber für alle drei Sensoren machen muss und die Sensoren in
> gezielte Lagen bewegen muss, dann verspielt man ja schon wieder den
> Vorteil des Filters ?!

Mit der geschilderten Methode die groben Startwerte bestimmen und dann 
mit dem Filter die feine Kalibrierung durchziehen?! Scheint mir auch 
etwas akademische Diskussion zu sein: Irgendeine Vorstellung hast Du 
doch von den gains/Offsets, setze die als Startwerte an und probier, ob 
das Filter auf was physikalisch sinnvolles konvergiert. Machst Du 
Theorie oder hast Du laufende Hardware?


Cheers
Detlef

Autor: Mario Hubner (mariohubner)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Hallo Detlef
ich habe auch HW hier. Die Sensorwerte liegen ohne Korrektur schon recht 
gut an den Sollwerten. Wenn ich da den Filter drüber laufen lasse, dann 
schätzt er den Bias richtig. Die Skalierungsfaktoren sind aber so klein 
e-5 (!), das die Messwerte quasi gar nicht mehr ins Gewicht fallen. 
Einzig x7 (= 0.99999) aus dem Statevektor bringt das Ergebnis auf 1. Ich 
muss jetzt nochmal deinen Ansatz probieren und die Skalierungswerte der 
Sensoren durch das Gleichungssystem vorbestimmen. Allerdings habe ich da 
wenig Hoffnung das es dadurch besser wird, da die Fehler nur minimal 
sind.

Desweiteren hab ich dein script ein wenig modifiziert. Ich gebe mir 
jetzt geschätzte Startwerte vor und lasse dafür die rand Funktion weg. 
Wenn man mit sg_est ein wenig rumspielt sieht man sehr gut, das die 
Startwerte sehr sehr nah an dem wahren Werten liegen müssen, andernfalls 
kommen nie die erwarteten Werte heraus. Einzig die Wahl der bias 
Startwerte ist sehr gutmütig. Mach ich da noch was falsch oder muss der 
Skalierungsstartwert so gut gewählt werden ?

clear

sg=[4 5 6]'; %die 'wahren' Skalierungsfaktoren, die suche ich
bg=[7 8 9]'; %die 'wahren' Offsets, die suche ich

sg_est=[2 3 4]'; %die geschätzten Skalierungsfaktoren, die suche ich
bg_est=[7 8 9]'; %die geschätzten Offsets, die suche ich

x=[ sg_est(1).^2 sg_est(1)*bg_est(1) sg_est(2).^2 sg_est(2)*bg_est(2) 
sg_est(3).^2 sg_est(3)*bg_est(3) (bg_est.^2)'*(sg_est.^2)]';

n=1000;
xall=zeros(7,n);
Kall=zeros(7,n);

%Kalman stuff
% großes  R treibt die K langsamer auf Ihren kleineren Endwert
% kleines R treibt die K schneller auf Ihren größeren  Endwert
%kleines Q macht den stationären Endfehler kleiner
%großes  Q macht den Schätzer schneller
%großes  Q treibt die K schneller auf ihren stionären
%Endwert, der mit Q wächst
P = 100*eye(7);
Q=0;
R=0;
A=eye(7);
for(k=1:n)
  a=rand(3,1)-0.5; %der Beschleunigungvektor, variabel, das Ding dreh im 
Raum
  a=a/norm(a); %g^2 soll mal 1 sein
  am=a./sg+bg; %das sind meine umskalierten/biased Messwerte
  C=[(am.^2)' ;-2*am'];
  C=[C(:) ; 1]'; % Das ist die Meßmatrix

  PS = A*P*A'+Q;
  K = PS*C'*inv(C*PS*C'+R);
  P = (eye(7)-K*C);
  P = P*PS*P'+K*R*K';

  x=x+K*(1-C*x);
  xall(:,k)=x;
  Kall(:,k)=K;
end

sqrt(x(1:2:5))
x(2:2:6)./x(1:2:5)
plot(xall(1:6,:)','.')

Autor: Detlef _a (detlef_a)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Möglicherweise gibt es Probleme, wenn die 'Dynamik' der Zustandsgrößen 
hoch ist, beispielsweise s nahe bei 1, Offsets im 10000er Bereich. 
Ensinne mich dunkel, gab auch Skalierungstricks. Einer dieser Tricks 
ging meiner Erinnerung nach so: Skalierte Zustandsgröße ansetzen (also 
z.B. Offset/65536 für 16Bit Meßwerte, die landen dann in der Nähe von 1) 
und die Skalierung in der Meßmatrix C wieder rückgängig machen. Dann 
werden die K's verändert. Muß aber zuhause nochmal bißchen wg. Literatur 
stöbern.

Poste doch mal Deine Meßwerte.

Cheers
Detlef

Autor: Mario Hubner (mariohubner)
Datum:
Angehängte Dateien:

Bewertung
0 lesenswert
nicht lesenswert
Hi Detlef,
die Offsets sind ebenfalls < 1. Aber schau selbst:

Im 4er Ordner hab ich die Sensoren so gedreht wie von dir vorgeschlagen, 
um die Skalierungsfaktoren per Hand rechnen zu können. Im 5er Ordner 
liegen die Sensoren bewegungslos auf dem Tisch. Im Z Sensor koppelt 1G 
ein X und Y sollte 0G sein.

Autor: Mario Hubner (mariohubner)
Datum:
Angehängte Dateien:

Bewertung
0 lesenswert
nicht lesenswert
Hier noch ein paar Daten, wo die Sensoren langsam ohne Beschleunigung 
leicht um die x,y Achse gekippt und um die z Achse im Kreis gedreht 
werden.

Autor: Mario Hubner (mariohubner)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Konntest du mit den Daten was anfangen ?

Autor: Detlef _a (detlef_a)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Hi, nein, bin noch nicht dazu gekommen reinzukucken. Ist aber nicht ver- 
oder gegesssen.

gute Nacht
Detlef

Autor: Detlef _a (detlef_a)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Hi Mario, habe mir Deine Daten angesehen. Für 004 habe ich per hand die 
Offsets/Faktoren bestimmt und dann das Filter in diversen Spielarten 
rüberlaufen lassen. Ich kriegs auch nicht zum Spielen: Die Summe der 
Quadrate der korrigierten Meßwerte ist nahe bei 1 und bleibt 1. Trotzdem 
konvergieren die sg/bg irgendwo hin. Gerne wird auch ne Lösung gefunden, 
bei denen alle Skalierungsfaktoren nahe 0 sind und die Summe der 
Quadrate der Offsets = 1 wird. Das ist ne gültige Lösung.

Ich bin da erstmal auch ratlos. Vielleicht steckt noch nen Fehler im 
grundsätzlichen Ansatz, der ermöglicht ja explizit 'falsche' Lösungen.

Ich habe nen ähnliches Problem zu lösen: Bei mir gehts um ne einfache 
Summe ohne Quadrate und die soll 0 werden anstatt 1 wie bei Dir. An der 
kann ich mit geringer Intensität knobeln, wenn sich da was Anwendbares 
ergibt melde ich mich.

:-(

Cheers
Detlef

clear

load c:\interim\tmp\004\bxAcceleration.log
load c:\interim\tmp\004\byAcceleration.log
load c:\interim\tmp\004\bzAcceleration.log

acc=[bxAcceleration byAcceleration bzAcceleration];

sg=[4 5 6]'; %die 'wahren' Skalierungsfaktoren, die suche ich
bg=[7 8 9]'; %die 'wahren' Offsets, die suche ich

sg_est=[1 1 1]'; %die geschätzten Skalierungsfaktoren, die suche ich
bg_est=[-0.03  0.025 0.02]'; %die geschätzten Offsets, die suche ich

x=[ sg_est(1).^2 sg_est(1)*bg_est(1) sg_est(2).^2 sg_est(2)*bg_est(2) 
...
sg_est(3).^2 sg_est(3)*bg_est(3) (bg_est.^2)'*(sg_est.^2)]';

n=length(acc);
%n=10000;
xall=zeros(7,n);
Kall=zeros(7,n);
Call=zeros(7,n);
eall=zeros(1,n);
%Kalman stuff
% großes  R treibt die K langsamer auf Ihren kleineren Endwert
% kleines R treibt die K schneller auf Ihren größeren  Endwert
%kleines Q macht den stationären Endfehler kleiner
%großes  Q macht den Schätzer schneller
%großes  Q treibt die K schneller auf ihren stionären
%Endwert, der mit Q wächst
P = 100*eye(7);
Q=0.00001;
R=0.01;
A=eye(7);
for(k=1:n)
  %a=rand(3,1)-0.5; %der Beschleunigungvektor, variabel, das Ding dreh 
im Raum
  %a=a/norm(a); %g^2 soll mal 1 sein
  %am=a./sg+bg; %das sind meine umskalierten/biased Messwerte
  am=acc(k,:).';
  C=[(am.^2)' ;-2*am'];
  C=[C(:) ; 1].'; % Das ist die Meßmatrix

  Call(:,k)=C.';
  PS = A*P*A'+Q;
  %C*PS*C'+R
  K = PS*C'*inv(C*PS*C'+R);
  P = (eye(7)-K*C);
  P = P*PS*P'+K*R*K';

  eall(:,k)=C*x;
  x=x+K*(1-C*x);
  xall(:,k)=x;
  Kall(:,k)=K;
end

sqrt(x(1:2:5))
x(2:2:6)./x(1:2:5)
plot(xall(1:6,:)','.-')
plot(Kall(1,:))

return

Autor: Mario Hubner (mariohubner)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Hi Detlef,
ja, das entspricht dann wohl dem Verhalten welches ich auch festgestellt 
habe. Schade. Hast du nochmal in das erwähnte pdf File geschaut ? Da 
wird beiläufig erwähnt, das sie auch ein Gleichungssystem lösen um damit 
den Startvektor bestimmen zu können. Aber was oder wie sie es genau 
machen, verraten sie leider auch nicht. Ich hoffe du hast noch Erfolg. 
Ich bin auf deine Ergebnisse gespannt.

Grüsse
Mario

Autor: Mario Hubner (mariohubner)
Datum:

Bewertung
0 lesenswert
nicht lesenswert
Evt könnte dieses Dokument noch interessant sein, hier wird das 
Verfahren genauer beschrieben:

http://einstein.stanford.edu/content/sci_papers/pa...

Antwort schreiben

Die Angabe einer E-Mail-Adresse ist freiwillig. Wenn Sie automatisch per E-Mail über Antworten auf Ihren Beitrag informiert werden möchten, melden Sie sich bitte an.

Wichtige Regeln - erst lesen, dann posten!

  • Groß- und Kleinschreibung verwenden
  • Längeren Sourcecode nicht im Text einfügen, sondern als Dateianhang

Formatierung (mehr Informationen...)

  • [c]C-Code[/c]
  • [avrasm]AVR-Assembler-Code[/avrasm]
  • [code]Code in anderen Sprachen, ASCII-Zeichnungen[/code]
  • [math]Formel in LaTeX-Syntax[/math]
  • [[Titel]] - Link zu Artikel
  • Verweis auf anderen Beitrag einfügen: Rechtsklick auf Beitragstitel,
    "Adresse kopieren", und in den Text einfügen




Bild automatisch verkleinern, falls nötig
Bitte das JPG-Format nur für Fotos und Scans verwenden!
Zeichnungen und Screenshots im PNG- oder
GIF-Format hochladen. Siehe Bildformate.
Hinweis: der ursprüngliche Beitrag ist mehr als 6 Monate alt.
Bitte hier nur auf die ursprüngliche Frage antworten,
für neue Fragen einen neuen Beitrag erstellen.

Mit dem Abschicken bestätigst du, die Nutzungsbedingungen anzuerkennen.