Application du filtrage particulaire au recalage de navigation

1 K

arim

DA

HIA

pré

sen

tati

on

Par

is

03/1

2/20

03

Application du filtrage particulaireau recalage de navigation

K. DAHIA Doctorant DGA

A. PHAM DINH Directeur de thèse LMC-IMAG Grenoble

Encadrants : J. P. Guibert (DPRS) et C. Musso (DTIM)Laboratoire d’accueil : ONERA

2 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Plan de la présentation

I - le Kalman-Particulaire Kernel Filter (KPKF)

La borne de Cramer Rao

II - Modélisation des équations d’erreurs inertielles Application au recalage altimétrique

Résultats

Conclusion

3 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Partie I

Le Kalman Particle Kernel Filter

4 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Décomposition la loi de densité conditionnelle en noyau Gaussien (RPF)

est un noyau Gaussien

la taille de la fenêtre du noyau :

La matrice de covariance des particules :

N

ikk

ikkk

ikkkk PhxxwYxp

11/

21/ )()/(

)2(

44

dNhd

)/cov( 1/1/ ki

kkkk wXP

Le KPKF

5 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

l’étape d ’initialisation :

Le KPKF

N

i

ii Phxxwxp1

0/1

2

0/110/110/1 )()(

)/cov( 0/10/12

0/1 wXhP ii Nwi /10/1

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

ikk ,...,1),( 1/1/

de norme de l’ordre 2h

6 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

A l’étape de correction :

)/)(()/()/( 1/1/

11/ kkkk

ikk

ikkk

N

i

ikkkkk RxHyPXxwYxp

Le KPKF

kx0 si n’est pas près dei

kkX 1/

Linéarisation de autour de :i

kkX 1/ )()( 1/1/i

kkkik

ikkkk XxHyxH

)(

)(

1/

1/1/

ikkk

ik

ikkk

ik

ikk

XHH

XxHy

)( kk xH

7 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Le KPKF

)/)(()/()/( 1/1/1/1/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

1/1

1/1/

1/1/

)(

)(


11/

1/

)(

ik

iTk

ikk

ik

kiTk

ikk

ik

ik

HPK

RHPH

)/()/(1

ik

ikk

N

i

ikkkk PXxwYxp

de norme de l’ordre

2h

)/( 1/1ik

ikkk

ik

ik yyww

8 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

u0 si n’est pas près de

Le KPKF

A l’étape de prédiction :

nR

ik

ikkkk

N

i

ikkkkk duPXuSuFxwYxp )/()/)(()/( 111

11/1

ikX

Linéarisation de autour de :)(1 uFkikX

)/)(()/( 111111

1/1ik

iTk

ik

ik

ikkk

N

i

ikkkkk SFPFXFxwYxp

n’est plus de norme de l’ordre 2h

« resampling »

9 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Le KPKF

Resampling :

Partiel :

On approche

Par la mixture

Critère utilisé : MISE ( Mean Integrated Square Error )------------------------------------------------------------------------

Total : S.I.R

si les poids sont dispersés:

)/)(()/( /1111

1/1i

kkikkk

N

i

ikkkkk PXFxwYxp

)/()/(ˆ /1/111

1/1 kki

kkk

N

i

ikkkkk PXxwYxp

)/1( Nwik

SeuilwwE ik

N

i

ik

loglog(N)ntropie1


10 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

)/()(1

ii

N

i

i PXxwxp

On a la loi de densité suivante :

On l’approche par :

N

i

ii Phxwxp1

2 )/()(ˆ

Resampling partiel :

11 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Si on tire les suivant :

N

iii

i PhPXxwxp1

2 )/()(~ N ,...,1

)()/()(ˆ1

xPPXxwxpEN

iii

i

Variance ? Si on choisit la densité :

N

iii

i PhPXxwxp1

2 )~

/()(~

N

iii

i PhhPXxwxpE1

22 ))~

