Hallo goara,

bzgl. Kalmanfilter - hier habe ich die Homepage www.ulrichradig.de gefunden mit einem kleinen Projekt - alle anderen Projekte sind sehr aufwendig, Code Interpretation = mehrere Tage.

Hier habe ich versucht das Script fc.c mit PHP zu simulieren um das Vorgehen zu verstehen. Mit Testwerten.

[php:1:bba2501f77]
<?php

// Werte initalisieren
$y_old = 0;
$y_sum = 0;
$y_e_sum = 0;
$y_rc_mittelwert = 0;

$x_old = 0;
$x_sum = 0;
$x_e_sum = 0;
$x_rc_mittelwert = 0;

$gier_old = 0;

// Neutralwert errechen (Normalerweise 100 Durchläufe, hier nur 10)
function adc_neutral($a, $b) {
$wert = 0;
$i = 10;
while($i--) {
$wert += rand($a, $b);
}
return (int)$wert / 10;
}
// Nochmal das gleiche für einen aktuellen Wert... (etwas doppelt gemoppelt)
function get_adc($neutral, $a, $b) {
$wert = 0;
$i = 10;
while($i--) {
$wert += $neutral - rand($a, $b);
}
return array((int)$wert, 10);
}

$y_rc_mittelwert = adc_neutral(100, 250);

