Kalman für Anfänger

[Ritchie]

Hallo Zusammen,
derzeit versuche ich mich an einem kalman Filter.
Ich beginne mit einer einfachen Implementation und habe
mir aus diesem Grunde eine kleine Excel Tabelle erstellt,
wo ich die entsprechenden Werte eingeben kann.

Hierbei will ich zuerst meine Temperatursensoren KTY81-110 von
der "simplen" Mittelwertberechnung auf einen Kalman Filter umsetzen.
Mit Floating Point Mathematik scheint das laut Tabelle schon ganz
gut zu klappen. Die genauen Parameter habe ich noch nicht gefunden (Optimiert).
Nur soll die Routine im Atmega8 nicht auf Float Mathematik arbeiten,
sondern auf Fix Point Mathematik. Und hier habe ich meine ersten Problem.
Meine Routine arbeitet nicht so wie ich es gerne hätte.

Floating Point Mathe


Public Sub CalculateSheet2Formular()
Dim nNoise As Double
Dim nEstimation As Double
Dim nLastEstimation As Double
Dim nPresition As Double
Dim nLastPresition As Double
Dim nKalmanKoeffizent As Double
Dim nValue As Double
Dim nADC As Long
Dim nTemperatur As Long
Dim objWorkbork As Workbook
Dim i As Integer
Set objWorkbork = ThisWorkbook
nNoise = 0.2
nLastEstimation = 0
nPresition = 1
For i = 2 To 22

nADC = CDbl(objWorkbork.Sheets(2).Cells(i, 1).Value)
nADC = nADC * 64 ' (<< 7) in ASM
nTemperatur = 82 + nADC
nTemperatur = nTemperatur / 101
nTemperatur = nTemperatur - 150

objWorkbork.Sheets(2).Cells(i, 2).Value = nTemperatur
nValue = nTemperatur
nLastPresition = nPresition
nKalmanKoeffizent = nPresition / (nPresition + nNoise)

nEstimation = nLastEstimation + nKalmanKoeffizent * (nValue - nLastEstimation)
nPresition = (1 - nKalmanKoeffizent) * nPresition
nLastEstimation = nEstimation

objWorkbork.Sheets(2).Cells(i, 3).Value = nKalmanKoeffizent
objWorkbork.Sheets(2).Cells(i, 4).Value = nPresition

objWorkbork.Sheets(2).Cells(i, 5).Value = i - 1
objWorkbork.Sheets(2).Cells(i, 6).Value = nEstimation
Next i

End Sub


Fix Point Mathe (Versuch der schief ging)


Public Sub CalculateSheet3Formular()
Dim nNoise As Long
Dim nEstimation As Long
Dim nLastEstimation As Long
Dim nPresition As Long
Dim nLastPresition As Long
Dim nKalmanKoeffizent As Long
Dim nValue As Long
Dim nADC As Long
Dim nTemperatur As Long
Dim objWorkbork As Workbook
Dim i As Integer
Set objWorkbork = ThisWorkbook
nNoise = 2
nLastEstimation = 10
nPresition = 16
For i = 2 To 22

nADC = CDbl(objWorkbork.Sheets(3).Cells(i, 1).Value)
nADC = nADC * 64 ' (<< 7) in ASM
nTemperatur = 82 + nADC
nTemperatur = nTemperatur / 101
nTemperatur = nTemperatur - 150

objWorkbork.Sheets(3).Cells(i, 2).Value = nTemperatur
nValue = nTemperatur
nLastPresition = nPresition
nKalmanKoeffizent = nPresition * 100 / (nPresition + nNoise)

nEstimation = nLastEstimation + (nKalmanKoeffizent * (nValue - nLastEstimation) / 100)
nPresition = (100 - nKalmanKoeffizent) * nPresition
nLastEstimation = nEstimation

objWorkbork.Sheets(3).Cells(i, 3).Value = nKalmanKoeffizent
objWorkbork.Sheets(3).Cells(i, 4).Value = nPresition

objWorkbork.Sheets(3).Cells(i, 5).Value = i - 1
objWorkbork.Sheets(3).Cells(i, 6).Value = nEstimation
Next i

End Sub

Hat jemand eine Hilfestellung ?
Wie ist generell der beste Ansatz eine Floating Point Mathe in Fix Point zu wandeln.
Ich dachte bis jetzt immer, da Ansatz wäre den Zahlenbereich nach oben zu schieben,
die Berechnungen durchzuführen und danach den Bereich wieder anpassen.
Viele Gruess
R.

[Sisor]

Das was du in Sheet 2 ausrechnest, kannst du im Sheet 3 ohne Fließkomma so ausrechnen.
(Beachte den Faktor 1024, um den jeder Wert zu groß ist.)


