C/C++ : einfach zu nutzende Kalman-Lib für den Pi

[HaWe]

hallo,
aus "gegebenem Anlass" bin ich nochmals auf den Kalman Filter gestoßen zur Sensor Fusion von (nicht Gauss'sch) verrauschten Sensoren.

Da nicht-Gauss'sch verrauscht, heißt das zunächst: man braucht den Extended Kalman.

Von alleine gelöst hat sich das Problem, Kompass, Accelerometer und Gyro per Kalman zu fusionieren - das macht mein IMU Sensor selbstständig per eigener cpu.
Drehung wird also perfekt gemessen.

Nun soll der IMU-Wert genutzt werden, um festzustellen, ob man geradeaus oder im Bogen fährt.

Jetzt geht es aber zusätzlich um den Vortrieb, und dazu bräuchte ich einen Kalman, der zwei gleichsinnige, aber sehr verrauschte Quellen miteinander verrechnen soll:
a) ein Accelerometer in x-Richtung (vorwärts/rückwärts), 2x über die Zeit integriert ergibt dann Sensorwert 1: die Strecke aus Acc.
b) ein Quadratur-Encoder an beiden Antriebsketten, aus den Laufstrecken (arithm. Mittel li+re oder kleinster Wert, je nach IMU) erhält man Sensorwert 2: die Strecke aus Enc.


diese beiden Werte sollen nun per Kalman "fusioniert und geglättet" werden, damit man einen besseren, saubereren Streckenwert erhält.


die Kalman-Lib bräuchte also nur 2 "Eingänge" und 1 "Ausgang",
zusätzlich die Standardabweichungen (empirisch ermittelt) der Sensorwerte als Eingabeparameter.


Ob es so etwas gibt und ob das dann so einfach geht ?

[HaWe]

hat denn irgendwer "fertige" Extended-Kalman-Libs in C/C++ bereits erfolgreich selber genutzt ?

[HaWe]

kann das sein, dass die Nutzung von Kalman Filtern hier im Forum nicht so recht bekannt und verbreitet ist?
Falls doch - wie macht ihr es stattdessen?

[Holomino]

Ich nehme die Odometrie pur (und jage die ermittelte Position zusammen mit den Daten des Rundumsensors durch einen Partikelfilter).
Aus meinem Billig-Beschleunigungssensor von Pollin kam bei meinen Tests nix vernünftiges raus. Der Beschleunigungswert im Bereich von +-2g wurde vom Rauschen (Chassisvibrationen bei laufenden Motoren) verschluckt. Da half weder mitteln noch integrieren. Schien mir im Nachhinein auch der falsche Messbereich für ein rampenbeschleunigtes Fahrzeug mit Schrittgeschwindigkeit. Man will ja gerade sanft beschleunigen, um den Schlupf der Räder zu minimieren.

[HaWe]

ich kriege leider den Schlupf bei meinem Kettenantrieb niemals raus - auf Teppichboden oder Rasen kein Problem, auf Parkett aber nicht zu machen, selbst bei langsamem ramp-up rutscht immer irgendwo was. Der Accelerometer meines CMPS11 ist aber recht gut, wenn auch nicht rauschfrei. Partikelfilter scheidet aus, da die Raumgeometrie nicnt bekannt ist (SLAM-Robot), und daher keine externen Referenzpunkte existieren.
Was bleibt, ist also der Kalman, zumindest für Anfahren und bremsen, um den wird also kein Weg herumführen.

[Mxt]

Bekannt ist mir das schon, ich habe sogar noch mal in zwei Bücher geschaut, als die Frage hier erschienen ist.

Und dann habe ich geschwiegen, weil

a) keine eigene Erfahrung
b) bei der Mathematik, die ich da gesehen habe, müsste ich erst mal eine Weile in mich gehen
c) da steht fertig und im Zusammenhang mit dem Autor bedeutet das, die Antwort muss 100 % passen
d) da steht C/C++ und im Zusammenhang mit dem Autor bedeutet das nur C wird akzepiert. Alles was ich kenne ist aber in C++

