Les ltres de Kalman

Caroline Keramsi Thibault Porteboeuf Florian Thorey

mars 2011
Table des matires
1 Introduction 1
2 Le ltre de Kalman conventionnel
1
2
2.1 Prliminaires . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
2.2 Hypothses . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
2.3 Dveloppement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.4 Simplication du ltre de Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
3 Limites du ltre de Kalman 4
3.1 Un constat . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
3.2 Des solutions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
4 Implantations alternatives du ltre de Kalman 5
4.1 Filtre de Kalman squentiel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
4.2 Le ltre de Kalman informationnel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
5 Techniques avances 7
5.1 Prcision et robustesse . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
5.1.1 Filtrage par racine carre . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
5.1.2 Filtrage U-D . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
5.1.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
5.2 Contrler ltat du ltre de Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
5.3 Filtre de Kalman pour les systmes non-linaires . . . . . . . . . . . . . . . . . . . . . . . 12
1 Introduction
Le ltre de Kalman est un outil mathmatique puissant et particulirement utile dans le monde
de lembarqu. Il permet en eet destimer ltat dun systme, en fonction de son tat prcdent, des
commandes appliques et de mesures bruites. Malheureusement son application dans le cadre dun
systme rel, pas forcment linaire et prsentant des ressources de calcul limites, pose un certain
nombre de problmes.
Pour pallier ces dicults, de nombreux ltres de Kalman alternatifs ont t dvelopps, tant et si bien
quil est dicile de sy retrouver. Lobjectif de cet article est de vous donner les cls pour comprendre
les ltres de Kalman et ainsi tre en mesure de construire le plus adapt au problme auquel vous
tes confronts. Dans un premier temps, nous allons vous prsenter le ltre de Kalman classique, celui
sur lequel tous les autres ltres se fondent. Ensuite, nous traiterons des direntes limites susceptibles
dapparatre, en particulier lors de limplantation du ltre dans un systme embarqu. Nous voquerons
alors les dirents moyens permettant de pallier ces problmes. Dans une dernire partie, nous aborderons
quelques techniques avances. Nous laissons volontairement de ct les ltres temps continu et les ltres
de lissage qui ne sont pas intressants dans le cadre de lembarqu.
1. ou ltre de Kalman temps discret
1
2 Le ltre de Kalman conventionnel
2
2.1 Prliminaires
Systme dynamique bruit : Systme qui volue au cours du temps et dont ltat courant dpend
de ltat prcdent, des commandes et dun processus alatoire.
De nombreux systmes rels peuvent tre modliss de cette manire. On peut prendre en exemple une
voiture, dont la position et la vitesse dpendent de la position et de la vitesse linstant prcdent, des
commandes entres par le conducteur (frein, acclrateur, volant), et de phnomnes alatoires tels des
bourrasques de vent, des nids-de-poule...
Mthode des moindres carrs : En partant dun nuage de points dans un espace n dimensions qui
correspondent des mesures bruites, la mthode des moindres carrs permet de trouver la modlisation
mathmatique linaire qui dcrit le mieux ces donnes. Pour cela on choisit le modle qui minimise une
fonction des carts quadratiques des points au modle.
La mthode classique des moindres carrs calcule ce modle dun seul coup en utilisant tous les points. Ces
calculs ncessitent la manipulation de matrice de la taille du nombre de points de mesure. Il nest donc
pas possible de lutiliser dans un systme embarqu puisque que la complexit des calculs augmenterait
constamment avec le temps et le nombre de mesures.
On utilise donc la mthode rcursive des moindres carrs qui permet de mettre jour le modle chaque
arrive dune nouvelle mesure. Les calculs se font alors avec une complexit constante au cours du temps.
Notations : Soit X une variable alatoire, on rappelle que lestimation de X se note usuellement

X.
Lerreur sur lestimation vaut X

X et se note

