Dim Kalman_dt as Single
Dim Kalman_gyro_sensor as Single
Dim Kalman_adxl_sensor as Single
Dim Neigungswinkel as Integer

Dim Buf_Ad_gyro as Integer
Dim Buf_Ad_adxl as Integer

Dim Ad_gyro as Integer
Dim Ad_adxl as Integer

Sub main()

 'Init
 init_Gyro1DKalman(0.0001, 0.0003, 0.3)  'Kalman Filter
     Kalman_dt = 0.0157                                     'time passed in s since last call

 'Auszug.. etc..
 'Kanal setzen
 Gyro_zero =  ADC_Read()
 'Kanal setzen
 Adxl_zero =  ADC_Read()



     'main loop
     Do While True     'Endlosschleife

 'Auszug.. etc..
 'Kanal setzen
 Ad_gyro =  ADC_Read()
 'Kanal setzen
 Ad_adxl =  ADC_Read()

 Buf_Ad_gyro = Ad_gyro - Gyro_zero
 Buf_Ad_adxl = Ad_adxl - Adxl_zero

 Kalman_gyro_sensor = Buf_Ad_gyro * 1.4634135 ' /1025*3.0V/0.002
 Kalman_adxl_sensor = Buf_ad_adxl * 1.5139                  ' /1025*3.0V/0.0019333 0,348V for 180° = 1,9333mV per °

 Neigungswinkel = Kalman_Filter_Auswertung(Kalman_gyro_sensor, Kalman_adxl_sensor, Kalman_dt)

     End While


End Sub


'#############################################################################

