Schätzung der Geschwindigkeit aus bekannter Position und Beschleunigung

Ich bin nicht in der Lage, ein Systemmodell zu modellieren, dh meinen Zustandsvektor und meinen Eingabevektor zu erhalten. Ich vermute, dass Position und Geschwindigkeit Zustandsvektor und Beschleunigung Eingangsvektor sind. Meine zweite Vermutung ist, dass sich alle drei Größen im Zustandsvektor und keine im Eingabevektor befinden.

Also ... was ist ein Zustandsvektor und was ist in meinem Fall ein Eingabevektor?

- -

Zusätzliche Information:

Ich bekomme Messungen vom Positionssensor und Beschleunigungssensor. Alles geschieht in 1D, zum Beispiel auf einer geraden Linie. Ich möchte diese Messwerte zusammenführen (und das Rauschen entfernen), um eine Geschwindigkeitsschätzung für jeden Zeitschritt zu erhalten.

Diese Gleichungen beschreiben das System; Ich bin mir nicht sicher, ob sie richtig modelliert sind. Wenn ich das richtig verstehe, kann ich mit Sicherheit vorhersagen, dass die Beschleunigung konstant ist (obwohl sie sich in Wirklichkeit ändert) - weil die Prozesskovarianzmatrix diese Annahme korrigiert (richtig?).

Ich habe auch einige Beispieldaten, mit denen ich arbeiten kann (Eingabewerte werden hier der Einfachheit halber nicht verrauscht):

 time    pos     acc      what I should get as output (velocity)
[0.0s]  0.000, -0.000  | 18.850
[0.1s]  1.885, -0.113  | 18.850
[0.2s]  3.768, -0.227  | 18.839
[0.3s]  5.650, -0.340  | 18.816
[0.4s]  7.528, -0.452  | 18.782
[0.5s]  9.401, -0.565  | 18.737

---

ZUSATZ 2:

Zur besseren Kommunikation erstelle ich eine neue Antwort, sollte aber als Kommentar zur ersten Antwort behandelt werden. Jason, du hast mir schon enorm geholfen und ich bin wirklich dankbar für deine Zeit. Ich habe immer noch Probleme damit - die Ergebnisse von Kalman Filter sind nicht wie erwartet. Mögen Sie die Zeit finden, überprüfen Sie bitte das Folgende, danke. Ich schulde dir schon ein oder zwei Bier (oder Coffies, wenn du magst) - wenn du Paypal hast, kontaktiere mich auf primoz [at] codehunter.eu :)

---

Ich habe das Modell implementiert, das Jason in der ersten Antwort vorgeschlagen hatte. Ich habe den Ruck als 4. Zustandsvariable hinzugefügt . Nach stundenlangem Überprüfen entschied ich mich, hierher zurückzukehren, um Hilfe zu erhalten. Die Werte, die ich von KF bekomme, sind nicht wie erwartet. Die folgende Tabelle zeigt die Daten der ersten 10 Iterationen des Algorithmus. Beachten Sie, wie der Ruck mit jedem Zeitschritt zunimmt, wodurch andere Schätzungen falsch sind. Nach einer Sekunde beträgt der Unterschied zwischen realer und geschätzter Beschleunigung mehr als 1 m / s² (siehe Tabelle, letzte Zeile)!

           real           measured                   estimated                    real 
time   pos     acc       pos    acc        pos      acc    jerk    vel[!]       velocity 
0.0   0.000  -0.000    -0.040  0.030  |  -0.300   -0.060   0.000   18.850  <-->  18.850
0.1   1.885  -0.113     1.965 -0.153  |   1.585   -0.061  -0.006   18.844  <-->  18.844
0.2   3.768  -0.227     3.778 -0.247  |   3.469   -0.066  -0.035   18.835  <-->  18.827
0.3   5.650  -0.340     5.750 -0.370  |   5.351   -0.090  -0.122   18.815  <-->  18.799
0.4   7.528  -0.452     7.358 -0.452  |   7.228   -0.152  -0.291   18.769  <-->  18.759
0.5   9.401  -0.565     9.251 -0.555  |   9.094   -0.282  -0.574   18.673  <-->  18.708
0.6   11.269 -0.677    11.309 -0.717  |   10.938  -0.518  -1.006   18.494  <-->  18.646
0.7   13.130 -0.788    13.260 -0.758  |   12.752  -0.840  -1.490   18.233  <-->  18.573
0.8   14.983 -0.899    15.043 -0.949  |   14.520  -1.286  -2.096   17.854  <-->  18.488
0.9   16.827 -1.009    16.977 -1.089  |   16.235  -1.838  -2.770   17.362  <-->  18.393
1.0   18.661 -1.118    18.831 -1.168  |   17.890  -2.477  -3.476   16.762  <-->  18.287

Meine Matrizen sind hier:

Was verursacht diesen Zusatz in jedem Zeitschritt für Ruck ? Ist eine meiner Matrizen falsch?

Gleiches gilt für die erste Lösung (nur 3-Zustands-Modell) - die Beschleunigung ändert sich nicht so, wie sie sollte.

LETZTE BEARBEITUNG:

Ich habe es endlich geschafft, dass es funktioniert. Ich bin nicht sicher, ob ein Implementierungsfehler oder falsche P & Q-Matrizen aufgetreten sind.