Was ich dazu habe, sammle ich gerade ein wenig und hänge gleich noch mal die Liste an. Sonst meckert das Forum wieder, wenn ich zu lange brauche einen Beitrag zu schreiben.

- - - Aktualisiert - - -

So hier der Rest:

Die Bucherkenntnis war: Dafür braucht man eine gute Matrizenbibiliothek.

Also wäre die erste Wahl
http://eigen.tuxfamily.org/index.php?title=Main_Page
und dann mit Beispiel aus Büchern und Tutorials mal was probieren.

Wenn aus irgendwelchen Gründen Eigen nicht in Frage kommt, wäre das die Alternative
http://arma.sourceforge.net/
das wäre die zweite "Quasi Standard" Lib.

Auf einer Lib aufbauend haben sich ja schon einige Leute abgearbeitet
https://github.com/hmartiro/kalman-cpp
http://kalman.sourceforge.net/
https://github.com/vancegroup/eigen-kalman

Es gibt auch sowas hier, bei den Drohnenfliegern
http://diydrones.com/profiles/blogs/tinyekf-lightweight-c-c-extended-kalman-filter-for

Dann gibt es natürlich noch "fertigeres", bei den großen Robotik-Frameworks
http://www.orocos.org/bfl
http://wiki.ros.org/robot_pose_ekf
http://wiki.ros.org/robot_localization

[HaWe]

hallo,
danke für die Links, tatsächlich habe ich sie überwiegend auch schon gefunden.
Zu C++: das würde mich dann nicht stören, wenn man nicht selber in C++ programmieren muss, sondern nur die paar Werte als Parameter übergibt.
Ob das geht per
function [new_mean, new_var]=update(mean1,var1,mean2,var2)
oder per
kalman.var1=...
kalman.var2=...
kalman.mean1=...
kalman.mean2=...
mean=kalman.new_mean
var=kalman.new_var

oder mit Doppelpunkten dazwischen ist mir (fast) egal.

Aber ich möchte nicht den mehrdimensionalen Kalman selber entwickeln müssen, mit allem Matrizendrumunddran, daher suche ich eine bereits fertige Implementierung, die (aus eigener Erfahrung) funktioniert - aus Gründen der sicherlich benötigten Hilfestellung.
Deshalb also "was fertiges".

[schorsch_76]

Kalman Filter hab ich selbst schon einsetzt. Es ist nur so das du das System kennen musst das du Verbessern willst.

http://bilgin.esme.org/BitsAndBytes/KalmanFilterforDummies

Komplexität willst du nicht, deshalb vermute ich das der Kalmanfilter nichts für dich ist.



STEP 1 - Build a Model

It's the most important step. First of all, you must be sure that, Kalman filtering conditions fit to your problem.

[HaWe]

der Kalman mag so komplex sein wie er will, es geht um seine Implementierung als Lib (prinzipiell verstehe ich schon, was er in welchen schritten macht).

Ich liefere die Messwerte und ihre Standardabweichungen, den Rest an Matrizenoperationen muss der Kalman machen, um den neuen geschätzten Wert zu errechnen, darum geht es.
Es ist wie mir dem (eingebauten) Kalman des CMPS11, der ja i.P auch nur die Rohwerte aus dem 9D IMU bekommt, die geglätteten Werte rechnet er dann per implementierten Kalman selber aus, was nichts anderes ist - nur einmal in Hardware gegossen, das andere Mal als Software-Lib.
Weiter theoretisieren brauchen wir darüber nicht, denn das Problem ist ja klar -

ich liefere die Messwerte,
der Kalman rechnet damit und gibt den geglätteten Wert aus, so wie er es immer macht.

Und genau dafür suche ich eine Lib, die sich genau so benutzen lässt (ps, mit persönlicher Erfahrung).