1 karim dahia présentation grenoble 04/01/2005 nouvelles méthodes en filtrage particulaire...

1 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

Nouvelles méthodes en filtrage particulaireApplication 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 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

• 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

3 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05







4 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

Données du problème

Critère d’estimation

Solution ?

]ˆ[2

kk xxE

Minimum de variance

Etat Observation

),(1 kkk wxfx ),( kkk vxhy

Espérance conditionnelle

Détermination de la densité de l’état )( :1 kk yxp kkkkkkk dxyxpxyxEx )(][ˆ :1:1

kk yyyy ,...,, 21:1 où

Le problème du filtrage

5 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

Les équations du filtre optimal

)( :1 kk yxpCalcul récursif de la densité conditionnelle :

Loi de transition de k-1 à k Equation d’évolution

)( 11 kk yxp

dR kkkkkkk dxyxpxxpyxp 11:1111:1 )()()(

Chapman – Kolmogorov

Prédiction)( 1:1 kk yxp

Loi de mesure = vraisemblance Equation d’observation

)( :1 kk yxpCorrection

dR kkkkk

kkkkkk

dxyxpxyp

yxpxypyxp

)()(

)()()(

1:1

1:1:1

Bayes

la mesure ky

6 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

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

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

)( :1 kk yxp

7 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05Le Filtrage Particulaire

N

i

ik

ikkk xwyxp

1111:11 )()(

x

)( 1:11 kk yxp

N

i

ikk

ikkk xwyxp

1111:1 )()(

x

densité prédite )( 1:1 kk yxp

prédiction

N

i

ik

ikkk xwyxp

1:1 )()(

vraisemblance )( kk xyp

x

densité conditionnelle correction

8 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

vraisemblance )( kk xyp

x

densité prédite )( 1:1 kk yxp

Le Filtrage Particulaire

densité conditionnelle

x

9 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05Le Filtrage Particulaire

Indicateur de dégénérescence :

,loglog(N)1

ThwwEnt ik

N

i

ik

log(N)0 Ent

Solution

Choisir une densité a priori adaptée (densité d’importance) (.)q

L’observation et le modèle d’évolution sont pris en compte pour le calcul des poids

),(

)()(

:1:1

11

kkk

kkkkkk yxxq

xxpxypww

La trajectoire de chaque particule est étendue avec la distribution d’importance

),( :1:1 kkk yxxq

Inconvénient : les trajectoires ne sont plus statistiquement indépendantes erreur Monte Carlo

10 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

Le Filtrage Particulaire (S.I.S)

ThEnt

N

ix

xN

xp i1

00 )(1

)(0

Initialisation

Prédiction

Correction - PondérationRé-échantillonnage

ThEnt ),(

)()(

:1:1

1

1

kik

ik

ik

ik

ikki

kik

yxxq

xxpxypww

Estimation

,ˆ1

ik

N

i

ikk xwx

Tk

ikk

ik

N

i

ikk xxxxwP )ˆ)(ˆ(

1

),(~ :11:1 kik

ik

ik yxxqx )(~ 1

ik

ik

ik xxpx

(S.I.R)

11 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

Analyse de l’erreur du filtre particulaire

L’erreur locale :

kkkkk dxyxpxyp )()( 1:1

Qualité de l’estimation

12 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

kkkkkpk dxyxpxxE )()()]([ :1

)()()()()(

)()( 1:11:1

1:1

:1

kkkkk

kkkkk

kkkk yxpxwyxp

dxyxpxyp

xypyxp

Analyse de l’erreur du filtre particulaire (erreur locale)

)(])(2

1exp[

2

1)( 2

22

222

1:1 kkkkkk

xxxxyxp

))((]))((2

1exp[

2

1)( 2

12

211

kkkkkk xHyxHyxyp

Si on pose

)/1()()(

)()()(var

1)ˆ(var 2/3

2

1:1

1:12

Ndxyxpxyp

dxyxpxypx

Nkkkkk

kkkkk

kk

)(ˆ1

ik

N

i

ikk xw

l’estimateur particulaire

),...,( 1 Nkk xx

Nwik /1

13 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

]))((exp[)'(

)'(1)ˆvar( 2

2/122

2211

22

221

kkk xHyCh

h

N

C 2

2h '2

(12 h '22

2 )(12 2h '22

2 )

)( )( 21 préditencevraisembla - var

- 0 : pas d’information - propagation de la densité prédite

N

fyxpyxp kkkkkf

2),(ˆ)(sup :1:1

iiiii

iixi

k

iik

dxyxpxyp

xyp

)()(

)(sup,

1:11

l’erreur globale :

Analyse de l’erreur du filtre particulaire

dx

xHdxh

)()('

'h'h- var

)( kk xHy lorsque

14 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

Le filtrage particulaire régularisé (RPF)

N

i

ikkki

kdkkk h

xxKw

hyxp

1

1

:1

1)(

)4/(1)( dNKAh

)(KA : noyau d’Epanechnikov, Gaussien, …

dN

: dimension de l’espace

: nombre de particules: coefficient de sur-lissage

15 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05







16 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

Décomposition de la loi de densité conditionnelle en noyaux Gaussiens

Préserver la structure petite

N

ikk

ikkk

ikkkk Pxxwyxp

111:1)()(

Le filtre de Kalman-particulaire à noyaux (KPKF)

)2det(/]2/)(exp[)( 1 PxPxPx T

1kkP

utilisation des EKF locaux

17 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

l’étape d’initialisation :

N

i

ii Pxxwxp1

0101101101 )()(

)cov( 01012

01 wxhP ii Nwi /101

On suppose qu’a l’instant k, on a : NiPx ikk

ikk ,...,1, 11

de norme de l’ordre h2


18 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

0 si n’est pas près de

l’étape de correction :

))(()()( 111

1:1 kkkkikk

ikkk

N

i

ikkkkk RxHyPxxwyxp

)()( 11i

kkkik

ikkkk xxHyxH

)(

)(

1

11

ikkk

ik

ikk

ik

ikk

xHH

xHy

linéarisation de autour de :ikkx 1)( kk xH

kx ikkx 1


19 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

))(()()( 11111

1:1 ki

kkkik

ikkk

ikk

ikkk

N

i

ikkkkk RxxHyyPxxwyxp

correction de Kalman

ikk

ik

ik

iTk

ikk

ikk

ik

ikkk

ik

ikk

ik

PHHPPP

yyKxx

11

11

11

)(

)(

)()(1

:1i

kikk

N

i

ikkkk Pxxwyxp

11

1

)(

ik

iTk

ikk

ik

kiTk

ikk

ik

ik

HPK

RHPH

)( 11ik

ikkk

ik

ik yyww


de norme de l’ordre h2

20 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

l’étape de prédiction :

dR

ik

ikkkk

N

i

ikkkkk duPxuSuFxwyxp )())(()( 111

1:111

0 si n’est pas près deu ikx

Linéarisation de autour de :)(1 uFkikx

))(()( 111111

:111ik

iTk

ik

ik

ikkk

N

i

ikkkkk SFPFxFxwyxp

n’est plus de norme d’ordre h2 « ré-échantillonnage»


21 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

On approche

Par la densité

et

Minimum de la distance minimum du MISE

))(()( 1111

:111i

kkikkk

N

i

ikkkkk PxFxwyxp

)()(ˆ1

211

11:111 kk

ikkk

N

i

ikkkkkk hxxwyxp

L’étape de ré-échantillonnage :

))(cov( 11

11ik

ikk

N

i

ikk

ikkk wxFPw

ikkx 1 h

)ˆ,( 11 kkkk pp

dxxpxpxpEMISE kkkkkk )(ˆvar)()(ˆ1

2

11


22 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

On tire les particules selon la loi de densité suivante :

),~

)(()(~~ 12

111

1 kki

kkikk

N

i

ik

ikk hPxFxwxpx

ikkx 1

est un facteur de dilatation optimal (Silverman)

Solutions :

40

2*22 )(, dopt

doptopt hhhhhh

,~ *hh

0h

)])((min[ 11

1*

CPCpvh ik

T

kkTCC 1avec :

qui minimise la sous la contrainte : )~

,( hh ,0~

12

1 kki

k hP)~

,( hhMISE


23 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

les matrices sont petites : pas de ré-échantillonnage

les matrices sont grandes :

- Ent > Th ré-échantillonnage total

- Ent Th ré-échantillonnage partiel

ikkP 1

ikkP 1

Mais en pratique, on laisse m cycles de calcul, sans faire de ré-échantillonnage

m = 15 (recalage inertielle), m =1 (pistage)

Résumé de l’algorithme du ré-échantillonnage :

ik

ikk ww 1

NwwxF ikk

ik

ikk /1,),( 11


24 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

Le filtre de Kalman-particulaire à noyaux (KPKF)L’algorithme du KPKF en pratique :

Initialisation :

N

i

ii Pxxwxp1

0101101101 )()(

Estimation

ik

N

i

ikk xwx

1

ˆ

Ré-échantillonnage total

Prédiction :),(11

ikk

ikk xFx

ik

iTk

ik

ik

ikk SFPFP 1111

Correction :

• Correction de Kalman :

• Correction particulaire :

ikk

ik

ik

iTk

ikk

ikk

ik

ikkk

ik

ikk

ik

PHHPPP

yyKxx

11

11

11

)(

)(

)( 11ik

ikkk

ik

ik yyww

Ré-échantillonnage partiel :

kkopti

kk hP 12

1

ik

ikk ww 1

Tirage multinomialNwi

k /1 ,),(1ik

ikk wxF

ThEnt

k est un multiple de m

),)((

~

1

2*11

1

kki

kkikk

ikk

hPxFx

x

25 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

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 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05







27 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

: accélération absolue non-gravitationnelle dans le repère

engin (capteur)

La centrale inertielle :

centrale inertielle = 3 accéléromètres + 3 gyromètres + 1 calculateur de bord.

)~

,~

,~

( DEN XXX)

~,

~,

~( DEN VVV

m

m

: vitesse inertielle

: position inertielle

: vitesse angulaire absolue dans le repère engin

Accéléromètres Gyromètres

Calculateur

m m

),,( : angles d’attitude

28 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

Équations de navigation

)(222 iebnbnmbn RRR

gVRV iemnb )2(2 XVX

gie )(gVfV iea )2(

XVX

Équations d’erreur

Équation de dynamique

29 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

Biais de mesure des capteurs inertiels

• erreur gyrométrique :

• erreur accélérométrique :

: processus de Wiener

: bruit coloré (Markov 1er Ordre)

: la période de corrélation du bruit coloréba,baa

aa

aam

bb

b

1

bggg

g

ggm

bb

b

1

bab ,

ba,: processus de Wienerbgba,

Équation de dynamique

30 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

y

)~

,~

( ENMNT XXh

DX

Position réelle

Position inertielle

)~

,~

,~

(0 DEN XXXM M

Hauteur sol

Terrain réel

Terrain numérisé

Niveau de référence

,)~

,~

(~ EENNMNTDD XXXXhXXy

MA

Équation d’observation

31 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

TgggaaaDENDEN zyxzyx

bbbbbbVVVXXXx ][

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.

Choix du vecteur d’état

32 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

centrale inertielle

Filtre d’hybridation

Mesure ext

Radio altimètre

+ MNT

Sorties capteurs Positions, vitesses et angles d’attitude

Positions, vitesses et angles d’attitude corrigées

Hauteur sol

Système de navigation

33 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05







34 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

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.

La Borne de Cramer Rao a Posteriori (PCRB)

111

111

1 ))((]))(())([(

kT

kkkT

kT

xkkT

xxk QFMFxHRxHEMk

Dans le cas ou la dynamique est linéaire :

kkkk

kkkk

VxHy

WxFx

)(1

perte de l’information due à la dynamique

l’information due à la variation de Htel que : 1)( kk MC

: PCRB

: matrice de Fisher

kC

kM

35 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

Les conditions initiales sont :

Nombre de mesure : 400Période de mesure dt : 0.3 SecWt est un bruit blanc gaussienBruit de mesure :Biais accélérométrique :Biais gyrométrique :Vitesse horizontale : 250 m/sIncertitude 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

mN 5000m

E5000

mD 100

mkV 15

111

ab gmab 1

gb hgb deg/20

smEV /10

smDV /1

smNV /10

36 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

Résultats de simulation

Terrain plat Terrain vallonné

37 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

Terrain vallonné

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

38 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05Résultats de simulation

Terrain vallonné A coût de calcul égal

39 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05Résultats de simulation

Terrain plat

40 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

Résultats de simulation (KPKF/RBPF)100 MC

Terrain plat A coût de calcul égal

41 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

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 %1 % 0 %

7 %

7 % 1 %

17 %22 %

42 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05







43 K

arim

DA

HIA

p

rése

nta

tio

n G

ren

ob

le

04/0

1/20

05

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.

1 karim dahia présentation grenoble 04/01/2005 nouvelles méthodes en filtrage particulaire...

Documents