Kombinieren von Gyroskop- und Beschleunigungsmesserdaten

Ich baue mit dem NXT-System von Lego Mindstorm einen Auswuchtroboter. Ich verwende zwei Sensoren von HiTechnic, wobei der erste ein Beschleunigungsmesser und der zweite ein Gyroskop ist. Ich habe erfolgreich Rauschen von beiden Sensoren und abgeleitete angular für beide in einem Bereich zwischen -90 und 90 Grad herausgefiltert, wobei 0 Grad perfekt ausgeglichen sind.

Meine nächste Herausforderung besteht darin, beide Sensorwerte zu kombinieren, um die Drift des Gyroskopes im Laufe der Zeit zu korrigieren. Unten ist ein Beispieldiagramm, das ich aus tatsächlichen Daten erstellt habe, um die Drift vom Gyroskop zu demonstrieren:

Bildbeschreibung hier eingeben

Der am häufigsten verwendete Ansatz, den ich gesehen habe, um diese Sensoren zu einem soliden Fundament zu machen, ist die Verwendung eines Kalman-Filters. Ich bin jedoch kein Experte in Infinitesimalrechnung und ich verstehe mathematische Symbole wirklich nicht, ich verstehe Mathematik im Quellcode jedoch.

Ich benutze RobotC (das ist wie jedes andere C-Derivat) und würde es wirklich schätzen, wenn mir jemand Beispiele dafür geben könnte, wie man das in C erreicht.

Danke für Ihre Hilfe!

LÖSUNGEN:

In Ordnung, Kersny hat mein Problem getriggers, indem er mich mit komplementären Filtern bekannt gemacht hat. Dies ist eine Grafik, die meine Ergebnisse zeigt:

Ergebnis # 1

Bildbeschreibung hier eingeben

Ergebnis # 2

Bildbeschreibung hier eingeben

Wie Sie sehen können, korrigiert der Filter die gyroskopische Drift und kombiniert beide Signale zu einem einzigen glatten Signal.

Edit: Da ich die kaputten Bilder sowieso repariert habe, dachte ich, es würde Spaß machen, das Rig zu zeigen, mit dem ich diese Daten erzeugt habe:

Bildbeschreibung hier eingeben

Kalman Filter sind großartig und alle, aber ich finde den Komplementärfilter viel einfacher zu implementieren mit ähnlichen Ergebnissen. Die besten Artikel, die ich für die Kodierung eines Komplementärfilters gefunden habe, sind dieses Wiki (zusammen mit diesem Artikel über das Umwandeln von Sensoren in technische Einheiten) und ein PDF in der Zip-Datei auf dieser Seite (Unter Technischer Dokumentation glaube ich der Dateiname in der Zip ist filter.pdf);

PS. Wenn Sie auf einem Kalman-Filter stecken, ist hier ein C-Syntax-Code für das Arduino, das es implementiert.

Gamasutra.com hat einen Artikel über die Verwendung von Kalman-Filtern für die WiiMote-Filterung veröffentlicht .

Es gibt einige Links zum C ++ – Quellcode am Ende des Artikels.