Kalman in der Lagebestimmung

[Patrick L. (crashdemon)]

Hallo,

bastel zurzeit an einem System mit dem ich die Neigung von einem 
dynamisch Bewegten System erfassen kann, das funktioniert auch schon 
ganz gut, allerdings will ich noch versuchen das optimum 
herauszukitzeln.
Der ganze Aufbau besteht aus einem Atmega8, einen dreiachsen 
Beschleunigungssensor (ADXL330) und dem Gyro (MLX90609), die bestimmung 
des Winkels mittels des Beschleunigungssesors funktioniert auch schon 
ganz gut, naja der Gyro ist noch verbesserungswürdig.
Jetzt zu dem eigentlichen Problem, da der ganze aufbau ja sehr 
empfindlich auf erschütterungen reagiert und somit das Messergebnis 
verfälscht, dachte ich mir das ich da mal nen Kalman-Filter drüberjagen 
könnte, also ein wenig im i-net gesucht und dann auf folgender seite: 
http://home.arcor.de/uffmann/ARTIST3.htm auch einen code dazu gefunden, 
allerdings habe ich einige Fragen dazu.

static const float  dt = 0.0041; // Die Aktualisierungsrate

// Die zwei Zustände, der Winkel und die Gyro Schräglage
float angle = 0.0; 
float q_bias = 0.0;
float rate = 0.0;

float angle_err;
float C_0;

// Die Kofaktor Matrix
static float P[2][2] = {
              { 1, 0 },
              { 0, 1 },
};

static const float  R_angle  = .2; // Rauschen des Beschleunigungssensors
static const float  Q_angle  = 0.001; // Trust Verhältnis von Winkel zum Gyro
static const float  Q_gyro  = 0.001; // Trust Verhältnis von Winkel zum Gyro


/******** Gyro Messung, Kalman Filtern ********/
void StateUpdate(const float q_m)
{
  const float q = q_m - q_bias; // Die wahre Schräglage rausrechnen

  // Ableitung der Kofaktor Matrix
  // Pdot = A*P + P*A' + Q
  const float Pdot[2 * 2] = {
    Q_angle - P[0][1] - P[1][0],  
            - P[1][1],    
            - P[1][1],    
    Q_gyro        
  };

  rate = q; // Gyro Neigung schätzen

  angle += q * dt; // Winkel Schätzung aktualisieren

  // Kofaktor Matrix aktualisieren
  P[0][0] += Pdot[0] * dt;
  P[0][1] += Pdot[1] * dt;
  P[1][0] += Pdot[2] * dt;
  P[1][1] += Pdot[3] * dt;
}

/******** Winkel Messung, Kalman Filtern ********/
void KalmanUpdate(const float angle_m)
{
  angle_err = angle_m - angle;
  
  C_0 = 1; // Beziehung zwischen aktueller Messung und alter
  
  const float PCt_0 = C_0 * P[0][0]; 
  const float PCt_1 = C_0 * P[1][0]; 
  const float E = R_angle + C_0 * PCt_0; // E = C P C' + R (Fehler schätzen)
  const float K_0 = PCt_0 / E; // K = P C' inv(E) (Die Kalman Filter verstärkung berechnen)
  const float K_1 = PCt_1 / E; // K = P C' inv(E) (Die Kalman Filter verstärkung berechnen)
  const float t_0 = PCt_0; // P = P - K C P (Kofaktor Matrix aktualisieren)
  const float t_1 = C_0 * P[0][1]; // P = P - K C P (Kofaktor Matrix aktualisieren)

  P[0][0] -= K_0 * t_0;
  P[0][1] -= K_0 * t_1;
  P[1][0] -= K_1 * t_0;
  P[1][1] -= K_1 * t_1;
  
  angle  += K_0 * angle_err; // X += K * err (Differenz zwischen geschätzten Winkel und gemessenen)
  q_bias  += K_1 * angle_err; // X += K * err (Differenz zwischen Drehraten Winkel und gemessenen)
}

Jetzt meine Fragen zu den abgebildeten Funktionen, woher kriege ich 
jetzt die Variable die den Winkel Kalman gefiltert enthält, da die 
Funktionen ja keinen Rückgabewert haben, oder wird dieser in einer 
globalen Variable verstaut?

Achtung, die Kommentare hab ich zum Code hinzugefügt und müssen nicht 
richtig sein.