X.
2.2 Hypothses
Pour pouvoir appliquer le ltre de Kalman conventionnel il faut :
1. Un systme dynamique linaire bruit. Lvolution du systme peut donc scrire sous la forme :
x
k+1
= F
k
x
k
+G
k
u
k
+w
k
(1)
o H
k
et G
k
sont des matrices qui dpendent de la modlisation du systme, x
k
est un vecteur
reprsentant ltat du systme, u
k
est un vecteur reprsentant les commandes appliques et w
k
est
un bruit. Chaque terme est pris linstant k.
2. Des mesures, lies linairement ltat du systme, et bruites :
y
k
= H
k
x
k
+v
k
o y
k
est le vecteur contenant les mesures eectues linstant k, H
k
est une matrice, et v
k
le bruit
de mesure.
3. Des bruits blancs, centrs, indpendants, de matrices de covariance Q
k
, R
k
connues :
i, j, v
i
w
j
i = j, v
i
v
j
i = j, w
i
w
j
w
k
(0, Q
k
)
v
k
(0, R
k
)
2. ou ltre de Kalman temps discret
2
2.3 Dveloppement
Nous allons maintenant suivre la dmarche qui mne de ces hypothses au ltre de Kalman. Nous ne
nous arrterons pas sur les calculs intermdiaires
3
.
Si les hypothses ci-dessus sont runies, le ltre de Kalman est le ltre optimal pour lestimation de
ltat du systme. Il garantit que lerreur sur lestimation est en moyenne nulle et il minimise lincertitude
sur cette erreur. Mathmatiquement on a E[ x
k
] = 0 et il est prouv quil nexiste pas de ltre tel que la
matrice de covariance de lerreur E[ x
k
x
k
T
] soit infrieure en norme celle du ltre de Kalman.
Initialisation : Au temps k = 0, nous navons aucune mesure disponible. La premire mesure arrive
au temps k = 1. x
0
est une variable alatoire, il parait logique en labsence de mesure de lestimer par
son esprance. Nous avons donc :
x
0
= E[x
0
]
Le principe du ltre de Kalman est de minimiser la covariance de lerreur. Nous avons pour cela besoin
de la calculer. Elle vaut :
P
0
= E[(x
0
x
0
)(x
0
x
0
)
T
]
Notez que si vous connaissez parfaitement ltat de dpart du systme, x
0
= x
0
et donc P
0
est nulle.
Dans le cas totalement oppos o lon ne connat absolument pas ltat du systme, P
0
est de norme
innie, ce qui rend les calculs numriques impossibles puisquon ne peut pas coder linni en binaire.
Plaons nous maintenant au temps k. Supposons que nous disposions de x
+
k1
et de

P
+
k1
.
Nous allons chercher trouver lestimation de x
k
qui minimise P
k
. Nous allons procder en 2 tapes.
En premier lieu, nous allons faire une estimation a priori, note x

k
sans tenir compte de la mesure y
k
.
Nous mettrons ensuite jour cette estimation laide de la mesure pour obtenir ce que nous noterons
x
+
k+1
, lestimation a posteriori
volution du systme dynamique On a x

k
= E[x
k
] En utilisant lquation 1, on obtient :
x

k
= F
k1
x
k1
+G
k1
u
k1
(2)
La covariance de lerreur se dduit galement de lquation 1 :
P

