Quadrocopter | FlightControler | Das Kalmanfilter

[Marco (Gast)]

Hallo,
seit ca. 4Monate baue ich einen Quadrocopter. Als Sensor nutze ich einen 
MPU 9250. Die ausgelesene Daten fusioniere und Filter ich mit ein 
Kalmanfilter. Habe probiert das Kalmanfilter zu verstehen, jedoch 
schaffe ich es nicht und deswegen habe ich einen OpenSource Code 
verwendet.

Nun macht der Filter schon recht geile Werte und ich bin wirklich 
erstaunt darüber was er kann. Würde Ihn nun aber gerne noch optimieren, 
da der Winkel noch um 4° schwankt. Im Code kann man Ihn optimieren über

Q_angle_pitch
Q_bias_pitch
R_measure_pitch

jedoch verstehe ich nicht genau, was dahinter steckt. Kann mir an dieser 
Stelle jemand weiter helfen und mir eventuell sagen, wie ich die 3 Werte 
einstellen kann, was ich berechnen muss bzw was sie bedeuten.

#include "Kalman.h"
#include "mbed.h"

/*PITCH*/
/*PITCH*/
/*PITCH*/

void Kalman_pitch(void) 
{
    /* We will set the varibles like so, these can also be tuned by the user */
    Q_angle_pitch = 0.001;
    Q_bias_pitch = 0.003;
    R_measure_pitch = 0.03;
    
    bias_pitch = 0; // Reset bias
    // Since we assume tha the bias is 0 and we know the starting angle (use setAngle), 
    // the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
    P_pitch[0][0] = 0;
    P_pitch[0][1] = 0;
    P_pitch[1][0] = 0;
    P_pitch[1][1] = 0;
}
// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
double getPitch(double *newAngle_pitch, double *newRate_pitch, double *dt) 
{
    // Discrete Kalman filter time update equations - Time Update ("Predict")
    // Update xhat - Project the state ahead
    /* Step 1 */
    rate_pitch = (*newRate_pitch) - bias_pitch;
    angle_pitch += (*dt) * rate_pitch;
    
    // Update estimation error covariance - Project the error covariance ahead
    /* Step 2 */
    P_pitch[0][0] += (*dt) * ((*dt) * P_pitch[1][1] - P_pitch[0][1] - P_pitch[1][0] + Q_angle_pitch);
    P_pitch[0][1] -= (*dt) * P_pitch[1][1];
    P_pitch[1][0] -= (*dt) * P_pitch[1][1];
    P_pitch[1][1] += Q_bias_pitch * (*dt);
    
    // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
    // Calculate Kalman gain - Compute the Kalman gain
    /* Step 4 */
    S_pitch = P_pitch[0][0] + R_measure_pitch;
    /* Step 5 */
    K_pitch[0] = P_pitch[0][0] / S_pitch;
    K_pitch[1] = P_pitch[1][0] / S_pitch;
    
    // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    /* Step 3 */
    y_pitch = (*newAngle_pitch) - angle_pitch;
    /* Step 6 */
    angle_pitch += K_pitch[0] * y_pitch;
    bias_pitch += K_pitch[1] * y_pitch;
    
    // Calculate estimation error covariance - Update the error covariance
    /* Step 7 */
    P_pitch[0][0] -= K_pitch[0] * P_pitch[0][0];
    P_pitch[0][1] -= K_pitch[0] * P_pitch[0][1];
    P_pitch[1][0] -= K_pitch[1] * P_pitch[0][0];
    P_pitch[1][1] -= K_pitch[1] * P_pitch[0][1];
    
    return angle_pitch;
}

[Florian (Gast)]

Hallo Marco,

ein Kalman-Filter ist alles andere als einfach zu verstehen und in 
Betrieb zu nehmen. Vor allem, weil Du Dir nie sicher sein kannst ob er 
sich nicht doch aufhängt. Das kannst Du nur über viele Simulationen 
sicher stellen.
Für Dein Projekt würde ich Dir dringend abraten diese Methode 
einzusetzen.
Man kann die Sensoren auch einfacher und vor allem nachvollziehbarer 
fusionieren

