Code übersetzen

[gast (Gast)]

Gute Abend Forum,

ich wende mich mal an Euch da ich seit Tagen (Nächten) schon nicht 
weiter komme! Es geht um eine Codeübersetzung für die scheinbar mein 
Wissen nicht ausreicht. Dabei geht es um den Kalman Filter. Diesen habe 
ich als Basic-Quellcode im Netz gefunden:

http://www.elektor.de/forum/foren-ubersicht/foren-zu-elektor-projekten/elektorwheelie/kalman-filter.1510605.lynkx

Diesen versuche ich nun ins Bascom zu bringen. Ich habe schon die ganze 
Bascomhilfe abgegrast, doch der Code will und will nicht. Anbei die 
Datei des Basic-Codes und hier was ich bisher erreicht habe. Ich hoffe 
mir kann hier jemand weiter helfen.

Vielen Dank im Voraus…

$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

[Quengel (Gast)]

In der "schöngeistigen" Literatur ist es ähnlich:

Wer nicht verstanden hat, was der Autor schrieb,
kann es auch nicht übersetzen...

[gast (Gast)]

vielen Dank für die Hilfe...

[Bernd B. (Firma: BB) (berndb)]

Hi Gast,

und Bascom sagt also "Ich will nicht" ? oder  ....... ?

Gruss Berndb

[gast (Gast)]

Hi Bernd,

ich muss einfach nur wissen ob ich mit der Syntax richtig liege in 
Vergleich zu der Basicvariante. Die Variable "Ausgabe" ist bei mir immer 
00.00
Leider verstehe ich nicht warum und wende mich daher an dieses Forum.

Gruß
Andre

[ich (Gast)]

Ja und ab welcher Zeile treten Abweichungen zu den theoretisch 
berechneten Werten auf?

Wenn der Neigungswinkel "richtig" ist, dann ist der Fehler lokalisiert

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

>Cls
>Lcd Ausgabe


Ist damit fehlerhaft.

[gast (Gast)]

die Theorie sollte eigentlich stimmen. Soweit meine Mathekenntnisse es 
zulassen habe ich es nachgerechtet. Problem ist halt dass ich nicht 
kontrollieren kann, da mir die Ausgabe fehlt. Ich gehe also stark von 
einem Syntaktischen Problem aus.
Ist die Werteübergabe „Single an String“ (Ausgabe) soweit richtig? Laut 
Bascom-Hilfe ist das OK, bin mir trotzdem nicht sicher weil Garnichts 
kommt.

Gruß
Andre

[Läubi .. (laeubi)]

gast schrieb:
> Ich gehe also stark von
> einem Syntaktischen Problem aus

Dann müsste dir der Compiler ja einen Syntax-Fehler melden...

[mue-c (Gast)]

Der Compiler gibt ja auch Fehler aus.
Return mit etwas dahinter ist in Bascom nicht erlaubt.
Also ändere das mal in Zeile 217 und 230.
Dann starte den Simulator und gebe mal Werte vor und schau, ob die 
Berechnung das gleiche Ergebnis liefert, wie in VB.

[Wolfgang S. (wsm)]

Ich suche auch gerade diesen Code.

Werde mich kommende Woche damit beschäftigen.

Schreib mir mal ne Mail.

Ggf. ist die Rückgabe des Funktionswertes am Schluss nicht korrekt.

Es gibt auch noche eine Alternative zu diesem Code. Ist eine 
Vereinfachung speziell für BASCOM. Funktioniert bei mir auch ganz 
passabel.

W.