Slam visuel Leçon 14 leçon 9 filtre Kalman

1. Qu'est - ce que le filtre Kalman

Définition de l'encyclopédie Baidu：Filtre Kalman（Kalman filtering）C'est unEn utilisant l'équation d'état du système linéaire,Les données d'observation sont entrées et sorties par le système,Estimation optimale de l'état du systèmeAlgorithme.Comme les données d'observation incluent les effets du bruit et des interférences dans le système,Donc l'estimation optimale peut aussi être considérée comme un processus de filtrage.

Il y a un peu de rhétorique pour résumer：

Le filtre Kalman est une méthode d'estimation optimale de l'état du système.

2. SLAMEstimation de l'état de

Dans l'optimisation de l'arrière - plan,Nous pouvons nous baser sur le moment considéré（Nombre de cadres）La quantité d'informations à classer：

• kL'état du moment est lié à l'information de tous les moments précédents,Et même les mettre à jour avec des informations futures,Ce traitement s'appelle“Lot（Batch）”.Cette méthode de traitement est couramment utilisée pour l'optimisation non linéaire.
• kL'état du moment ne correspond qu'àk-1L'état du moment est lié à,C'est - à - dire la nature markovienne,Et c'est le premier degré de Markov.Ce traitement s'appelle la méthode du filtre（Filtre Kalman）.

3. Dérivation du filtre Kalman

Idées de base：Rappelez - vous que le filtre Kalman est la meilleure façon d'estimer l'état du système,Et en utilisant l'équation d'état du système linéaire.

Première étape：Estimation du Statut：