Ev. kannst Du mal im Projekt "autoquad.org" source code durchsehen, dort 
wird ebenfalls ein Kalman-Filter verwendet. Dies führt zu sehr sehr 
guten Flugeigenschafften. Leider ist das Projekt aber "verdampft".

Gruß
Florian

[Marco (Gast)]

Hey Florian,
Das ich den Filter nicht verstehen werde, ist mir wie oben erwähnt 
völlig bewusst.

Meine Rohdaten schwanken um 40° und bis jetzt macht das Kalman Filter 
gute Arbeit und ich erhalte Schwankungen von 2°, denke aber das man es 
noch optimieren kann, mit den drei Werten  Q_angle_pitch, Q_bias_pitch 
und R_measure_pitch. Deswegen meine Frage, ob mir jmd sagen kann, was 
die Werte aussagen bzw wie ich den optimalen Wert finden kann.

autoquad.org nutzt einen fertigen FlightControler, da es mir um die 
Entwicklung eines eigenen geht, bringt mir die Seite nicht so viel. 
Zumindest was ich auf den ersten Blick dort sehe.

Danke Dir trotzdem für deine Antwort.

Gruß
Marco

[Marco (Gast)]

Ist der Kalmanfilter echt so kompliziert, dass keiner hier Ahnung davon 
hat?

[flips (Gast)]

Warum schwanken denn die Werte so extrem? Ich habe meinen Quadro damals 
komplett ohne Filter realisiert und lediglich ganz simple PID-Regler und 
einfachste Fusionierung realisiert und er steht wie eine 1 in der Luft.

Mein Fazit war damals sowas ist völlig unnötig. :-)

[Sinus T. (micha_micha)]

Das kennst du?
http://www.cbcity.de/das-kalman-filter-einfach-erklaert-teil-1

[Vincent H. (vinci)]

Marco schrieb:
> Ist der Kalmanfilter echt so kompliziert, dass keiner hier Ahnung davon
> hat?

Du hast 8! (in Worten: ACHT) Sätze Einleitung geschrieben und am Ende 
des ersten Absatzes selbst geschrieben, dass du das Filter nicht 
verstehst. Die Methoden um die Kovarianzmatrizen Q und R zu ermitteln 
sind (meist) derart komplex, dass man in Relation das Grundverständnis 
des Filters als lineare Algebra betrachten kann... Da reichen kaum 8 
Semester Uni...

Weißt du überhaupt was Q und R so grob aussagen? Die hängen stark 
vereinfacht ausgedrückt von der Güte deiner Sensoren ab. Bevor du nicht 
gewillt bist ein Gyroskop in der Preisklasse eines Mittelklasse-PKWs zu 
kaufen, brauchst du dir über die Optimierung von Q und R abseits von 
klassischem Ausprobieren nicht wirklich den Kopf zu zerbrechen.


flips schrieb:
> Mein Fazit war damals sowas ist völlig unnötig. :-)

Bei einem Hobby-Modell, dass mit einem 0.20€ 9-DOF Sensor ein bisserl 
herumfliegen soll... ja. Bei einem 320M$ U-Boot, dass ein halbes Jahr 
auf Tauchgang geht, nein.

[flips (Gast)]

> Bei einem 320M$ U-Boot, dass ein halbes Jahr
> auf Tauchgang geht, nein.

ich hoffe doch, dass der Quadro nicht auf Tauchgang geht. Von 
nukleargetriebenen Ubooten mit Atomraketen-Bewaffnung war nie die Rede

Aber ansonsten hast du natürlich recht. Mein Hinweis an den TO war 
lediglich auf dem Verdacht gegründet, dass etwas anderes nicht stimmt. 
Ev. ein Hardwarefehler oder ein mechanisches Problem...

:-))

[Dieter F. (Gast)]

Marco schrieb:
> Ist der Kalmanfilter echt so kompliziert, dass keiner hier Ahnung davon
> hat?

Wenn Du des englischen mächtig bist und etwas Zeit aufwenden möchtest

https://www.youtube.com/watch?v=CaCcOwJPytQ&list=PLX2gX-ftPVXU3oUFNATxGXY90AULiqnWT&index=1

Da ist er erschöpfend und wirklich gut erklärt.

[Chris J. (Gast)]

Hallo,

falls Du gewillst bist dich in die Theorie der Sensor Verknüpfung 
einzuarbeiten kann ich Dir begleitend den Source Code des Ardupilot 
Flightstack empfehlen, den ich selbst für meinen Copter benutze. Er 
fliegt mit einer Pixhawk, die einen F429 ARM Cortex enthält, der auch 
jede Menge Kalman Filter als SW aufgespielt bekommt. Die verknüpfen 
Gyros, Luftdruck, GPS und Kompass miteinander.

Nur so als Tip: In solchen Flugsteuerungen stecken hunderte Mannjahre 
ganzer Entwicklerteams. Das kriegst du nicht mal eben allein hin.

Gruss,
Chris

[Wolfgang (Gast)]

Chris J. schrieb:
> Die verknüpfen Gyros, Luftdruck, GPS und Kompass miteinander.

Gyros und Luftdruck verknüpfen die bestimmt nicht miteinander - was soll 
das bringen.

Der barometrische Luftdruck wird mit der GPS-Höhe zu einer sauberen 
Flughöhe fusioniert, damit bei örtlichen Luftdruckänderungen die Höhe 
nicht weg läuft, aber man trotzdem nicht an den aberwitzigen 
Höhenschwankungen des GPS dran hängt. Durch die Fusion von Kompass und 
Gyro wird die Gyrodrifterei eliminiert. So ein Hexenwerk ist das nun 
auch wieder nicht.

[Conny G. (conny_g)]

Marco schrieb:
> Hey Florian,
> Das ich den Filter nicht verstehen werde, ist mir wie oben erwähnt
> völlig bewusst.
>
> Meine Rohdaten schwanken um 40° und bis jetzt macht das Kalman Filter
> gute Arbeit und ich erhalte Schwankungen von 2°, denke aber das man es
> noch optimieren kann, mit den drei Werten  Q_angle_pitch, Q_bias_pitch
> und R_measure_pitch. Deswegen meine Frage, ob mir jmd sagen kann, was
> die Werte aussagen bzw wie ich den optimalen Wert finden kann.
>
> autoquad.org nutzt einen fertigen FlightControler, da es mir um die
> Entwicklung eines eigenen geht, bringt mir die Seite nicht so viel.
> Zumindest was ich auf den ersten Blick dort sehe.
>
> Danke Dir trotzdem für deine Antwort.
>
> Gruß
> Marco

Hi Marco,

ich habe selbst noch nicht mit dem Kalman gearbeitet, mich aber kürzlich 
mal ein bisschen eingelesen.
Wenn ich das richtig verstehe, dann ist die P_Pitch Matrix der Kalman 
Gain und liegt als Vektorlänge zwischen 0 und 1. Wobei der Gain sagt 
wieviel von neuen Messwerten in die neue Schätzung eingeht, 1 heißt: die 
Werte werden als zuverlässig eingestuft, die neue Schätzung geht schnell 
/ sofort den neuen Messwerten nach. 0 oder sehr klein das Gegenteil, die 
neue Schätzung wird schwach von den neuen Messwerten beeinflusst.
Nun kann Deine Abweichung von 2 Dingen kommen: die Messwerte sind sehr 
ungenau oder der Kalman reagiert zu träge.
Da die Werte Q_angle_pitch und Q_bias_pitch sehr niedrig sind kannst Du 
einfach mal versuchen Sie um den Faktor 10 auf 0.01 zu erhöhen.
Dann reagiert der Filter sehr viel schneller.
Im Prinzip macht der Filter einen Kompromiss aus Genauigkeit und 
Trägheit - man kann Messfehler kompensieren, aber es kostet Zeit, weil 
ein niedriger Gain neue Werte nur zögerlich durchlässt.
Wenn wiederum der Gain zu hoch wird dann kommen die Messfehler durch.

Viele Grüße,
Conny