Hallo,
hat von euch schon mal jemand versucht einen Kalman Filter in Bascom zu schreiben?
Gruß
Harry
Druckbare Version
Hallo,
hat von euch schon mal jemand versucht einen Kalman Filter in Bascom zu schreiben?
Gruß
Harry
hab ich nicht, hast mich aber auf ne Idee gebracht ...
werd mir mal diesen:
http://www.techsystemsembedded.com/Kalman.html
Quellcode zur Brust nehmen ... schaut nicht schlecht aus.
Hallo Vitis,
meinst du du krigst das in Bascom zum laufen, das wäre KLASSE.
Gruß
Harry
der Filter 1. Ordnung ging recht easy:
Code:
'%************************************************************
'% Marko Hoerner, 2009
'%
'% Kalman Filter For 1st Order System Sensor Input
'% ported to Bascom AVR
'%
'%************************************************************
Dim Noisysen(128) As Integer ' Sensordata
Dim Z(128) As Byte
Dim Index(128) As Integer
Dim X_hat(128) As Integer
Dim X_hat_min(128) As Integer
Dim Tempsingle_1 As Single
Dim Tempsingle_2 As Single
Dim N As Byte
Dim N2 As Byte
Dim Pmin1 As Single
Dim P As Single
Dim K As Single
Dim Pmin As Single
Dim Tempbyte As Byte
'Adjust Length for length of dataarray
Const Length = 40
Const Q = 1
'Adjust R for Degree of Damping
Const R = 10
' randomized data for test
For N = 1 To Length
Tempbyte = Rnd(100)
Noisysen(n) = Tempbyte
Print Noisysen(n) ; " ";
Next
For N = 1 To Length
Z(n) = Noisysen(n)
Next
For N = 1 To Length
Index(n) = N
Next
Pmin1 = 0
Tempsingle_1 = Pmin1 + R
K = Pmin1 / Tempsingle_1
Tempsingle_1 = Z(1) - Noisysen(1)
Tempsingle_1 = K * Tempsingle_1
X_hat(1) = Tempsingle_1 + Noisysen(1)
P = 1 - K
P = P * Pmin1
Pmin = P + Q
X_hat_min(1) = X_hat(1)
For N = 2 To Length
K = Pmin + R
K = Pmin / K
N2 = N - 1
X_hat(n) = Z(n) - X_hat_min(n2)
X_hat(n) = K * X_hat(n)
X_hat(n) = X_hat_min(n2) + X_hat(n)
P = 1 - K
P = P * Pmin
Pmin = P + Q
X_hat_min(n) = X_hat(n)
Next
For N = 1 To Length
'Print Fusing(x_hat(n) , "#.#") ; " ";
Print X_hat(n) ; " ";
Next
Hallo Vitis,
wie kombinier ich das jetzt mir einem Gyro und ACC.
Gruß
Harry
oder als Subroutine:
einmal der headerfile
und die sub:Code:
$nocompile
'Adjust Length for length of dataarray
Const Kal_length = 70
Const Kal_q = 1
'Adjust R for Degree of Damping
Const Kal_r = 3
Dim Kal_noisysen(kal_length) As Integer ' Sensordata
Dim Kal_z(kal_length) As Byte
'Dim Index(70) As Integer
Dim Kal_x_hat(kal_length) As Integer
Dim Kal_x_hat_min(kal_length) As Integer
Declare Sub Kalman_filter()
Code:$nocompile
'%************************************************************
'% Marko Hoerner, 2009
'%
'% Kalman Filter For 1st Order System Sensor Input
'% ported to Bascom AVR
'%
'%************************************************************
Sub Kalman_filter()
Local Kal_tempsingle_1 As Single
Local Kal_tempsingle_2 As Single
Local Kal_n As Byte
Local Kal_n2 As Byte
Local Kal_pmin1 As Single
Local Kal_p As Single
Local Kal_k As Single
Local Kal_pmin As Single
Local Kal_tempbyte As Byte
For Kal_n = 1 To Kal_length
Kal_z(kal_n) = Kal_noisysen(kal_n)
Next
Kal_pmin1 = 0
Kal_tempsingle_1 = Kal_pmin1 + Kal_r
Kal_k = Kal_pmin1 / Kal_tempsingle_1
Kal_tempsingle_1 = Kal_z(1) - Kal_noisysen(1)
Kal_tempsingle_1 = Kal_k * Kal_tempsingle_1
Kal_x_hat(1) = Kal_tempsingle_1 + Kal_noisysen(1)
Kal_p = 1 - Kal_k
Kal_p = Kal_p * Kal_pmin1
Kal_pmin = Kal_p + Kal_q
Kal_x_hat_min(1) = Kal_x_hat(1)
For Kal_n = 2 To Kal_length
Kal_k = Kal_pmin + Kal_r
Kal_k = Kal_pmin / Kal_k
Kal_n2 = Kal_n - 1
Kal_x_hat(kal_n) = Kal_z(kal_n) - Kal_x_hat_min(kal_n2)
Kal_x_hat(kal_n) = Kal_k * Kal_x_hat(kal_n)
Kal_x_hat(kal_n) = Kal_x_hat_min(kal_n2) + Kal_x_hat(kal_n)
Kal_p = 1 - Kal_k
Kal_p = Kal_p * Kal_pmin
Kal_pmin = Kal_p + Kal_q
Kal_x_hat_min(kal_n) = Kal_x_hat(kal_n)
Next
End Sub
hoi,
ich denke das ding zu programmieren ist nicht so schwer. das hauptproblem beim kalman filter sehe ich eher im finden von geeigneten paametern.
mfg jeffrey
jeffrey, full ACK ... wie er das Ding nun
in sein Projekt integriert, also einfacher kann ichs nicht machen,
den Rest muss er halt per Try and Error austüfteln.
Die Aufgabe war ja nen Kalman-Filter, nicht wie bekomm
ich den Gyro stabil :)
aber komischerweise hab ich trotz googeln nicht wirklich
nen lauffähigen Code für bascom gefunden, nur öfters
der Hilferuf nach selbigem.
Das stimmt allerdings. Vielleicht finde ich ja auch mal Zeit diesen Filter zu testen. Könnte der Threadstarter bitte den Titel des ganzen Threads ändern in "Kalman Filter für BASCOM - mit Lösung", dann findet sich das hier leichter in Google (einfach den ersten Post editieren).Zitat:
aber komischerweise hab ich trotz googeln nicht wirklich
nen lauffähigen Code für bascom gefunden, nur öfters
der Hilferuf nach selbigem.
Hab den Code bei mir mal eingebaut ... also er macht was ...
einmal die gefilterten Messwerte:
Code:------- Graph Temperatur --------
-
-
20********************************* * *
- ******* * *
- * *****
- * * *
- ****
- * *
- ***
- *
- *
- *
19 *
- ****
-
-
-
-
----------- Graph Gas ------------
8
-
-
-
-
7
-**************************************************
- ***********
- ***
- ******
6
-
-
-
-
5
und einmal die ungefilterten:
sorry, mein atmega128 gibt keine jpg aus, das sindCode:
------- Graph Temperatur --------
- * * ****** * ** * ***
-*** ************ ** ** * ** ***
20 * * * ** ** *
- ** * * * * *
- * * *
- * *
- *
- *
- *
- * * *
- ** *
-
19 *
- **
- ** *
- *
-
-
-
-
-
----------- Graph Gas ------------
8
-
-
-
-
7 * **
- * ****** **** ** ******** * ** *
-***** * *** * ******** **** * *
- ** ******
- *** ***********
6 * *
- *
-
-
-
5
die werte graphisch vom AVR aufbereitet und per
RS485 als Text an den PC übertragen ...