k
= E[(x
k
x

k
)[(x
k
x

k
)
T
] = F
k1
P
k1
F
T
k1
+Q
k1
Prise en compte de la mesure : Nous allons maintenant utiliser la mesure y
k+1
pour corriger
lestimation a priori x

k
et obtenir lestimation a posteriori note x
+
k
. Nous allons pour cela utiliser le
thorme rcursif des moindres carrs. On obtient :
K
k
= P

k
H
T
k
(H
k
P

k
H
T
k
+R
k
)
1
(3)
= P
+
k
H
T
k
R
1
k
(4)
x
+
k
= x

k
+K
k
(y
k
H
k
x

k
) (5)
P
+
k
= (I K
k
H
k
)P

k
(I K
k
H
k
)
T
+k
k
R
k
K
T
k
(6)
= (I k
k
H
k
)P

k
(7)
On remarque que lestimation a priori est corrige par lajout du terme K
k
(y
k
H
k
x
k
). Celui-ci est
appel innovation car il reprsente les informations supplmentaires apportes par la mesure. En eet il
est proportionnel la dirence entre la mesure que nous avons obtenue et celle que nous aurions d
obtenir si la mesure et le processus lui-mme ntaient pas bruits.
Il est galement proportionnel K
k
, le gain de Kalman au temps k, qui traduit limportance relative
accorder la mesure par rapport lestimation a priori. On peut remarquer en regardant lquation
4, que la norme de K
k
est dautant plus grande que la norme de R
k
la covariance du bruit sur la
mesure est petite, et que la norme de la covariance de lerreur sur lestimation est grande. Autrement
dit, plus lincertitude sur lestimation est grande ou plus celle sur la mesure est petite, plus on accorde
dimportance la mesure plutt qu lestimation, et vice-versa.
Nous avons donc un processus itratif bas sur un schma de prdictioncorrection par la mesure.
3. Si ces calculs vous intressent, reportez vous aux chapitres 3,4 et 5 de Optimal State Estimation de Dan Simon
3
2.4 Simplication du ltre de Kalman
On peut noter que si H
k
, G
k
, Q
k
et R
k
sont des constantes, ce qui est souvent vrai en pratique,
on peut simplier le ltre de Kalman de faon importante. K ne dpend alors plus de k et peut-tre
pr-calcule. Pendant lexcution, le ltre de Kalman se rsume alors deux calculs, celui de x

k
puis
celui de x
+
k
, ainsi que des matrices dincertitude associes P

k
et P
+
k
. Cela facilite grandement les choses
pour un systme embarqu temps-rel.
3 Limites du ltre de Kalman
3.1 Un constat
Nous avons vu que le ltre de Kalman est un ensemble dquations trs ecace permettant dobtenir
le ltrage optimal du point de vue de lerreur faite sur lestimation de ltat du systme.
Cependant lors de limplantation du ltre sur un systme rel, par exemple un systme embarqu, deux
lments importants, absents de la thorie, surgissent et viennent dgrader les performances du ltrage.
Tout dabord, les processeurs chargs des calculs et permettant de mettre en place le ltre de Kal-
man ne disposent pas dune prcision numrique innie, seulement un nombre ni de bits sont utiliss
pour reprsenter les nombres. Ainsi des erreurs de calcul dues cette limitation sont susceptibles de se
produire tout moment et de se propager, pouvant ainsi mener la divergence du ltre.
Ensuite, la thorie prend pour acquis la connaissance parfaite des direntes matrices intervenant dans
la modlisation de lvolution de ltat du systme auquel est appliqu le ltrage de Kalman. Cependant,
il nest pas improbable que dans certains cas on ne puisse pas avoir accs toutes les informations sur
le systme et donc de ne disposer que dune connaissance imparfaite des direntes matrices intervenant
dans la modlisation, ce qui peut alors mener, l aussi, une divergence du ltre.
Enn, lors de ltude thorique du ltre de Kalman, un certain nombre dhypothses sont prises concer-
nant le bruit intervenant dans les quations. Notamment le fait que ce dernier doive tre blanc et ses
direntes composantes prises des instants dirents compltement dcorrles. Cependant, dans la
ralit ces hypothses ne sont jamais parfaitement vries et le ltrage est alors susceptible de ne pas
produire les rsultats escompts ou mme dchouer.
3.2 Des solutions
Il est cependant possible de mettre en place un certain nombre de stratgies permettant de palier les
dirents problmes soulevs dans la section 3.1.
1. Augmenter la prcision numrique.
2. Symtriser la matrice P chaque itration de calcul. P =
P+P
T
2
.
3. Utiliser un bruit de processus ctif.
4. Utiliser des implantations direntes du ltre de Kalman comme le square root ltering (dtaill
plus loin).
Avant de donner plus de dtails, Il est ncessaire de souligner le fait que lutilisation de telle ou telle
stratgie plutt quune autre dpend grandement du systme particulier sur lequel on dsire appliquer
le ltrage de Kalman. Telle solution sera plus adapte tel systme physique alors quelle ne sera pas
du tout adapte pour tel autre. A chaque systme sa solution propre.
Nous allons passer en revue ces direntes stratgies an que lutilisateur puisse choisir de manire
claire quelle stratgie lui convient le mieux.
1. Augmenter la prcision numrique peut tre une solution envisageable. En passant par exemple
la prcision de toutes les variables double sur une architecture de type PC. Cependant dans le
cadre dune utilisation sur un systme embarqu pour lequel les ressources sont trs limites cette
solution est souvent impossible mettre en place.
4
2. Daprs la thorie, la matrice P reprsentant la covariance de lerreur sur lestimation de ltat du
systme est par construction une matrice symtrique et dnie positive. Cependant, aprs quelques
erreurs de calcul dues par exemple la prcision nie des calculs voqus plus haut il est possible
que la matrice P perde lune ou lautre de ces caractristiques. An de sen prmunir on peut choisir,
soit de resymtriser explicitement cette matrice chaque tape, soit dutiliser des formes de calcul
de cette matrice ncessitant plus de calculs, mais garantissant la persistance de ces caractristiques
chaque itration.
3. Dans le cas dune connaissance imparfaite du systme, par exemple le biais intrinsque dun gyro-
scope, on peut ajouter la modlisation du systme un bruit de processus ctif. Ce bruit sera plus
ou moins important selon que notre ignorance de certaines caractristiques du systme modliser
est importante. Par construction, lajout de ce bruit permettra de signier au ltre de Kalman que
lors de ltape de correction, Il est ncessaire daccorder un poids plus important la mesure plutt
qu lestimation imparfaite faite lors de ltape destimation dans le calcul de linnovation.
On notera toutefois que lorsque le biais dun lment du systme drive au cours du temps, par
exemple le biais dun gyroscope, il vaut mieux lincorporer comme tant lune des composantes de
ltat du systme, ce qui permet de lintgrer dans le processus de prdiction-correction du ltre
de Kalman.
4. Il existe des implantations du ltre de Kalman direntes mais nanmoins plus robustes aux erreurs
dues la prcision nie des calculs. Ces implantations, mathmatiquement quivalentes celle du
ltre de Kalman conventionnel seront dvelopps dans les parties suivante. On peut par exemple
citer le square root ltering ou encore le U-D ltering.
4 Implantations alternatives du ltre de Kalman
4.1 Filtre de Kalman squentiel
Le ltre de Kalman squentiel est une forme drive du ltre de Kalman conventionnel. Le princi-
pal avantage de cette forme est quelle permet dimplanter le ltre sans ltape dinversion de matrice,
souvent coteuse en ressources et sources derreurs diverses. Cette implantation dirente nest possible
que sous certaines conditions que nous numrerons dans la suite. Cette forme du ltrage de Kalman
peut se rvler particulirement adapte au cas dune implantation sur des systmes embarqus, pour
lesquels les ressources en calcul sont assez limites et qui ne disposent pas de bibliothques optimisant
par exemple le calcul matriciel.
Linversion de matrice apparaissant dans la forme non squentielle du ltrage de Kalman provient du
fait que lors de la phase de correction de litration k de limplantation du ltre, on traite simultanment
lensemble des donnes provenant des dirents capteurs an dobtenir le gain de Kalman K
k
.
Ainsi si on dispose de r capteurs dirents, il sera ncessaire dinverser une matrice carre de taille r. Si
le nombre de capteurs est important, on se retrouve rapidement avec des calculs impossibles raliser
en pratique en embarqu.
Il est cependant possible de traiter de faon squentielle chaque mesure et de parvenir pas pas, en
traitant chaque mesure lune aprs lautre, au rsultat.
Dans le ltre de Kalman squentiel, on introduit un nouvel indice i qui permettra lors de la phase de
correction de dsigner chaque tape pendant laquelle on va prendre en compte la mesure du capteur i
(0 < i < r).
On fait donc voluer la valeur du vecteur dtat a posteriori x
+
ik
petit petit, de x
+
0k
x
+
rk
pour -
naliser litration k du ltrage de Kalman.
On a alors :
x
+
0k
= x

k
x
+
rk
= x
+
k
O x

k
est la valeur du vecteur dtat a priori litration k et x
+
k
est la valeur du vecteur dtat a pos-
teriori litration k. Il est ncessaire de procder de la mme manire pour la matrice P de covariance
5
de lerreur faite sur lestimation.
En rsum, pour le ltre de Kalman squentiel, ltape destimation est exactement la mme que pour
le ltre de Kalman conventionnel. On obtient x

k
exactement de la mme manire.
Cependant, an de tenir compte de lvaluation squentielle des r direntes mesures, il est ncessaire
dutiliser des formules direntes pour ltape de correction. Ces formules sont donnes ci-dessous :
Pour i allant de 1 r :
K
ik
=
P
+
ik
H
T
ik
R
ik
x
+
ik
= x
+
i1,k
+K
ik
(y
ik
H
ik
x
+
i1,k
)
P
+
ik
= (I K
ik
H
ik
)P
+
i1,k
Enn, pour pouvoir utiliser ces formules, un certain nombre dhypothses doivent tre vries. On
peut utiliser lalgorithme de Kalman squentiel si lune de ces deux conditions est vrie :
La covariance du bruit de processus R est diagonale
La covariance du bruit de processus R est constante mais non diagonale, auquel cas il est ncessaire
avant de procder lapplication de formules pour diagonaliser R
4.2 Le ltre de Kalman informationnel
Limplantation du ltre de Kalman informationnel est conceptuellement trs simple et relativement
proche de celle du ltre de Kalman conventionnel.
En eet dans le cas du ltrage informationnel, au lieu de propager la matrice P chaque itration
du ltrage, on propage son inverse : P
1
= I. Ceci permettant dans certain cas de simplier le problme.
Le nom de ltrage informationnel provient du fait que la matrice P par dnition reprsente la cova-
riance de lerreur faite sur lestimation. Elle nous donne donc une ide de lincertitude que lon a sur
notre estimation de ltat du systme. Ainsi son inverse I = P
1
nous donne, linverse, la certitude que
lon a sur lestimation de ltat du systme. Plus I = P
1
est petite, plus notre connaissance du systme
est imparfaite, plus I = P
1
est haute, mieux on connat le systme. Les cas limites tant :
I = P
1
= 0 Ltat du systme est compltement inconnu.
I = P
1
= Ltat du systme est connu parfaitement.
linverse :
P = 0 Ltat du systme est connu parfaitement.
P = Ltat du systme est compltement inconnu.
Lutilisation dune telle forme est particulirement adapte dans certains cas que nous dtaillerons par
la suite.Les formules permettant dimplanter le ltre de Kalman informationnel sont donnes ci-dessous,
ces dernires ne sont pas dmontres dans la mesure ou elles se dduisent directement de celles du ltrage
de Kalman conventionnel :
I

k
= Q
1
k1
Q
1
k1
F
k1
(I
+
k1
+F
T
k
Q
1
k1
F
k1
)
1
F
T
k1
Q
1
k1
I
+
k
= I

k
+H
T
k
R
1
k
H
k
K
k
= (I
+
k
)
1
H
T
k
R
1
k
x

k
= F
k1
x
+
k1
+G
k1
uk 1
x
+
k
= x

k
+K
k
(y
k
H
k
x

k
)
Limplantation du ltre de Kalman informationnel ne ncessite linversion que de matrices carres de
taille n, dans le cas o le nombre de mesures r est trs grand devant le nombre dtats du systme n
(r >> n), alors lutilisation du ltre de Kalman informationnel peut se rvler tre un trs bon choix.
De plus, si notre incertitude initiale sur le systme P
0
est totale,on a alors P
0
= , il est impos-
sible de reprsenter de faon numrique une telle valeur. Mais si P
0
est inni, alors on a : P
1
0
= 0 ce qui
linverse est reprsentable de faon triviale numriquement. Dans pareil cas on a donc aussi avantage
utiliser le ltrage de Kalman informationnel plutt que le ltrage de Kalman conventionnel.
6
5 Techniques avances
Comme nous lavons vu prcdemment, le ltre de Kalman peut poser certains problmes de stabilit
et de prcision, notamment lors dune intgration de lalgorithme un systme embarqu qui dispose de
capacits de calcul rduites.
Le but de cette partie est de prsenter des techniques avances se basant sur les proprits mathma-
tiques de certains composants du ltre de Kalman conventionnel an daugmenter la prcision ou encore
la robustesse.
Nous commencerons par dvelopper des techniques visant amliorer la prcision du ltre et la
robustesse aux erreurs de calcul inhrentes aux microprocesseurs, puis nous aborderons une technique
permettant de contrler ltat du ltre et enn nous aborderons le cas des systmes non linaires.
5.1 Prcision et robustesse
On sait daprs la thorie mathmatique expose dans louvrage [1] que la matrice de covariance P
doit vrier certaines proprits rappeles ci-dessous. Cependant, il est rare en pratique que la matrice
de covariance qui est calcule de faon approche par un microprocesseur digital vrie exactement ces
proprits.
Nous pouvons noter en guise dexemple le fait que pour un microprocesseur de faible prcision,
10
6
= 10
6
= 0. Ainsi certains coecients de la matrice peuvent, suite des erreurs de calculs, tre
nuls en pratique alors quils ne doivent pas ltre mathmatiquement.
Proprits La matrice de covariance P est symtrique, dnie positive. Cela implique donc en parti-
culier que P = P
T
, et que pour tout vecteur x = 0, x
T
Px > 0.
Une premire solution peut donc tre par exemple de forcer P =
P+P
T
2
chaque itration du ltre,
an de minimiser la propagation derreurs en imposant la symtrie de la matrice, mais il est possible
daller plus loin que cela.
tant donn les proprits de la matrice de covariance P, nous pouvons proposer plusieurs critures
ou factorisations qui vont nous amener de nouveaux ltrages intressants.
5.1.1 Filtrage par racine carre
La premire mthode permettant de doubler la prcision du calcul du ltre de Kalman provient dune
factorisation de la matrice de covariance sous la forme P = SS
T
.
Nous verrons dans un premier temps le ltre de Kalman associ cette factorisation, puis dans un
deuxime temps nous citerons un algorithme permettant de trouver une racine carre S partir dune
matrice P.
Lalgorithme :
Nous pouvons factoriser la matrice comme dcrit plus haut sous la forme P = SS
T
car elle est
symtrique dnie positive. Sachant donc quune telle matrice S existe, nous pouvons tenter dinjecter
cette criture dans les quations du ltre conventionnel rappeles ci-dessous :
x

k
= F
k1
x
+
k1
+G
k1
u
k1
(8)
P

k
= F
k1
P
+
k1
F
T
k1
+Q
k1
(9)
x
+
k
= x

k
+K
k
_
y
k
H
k
x

k
_
(10)
P
+
k
= (I K
k
H
k
) P

k
(I K
k
H
k
)
T
+K
k
R
k
K
T
k
(11)
Partant de lquation 9, on obtient lquation de mise jour temporelle (prdiction) suivante [1] :
S

k
_
S

k
_
T
= F
k1
S
+
k1
_
S
+
k1
_
T
F
T
k1
+Q
1
2
k1
_
Q
1
2
k1
_
T
(12)
7
Nous avons donc une premire quation de mise jour de la racine de P

k
qui dpend des matrices
du systme F
k1
et Q
k1
et de la racine de la matrice P
+
k1
que lon a not S
+
k1
.
De la mme manire, nous pouvons partir de lquation 11 pour obtenir une nouvelle quation de
mise jour par la mesure (correction). La mthode qui en rsulte est dcrite sous forme dun algorithme
squentiel relativement adapt lembarqu comme suit :
Lalgorithme de mise jour par la mesure de Potter
1. Une fois que la racine de la covariance a priori S

k
et lestimateur de ltat du systme a priori x

k
a t calcule, initialiser le processus squentiel comme suit :
x
+
0k
= x

k
S
+
0k
= S

k
2. On considre que les r mesures provenant de lensemble des capteurs ont t collectes un instant
k, et mises dans un vecteur (en pratique un buer) not y
k
. Lensemble des mesures prises linstant
k est donc y
ik
, i 1, r. On note donc y
ik
la i-me mesure prise linstant k, H
ik
la i-me ligne
de la matrice H
k
qui est donc celle qui lie ltat du systme la i-me mesure et enn R
ik
la variance du bruit associ cette i-me mesure. On eectue alors les oprations suivantes :
(a) Calculer les lments suivants :

i
= S
+T
i1,k
H
T
ik
a
i
=
1

T
i

i
+R
ik

i
=
1
1

a
i
R
ik
S
+
ik
= S
+
i1,k
_
I a
i

T
i
_
(b) Calculer le gain de Kalman pour cette mesure :
K
ik
= a
i
S
+
ik

i
(c) Mettre jour lestimation a posteriori grce linformation provenant de cette i-me mesure :
x
+
ik
= x
+
i1,k
+K
ik
_
y
ik
H
ik
x
+
i1,k
_
3. Une fois les r calculs prcdents eectus, en dduire la covariance et lestimation a posteriori
comme tant :
S
+
k
= S
+
rk
x
+
k
= x
+
rk
Il est donc possible en combinant lquation 12 et lalgorithme de mise jour par la mesure de Potter
prcdemment dcrit de doubler la prcision du ltre de Kalman. Lalgorithme dcrit ici est squentiel,
ce qui vite davoir intgrer des outils dinversion de matrices au systme embarqu.
On peut toutefois noter que cette augmentation de la prcision du ltre se fait bien entendu au prix
dune complexit de calcul accrue en comparaison avec un ltre de Kalman squentiel classique.
Recherche de racines carres :
Un premier algorithme permettant de calculer la racine carre dune matrice a t donn par Andr
Cholesky. Le pseudo-code dcrit dans la littrature ce sujet [1] est nonc comme suit :
8
Algorithm 1 Algorithme de Cholesky
for i = 1,i<n,i++ do
S
ii
=
_
P
ii

i1
j=1
S
2
ij
for j=1,j<n,j++ do
S
ji
= 0 pour j < i
S
ij
=
1
S
ii
_
P
ji

i1
k=1
S
jk
S
ik
_
pour j > i
end for
end for
Dautres algorithmes existent, mais celui-ci a notamment lavantage dtre trs simple intgrer dans
un systme embarqu.
5.1.2 Filtrage U-D
Dans cette partie nous allons utiliser une factorisation dirente de la matrice de covariance, an l
encore de driver un algorithme squentiel de prcision double.
Partant des proprits de la matrice de covariance P nonces plus haut, on sait que nous pouvons
factoriser cette matrice sous la forme P = UDU
T
, o U est une matrice triangulaire suprieure de
coecients diagonaux gaux 1, et D une matrice diagonale.
Les calculs ne seront pas dtaills ici, la littrature les fournissant. Nous noncerons donc directement
lalgorithme.
Mise jour temporelle (prdiction a priori ) :
On suppose que lon a obtenu de lestimation prcdente ou encore de linitialisation une matrice
P
+
k1
= U
+
k1
D
+
k1
U
+T
k1
.
On dnit donc les matrices suivantes :
W
k
=
_
F
k
U
+
k
I

D
k
=
_
D
+
k
0
0 Q
k
_
partir de la matrice

D
k
, nous dnissons le produit scalaire entre x et y suivant : x

D
k
y
T
. On utilise
alors le procd de Gram-Schmidt pour calculer une famille orthogonale de vecteurs partir des vecteurs
colonne de la matrice W
k
et en utilisant le produit scalaire prcdemment dnit. Si on note (v
ik
) la
famille de ces vecteurs orthogonaliss et (w
ik
) la famille des vecteurs colonne de W, on utilise donc la
formule suivante :
v
ik
= w
ik

j=i+1
w
ik

D
k
v
T
jk
v
jk

D
k
v
T
jk
v
jk
On va se servir de ces vecteurs v
ik
pour calculer les coecients de la matrice triangulaire suprieure U

k
.
On dnit la matrice U

k
comme suit :
U

k
=
_

_
1 u
k
(1, 2) u
k
(1, n)
0 1
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
u
k
(n 1, n)
0 0 1
_

_
Avec u
k
(i, j) =
w
ik

D
k
v
T
jk
v
jk

D
k
v
T
jk
pour j 0, n
Pour nir, nous dnissons D

k
comme tant D

k
= V
k

D
k
V
T
k
9
La mise jour par la mesure (estimation a posteriori ) :
Maintenant que nous avons vu comment eectuer la prdiction en propageant les matrices D

k
et U

k
au
lieu de P

k
, nous allons voir comment eectuer la mise jour en utilisant les mesures et en propageant
U
+
k
et D
+
k
au lieu de P
+
. Lalgorithme que nous allons dcrire ici est nouveau un algorithme squentiel,
dont nous avons dj plusieurs reprises voqu les intrts.
Nous commenons avec lestim P

k
= U

k
D

k
U
T
k
et nous initialisons donc le processus squentiel ainsi :
P
+
0k
= U
+
0k
D
+
0k
U
+T
0k
= U

k
D

k
U
T
k
Pour chaque mesure i note y
ik
, associe la covariance R
ik
et la i-me ligne de la matrice H
k
note
H
ik
, on eectue les tapes suivantes :
1. Calculer
i
= H
ik
P
+
i1,k
H
T
ik
+R
i
2. Calculer la matrice

P
ik
=
_
D
i1,k

i
_
D
i1,k
U
T
i1,k
H
T
ik
__
D
i1,k
U
T
i1,k
H
T
ik
_
T
_
3. Factoriser la matrice

P
ik
sous la forme

P
ik
=

U
ik

D
ik

U
T
ik
4. Mettre jour les valeurs de U
+
ik
et D
+
ik
en suivant les formules suivantes :
U
+
ik
= U
+
i1,k

U
ik
D
+
ik
=

D
ik
Nous navons plus qu dduire lestimation posteriori une fois que linformation des direntes
mesures a t prise en compte.
P
+
k
= U
+
rk
D
+
rk
U
+T
rk
Il ne reste plus, pour nir, qu trouver un algorithme de factorisation permettant de calculer U et
D partir dune matrice P.
La factorisation :
Un algorithme de factorisation peut tre dduit en observant le systme dquations donns par les
coecients des matrices de lgalit suivante :
P = UDU
T
Il est conseill dans la littrature dobserver le cas simple de matrices de taille 3x3 an de se rendre
compte de la forme des quations, ce qui permettra ensuite den dduire un algorithme de rsolution.
Nous allons ici crire les formules plus gnrales associes ce problme de factorisation, et tenter de
montrer les simplications qui apparaissent du fait des proprits des matrices U et D. Cette approche
doit pouvoir complter ecacement lexemple simple des matrices de taille 3x3.
Daprs la formule bien connue du produit matriciel, si on note p
ij
le coecient de la matrice P au
niveau de la i-me ligne et j-me colonne et si nxn est la taille des matrices P, U et D, alors on a :
p
ij
=
n

k=0
u
ik
d
kk
u
jk
Du fait de la forme triangulaire de la matrice U, nous pouvons encore simplier cette expression.
En eet, pour i > j, on a u
ij=0
. Ainsi, lexpression prcdente peut scrire :
p
ij
=
n

k=max i,j
u
ik
d
kk
u
jk
En particulier, p
nn
= d
nn
, p
n1,n
= u
n1,n
d
nn
u
nn
= u
n1,n
d
nn
, p
n,n1
= d
nn
u
n,n1
et ainsi de suite.
10
Il est donc possible de rsoudre progressivement le systme en partant du coecient infrieur droit de
la matrice P et en remontant progressivement vers le coecient suprieur gauche. Ce faisant nous pouvons
calculer les coecients de la matrice D et de la matrice U partir de la matrice P tout en utilisant
une empreinte mmoire rduite, en comparaison avec lempreinte mmoire ncessaire linversion dun
systme dquation dans le cas gnral.
_

_
p
11
p
12
p
1n
p
21
.
.
.
.
.
.
p
2n
.
.
.
.
.
.
.
.
.
.
.
.
p
n1
p
n2
p
nn
_

_
=
_

n
k=1
u
ik
d
kk
u
jk

n
k=2
u
ik
d
kk
u
jk
u
1n
d
nn

n
k=2
u
ik
d
kk
u
jk
.
.
.
.
.
.
u
2n
d
nn
.
.
.
.
.
.
.
.
.
.
.
.
u
1n
d
nn
u
2n
d
nn
d
nn
_

_
On remarque sur lexpression ci-dessus que la matrice P est bien symtrique, ce qui permet de diviser
par deux le nombre dquations et dinconnues.
5.1.3 Conclusion
Nous avons donc dcrit deux nouveaux algorithmes squentiels permettant de doubler la prcision du
ltre de Kalman. Il semblerait daprs la littrature [1] que le ltre U-D soit plus ecace que le ltrage
par racine carre en terme de temps de calcul.
Il faut noter que ces algorithmes squentiels ne fonctionnent uniquement que lorsque la matrice de
covariance du bruit de mesure est diagonale ou constante.
Il est donc ncessaire en pratique de sassurer que ces hypothses sont raisonnables avant de choisir
cette forme dimplantation.
5.2 Contrler ltat du ltre de Kalman
Comme nous lavons vu prcdemment, le bon fonctionnement du ltre de Kalman repose sur le
respect dun certain nombre de proprits mathmatiques qui ne sont pas toujours vriable sur un
systme embarqu en pratique.
Nous avons dj abord deux mthodes permettant de doubler la prcision du ltre de Kalman,
mais certains problmes peuvent tout de mme subsister. Par exemple, nous pouvons avoir une erreur
destimation des matrices de covariance des bruits de mesure et du systme dynamique, ou encore une
erreur dans la modlisation du systme lui mme, ce qui peut provoquer un comportement non dsir
du ltre.
Ainsi, tant donn quil sagit l derreurs dans le modle, les comportements imprvus peuvent
survenir lors de lessai sur le matriel rel, tandis que toutes les simulations fonctionnent parfaitement.
An de prserver le matriel et de pouvoir dtecter plus rapidement la source de lerreur, il serait
intressant davoir une mthode de dtection de divergence du ltre, an de pouvoir interrompre le
systme immdiatement.
Comme cela a t prcdemment expos, lquation de mise jour par la mesure du ltre de Kalman
conventionnel scrit :
x
+
k
= x

k
+K
k
_
y
k
H
k
x

k
_
La partie de lquation entre parenthses
_
y
k
H
k
x

k
_
est appele linnovation.
La particularit de linnovation est quelle se comporte comme un bruit blanc de moyenne nulle et
de matrice de covariance
_
H
k
P

k
H
T
k
+R
k
_
. Nous pouvons donc en dduire des mthodes permettant de
contrler les performances du ltre.
Nous pourrions en thorie imaginer calculer une transforme de Fourier sur les valeurs de linnovation
an de vrier sa blancheur, mais cela prsente linconvnient de requrir des calculs supplmentaires
lourds si le systme na pas de dsp, ainsi quune taille mmoire consquente pour stocker ces direntes
valeurs.
Une autre ide pourrait tre de calculer la covariance de linnovation et de la confronter avec la valeur
thorique, l encore au prix de calculs matriciels supplmentaires.
11
Si nous ne souhaitons pas implmenter ces calculs, nous pouvons tout de mme vrier que linnovation
est bien de moyenne nulle, et ce moindre cot puisquil sut de maintenir dans un registre ou une zone
mmoire le cumul des valeurs, qui doit rester le plus possible proche de 0. Nous avons donc une premire
vrication trs simple et peu coteuse, que nous pouvons complter par des vrications de plus haut
niveau.
5.3 Filtre de Kalman pour les systmes non-linaires
Lorsquon aborde le problme des ltres de Kalman dans le cadre de systmes non-linaires, lide
premire acquise au dbut des tudes suprieures convient tout fait : il nous faut linariser.
Dans la plupart des cas, nous pouvons extraire des quations du problme un tat dquilibre autour
duquel notre systme va voluer. Si tel est le cas, nous pouvons alors linariser notre systme an de nous
ramener un cas classique pour lequel nous connaissons dj des solutions ecaces qui ont t dcrites
prcdemment.
Lorsque nous ne pouvons pas exhiber un tel tat, par exemple dans le cas dune voiture en mouvement
puisquil nest pas possible de trouver un tat stable de la position de la voiture, la question suivante se
pose : Autour de quel tat doit-on linariser ?
La rponse semble simple priori : il faut linariser le systme autour de lestimation prcdente
x
+
k1
.
Ce ltre est appel ltre de Kalman tendu, et nous en donnons ici le fonctionnement, comme dcrit
dans louvrage de rfrence [1].
Nous rappelons les quations du systme, an de prciser la notation que nous allons utiliser :
x
k
= f
k1
(x
k1
, u
k1
, w
k1
) (13)
y
k
= h
k
(x
k
, v
k
) (14)
w
k
(0, Q
k
) (15)
v
k
(0, R
k
) (16)
Nous suivons un processus toujours similaire lalgorithme de Kalman conventionnel, mais adapt
au cas non linaire, dcrit comme suit :
1. Initialiser le processus avec :
x
+
0
= E(x
0
) (17)
P
+
0
= E
_
(x
0
x
+
0
)(x
0
x
+
0
)
T

(18)
2. Pour k 1, +
(a) Linariser le systme autour de ltat estim prcdent :
F
k1
=
f
k1
x

x
+
k1
(19)
L
k1
=
f
k1
w

x
+
k1
(20)
(b) Mettre jour lestimation par la prdiction de ltat linstant k :
P

k
= F
k1
P
+
k1
F
T
k1
+L
k1
Q
k1
L
T
k1
(21)
x

k
= f
k1
_
x
+
k1
, u
k1
, 0
_
(22)
(c) Calculer les drives partielles suivantes :
H
k
=
h
k
x

k
(23)
M
k
=
h
k
v

k
(24)
12
(d) Eectuer la correction de lestimation par la mesure en suivant les formules suivantes, ou
encore en utilisant un algorithme squentiel :
K
k
= P

k
H
T
k
_
H
k
P

k
H
T
k
+M
k
R
k
M
T
k
_
1
(25)
x
+
k
= x

k
+K
k
_
y
k
h
k
_
x

k
, 0
_
(26)
P
+
k
= (I K
k
H
k
) P

k
(27)
3. Boucler sur ltape 2...
Ces linarisations successives peuvent poser problme lors de limplantation sur un systme embarqu.
Il est possible dimaginer direntes solutions, par exemple de calculer la linarisation des quations
autour de dirents tats choisis au pralable avant limplantation, puis de choisir dynamiquement au
moment de lexcution le ltre le mieux adapt.
Le ltre de Kalman tendu est dcrit en dtail dans les ouvrages cits en rfrence, il est donc possible
de sy rfrer pour plus de dtails.
Rfrences
[1] Dan Simon, Optimal State Estimation : Kalman, H Innity, and Nonlinear Approaches. John Wiley
& Sons, Inc. 1st Edition, 2006.
[2] Greg Welch and Gary Bishop, An Introduction to the Kalman Filter. Department of Computer Science
University of North Carolina at Chapel Hill, 2006.
13