Kalman Filter Matrizen erstellen

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

---