Einfaches Kalman Filter

[Markus (Gast)]

Hier gibt es ein sehr schönes Beispiel für die Implementierung eines 
sehr einfachen Kalman-Filters:
https://www.youtube.com/watch?v=X9cC0o9viTo&index=4&list=PLX2gX-ftPVXU3oUFNATxGXY90AULiqnWT

Für den Schätzfehler "Eest" gibt es die Formel 3:
Eest=(1-KG)*EestOld

Irgendwie scheint mir die Formel aber falsch zu sein. Wenn der alte 
Schätzfehler gleich Null ist, ist der neue Schätzfehler auch immer Null. 
Das kann aber wohl kaum sein, denn eine neue Schätzung kann ja 
fehlerbehaftet sein. Wo liegt das Problem?

[Mark B. (markbrandis)]

Markus schrieb:
> Für den Schätzfehler "Eest" gibt es die Formel 3:
> Eest=(1-KG)*EestOld
>
> Irgendwie scheint mir die Formel aber falsch zu sein. Wenn der alte
> Schätzfehler gleich Null ist, ist der neue Schätzfehler auch immer Null.
> Das kann aber wohl kaum sein, denn eine neue Schätzung kann ja
> fehlerbehaftet sein. Wo liegt das Problem?

Dann wird der initiale Schätzfehler zum Zeitpunkt t=0 eben nicht bei 0 
liegen, gehe ich mal stark von aus. Und der Gain liegt auch nicht bei 
100%, also wird der ganze Term niemals 0.

[Matthias M (Gast)]

In seinem Beispiel sind die Fehler Eest & Emea die Kovarianz-Matrizen 
und nicht etwa Fehler im Sinne "Messung - Schätzung". Üblicherweise wird 
der Schätzfehler für Eest tatsächlich eher "hoch" initialisiert. Der 
Filter läuft sich dann ein. Ist er eingelaufen kann man bei LTI Systemen 
KG übernehmen und muss es nicht mehr berechnen. Das KG ist im Grunde die 
Fehlerrückführmatrix beim Luenberger-Beobachter, aber nach einem anderen 
Prinzip berechnet (uU ein optimales Prinzip) .

Eest besteht eigentlich aus

Eest = Eest_alt + Q

Emea ist die Angabe des Messrauschens "R".

(Q,R : Konvention gemäß seinem Video 7)

Im Falle des skalaren Beispiels vom Video:
Der Schätzfehler wäre 0, wenn kein Rauschen (perfekte Messung) und kein 
Systemrauschen (exaktes Modell, kein Eingang). Dann ergibt sich für KG = 
0 und auch der Term [MEA-EST_t-1] wäre immer 0. Wir hätten einen 
konstanten Wert (für das eingangslose 1D Modell).


Nächste Annahme: Sagen wir perfekte Messung, aber ein Eingang, den wir 
als Noise ansehen. Wir setzen den Schätzfehler auf einen hohen Wert und 
er läuft sich ein und hat dann eine bestimmte Gewichtung auf die 
Schätzung: und zwar genau KG=1, dh vertrau der Messung zu 100%.

Oder umgekehrt: perfektes Modell, kein Eingang oder aber perfekt 
bekannter Eingang... kurzum wir wissen alles. Messung ist schlecht => 
"unendliches" R :

dann folgt für KG=(Eest+Q)/(Eest+Q+R)=0

Macht Sinn, da wir der Messung nicht vertrauen. Wir schätzen unser 
Modell ja besser.

LG

[Markus (Gast)]

Erst mal Danke für eure Antworten.

 Mark Brandis (markbrandis)
>Dann wird der initiale Schätzfehler zum Zeitpunkt t=0 eben nicht bei 0
>liegen, gehe ich mal stark von aus.

Das habe ich mir auch gedacht. Wenn man den Schätzfehler am Anfang schon 
auf 0 setzt, verwandelt sich das ganze sowieso in einen einfachen 
Tiefpass. Das habe ich in einer Simulation ausprobiert und der Filter 
war dann erstaunlich langsam.

Matthias M (Gast)
>In seinem Beispiel sind die Fehler Eest & Emea die Kovarianz-Matrizen
>und nicht etwa Fehler im Sinne "Messung - Schätzung".