// Drei Testdurchläufe
$i = 3;
while ($i--) {

// Initalisieren neuer Werte
list($adc0_value_mux["ADC_GIER"], $adc_count["ADC_GIER"]) = get_adc(adc_neutral(507, 567), 507, 567);
list($adc0_value_mux["ADC_ACCY"], $adc_count["ADC_ACCY"]) = get_adc(adc_neutral(420, 480), 420, 480);
list($adc0_value_mux["ADC_ACCX"], $adc_count["ADC_ACCX"]) = get_adc(adc_neutral(220, 280), 220, 280);
list($adc0_value_mux["ADC_NICK"], $adc_count["ADC_NICK"]) = get_adc(adc_neutral(276, 326), 276, 326);
list($adc0_value_mux["ADC_ROLL"], $adc_count["ADC_ROLL"]) = get_adc(adc_neutral(376, 416), 376, 416);

$rc_ch["RC_NICK"] = adc_neutral(100, 250);
$rc_ch["RC_GAS"] = adc_neutral(100, 250);
$rc_ch["RC_GIER"] = adc_neutral(100, 250);

// ----------- Ab hier Starte die Code Interpretation fc.c - Copyright UlrichRadig.de !!! -------------


$gier = -($adc0_value_mux["ADC_GIER"] / $adc_count["ADC_GIER"]);

$y_out = 0;
$y_gyro_error = 0;
$y_b = $adc0_value_mux["ADC_ACCY"] / $adc_count["ADC_ACCY"];
$y_g = $adc0_value_mux["ADC_NICK"] / $adc_count["ADC_NICK"];

$x_out = 0;
$x_gyro_error = 0;
$x_b = $adc0_value_mux["ADC_ACCX"] / $adc_count["ADC_ACCX"];
$x_g = $adc0_value_mux["ADC_ROLL"] / $adc_count["ADC_ROLL"];

$e = $w = $rc_error = 0;
$motor_value = 10;

$tmp;

$adc_count["ADC_NICK"] = 0;
$adc_count["ADC_ACCY"] = 0;
$adc_count["ADC_ACCX"] = 0;
$adc_count["ADC_ROLL"] = 0;
$adc_count["ADC_GIER"] = 0;

$adc0_value_mux["ADC_NICK"] = 0;
$adc0_value_mux["ADC_ACCY"] = 0;
$adc0_value_mux["ADC_ACCX"] = 0;
$adc0_value_mux["ADC_ROLL"] = 0;
$adc0_value_mux["ADC_GIER"] = 0;

//---------------------------------------------------------------------------
print('$gier = '. $gier . ', $y_b = ' . $y_b . ', $y_g = ' . $y_g .
', $x_b = ' . $x_b . ', $x_g = ' . $x_g . '
');
//---------------------------------------------------------------------------
//PD Gyro (GIER)

//e = w - x (Sollwert = 0)
$e = ($gier * 0.5) - ($rc_ch["RC_GIER"] * 0.4);

//---------------------------------------------------------------------------
print(' $e = ' . $e );
//---------------------------------------------------------------------------

//P-Regelpart Kp * e
$gier = 4 * $e;

//---------------------------------------------------------------------------
print(' $gier = ' . $gier );
//---------------------------------------------------------------------------

//D-Regelpart Kd/ta * e (Zeit ta = 0,01s)
$gier += 10 * ($e - $gier_old);

//---------------------------------------------------------------------------
print(' $gier = ' . $gier . "
");
//---------------------------------------------------------------------------

$gier_old = $e;


//---------------------------------------------------------------------------
//Gyro Signal integrieren und an AAC abgleichen
$rc_error = $rc_ch["RC_NICK"] - $y_rc_mittelwert;

if (abs($rc_error) > 20) {
if ($rc_ch["RC_NICK"] > $y_rc_mittelwert) $y_rc_mittelwert += 3;
if ($rc_ch["RC_NICK"] < $y_rc_mittelwert) $y_rc_mittelwert -= 3;
} else {
if ($rc_ch["RC_NICK"] > $y_rc_mittelwert) $y_rc_mittelwert++;
if ($rc_ch["RC_NICK"] < $y_rc_mittelwert) $y_rc_mittelwert--;
}

//---------------------------------------------------------------------------
print(' $y_rc_mittelwert = ' . $y_rc_mittelwert);
//---------------------------------------------------------------------------

$y_sum += $y_g;

//---------------------------------------------------------------------------
print(' $y_sum = ' . $y_sum);
//---------------------------------------------------------------------------

$y_gyro_error = $y_sum - ($y_b * 70);

//---------------------------------------------------------------------------
print(' $y_gyro_error = ' . $y_gyro_error );
//---------------------------------------------------------------------------

if($y_gyro_error > 3000) $y_gyro_error = 3000;
if($y_gyro_error < -3000) $y_gyro_error =-3000;

$y_sum -= $y_gyro_error * 0.005;

//---------------------------------------------------------------------------
print(' $y_sum = ' . $y_sum . "
");
//---------------------------------------------------------------------------


//----------------------------------------
//PI - Regler

//e = w - x (Sollwert = 0)
$w = ($y_g * 0.3) + ($y_sum * 0.003);

//---------------------------------------------------------------------------
print(' $w = ' . $w);
//---------------------------------------------------------------------------

$e = $w - ($y_rc_mittelwert * 0.2);

//---------------------------------------------------------------------------
print(' $e = ' . $e);
//---------------------------------------------------------------------------

//Integral bilden und begrenzen (I-Anteil)
$y_e_sum += $e;
if ($y_e_sum > 3000) $y_e_sum = 3000;
if ($y_e_sum < -3000) $y_e_sum = -3000;

//---------------------------------------------------------------------------
print(' $y_e_sum = ' . $y_e_sum);
//---------------------------------------------------------------------------

//P-Regler Kp * e + Ki + e_sum_nick1 (P + I-Anteil)
$y_out = 3 * $e + ($y_e_sum * 0.025);

//---------------------------------------------------------------------------
print(' $y_out = ' . $y_out . "<hr>");
//---------------------------------------------------------------------------


//D-Regelpart Kd/ta * e (Zeit ta = 0,01s) (+D Anteil)
//$y_out += 2 * ($e - $y_old);
$y_old = $e;


//----------------------------------------
//Werte mischen

$motor_value = $rc_ch["RC_GAS"] + 70 + $y_out + $gier;
if ($motor_value < 5) $motor_value = 5;
if ($motor_value > 255) $motor_value = 255;
$PWM_Motor[0]= $motor_value;

$motor_value = $rc_ch["RC_GAS"] + 70 - $y_out + $gier;
if ($motor_value < 5) $motor_value = 5;
if ($motor_value > 255) $motor_value = 255;
$PWM_Motor[1]= $motor_value;

//---------------------------------------------------------------------------
print('$rc_ch["RC_GAS"] + 70 = ' . ($rc_ch["RC_GAS"] + 70) .
' $y_out = ' . $y_out .
' $gier = ' . $gier . "
");
print(' $PWM_Motor[1] = ' . $PWM_Motor[1] . ' $PWM_Motor[0] = ' . $PWM_Motor[0] . "<hr>");
//---------------------------------------------------------------------------


}

[/php:1:bba2501f77]

Das Debug Ergebnis sieht dann so aus.

[php:1:bba2501f77]

....


// Zweiter Durchlauf

$gier = 0.9, $y_b = 7.6, $y_g = 4.8, $x_b = 6.8, $x_g = 1.1
$e = -73.83 $gier = -295.32 $gier = -320.92
$y_rc_mittelwert = 192 $y_sum = -0.931 $y_gyro_error = -532.931 $y_sum = 1.733655
$w = 1.445200965 $e = -36.954799035 $y_e_sum = -77.911992035 $y_out = -112.812196906

$rc_ch["RC_GAS"] + 70 = 249.9 $y_out = -112.812196906 $gier = -320.92
$PWM_Motor[1] = 41.7921969059 $PWM_Motor[0] = 5

// Dritter Durchlauf

$gier = 5, $y_b = 2.4, $y_g = 10.9, $x_b = -6.3, $x_g = -5.9
$e = -69.3, $gier = -277.2, $gier = -231.9
$y_rc_mittelwert = 189, $y_sum = 12.633655 $y_gyro_error = -155.366345 $y_sum = 13.410486725
$w = 3.31023146017 $e = -34.4897685398 $y_e_sum = -112.401760575 $y_out = -106.279349634

$rc_ch["RC_GAS"] + 70 = 230.2 $y_out = -106.279349634 $gier = -231.9
$PWM_Motor[1] = 104.579349634 $PWM_Motor[0] = 5
[/php:1:bba2501f77]

Das Vorgehen kapier ich leider nicht... Mir erschließt sich hier keinerlei Logik... Bis jetzt. Naja, ich bleib dran...

Soweit Viele Grüße aus Neuss
ABSimon