Nouvelles méthodes en filtrage particulaire Application au recalage de navigation inertielle par mesures radio-altimétriques K. DAHIA Doctorant.

Présentation au sujet: "Nouvelles méthodes en filtrage particulaire Application au recalage de navigation inertielle par mesures radio-altimétriques K. DAHIA Doctorant."— Transcription de la présentation:

1 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

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

4 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ù

5 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

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 Le Filtrage Particulaire
x densité prédite prédiction x vraisemblance x densité conditionnelle correction

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

9 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

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

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

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

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

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

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 de autour de :

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

20 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

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

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

24 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

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

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

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

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

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

34 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.

35 Les conditions initiales sont :
Nombre de mesure : 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 = 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
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 %

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

43 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.