1 karim dahia présentation grenoble 04/01/2005 nouvelles méthodes en filtrage particulaire...
TRANSCRIPT
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
• 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 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
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 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
Le filtre de Kalman-particulaire à noyaux (KPKF)
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
Le filtre de Kalman-particulaire à noyaux (KPKF)
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
Le filtre de Kalman-particulaire à noyaux (KPKF)
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»
Le filtre de Kalman-particulaire à noyaux (KPKF)
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
Le filtre de Kalman-particulaire à noyaux (KPKF)
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
Le filtre de Kalman-particulaire à noyaux (KPKF)
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
Le filtre de Kalman-particulaire à noyaux (KPKF)
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
Le filtre de Kalman-particulaire à noyaux (KPKF)
26 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
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
• 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 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
• 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 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.