Vous êtes sur la page 1sur 11

FILTRAGE DE KALMAN APPLIQUE A LA

NAVIGATION INERTIELLE
Olivier BONNET TORRES
Onera-DCSD
15 dcembre 2003
Le ltrage de Kalman fait appel la reprsentation dtat des systmes. Il d-
coule de certaines approches menes dans le contexte des ordinateurs analogiques
(ltre de Wiener) pour la rsolution de problmes destimation de processus sto-
chastiques en rgime tabli. Lavnement de lordinateur digital a permis une sim-
plication et une extension de lapproche : le ltre de Kalman. La version classique
du ltre de Kalman est optimale pour les problmes linaires, avec une approche
temporelle discrte ou continue. Nous prsenterons la version tendue du ltre, qui
traite des problmes non linaires, sur un exemple de la navigation inertielle.
1 Les quations du ltre de Kalman classique
Le ltrage, ou estimation, de Kalman utilise la reprsentation dtat. La partie
stochastique est modlise par des bruits blancs gaussiens.
1.1 Le temps discret
Modle dynamique du systme, ou quation dtat :
x
k
=
k1
.x
k1
+v
k1
+G
k
.u
k
avec x
k
vecteur dtat linstant t
k

k
matrice de transition
G
k
matrice de commande
u
k
vecteur de commande
v
k
bruit de modle, blanc gaussien (distribution normale de covariance Q
k
) :
v
k
N (0, Q
k
)
1
Modle de mesure :
z
k
= H
k
.x
k
+w
k
+D
k
.u
k
avec z
k
vecteur de mesure linstant t
k
H
k
matrice de mesure
D
k
matrice dobservation de la commande
w
k
bruit de mesure, blanc gaussien (distribution normale de covariance R
k
) :
w
k
N (0, R
k
)
Les conditions initiales se rsument :
x
0
= E(x
0
)
P
0
= E( x
0
. x
T
0
),
o E(.) est compris au sens de lesprance mathmatique,
x
0
est lestime de x t
0
,
x
k
= x
k
x
k
,
P
k
est la covariance de lerreur destimation.
Lhypothse dindpendance des bruits de mesure et de modle est prise, ce
qui se traduit par j, k, E(w
k
.v
T
j
)
k, j
,
.,.
tant le symbole de Kronecker. Lors
du dveloppement du ltre, voire du fonctionnement, on veillera ce que lhypo-
thse soit vrie. En effet, une dpendance se traduit par une singularit dans les
matrices et les calculs ne sont plus possibles.
Les calculs sont faits de faon itrative et incrmentale. En fait, les calculs font
intervenir des matrices intermdiaires, auxquelles on attribuera un indice par
opposition aux matrices nales, distingues par un +.
Supposons que nous connaissons toutes les matrices nales linstant t
k1
.
Voici comment propager lestimation au pas suivant.
Prdiction de ltat (ou extrapolation du vecteur dtat) :
x

k
=
k1
. x
+
k1
+G
k1
.u
k1
Cet tat extrapol permet de fournir une estime intermdiaire. En utilisant un pas
de discrtisation de deuxime niveau, entre t
k1
et t
k
, on peut alors fournir des
estimes plusieurs instants entre deux calculs du ltre.
Prdiction de la covariance de lerreur destimation (ou extrapolation de la co-
variance destimation) :
P

k
=
k1
.P
+
k1
.
T
k1
+Q
k1
,
2
Calcul du gain de Kalman :
K
k
= P

k
.H
T
k
.(H
k
.P

k
.H
T
k
+R
k
)
1
Correction de la covariance derreur destimation (ou mise jour de la cova-
riance destimation) :
P
+
k
= (I K
k
.H
k
).P

k
,
que, pour des raisons de sensibilit aux erreurs de modlisation du problme, lon
trouve aussi crit de la faon suivante :
P
+
k
= (I K
k
.H
k
).P

k
.(I K
k
.H
k
)
T
+K
k
.R
k
.K
T
k
.
Cest la forme de Joseph qui garantit la fois la conservation de la symtrie et le
caractre dni-positif de la matrice de covariance.
Correction de ltat (ou mise jour observationnelle de ltat estim) :
x
+
k
= x

