Kalman Filter in Bascom

[Michael L. (nightflyer88)]

Hallo zusammen

Ich habe mal einen Beispiel Code für einen Kalman Filter angehängt. Den 
Code habe ich aus einem C-Code übersetzt, leider habe ich den link dazu 
nicht mehr. Ist jedoch egal, funktionieren tuts soweit einwandfrei.

Den Filter benutze ich für ein Vario um den Wert des Luftdrucksensors zu 
filtern.


Bei Fragen, Anregungen, oder Verbesserungsvorschlägen meldet euch.

Gruss Michael

[Marc H. (marchorby)]

'** DER CODE DARF NICHT KOMMERZIELL GENUTZT WERDEN !!! **

mach das raus!

[David P. (chavotronic)]

'** DER CODE DARF NICHT KOMMERZIELL GENUTZT WERDEN !!! **

Ach nein? Ein paar Zeilen Code die im Grunde die Kalmangleichungen sind?

[Marc H. (marchorby)]

David P. schrieb:
> '** DER CODE DARF NICHT KOMMERZIELL GENUTZT WERDEN !!! **
>
> Ach nein? Ein paar Zeilen Code die im Grunde die Kalmangleichungen sind?

Wenn wir:

Function Kalman_filter(filter_wert As Long) As Long
  Local Yt As Double
  Local Kt As Double
  Local Xt As Double
  Local Pt As Double
  Local Pt_update As Double

  Yt = Filter_wert

  Pt_update = Pt_prev + Q

  Kt = Pt_update + R
  Kt = Pt_update / Kt

  Xt = Yt - Xt_prev
  Xt = Kt * Xt
  Xt = Xt_prev + Xt

  Pt = 1 - Kt
  Pt = Pt * Pt_update

  Xt_prev = Xt
  Pt_prev = Pt

  Kalman_filter = Xt
End Function

ändern in:

Function Filter_Kalman(filter_wert As Long) As Long
  Local Ypsilont As Double
  Local Kat As Double
  Local iXt As Double
  Local Pet As Double
  Local Pet_update As Double

  Ypsilont = Filter_wert

  Pet_update = Pet_prev + Q

  Kat = Pet_update + R
  Kat = Pet_update / Kat

  iXt = Ypsilont - iXt_prev
  iXt = Kat * iXt
  iXt = iXt_prev + iXt

  Pet = 1 - Kat
  Pet = Pt * Pet_update

' hier noch ein paar Leerzeilen einfügen



  iXt_prev = iXt
  Pet_prev = Pet

  Filter_Kalman = iXt
End Function

fällt das später bei seiner Klage vor Gericht nicht mehr auf!

[Herbert (Gast)]

Ich habe hier ebenfalls einen Code geschrieben:

int i=0;

Code darf nicht kommerziell verwendet werden!

[Kraftverkehr Venedig (Gast)]

@Michael
Laß Dich nicht verdrießen. Es ist hier Gang und Gäbe, die Leute, die 
Programme zur Verfügung stellen, zum Dank zu demontieren. Es ist eine 
Unsitte, es ist eine Frechheit, es ist teilweise auch die pure Bosheit 
und:
Es ist jammerschade!

Warum? Weil immer weniger Leute die Lust verspüren, sich für eine gute 
Tat auch noch rechtfertigen zu müssen.