In diesem Fall würden Sie keine Annahme einer konstanten Beschleunigung treffen. Sie würden dies normalerweise tun, wenn Sie keine Möglichkeit hätten, die Beschleunigung des Systems zu messen, aber Sie sagen, dass dies für Ihren Fall beobachtbar ist. Der naheliegendste Weg, dieses System zu modellieren, wäre die Verwendung des Zustandsvektors

$$\mathbf{x_k} = \left [ \begin{array}{c}x_k \\ \dot{x}_k \\ \ddot{x}_k \end{array} \right ] = \left [ \begin{array}{c}x_k \\ v_k \\ a_k \end{array} \right ]$$

$x_k$$v_k$$a_k$$k$$\mathbf{z_k}$

$$\mathbf{z_k} = \mathbf{Hx_k} + \mathbf{v_k}$$

mit

$$\mathbf{H} = \left[ \begin{array}{cc} 1 && 0 && 0 \\ 0 && 0 && 1 \end{array} \right]$$

Daraus ergibt sich das Messmodell:

$$\mathbf{z_k} = \left[ \begin{array}{c} x_k \\ a_k \end{array} \right] + \mathbf{v_k}$$

$\mathbf{v_k}$$\mathbf{u_k} = \mathbf{0}$

$a_{k+1}=a_k$

$$\mathbf{x_k} = \left [ \begin{array}{c}x_k \\ \dot{x}_k \\ \ddot{x}_k \\ \dddot{x_k} \end{array} \right ] = \left [ \begin{array}{c}x_k \\ v_k \\ a_k \\ j_k \end{array} \right ]$$

Dabei ist der Ruck des Systems oder die zeitliche Änderungsrate der Beschleunigung zum Zeitpunkt . Da Sie den Ruck nicht direkt messen, haben Sie eine Messmatrix von:$j_k$$k$

$$\mathbf{H} = \left[ \begin{array}{cc} 1 && 0 && 0 && 0\\ 0 && 0 && 1 && 0 \end{array} \right]$$

Und die Zustandsübergangsmatrix würde werden:

$$\mathbf{F} = \left[ \begin{array}{cccc} 1 && dt && \frac{1}{2}dt^2 && \frac{1}{6} dt^3 \\ 0 && 1 && dt && \frac{1}{2}dt^2 \\ 0 && 0 && 1 && dt \\ 0 && 0 && 0 && 1 \end{array} \right]$$

Dies ist ein "Konstant-Ruck" -Modell (ähnlich den Modellen mit konstanter Geschwindigkeit oder konstanter Beschleunigung, die Sie in einfachen Beispielen sehen können). Ihre Zustandsübergangsmatrix geht implizit davon aus, dass der Ruck für alle Werte von k konstant ist. Dies ist wahrscheinlich nicht wahr. Um diesen Aspekt des Problems zu behandeln, würden Sie einen Prozessrauschterm in die Ruckkomponente der Zustandsübergangsgleichung einfügen.

Qualitativ können Sie so ausdrücken, dass Sie nicht sicher sind, wie sich der Ruckbegriff von Zeitschritt zu Zeitschritt ändern wird, aber dass Sie erwarten, dass die Änderungen bei einer Gaußschen Verteilung zufällig sind. Dies ist ein Tool, das häufig verwendet wird, um adaptive Filtermodelle wie dieses zu "optimieren", bis Sie eine Reihe von Parametern finden, die für Ihre Anwendung gut geeignet sind.

Ich gehe davon aus, dass Ihre Zustandsvariablen der Antwort von Jason entsprechen. Wenn dies der Fall ist, dann glaube ich, dass Ihr Problem möglicherweise Ihre Matrix (Prozessrauschen) ist: Da sie bis auf die Ruckaktualisierung überall Null ist, sind die einzigen Begriffe, die die Werte für Position, Geschwindigkeit und Beschleunigung aktualisieren, die deterministischen Teile der Gleichungen aktualisieren (außer wenn sich die stochastische Natur des Ruckes durchsetzt).$Q$

Versuchen Sie es mit einer Matrix wie:$Q$

$$Q = \left [ \begin{array}{cccc} 0.00001 & 0& 0& 0\\ 0 & 0.00001 & 0 & 0\\ 0 & 0 & 0.00001 & 0 \\ 0 & 0& 0& 0.001 \end{array} \right ]$$

Wenn diese diagonalen Elemente Null sind, sagt das Modell "Ich erwarte nicht, dass sich Position, Geschwindigkeit oder Beschleunigung ändern (außer durch die Aktualisierungsgleichungen)" - was eindeutig nicht der Fall ist.

Ich denke, das Problem liegt in Q. Q laden K mit ihren Werten auf. Ich erziele gute Ergebnisse, wenn Q mit Potenzen von delta_t vorgeladen ist.

Siehe jeden Artikel über Wiener Modell. Es hat eine gute Q-Matrix.

A00 muss ein Vielfaches der Delta_t-Leistung von 5 sein. A11 Delta_t-Leistung von 3 A22 Delta_t-Leistung von 1 A01 Vielfaches der Delta_t-Leistung von 4

Und so weiter

Ansehen

https://www.google.com/url?sa=t&source=web&rct=j&url=https://pdfs.semanticscholar.org/9c1b/db6b5a25c1231e775c654f731e1ca1fef31a.pdf&ved=2ahUKEwj4vJbU1qLlAhUWAmMBHe7-DYcQFjADegQIBRAB&usg=AOvVaw1ELb4Ql84HAJot2rzktVsI