k
+K
k
.(z
k
D
k
.u
k
H
k
. x

k
)
1.2 Le temps semi-continu
Nous venons de voir le fonctionnement du ltre de Kalman en temps discret.
Par temps discret, nous entendons que lvolution du systme nest pas prise en
compte de faon continue par la machine informatique qui effectue les calculs et
que les mesures ne sont disponibles qu certains instants et non en continu. Physi-
quement, les limitations des systmes informatiques embarqus proviennent plutt
des temps de traitement des donnes des capteurs dans les convertisseurs. Dautre
part, lheure actuelle, les machines sont sufsamment rapides pour pouvoir effec-
tuer des calculs trs grande vitesse. Une approche entirement continue du ltre
de Kalman semble donc noffrir que peu dintrt pour linstant. . .En revanche, une
approche semi-continue, qui autorise une volution continue du modle du systme
tout en conservant un chantillonnage des mesures, est tout--fait pertinente. Voici
les principales quations du ltre de Kalman adaptes un traitement temporel
semi-continu.
Modle dynamique du systme :

X(t) = F(t).X(t) +G(t).U(t) +v(t)


Modle de mesure :
Z(t) = H(t).X(t) +D(t).U(t) +w(t)
Les relations entre le systme discret et le systme continu seules les mesures
restent discrtes sont les suivantes :

k
=L
1
{(s.I F)
1
}(t = k.T
s
),
3
avec L
1
la tranforme de Laplace inverse,
s la variable de Laplace,
T
s
la priode dchantillonnage.
En ralit, il ne sagit que dune possibilit dintgration, lquation relle est :

k
=

i=0
Z
t
k+1
t
k
F()
i
i!
d = exp
Z
t
k+1
t
k
F()d
Cette formule peut galement sappliquer H
k
et H(t).
G
k
.u
k
=
Z
t
k+1
t
k
().G().U()d
soit
G
k
=
Z
t
k+1
t
k
().G()d
si U(t) est constant entre deux pas dchantillonnage conscutifs.
2 Le ltre de Kalman tendu appliqu la navigation iner-
tielle
La navigation inertielle requiert dutiliser une reprsentation du temps semi-
continue pour des raisons de disponibilit des diffrentes mesures, par exemple les
acclromtres, les gyromtres, les magntomtres ou les donnes GPS. Dautre
part, les quations de la dynamique du problme ne sont plus du tout linaires.
Dans ce cas il devient ncessaire dutiliser soit une version linarise autour dun
tat dquilibre (dont nous ne parlerons pas ici), soit une version linarise autour
de ltat courant. Cette dernire approche constitue le ltre de Kalman tendu.
2.1 Le vecteur dtat et les quations dtat
On choisit le vecteur dtat suivant :
X
T
= ( u v w
. .
x y z
. .
q
0
q
1
q
2
q
3
. .

p

q

r
. .

x

y

z
. .
),
V Q
o V est la vitesse dans les axes mobiles,
est la position en coordonnes terrestres projetes plat,
Q est le quaternion des attitudes, que lon prfre aux angles dEuler
1
,
est le vecteur des biais gyromtriques,
est le vecteur des biais acclromtriques.
4
La dynamique non linaire scrit de la faon suivante :

X(t) = f (t, X(t), u(t)).
Le problme se pose de dnir la fonction f dans le cas de la navigation inertielle.
On doit crire la transformation C qui permet de passer de la base terrestre la
base associe au mobile :
_
_

_
_
terr
=C.
_
_

_
_
mob
.
C est la matrice de passage de la base mobile vers la base terrestre et sexprime en
fonction des composantes des quaternions.
Exprimons maintenant par groupes dtats les diverses quations de la dyna-
mique du systme. Lvolution des vitesses est rgie par lquation de laccl-
ration dans la base mobile. On notera bien quil faut tenir compte de la gravit.
Lexpression de lacclration non gravitationnelle dans le repre mobile, seule
mesure par les acclromtres, prend la forme :
A =
_
_
a
x
a
y
a
z
_
_
accel
=
_
V
t
_
mobile
+
rel
V C
T
.
_
_
0
0
g
_
_
avec C
1
=C
T
. Du coup, on a :
_
V
t
_
mobile
=
_
_
u
v
w
_
_
=
_
_
a
x
a
y
a
z
_
_
accel
+C
T
.
_
_
0
0
g
_
_
() V.
La correction des vitesses angulaires mesures
gyro
permet de prendre en compte
les biais des gyromtres . est exprim dans la base mobile : =
gyro
. Pour
mmoire, on a :
(
gyro
) V =
_
_
(
y

y
).w(
z

z
).v
(
z

z
).u(
x

x
).w
(
x

x
).v (
y

y
).u
_
_
.
Nous avons signal que C est la matrice de passage de la base mobile la base
terrestre : (terr) = C. (mobile). Les formules des quaternions donnent la matrice
1
La drivation des quaternions revient raliser une multiplication de matrices de dimension
4, nettement plus simple que la drivation des formules faisant intervenir les angles dEuler ,
et . Dautre part, grce une proprit particulire des quaternions de rotation, il ny a pas de
conguration singulire : la description des congurations par les quaternions est complte.
5
des cosinus directeurs :
C =
_
_
(q
2
0
+q
2
1
+q
2
2
+q
2
3
) 2(q
1
q
2
q
0
q
3
) 2(q
0
q
2
+q
1
q
3
)
2(q
1
q
2
+q
0
q
3
) (q
2
0
q
2
1
+q
2
2
q
2
3
) 2(q
2
q
3
+q
0
q
1
)
2(q
1
q
3
q
0
q
2
) 2(q
0
q
1
+q
2
q
3
) (q
2
0
q
2
1
q
2
2
+q
2
3
)
_
_
.
En drivant le vecteur position, on trouve :

t
=C.V.
La drivation des quaternions fait apparatre une partie arbitraire dans la direc-
tion du quaternion driver. On dcide dutiliser le facteur k., avec = 1(q
2
0
+
q
2
1
+q
2
2
+q
2
3
) et k
1
t
, o t dsigne le temps entre deux calculs conscutifs de la
drive. Le quaternion driv scrit donc :

Q = f onc(
gyro
)
=
1
2
_
_
_
_
q
1
q
2
q
3
q
0
q
3
q
2
q
3
q
0
q
1
q
2
q
1
q
0
_
_
_
_
.
_
_

z
_
_
+k.
_
_
_
_
q
0
q
1
q
2
q
3
_
_
_
_
En ce qui concerne les biais, il existe deux modles (simples) couramment
utiliss. Il faut se rappeler que le systme est tudi dans le contexte probabiliste.
On peut donc considrer que le biais est une variable alatoire dont lesprance
ne dpend pas du temps : il sagit dun bruit blanc gaussien ajout une valeur
moyenne. Pour se rapprocher de la ralit, il peut convenir de mettre en uvre un
modle derreur plus complexe : lvolution du biais en fait de sa valeur moyenne
suit une loi du premier ordre. On appelle ce modle Random Walk (marche au
hasard). Il est intressant de noter que lobservation du comportement des capteurs
inertiels (surtout les gyromtres) montre que lerreur prpondrante de ces capteurs
est due un bruit de type Random Walk. Dans le premier cas, lquation rgissant
lvolution du biais sera :
=(t),
o (t) est un bruit blanc gaussien. Dans le deuxime modle, on crira plutt :
=
_
1

_
.+(t),
avec (t) bruit blanc gaussien,
et
_
1

_
=
_
_
1/
x
0 0
0 1/
y
0
0 0 1/
z
_
_
.
6
A noter que la matrice des constantes de temps est diagonale, dans le respect de la
condition dindpendance des bruits derreur. Le premier modle est utilis lorsque
les capteurs sont de bonne qualit par rapport la prcision de lestimation recher-
che. Dans le cadre de la navigation inertielle, cest le cas dacclromters de
bonne qualit. En revanche, les gyromtres se comportent gnralement beaucoup
moins bien et leur sensibilit la temprature, entre autres, requiert lutilisation du
second modle.
2.2 Linarisation autour de ltat courant estim

X =
_
_
_
_
_
_

Q


_
_
_
_
_
_
=
_
_
_
_
_
_
(
gyro
) 0 A
v
Q
A
V

I
C 0 A

Q
0 0
0 0 A
Q
A
Q

0
0 0 0 0 0
0 0 0 0 0
_
_
_
_
_
_
.
_
_
_
_
_
_
V

_
_
_
_
_
_
.
Explicitons les termes en A de la matrice :
A
v
Q
=
C
T
Q
.
_
_
0
0
g
_
_
=

Q
_
_
2(q
1
q
3
q
0
q
2
).g
2(q
0
q
1
+q
2
q
3
).g
(q
2
0
q
2
1
q
2
2
+q
2
3
).g
_
_
= 2g.
_
_
q
2
q
3
q
0
q
1
q
1
q
0
q
3
q
2
q
0
q
1
q
2
q
3
_
_
,
A
V

=
(V)

_
_

z
.v +
y
.w

x
.w+
z
.u

y
.u+
x
.v
_
_
=
_
_
0 w v
w 0 u
v u 0
_
_
,
A

Q
=
(C.V)
Q
=
_
C
Q
_
.V
=2
_
_
q
0
uq
3
v +q
2
w q
1
u+q
2
v +q
3
w q
2
u+q
1
v +q
0
w q
3
uq
0
v +q
1
w
q
3
u+q
0
v q
1
w q
2
uq
1
v q
0
w q
1
u+q
2
v +q
3
w q
0
uq
3
v +q
2
w
q
2
u+q
1
v +q
0
w q
3
u+q
0
v q
1
w q
0
u+q
3
v q
2
w q
1
u+q
2
v +q
3
w
_
_
,
A
Q
=
1
2