Uihh ... Deine Kommentar ist ganz schön aufwendig, den muss ich erst mal 
"verdauen". Ich werde noch mal versuchen eine Simulation zu machen.

[Markus (Gast)]

Die Simulation zeigt eine schlechte Reaktion des Filters auf die 
Sprungantwort. Ich hätte erwartet, dass "ErrorEstimated" beim Sprung 
auch größer wird und sich dann der Filter schnell anpassen kann. Statt 
dessen bleibt aber "ErrorEstimated" immer klein und hat sich nur am 
Anfang angepasst.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% create a step signal for test
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
signalLength          = 1000;
stepHeigth            = 1;
noiseStandadDeviation = 0.1;

n1=signalLength/2;
n2=signalLength-n1;
signal=zeros(1,n1);
signal=[signal ones(1,n2)*stepHeigth];
signal=signal+randn(1,signalLength)*noiseStandadDeviation; % add some noise

subplot(2,1,1);
plot(signal,'g');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% kalman filter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

measureError      = 0.1;
% initial values:
ErrorEstimated    = 1;
ErrorEstimatedOld = ErrorEstimated;
yOld = 0;

l=length(signal);

for n=1:l,
  
  KalmanGain        = ErrorEstimated / ( ErrorEstimated + measureError );
  y(n)              = yOld + KalmanGain * ( signal(n)- yOld );
  ErrorEstimated    = (1-KalmanGain) * ErrorEstimatedOld;
  
  ErrorEstimatedOld = ErrorEstimated;
  yOld              = y(n);
  
  ErrorEstimatedStore(n)=ErrorEstimated;  

end

hold on 
plot(y);

title("green: signal  blue: filter result");
grid on

subplot(2,1,2);
plot(ErrorEstimatedStore);
title("ErrorEstimated");
grid on

[Matthias M (Gast)]

Hab gerade kein Matlab oä zur Hand zum Testen. Folgende Punkte:

Für die Messung verwendest du Sigma = 0.1
Varianz = sigma^2 => measureError = 0.1 * 0.1


Bedenke:

Ich schrieb:
> Im Falle des skalaren Beispiels vom Video:
> Der Schätzfehler wäre 0, wenn kein Rauschen (perfekte Messung) und kein
> Systemrauschen (exaktes Modell, kein Eingang). Dann ergibt sich für KG =
> 0 und auch der Term [MEA-EST_t-1] wäre immer 0. Wir hätten einen
> konstanten Wert (für das eingangslose 1D Modell).

Es ist so langsam, da dein "Modell" im Grunde keine Änderung erwartet.
Wenn du die Q-Matrix hinzufügst, müsste die Anpassung schneller gehen:

ErrorEstimatedOld = ErrorEstimated + Q;

Probiere mal ein paar Werte für Q (zB.: 0.2) durch. ErrorEstimated 
kannst du am Anfang auch ruhig größer setzen.

LG

[Markus (Gast)]

>Hab gerade kein Matlab oä zur Hand zum Testen.
Trotzdem Danke für die Hinweise. Bei mir ist immer Standardmäßig das 
freie Octave installiert.

Ich habe es jetzt mal geändert.
Tatsächlich wird bei der Einführung von "Q" das Ganze zu einem 
Tiefpassfilter. Bei Q=0.2 ergibt sich fast keine Filterwirkung, deshalb 
musste ich es verkleinern.
Man sieht aber wieder, dass sich "ErrorEstimated" und "KalmanGain" nur 
am Anfang ändern, danach bleiben sie stabil und es ist tatsächlich ein 
einfaches Tiefpassfilter. Das Verhalten ist eigentlich kein Wunder, wenn 
man den Code analysiert: Das Signal beeinflußt "ErrorEstimated" 
überhaupt nicht.
Eigentlich will ich mit dem Filter ein sich eindimensional bewegendes 
Objekt verfolgen. Vielleicht sollte ich die Bewegungsgleichung s=s0+v*t 
ins Modell einbauen?

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% create a step signal for test
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
signalLength          = 1000;
stepHeigth            = 1;
noiseStandadDeviation = 0.1;

n1=signalLength/2;
n2=signalLength-n1;
signal=zeros(1,n1);
signal=[signal ones(1,n2)*stepHeigth];
signal=signal+randn(1,signalLength)*noiseStandadDeviation; % add some noise

subplot(3,1,1);
plot(signal,'g');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% kalman filter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

