[PYTHON] Filtre de Kalman non linéaire (1)
À propos de cette série
Puisque j'ai étudié le filtre de Kalman pour les phénomènes non linéaires dans un livre, je vais également le résumer sous forme de mémo. Le filtre de Kalman est connu comme un algorithme d'optimisation séquentielle et d'estimation du vecteur d'état d'un système de probabilité linéaire basé sur des données d'observation. Le filtre de Kalman suppose une linéarité dans l'évolution temporelle du vecteur d'état et l'observation du vecteur d'état. Par conséquent, une certaine ingéniosité est nécessaire pour l'appliquer à des phénomènes non linéaires. Dans cette série, nous passerons du filtre de Kalman linéaire à la mise en œuvre du filtre de Kalman non linéaire. Le matériel de référence est [Non-linear 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).
Cette fois, considérons le cas où la transition d'état et l'observation sont linéaires (peut être décrit par un calcul matriciel). TL;DR Le code (notebook Jupyter) est ici https://github.com/Kosuke-Szk/nonlinear_kasmanfilter/blob/master/ex2.ipynb
Système de probabilité linéaire représenté par un modèle d'espace d'états
\begin{align}
x_{t+1} = F_t x_t + G_t w_t \\
y_t = H_tx_t + v_t\\
\end{align}
Penser à. Où $ x_t \ in \ mathbb {R} ^ n $ est le vecteur d'état, $ y_t \ in \ mathbb {R} ^ p $ est le vecteur d'observation, $ w_t \ in \ mathbb {R} ^ m $, $ v_t \ in \ mathbb {R} ^ p $ est un vecteur de bruit gaussien, et $ F_t \ in \ mathbb {R} ^ {n \ times n} $ est une matrice de transition, $ G_t \ in \ mathbb {R} ^ {n \ times m} $ est appelée la matrice de pilotage, et $ H_t \ in \ mathbb {R} ^ {p \ times n} $ est appelée la matrice d'observation. De plus, nous introduisons $ P, Q, R $ comme matrices de covariance d'erreur pour $ x_t $, $ w_t $, $ v_t $ respectivement. Dépêchez-vous, vous pouvez créer un filtre Kalman en implémentant les formules suivantes. L'indice $ t / t-1 $ représente l'opération (prédiction) d'estimation de l'état du temps $ t $ en utilisant les informations de temps $ t-1 $, et $ t / t $ utilise les informations de l'heure courante. Représente l'opération pour optimiser l'état (filtrage). $ T / N $ représente l'opération pour optimiser le temps $ t $ (lissage) lorsque la prédiction est terminée jusqu'au temps $ N $.
Filtre de Kalman
- Définissez la valeur initiale sur $ \ hat {x} \ _ {0 / -1} = \ bar {x} _0 $, $ P \ _ {0 / -1} = P \ _0 $, $ t = 0 Disons $.
- Étape de mise à jour des observations
a. Gain de Kalman
- Étape de mise à jour de l'heure
a. Valeur prévue une longueur d'avance
Kalman plus lisse
\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}
la mise en oeuvre
La prédiction séquentielle d'un système linéaire dynamique est effectuée à l'aide du filtre de Kalman.Comme le montre l'exemple de la page 56 du matériau de référence, considérons un système à quatre dimensions qui se rapproche linéairement du mouvement de rotation d'un satellite artificiel.
\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}
Cependant, $ {x} \ _ t ^ {(1)} $: angle d'attitude du satellite artificiel, $ {x} \ _ t ^ {(2)} $: vitesse de l'angle, $ {x} \ _ t ^ {(3)} $: Composante moyenne de l'accélération angulaire, $ {x} \ _ t ^ {(4)} $: Composante aléatoire de l'accélération angulaire, $ \ xi_t $ est un bruit gaussien avec une moyenne de 0. Intervalle d'échantillonnage $ \ Delta = 1.0 L'évolution temporelle est décrite ci-dessous lorsqu'elle est discrète avec $.
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
Cependant, la distribution du bruit gaussien $ w_t et v_t $ est $ Q = 0,64 \ fois 10 ^ {-2} $, $ R = 1 $. La valeur initiale du vecteur d'état et la valeur initiale de la valeur estimée
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]
La simulation est effectuée comme suit: Le résultat de l'estimation de l'angle d'attitude du satellite est montré dans la figure ci-dessous.
Les estimations de filtre sur la ligne verte sont fortement influencées par les valeurs observées, mais les estimations de lissage sur la ligne rouge semblent se rapprocher du mouvement réel de l'angle d'attitude.
Le code (notebook Jupyter) est ici https://github.com/Kosuke-Szk/nonlinear_kasmanfilter/blob/master/ex2.ipynb
Une histoire un peu plus détaillée
Coming soon