GPS - IMU Kalmanfilter

[mischi]

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

[UlliC]

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

Hallo Michael,
wie erfaßt Du die Meßwerte ax,... und auf welchem Rechner läuft der Kalmanfilter.
Welche Matlab Version nutzt Du?
Ich arbeite mit Version 4.2c.1. Wenn Dein Matlabcode kompatibel ist, kann ich ja mal reinschauen.
Gruß
Ulli

danke fuer die antwort, hab den fehler gefunden hatte einen vorzeichenfehler bei der jakobimatrix. Ich arbeite mit einer imu von rotomotion
Zwar ungenau aber es funktioniert
Spielst du dich auch mit solchem Zeugsrum?

Michael

[UlliC]

danke fuer die antwort, hab den fehler gefunden hatte einen vorzeichenfehler bei der jakobimatrix. Ich arbeite mit einer imu von rotomotion
Zwar ungenau aber es funktioniert
Spielst du dich auch mit solchem Zeugsrum?

Hallo Michael,

Die IMU von rotomotion habe ich auch, ich setze allerdings nur die Sensorik in meinem balancierenden 2-Rad Roboter (UCBalBot) ein.
Auch dort läuft das System erstaunlich gut.
Mein TWR ist nur ein lehrreicher Zwischenschritt auf dem Weg zu einem autonom fliegenden Elektrohubschrauber......

Gruß
Ulli

[Stefan Easycopter]

Hi,

ich hab ne allgemeine Frage zu den rotomotion IMUs.

- hat jemand von euch Erfahrung mit einer IMU an einem PC board? geht das problemlos?
- wie genau arbeitet die IMU (mich interessieren eigentlich vor allem nur die Winkel (pitch, roll), die Position muss ich nciht bestimmen
- woher bekommt man die IMUs, direkt aus USA oder gibt's da einen Vertrieb in Europa.

Danke

Stefan

[UlliC]

Hi,

ich hab ne allgemeine Frage zu den rotomotion IMUs.

- hat jemand von euch Erfahrung mit einer IMU an einem PC board? geht das problemlos?
- wie genau arbeitet die IMU (mich interessieren eigentlich vor allem nur die Winkel (pitch, roll), die Position muss ich nciht bestimmen
- woher bekommt man die IMUs, direkt aus USA oder gibt's da einen Vertrieb in Europa.

Danke

Stefan

Hallo Stefan,

was meinst du mit PC Board ?
Ich habe die analogen Ausgänge Sensorplatinen direkt an die AD-Eingänge des Rechners meines UCBalBot angeschlossen. Der Spannungsbereich ist 0 - 5V. Die Nulllage liegt nominal bei 2,5 V. Hinzu kommt ein sensorbedingter Offset von einigen 100mV, den man einkalibrieren kann.
Genauigkeit: Ist im Datenblatt der Sensoren angegeben. Rotomotion setzt recht gute OPs ein OPA4342 und LT1014, sodaß kaum Offset und Drift hinzukommt. Bei entsprechender Auswertung (Filterung) der Sensorsignale liegt die Genauigkeit bei etwa 1-2°. Ich messe das demnächst mal nach.

Beschaffung:
Ich habe meine IMU direkt aus den USA. Und hatte ziemlichen Ärger damit, weil ich fast 50 € Zoll nachzahlen mußte. Im Gegensatz zu PNI Corporation hatte rotomotion nicht per global priority mail verschickt sondern per Post mit einer "falschen" Zollerklärung.

[Stefan Easycopter]

Hi Ulli,

Danke für die Hinweise. Meine Frage mit dem PC Board bezog sich auf die Datenübertragung zu mPC. Ich dachte, dass die bereits digitalisierte Werte über eine Schnittstelle (da steht irgendwas von RS 232 auf der Web Seite) übertragen ... und es gibt Screenshots von Daten der IMU, die am PC dargestellt werden. Liege ich nuin völlig falsch...?

Stefan

[UlliC]

Danke für die Hinweise. Meine Frage mit dem PC Board bezog sich auf die Datenübertragung zu mPC. Ich dachte, dass die bereits digitalisierte Werte über eine Schnittstelle (da steht irgendwas von RS 232 auf der Web Seite) übertragen ... und es gibt Screenshots von Daten der IMU, die am PC dargestellt werden. Liege ich nuin völlig falsch...?

Hallo Stefan,
Die IMU besteht aus den Sensoren und einer Platine mit einem ATmega32 als CPU. Die CPU hat 8 10Bit ADC Kanäle und wertet die Sensordaten aus. Die Meßwerte werden über die RS232 der Platine an den Host-PC geschickt, wenn man das mitgelieferte Programm (xxx?.exe) auf dem PC startet und die richtigen Knöpfe drückt. Das Protokoll und solche Nebensächlichkeiten sind natürlich nicht bekannt. Angeblich soll es sogar den SourceCode im Netz geben. Ich habe aber bisher noch nicht wirklich was gefunden. Wobei es auch nicht so schwer sein dürfte ein derartiges Programm zu erstellen. Da machen die Jungs hier im Forum schon ganz andere Sachen.
Wichtig: Die Schaltpläne und Bestückungslisten, die man sich dowloaden kann (und die auf der CD sind) stimmen nur zum Teil.
Man hat mir vor 8 Monaten versprochen, das zu korrigieren.....