measureError      = 0.01;
Q                 = 0.00005;

% initial values:
ErrorEstimated    = 1;
ErrorEstimatedOld = ErrorEstimated;
yOld = 0;

l=length(signal);

for n=1:l,
  
  KalmanGain        = ErrorEstimated / ( ErrorEstimated + measureError );
  y(n)              = yOld + KalmanGain * ( signal(n)- yOld );
  ErrorEstimated    = (1-KalmanGain) * ErrorEstimatedOld;
  
  ErrorEstimatedOld = ErrorEstimated + Q;
  yOld              = y(n);
  
  ErrorEstimatedStore(n) = ErrorEstimated;  
  KalmanGainStore(n)     = KalmanGain;
end

hold on 
plot(y);

title("green: signal  blue: filter result");
grid on

subplot(3,1,2);
plot(ErrorEstimatedStore,'r');
grid on
title("ErrorEstimated");

subplot(3,1,3);
plot(KalmanGainStore);
title("KalmanGain");
grid on

[Matthias M (Gast)]

Markus schrieb:
> Man sieht aber wieder, dass sich "ErrorEstimated" und "KalmanGain" nur
> am Anfang ändern, danach bleiben sie stabil und es ist tatsächlich ein
> einfaches Tiefpassfilter.

Ja, ist normal wennn sich das System nicht ändert, dann bleibt KG 
konstant. Siehe:

Matthias M schrieb:
> Ist er eingelaufen kann man bei LTI Systemen
> KG übernehmen und muss es nicht mehr berechnen. Das KG ist im Grunde die
> Fehlerrückführmatrix beim Luenberger-Beobachter, aber nach einem anderen
> Prinzip berechnet (uU ein optimales Prinzip)


Schau mal da:

Beitrag "Re: Sensorfusion mit Kalman"

...das diese Matthiase immer mit Kalman Filter spielen...

[Dieter F. (Gast)]

Matthias M schrieb:
> Ja, ist normal wennn sich das System nicht ändert, dann bleibt KG
> konstant.

Ja, so funktioniert das beschriebene "einfache Kalman Filter". Dient nur 
dem "entrauschen".

Da gibt es noch x weitere Videos für komplexere Anwendungen ...

Nicht nur Matthiase beschäftigen sich damit :-)

[Stm M. (stmfresser)]

Markus schrieb:

> Man sieht aber wieder, dass sich "ErrorEstimated" und "KalmanGain" nur
> am Anfang ändern, danach bleiben sie stabil und es ist tatsächlich ein
> einfaches Tiefpassfilter.

Kalman Gain konvergiert schnell, wenn man die Kovarianzmatrix t->∞ 
berechnet. Das Ausgangsverhalten möge mit einem einfachen Tiefpassfilter 
ähneln, aber es ist völlig anderes Konstrukt,


> Eigentlich will ich mit dem Filter ein sich eindimensional bewegendes
> Objekt verfolgen. Vielleicht sollte ich die Bewegungsgleichung s=s0+v*t
> ins Modell einbauen?
Ohne Modell wird nix mit dem Kalman Filter

[Markus (Gast)]

Stm Mc (stmfresser)
>Kalman Gain konvergiert schnell, wenn man die Kovarianzmatrix t->∞
>berechnet. Das Ausgangsverhalten möge mit einem einfachen Tiefpassfilter
>ähneln, aber es ist völlig anderes Konstrukt,

Wenn man die Code-Zeile aus meinen Beispielen oben nimmt, welches ich 
dem Lernvideo nachgebaut habe

y(n) = yOld + KalmanGain * ( signal(n)- yOld );

ergibt sich bei KalmanGain=Konstant

y(n) = yOld + k * ( signal(n)- yOld );
     = yOld + k*signal(n) - k* yOld;

als Z-transformierte ( falls ich mich nicht verrechnet habe )

Y = Y*z⁻¹ + k * X - k*Y*z⁻¹ 
Y/X= 1/( 1 - (1-k)*z⁻¹)

was einem 1-poligen LowPass entspricht.

Bei deinem Beispiel wäre eine Sprung an der Stelle T=20 und ein anderer 
Sprung in der Frequenz interessant, um zu sehen, ob Dein Filter wirklich 
mehr ist als ein ein- oder zwei poliger IIR Filter.