'######################## Kalman Filter - Datei ##############################

      'struct Gyro1DKalman

        '* These variables represent our state matrix x
        Dim Gyro1DKalman_x_angle As Single
        Dim Gyro1DKalman_x_bias As Single

        '* Our error covariance matrix
        Dim Gyro1DKalman_P_00 As Single
        Dim Gyro1DKalman_P_01 As Single
        Dim Gyro1DKalman_P_10 As Single
        Dim Gyro1DKaalman_P_11 As Single

        '* Q is a 2x2 matrix of the covariance. Because we
        '* assuma the gyro and accelero noise to be independend
        '* of eachother, the covariances on the / diagonal are 0.
        '*
        '* Covariance Q, the process noise, from the assumption
        '*    x = F x + B u + w
        '* with w having a normal distribution with covariance Q.
        '* (covariance = E[ (X - E[X])*(X - E[X])' ]
        '* We assume is linair with dt
        Dim Gyro1DKalman_Q_angle As Single
        Dim Gyro1DKalman_Q_gyro As Single

        '* Covariance R, our observation noise (from the accelerometer)
        '* Also assumed to be linair with dt
        Dim Gyro1DKalman_R_angle As Single

    'End Structure

    '#######################################################################

    '***********************************************************************
    '*                                                                     *
    '* This file contains the code for the kalman filter that uses the     *
    '* sensor data as inputs.                                              *
    '*                                                                     *
    '***********************************************************************
    '*                                                                     *
    '*    Author:         Tom Pycke                                        *
    '*    Filename:       ars.c                                            *
    '*    Date:           17/10/2007                                       *
    '*    File Version:   1.00                                             *
    '*                                                                     *
    '***********************************************************************
    '*                                                                     *
    '* Comments:                                                           *
    '*   To help others to understand the kalman filter, I used one of     *
    '*   the most accessible sources with information on it:               *
    '*   http://en.wikipedia.org/wiki/Kalman_filter                        *
    '*   The code is split into 2 parts: a Predict function and an         *
    '*   Update function, just as the wikipedia page does.                 *
    '*   The model I used in the kalman filter is the following:           *
    '*   Our gyroscope measures the turnrate in degrees per second. This   *
    '*   is the derivative of the angle, called dotAngle. The bias is the  *
    '*   output of our gyro when the rotationrate is 0 degrees per second  *
    '*   (not rotating). Because of drift it changes over time.            *
    '*   So mathematically we just integrate the gyro (dt is timespan      *
    '*   since last integration):                                          *
    '*              angle = angle + (dotAngle - bias) * dt                 *
    '*   When we include the bias in our model, the kalman filter will     *
    '*   try to estimate the bias, thanks to our last input: the measured  *
    '*   angle. This is just an estimate and comes from the accelerometer. *
    '*   So the state used in our filter had 2 dimensions: [ angle, bias ] *
    '*   Jump to the functions to read more about the actual               *
    '*   implementation.                                                   *
    '*                                                                     *
    '***********************************************************************

    Sub init_Gyro1DKalman( ByVal Q_angle As Single, ByVal Q_gyro As Single, ByVal R_angle As Single)
        Gyro1DKalman_Q_angle = Q_angle
        Gyro1DKalman_Q_gyro = Q_gyro
        Gyro1DKalman_R_angle = R_angle
    End Sub

    '* The predict function. Updates 2 variables:
    '* our model-state x and the 2x2 matrix P
    '*
    '* x = [ angle, bias ]'
    '*
    '*   = F x + B u
    '*
    '*   = [ 1 -dt, 0 1 ] [ angle, bias ] + [ dt, 0 ] [ dotAngle 0 ]
    '*
    '*   => angle = angle + dt (dotAngle - bias)
    '*      bias  = bias
    '*
    '*
    '* P = F P transpose(F) + Q
    '*
    '*   = [ 1 -dt, 0 1 ] * P * [ 1 0, -dt 1 ] + Q
    '*
    '*  P(0,0) = P(0,0) - dt * ( P(1,0) + P(0,1) ) + dt˛ * P(1,1) + Q(0,0)
    '*  P(0,1) = P(0,1) - dt * P(1,1) + Q(0,1)
    '*  P(1,0) = P(1,0) - dt * P(1,1) + Q(1,0)
    '*  P(1,1) = P(1,1) + Q(1,1)
    '*

    Sub ars_predict( ByVal dotAngle As Single, ByVal dt As Single)

        Gyro1DKalman_x_angle = Gyro1DKalman_x_angle + (dt * (dotAngle - Gyro1DKalman_x_bias))

        Gyro1DKalman_P_00 = Gyro1DKalman_P_00 + (-dt * (Gyro1DKalman_P_10 + Gyro1DKalman_P_01) + Gyro1DKalman_Q_angle * dt)
        Gyro1DKalman_P_01 = Gyro1DKalman_P_01 + (-dt * Gyro1DKalman_P_11)
        Gyro1DKalman_P_10 = Gyro1DKalman_P_10 + (-dt * Gyro1DKalman_P_11)
        Gyro1DKalman_P_11 = Gyro1DKalman_P_11 + (+Gyro1DKalman_Q_gyro * dt)

    End Sub

    '*  The update function updates our model using
    '*  the information from a 2nd measurement.
    '*  Input angle_m is the angle measured by the accelerometer.
    '*
    '*  y = z - H x
    '*
    '*  S = H P transpose(H) + R
    '*    = [ 1 0 ] P [ 1, 0 ] + R
    '*    = P(0,0) + R
    '*
    '*  K = P transpose(H) S^-1
    '*    = [ P(0,0), P(1,0) ] / S
    '*
    '*  x = x + K y
    '*
    '*  P = (I - K H) P
    '*
    '*    = ( [ 1 0,    [ K(0),
    '*          0 1 ] -   K(1) ] * [ 1 0 ] ) P
    '*
    '*    = [ P(0,0)-P(0,0)*K(0)  P(0,1)-P(0,1)*K(0),
    '*        P(1,0)-P(0,0)*K(1)  P(1,1)-P(0,1)*K(1) ]
    '*

    ' Gyro1DKalman *Gyro1DKalman
    Sub ars_update( ByVal angle_m As Single) As Single

        Dim y As Single
        Dim S As Single
        Dim K_0 As Single
        Dim K_1 As Single

        y = angle_m - Gyro1DKalman_x_angle
        S = Gyro1DKalman_P_00 + Gyro1DKalman_R_angle
        K_0 = Gyro1DKalman_P_00 / S
        K_1 = Gyro1DKalman_P_10 / S

        Gyro1DKalman_x_angle = Gyro1DKalman_x_angle + (K_0 * y)
        Gyro1DKalman_x_bias = Gyro1DKalman_x_bias + (K_1 * y)

        Gyro1DKalman_P_00 = Gyro1DKalman_P_00 - (K_0 * Gyro1DKalman_P_00)
        Gyro1DKalman_P_01 = Gyro1DKalman_P_01 - (K_0 * Gyro1DKalman_P_01)
        Gyro1DKalman_P_10 = Gyro1DKalman_P_10 - (K_1 * Gyro1DKalman_P_00)
        Gyro1DKalman_P_11 = Gyro1DKalman_P_11 - (K_1 * Gyro1DKalman_P_01)

        Return Gyro1DKalman_x_angle
    End Sub

    Sub Kalman_Filter_Auswertung(gyro As Single, adxl As Single, dt As Single) As Single
        '----------------Kalman Filter----------------------------------
        'ars_predict(Kalman_gyro_sensor, Kalman_dt)    'Kalman predict
        'Neigungswinkel_Filter = ars_update(Kalman_adxl_sensor)
        '---------------------------------------------------------------

        Dim Wert As Single

        ars_predict(gyro, dt)    'Kalman predict
        Wert = ars_update(adxl)
        Return Wert
    End Sub