Filtre de Kalman

${\displaystyle {\hat {\textbf {x}}}_{k|k-1}={\textbf {F}}_{k}{\hat {\textbf {x}}}_{k-1|k-1}+{\textbf {B}}_{k}{\textbf {u}}_{k}}$ (état prédit)

${\displaystyle {\textbf {P}}_{k|k-1}={\textbf {F}}_{k}{\textbf {P}}_{k-1|k-1}{\textbf {F}}_{k}^{T}+{\textbf {Q}}_{k}}$ (estimation prédite de la covariance)

avec :

• ${\displaystyle {\textbf {F}}_{k}}$ : matrice qui relie l'état précédent ${\displaystyle k-1}$ à l'état actuel ${\displaystyle k}$
• ${\displaystyle {\textbf {u}}_{k}}$ : entrée de commande
• ${\displaystyle {\textbf {B}}_{k}}$ : matrice qui relie l'entrée de commande ${\displaystyle \mathbf {u} _{k}}$ à l'état ${\displaystyle \mathbf {x} _{k}}$
• ${\displaystyle {\textbf {P}}_{k-1|k-1}}$ : matrice de covariance a posteriori estimée à l'état précédent ${\displaystyle k-1}$
• ${\displaystyle {\textbf {Q}}_{k}}$ : matrice de covariance du bruit d'évolution

${\displaystyle {\tilde {\textbf {y}}}_{k}={\textbf {z}}_{k}-{\textbf {H}}_{k}{\hat {\textbf {x}}}_{k|k-1}}$ (innovation)

