BeschleunigungsVektor Auswerten

cl90

Hallo

Ich habe Beschleunigungsensordaten von meinem Smartphone vorliegen.
Die beeinhalten ja auch die Erdbeschleunigung. Und je nach dem Wie ich
das Smartphone neige, Bekomme ich dieses G anteilig auf X Y und Z
verteilt.

Das ist super praktisch um die Neigung des Phones zu ermitteln, aber wie
berechne ich die Neigung wenn das Smartphone auch noch Bewegt wird?
Kann man Erdbeschleunigung und sonstige Beschleunigung irgendwie
trennen? also mathematisch so, dass ich 2 Vektoren bekomme?

Gruß, Chris

SeppJ

Sofern du nicht weißt, wie die Bewegung genau aussieht: Nein.

C++ Forumbot

Dieser Thread wurde von Moderator/in SeppJ aus dem Forum C++ (auch C++0x und C++11) in das Forum Mathematik und Physik verschoben.

Im Zweifelsfall bitte auch folgende Hinweise beachten:
C/C++ Forum :: FAQ - Sonstiges :: Wohin mit meiner Frage?

Dieses Posting wurde automatisch erzeugt.

cl90

danke.
leider schade

cl90 schrieb:

Kann man Erdbeschleunigung und sonstige Beschleunigung irgendwie
trennen? also mathematisch so, dass ich 2 Vektoren bekomme?

wenn Du heraus bekommst, wie das geht, ist Dir der nächste Nobelpreis für Physik sicher

krümelkacker

cl90 schrieb:

Ich habe Beschleunigungsensordaten von meinem Smartphone vorliegen.
Die beeinhalten ja auch die Erdbeschleunigung. Und je nach dem Wie ich
das Smartphone neige, Bekomme ich dieses G anteilig auf X Y und Z
verteilt.

Das ist super praktisch um die Neigung des Phones zu ermitteln, aber wie
berechne ich die Neigung wenn das Smartphone auch noch Bewegt wird?
Kann man Erdbeschleunigung und sonstige Beschleunigung irgendwie
trennen? also mathematisch so, dass ich 2 Vektoren bekomme?

Hat Dein Handy auch einen 3-Achsen-Drehraten-Sensor? Wenn ja, dann ist das Problem unter gewissen Annahmen gut mit "Sensor Fusion" lösbar. Guck dir mal folgenden Vortrag an:

https://www.youtube.com/watch?v=C7JQ7Rpwn2k

Die Annahme ist, dass die mittleren g-Kräfte ohne Erzanziehungskomponente ganz nah bei 0 sind. Anhand der Drehratensensoren kannst du die Rotation der Erdanziehungskomponente im Beschleunigungssignal kompensieren. Du drehst also dein Beschleunigungssignal mit Hilfe der Information der Drehratensensoren so, dass die Erdanziehungskomponente immer in dieselbe Richtung zeigt (welche das auch ist, spielt keine große Rolle). Dabei kommt ein zweites Signal heraus, was Du dir als Summe eines konstanten Vektors (Erdanziehung) plus Schwankungen anderer Bewegungen vorstellen kannst. Die Schwankungen der anderen Bewegungen kannst du mit einem Tiefpass-Filter unterdrücken. Übrig bleibt die Erdanziehungskoponente. Das drehst du dann wieder zurück in das Koordinatensysten des Handys, damit du die die Gravitationskomponente im Handy-Koordinatensystem bekommst. Sie sagt dir, wo oben und unten relativ zum Handy ist.

Das mit dem Drehen geht dann so, dass du dir eine Rotationsmatrix merken musst, die du regelmäßig anhand der Drehratensensordaten updatest. Die kannst du für diesen Fall einfach am Anfang mit der Einheitsmatrix initialisieren; denn hinterher gehst du ja wieder zurück in das Handykoordinatensystem über die inverse Rotationsmatrix. Wie das andere Koordinatensystem orientiert ist, ist also gar nicht wichtig. Das brauchst du nur, weil du darin den Filter anwendest. Hauptsache, die Erdanziehungskomponente zeigt in dem anderen Koordinatensystem immer in dieselbe Richtung.

Der Loop sieht im Wesentlichen so aus:

struct sensor_data {
  vector3d accel; // Beschleunigung in m/s^2
  vector3d rrate; // Drehrate in rad/s
  float  delta_t; // Zeitschritt in s
};

matrix3d Rodrigues(vector3d const& a);

:::

matrix3d rotm = matrix3d::Identity();
filter lowpass = ...; // Tiefpass
sensor_data sd;
for (ever) {
   read_sensor_data_into(&sd);
   vector3d angle3d = sd.rrate * sd.delta_t;
   rotm = rotm * Rodrigues(angle3d);
   vector3d up = rotm.transpose() * lowpass(rotm * sd.accel);
   // up zeigt nach oben (also Richtung Himmel/Zimmerdecke)
   // im Koordinatensystem des Handys.
}

Rodrigues ist eine Funktion, die aus 3 Drehwinkeln im Bogenmaß eine Rotationsmatrix bastelt. lowpass ist ein Funktionsobjekt mit einem änderbaren Zustand, was für ein neues Sample eines 3D-Vektors ein gefiltertes rausspuckt.

Ein kleiner Haken ist noch, dass nach einiger Zeit die Rundungsfehler häufen und rot_matrix keine Rotationsmatrix mehr ist. Da muss man also abundzu mal (vielleicht alle 1000 Schritte mal) die Rundungsfehler mal wieder glattbügeln. Das kann man z.B. mit ein paar Kreuztprodukten und Normalisierungen machen, weil ja die Spalten einer Rotationsmatrix alle senkrecht aufeinander stehen müssen und auch jeweils die Länge 1 haben müssen. Mit Quaternionen statt 3x3 Rotationsmatritzen kann man das auch lösen. Da ist das "Normieren" jedenfalls einfacher. Und wahrscheinlich ist es dann auch ein bisschen schneller zu berechnen.

Die Vektor/Matrix-Syntax (mit Identity und transpose) ist hier von der Eigen-Bibliothek inspiriert.

Den Filter könnte man so basteln:

template<class T, class S=T>
struct biquad
{
  T t1, t2; // Zustand
  S a1, a2, b0, b1, b2; // Filter-Parameter (Biquad)

  T operator()(T in)
  {
    T t0  = in - a1 * t1 - a2 * t2;
    T out = t0 * b0 + t1 * b1 + t2 * b2;
    t2 = t1;
    t1 = t0;
    return out;
  }
}

typedef biquad<vector3d,float> filter;

Das ist ein Biquad-Filter, mit dem man einiges machen kann, u.a. auch hohe Frequenzen rausschmeißen. Du brauchst nur die richtigen Filterparameter dafür.

Dieser Filter hier lässt beispielsweise nur das untere 1% des Spektrums durch:

b0 =  0.00024135904904196;
b1 =  0.00048271809808392;
b2 =  0.00024135904904196;
a1 = -1.95557824031504;
a2 =  0.95654367651120

Die Grenzfrequenz hängt dann natürlich von der Abtastrate (also deinem delta_t ab). du findest bestimmt im Netz auch irgendwelche Applets, mit denen du so einen Filter nach deinen Wünschen bauen kannst. Man nennt diese Art Filter auch "IIR 2. Ordnung". Aber so wichtig mit der Grenzfrequenz ist das glaub'ich nicht, da die Beschleunigungen vom Hinundherwackeln mit konstantem Hub proportional zum Quadrat der Frequenz sind. Das sind also bei sehr langsamen Bewegungen extrem kleine Beschluenigungen im Vergleich zu schnelleren Bewegungen.

Als ich mich damit mal intensiver beschäftigt hatte, lernte ich folgendes:

Wo oben und unten ist, bekommt man prima aus den Sensordaten von Beschleunigungssensoren und Drehratensensoren raus
Für den einen fehlenden Freiheitsgrad (bzgl. absolute Orientierung) bräuchte man noch einen Kompass
Positionserfassung kann man vergessen. Theoretisch müsste man das Beschleunigungssignal, nachdem man die Erdanziehung rausgerechnet hat, 2mal integrieren. Aber da kommt nur Murks raus, weil man das alles gar nicht so genau hinkommt. Hast du einen kleinen Bias in der Orientierung, dann haut deine integrierte Position quadratisch mit der Zeit ab. Das geht echt schnell.
Was man da machen kann bzgl Position mit-tracken ist noch ZUPT, "Zero-Update". Wenn du dir dein Handy an einen Fuß schnürst, kann man noch die Schritte erkennen. Und es ist klar, dass ca 50% der Zeit beim Gehen, nämlich dann, wenn dein Fuß am Boden ist, die Geschwindigkeit 0 m/s ist. Das kann man ausnutzen, um damit den Geschwindigkeitsdrift, den man sonst bekommt, zu vermeiden. Dann kannst du Schritte separat integrieren und schätzen, wie groß die Schritte waren und in welche Richtung sie gingen. Das haben sie auch auf der diesjährigen CeBit gezeigt. Du bist mit so einem Sensor-Bluetooth-Dingen am Schuh und einem Tablet durch die Gegendgelaufen, und das Tablet konnte dir ohne GPS anzeigen, wo du bist.

cl90

Danke!!

krümelkacker

cl90 schrieb:

Danke!!

Bitte, gerne!

krümelkacker schrieb:

...

cl90

ok ich merk grade das das alles andere als leicht wird das auf dem PC zu realisieren...

