Академический Документы
Профессиональный Документы
Культура Документы
her
he
Développement d'une
entrale inertielle
par le ltre de Kalman
Rata Suwantong
er
1 mars 2011
Remer
iement
Je tiens à remer
ier dans un premier temps, toute l'équipe pédagogique
du département DMIA-ISAE Supaéro pour avoir m'a
ueillie très
haleureu-
sement dans le département.
Monsieur François DEFAY, mon maître de projet de re
her
he, pour l'aide
et les
onseils
on
ernant les missions évoquées dans
e rapport, qu'il m'a
apporté lors des diérents suivis. Il m'a formé et a
ompagné tout au long
de
ette expérien
e de re
her
he ave
beau
oup de patien
e.
Monsieur Yves BRIERE pour ses réponses sur toutes mes questions dans
les parties théoriques et pratiques du ltre de Kalman.
1
Table des matières
1 Eléments théoriques 5
1.1 Représentation et détermination de l'orientation . . . . . . . . 5
1.1.1 Angles d'Euler . . . . . . . . . . . . . . . . . . . . . . 5
1.1.2 Quaternion . . . . . . . . . . . . . . . . . . . . . . . . 6
1.2 La
entrale inertielle . . . . . . . . . . . . . . . . . . . . . . . 7
1.3 Filtre de Kalman
lassique et étendu . . . . . . . . . . . . . . 8
1.3.1 Introdu
tion . . . . . . . . . . . . . . . . . . . . . . . . 8
1.3.2 Le ltre de Kalman
lassique . . . . . . . . . . . . . . 8
1.3.3 Le ltre de Kalman étendu . . . . . . . . . . . . . . . 10
1.4 Le ltre de Kalman appliqué à la navigation inertielle . . . . 12
1.4.1 Le ltre de Kalman à 13 états . . . . . . . . . . . . . . 12
1.4.2 Le ltre de Kalman à 16 états . . . . . . . . . . . . . . 14
3 Con lusion 24
2
Introdu
tion
Une
entrale inertielle est utilisée pour déterminer l'altitude ainsi que sa
position en intégrant les a
élérations et les vitesses angulaires mesurées par
des a
éléromètres et des gyromètres. Pour
ela il sut pour les utilisateurs
de
onnaître les
oordonnées et l'attitude de la
entrale inertielle de départ.
Néanmoins, le problème prin
ipal de la lo
alisation par une
entrale iner-
tielle est que en intégrant les a
élérations et les vitesses angulaires, nous
intégrons également les bruits de
eux-
i donnant les biais d'estimations.
Dans
e projet, nous avons
ollé la
entrale SBG et la
entrale CHR sur
un quadriroteur mobile. A partir du temps de mesure à 55 sé
ondes, le qua-
driroteur mobile est en mouvement dans le plan horizontal. De
e fait, le
roulis φ et le tangage θ doivent être quasiment nuls. La
entrale SBG donne
la mesure de
es deux angles presque nuls mais
e n'est pas le
as pour la
entrale CHR.
3
80
CHR
60 SBG
40
20
phi angle(°)
−20
−40
−60
−80
−100
0 10 20 30 40 50 60 70 80
time(s)
100
CHR
80 SBG
60
40
theta angle(°)
20
−20
−40
−60
−80
0 10 20 30 40 50 60 70 80
time(s)
4
400
CHR
SBG
300
200
psi angle(°)
100
−100
−200
0 10 20 30 40 50 60 70 80
time(s)
5
Chapitre 1
Eléments théoriques
1.1.2 Quaternion
Un quaternion q est une
olle
tion de quatre paramètres réels desquels
le premier est
onsidéré
omme un s
alaire et les trois autres
omme un
6
Figure 1.1 Angles d'Euler.
7
La transformée entre deux représentation, relativement au repère initial N
et au repère mobile B, d'un ve
teur x(t) ~ est
2
q0 + q12 − q22 − q32
2(q1 q2 + q0 q3 ) 2(q1 q3 − q0 q2 )
~x(t)b = 2(q1 q2 − q0 q3 ) q02 − q12 + q22 − q32 2(q2 q3 + q0 q1 ) ~x(t)n = DCM (q)~x(t)n
2(q1 q3 + q0 q2 ) 2(q2 q3 − q0 q1 ) q0 − q12 − q22 + q32
2
(1.3)
La matri
e DCM est appelée Dire
tion Cosine Matrix en anglais. La dérivée
du quaternion peut s'é
rire
1
q̇ = Ωq (1.4)
2
0 −p −q −r
p 0 r −q
Ω= q −r 0
(1.5)
p
r q −p 0
Nous pouvons obtenir les angles d'Euler dé
rivant l'attitude du mobile à
partir des
omposantes de quaternion de la façon suivante :
2(q2 q3 + q0 q1 )
arctan
φ 1 − 2(q12 + q22
(1.6)
θ = arcsin(−2(q1 q3 − q0 q2 ))
ψ 2(q q
1 2 + q q
0 3 )
arctan
1 − 2(q22 + q32
8
angles dits de
ap, de roulis et de tangage. A bord d'un mobile, le
ap se
dénit par rapport au Nord, le roulis est un angle de rotation autour de l'axe
longitudinal du mobile et le tangage autour de l'axe transversal.
En
onnaissant la dire
tion du ve
teur gravité (lo
alement), mesurée par les
a
éléromètres au repos, et
elle du Nord, mesuré par les magnétomètres, la
entrale peut déterminer le
ap,le roulis et le tangage. Lorsque l'on
onnaît
l'attitude de la
entrale, on peut déterminer sa position par le prin
ipe de la
navigation. Cette détermination n'est possible que si l'on dispose des
oor-
données d'un point de départ.
9
de navigation. Le lre de Kalman est un ensemble d'équations mathéma-
tiques qui fournit un moyen de
omputation ré
ursif e
a
e an d'estimer
l'état d'un pro
édé, tout en minimisant la moyenne d'erreurs quadratiques.
Le ltre est très puissant pour plusieures raisons : il utilise des estimations
d'état du passé, du présent et aussi du futur. De plus il est e
a
e même si
le modèle du système n'est pas bien
onnu.
zk = Hxk + vk (1.9)
e− −
k ≡ xk − x̂k (1.12)
ek ≡ xk − x̂k (1.13)
La matri
e de
ovarian
e des erreurs à priori est don
Pk− = E[e− −T
k ek ] (1.14)
10
En dérivant les équation pour le ltre de Kalman, nous allons
ommen
er
par
her
her une équation qui
al
ule l'état estimé à posteriori x̂k
omme
une
ombinaison linéaire de l'état estimé à priori x̂−
k et une diéren
e entre
la mesure vraie zk et la mesure estimée H x̂−
k . On é
rit
x̂k = x̂− −
k + K(zk − H x̂k ) (1.16)
On remarque que
Lorsque R tend vers zéro, le gain K pondère plus de l'innovation. En
eet
lim Kk = H −1 (1.18)
Rk →0
11
Figure 1.4 Le s
héma de fon
tionnement du ltre de Kalman
x̂k = x̂− −
k + K(zk − H x̂k ) (1.23)
Pk = (I − Kk H)Pk− (1.24)
Après
haque paire de prédi
tion-mise à jour, le pro
édé est répété en utili-
sant l'état estimé à posteriori à
et instant pour prédire l'état estimé à priori
au pro
hain instant. Cette propriété ré
ursive rend le ltre plausible à im-
plémenter
ar l'état estimé à posteriori à l'instant k
ontient l'information
de tous les états pré
édents.
Pour utiliser le ltre de Kalman, la
ovarian
e du bruit de mesure (R) et
la
ovarian
e du bruit du système (Q) doivent être évaluées à l'avan
e. La
détermination de la
ovarian
e du bruit du système est généralement plus
di
ile par
e que l'observation dire
te de l'état du système est impossible.
zk = h(xk , vk ) (1.26)
12
l'origine du ltre En pratique, les valeurs des bruits wk et vk à
haque
itération ne sont pas a
essibles. Néanmoins, on peut faire l'approximation
du ve
teur d'état et du ve
teur de mesure ainsi :
13
Figure 1.5 Le s
héma
omplète de fon
tionnement du ltre de Kalman
étendu
x̂k = x̂− −
k + Kk (zk − h(x̂k , 0)) (1.38)
Pk = (I − Kk H)Pk− (1.39)
q
T
x= p q r δp δq δr δmx δmy δmz (1.40)
14
mesurés par les magnétomètres suivant l'axe x, y ,z dans le repère mobile.
T
~ = mx my mz . Les équations de l'évolution sont
On note m
1
q(k + 1) = q(k) + Ωq(k)Te (1.41)
2
ω (k + 1) = ω
~ ~ (k) + δpqr (k) (1.42)
δpqr (k + 1) = δpqr (k) (1.43)
δm (k + 1) = δm (k) (1.44)
En eet, puisque nous ne
onnaissons pas pré
isement l'évolution des biais,
nous allons laisser les termes δpqr et δm évaluer pendant l'état de mise à jour
de l'état. D'où
1
q(k + 1) q(k) + Ωq(k)Te
~ 2
ω (k + 1) =
ω
~ (k) + δpqr (k) (1.45)
δpqr (k + 1)
δpqr (k)
δm (k + 1) δ (k) m
∂f (xk )
La matri
e de ja
obienne F = s'é
rit :
∂xk
1 1
T Ω Te Φ 04×3 04×3
2 e 2
F = I13×13 + 03×4 03×3 I3×3 03×3 (1.46)
03×4 03×3 03×3 03×3
03×4 03×3 03×3 03×3
En eet
0 −p −q −r q0 −q1 −q2 −q3
p 0 r −q q1 q0 −q3 q2 p
Ωq =
= ω (1.47)
q ≡ Φ~
q −r 0 p q2 q3 q0 −q1
r
r q −p 0 q3 −q2 q1 q0
(1.48)
z = ~a ~ω m ~
Nous allons é
rire les mesures estimées à partir de l'information d'états. Pour
ela, on utilise à la relation 1.3, nous pouvons é
rire l'a
élération dans le
15
repère mobile estimée (don
la mesure des a
éléromètres) en fon
tion de
l'a
élération de l'engin dans le repère mobile en fon
tion de l'a
élération
gravitationnelle dans le repère inertiel :
ˆ = DCM (q)~g
~a (1.49)
ˆ s'é
rit :
la vitesse angulaire estimée ~
ω
~ˆ = ω
ω ~ + δpqr (1.50)
Ave
~ω et δpqr extraits du ve
teur d'état à l'instant là,
'est à dire X(5 : 7)
et X(8 : 10) respe
tivement.
la relation entre le
hamp magnétique dans le repère mobile estimés, m ~ˆ en
fon
tion du
hamp magnétique terrestre dans le repère inertiel, ~h est don
:
T
~ˆ = DCM (q)~h + δmx δmy δmz
m (1.51)
~ extraits du ve
teur d'état à l'instant là,
'est à dire X(11 : 13) Pour
Ave
δm
∂h(xk )
résumer, la matri
e de ja
obienne H = s'é
rit :
∂xk
ˆ
∂~a(k)
∂q(k) 03×3 03×3 03×3
(1.52)
H=
03×4 I3×3 I3×3 03×3
~ˆ
∂ m(k)
03×3 03×3 I3×3
∂q(k)
ave
ˆ(k)
~a ∂DCM~g
= (1.53)
∂q(k) ∂q(k)
Don
ˆ q0~gx + q3~gy − q2~gz q1~gx + q2~gy + q3~gz −q2~gx + q1~gy − q0~gz −q3~gx + q0~gy + q1~gz
~a(k)
= 2 q3~gx + q0~gy + q1~gz q2~gx − q1~gy + q0~gz q1~gx + q2~gy + q3~gz −q0~gx − q3~gy + q2~gz
∂q(k)
q2~gx − q1~gy + q0~gz q3~gx − q0~gy − q1~gz q0~gx + q3~gy − q2~gz q1~gx + q2~gy − q3~gz
(1.54)
~ˆ
∂ m(k) ∂(DCM ~h)(k)
= (1.55)
∂q(k) ∂q(k)
Après avoir fait le
al
ul matri
iel, nous trouverons
q0 hx + q3 hy − q2 hz q1 hx + q2 hy + q3 hz −q2 hx + q1 hy − q0 hz −q3 hx + q0 hy + q1 hz
~ˆ b (k)
m
= 2 −q3 hx + q0 hy + q1 hz q2 hx − q1 hy + q0 hz q1 hx + q2 hy + q3 hz −q0 hx − q3 hy + q2 hz
∂q(k)
q2 hx − q1 hy + q0 hz q3 hx − q0 hy − q1 hz q0 hx + q3 hy − q2 hz q1 hx + q2 h− q3 hz
(1.56)
16
1.4.2 Le ltre de Kalman à 16 états
Equations d'états Nous allons ajouter les biais de a
élérations en plus
dans le ve
teur d'état. Ce qui donne le ve
teur d'état et l'équation d'évolution
sous les formes suivantes :
x = q p q r δpqr δm δax δay δaz
T
(1.57)
1
q(k + 1) q(k) + Ωq(k)Te
~ 2
ω (k + 1)
ω
~ (k) + δpqr (k)
δpqr (k + 1) = (1.58)
δpqr (k)
δm (k + 1)
δm (k)
δa (k + 1) δ (k)
a
∂f (xk )
La matri
e de ja
obienne F = s'é
rit :
∂xk
1 1
Te Ω Te Φ 04×3 04×3 04×3
2 2
F = I16×16 + 03×4 03×3 I3×3 03×3 03×3 (1.59)
03×4 03×3 03×3 03×3 03×3
03×4 03×3 03×3 03×3 03×3
Ave
les relations de Ω et Φ dans l'équation 1.47.
17
ˆ(k)
~a ~ˆ
∂ m(k)
ave
et dans 1.54 et 1.56 respe
tivement.
∂q(k) ∂q(k)
18
Chapitre 2
19
H. Jazwinski a
ité dans son livre (Sto
hasti
pro
esses and ltering theory)
que l'utilisation d'un très grand bruit de mesures peut
a
her la non-linéarité
de
elles-
i. De plus,
es grandes valeurs de bruit de mesures sont dûes au
fait que les bruits de mesures ne sont pas gaussiens.
~ˆ = DCM (~
m ω = 0)~h = ~h (2.2)
Nous allons
omparer les eets de
haque sortie en mettant le bruit de me-
sure à l'inni (numériquement très grand : 108 dans le
ode) pour les sorties
que l'on ne veut pas que le ltre utilise. Nous allons don
avoir 3 types des
estimations pour
haque type de ltre ainsi :
20
par la
entrale inertielle CHR. Les
ourbes noires représentent les estima-
tions des angles d'Euler par la
entrale inertielle SBG. Les
ourbes bleues
laires représentent les estimations des angles d'Euler par le ltre de Kalman
13 états en utlisant les sorties des gyromètres de la
entrale inertielle CHR.
Les
ourbes roses représentent les estimations des angles d'Euler par le ltre
de Kalman 13 états en utlisant les sorties des gyromètres et des a
éléro-
mètres de la
entrale inertielle CHR. Et les
ourbes vertes représentent les
estimations des angles d'Euler par le ltre de Kalman 13 états en utlisant
les sorties des gyromètres, des a
éléromètres et des magnétomètres de la
entrale inertielle CHR.
80
CHR
SBG
13 states gyro
60 13 states gyro acc
13states gyro acc mag
40
20
0
phi angle(°)
−20
−40
−60
−80
−100
0 10 20 30 40 50 60 70 80
time(s)
21
CHR
40 SBG
13 states gyro
13 states gyro acc
13states gyro acc mag
30
20
10
phi angle(°)
−10
−20
−30
−40
40 45 50 55 60 65 70 75 80
time(s)
22
100
CHR
SBG
13 states gyro
80 13 states gyro acc
13states gyro acc mag
60
40
theta angle(°)
20
−20
−40
−60
−80
0 10 20 30 40 50 60 70 80
time(s)
50
CHR
SBG
13 states gyro
40 13 states gyro acc
13states gyro acc mag
30
20
theta angle(°)
10
−10
−20
−30
40 45 50 55 60 65 70 75 80
time(s)
SBG.
23
400
CHR
SBG
13 states gyro
13 states gyro acc
13states gyro acc mag
300
200
psi angle(°)
100
−100
−200
0 10 20 30 40 50 60 70 80
time(s)
80
CHR
SBG
13 states gyro
60 13 states gyro acc
13states gyro acc mag
16states gyro acc mag
40
20
0
phi angle(°)
−20
−40
−60
−80
−100
0 10 20 30 40 50 60 70 80
time(s)
24
20 CHR
SBG
13 states gyro
15 13 states gyro acc
13states gyro acc mag
16states gyro acc mag
10
5
phi angle(°)
−5
−10
−15
−20
56 58 60 62 64 66 68 70 72 74
time(s)
100
CHR
SBG
13 states gyro
80 13 states gyro acc
13states gyro acc mag
16states gyro acc mag
60
40
theta angle(°)
20
−20
−40
−60
−80
0 10 20 30 40 50 60 70 80
time(s)
25
50
CHR
SBG
13 states gyro
40 13 states gyro acc
13states gyro acc mag
16states gyro acc mag
30
20
theta angle(°)
10
−10
−20
−30
40 45 50 55 60 65 70 75 80
time(s)
400
CHR
SBG
13 states gyro
13 states gyro acc
13states gyro acc mag
300
16states gyro acc mag
200
psi angle(°)
100
−100
−200
0 10 20 30 40 50 60 70 80
time(s)
26
périeure sur la norme du
hamp magnétique mesuré. Si la norme ou la va-
riation entre deux instants
onsé
utifs de
hamp magnétique mesuré dépasse
les bornes, on met le bruit de mesure de magnétomètres à l'inni (108 dans
le
ode i
i). De
e fait, le ltre de Kalman n'utilise plus le mesure du
hamp
et passe au ltre de Kalman ave
les mesures des gyromètres et des a
éléro-
mètres uniquement. Théoriquement la norme de
hamp magnétique mesuré
~ doit être égale à
elle du
hamp magnétique terrestre ~h
omme
'est la
m
mesure du même
hamp, il n' y a que l'orientation qui est
hangée. Ainsi
théoriquement, nous pouvons dire que si la norme du
hamp magnétique
mesuré est diérente
elle du
hamp terrestre, on n'utilisera plus les mesures
du
hamp magnétique. Par
ontre, dans la réalité, la
entrale inertielle CHR
n'est pas assez parfait pour donner une bonne estimation du
hamp magné-
tique terrestre, on risque à laisser tomber les informations de
hamp qui sont
utiles pour le ltre de Kalman étendu. Ainsi, nous pouvons é
rit que si
~ > nnormk~hk
kmk (2.3)
~ < nnormk~hk
kmk (2.4)
σm = ∞ (108 dans le
ode) et on adapte la valeure de nnorm
omme l'on
veut. L'auteur propose de
hoisir nnorm entre 1.2 et 1.6. Notons qu'il faut
mettre des bornes dans deux sans par
e que le
hamp magnétique parasite qui
vient perturber l'engin peut donner un
hamp négatif. Nous allons mettre
es bornes dans le
ode de ltre de Kalman et l'appeler ltre de Kalman
robuste. Puis nous testons
e ltre robust par le résultat de la mesure des
angles d'Euler par la
entrale CHR dans le milieu fortement pertubé par du
hamp magnétique extérieur. En eet pour pertuber la
entrale, on amène
la
entrale près d'une grande bobine.
27
250
m
x
m
y
200
m
z
150
magnetic fields mesured by CHR (mGauss)
100
50
−50
−100
−150
−200
−250
0 10 20 30 40 50 60 70 80 90 100
time(s)
300
phi
theta
psi
200
100
Euler angles mesured by CHR (°)
−100
−200
−300
−400
0 10 20 30 40 50 60 70 80 90 100
time(s)
Figure 2.12 les angles d'Euler brutes estimés par la
entrale CHR dans
un milieu perturbé de
hamps magnétiques extérieurs
28
200
Kalman 16 states
Robust Kalman 16 states
CHR
150
100
50
phi angle(°)
−50
−100
−150
−200
0 10 20 30 40 50 60 70 80 90 100
time(s)
100
Kalman 16 states
Robust Kalman 16 states
80 CHR
60
40
20
theta angle(°)
−20
−40
−60
−80
−100
0 10 20 30 40 50 60 70 80 90 100
time(s)
29
300
Kalman 16 states
Robust Kalman 16 states
CHR
200
100
0
psi angle(°)
−100
−200
−300
−400
0 10 20 30 40 50 60 70 80 90 100
time(s)
80
SBG
gyro
gyro acc
60 13states
13states corrected
16states
40 16states corrected
20
0
phi angle(°)
−20
−40
−60
−80
−100
0 10 20 30 40 50 60 70 80
time(s)
30
SBG
25 gyro
gyro acc
13states
20 13states corrected
16states
16states corrected
15
10
phi angle(°)
−5
−10
−15
−20
56 58 60 62 64 66 68 70 72 74 76
time(s)
SBG
40 gyro
gyro acc
13states
13states corrected
30 16states
16states corrected
20
10
phi angle(°)
−10
−20
−30
−40
15 20 25 30 35 40 45 50
time(s)
31
100
SBG
gyro
gyro acc
80 13states
13states corrected
16states
60 16states corrected
40
theta angle(°)
20
−20
−40
−60
−80
0 10 20 30 40 50 60 70 80
time(s)
SBG
30 gyro
gyro acc
13states
13states corrected
25 16states
16states corrected
20
15
theta angle(°)
10
−5
−10
56 58 60 62 64 66 68 70 72 74
time(s)
32
200
SBG
gyro
gyro acc
150 13states
13states corrected
16states
16states corrected
100
50
psi angle(°)
−50
−100
−150
−200
0 10 20 30 40 50 60 70 80
time(s)
100
SBG
gyro
gyro acc
80 13states
13states corrected
16states
60 16states corrected
40
20
psi angle(°)
−20
−40
−60
−80
40 45 50 55 60 65 70 75
time(s)
33
Chapitre 3
Con
lusion
L'estimation des angles d'Euler par la
entrale inertielle peut être amé-
liorée grâ
e à l'implémentation de ltre de Kalman étendu, soit 13 états soit
16 états. Si l'engin n'est pas perturbé par un grand
hamp magnétique pa-
rasite, l'utilisation des mesures de magnétomètres est intéressante pour le
ltre de Kalman utilisé. Par
ontre, dans un milieu fort perturbé, il faut lais-
ser tomber
es mesures et passe au ltre qui utilise uniquement les mesures
des gyromètres et des a
éléromètres. Pour
ela, nous pouvons implémenter
la méthode de robustesse proposée dans
e projet, en mettant les bornes
supérieures sur la norme du
hamp magnétique mesuré.
34
de MATLAB qui est plus rigoureux que le règlage à la main fait dans
e projet.
Comparer la performan
e des ltres en traçant l'évolution des innova-
tions de
haque ltre peut être un bon
ritère et plus rigoureux que
dire
e ltre est mieux que
e ltre, et
.
35
Bibliographie
Angelo M. Sabatini, Quaternion-Based Extended Kalman Filter for De-
termining Orientation by Inertial and Magneti
Sensing, IEEE TRANSAC-
TIONS ON BIOMEDICAL ENGINEERING, VOL. 53, NO. 7, JULY 2006.
LIU Cheng et al, Attitude Determination for MAVs Using a Kalman Fil-
ter, TSINGHUA SCIENCE AND TECHNOLOGY, pp593-597, Volume 13,
Number 5, O
tober 2008.
Benoît Huyghe et al, 3D Orientation Tra
king Based on Uns
ented Kalman
Filtering of A
elerometer and Magnetometer Data, IEEE Sensors Appli
a-
tions Symposium New Orleans, LA, USA - February 17-19, 2009.
36