Code übersetzen

$regfile = "m128def.dat"

$crystal = 10000000

$hwstack = 40

$swstack = 16

$framesize = 32

$baud = 9600

Dim Kalman_dt As Single

Dim Kalman_gyro_sensor As Single

Dim Kalman_adxl_sensor As Single

Dim Neigungswinkel As Single

Dim Buf_ad_gyro As Integer

Dim Buf_ad_adxl As Integer

Dim Ad_gyro As Integer

Dim Ad_adxl As Integer

Dim Gyro_zero As Integer

Dim Adxl_zero As Integer

Dim Gyro1dkalman_x_angle As Single

Dim Gyro1dkalman_x_bias As Single

Dim Gyro1dkalman_p_00 As Single

Dim Gyro1dkalman_p_01 As Single

Dim Gyro1dkalman_p_10 As Single

Dim Gyro1dkalman_p_11 As Single

Dim Gyro1dkalman_q_angle As Single

Dim Gyro1dkalman_q_gyro As Single

Dim Gyro1dkalman_r_angle As Single

Dim Wert As Single

Dim Y As Single

Dim S As Single

Dim K_0 As Single

Dim K_1 As Single

Dim Temp As Single

Dim Temp1 As Single

Dim Ausgabe As String * 4

Declare Sub Init_gyro1dkalman(byval Q_angle As Single , Byval Q_gyro As Single , Byval R_angle As Single)

Declare Sub Ars_predict(byval Dotangle As Single , Byval Dt As Single)

Declare Function Ars_update(byval Angle_m As Single) As Single

Declare Function Kalman_filter_auswertung(gyro As Single , Adxl As Single , Dt As Single) As Single

Config Lcdpin = Pin , Db4 = Portc.4 , Db5 = Portc.5 , Db6 = Portc.6 , Db7 = Portc.7 , E = Portc.1 , Rs = Portc.0

Config Lcd = 16 * 2

Cls

Config Adc = Single , Prescaler = 128 , Reference = Off

Start Adc

'Init

Call Init_gyro1dkalman(0.0001 , 0.0003 , 0.3)               'Kalman Filter

Kalman_dt = 0.0157                                          'time passed in s since last call

Gyro_zero = Getadc(5)

Adxl_zero = Getadc(2)

Do                                                          'Endlosschleife

   Ad_gyro = Getadc(5)

   Ad_adxl = Getadc(2)

   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)

   Ausgabe = Fusing(neigungswinkel , "##.###")

   Cls

   Lcd Ausgabe

   Waitms 150

Loop

End

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

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

      'struct Gyro1DKalman

        '* These variables represent our state matrix x

    '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

Sub Ars_predict(byval Dotangle As Single , Byval Dt As Single)

   'Gyro1dkalman_x_angle = Gyro1dkalman_x_angle +(dt *(dotangle - Gyro1dkalman_x_bias))

   Temp = Dotangle - Gyro1dkalman_x_bias

   Temp = Temp * Dt

   Gyro1dkalman_x_angle = Gyro1dkalman_x_angle + Temp

   'Gyro1dkalman_p_00 = Gyro1dkalman_p_00 +( -dt *(gyro1dkalman_p_10 + Gyro1dkalman_p_01) + Gyro1dkalman_q_angle * Dt)

   Temp = -dt

   Temp1 = Gyro1dkalman_p_10 + Gyro1dkalman_p_01

   Temp = Temp * Temp1

   Temp1 = Gyro1dkalman_q_angle * Dt

   Temp = Temp + Temp1

   Gyro1dkalman_p_00 = Gyro1dkalman_p_00 + Temp

   'Gyro1dkalman_p_01 = Gyro1dkalman_p_01 +( -dt * Gyro1dkalman_p_11)

   Temp = -dt

   Temp = Temp * Gyro1dkalman_p_11

   Gyro1dkalman_p_01 = Gyro1dkalman_p_01 + Temp

   'Gyro1dkalman_p_10 = Gyro1dkalman_p_10 +( -dt * Gyro1dkalman_p_11)

   Temp = -dt

   Temp = Temp * Gyro1dkalman_p_11

   Gyro1dkalman_p_10 = Gyro1dkalman_p_10 + Temp

   'Gyro1dkalman_p_11 = Gyro1dkalman_p_11 +( + Gyro1dkalman_q_gyro * Dt)

   Temp = Gyro1dkalman_q_gyro * Dt

   Gyro1dkalman_p_11 = Gyro1dkalman_p_11 + Temp

End Sub

Function Ars_update(byval Angle_m As Single) 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)

      Temp = K_0 * Y

      Gyro1dkalman_x_angle = Gyro1dkalman_x_angle + Temp

      'Gyro1dkalman_x_bias = Gyro1dkalman_x_bias +(k_1 * Y)

      Temp = K_1 * Y

      Gyro1dkalman_x_bias = Gyro1dkalman_x_bias + Temp

      'Gyro1dkalman_p_00 = Gyro1dkalman_p_00 -(k_0 * Gyro1dkalman_p_00)

      Temp = K_0 * Gyro1dkalman_p_00

      Gyro1dkalman_p_00 = Gyro1dkalman_p_00 - Temp

      'Gyro1dkalman_p_01 = Gyro1dkalman_p_01 -(k_0 * Gyro1dkalman_p_01)

      Temp = K_0 * Gyro1dkalman_p_01

      Gyro1dkalman_p_01 = Gyro1dkalman_p_01 - Temp

      'Gyro1dkalman_p_10 = Gyro1dkalman_p_10 -(k_1 * Gyro1dkalman_p_00)

      Temp = K_1 * Gyro1dkalman_p_00

      Gyro1dkalman_p_10 = Gyro1dkalman_p_10 - Temp

      'Gyro1dkalman_p_11 = Gyro1dkalman_p_11 -(k_1 * Gyro1dkalman_p_01)

      Temp = K_1 * Gyro1dkalman_p_01

      Gyro1dkalman_p_11 = Gyro1dkalman_p_11 - Temp

      Return Gyro1dkalman_x_angle                           'was mache ich hiermit???

End Function

Function 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)

'---------------------------------------------------------------

    Call Ars_predict(gyro , Dt)                             'Kalman predict

    Wert = Ars_update(adxl)

    Return Wert                                             'und damit???

End Function