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
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
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 ?
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
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
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
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 ?
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
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 ?!
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
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 ?!
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
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,:)','.')
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
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.
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.
Hi, nein, bin noch nicht dazu gekommen reinzukucken. Ist aber nicht ver- oder gegesssen. gute Nacht Detlef
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
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
Evt könnte dieses Dokument noch interessant sein, hier wird das Verfahren genauer beschrieben: http://einstein.stanford.edu/content/sci_papers/papers/HauptG_1995_57.pdf
Thread beobachten
Seitenaufteilung abschaltenBitte melde dich an um einen Beitrag zu schreiben. Anmeldung ist kostenlos und dauert nur eine Minute.
Bestehender Account
Schon ein Account bei Google/GoogleMail? Keine Anmeldung erforderlich!
Mit Google-Account einloggen
Mit Google-Account einloggen
Noch kein Account? Hier anmelden.