# Filtre de Kalman

En statistique et en théorie du contrôle, le filtre de Kalman est un filtre à réponse impulsionnelle infinie qui estime les états d'un système dynamique à partir d'une série de mesures incomplètes ou bruitées. Le filtre a été nommé d'après le mathématicien et informaticien américain d'origine hongroise Rudolf Kálmán.

## Exemples d'applications

Le filtre de Kalman est utilisé dans une large gamme de domaines technologiques (radar, vision électronique, communication…). C'est un thème majeur de l'automatique et du traitement du signal. Un exemple d'utilisation peut être la mise à disposition, en continu, d'informations telles que la position ou la vitesse d'un objet à partir d'une série d'observations relatives à sa position, incluant éventuellement des erreurs de mesures.
Par exemple, pour le cas des radars où l'on désire suivre une cible, des données sur sa position, sa vitesse et son accélération sont mesurées à chaque instant mais avec énormément de perturbations dues au bruit ou aux erreurs de mesure. Le filtre de Kalman fait appel à la dynamique de la cible qui définit son évolution dans le temps pour obtenir de meilleures données, éliminant ainsi l'effet du bruit. Ces données peuvent être calculées pour l'instant présent (filtrage), dans le passé (lissage), ou sur un horizon futur (prédiction).
Le filtrage de Kalman est aussi de plus en plus utilisé en dehors du domaine de l'électronique, par exemple en météorologie et en océanographie, pour l'assimilation de données dans un modèle numérique, en finance ou en navigation et il est même utilisé dans l'estimation des états de trafic routier dans le cas de commande par rampe d'accès où le nombre de boucles magnétiques sur la route est insuffisant.

## Paternité

Le filtre de Kalman doit son nom à Rudolf Kalman bien que Thorvald Nicolai Thiele et Peter Swerling aient développé un algorithme similaire avant lui. La paternité du filtre fait l'objet d'une petite controverse dans la communauté scientifique. Le filtre a été décrit dans diverses publications par Swerling (1958), Kalman (1960) et Kalman-Bucy (1961).
Stanley Schmidt est reconnu comme ayant réalisé la première mise en œuvre du filtre. C'était lors d'une visite de Rudolf Kalman au NASA Ames Research Center qu'il vit le potentiel du filtre pour l'estimation de la trajectoire pour le programme Apollo. Ceci conduisit à l'utilisation du filtre dans l'ordinateur de navigation.
Une grande variété de filtres de Kalman a été, depuis, développée à partir de la formulation originale dite filtre de Kalman simple. Schmidt développa le filtre de Kalman étendu, Bierman, Thornton et bien d'autres développèrent toute une gamme de filtres racine carrée. Le filtre le plus utilisé est vraisemblablement la phase-locked loop, largement répandue dans les radios, ordinateurs, équipements de communication, etc.

## Le filtre de Kalman en contexte discret

Le filtre de Kalman en contexte discret est un estimateur récursif : pour estimer l'état courant, seules l'estimation de l'état précédent et les mesures actuelles sont nécessaires. En d'autres termes, l'historique des observations et des estimations n'est ainsi pas requis. Le filtre de Kalman suppose que le processus discret réel ${\textbf {x}}_{k}$ (où $k$ dénote l'indice de temps), suit la loi d'évolution linéaire suivante :
${\textbf {x}}_{k}={\textbf {F}}_{k}{\textbf {x}}_{k-1}+{\textbf {B}}_{k}{\textbf {u}}_{k}+{\textbf {w}}_{k}$
dans laquelle :
* ${\textbf {F}}_{k}$ est la matrice d'évolution, ou matrice de transition, reliant l'état précédent $k-1$ à l'état actuel $k$ ;
* ${\textbf {u}}_{k}$ est la commande en entrée ;
* ${\textbf {B}}_{k}$ est la matrice de contrôle de la commande reliant la commande à l'état $\mathbf {x} _{k}$ ;
* ${\textbf {w}}_{k}$ est le bruit d'évolution, gaussien centré et de matrice de covariance ${\textbf {Q}}_{k}$.
À chaque instant $k$, le processus est observé par des mesures ${\textbf {z}}_{k}$ s'exprimant comme suit :
${\textbf {z}}_{k}={\textbf {H}}_{k}{\textbf {x}}_{k}+{\textbf {v}}_{k}$dans laquelle :
* ${\textbf {H}}_{k}$ est la matrice d'observation ;
* ${\textbf {v}}_{k}$ est le bruit de mesure, gaussien centré et de matrice de covariance ${\textbf {R}}_{k}$.
Ces deux équations sont appelées les équations d'évolution et d'observation du système.
L'état réel ${\textbf {x}}_{k}$ est estimé par 2 variables :
* ${\hat {\textbf {x}}}_{k|k}$, l'estimation de l'état à l'instant k ;
* ${\textbf {P}}_{k|k}$, la matrice de covariance de l'erreur (une mesure de la précision de l'état estimé).
Les notations ${\hat {\textbf {x}}}_{m|n}$ et ${\textbf {P}}_{m|n}$ désignent l'estimation et la covariance de l'estimateur obtenues à l'instant $m$ à partir des observations jusqu'à l'instant $n\leq m$.
Si les bruits d'évolution et de mesure ne sont pas gaussiens, le filtre optimal n'est pas linéaire en général mais, comme on le montre aisément, les formules de récurrence qui suivent, utilisant la matrice de covariance et la moyenne des bruits d'évolution et de mesure, déterminent le meilleur estimateur linéaire possible au sens des moindres carrés. Néanmoins, si les lois de probabilité de ces bruits sont très éloignées de lois gaussiennes, par exemple des lois de Poisson, le filtre linéaire optimal sera de piètre qualité en comparaison du filtre optimal (au sens des moindres carrés), qui est alors non linéaire.
Le filtre de Kalman est composé de deux phases distinctes : la prédiction et la mise à jour. La phase de prédiction utilise l'état estimé de l'instant précédent pour produire une estimation de l'état courant. Dans l'étape de mise à jour, les observations de l'instant courant sont utilisées pour corriger l'état prédit dans le but d'obtenir une estimation plus précise.