Q
_
_
_
_
q
1
(
x

x
) q
2
(
y

y
) q
3
(
z

z
) +2kq
0
q
0
(
x

x
) q
3
(
y

y
) +q
2
(
z

z
) +2kq
1
q
3
(
x

x
) +q
0
(
y

y
) q
1
(
z

z
) +2kq
2
q
2
(
x

x
) +q
1
(
y

y
) +q
0
(
z

z
) +2kq
3
_
_
_
_
7
=
1
2
_
_
_
_
2k (
x

x
) (
y

y
) (
z

z
)
(
x

x
) 2k (
z

z
) (
y

y
)
(
y

y
) (
z

z
) 2k (
x

x
)
(
z

z
) (
y

y
) (
x

x
) 2k
_
_
_
_
,
A
Q

=
1
2
_
_
_
_
q
1
q
2
q
3
q
0
q
3
q
2
q
3
q
0
q
1
q
2
q
1
q
0
_
_
_
_
.
2.3 Mise en uvre du ltre
La mise en uvre du ltre se passe deux niveaux : la propagation dont nous
allons parler tout de suite et la mise jour objet du prochain paragraphe.
2.3.1 Propagation
La propagation constitue la partie temps continu (ou plutt quasi-continu) du
ltre. Il sagit dintgrer haute frquence (en gnral entre 20 et 200 Hz) les
drives du vecteur dtat et de la covariance dtat. Lintgration peut se faire par
la mthode des rectangles : X
i+1
= X
i
+

X
i
.dt. Mais pour les applications haute
dynamique, il est possible dutiliser des mthodes plus labores, les mthodes
Runge-Kutta dordre 2 ou 4.
On commence gnralement par propager le vecteur dtat X. Puis on soccupe
de la matrice de covariance dtat P. En notant A la jacobienne de X, on utilise
lquation de propagation suivante :
P
i+1
= P
i
+(A
i
.P
i
+(A
i
.P
i
)
T
+Q(t)).dt,
o Q(t) est la matrice de covariance du bruit de modle. Cette formule est valide
lorsque le pas dintgration est petit. Lintrt de cette formulation est quelle pr-
serve la fois la symtrie de la matrice et son caractre dni-positif, mme en cas
derreurs darrondi au moment des calculs. Dautres moyens existent pour garantir
la symtrie de la matrice : on peut calculer uniquement la partie triangulaire sup-
rieure et dupliquer les rsultats pour peupler la partie infrieure, ou on peut passer
par une dcomposition LU. Comme les variations de P sont plus lentes que celles
de X, on utilise gnralement un pas dintgration lgrement plus grand pour ne
pas manipuler trop de matrices. Au cas o le pas dintgration est trop grand, on
prfrera une formulation proche du cas continu. P quantie la conance que lon
peut apporter lestime du vecteur dtat X.
8
2.3.2 Traitement des mesures de recalage : la mise jour
La mise jour des lments du ltre se fait grce la prise en compte des infor-
mations donnes par les mesures. Suivant la disponibilit des mesures, dans notre
cas en temps discret et pas ncessairement priodique, le recalage de lestimation
se fait relativement basse frquence (de lordre du hertz pour un GPS, dix fois
plus vite pour un capteur magntique par exemple).
Nous proposons, en vue doptimiser la taille du code embarqu, un traitement
squentiel des mesures suivant leur origine : vitesse GPS, position GPS, champ
magntique... Si la capacit de calcul du calculateur embarqu est trop faible pour
raliser des inversions en dimension 3, on peut dcoupler encore chacun des axes de
chaque capteur pour se retrouver avec des mesures scalaires. Du coup les matrices
traiter seront scalaires. On se rappelera que la dimension du vecteur dtat actuel
est de 16. Commenons par la prise en compte de la vitesse GPS.
Z
vit
=
_
_
Z
1
Z
2
Z
3
_
_
=
_
_
V
gpsN
X
1
V
gpsE
X
2
V
gpsD
X
3
_
_
.
Puis on calcule la matrice H :
H
vit
=
_
_
1 0 0 . . . 0
0 1 0 . . . 0
0 0 1 . . . 0
_
_
.
Ensuite on calcule le gain de Kalman :
K
vit
= P.H
T
vit
(H
vit
.P.H
T
vit
+R
vit
)
1
,
o la covariance du bruit de mesure est :
R
vit
=
_
_
R
1
0 0
0 R
2
0
0 0 R
3
_
_
.
La correction de P est ltape suivante :
P
n+1
= P
n
K
vit
.H
vit
.P
n
.
Enn on corrige le vecteur dtat :
X
n+1
= X
n
K
vit
.Z
vit
.
Cette mthode est applique ici la mesure de la vitesse GPS. Mais on peut
la transposer la mesure de position GPS. Contrairement aux vitesses donnes
9
en mtres par seconde vers le nord, lest et le bas, celle-ci propose un reprage
latitude-longitude-altitude (, , Z
alt
) respectivement en radians et en mtres. On
utilisera une approche relative par rapport au point de dpart en ce qui concerne la
latitude et la longitude.
Z
pos
=
_
_
(
GPS

0
).k X
4
(
GPS

0
). cos
GPS
.k X
5
Z
alt
X
6
_
_
.
La matrice H correspondante est :
H
pos
=
_
_
0 0 0 1 0 0 . . . 0
0 0 0 0 1 0 . . . 0
0 0 0 0 0 1 . . . 0
_
_
.
Le reste de la mise jour se fait de la mme faon.
Pour la mesure du champ magntique, on utilise les relations suivantes. Le
champ magntique terrestre, suppos connu en tout point, est not E
T
. La mesure
donne par le magntomtre est dsigne par E
mag
.
Z
mag
=
_
_
E
mag1
E
mag2
E
mag3
_
_
+(C
T
.E
T
)
T
V Q
H
mag
= ( 0 0
C
Q
0 0 ),
avec :
C
Q
=2
_
_
(q
0
.E
1
+q
3
.E
2
q
2
.E
3
) (q
1
.E
1
+q
2
.E
2
+q
3
.E
3
) (q
2
.E
1
+q
1
.E
2
q
0
.E
3
) (q
3
.E
1
+q
0
.E
2
+q
1
.E
3
)
(q
3
.E
1
+q
0
.E
2
q
1
.E
3
) (q
2
.E
1
q
1
.E
2
+q
0
.E
3
) (q
1
.E
1
+q
2
.E
2
+q
3
.E
3
) (q
0
.E
1
q
3
.E
2
+q
2
.E
3
)
(q
2
.E
1
q
1
.E
2
+q
0
.E
3
) (q
3
.E
1
q
0
.E
2
q
1
.E
3
) (q
0
.E
1
+q
3
.E
2
q
2
.E
3
) (q
1
.E
1
+q
2
.E
2
+q
3
.E
3
)
_
_
.
La correction de X fait intervenir un terme de rotation :
X
n+1
= X
n
+K
mag
.(Z
mag
H
mag
.X
n
).
Un des intrts de traiter squentiellement les mesures est la manipulation de
matrices de taille moindre. Dautre part, il nest pas ncessaire dobtenir toutes les
mesures au mme instant pour les traiter.
10
2.4 Initialisation
Maintenant que nous avons vu lensemble des quations permettant le fonc-
tionnement du ltre de Kalman, il nous reste linitialiser. Cette initialisation est
plutt longue : il nest pas possible dallumer le calculateur du mobile dans la se-
conde qui prcde le dpart.
Linitialisation passe par une phase de calcul statique. Le mobile est larrt et
le ltre teint. A partir des donnes manant des capteurs, et des informations lies
la position et la staticit du mobile, on met les vitesses linaires et angulaires
zro, tout en tenant compte de la rotation de la Terre, on prinitialise lattitude grce
la mesure de lacclration
2
, on dduit de la moyenne des mesures capteurs une
estimation grossire mais ncessaire des biais acclromtriques et gyromtriques.
Dans un second temps, le ltre de Kalamn est mis en route. Mais les informa-
tions fournies court-circuitent les donnes capteurs. Typiquement, les vitesses sont
toujours donnes comme nulles. Cette phase permet de calculer la covariance des-
timation tout en utilisant des informations considres comme trs sres : le ltre
converge vers un tat. La troisime phase permet de prendre en compte les donnes
vraies, issues des capteurs, et le ltre peut dsormais prendre en compte leur bruit
intrinsque. Aprs cette dernire tape, le ltre est prt fonctionner. A chacune
de ces phases est associ un temps, ou un nombre de mesures, prendre en compte
pour que ltape ralise son but. Ces temps de calcul dpendent de la qualit des
capteurs, de la prcision du modle et de la prcision requise en sortie. Ils sont
donc ajuster pour chaque application.
2
En ralit, le mobile peroit la raction de la Terre son poids.
11