Drauf gespuckt!
:-(

[Michael L. (nightflyer88)]

Ist ja wirklich jammerschade hier etwas für alle bereit zu stellen... 
:-(


Möchte jemand den Code 1:1 in sein privates Projekt einbauen, darf er 
das natürlich ohne jemanden fragen zu müssen !! (Sonst würde ich den ja 
hier nicht veröffentlichen)

Möchte jemand den Code 1:1 in ein kommerzielles Projekt einbauen, dann 
bitte kurz ein Email an mich...

[Stm M. (stmfresser)]

Michael L. schrieb:
> Hallo zusammen
> Ich habe mal einen Beispiel Code für einen Kalman Filter angehängt. Den
> Code habe ich aus einem C-Code übersetzt, leider habe ich den link dazu
> nicht mehr. Ist jedoch egal, funktionieren tuts soweit einwandfrei.
> Den Filter benutze ich für ein Vario um den Wert des Luftdrucksensors zu
> filtern.
> Bei Fragen, Anregungen, oder Verbesserungsvorschlägen meldet euch.
> Gruss Michael

Wie schaut die Zustandsdifferenzengleichung deines beobachtenden Systems 
aus?
Welche Größen sind messbar bzw. beobachtbar und welche nicht?

Ohne die oben genannten Daten werden deine paar Zeilenquellcode von 
niemanden (miss)gebraucht. Wer möchte denn heute in bascom 
programmieren.

Wenn man Kalmanfilter bzw. solchen Zustandsschätzer verstehen will:

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

https://github.com/TKJElectronics/KalmanFilter/blob/master/Kalman.h

http://www.cbcity.de/das-kalman-filter-einfach-erklaert-teil-2

[Michael L. (nightflyer88)]

Also grafisch sieht das ganze so aus. Schwarz: vor dem Filter, rot: nach 
dem Filter.

[Klaus (Gast)]

Michael L. schrieb:
> Also grafisch sieht das ganze so aus. Schwarz: vor dem Filter, rot: nach
> dem Filter.

Für mich sieht das aus, wie ein normaler Tiefpass. Den kann man auch 
billiger haben.

MfG Klaus

[Wolfgang (Gast)]

Klaus schrieb:
> Für mich sieht das aus, wie ein normaler Tiefpass. Den kann man auch
> billiger haben.

Aber ein Tiefpass schätzt dir nicht die Genauigkeit der Werte und kann 
keine dynamischen Eigenschaften des Systems berücksichtigen. Ein 
Tiefpass merkt (mit Verzögerung), dass sich der Druck ändert, bei einem 
Kalman Filter hätte man die Möglichkeit die aktuelle 
Druckänderungsgeschwindigkeit mit zu schätzen und in den Vorhersagewert 
einfließen zu lassen. Dann läuft der Wert nicht hinterher, solange die 
Geschwindigkeit konstant ist.

Dafür muss das Kalman-Filter natürlich mehr als den Luftdruck schätzen.

[Klaus (Gast)]

Wolfgang schrieb:
> Aber ein Tiefpass schätzt dir nicht die Genauigkeit der Werte und kann
> keine dynamischen Eigenschaften des Systems berücksichtigen. Ein
> Tiefpass merkt (mit Verzögerung), dass sich der Druck ändert, bei einem
> Kalman Filter hätte man die Möglichkeit die aktuelle
> Druckänderungsgeschwindigkeit mit zu schätzen und in den Vorhersagewert
> einfließen zu lassen. Dann läuft der Wert nicht hinterher, solange die
> Geschwindigkeit konstant ist.

Schon klar, aber woran erkennt man das auf dem gezeigten Bild? 
Vielleicht wenn man zum Vergleich auch einen Tiefpass plottet?

MfG Klaus

[Wolfgang (Gast)]

Klaus schrieb:
> Schon klar, aber woran erkennt man das auf dem gezeigten Bild?

Gar nicht, weil der Code nur den Druck schätzt, aber nicht zusätzlich 
die Änderungsgeschwindigkeit mit einbezieht. In obiger Implementation 
ist es ein simpler Tiefpass, da hast du wohl Recht. Einziger Unterschied 
ist, dass nicht direkt die Zeitkonstante vorgegeben wird, sondern sich 
aus angenommenem Prozessrauschen (Q) und Sensorrauschen (R) ergibt.

Die zugehörigen Gleichungen sind Standard und z.B. hier zusammen mit 
einer C Implementierung zu finden:
http://interactive-matter.eu/blog/2009/12/18/filtering-sensor-data-with-a-kalman-filter/

[Michael L. (nightflyer88)]

Klaus schrieb:
> Vielleicht wenn man zum Vergleich auch einen Tiefpass plottet?

Kann ich machen, wenn jemand gerade einen fertigen Code zur Hand hat ?

[David P. (chavotronic)]

Michael L. schrieb:
> Kann ich machen, wenn jemand gerade einen fertigen Code zur Hand hat ?

Hab jetzt keinen Code zur Hand den man kommerziell nutzen dürfte.

[Klaus (Gast)]

Na soetwas in der Art:

Wert = (AlterWert + Messwert) / 2
AlterWert = Wert

Und das über alle Messwerte

MfG Klaus

[Hugo (Gast)]

Klaus schrieb:
> Na soetwas in der Art:
>
> Wert = (AlterWert + Messwert) / 2
> AlterWert = Wert
>
> Und das über alle Messwerte
>
> MfG Klaus

Oder auch kurz:

mittelwert = summe_aller_werte / anzahl_werte

[Klaus (Gast)]

Hugo schrieb:
> Klaus schrieb:
>> Na soetwas in der Art:
>>
>> Wert = (AlterWert + Messwert) / 2
>> AlterWert = Wert
>>
>> Und das über alle Messwerte
>>
>> MfG Klaus
>
> Oder auch kurz:
>
> mittelwert = summe_aller_werte / anzahl_werte

Das gibt nur einen Wert, mein Vorschlag liefert eine Funktion, die man 
Plotten kann. Das funktioniert auch über kontinuierliche Messwerte deren 
Anzahl man nicht kennt, wie ein Tiefpass eben.

MfG Klaus

[Hugo (Gast)]

Klaus schrieb:
> Das gibt nur einen Wert, mein Vorschlag liefert eine Funktion, die man
> Plotten kann. Das funktioniert auch über kontinuierliche Messwerte deren
> Anzahl man nicht kennt, wie ein Tiefpass eben.

Man muß doch nicht "alle Werte" wörtlich nehmen. Damit ist gemeint, daß 
man eine bestimmte Anzahl Werte nimmt, die addiert man und teilt durch 
diese Anzahl. Also z.B. (w1+w2+w3)/3, als nächstes (w2+w3+w4)/3, dann 
(w3+w4+w5)/3 und so weiter. Durch diese Fensterung bekommt man die 
gewünschte Kurve. Je mehr Werte man nimmt, desto glatter wird die Kurve, 
aber man bekommt ebenso eine zeitliche Verzögerung.

[Klaus (Gast)]

Hugo schrieb:
> Man muß doch nicht "alle Werte" wörtlich nehmen.

Ich meinte einen, nur einen. So etwas wie "mitteln über alten Mittelwert 
und neuen Messwert". Das kann ohne Buffer nur mit einem Speicher für den 
alten Mittelwert mit der Messung mitlaufen. Ein einfacher Tiefpass. Was 
du beschreibst ist IMHO ein FIR Filter.

MfG Klaus

[Hugo (Gast)]

Du hast das gleiche gemacht wie ich, nur mit zwei statt drei Werten. 
Mann nennt das (glaube ich) gleitenden Mittelwert.
Ich hab das nur in allgemeinerer Form geschrieben, damit man das auch 
über jeweils vier Werte oder fünf Werte usw. machen kann.
Du addierst zwei Werte und teilst durch zwei. Ich hab drei Werte addiert 
und durch drei geteilt. Also meinen wir schon das gleiche :-)

[Wolfgang (Gast)]

Hugo schrieb:
>> Wert = (AlterWert + Messwert) / 2
>> AlterWert = Wert

Wenn man AlterWert und Messwert unterschiedlich gewichtet, hat man auch 
eine Chance, die Grenzfrequenz zu ändern (Stichwort: IIR-Filter), also 
z.B.

Summe = Summe - Wert + Messwert
Wert  = Summe / n

Der Messwert geht dann mit 1/n in den neuen Mittelwert ein und bei n=2^k 
wird das auch noch µC-freundlich.

[Klaus (Gast)]

Hugo schrieb:
> Du hast das gleiche gemacht wie ich, nur mit zwei statt drei Werten.
> Mann nennt das (glaube ich) gleitenden Mittelwert.

Nicht wirklich. Ich verwende nur einen Wert (der ist errechnet) und den 
neuen Messwert. Daraus wird der neue Wert errechnet nach der allgemeinen 
Formel: (x * AlterWert + y * Messwert) / x+y . Für x = y = 1 ist das 
Beispiel oben. Der Messwert selbst wird verworfen und mit dem 
errechneten Werten bei der nächsten Messung weitergerechnet. Das ist 
dann ein Tiefpass, ein IIR Filter.

Ein gleitender Mittelwert ist ein FIR Filter und bezieht sich auf alte 
Messwerte. Es hat auch andere Eigenschaften und ist aufwändiger zu 
rechnen

Hier gibts den Frequenzgang dazu

Beitrag "Re: gleitender Mittelwert"

MfG Klaus

[Michael L. (nightflyer88)]

also habe mal einen Vergleich gemacht

schwarz: Rohwerte
rot: Kalman Filter
grün: FIR Filter (Mittelwert über 3 Messwerte)
blau: IIR Filter


Code FIR Filter:

Const Anzahl_filterwert = 3
Dim Filterwert_count As Byte
Dim Filterwert_old(anzahl_filterwert) As Long
Declare Function Fir_filter(byval Filter_wert As Long) As Long

Function Fir_filter(filter_wert As Long) As Long
  Local X As Byte

  For X = 1 To Anzahl_filterwert
    Filterwert_old(x) = Filterwert_old(x) + Filter_wert
  Next

  Incr Filterwert_count
  Fir_filter = Filterwert_old(filterwert_count) / Anzahl_filterwert
  Filterwert_old(filterwert_count) = 0

  If Filterwert_count = Anzahl_filterwert Then
    Filterwert_count = 0
  End If

End Function

Code IIR Filter:

Const X_para = 1
Const Y_para = 1
Dim Iir_filterwert_old As Long
Declare Function Iir_filter(filter_wert As Long) As Long

Function Iir_filter(filter_wert As Long) As Long
  Local X As Long
  Local Y As Long

  X = X_para * Iir_filterwert_old
  Y = Y_para * Filter_wert
  X = X + Y

  Y = X_para + Y_para

  Iir_filter = X / Y

  Iir_filterwert_old = Filter_wert

End Function

[Michael L. (nightflyer88)]

zoom..

[Michael L. (nightflyer88)]

Klaus schrieb:
> Formel: (x * AlterWert + y * Messwert) / x+y . Für x = y = 1

@Klaus
habe ich den "AlterWert" richtig implementiert ?

sollte wahrscheinlich so sein:
Iir_filterwert_old = Iir_filter

[Wolfgang (Gast)]

Michael L. schrieb:
> Const X_para = 1
> Const Y_para = 1

Das glättet noch kaum, wie man auch in der Vergleichsgraphik sieht.

Probier doch mal meine Formulierung des IIR-Filters mit n=16 oder 32.
Summe sollte 32 Bit haben und sinnvollerweise mit dem n-fachen vom 
ersten Messwert initialisiert werden. Startwert für Wert sollte 
ebenfalls der erste Messwert sein ;-)

