Nouvelles méthodes en filtrage particulaire Application au recalage de navigation inertielle par mesures radio-altimétriques K. DAHIA Doctorant 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

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

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 Plan de la présentation

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Le filtre de Kalman-particulaire à noyaux (KPKF) correction de Kalman de norme de l’ordre h2

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

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 :

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

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 > Th ré-échantillonnage total - Ent Th ré-échantillonnage partiel Mais en pratique, on laisse m cycles de calcul, sans faire de ré-échantillonnage m = 15 (recalage inertielle), m =1 (pistage)

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

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 h adaptatif en fonction de la PCRB

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 Plan de la présentation

: vitesse angulaire absolue dans le repère engin La centrale inertielle : centrale inertielle = 3 accéléromètres + 3 gyromètres + 1 calculateur de bord.  Accéléromètres Gyromètres : 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

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

É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é

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

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.

Système de navigation centrale inertielle 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

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 Plan de la présentation

La Borne de Cramer Rao a Posteriori (PCRB) Dans le cas ou la dynamique est linéaire : perte de l’information due à la dynamique l’information due à la variation de H 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.

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

Résultats de simulation Terrain plat Terrain vallonné

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

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

Résultats de simulation Terrain plat

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

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

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 Plan de la présentation

Conclusions Perspectives 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.