Aller au contenu

TP — Filtrage de Kalman

Auteur

Cadre — MSO 3.7, Apprentissage bayésien, séance #7 (4h en présentiel, CR 2 noté, déposé en fin de séance).

Objectifs

L’objectif de ce TP est de programmer le filtre et le lisseur de Kalman, puis de les étendre au cas non linéaire via l’EKF (Extended Kalman Filter). On applique enfin l’ensemble à un cas de suivi 2D d’une trajectoire bruitée.

Ce TP étend le TP HMC (séance #5) : un système dynamique linéaire-gaussien \(x_{t+1} = F x_t + w_t,\ y_t = H x_t + v_t\) est une chaîne de Markov cachée à états continus. Le filtre de Kalman est exactement l’analogue continu des équations forward (getAlpha) du TP HMC, et le lisseur RTS l’analogue du forward-backward. L’EKF ajoute la linéarisation locale quand \(f\) ou \(h\) ne sont pas linéaires.

Format et pré-requis

Projet multi-fichiers .py à exécuter dans VS Code ou un terminal Python. Dépendances : numpy, scipy, matplotlib.

Table des matières


Introduction

Téléchargez et décompressez Kalman_Skeleton.zip. Vous disposez ainsi de 4 fichiers Python :

  • func.py : algorithmes de Kalman et EKF. Quatre fonctions sont à programmer : kalman_predict, kalman_update, rts_smoother_step, ekf_update. Les autres (kalman_filter_pass, rts_smoother_pass, ekf_predict, ekf_filter_pass, mse, plot_*) sont déjà fournies.
  • SimulDLG.py : simule un système dynamique linéaire-gaussien 2D (vitesse-constante) et sauvegarde la trajectoire dans sources/XY.npz.
  • KalmanFiltering.py : applique le filtre et le lisseur à la simulation DLG, affiche les plots et les MSE.
  • EKF_pendulum.py : applique l’EKF à un pendule simple (non linéaire).

Bon à savoir. Le squelette tourne sans crasher même si vous n’avez encore rien codé : les TODO renvoient les entrées inchangées. Les résultats sont alors triviaux (pas d’estimation), mais cela vous permet de vérifier le pipeline dès le début.

1. Simulation d’un systeme dynamique lineaire-gaussien

Fichier principal : SimulDLG.py

  • Lisez le code et vérifiez que vous comprenez bien : la matrice de transition \(F\) (vitesse-constante), la matrice d’observation \(H\) (on n’observe que la position), les covariances \(Q\) et \(R\).
  • Lancez python SimulDLG.py. Observez la figure results/SimulDLG.png : trajectoire vraie 2D + observations bruitées.

np.random.seed(42) est fixé en début de programme pour la reproductibilité.

2. Filtre de Kalman

Fichier principal : KalmanFiltering.py. Code à compléter dans func.py.

  • Programmez kalman_predict(x, P, F, Q) (TODO 1) — prédiction \(x_\text{pred} = F\,x\), \(P_\text{pred} = F\,P\,F^T + Q\).
  • Programmez kalman_update(x, P, y, H, R) (TODO 2) — correction par l’observation \(y\).
  • Lancez python KalmanFiltering.py. Vous devriez obtenir des MSE de l’ordre de : 
    MSE observation : ≈ 0.50  (variance R)
MSE filtre      : ≈ 0.10  (gain net)
MSE lisseur     : (cf. partie 3)

3. Lisseur RTS

Code à compléter dans func.py. Pour visualiser, c’est toujours KalmanFiltering.py.

  • Programmez rts_smoother_step(...) (TODO 3) — une étape backward du lisseur Rauch-Tung-Striebel.
  • Relancez python KalmanFiltering.py. Le lisseur doit avoir une MSE inférieure à celle du filtre (~30 à 50 % de gain selon les paramètres).

4. EKF — Extended Kalman Filter

Fichier principal : EKF_pendulum.py. Code à compléter dans func.py.

On modélise un pendule simple par $\(\theta_{t+1} = \theta_t + \Delta t\, \omega_t,\quad \omega_{t+1} = \omega_t - \Delta t\, \tfrac{g}{L}\sin(\theta_t).\)$ L’observation est \(y_t = \sin(\theta_t) + \text{bruit}\) — non linéaire en \(\theta\).

  • Programmez ekf_update(x, P, y, h, H_jac, R) (TODO 4) — identique à kalman_update, mais on remplace \(H\) par sa jacobienne évaluée en \(x_\text{pred}\), et on calcule l’innovation \(y - h(x_\text{pred})\) avec \(h\) non linéaire.
  • (La prédiction ekf_predict est fournie, car structurellement très proche de kalman_predict.)
  • Lancez python EKF_pendulum.py. Observez : l’EKF parvient à reconstituer \(\theta_t\) à partir des seules observations \(\sin(\theta_t)\) bruitées.

5. Application : suivi 2D (partie reportee dans votre CR)

Écrivez votre propre programme principal — proposé : SI_Tracking.py.

Choisissez une trajectoire 2D synthétique intéressante (figure-en-8, spirale d’Archimède, trajectoire avec changements de direction, etc.). Vous devez :

  1. Simuler la trajectoire vraie et les observations bruitées, en partant des matrices \(F, H, Q, R\) du modèle vitesse-constante (ou autre — à vous de choisir/justifier).
  2. Optionnel mais recommandé : introduire des observations manquantes sur un intervalle (occlusion). Pour le filtre on peut « répéter la dernière observation valide » comme baseline, ou sauter l’update sur les pas manquants — proposez votre choix.
  3. Appliquer le filtre et le lisseur (en utilisant kalman_filter_pass et rts_smoother_pass de func.py).
  4. Tracer : trajectoire vraie, observations, filtre, lisseur — sur le même plot 2D.
  5. Étudier la sensibilité aux paramètres : faites varier \(Q\) et \(R\) d’un facteur ×10 dans les deux sens (4 régimes), et commentez l’effet sur le filtre.

Consignes pour le compte-rendu (CR 2, noté)

Le CR est individuel, déposé sur Pedagogie3 avant la fin de séance, sous forme d’un fichier compressé contenant :

  • les sources : SI_Tracking.py (+ tout autre code écrit ou modifié) ;
  • un mini-rapport (.pdf ou .md ; pas de .doc / .docx), avec page de garde (noms, prénoms, date, titre du TP, module, encadrant).

Le rapport doit contenir :

  1. Trajectoire choisie : équations / type, paramètres du modèle DLG (\(F, H, Q, R\)), justification.
  2. Plot 2D : trajectoire vraie · observations · filtre · lisseur.
  3. Tableau des MSE : observation / filtre / lisseur. Commenter (gain du filtre, gain du lissage).
  4. Étude paramétrique \(Q\) et \(R\) (4 régimes) : plots et / ou tableau, et interprétation physique (que se passe-t-il quand on fait trop confiance aux observations ? au modèle ?).
  5. (Optionnel) gestion des observations manquantes : comparer la stratégie « répéter » et la stratégie « sauter l’update ».
  6. Commentaire qualitatif final : où Kalman gagne, où il échoue (transitions brusques non modélisées, biais dans \(F\), …).