Filtre de Kalman Etendu (EKF)

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

Prediction

Prédiction de l’état :

 \hat{x}_{i}^p = f(\hat{x}_{i-1},u_i)

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) :

 P_i^p = F_i.P_{i-1}.F_i^\top + Q_i

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}

Mise à jour

Innovation or erreur résiduelle :

 \widetilde{y}_i = z_i - h(\hat{x}_i^p)

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 :

 S_i=H_i.P_i^p.H_i^\top+R_i

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 :

 K_i=P_i^p.H_i^\top.S_i^{-1}



  • 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é :

 \hat{x}_i=\hat{x}_i^p + K_i.\widetilde{y}_i

Mise à jour de la covariance estimée

 P_i=(I-K_i.H_i).P_i^p

Où:


  • I est la matrice identité.

Laisser un commentaire

Votre adresse de messagerie ne sera pas publiée. Les champs obligatoires sont indiqués avec *