mischi
12.03.2005, 20:17
Frage zu Kalmanfilter!
Ich habe in meinem Auto eine IMU von Rotomotion und ein Trimble GPS.
Rotomotion IMU Output: ax, ay, az, omegax, omegay, omegaz
GPS: lambda, phi, h, v, heading
Das ganze ist ja recht und schoen moechte das jetzt in einem Kalmanfilter zusammenfassen. Der Integrator laeuft bereits einwandfrei, hab ich auch entsprechn kalibriert. Integration der Navigationsgleichung mittels Eulerintegration, Kovarianzupdate fuktioniert auch.
Nur mit dem GPS hab ich Probleme. Hab die Zustaende des Integrators im Navigationsframe. Transformiere diese in ECEF frame mittels folgender Gleichung: xECEF = xstartpos + R'*x
R ist die Transformationsmatrix vom Navigationsframe nach ECEF frame, wobei ich die Werte von xStartpos verwende und diese Rotationmatrix somit konstant ist. x ist der aktuelle Zustand des Integrator im Navigationsframe.
Da ja die Werte des GPS im Geodeatischen Frame (lambda, phi, h) sind, linearisiere ich nun die nichtlineare Transformation (verwende hier vorlaeufig nur Kugelkoordinatentransformation, da die ich vermute das elliptische Modell der Erde geht im Rauschen unter) und erhalte somit die die linearisierte Messuebertragungsfunktion: z = H*xECEF, wobei ich fuer H die aktuellen Werte xECEF einsetze.
Die entsprechenden Kalmanfiltergleichungen habe ich mit einer UD-Faktorisierung implementiert, um die numerische Probleme bei der Matrixinvertierung des Kovarianzupdates zu umgehen (Funktioniert auch, hab ich mit Matlab getestet.
Nun mein Problem: 1.) Ist diese Vorgehensweise richtig
2.) wenn ich nun ohne Integrationsschritt arbeite und nur den GPS - Update mit immer dem gleichen Messwerten fuer (lambda phi und h) durchfuehre sollte man meinen, die Kovarianz konvergiert gegen ein Minimum und x konverigert gegen den aktuen Messwert des GPS.
Ich mache nun folgendes: Ich setze die Kovarianz der Position auf einen sehr hohen Wert 10000, da ich davon ausgehen, die Positon net genau stimmt. dann lasse ich den GPS update laufen. zuerst passt alles: Zustand wandert gegen Messwert, Kovarianzwerte wandert nach unten. Sobald die Zustaende erreicht sind, sollten sie gegen den Wert konvergieren bzw. dann um den Wert herum oszillieren, aber der Wert waechst langsam weiter, waehrend die Werte der Kovarianzmatrix langsam weiter richtig nach unten wandern, nach einer Weile, folgt dann folgendes: Zuerst wird die Differenz zwischen aktuellem Zustand so gross, dass auf einmal die Werte in 10 Potenzen explodieren, dies wiederum bewirkt, dass die Kovarianz auf realistische Werte runterkommt (Genauigkeit von ca. 30cm), Nun die Frage Was kann es dan haben, sofern hier eine Ferndiagnose moeglich ist. Ich wuerd auch gern meinen Matlabcode zur Verfuegung stellen zum Testen. Und nun hoff ich auf viele gute geistreiche Antworten
Michael
Ich habe in meinem Auto eine IMU von Rotomotion und ein Trimble GPS.
Rotomotion IMU Output: ax, ay, az, omegax, omegay, omegaz
GPS: lambda, phi, h, v, heading
Das ganze ist ja recht und schoen moechte das jetzt in einem Kalmanfilter zusammenfassen. Der Integrator laeuft bereits einwandfrei, hab ich auch entsprechn kalibriert. Integration der Navigationsgleichung mittels Eulerintegration, Kovarianzupdate fuktioniert auch.
Nur mit dem GPS hab ich Probleme. Hab die Zustaende des Integrators im Navigationsframe. Transformiere diese in ECEF frame mittels folgender Gleichung: xECEF = xstartpos + R'*x
R ist die Transformationsmatrix vom Navigationsframe nach ECEF frame, wobei ich die Werte von xStartpos verwende und diese Rotationmatrix somit konstant ist. x ist der aktuelle Zustand des Integrator im Navigationsframe.
Da ja die Werte des GPS im Geodeatischen Frame (lambda, phi, h) sind, linearisiere ich nun die nichtlineare Transformation (verwende hier vorlaeufig nur Kugelkoordinatentransformation, da die ich vermute das elliptische Modell der Erde geht im Rauschen unter) und erhalte somit die die linearisierte Messuebertragungsfunktion: z = H*xECEF, wobei ich fuer H die aktuellen Werte xECEF einsetze.
Die entsprechenden Kalmanfiltergleichungen habe ich mit einer UD-Faktorisierung implementiert, um die numerische Probleme bei der Matrixinvertierung des Kovarianzupdates zu umgehen (Funktioniert auch, hab ich mit Matlab getestet.
Nun mein Problem: 1.) Ist diese Vorgehensweise richtig
2.) wenn ich nun ohne Integrationsschritt arbeite und nur den GPS - Update mit immer dem gleichen Messwerten fuer (lambda phi und h) durchfuehre sollte man meinen, die Kovarianz konvergiert gegen ein Minimum und x konverigert gegen den aktuen Messwert des GPS.
Ich mache nun folgendes: Ich setze die Kovarianz der Position auf einen sehr hohen Wert 10000, da ich davon ausgehen, die Positon net genau stimmt. dann lasse ich den GPS update laufen. zuerst passt alles: Zustand wandert gegen Messwert, Kovarianzwerte wandert nach unten. Sobald die Zustaende erreicht sind, sollten sie gegen den Wert konvergieren bzw. dann um den Wert herum oszillieren, aber der Wert waechst langsam weiter, waehrend die Werte der Kovarianzmatrix langsam weiter richtig nach unten wandern, nach einer Weile, folgt dann folgendes: Zuerst wird die Differenz zwischen aktuellem Zustand so gross, dass auf einmal die Werte in 10 Potenzen explodieren, dies wiederum bewirkt, dass die Kovarianz auf realistische Werte runterkommt (Genauigkeit von ca. 30cm), Nun die Frage Was kann es dan haben, sofern hier eine Ferndiagnose moeglich ist. Ich wuerd auch gern meinen Matlabcode zur Verfuegung stellen zum Testen. Und nun hoff ich auf viele gute geistreiche Antworten
Michael