siehe:
Wolfgang schrieb:
> Der Messwert geht dann mit 1/n in den neuen Mittelwert ein ...

[Klaus (Gast)]

Michael L. schrieb:
> Const X_para = 1
> Const Y_para = 1

Da du ja mit floats rechnest, kannst du auch X_para und Y_para so 
ansetzen, daß die Summe 1 ist, für dieses Beispiel jeweils 0.5. Dann 
fällt das Teilen weg.

Michael L. schrieb:
>   X = X_para * Iir_filterwert_old
>   Y = Y_para * Filter_wert
>   X = X + Y
>
>   Y = X_para + Y_para
>
>   Iir_filter = X / Y

Dann wird daraus:

Const X_para = 0.5
Const Y_para = 0.5

.
.
.

   X = X_para * Iir_filterwert_old
   Y = Y_para * Filter_wert
   Iir_filter = X + Y

So, und jetzt kannst du X_para und Y_para so einstellen, daß die 
Zeitkonstante der des Kalman entspricht. Wenn X_para größer wird, wird 
stärker geglättet.

MfG Klaus

[Martin (Gast)]

Klaus schrieb:
> Für mich sieht das aus, wie ein normaler Tiefpass. Den kann man auch
> billiger haben.

Ein eindimensionaler Kalman-Filter mit konstanter Kovarianz macht 
tatsächlich wenig Sinn. Die Fehler-Kovarianz und der Kalman-Gain 
konvergieren zu fixen Werten, die man vorab berechnen kann (Stichwort 
steady-state kalman). Im eindimensionalen Fall entspricht das dann einem 
einpoligen Tiefpass.