Ich glaube das größte problem dabei ist das ich mir mommentan Sensor-Datanpäckchen über Wifi von Smartphone zu PC schicken lasse. Und zumindest habe ich das Gefühl das im Gyro sprünge sind...

Ich lerne, zwar gerne dazu, aber Ich muss das Rad nicht neu erfinden.
Gibt es vlt. schon eine App die Sensorfusion macht und die ergebnisse dann per WiFi loschickt?

cl90

Ich werde mich jetzt doch daran versuchen, da es offensichtlich nirgends eine fertige Sensorfusion zu holen gibt..
Ist ein bisschen ärgerlich, da das hier nur ein Bruchteil eines Projektes ist das Sich um einen Roboter dreht.

Ok. Ich habe eine Frage zur Rodrigues Matrix. Ich habe ein problem das ich mit DirectX9 arbeite und es diese matrix nicht gibt.
Ich habe:
D3DXMatrixRotationAxis
D3DXMatrixRotationX
D3DXMatrixRotationY
D3DXMatrixRotationYawPitchRoll
D3DXMatrixRotationZ

Aber ich glaube der Ansatz zur Konstruktion einer Rodrigues: matRotX * matRotY * matRotZ führt nicht zum gleichen ergebniss oder?

krümelkacker

Wenn du den Direct3D-Kram von Microsoft verwenden willst, dann kannst du es entweder per D3DXMatrixRotationAxis oder per D3DXQuaternionRotationAxis mit D3DXQuaternionMultiply und D3DXMatrixRotationQuaternion lösen. Dabei ist die Achse genau der angle3d-Vektor und der Drehwinkel (im Bogenmaß) die Länge dieses Vektors.

Ich empfehle dir die Quaternionen. Denk dir einfach normalisierte Quaternionen als komprimierte 3D-Rotationsmatritzen (speichert nur 4 statt 9 Werte). Achte darauf, in welcher Reihenfolge die Funktion D3DXQuaternionMultiply ihre Parameter erwartet. Das ist wichtig. Denn die Multiplikation auf Quaternionen ist genausowenig kommutativ wie die auf Matritzen. Aber beide Multiplikationen entsprechen der Hintereinanderausführung der einzelnen Rotationen. Die Funktion D3DXQuaternionMultiply will die erste Rotation zu erst haben, wobei in der Matrixschreibweise die erste Rotation bei "P * Q" das Q wäre.

Zwischendurch normalisierst du Dein Quaternion immer wieder mal -- so alle 1000 Iterationen einmal oder sowas; denn aufgrund der Rundungsfehler kann die Norm mit der Zeit von 1 abweichen. Aber nur normierte Quaternionen beschreiben Rotationen. Matritzen kann man auch "normieren", also so verändern, dass da eine orthogonale Matrix rauskommt. Aber das ist komplizierter. Deswegen die Empfehlung für Quaternionen.

Ggf. musst du für das Anwenden der Rotation auf den Beschleunigungsvektoren dann doch das Quaternion zu einer Matrix konvertieren.

cl90 schrieb:

Aber ich glaube der Ansatz zur Konstruktion einer Rodrigues: matRotX * matRotY * matRotZ führt nicht zum gleichen ergebniss oder?

Nein. Das passt nur näherungsweise für kleine Winkel. Du würdest dir damit die Genauigkeit bei schnellen Drehungen versauen.

cl90

sooo.

Ich melde mich dann mal wieder zu Wort
Es ist jetzt ein paar Klausuren und einige Tage programmieren später.

Es gab tatsächlich keine App die SensorFusion zur Verfügung stellt.
Also habe ich mich kurzerhand in AndoridDev eingelesen und eine eigene App geschrieben.

Ich lese die Sensordaten jetzt selbst aus, und filtere den Acc Sensor mit einem Tiefpass, den Gyro mit einem Hochpass und füge beide mit dem Magnetsensor in einem Kalmanfilter zusammen.
Zu guter letzt schick ich mir das ganze über Wifi zu meinem PC.
Erste Tests zeigen das das echt super funktioniert.
Rausch und drift freie Orientierungswinkel: Pitch, roll, yaw.

Die App ist leider noch nicht benutzerfreundlich genug um Sie hochzuladen.
IP, Port, Filterfaktoren etc sind alle nur fest im Code. Das "Interface" bietet nur einen Button über den ich das Senden ein oder ausschalten kann^^

Nochmals vielen Dank für deine Aufwendige Bescheibung.
Es hat mich nur anfangs erschlagen, was ich wieder alles tun muss...
Bei dem Projekt das ich mache, war es leider nicht das erste Mal, dass ich mich für ein kleines Feature in zig völlig andere Bereiche einlesen musste...

Naja. aber langsam wirds.
Der kleine Krabler(Hexapod) kann in der Simulation schon eine Menge