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
Bitte 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.