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

Print

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
Print