### Prédiction

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

### Mise à jour

    ${\tilde {\textbf {y}}}_{k}={\textbf {z}}_{k}-{\textbf {H}}_{k}{\hat {\textbf {x}}}_{k|k-1}$ (innovation)
    ${\textbf {S}}_{k}={\textbf {H}}_{k}{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}+{\textbf {R}}_{k}$ (covariance de l'innovation)
    ${\textbf {K}}_{k}={\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {S}}_{k}^{-1}$ (gain de Kalman)
    ${\hat {\textbf {x}}}_{k|k}={\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}{\tilde {\textbf {y}}}_{k}$ (état mis à jour)
    ${\textbf {P}}_{k|k}=(I-{\textbf {K}}_{k}{\textbf {H}}_{k}){\textbf {P}}_{k|k-1}$ (covariance mise à jour)
avec :
* ${\textbf {z}}_{k}$ : observation ou mesure du processus à l'instant k
* ${\textbf {H}}_{k}$ : matrice qui relie l'état ${\textbf {x}}_{k}$ à la mesure ${\textbf {z}}_{k}$
* ${\textbf {P}}_{k|k}$ : matrice d'estimation a posteriori de la covariance de l'erreur
* ${\textbf {R}}_{k}$ : matrice de covariance du bruit de mesure
* ${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.

## Le filtre d'information

Durrant Whyte a bien traité le filtre informationnel et a montré ses avantages par rapport au filtre de Kalman, il a surtout traité son aspect décentralisé. Dans le cas décentralisé on n'a pas besoin d'une unité centrale de traitement de données, ce qui réduit fortement le temps d'exécution.
Dans le filtre de l'information, la covariance et l'état estimés sont respectivement remplacés par la matrice d'information et le vecteur d'information. Ils sont définis par :
    ${\textbf {Y}}_{k|k}\equiv {\textbf {P}}_{k|k}^{-1}$
    ${\hat {\textbf {y}}}_{k|k}\equiv {\textbf {P}}_{k|k}^{-1}{\hat {\textbf {x}}}_{k|k}$
De même, la covariance et l'état prédits ont les formes d'information équivalentes, définies par :
    ${\textbf {Y}}_{k|k-1}={\textbf {P}}_{k|k-1}^{-1}$
    ${\hat {\textbf {y}}}_{k|k-1}={\textbf {P}}_{k|k-1}^{-1}{\hat {\textbf {x}}}_{k|k-1}$
La covariance et le vecteur de mesure sont définis par :
    ${\textbf {I}}_{k}={\textbf {H}}_{k}^{T}{\textbf {R}}_{k}^{-1}{\textbf {H}}_{k}$
    ${\textbf {i}}_{k}={\textbf {H}}_{k}^{T}{\textbf {R}}_{k}^{-1}{\textbf {z}}_{k}$
La mise à jour de l'information devient maintenant une somme triviale :
    ${\textbf {Y}}_{k|k}={\textbf {Y}}_{k|k-1}+{\textbf {I}}_{k}$
    ${\hat {\textbf {y}}}_{k|k}={\hat {\textbf {y}}}_{k|k-1}+{\textbf {i}}_{k}$
L'avantage principal du filtre de l'information est que N mesures peuvent être filtrées à chaque instant simplement en additionnant leurs matrices et vecteurs de l'information si la matrice R est une matrice diagonale.
    ${\textbf {Y}}_{k|k}={\textbf {Y}}_{k|k-1}+\sum _{j=1}^{N}{\textbf {I}}_{k,j}$
    ${\hat {\textbf {y}}}_{k|k}={\hat {\textbf {y}}}_{k|k-1}+\sum _{j=1}^{N}{\textbf {i}}_{k,j}$
Pour prédire le filtre d'information, la matrice et le vecteur d'information peuvent être convertis de nouveau à leurs équivalents de l'espace d'état ou, alternativement, la prédiction de l'espace d'information peut être utilisée.
    ${\textbf {M}}_{k}=[{\textbf {F}}_{k}^{-1}]^{\text{T}}{\textbf {Y}}_{k-1\mid k-1}{\textbf {F}}_{k}^{-1}$
    ${\textbf {C}}_{k}={\textbf {M}}_{k}[{\textbf {M}}_{k}+{\textbf {Q}}_{k}^{-1}]^{-1}$
    ${\textbf {L}}_{k}=I-{\textbf {C}}_{k}$
    ${\textbf {Y}}_{k|k-1}={\textbf {L}}_{k}{\textbf {M}}_{k}{\textbf {L}}_{k}^{T}+{\textbf {C}}_{k}{\textbf {Q}}_{k}^{-1}{\textbf {C}}_{k}^{T}$
    ${\hat {\textbf {y}}}_{k|k-1}={\textbf {L}}_{k}[{\textbf {F}}_{k}^{-1}]^{T}{\hat {\textbf {y}}}_{k|k}$
Noter également que F et Q doivent être inversibles.
L'avantage principal du filtre informationnel apparait dans son étape de correction qui est beaucoup plus simple que celle du filtre de Kalman. Ceci apporte de nombreux avantages au problème de diagnostic et surtout dans le cas de détection de plusieurs défauts simultanés.

## Les filtres non linéaires

Le filtre de Kalman est limité aux systèmes linéaires. Cependant, la plupart des systèmes physiques sont non linéaires. Le filtre n'est donc optimal que sur une petite plage linéaire osculatrice des phénomènes réels pris en compte par la linéarisation de l'équation physique. La non-linéarité peut être associée au modèle du processus, au modèle d'observation ou bien aux deux.

### Filtre de Kalman étendu

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.
    ${\textbf {x}}_{k}=f({\textbf {x}}_{k-1},{\textbf {u}}_{k},{\textbf {w}}_{k})$
    ${\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
    ${\hat {\textbf {x}}}_{k|k-1}=f({\hat {\textbf {x}}}_{k-1|k-1},{\textbf {u}}_{k},0)$
    ${\textbf {P}}_{k|k-1}={\textbf {F}}_{k}{\textbf {P}}_{k-1|k-1}{\textbf {F}}_{k}^{T}+{\textbf {Q}}_{k}$
Mise à jour
    ${\tilde {\textbf {y}}}_{k}={\textbf {z}}_{k}-h({\hat {\textbf {x}}}_{k|k-1},0)$
    ${\textbf {S}}_{k}={\textbf {H}}_{k}{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}+{\textbf {R}}_{k}$
    ${\textbf {K}}_{k}={\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {S}}_{k}^{-1}$
    ${\hat {\textbf {x}}}_{k|k}={\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}{\tilde {\textbf {y}}}_{k}$
    ${\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 :
    ${\textbf {F}}_{k}=\left.{\frac {\partial f}{\partial {\textbf {x}}}}\right\vert _{{\hat {\textbf {x}}}_{k-1|k-1},{\textbf {u}}_{k}}$
    ${\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.

### Filtre de Kalman sans parfum

Le filtre de Kalman sans parfum (Unscented Kalman filter, UKF), 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
    $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 :
    ${\hat {x}}_{k-1}^{a}=[{\hat {x}}_{k-1|k-1}^{T}\quad 0_{dim(w_{k}-1)}^{T}]$
et
    $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
    $s_{k|k-1}^{x}=f_{k-1}(s_{k-1}^{x},s_{k-1}^{w})$
    ${\hat {x}}_{k|k-1}=\sum \limits _{i=0}^{2n}W_{i}s_{k|k-1}^{x}$
    $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}$
    $\mathrm {Z} _{i,k|k-1}^{x}=h_{t}(s_{k|k-1}^{x})$
    ${\hat {z}}_{k|k-1}=\sum \limits _{i=0}^{2n}W_{i}\mathrm {Z} _{i,k|k-1}^{x}$
Estimation
    $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}$
    $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}$
    ${\hat {x}}_{k|k}={\hat {x}}_{k|k-1}+K_{k}(z_{k}-{\hat {z}}_{k|k-1})$
    $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. Cependant, ce filtre montre ses limites dans le cas de systèmes fortement non linéaires et/ou non gaussiens$^,$.

## Applications

* Pilote automatique
* Centrale inertielle
* Systèmes de localisation dynamique
* Cartographie et localisation simultanées (SLAM)
* Système de positionnement par satellites
* Assimilation de données