1 karim dahia présentation paris 03/12/2003 application du filtrage particulaire au recalage de...
TRANSCRIPT
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/
)(
)(
de norme de l’ordre 2h
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
de norme de l’ordre 2h
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 :
Resampling partiel :
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
Resampling partiel :
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 :
Erreurs de navigation inertielles (l’approche en phi)
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
Erreurs de navigation inertielles (l’approche en phi)
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
Erreurs de navigation inertielles (l’approche en phi)
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
simulation des erreurs inertielles
38 K
arim
DA
HIA
p
rése
nta
tio
n P
aris
03
/12/
2003
simulation des erreurs inertielles
39 K
arim
DA
HIA
p
rése
nta
tio
n P
aris
03
/12/
2003
simulation des erreurs inertielles
40 K
arim
DA
HIA
p
rése
nta
tio
n P
aris
03
/12/
2003
simulation des erreurs inertielles
41 K
arim
DA
HIA
p
rése
nta
tio
n P
aris
03
/12/
2003
simulation des erreurs inertielles
42 K
arim
DA
HIA
p
rése
nta
tio
n P
aris
03
/12/
2003
simulation des erreurs inertielles
43 K
arim
DA
HIA
p
rése
nta
tio
n P
aris
03
/12/
2003
simulation des erreurs inertielles
44 K
arim
DA
HIA
p
rése
nta
tio
n P
aris
03
/12/
2003
simulation des erreurs inertielles
45 K
arim
DA
HIA
p
rése
nta
tio
n P
aris
03
/12/
2003
simulation des erreurs inertielles
46 K
arim
DA
HIA
p
rése
nta
tio
n P
aris
03
/12/
2003
simulation des erreurs inertielles
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)