Public Sub CalculateSheet3Formular()
Dim nNoise As Long
Dim nEstimation As Long
Dim nLastEstimation As Long
Dim nPresition As Long
Dim nLastPresition As Long
Dim nKalmanKoeffizent As Long
Dim nADC As Long
Dim nTemperatur As Long
Dim objWorkbork As Workbook
Dim FAKTOR As Long
Dim i As Integer

Set objWorkbork = ThisWorkbook

FAKTOR = 1024 'am besten Potenz von 2 als Faktor (hier: 2^10)
nNoise = 2 * FAKTOR / 10 ' enspricht 0,2 * FAKTOR
nLastEstimation = 0
nPresition = 1 * FAKTOR

For i = 2 To 22

nADC = objWorkbork.Sheets(3).Cells(i, 1).Value
nADC = nADC * FAKTOR * 64

nTemperatur = 82 * FAKTOR + nADC
nTemperatur = nTemperatur / 101
nTemperatur = nTemperatur - (150 * FAKTOR)

objWorkbork.Sheets(3).Cells(i, 2).Value = nTemperatur

nLastPresition = nPresition
nKalmanKoeffizent = nPresition * FAKTOR / (nPresition + nNoise)

nEstimation = (nLastEstimation * FAKTOR + nKalmanKoeffizent * (nTemperatur - nLastEstimation)) / FAKTOR
nPresition = ((FAKTOR - nKalmanKoeffizent) * nPresition) / FAKTOR
nLastEstimation = nEstimation

objWorkbork.Sheets(3).Cells(i, 3).Value = nKalmanKoeffizent
objWorkbork.Sheets(3).Cells(i, 4).Value = nPresition

objWorkbork.Sheets(3).Cells(i, 5).Value = i - 1
objWorkbork.Sheets(3).Cells(i, 6).Value = nEstimation

' Zu Vergleichszwecken Tabelle mit rückgewandelten Werten:

objWorkbork.Sheets(3).Cells(i, 8).Value = CDbl(objWorkbork.Sheets(3).Cells(i, 3).Value) / FAKTOR
objWorkbork.Sheets(3).Cells(i, 9).Value = CDbl(objWorkbork.Sheets(3).Cells(i, 4).Value) / FAKTOR

objWorkbork.Sheets(3).Cells(i, 10).Value = CDbl(objWorkbork.Sheets(3).Cells(i, 5).Value) / FAKTOR
objWorkbork.Sheets(3).Cells(i, 11).Value = CDbl(objWorkbork.Sheets(3).Cells(i, 6).Value) / FAKTOR

Next i


End Sub

[Ritchie]

Hi,

jetzt verstehe ich meinen Fehler. Ich habe hier bei nicht den eigentlich Messwert nach "oben" verschoben und alle damit resultierende Werte/Vergleiche.
Daher hat der ganze Spass dann nicht funktioniert. Auch der Trick ein Vielfaches von Zwei zu nehmen, was mir später das umrechen in C das Leben
einfacher macht ( << -Operator), ist ein guter Hinweis.

Gibt es eine Webseite, welches sowas nochmal genau beschreibt oder google-Stichwort.

Vielen Dank für die Hilfe.

R.

[HaWe]

hallo,
ist zwar OT - aber ich suche einen Kalman für meinen Arduino Mega (ggf Due),
und zwar um mehrere Sensoren zu fusionieren (einen nach dem anderen ntl...).
Mit Excel kenne ich mich nun gar nicht aus -
kennst du sowas wie eine Anleitung "Kalman für Dummies" , die man einfach in Sketch schrittweise umsetzen kann ?

[Ritchie]

Hallo,

die oben gezeigte Funktion ist ein einfacher Kalman Filter. Was Sketch angeht, bin ich Ahnungslos. Ich schreibe in C/C++.

Vielleicht hilft ja diese Seite:

http://bilgin.esme.org/BitsBytes/KalmanFilterforDummies.aspx

Das Beispiel am Ende habe ich in der Excel-Tabelle auf dem ersten Sheet dargestellt.

Hier musst Du Dir nur die Macros ansehen, siehe auch Thread.

Gruss R.

[HaWe]

hey, danke!
ja, das ist wirklich gut!

Sketch ist übrigens C/C++, ein Stück einfacher sogar in der Handhabung was die Klassen- und Linker-Geschichten angeht (geht automatisch).

In den Dummie-Artikel werd ich mich mal gleich einlesen - danke nochmal! 8-)

[Ritchie]

Hi,

auf meiner Suche nach einem weiteren Erklärung habe ich diese Seite gefunden.

http://www.instructables.com/id/Guide-to-gyro-and-accelerometer-with-Arduino-inclu/?lang=de

Ist vielleicht auch was für Dich, da das Beispiel in Sketch beschrieben ist.

Gruss R.

[HaWe]

danke, der Ursprungslink ist zwar sehr Sensor-bezogen, aber es sind ein paar wirklich gute weiterführende Links dabei.
Mach ich mich sofort dran, sobald ich den Dummie-Guide durch habe! 8-)