-         

Ergebnis 1 bis 1 von 1

Thema: Kalman Filter Matrizen erstellen

  1. #1
    Erfahrener Benutzer Roboter Genie
    Registriert seit
    08.09.2007
    Ort
    Berlin
    Alter
    24
    Beiträge
    1.544

    Kalman Filter Matrizen erstellen

    Anzeige

    Hi,

    ich beschäftige mich nun seit einiger Zeit mit dem Kalman Filter und habe wohl auch dessen Funktionsweise verstanden. Einen 1-d Filter hab ich schonmal erfolgreich implementiert, jetzt soll er 3D werden. Hab mal versucht, die Matrizen aufzustellen, wäre toll, wenn jemand mal drüber sehen könnte und mir sagen könnte, ob ich einen Fehler gemacht habe.

    Es wird eine Position und eine Beschleunigung gemessen. Als Ergebnis hätte ich gerne die Geschwindigkeit und die Position.
    Code:
    //Model:
    p = p + v
    v = v + a
    a = a + AccZ //AccZ is input command vector
    
    pn = pn-1 + vn-1*dt
    vn = vn-1 + an-1*dt
    an = an-1 + AccZ
    
    //Matrizen:
    |pn|   |pn-1+vn-1*dt       |   | 0  |
    |vn| = |     vn-1 + an-1*dt| + | 0  |
    |an|   |            an-1   |   |AccZ|
    
         |1 dt 0|        |0 0 0| |0   |
    xn = |0 1 dt|*xn-1 + |0 0 0|*|0   |
         |0 0  1|        |0 0 1| |AccZ|
    
        |1 dt 0|
    A = |0 1 dt|
        |0 0  1|
    
        |0 0 0|
    B = |0 0 0|
        |0 0 1|
    
        |0   |
    u = |0   |
        |AccZ|
    
        |1 0 0|
    H = |0 0 0|
        |0 0 1|
    
        |1 0 0|
    P = |0 1 0|
        |0 0 1|
    
        |0 0 0|
    Q = |0 0 0|
        |0 0 0|
    
        |0.5 0   0|
    R = |0   0   0|
        |0   0 0.5|
    
        |pMess  |
    z = |0      |
        |AccZ|
    Was haltet ihr grundsätzlich von dem Ansatz, die Position mit der Beschleunigung zu stützen?

    Vielen Dank & Gruß
    Chris

    EDIT:
    Soll eigentlich der Sensor-Input (Beschl. & Position) noch in das Model dieser (H) mit rein? Wenn ja, wie formuliert man da bspw. ein Integral oder Differential?
    Hier noch die Formeln:
    Code:
    1. yn = A*yn-1 + B*un
    2. Pn = A*Pn-1 * A(transponiert) + Q
    3. K = Pn * H(transponiert)*(H*Pn*H(transponiert) + R)
    4. yn = yn + K*(zn - H*yn)
    5. Pn = (I - K*H)*Pn
    Geändert von Che Guevara (11.02.2015 um 16:50 Uhr)

Ähnliche Themen

  1. Fragen zu Kalman-Filter
    Von Flo89 im Forum Sensoren / Sensorik
    Antworten: 3
    Letzter Beitrag: 04.10.2013, 21:50
  2. ST LPR530AL + Kalman Filter
    Von Pepade im Forum Sensoren / Sensorik
    Antworten: 3
    Letzter Beitrag: 23.04.2010, 08:29
  3. Kalman Filter für BASCOM
    Von harry1 im Forum Sensoren / Sensorik
    Antworten: 30
    Letzter Beitrag: 29.10.2009, 00:22
  4. An die Kalman-Filter-Fachleute
    Von Crazy Harry im Forum Sensoren / Sensorik
    Antworten: 2
    Letzter Beitrag: 08.05.2006, 22:33
  5. Kalman-Filter - wie/wo/was ?
    Von Crazy Harry im Forum Sensoren / Sensorik
    Antworten: 4
    Letzter Beitrag: 02.02.2006, 11:23

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •