Nouvelles méthodes en filtrage particulaire Application au recalage de navigation inertielle par mesures radio-altimétri

1. Nouvelles méthodes en filtrage particulaireApplication au recalage de navigation inertiellepar mesures radio-altimétriques K. DAHIADoctorant DGA A. D. T. PHAM Directeur de thèse LMC-IMAG Grenoble J. P. GUIBERT Responsable du domaine navigation DPRS / ONERA C. MUSSO Responsable du domaine filtrage DTIM / ONERA

2. Plan de la présentation • Le filtrage non linéaire : • Méthodes de résolution analytiques et numériques • Le filtrage particulaire • Le filtre de Kalman-particulaire à noyaux (KPKF) : • Présentation du filtre • Ré-échantillonnage • Application au recalage de la navigation inertielle : • Les équations d’erreurs de navigation inertielle • Simulations : • Contexte • Résultats • Conclusions & Perspectives

3. Plan de la présentation • Le filtrage non linéaire : • Méthodes de résolution analytiques et numériques • Le filtrage particulaire • Le filtre de Kalman-particulaire à noyaux (KPKF) : • Présentation du filtre • Ré-échantillonnage • Application au recalage de la navigation inertielle : • Les équations d’erreurs de navigation inertielle • Simulations : • Contexte • Résultats • Conclusions & Perspectives

4. Etat • Observation • Minimum de variance • Espérance conditionnelle • Détermination de la densité de l’état où Le problème du filtrage Données du problème Critère d’estimation Solution ?

5. la mesure Prédiction Correction Chapman – Kolmogorov Bayes Loi de transition de k-1 à k Equation d’évolution Loi de mesure = vraisemblance Equation d’observation Les équations du filtre optimal Calcul récursif de la densité conditionnelle :

6. Méthodes analytiques et numériques • Le filtre de Kalman étendu (EKF) • L’EKF évalue en propageant et corrigeant sa moyenne et sa matrice de covariance • Limites : • non-linéarités trop fortes. • exige une bonne initialisation. • ne traite pas le cas de la multimodalité (recalage altimétrique) • Les méthodes de maillage • coût de calcul important pour des espaces de dimension > 3

7. densité prédite vraisemblance prédiction x x correction densité conditionnelle x Le Filtrage Particulaire

8. Le Filtrage Particulaire vraisemblance densité prédite x densité conditionnelle x

9. Inconvénient : les trajectoires ne sont plus statistiquement indépendantes erreur Monte Carlo Solution Choisir une densité a priori adaptée (densité d’importance) La trajectoire de chaque particule est étendue avec la distribution d’importance L’observation et le modèle d’évolution sont pris en compte pour le calcul des poids Le Filtrage Particulaire Indicateur de dégénérescence :

10. (S.I.R) Estimation Le Filtrage Particulaire (S.I.S) Initialisation Prédiction Ré-échantillonnage Correction - Pondération

11. Qualité de l’estimation Analyse de l’erreur du filtre particulaire • L’erreur locale :

12. Analyse de l’erreur du filtre particulaire (erreur locale) l’estimateur particulaire Si on pose

13. Analyse de l’erreur du filtre particulaire lorsque 0 : pas d’information - propagation de la densité prédite l’erreur globale : - var - var -

14. Le filtrage particulaire régularisé (RPF) : noyau d’Epanechnikov, Gaussien, … : dimension de l’espace : nombre de particules : coefficient de sur-lissage

15. Plan de la présentation • Le filtrage non linéaire : • Méthodes de résolution analytiques et numériques • Le filtrage particulaire • Le filtre de Kalman-particulaire à noyaux (KPKF) : • Présentation du filtre • Ré-échantillonnage • Application au recalage de la navigation inertielle : • Les équations d’erreurs de navigation inertielle • Simulations : • Contexte • Résultats • Conclusions & Perspectives

16. utilisation des EKF locaux Le filtre de Kalman-particulaire à noyaux (KPKF) • Décomposition de la loi de densité conditionnelle en noyaux Gaussiens Préserver la structure petite

17. Le filtre de Kalman-particulaire à noyaux (KPKF) l’étape d’initialisation : On suppose qu’a l’instant k, on a : de norme de l’ordre h2

18. Le filtre de Kalman-particulaire à noyaux (KPKF) l’étape de correction: 0 si n’est pas près de linéarisation deautour de :

19. de norme de l’ordreh2 Le filtre de Kalman-particulaire à noyaux (KPKF) correction de Kalman

20. n’est plus de norme d’ordre h2 « ré-échantillonnage» Le filtre de Kalman-particulaire à noyaux (KPKF) l’étape de prédiction: 0 si n’est pas près de Linéarisation deautour de :

21. Le filtre de Kalman-particulaire à noyaux (KPKF) On approche Par la densité et  Minimum de la distance minimum du MISE L’étape de ré-échantillonnage :

22. Le filtre de Kalman-particulaire à noyaux (KPKF) qui minimise la sous la contrainte : Solutions : avec : est un facteur de dilatation optimal (Silverman) On tire les particules selon la loi de densité suivante :

23. Le filtre de Kalman-particulaire à noyaux (KPKF) Résumé de l’algorithme du ré-échantillonnage : • les matrices sont petites : pas de ré-échantillonnage • les matrices sont grandes : • - Ent > Thré-échantillonnage total • - EntThré-échantillonnage partiel Mais en pratique, on laisse m cycles de calcul, sans faire de ré-échantillonnage m = 15 (recalage inertielle), m =1 (pistage)

24. Le filtre de Kalman-particulaire à noyaux (KPKF) • Correction : • Correction de Kalman : • Correction particulaire : Estimation Tirage multinomial Prédiction : L’algorithme du KPKF en pratique : Initialisation : Ré-échantillonnage total Ré-échantillonnage partiel : k m est un multiple de

25. Le filtre de Kalman-particulaire à noyaux (KPKF) • Originalité du KPKF : • Combinaison du EKF (pas d’approximation MC) avec le RPF (multimodalité, non linéarité ) • Méthode de redistribution : diminution des fluctuations MC • Initialisation en tenant compte des premières mesures • hadaptatif en fonction de la PCRB

26. Plan de la présentation • Le filtrage non linéaire : • Méthodes de résolution analytiques et numériques • Le filtrage particulaire • Le filtre de Kalman-particulaire à noyaux (KPKF) : • Présentation du filtre • Ré-échantillonnage • Application au recalage de la navigation inertielle : • Les équations d’erreurs de navigation inertielle • Simulations : • Contexte • Résultats • Conclusions & Perspectives

27. Accéléromètres Gyromètres La centrale inertielle : centrale inertielle = 3 accéléromètres + 3 gyromètres + 1 calculateur de bord.  : accélération absolue non-gravitationnelle dans le repère engin (capteur) : vitesse angulaire absolue dans le repère engin Calculateur : position inertielle : vitesse inertielle : angles d’attitude

28. Équations d’erreur Équation de dynamique Équations de navigation

29. Équation de dynamique Biais de mesure des capteurs inertiels • erreur accélérométrique : : bruit coloré (Markov 1er Ordre) : processus de Wiener : processus de Wiener • erreur gyrométrique : : la période de corrélation du bruit coloré

30. Équation d’observation Position inertielle Position réelle Hauteur sol Terrain numérisé Terrain réel Niveau de référence

31. Choix du vecteur d’état On estime un vecteur d’état à 15 variables d’état, les 9 variables cinématiques, ainsi que les 6 biais accélérométrique et gyrométriques.

32. centrale inertielle Système de navigation Positions, vitesses et angles d’attitude Sorties capteurs Mesure ext Radio altimètre + MNT Filtre d’hybridation Positions, vitesses et angles d’attitude corrigées Hauteur sol

33. Plan de la présentation • Le filtrage non linéaire : • Méthodes de résolution analytiques et numériques • Le filtrage particulaire • Le filtre de Kalman-particulaire à noyaux (KPKF) : • Présentation du filtre • Ré-échantillonnage • Application au recalage de la navigation inertielle : • Les équations d’erreurs de navigation inertielle • Simulations : • Contexte • Résultats • Conclusions & Perspectives

34. perte de l’information due à la dynamique l’information due à la variation de H La Borne de Cramer Rao a Posteriori (PCRB) Dans le cas ou la dynamique est linéaire : tel que : : PCRB : matrice de Fisher l’intérêt : • Évaluation des performances d’un filtre. • Borne inférieure de la matrice de covariance de n’importe quel estimateur non biaisé. • Indicateur quantitatif pour l’évaluation de la qualité de la navigation.

35. Les conditions initiales sont : Nombre de mesure : 400 Période de mesure dt : 0.3 Sec Wt est un bruit blanc gaussien Bruit de mesure : Biais accélérométrique : Biais gyrométrique : Vitesse horizontale : 250 m/s Incertitude initiale en Nord : Incertitude initiale en Est : Incertitude initiale en Down : Incertitude initiale en VN : Incertitude initiale en VE : Incertitude initiale en VD : Incertitude initiale en  : Incertitude initiale en  : Incertitude initiale en  : Nombre de particules : N = 1000 pour le KPKF

36. Résultats de simulation Terrain plat Terrain vallonné

37. Résultats de simulation (KPKF/RPF) 100 MC Terrain vallonné

38. Résultats de simulation Terrain vallonné A coût de calcul égal

39. Résultats de simulation Terrain plat

40. Résultats de simulation (KPKF/RBPF) 100 MC Terrain plat A coût de calcul égal

41. zone initiale de position horizontale (3 km, 3 km) (1 km, 1 km) (5 km, 5 km) RPF 7 % 5 % 1 % RBPF 22 % 17 % 7 % KPKF 1 % 1 % 0 % Résultats de simulation Taux de divergence(terrain vallonné)

42. Plan de la présentation • Le filtrage non linéaire : • Méthodes de résolution analytiques et numériques • Le filtrage particulaire • Le filtre de Kalman-particulaire à noyaux (KPKF) : • Présentation du filtre • Ré-échantillonnage • Application au recalage de la navigation inertielle : • Les équations d’erreurs de navigation inertielle • Simulations : • Contexte • Résultats • Conclusions & Perspectives

43. Conclusions • Le KPKF est plus précis que les autres filtres particulaires (RPF, RBPF) en position. • La zone d’incertitude initiale admissible du KPKF est beaucoup plus grande qu’avec le RBPF et le RPF. A temps de calcul égal, le KPKF fournit une meilleure robustesse. • La mise en œuvre du KPKF reste simple. Cette simplicité algorithmique permet de traiter facilement d’autres problèmes complexes comme le pistage. • Perspectives • Adaptation du nombre de particules au cours du temps. • Extraction d’un critère simple de qualité du recalage altimétrique à partir de la PCRB.