Filtre de Kalman Etendu (EKF)

Cet article détaille les équations du filtre de Kalman

Prediction

Prédiction de l’état :

Où:

$\hat{x}_i^p$ est a prédiction de l’état à l’instant $i$ .
$\hat{x}_{i}$ est l’état estimé à l’instant $i$ .
$f$ est une fonction dérivable qui prédit l’évolution de l’état en fonction de l’état précédent (prediction) et de l’entrée du système ( $u_i$ )
$u_i$ est l’entrée du système à l’instant $i$ .

Prédiction de l’incertitude (ou de la covariance) :

Où:

$P_i^p$ est la matrice de covariance associée à l’état $\hat{x}_i^p$ à l’instant $i$ .
$P_i$ est la matrice de covariance associée à l’état $\hat{x}_{i}$ .
$Q_i$ est la covariance liée à la prédiction (ou bruit) du système.
$F_i$ est la matrice de transition. Elle est donnée par la Jacobienne de $f()=\frac{df}{dx}$

Innovation or erreur résiduelle :

Où:

$\widetilde{y}_i$ est l’erreur de mesure : c’est la différence entre la mesure (ou observation) $z_i$ et la mesure estimée depuis l’état prédit $\hat{x}_i^p$ .
$z_i$ est l’observation (or mesure) depuis l’état réel $x_i$ .
$h$ est une fonction dérivable qui permet de passer de l’espace d’état vers l’espace des observations.

Covariance de l’innovation résiduelle :

Où:

$S_i$ est la matrice de covariance associée à la mesure d’erreur $\widetilde{y}_i$ .
$R_i$ est la matrice de covariance associée au bruit de mesure.
$H_i$ est une matrice de transition qui qui permet de passer de l’espace d’état vers l’espace des observations. Elle est donné epar la Jacobienne de $H_i=\frac{dh}{dx}$ .

Gain optimal de Kalman :

Où

$K_i$ est le gain de Kalman. Cette matrice contient la pondération entre les prédictions et les mesures. C’est cette matrice qui va permettre de réaliser la fusion entre la prédiction et les différentes observations.

Mise à jour de l’état estimé :

Mise à jour de la covariance estimée

Où: