[PYTHON] Nichtlinearer Kalman-Filter (1)

Über diese Serie

Da ich den Kalman-Filter in einem Buch auf nichtlineare Phänomene untersucht habe, werde ich ihn auch als Memo zusammenfassen. Das Kalman-Filter ist als Algorithmus zur sequentiellen Optimierung und Schätzung des Zustandsvektors eines linearen Wahrscheinlichkeitssystems auf der Grundlage von Beobachtungsdaten bekannt. Das Kalman-Filter nimmt eine Linearität in der zeitlichen Entwicklung des Zustandsvektors und der Beobachtung des Zustandsvektors an. Daher ist ein gewisser Einfallsreichtum erforderlich, um es auf nichtlineare Phänomene anzuwenden. In dieser Reihe werden wir vom linearen Kalman-Filter zur Implementierung des nichtlinearen Kalman-Filters übergehen. Das Referenzmaterial ist [Nichtlinearer Kalman-Filter (Toru Katayama)](https://www.amazon.co.jp/%E9%9D%9E%E7%B7%9A%E5%BD%A2%E3%82%AB % E3% 83% AB% E3% 83% 9E% E3% 83% B3% E3% 83% 95% E3% 82% A3% E3% 83% AB% E3% 82% BF-% E7% 89% 87% E5% B1% B1-% E5% BE% B9 / dp / 4254201486).

Betrachten Sie diesmal den Fall, in dem sowohl der Zustandsübergang als auch die Beobachtung linear sind (kann durch Matrixberechnung beschrieben werden). TL;DR Der Code (Jupyter-Notizbuch) ist hier https://github.com/Kosuke-Szk/nonlinear_kasmanfilter/blob/master/ex2.ipynb

Lineares Wahrscheinlichkeitssystem, dargestellt durch ein Zustandsraummodell

\begin{align}
x_{t+1} = F_t x_t + G_t w_t \\
y_t = H_tx_t + v_t\\
\end{align}

Nachdenken über. Wobei $ x_t \ in \ mathbb {R} ^ n $ der Zustandsvektor ist, $ y_t \ in \ mathbb {R} ^ p $ der Beobachtungsvektor ist, $ w_t \ in \ mathbb {R} ^ m $, $ v_t \ in \ mathbb {R} ^ p $ ist ein Gaußscher Rauschvektor, und $ F_t \ in \ mathbb {R} ^ {n \ times n} $ ist eine Übergangsmatrix, $ G_t \ in \ mathbb {R} ^ {n \ times m} $ heißt Fahrmatrix und $ H_t \ in \ mathbb {R} ^ {p \ times n} $ heißt Beobachtungsmatrix. Zusätzlich führen wir $ P, Q, R $ als Fehlerkovarianzmatrizen für $ x_t $, $ w_t $ bzw. $ v_t $ ein. Beeilen Sie sich, Sie können einen Kalman-Filter erstellen, indem Sie die folgenden Formeln implementieren. Der Index $ t / t-1 $ repräsentiert die Operation (Vorhersage) zum Schätzen des Zeitzustands $ t $ unter Verwendung der Informationen der Zeit $ t-1 $, und $ t / t $ verwendet die Informationen der aktuellen Zeit. Stellt die Operation (Filterung) dar, die den Zustand optimiert, und $ t / N $ repräsentiert die Operation (Glättung), die die Zeit $ t $ optimiert, wenn die Vorhersage bis zur Zeit $ N $ abgeschlossen ist.

Kalman Filter

1. Setzen Sie den Anfangswert auf $ \ hat {x} \ _ {0 / -1} = \ bar {x} _0 $, $ P \ _ {0 / -1} = P \ _0 $, $ t = 0 Sagen wir $.
2. Beobachtungsaktualisierungsschritt

a. Kalman-Gewinn $K_t = P\_{t/t-1}H_t^T [H_t P\_{t/t-1} H_t^T + R_t]^{-1}$ b. Filterschätzungen $ \hat{x}\_{t/t} = \hat{x}\_{t/t-1} + K_t [y_t - H_t \hat{x}\_{t/t-1}]$ c. Filterschätzungsfehler-Kovarianzmatrix $ P\_{t/t} = P\_{t/t-1} - K_t H_t P\_{t/t-1} $

3. Zeitaktualisierungsschritt

a. Voraussichtlicher Wert einen Schritt voraus $ \hat{x}\_{t+1/t} = F_t \hat{x}\_{t/t}$ b. Vorhersagefehler-Kovarianzmatrix $ P\_{t+1/t} = F_t P\_{t/t} F_t^T + G_t Q_t G_t^T $ 4. Kehren Sie zu Schritt 2 als $ t \ leftarrow t + 1 $ zurück.

Kalman Smoother

\begin{align}
\hat{x}_{t/N} &= \hat{x}_{t/t} + C_t(\hat{x}_{t+1/N} - \hat{x}_{t+1/t})\\
C_t &= P_{t/t} F_t^T P_{t+1/t}^{-1} \\
P_{t/N} &= P_{t/t} + C_t [P_{t+1/N}-P_{t+1/t}]C_t^T\\
\end{align}

Implementierung

Die sequentielle Vorhersage eines dynamischen linearen Systems wird unter Verwendung des Kalman-Filters durchgeführt. Wie im Beispiel auf S. 56 des Referenzmaterials gezeigt, wird ein vierdimensionales System betrachtet, das die Rotationsbewegung eines künstlichen Satelliten linear approximiert.

\begin{align}
\dot{x}_t^{(1)} &= x_t^{(2)}\\
\dot{x}_t^{(2)} &= x_t^{(3)} + x_t^{(4)}\\
\dot{x}_t^{(3)} &= 0\\
\dot{x}_t^{(1)} &= -0.5 x_t^{(4)} + \xi_t \\
\end{align}

$ {X} \ _ t ^ {(1)} $: Einstellungswinkel des künstlichen Satelliten, $ {x} \ _ t ^ {(2)} $: Winkelgeschwindigkeit, $ {x} \ _ t ^ {(3)} $: Durchschnittliche Komponente der Winkelbeschleunigung, $ {x} \ _ t ^ {(4)} $: Zufällige Komponente der Winkelbeschleunigung, $ \ xi_t $ ist Gaußsches Rauschen mit durchschnittlichem 0. Abtastintervall $ \ Delta = 1.0 Die zeitliche Entwicklung wird unten beschrieben, wenn sie mit $ dezentralisiert wird.

x_{t+1} = \left(
    \begin{array}{ccc}
      1 & 1 & 0.5 & 0.5\\
      0 & 1 & 1 & 1\\
      0 & 0 & 1 & 0\\
      0 & 0 & 0 & 0.606
    \end{array}
  \right) x_t + \left(
    \begin{array}{ccc}
0\\
0\\
0\\
1
    \end{array}
  \right) w_t

Die Verteilung des Gaußschen Rauschens $ w_t und v_t $ beträgt jedoch $ Q = 0,64 \ mal 10 ^ {-2} $, $ R = 1 $. Der Anfangswert des Zustandsvektors und der Anfangswert des geschätzten Wertes

x_0 = \left(\begin{array}{ccc}
1.25\\
0.06\\
0.01\\
-0.003
\end{array}
\right),

\hat{x}_{0/-1} = \left(\begin{array}{ccc}
0\\
0\\
0\\
0
\end{array}
\right),

P_{0/-1} = \rm{diag}[10,10,10,10]

Die Simulation wird durchgeführt als. Das Ergebnis der Satellitenwinkelwinkelschätzung ist in der folgenden Abbildung dargestellt.

Die Filterschätzungen auf der grünen Linie werden stark von den beobachteten Werten beeinflusst, aber die Glättungsschätzungen auf der roten Linie scheinen sich der tatsächlichen Bewegung des Lagewinkels zu nähern.

Der Code (Jupyter-Notizbuch) ist hier https://github.com/Kosuke-Szk/nonlinear_kasmanfilter/blob/master/ex2.ipynb

Eine etwas detailliertere Geschichte

Coming soon