(/()(ˆ

on a :


12 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

)~

/()(~

~ 2

1

PhPXxwxP iiN

i

ii

)/()( 2

1

PhxwxP iN

i

i

On cherche qui minimise la MISE (la variance et le biais ) :

Sous la contrainte :

avec

0~2 PhPi

max11 ~)])((min[

~hCPCvph Ti

PCCT

)~

,( hh

dxxpxPxPEMISE )(ˆvar)()(2


Solutions : )( DecompKernhh opt max~~hh

13 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Le KPKF

l’entropie Seuil

• de norme de l’ordre de : (correction / prédiction) avec le EKF

pas de resampling partiel

• > de norme de l’ordre de : resampling partiel

l’entropie Seuil : resampling total

i

kkP /12h

ikkP /1

2h

Mais en pratique en laisse m cycles de calcul, sans faire de resampling

14 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Le KPKF

Originalité du KPKF :

Combinaison du EKF (pas d’approximation MC) avec le RPF (multimodalité, non linéarité )

Algorithme récursif sans redistribution systématique (plus précis)

15 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

111

111

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

kT

kkkT

kT

xkkT

xxk QFFishFxHRxHEFishk

Tel que : 1kk )Fish(Bcr

Dans le cas ou la dynamique est linéaire :

kkkk

kkkk

vxHy

WxFx

)(1

l’intérêt : Évaluation des performances d’un filtre.

Borne inférieure de la matrice de covariance et limite de précision de n’importe quel estimateur.

La Borne de Cramer RaoPCRB

perte de l’info due a la dynamique

L’info due à la variation de H

16 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Partie II

Modélisation des équations d’erreurs inertielles

17 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

L’ellipsoïde terrestre

Définition des grandeurs

Ellipsoïde terrestre

La terre est représentée par un ellipsoide.

Cet ellipsoide est défini par :

L’excentricité 2

2

1a

b

Le modèle WGS 84 (World Geodetic System 1984) fournit les données les plus à jour de ces paramètres.

a et b sont le demi-grand axe et le demi-petit axe, respectivement de l’ellipsoïde

Le rayon de courbure dans le plan méridien : 2/3222 )sin1/()1( aRm La grande normale : 2/122 )sin1/( -aRt

Axe équatorial

a

b

Axe p

ola

ire

18 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Le Trièdre Géographique Local (TGL, n) :

Le TGL est le repère de navigation; son origine est située en O, projection de M sur l’ellipsoïde et ses trois axes sont dirigés respectivement vers le nord, vers l’est et la verticale descendante. Il se déplace à la surface de l’ellipsoide en même temps que le mobile,.

Le trièdre mobile (b) :

Ce trièdre est lié a la structure du véhicule. Dans le cas d’une centrale inertielle à composants liés ce trièdre est en général matérialisé par l’orientation des capteurs (accéléromètres et gyromètres)

Le trièdre terrestre (e) :

Ce repère est centré sur la terre, ses axes ayant une direction fixe par rapport aux étoiles. Le trièdre inertiel (i) :

Déduit du précédent par la vitesse de rotation de la terre.

Les différents trièdres

19 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Les différents trièdres Trièdre mobile(lié à l’avion)

Trièdre Géographique Local (TGL)

Trièdre Terrestre

Trièdre inertiel

20 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Les équations de la navigation

;

;)cos()(

;)(

D

t

E

m

N

Vh

hR

V

hR

V

Les coordonnées géographiques du mobile :

latitude

longitude

l’altitude

21 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

R : vecteur de position du mobile par rapport à la terre. V : vitesse de déplacement du mobile par rapport à la terre. A : matrice d’angle d’attitude

: l’accélération spécifique. : la vitesse angulaire de rotation du repère de navigation par rapport à la terre. : la vitesse de rotation de la terre. : la gravité.

Les équations de la navigation dans le TGL

gVAV mT )2(

mg

)( AAA m

: la vitesse de rotation absolue du corps mesurée par les gyromètres.m

22 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

La vitesse angulaire de rotation du TGL par rapport à la terre et la vitesse absolue de rotation de la terre s’expriment dans le TGL de la façon suivante :

;sin,0,cos

;,,

00

hR

tgV

hR

V

hR

V

t

E

m

N

t

E

srd /1029,7 50

avec

Les équations de la navigation dans le TGL

23 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Erreurs de navigation inertielles

Les erreurs de navigation inertielle proviennent :

des erreurs de capteurs (accéléromètres et gyromètres)

du modèle de la pesanteur

couplage entre l’erreur de position et de vitesse (phénomène de Schuler)

les erreurs d’alignement

24 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

On établit les équations d’erreurs inertielles qui représentent l’évolution des erreurs de navigation d’une centrale inertielle. Ces équations indiquent la manière dont les erreurs de mesure accélérométriques et gyrométriques se transforment en erreurs de position, de

vitesse et d’attitude

Erreurs de navigation inertielles (l’approche en phi)

RRR ˆVVV ˆ

mmm ˆ

mmm ˆ

et les erreurs de mesure :

Les erreurs de position et de vitesse sont définies comme suit :

25 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Equation d’erreur d’angle d’attitude:

On différencie l’équation suivante :

g)(

représente l’erreur de mesure des gyromètres telle que g mT

g A Equation d’erreur de position :

VRVR

On obtient alors l’équation d’erreur d’angle d’attitude :

)( AAA m

avec T][

On a :


hR

hRR

hRR

D

tE

mN

cos)(

)(

26 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

représente l’erreur de mesure des accéléromètres telle que

Equation d’erreur de vitesse :

On différencie l’équation suivante :

VgVfV a )2()2(

mT

a A

terreladeRayon

gS 2

( pulsation de Schuler )

DS R

g

22

0

0

On obtient alors l’équation d’erreur de vitesse :

gVAV mT )2(

mTAf


a

27 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Ces équations sont écrites selon l’approche en phi et dépendent des 9 variables et constituent un système d’équations différentielles couplées.

,, VR

g)(

111 )2()2(1

VgVfV a

VRVR 1


Via un changement de variable,

on obtient un autre système d’équations différentielles plus simple.

)(

28 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

erreur gyrométrique :

erreur accélérométrique :

: bruit blanc Gaussien

: bruit coloré (Markov 1er Ordre)

: facteur d’échelle accélérométrique

: facteur d’échelle gyrométrique

: la période de corrélation

du bruit coloré

ba,

baaa

a

amFEaam

wbb

wKb

1

bggg

g

gmFEggm

wbb

wKb

1

bab ,

baw ,

: bruit blanc Gaussienbgbaw ,

FEaK

FEgK

Simulations des erreurs de capteur

29 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

bg

ba

g

a

T

T

g

a

f

f

f

b

a

T

Tvvvvp

pp

g

a

f

f

f

w

w

w

w

I

I

A

A

b

b

v

p

0000

0000

A0F00

0AFFF

000IF

b

b

v

p

f

f

f

f

'

'

000

000

000

000

0000

/1

/1

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.Le vecteur d’état à estimé est :

TgggaaaDENEN zyxzyx

bbbbbbvvvhRRX ][

Les équations du filtre

30 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

0

0

0

NE

ND

ED

ppF

2

2

2

200

00

00

s

s

s

vpF

0)2(

)2(0)2(

)2(0

NNE

NNDD

EDD

vvF

0

0

0

NE

ND

ED

v

ff

ff

ff

F

0)(

)(0)(

)(0

NNE

NNDD

EDD

F

La mise des équation sous forme d’état :

Les équations du filtre

31 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Principe de la méthode altimétrique

Ah

)ˆ,ˆ( yxhMNT

z

TZ

Position réellePosition inertielle

)ˆ,ˆ,ˆ(0 zyxM M

Hauteur sol

Terrain réel

Terrain numérisé

Niveau de référence

32 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

L’équation d’observation

AEINNINMNTIN RyRxhhzy ),(

avec :T

gggaaaDENEN zyxzyxbbbbbbvvvhRRX ][

A : l’erreur de mesure

TINSINSINSinsmes zyxX ][_ : mesures inertielles

y : mesure du radio altimètre

33 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Contexte applicatif : recalage altimétrique

Algorithmes existants

Maximum de vraisemblance (maillage) (vitesses assez bien connues et zone initiale d’incertitude assez précise) puis Kalman (EKF), et la Rao-Blackwellized Particle Filter.

Apport du KPKF : conditions d’emploi plus générales

Vitesses initiales moins bien connues

Zones d’incertitudes + importantes (6 km en x, y par ex)

Estimation conjointe des positions et vitesses

Intérêt

Survol + long de zones plates

Utilisation d’une centrale inertielle moins performante

34 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

The Rao-Blackwellized Particle Filter(Per-Johan Nordlund, Niclas Bergman)

kt

pt

kt

kt

pt

kt

pt

kkt

pt

pt

pt

kt

pt

pt

pt

ppt

uxGxxFxfx

uxGxxFxfx

)()()(

)()()(

1

1

On écrit notre modèle sous la forme :

TTgggaaaDEN

TEN zyxzyx

bbbbbbvvvhRRX )()(

Bcp de divergences pour des grandes zones d’incertitudes !

35 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Les conditions initiales sont :

Nombre de mesures : 400Période de mesures dt : 0.7 SecWt est un bruit blanc gaussienBruit de mesure du radio altimètre :Biais accélérométrique :Biais gyrométrique :Vitesse horizontal : 250 m/sIncertitude initiale en x : Incertitude initiale en y :Incertitude initiale en z :Incertitude initiale en vx :Incertitude initiale en vY :Incertitude initiale en vz :Incertitude initiale en :Incertitude initiale en :Incertitude initiale en :Nombre de particules : N = 1500 pour le KPKF

mx 5000my 5000mz 100

smvx /5smvy /5smvz /1

mtW 15

111

gbab 22 /10 sm

ab

sradgb /10 4

50 tirages MC

KPKF : 2% de divergences

RB : 20 % de divergences

36 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

simulation des erreurs inertielles

37 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003


38 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003


39 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003


40 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003


41 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003


42 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003


43 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003


44 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003


45 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003


46 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003


47 K

arim

DA

HIA

p

rése

nta

tio

n P

aris

03

/12/

2003

Conclusions

Le KPKF réactualise l’ensemble des paramètres positions et vitesses; il prend en compte le caractère multimodal associé aux ambiguïtés de position de l’avion dans le plan horizontal, ce qui n’est pas vrai pour les filtres de recalage classique du type Kalman. Le nombre de particules requis par l’algorithme n’augmente que peu avec la dimension de l’espace d’état (de dimension 15 dans notre cas).

Le KPKF converge plus rapidement que le RPF, la courbe d’écart type du KPKF atteint plus rapidement la PCRB et présente moins de divergences. Incertitude initiale beaucoup plus grande qu’avec la Rao Blackwellised Particle Filter.

La mise en œuvre du KPKF est simple et rapide. Cette simplicité algorithmique permet de traiter facilement d’autres problèmes plus complexes.

Le KPKF peut être appliqué dans un cadre plus général (non-linéarité de la dynamique/mesure pour toutes les composantes de l’état)

Documents

Application du filtrage particulaire au recalage de navigation