Wirklich lustig wird der Kalman, wenn ein (mehrdimensionales) 
Systemmodell dahintersteht und richtig gut, wenn die Kovarianzmatrix 
bekannt ist. Sehr gut funktioniert das in einer Motorsteuerung, wenn man 
Motorenstrom, Encoderwert und Streckendynamik kennt. Ich denke, dass 
meistens (so wird das auch in diversen Publikationen beschrieben) die 
Kovarianzdaten zuerst geschätzt und dann pi mal Daumen so gedreht, dass 
die Dynamik so aussieht wie gewünscht. Aber auch dann konvergiert bei 
fixer Kovarianzmatrix der Kalman-Gain zu einem fixen Wert und der ganze 
Tamtam ist für die Katz - man hätte auch grad einen Beobachter auslegen 
können. Ausnahme ist ein extended Kalman bei nichtlinearem System.

Wirklich gut ist ein Kalman-Filter wohl dann, wenn man eine 
veränderliche Kovarianzmatrix hat. Um das ursprüngliche Beispiel 
aufzugreifen: Wenn man zur Bestimmung einen Schätzwertes für die Höhe 
sowohl GPS-Daten als auch barometrische Höhendaten verwursten will, kann 
man die vom GPS angegebene Genauigkeit in die Kovarianzmatrix 
einfliessen lassen, so dass die Gewichtung von GPS und barometrischem 
Höhenmesser vom GPS-Empfang abhängt.

- Martin

[Michael L. (nightflyer88)]

Martin schrieb:
> Im eindimensionalen Fall entspricht das dann einem
> einpoligen Tiefpass.

