oder als Subroutine:

einmal der headerfile

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()
und die sub:

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