${\displaystyle {\textbf {S}}_{k}={\textbf {H}}_{k}{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}+{\textbf {R}}_{k}}$ (covariance de l'innovation)

${\displaystyle {\textbf {K}}_{k}={\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {S}}_{k}^{-1}}$ (gain de Kalman)

${\displaystyle {\hat {\textbf {x}}}_{k|k}={\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}{\tilde {\textbf {y}}}_{k}}$ (état mis à jour)

${\displaystyle {\textbf {P}}_{k|k}=(I-{\textbf {K}}_{k}{\textbf {H}}_{k}){\textbf {P}}_{k|k-1}}$ (covariance mise à jour)

avec :

• ${\displaystyle {\textbf {z}}_{k}}$ : observation ou mesure du processus à l'instant k
• ${\displaystyle {\textbf {H}}_{k}}$ : matrice qui relie l'état ${\displaystyle {\textbf {x}}_{k}}$ à la mesure ${\displaystyle {\textbf {z}}_{k}}$
• ${\displaystyle {\textbf {P}}_{k|k}}$ : matrice d'estimation a posteriori de la covariance de l'erreur
• ${\displaystyle {\textbf {R}}_{k}}$ : matrice de covariance du bruit de mesure
• ${\displaystyle {I}}$ : matrice identité aux dimensions adéquates

Le gain de Kalman est choisi pour minimiser la valeur de la covariance a posteriori de l'estimateur.

Dans le filtre de Kalman étendu (FKE), les modèles d'évolution et d'observation n'ont pas besoin d'être des fonctions linéaires de l'état mais peuvent à la place être des fonctions (différentiables).

${\displaystyle {\textbf {x}}_{k}=f({\textbf {x}}_{k-1},{\textbf {u}}_{k},{\textbf {w}}_{k})}$

${\displaystyle {\textbf {z}}_{k}=h({\textbf {x}}_{k},{\textbf {v}}_{k})}$

La fonction f peut être utilisée pour calculer l'état prédit à partir de l'état estimé précédent et, semblablement, la fonction h peut être employée pour calculer l'observation prédite de l'état prédit. Cependant, f et h ne peuvent pas être appliqués directement au calcul de la covariance : une matrice des dérivées partielles, la Jacobienne, est calculée.

À chaque instant, la Jacobienne est évaluée avec les états estimés courants. Ces matrices peuvent être employées dans les équations du filtre de Kalman. Ce processus linéarise essentiellement la fonction non linéaire autour de l'estimation courante.

Ceci donne les équations du filtre de Kalman étendu suivantes :

Prédiction

${\displaystyle {\hat {\textbf {x}}}_{k|k-1}=f({\hat {\textbf {x}}}_{k-1|k-1},{\textbf {u}}_{k},0)}$

${\displaystyle {\textbf {P}}_{k|k-1}={\textbf {F}}_{k}{\textbf {P}}_{k-1|k-1}{\textbf {F}}_{k}^{T}+{\textbf {Q}}_{k}}$

Mise à jour

${\displaystyle {\tilde {\textbf {y}}}_{k}={\textbf {z}}_{k}-h({\hat {\textbf {x}}}_{k|k-1},0)}$

${\displaystyle {\textbf {S}}_{k}={\textbf {H}}_{k}{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}+{\textbf {R}}_{k}}$

${\displaystyle {\textbf {K}}_{k}={\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {S}}_{k}^{-1}}$

${\displaystyle {\hat {\textbf {x}}}_{k|k}={\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}{\tilde {\textbf {y}}}_{k}}$

${\displaystyle {\textbf {P}}_{k|k}=(I-{\textbf {K}}_{k}{\textbf {H}}_{k}){\textbf {P}}_{k|k-1}}$

Où les matrices de transition et d'observation sont définies comme étant les Jacobiennes suivantes :

${\displaystyle {\textbf {F}}_{k}=\left.{\frac {\partial f}{\partial {\textbf {x}}}}\right\vert _{{\hat {\textbf {x}}}_{k-1|k-1},{\textbf {u}}_{k}}}$

${\displaystyle {\textbf {H}}_{k}=\left.{\frac {\partial h}{\partial {\textbf {x}}}}\right\vert _{{\hat {\textbf {x}}}_{k|k-1}}}$

Remarque : la convergence de ce filtre n'est aucunement assurée car il s'agit d'une convergence locale. En fait, il existe de nombreux exemples pour lesquels la convergence du filtre dépend de l'initialisation de l'état à l'instant initial.

Le filtre de Kalman sans parfum (Unscented Kalman filter, UKF)[7], procède à une approximation de la densité a posteriori par une gaussienne comme dans le filtre de Kalman étendu. Mais plutôt que de faire une approximation des fonctions non linéaires du modèle d'évolution et du modèle de mesure, il réalise une approximation de la densité de probabilité par un ensemble de points pondérés convenablement choisis de façon déterministe. Ces points sont transformés par les fonctions non linéaires d'évolution et de mesure afin d'obtenir une nouvelle densité de probabilité. Cette approximation est appelée la transformée sans parfum (Unscented Transform).

Les modèles d'évolution et de mesure sont les mêmes que celles du filtre de Kalman étendu.

L'algorithme du filtre de Kalman sans parfum est le suivant :

Calcul des points d'approximation

${\displaystyle s_{k-1}^{a}=[{\hat {x}}_{k-1}^{a}\qquad ({\hat {x}}_{k-1}^{a}\dots {\hat {x}}_{k-1}^{a})+{\sqrt {(n+\lambda )P_{k-1}^{a}}}\qquad ({\hat {x}}_{k-1}^{a}\dots {\hat {x}}_{t-1}^{a})-{\sqrt {(n+\lambda )P_{k-1}^{a}}}]}$

avec :

${\displaystyle {\hat {x}}_{k-1}^{a}=[{\hat {x}}_{k-1|k-1}^{T}\quad 0_{dim(w_{k}-1)}^{T}]}$

et

${\displaystyle P_{k-1}^{a}={\begin{pmatrix}P_{k-1|k-1}&0\\0&Q_{k-1}\end{pmatrix}}}$

Calcul des poids associés

Ceci est réalisé par l'usage de la transformée sans parfum.

Prédiction

${\displaystyle s_{k|k-1}^{x}=f_{k-1}(s_{k-1}^{x},s_{k-1}^{w})}$

${\displaystyle {\hat {x}}_{k|k-1}=\sum \limits _{i=0}^{2n}W_{i}s_{k|k-1}^{x}}$

${\displaystyle P_{k|k-1}=\sum \limits _{i=0}^{2n}W_{i}(s_{k|k-1}^{x}-{\hat {x}}_{k|k-1})(s_{k|k-1}^{x}-{\hat {x}}_{k|k-1})^{T}+Q_{k}}$

${\displaystyle \mathrm {Z} _{i,k|k-1}^{x}=h_{t}(s_{k|k-1}^{x})}$

${\displaystyle {\hat {z}}_{k|k-1}=\sum \limits _{i=0}^{2n}W_{i}\mathrm {Z} _{i,k|k-1}^{x}}$

Estimation

${\displaystyle S_{k}=P_{\nu _{k}\nu _{k}}=R_{k}+\sum \limits _{i=0}^{2n}W_{i}(\mathrm {Z} _{i,k|k-1}^{x}-{\hat {z}}_{k|k-1})(\mathrm {Z} _{i,k|k-1}^{x}-{\hat {z}}_{k|k-1})^{T}}$

${\displaystyle K_{k}=P_{x_{k}z_{k}}S_{k}^{-1}=\sum \limits _{i=0}^{2n}W_{i}(s_{j}-{\hat {x}}_{k|k-1})(\mathrm {Z} _{i,k|k-1}^{x}-{\hat {z}}_{k|k-1})^{T}S_{k}^{-1}}$

${\displaystyle {\hat {x}}_{k|k}={\hat {x}}_{k|k-1}+K_{k}(z_{k}-{\hat {z}}_{k|k-1})}$

${\displaystyle P_{k|k}=P_{k|k-1}-K_{k}S_{k}K_{k}}$

Le filtre de Kalman sans parfum fournit une alternative intéressante au filtre de Kalman étendu et donne dans plusieurs cas de meilleurs résultats que ce dernier, pour une complexité équivalente[8]. Cependant, ce filtre montre ses limites dans le cas de systèmes fortement non linéaires et/ou non gaussiens[9] - [10].