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?). Geben Sie hier die Bildbeschreibung ein

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.

kalman-filters

— Primož Kralj
quelle

Um eine nützliche Antwort zu erhalten, müssen Sie weitere Informationen angeben. Beschreiben Sie das System. Zeigen Sie uns, welche Gleichungen Sie zusammengestellt haben.

— Jim Clay

Danke für Ihre Antwort. Ich habe zusätzliche Informationen bereitgestellt.

— Primož Kralj

Bitte hilft mir jemand dabei - ich habe das Gefühl, ich verliere den Verstand.

— Primož Kralj

Ich sehe kein offensichtliches Problem. Es ist wahrscheinlich, dass Sie einen Implementierungsfehler haben.

— Jason R

@Phonon, keine harten Gefühle. Ich fühle mich wie auf einer Achterbahn gewesen :)

— Primož Kralj

Antworten:

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

x_{k} = [\begin{matrix} x_{k} \\ {\dot{x}}_{k} \\ {\ddot{x}}_{k} \end{matrix}] = [\begin{matrix} x_{k} \\ v_{k} \\ a_{k} \end{matrix}]

$\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}$

z_{k} = H x_{k} + v_{k}

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

mit

H = [\begin{array}{cc} 1 & 0 & 0 \\ 0 & 0 & 1 \end{array}]

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

Daraus ergibt sich das Messmodell:

z_{k} = [\begin{matrix} x_{k} \\ a_{k} \end{matrix}] + v_{k}

$\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$

x_{k} = [\begin{matrix} x_{k} \\ {\dot{x}}_{k} \\ {\ddot{x}}_{k} \\ \overset{⃛}{x_{k}} \end{matrix}] = [\begin{matrix} x_{k} \\ v_{k} \\ a_{k} \\ j_{k} \end{matrix}]

$\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$

H = [\begin{array}{cc} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{array}]

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

Und die Zustandsübergangsmatrix würde werden:

F = [\begin{array}{cccc} 1 & d t & \frac{1}{2} d t^{2} & \frac{1}{6} d t^{3} \\ 0 & 1 & d t & \frac{1}{2} d t^{2} \\ 0 & 0 & 1 & d t \\ 0 & 0 & 0 & 1 \end{array}]

$\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.

— Jason R.
quelle

Danke, du hast wirklich ein Licht in meinem Kopf angezündet. Aber ich verstehe immer noch nicht, was meine Beschleunigung im nächsten Schritt erwartet (wenn nicht konstant) - dies ist wichtig, um die Übergangsmatrix A zu erstellen, die letzte Zeile, um genau zu sein. Was würdest du vorschlagen?

— Primož Kralj

Es gibt ein ganzes Durcheinander mit migrierenden Fragen. Ein Teil Ihrer Antwort ist weg. Ich hoffe Mods reparieren das. Schauen Sie sich auch Addition 2 in meiner ursprünglichen Frage an.

— Primož Kralj

Ich konnte den bearbeiteten Text wiederherstellen und erneut auf meine Antwort anwenden.

— Jason R

Können Sie erklären, wie Sie die Prozesskovarianz für die erste Lösung (3 Zustandsvariablen) erhalten? Ich kann online keinen anständigen Link finden, der dies in der Praxis gründlich erklären würde.

— Primož Kralj

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 = [\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}]

$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.

— Peter K.
quelle

Schöne Erklärung - aber warum wählen Sie 0,001, warum nicht zum Beispiel 2,84?

— Primož Kralj

Vielen Dank! Der Grund für die Wahl von 0,00001 gegenüber 2,84: Nun, 2,84 ist für eine Varianz SEHR groß. Ich nahm an, dass Sie eine gültige Begründung für die Ruckzahl (0,001) hatten und dass die anderen Zahlen deutlich kleiner sein sollten. Wenn es keinen wirklichen Grund für die Wahl der Rucknummer gibt, versuchen Sie es mit dem gleichen Wert in allen diagonalen Elementen.

— Peter K.

Aber was bedeutet das in der Praxis? Wenn ich sage, mein Positionssensor hat eine Varianz von 0,00001 (erstes diag. Element in der Q-Matrix), bedeutet das, dass der Fehler beispielsweise + - 0,00001 Meter beträgt (ich denke nicht)? Ich denke, Sie wählen eine kleinere Varianz für einen Sensor, der präziser ist (Position), und eine größere Varianz für einen Sensor mit weniger Präzision (Beschleunigung).

— Primož Kralj

0.00001

$0.00001$

\sqrt{0.00001}

$\sqrt{0.00001}$

\sqrt{0.00001}

$\sqrt{0.00001}$

2 \sqrt{0.00001}

$2\sqrt{0.00001}$

Vielen Dank an beide, ich hoffe, dieser Thread wird auch zukünftigen Besuchern helfen.

— Primož Kralj

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

— Jm Rams
quelle