Selon l'équation d'état de l'équation de mouvement （Équation9.3）, Nous utilisons la loi bayésienne 0Àk Pour estimer k Distribution de l'état du temps
$P(x_k|x_0,u_{1:k},z_{1:k})\propto P(z_k|x_k)P(x_k|x_0,u_{1:k},z_{1:k-1})$
À gauche. k Distribution de l'état du temps （Distribution a posteriori, Il est basé sur $z_k$ Observation des données pour déterminer l'état , C'est la connaissance de la cause ）. Le premier élément à droite est la probabilité （ Connaître les causes et les résultats ）, Le deuxième élément est a priori （ Causes historiques ）：
$AprèsOui.\propto Oui.Mais\times D\text{'}abord.Oui.$
Notre filtre Kalman , Ce qu'il faut, c'est cette formule , Nous voulons estimer k Distribution a posteriori du temps , Alors, il suffit de trouver k La probabilité et l'a priori du moment peuvent être , Gardez cette formule à l'esprit tout au long du processus de dérivation , Pour ne pas se perdre dans la dérivation .（ On peut même penser grossièrement que , Le filtre Kalman est une matrice ,k-1 A posteriori du moment x,AdoptionAx=b On va trouver k Distribution a posteriori du temps .）

Et puis, Nous allons a priori $x_{k-1}$Le temps se développe comme une probabilité conditionnelle：

$xHeureGravureDeD\text{'}abord.Oui.P(A|B)=\int P(A|B,x_{k-1})P(x_{k-1}|B)d{x}_{k-1}$
Ensuite, nous supposons que Markov est , Simplifier cette formule , On obtient une relation abstraite ：
$xHeureGravureDeD\text{'}abord.Oui.=kHeureGravureDeTransportBougeFangCheng\times (k-1)HeureGravureDeFormeÉtatPointsTissu（AprèsOui.）$
Ce que ces deux étapes veulent illustrer , Ce qu'on fait, c'est “Comment mettrek-1La distribution de l'état du temps est dérivée dekLe moment”,C'est - à - dire que nous utilisonsk-1 Distribution a posteriori du temps ,Calculk A priori du moment ,Enfin.kA posteriori du moment（Répartition par État）.Il suffit de maintenir une quantité d'état（A posteriori）, Il suffit d'itérer et de mettre à jour . Les preuves suivantes n'utiliseront pas ces deux étapes ,Supposons quek-1 Le moment est connu a posteriori , La moyenne de la distribution a posteriori est de ${\hat{x}}_{k-1}$, La covariance de la distribution postérieure est ${\hat{P}}_{k-1}$

Deuxième étape：Système gaussien linéaire

Nous savons que le filtre Kalman utilise l'équation d'état du système linéaire , Nous supposons donc que les équations de mouvement et d'observation peuvent être décrites par des équations linéaires ：
$\{\begin{array}{l}{x}_k=A_k{x}_{k-1}+u_k+w_k\\ z_k=C_k{x}_k+v_k\end{array}k=1,...,N$
Le bruit ici obéit à une distribution gaussienne à matrice nulle $w_k\sim N(0,R).v_k\sim N(0,Q)$
En fait,, Équation du mouvement l'information cachée ici est ：k A priori du moment =k-1 Cartographie linéaire a posteriori du temps

Troisième étape：Prévisions

Prévisions, Ça veut dire comment passer de l'état du moment précédent （ C'est - à - dire a posteriori ,Supposons qu'il soit connu）, Selon les informations saisies （Il y a du bruit.,Obéissez à la distribution gaussienne） Inférez la distribution de l'état au moment actuel （A priori）. Ensuite, selon l'équation linéaire, c'est - à - dire l'équation du mouvement et l'annexe A.3（ Système gaussien composé plus 、 Variation de la moyenne et de la covariance après multiplication ）,Vous pouvezk La distribution a priori du temps est calculée ：
$P(x_k|x_0,u_{1:k},z_{1:k-1})=N(A_k{\hat{x}}_{k-1}+u_k,A_k{\hat{P}}_{k-1}{A}_k^{\top }+R)$
Nous prenons les deux équations de la prédiction comme prédiction ：（k A priori du moment =Linéariték-1A posteriori du moment）
${\check{x}}_k=A_k{\hat{x}}_{k-1}+uk,{\check{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^{\top }+R$

Quatrième étape： Dérivation de la distribution a posteriori du gain de Kalman

1. Nous savons comment calculer la distribution a priori , Ensuite, je cherche la probabilité . Selon l'équation d'observation et l'annexe A.3（ Propriétés de la recombinaison des systèmes Gaussiens ） Trouver la moyenne et la covariance de la probabilité ：
   $P(z_k|x_k)=N(C_k{x}_k,Q)$
   Quant à la raison pour laquelle la covariance n'est pas une annexe ici 4.3 La forme en est $C_k{\hat{P}}_k{C}_k^{\top }+Q$,Cette question,Blogszkk9527Une explication est donnée.Et je comprends que, Je passe en revue tout le processus de dérivation ,$x_k$（ Ni a posteriori ni a priori ） Est utilisé comme quantité de référence ,Comparer avec$x_k$ Les coefficients du terme primaire et du terme quadratique sont corrélés pour obtenir la prédiction et le gain Kalman . Il n'existe pas dans notre dernier filtre Kalman 5 Parmi les formules ,Donc ici$x_k$Est une valeur constante, S'il s'agit d'une valeur constante, il n'y a pas de covariance , Et la moyenne est égale à elle - même .
2. Maintenant il y a une probabilité de somme a priori , On multiplie les deux éléments et on obtient kA posteriori du moment：
   $N({\hat{x}}_k,{\hat{P}}_k)=\eta N(C_k{x}_k,Q)\cdot N({\check{x}}_k,{\check{P}}_k)$
3. Nous savons déjà que les deux côtés de l'équation sont des distributions gaussiennes , Il suffit donc de comparer la partie exponentielle , Il n'est pas nécessaire de comparer la partie factorielle devant la distribution gaussienne （Je crois comprendre que, Regardez dans le livre P236Équation9.5 La formule bayésienne dans ,DénominateurevidenceC'est - à - dire:$P(A|B)=\frac{P(B|A)P(A)}{P(B)}$Dans$P(B)$Omis, L'équation est représentée par un symbole proportionnel à . Après avoir déduit la moyenne et la covariance de la distribution gaussienne des deux côtés , Et remplacer le signe proportionnel par le signe égal , Ajout d'un facteur d'échelle $\eta$, Donc techniquement, ici $\eta$,Juste devantevidenceCompte à rebours, Même si je le savais. , La dérivation du livre suppose encore que les deux facteurs sont égaux ,Je ne sais pas pourquoi.）, Donc nous avons élargi la partie exponentielle ：

Jusqu'ici., Nous avons déduit tout le processus du filtre Kalman .Encore un résumé, Après déduction du filtre Kalman 5Formule（2 Les prédictions sont la moyenne et la covariance a priori ,1 Un gain Kalman , Moyenne et covariance des distributions a posteriori ）：

• Prévisions：
  - ${\check{x}}_k=A_k{\hat{x}}_{k-1},{\check{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^{\top }+R$
• Mise à jour
  • Gain Kalman：
    • $K={\check{P}}_k{C}_k^{\top }(C_k{\check{P}}_k{C}_k^{\top }+Q_k{)}^{-1}$
  • A posteriori：
    • ${\hat{x}}_k={\check{x}}_k+K(z_k-C_k{\check{x}}_k)$
    • ${\hat{P}}_k=(I-K{C}_k){\check{P}}_k$

Comment améliorer la mémoire ？
k Somme a priori des moments k-1 Une cartographie linéaire a posteriori du temps ;k La moyenne postérieure est égale au produit de la moyenne antérieure plus un gain Kalman et une erreur d'observation .

Procédure simplifiée：

1. Estimation du Statut： L'estimation de l'état obtenue par le théorème bayésien est a posteriori proportionnelle à la probabilité ×A priori
2. Système linéaire： Formulation d'équations linéaires pour les équations de mouvement et d'observation
3. Prévisions： La dérivation a priori de l'équation bayésienne , En utilisant les propriétés Additives des équations linéaires de mouvement et des équations gaussiennes
4. Calcul du gain de Kalman：
   1. Calculer la probabilité en utilisant la propriété de distribution gaussienne de l'équation d'observation .
   2. A posteriori=Probabilité×A priori
   3. Comparer les parties exponentielles des deux côtés de l'équation , Obtenir le gain Kalman KDéfinitions（Le dernier.KAvecSMW Faire l'égalité ）
5. Mise à jour： En utilisant l'équation a posteriori et le gain Kalman , On déduit les équations de moyenne et de covariance a posteriori

4. Filtre Kalman étendu

Nous savons que, Le filtre Kalman utilise des équations de systèmes linéaires ,EtSLAM Les équations de mouvement et d'observation sont généralement non linéaires , En particulier, la position et la posture représentées par le modèle paramétrique interne de la caméra et l'algèbre de lie sont ajoutées ,Encore moins un système linéaire.Une distribution gaussienne,Après transformation non linéaire,Souvent, ce n'est plus une distribution gaussienne.

Nous devons donc étendre les résultats du filtre Kalman aux systèmes non linéaires .Le traitement est： L'expansion Taylor de premier ordre de l'équation de mouvement et de l'équation d'observation est considérée près d'un point , Ne gardez que les termes du premier ordre , C'est - à - dire la partie linéaire . Ensuite, le système linéaire est déduit .En général, Est d'approximation de la non - linéarité à la linéarité , Ensuite, en utilisant une dérivation similaire, on déduit un filtre Kalman étendu .

Processus：

1. Non linéaire à linéaire ： Équation du Mouvement （xk-1） Et les équations d'observation （xk）Premier ordre Taylor expansion
2. Estimation du Statut： C'est - à - dire l'équation bayésienne postérieure ,A posteriori=nProbabilité×A priori
3. Système linéaire： La première étape a été donnée
4. Prévisions： En utilisant des équations de mouvement linéaire et xk-1 La somme postérieure du temps et la loi complexe gaussienne ,Allez.k A priori du moment
5. Déduction du gain de Kalman： Calculer la probabilité , On obtient les résultats des deux côtés de l'équation bayésienne . Comparer la section index , Définir le gain Kalman
6. Mise à jour： Obtenir une cartographie a posteriori de la relation a priori