Da hast du wirklich recht ;-)


Ich hatte vor dem Kalman mit diversen Filtern herumprobiert, jedoch war 
das Ergebnis nie befriedigend. Aber wie man nun im praktischen Beispiel 
sieht kommt man mit einem richtig eingestellten Tiefpass aufs gleiche... 
:-)

@Klaus
danke für deine mithilfe

[Michael L. (nightflyer88)]

Nach einigem herumprobieren mit dem Tiefpass und dem Kalman finde ich 
das man die beiden Filter trotzdem nicht 1:1 vergleichen kann. Bei 
meinem Anwendungsfall mit dem Luftdruck ist der Tiefpass sogar eher 
besser geeignet. Bei richtiger Einstellung kann man das Rauschen relativ 
tief halten, hat aber trotzdem eine schnelle Reaktion auf grosse 
Messwertänderungen.

Beim Kalman kann man das Rauschen auch sehr tief halten, und reagiert 
auch schnell auf grosse Messwertänderungen, der Nachteil je länger der 
Messwert gleich bleibt, desto träger reagiert der Filter dann auf grosse 
Änderungen.

Je nach Anwendungsfall ist demnach der Kalman oder der Tiefpass im 
Vorteil.

[Wolfgang (Gast)]

Michael L. schrieb:
> Beim Kalman kann man das Rauschen auch sehr tief halten, und reagiert
> auch schnell auf grosse Messwertänderungen, der Nachteil je länger der
> Messwert gleich bleibt, desto träger reagiert der Filter dann auf grosse
> Änderungen.

IMHO ist dann die Verstärkung (Kalman Gain) zu klein, i.e. das 
Prozessrauschen zu klein angesetzt.

[Martin (Gast)]

Michael L. schrieb:
> Beim Kalman kann man das Rauschen auch sehr tief halten, und reagiert
> auch schnell auf grosse Messwertänderungen, der Nachteil je länger der
> Messwert gleich bleibt, desto träger reagiert der Filter dann auf grosse
> Änderungen.

Ich denke, das hast Du etwas falsch verstanden. Der Kalman-Filter, wie 
Du ihn implementiert hast, ändert seine Zeitkonstante nicht abhängig vom 
Eingangswert. Er würde sie abhängig von der Fehlerkovarianz ändern, aber 
die hast Du fix codiert. Damit (wie ich schon oben schrieb) konvergiert 
der Kalman-Gain gegen einen fixen Wert. Du kannst einfach mal 
beobachten, welchen Wert Kt nach einiger Zeit annimmt und dann diesen 
fest einprogrammieren. Damit entfällt auch die Rechnerei um P, Q und R 
und Du bist beim Tiefpass angelangt...

Was Du vielleicht beobachtet hast, ist, dass der Kalman-Filter seine 
Zeitkonstante mit der Zeit ändert. Am Anfang hast Du eine andere wie 
später (wenn Kt fast seinen Endwert errreicht hat). Das hat aber mit den 
Eingangswerten nichts zu tun.

Wenn Du ein Variometer bauen willst, brauchst Du ja nicht die Höhe 
sondern deren Ableitung. Das kann ein Kalman-Filter auch sehr elegant 
erledigen, wenn Du ihm ein zweidimensionales Systemmodell (ein 
Integrator) vorgibst. Auch das läuft auf ein Tiefpassfilter raus, es 
wird Dir kaum bessere Werte geben als wenn Du die Höhenwerte 
differenzierst und nochmal filterst. Aber wenn Dich jetzt doch mal 
interessiert, wie die Dinger genau funktionieren, wäre das eine nette 
Übung. Ich fand diese 
http://www.cbcity.de/das-kalman-filter-einfach-erklaert-teil-1 Erklärung 
übrigens sehr hilfreich.

[Pandur S. (jetztnicht)]

Ein Kalman Filter ist ja eine Matrix. Der Code dazu heisst also : jetzt 
stampfen wir die Matrix durch. Das wissen liegt aber eher in der 
Bedeutung der Werte und die Dimensionierung der Koeffizienten. Nein ?

[Paul B. (paul_baumann)]

Oh D. schrieb:
> Nein ?
Doch! Ohhhhh!
https://www.youtube.com/watch?v=w4aLThuU008

MfG Paul

[Peter P. (peter_p781)]

Was will man mit einem Kalman Filter, wenn nur eine Variable gemessen 
wird?
Die Stärke des Kalman Filters besteht doch darin, inkrementelle und 
absolute Messwerte fusionieren zu können.. z.B. Gyroskop + 
Beschleunigungssensor zur Schätzung der Lage.