[TOM (Gast)]

angle und q_bias sind die Zustandsgrößen des Systems und gleichzeitig 
die Ausgänge, da die Ausgangsmatrix ja 1 ist

[Patrick L. (crashdemon)]

TOM wrote:
> angle und q_bias sind die Zustandsgrößen des Systems und gleichzeitig
> die Ausgänge, da die Ausgangsmatrix ja 1 ist

d.h. ich musst lediglich die beiden funktionen aufrufen und zur ausgabe 
auf meinem display später die korriegierten variablen angle und q_bias 
benutzen, wenn ich das richtig verstehe?

Dann noch eine Frage zu den Übergabewerten der Funktionen, in welchen 
form müssen die vorliegen, als Winkel oder "raw" so wie sie der 
jeweilige sensor ausgiebt.

[Patrick L. (crashdemon)]

Hat denn keiner eine Antwort?

[MrStrom (Gast)]

naja ich denke mal, die die kalmanfilter verstehen,
programmieren nicht .. und die die programmieren, verstehen
kalman filter nicht. das ist so ein dilema^^

da die funktionen weder zeigern nehmen, noch was zurückgeben
so werden sie sicher über globale variablen die werte
zurückgeben.

[Santiago m. H. (mausschubser)]

Hallo Patrick,

> Dann noch eine Frage zu den Übergabewerten der Funktionen, in welchen
> form müssen die vorliegen, als Winkel oder "raw" so wie sie der
> jeweilige sensor ausgiebt.

Ich kenne jetzt Deinen Sensor nicht, aber ich denke mal, dass der eine 
variable Spannung erzeugt, die Du per ADC einliest - d.h. Du hast 
erstmal ganzzahlige Werte, die umgerechnet werden müssen (per Tabelle 
oder Berechnung).
Bei Winkelberechnungen werden Winkel gerne im Bogenmaß-Format verwendet, 
deshalb auch float als Parametertyp.

Gruß Santi

[TOM (Gast)]

>> naja ich denke mal, die die kalmanfilter verstehen,
>> programmieren nicht .. und die die programmieren, verstehen
>> kalman filter nicht. das ist so ein dilema^^

widerspruch :-)

Orientiere dich nicht zu sehr an dem Code, schreib dir das Ganze mal in 
Zustandraumdarstellung auf und dir werden die Skalierungen der Variablen 
klar. Angle ist einfach der Winkel, der ja in der Prädiktion durch 
Integration von q über dt berechnet wird.

[Patrick L. (crashdemon)]

Santiago m. H. wrote:
> Ich kenne jetzt Deinen Sensor nicht, aber ich denke mal, dass der eine
> variable Spannung erzeugt, die Du per ADC einliest - d.h. Du hast
> erstmal ganzzahlige Werte, die umgerechnet werden müssen (per Tabelle
> oder Berechnung).
> Bei Winkelberechnungen werden Winkel gerne im Bogenmaß-Format verwendet,
> deshalb auch float als Parametertyp.
>
> Gruß Santi

Danke ersteinmal für die noch eingetroffenen Antworten, ja du vermutest 
schon richtig das ich die Werte des Beschleunigungssensors Analog 
einlesen, wobei ich dann je nach Stellung des Sensors ein 
proportionalens Spannungssignal erfasse, den Gyro lese ich allerdings 
per SPI aus, sprich digital, da ich relativ neu in Microcontroller bin, 
kam zwei verschiedene Varianten zum auslesen in Frage, damit ich dabei 
auch noch was neues lerne.

Hier ein paar infos zu den eingesetzten Sensoren:
http://www.analog.com/en/mems-and-sensors/imems-accelerometers/adxl330/products/product.html
http://www.melexis.com/ProdMain.aspx?nID=582

Habe mich mittlerweile ein wenig mit dem Kalman Filter beschäftigt und 
herausgefunden das eure vermutungen Richtig waren die variablen angle 
und q_bias tatsächlich die rückgabewerte als globale variable 
darstellen.
Naja das ganze funktioniert zwar schon ganz schön allerdings werde ich 
wohl nicht darum herumkommen, mich noch ein bisschen tiefer in die 
Thematik einzuarbeiten.

[TOM (Gast)]

Wenn Du nicht ganz der Mathematiklaie bist, dann ist das eine recht 
einfache Erklärung:
http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf