Gyroskop Drift schätzen mit Kalman

Marcel Gast #3720617
Lesenswert?
• ▲
▼ 
Hi,

an diversen Stellen im Netz findet man das gleiche Modell für ein 
Kalmanfilter, das den Drift des Gyros schätzen soll (zb hier 
http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/).

Ich habe das Filter in matlab nachgebaut und "drehe" mein virtuelles 
Objekt. Dabei wird zwar der Winkel ganz gut geschätzt, jedoch liegt die 
Schätzung des Drifts meilenweit daneben.

Ich habe im Code den Drift konstant auf 0.01 gesetzt, das KF kann jedoch 
den Drift und die Winkelgeschwindigkeit nicht auseinanderhalten.

Habe ich beim Abtippen irgend etwas übersehen? Oder ist das Modell 
grundsätzlich falsch?

Weiterhin ist mir noch nicht ganz klar, wie ich das Prozessrauschen des 
Drifts modelliere. Was kommt richtigerweise bei q22 rein und warum? 
Bisher habe ich das so verstanden, dass dort die Unsicherheit des 
Modells reinkommt. Beim vorhandenen Modell gibt es aber keine Gleichung 
für den Drift, daher bin ich etwas ratlos.
Angehängte Dateien:
Antwort Zitieren Bearbeiten Thread verschieben Thread sperren Anmeldepflicht aktivieren Anpinnen Thread löschen Thread mit anderem zusammenführen Beitrag melden
Detlef _. #3726041
Lesenswert?
• ▲
▼ 
Hallo Marcel,

die Schätzung der Gyrodrift klappt schon, siehe anhängendes Bild. Dein 
Modell paßte nicht zu der Berechnung der Ausgangsgröße. Ich habe Dir nen 
File angehängt, das funzt, vergleich das mit Deinen Berechnungen. Dein 
Modell ist nen Integrierer, der ist instabil, das mag Kalman oft nicht 
so gerne.

math rulez!
Cheers
Detlef
Angehängte Dateien:
Antwort Zitieren Bearbeiten Löschen Beitrag melden
Marcel Gast #3778810
Lesenswert?
• ▲
▼ 
Ach, ich hatte noch ein dt vergessen an die Gyro-Werte heran zu 
multiplizieren. Jetzt sieht es gut aus.

Noch ne prinzipielle Frage: Ich finde verschiedene Kalmangleichungen, 
worin bestehen die Unterschiede?

A:

K = A*P*C'*inv(S);
P = A*P*A' + Q - A*P*C'*inv(R)*C*P*A';

B:

K = P*C'*inv(S);
P = A*P*A' + Q;


Und was hat es bei Matrizenrechnung allgemein mit A*B*A' auf sich? So 
etwas sieht man ziemlich oft, dass eine Matrix B von links mit A und von 
rechts mit A' multipliziert wird. (Das Hochkomma ist die 
Matlabschreibweise der Transponierten)
Antwort Zitieren Bearbeiten Löschen Beitrag melden
Detlef _. #3778995
Lesenswert?
• ▲
▼ 
Hi,

>>Ich finde verschiedene Kalmangleichungen, worin bestehen die Unterschiede?

Ich hatte in Deinem Code rumgefuscht, weiß nicht mehr genau, was wie und 
wo klemmte, kann das nicht mehr nachvollziehen. Unten hab ich funzenden 
Kalman Code angehängt, den hatte ich in
Beitrag "Amplitude und Phase einer festen Frequenz bestimmen."
mal gepostet. Der orientiert sich an 'Anwendungen der Kalman Filter 
Technik, Schrick Herausgeber, Oldenburg Verlag 1977'

>>Und was hat es bei Matrizenrechnung allgemein mit A*B*A' auf sich?
Das heißt bei Mathematikern 'quadratische Form' afaik. Auf jeden Fall 
macht Dir A*B*A' eine Matrix aus (gemischt-)quadratischen Polynomen der 
Elemente von A mit den Elementen von B als Koeffizienten. Sowas taucht 
immer dann auf, wenn man einen Fehler minimieren will z.B. bei Kalman.

hth
Cheers
Detlef

for k = 1:n,
 PS = A*P*A'+Q;
 K = PS*C'*inv(C*PS*C'+R);
 P = (eye(2)-K*C);
 P = P*PS*P'+K*R*K';
 xds=A*xd;
 xd = xds+K*(y(k) -C*xds);
end;
Antwort Zitieren Bearbeiten Löschen Beitrag melden
Thread beobachten

Antwort schreiben

Bitte melde dich an, um einen Beitrag zu schreiben.

Bestehender Account

oder

Mit Google-Account einloggen

Noch kein Account?

Die Registrierung ist kostenlos und dauert nur eine Minute.

Jetzt registrieren