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
'** DER CODE DARF NICHT KOMMERZIELL GENUTZT WERDEN !!! ** mach das raus!
'** DER CODE DARF NICHT KOMMERZIELL GENUTZT WERDEN !!! ** Ach nein? Ein paar Zeilen Code die im Grunde die Kalmangleichungen sind?
David P. schrieb: > '** DER CODE DARF NICHT KOMMERZIELL GENUTZT WERDEN !!! ** > > Ach nein? Ein paar Zeilen Code die im Grunde die Kalmangleichungen sind? Wenn wir:
1 | Function Kalman_filter(filter_wert As Long) As Long |
2 | Local Yt As Double |
3 | Local Kt As Double |
4 | Local Xt As Double |
5 | Local Pt As Double |
6 | Local Pt_update As Double |
7 | |
8 | Yt = Filter_wert |
9 | |
10 | Pt_update = Pt_prev + Q |
11 | |
12 | Kt = Pt_update + R |
13 | Kt = Pt_update / Kt |
14 | |
15 | Xt = Yt - Xt_prev |
16 | Xt = Kt * Xt |
17 | Xt = Xt_prev + Xt |
18 | |
19 | Pt = 1 - Kt |
20 | Pt = Pt * Pt_update |
21 | |
22 | Xt_prev = Xt |
23 | Pt_prev = Pt |
24 | |
25 | Kalman_filter = Xt |
26 | End Function |
ändern in:
1 | Function Filter_Kalman(filter_wert As Long) As Long |
2 | Local Ypsilont As Double |
3 | Local Kat As Double |
4 | Local iXt As Double |
5 | Local Pet As Double |
6 | Local Pet_update As Double |
7 | |
8 | Ypsilont = Filter_wert |
9 | |
10 | Pet_update = Pet_prev + Q |
11 | |
12 | Kat = Pet_update + R |
13 | Kat = Pet_update / Kat |
14 | |
15 | iXt = Ypsilont - iXt_prev |
16 | iXt = Kat * iXt |
17 | iXt = iXt_prev + iXt |
18 | |
19 | Pet = 1 - Kat |
20 | Pet = Pt * Pet_update |
21 | |
22 | ' hier noch ein paar Leerzeilen einfügen |
23 | |
24 | |
25 | |
26 | iXt_prev = iXt |
27 | Pet_prev = Pet |
28 | |
29 | Filter_Kalman = iXt |
30 | End Function |
fällt das später bei seiner Klage vor Gericht nicht mehr auf!
Ich habe hier ebenfalls einen Code geschrieben:
1 | int i=0; |
Code darf nicht kommerziell verwendet werden!
@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! :-(
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...
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
Also grafisch sieht das ganze so aus. Schwarz: vor dem Filter, rot: nach dem Filter.
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
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.
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
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/
Klaus schrieb: > Vielleicht wenn man zum Vergleich auch einen Tiefpass plottet? Kann ich machen, wenn jemand gerade einen fertigen Code zur Hand hat ?
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.
Na soetwas in der Art: Wert = (AlterWert + Messwert) / 2 AlterWert = Wert Und das über alle Messwerte MfG Klaus
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
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
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.
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
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 :-)
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.
1 | Summe = Summe - Wert + Messwert |
2 | 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.
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
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:
1 | Const Anzahl_filterwert = 3 |
2 | Dim Filterwert_count As Byte |
3 | Dim Filterwert_old(anzahl_filterwert) As Long |
4 | Declare Function Fir_filter(byval Filter_wert As Long) As Long |
5 | |
6 | Function Fir_filter(filter_wert As Long) As Long |
7 | Local X As Byte |
8 | |
9 | For X = 1 To Anzahl_filterwert |
10 | Filterwert_old(x) = Filterwert_old(x) + Filter_wert |
11 | Next |
12 | |
13 | Incr Filterwert_count |
14 | Fir_filter = Filterwert_old(filterwert_count) / Anzahl_filterwert |
15 | Filterwert_old(filterwert_count) = 0 |
16 | |
17 | If Filterwert_count = Anzahl_filterwert Then |
18 | Filterwert_count = 0 |
19 | End If |
20 | |
21 | End Function |
Code IIR Filter:
1 | Const X_para = 1 |
2 | Const Y_para = 1 |
3 | Dim Iir_filterwert_old As Long |
4 | Declare Function Iir_filter(filter_wert As Long) As Long |
5 | |
6 | Function Iir_filter(filter_wert As Long) As Long |
7 | Local X As Long |
8 | Local Y As Long |
9 | |
10 | X = X_para * Iir_filterwert_old |
11 | Y = Y_para * Filter_wert |
12 | X = X + Y |
13 | |
14 | Y = X_para + Y_para |
15 | |
16 | Iir_filter = X / Y |
17 | |
18 | Iir_filterwert_old = Filter_wert |
19 | |
20 | End Function |
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
:
Bearbeitet durch User
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 ...
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
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
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
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.
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.
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.
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 ?
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.
:
Bearbeitet durch User
Bitte melde dich an um einen Beitrag zu schreiben. Anmeldung ist kostenlos und dauert nur eine Minute.
Bestehender Account
Schon ein Account bei Google/GoogleMail? Keine Anmeldung erforderlich!
Mit Google-Account einloggen
Mit Google-Account einloggen
Noch kein Account? Hier anmelden.