Kalman

Introduction au ltre de Kalman
Notes de cours Exercices corrig es Sessions Matlab
D.Alazard
Octobre 2006 - version 0.2
PAGE SANS TEXTE
Contents
Introduction 1 Signaux al eatoires et syst` emes lin eaires 1.1 Position du probl` eme . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2 Rappels et d enitions . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.1 Caract erisation dune variable al eatoire scalaire . . . . . . . . 1.2.2 Caract erisation dune variable al eatoire ` a plusieurs dimensions 1.2.3 Signal al eatoire (processus stochastique) . . . . . . . . . . . . 1.2.4 Moments dun signal al eatoire . . . . . . . . . . . . . . . . . . 1.2.5 Stationnarit e . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.6 Spectre complexe . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.7 Bruit blanc . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3 Passage dun signal al eatoire dans un syst` eme lin eaire . . . . . . . . . 1.3.1 Approche temporelle . . . . . . . . . . . . . . . . . . . . . . . 1.3.2 Approche fr equentielle (spectrale) . . . . . . . . . . . . . . . . 1.4 Illustration sous Matlab . . . . . . . . . . . . . . . . . . . . . . . . . 1.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 Le ltre de Kalman 2.1 Principe du ltre de Kalman . . . . . . . . . . 2.1.1 Le mod` ele de Kalman . . . . . . . . . . 2.1.2 Hypoth` eses . . . . . . . . . . . . . . . . 2.1.3 Structure dun estimateur non biais e . . 2.2 Estimateur ` a variance minimale . . . . . . . . . 2.2.1 Solution g en erale . . . . . . . . . . . . . 2.2.2 R egime permanent du ltre de Kalman 2.2.3 R eglage du ltre de Kalman . . . . . . 2.3 Exercices corrig es . . . . . . . . . . . . . . . . . Introduction au ltre de Kalman
Page 3/74
5 7 7 9 9 11 14 14 15 17 20 20 20 21 23 26 27 27 27 27 29 31 31 33 33 34
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
. . . . . . . . .
4 2.3.1 Syst` eme du premier ordre . . . . . . . . . . . . . . 2.3.2 Estimation dun biais . . . . . . . . . . . . . . . . . Le ltre de Kalman discret . . . . . . . . . . . . . . . . . 2.4.1 Le mod` ele de Kalman discret . . . . . . . . . . . . 2.4.2 Cas particulier dun syst` eme continu echantillonn e. 2.4.3 Les equations r ecurrentes du ltre de Kalman . . 2.4.4 Exemple . . . . . . . . . . . . . . . . . . . . . . . . Exercices . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.5.1 Syst` eme du second ordre: . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Contents . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34 41 44 44 45 47 51 52 52 55 57
2.4
2.5
3 A propos des unit es physiques R ef erences
A Int egration de l equation l etat 59 A.1 Cas continu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59 A.2 Cas discret . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61 B Passage dun bruit dans un syst` eme lin eaire B.1 Compl ement: caract erisation des signaux al eatoires discrets B.2 Approche temporelle . . . . . . . . . . . . . . . . . . . . . B.2.1 Cas continu . . . . . . . . . . . . . . . . . . . . . . B.2.2 Cas discret . . . . . . . . . . . . . . . . . . . . . . B.3 Approche fr equentielle . . . . . . . . . . . . . . . . . . . . B.3.1 Cas continu . . . . . . . . . . . . . . . . . . . . . . B.3.2 Cas discret . . . . . . . . . . . . . . . . . . . . . . 63 63 64 64 66 68 68 69
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
. . . . . . .
C Code source Matlab des chiers de d emonstration 71 C.1 Fonction Kf t.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71 C.2 Fichier demoKalman.m . . . . . . . . . . . . . . . . . . . . . . . . . . 71 C.3 Fichier demoKalmand.m . . . . . . . . . . . . . . . . . . . . . . . . . . 73
Page 4/74
Introduction
Ce document est une introduction au ltre optimal de Kalman appliqu e aux syst` emes lin eaires. On suppose connues la th eorie des asservissements lin eaires et du ltrage fr equentiel (continu et discret) ainsi que les notions d etats pour repr esenter les syst` emes dynamiques lin eaires. Dune fa con g en erale, la fonction de ltrage consiste ` a estimer une information (signal) utile qui est pollu ee par un bruit. Alors que le ltrage fr equentiel suppose quil existe une s eparation entre les r eponses fr equentielles du signal utile et du bruit et consiste ` a trouver une fonction de transfert satisfaisant un gabarit sur le gain de sa r eponse fr equentielle (et beaucoup plus rarement sur la courbe de phase), le ltre de Kalman vise ` a estimer de fa con optimale l etat du syst` eme lin eaire (cet etat correspond donc ` a linformation utile). Avant de d enir le crit` ere doptimalit e qui permettra de calculer le ltre de Kalman (et qui est en fait un crit` ere stochastique), il est n ecessaire de faire quelques rappels sur les signaux al eatoires. Dans la premier chapitre de ce document, nous rappelons comment on caract erise math ematiquement un signal al eatoire et nous etudierons la r eponse dun syst` eme lin eaire ` a un signal al eatoire de fa con tout ` a fait compl ementaire ` a la r eponse dun syst` eme lin eaire ` a un signal d eterministe ( echelon, rampe, ...). Dans le second chapitre nous donnerons tous les el ements d enissant la structure du ltre de Kalman. Le lecteur d esirant sinformer sur la m ethodologie g en erale de r eglage dun ltre de Kalman pourra directement aller au chapitre 2. Par contre la lecture du chapitre 1, certes plus lourd sur le plan th eorique, sera incontournable ` a celui qui voudra comprendre les principes de base utilis es dans le traitement des signaux al eatoires et sur lesquels est fond e le ltre de Kalman. Les applications du ltre de Kalman sont nombreuses dans les m etiers de ling enieur. Le ltre de Kalman permettant de donner un estim e de l etat de syst` eme ` a partir dune information a priori sur l evolution de cet etat (mod` ele) et de mesures r eelles, il sera utilis e pour estimer des conditions initiales inconnues (balistique), pr edire des trajectoires de mobiles (trajectographie), localiser un engin (navigation, radar,...) et egalement pour implanter des lois de commande fond ees sur un estimateur de l etat et un retour d etat (Commande Lin eaire Quadratique Gaussienne). Les bases de traitement de signal sur lesquelles re-
Page 5/74
Introduction
pose le ltre de Kalman seront egalement utiles ` a tout ing enieur confront e ` a des probl` emes de d enition de protocoles dessais, de d epouillements dessais et egalement didentication param etrique, cest-` a-dire la d etermination exp erimentale de certains param` etres du mod` ele.
Page 6/74
Chapter 1 Signaux al eatoires et syst` emes lin eaires

1.1 Position du probl` eme
x (t) = Ax(t) + Bu(t) + M w(t) ( equation d etat) y (t) = Cx(t) + Du(t) + v (t) ( equation de mesure) o` u: x(t) Rn est le vecteur d etat du syst` eme, u(t) Rm est le vecteur des entr ees d eterministes et connues (commandes,...), w(t) Rq est le vecteur des signaux al eatoires inconnus qui viennent perturber directement l equation d etat du syst` eme ` a travers une matrice dentr ee Mnq (on note wx = M w le bruit d etat), y (t) Rp est le vecteur des mesures, v (t) Rp est le vecteur des signaux al eatoires (bruit de mesure) qui polluent les mesures y (t) (on suppose quil y a autant de bruits que de mesures). Exemple 1.1 Le mod` ele dit quart de v ehicule utilis e pour etudier une suspension active de voiture est repr esent e sur le gure 1.1. Lorgane de commande permet dappliquer des eorts u entre la roue (de masse m rep er ee par sa position verticale z ) et la caisse du v ehicule (de masse M rep er ee par sa position verticale Z ). On d esigne par K et f la raideur et le frottement visqueux de la suspension passive, par k la raideur du pneu entre la roue et la route. Enn, on note w la position verticale du point de contact pneu/sol sollicit ee par les irr egularit es de la route (bruit de Introduction au ltre de Kalman
Page 7/74
Consid erons le mod` ele d etat dun syst` eme suppos e lin eaire et stationnaire : (1.1)
atoires et syste `mes line aires 1. Signaux ale
roulement). On mesure lacc el eration de la caisse avec un acc el erom` etre sensible ` a 2 la gravit e (g = 9.81 m/s ) et on note v le bruit de mesure de ce capteur.
K g m
Z k z ground w
Figure 1.1: Mod` ele 1/4 de v ehicule.
On note z et Z les variations de z et Z autour dune position d equilibre z0 et Z0 (obtenue en supposant que w = 0, u = 0 et que la gravit e est enti` erement compens ee par l ecrasement des 2 raideurs K et k ). En appliquant le principe fondamental de la dynamique sur les 2 masses M et m (calcul aux variations), on obtient : = K (z Z ) + f (z Z )+u M Z = K (z Z ) f (z Z ) u k (z w) . mz z ]T comme vecteur d En choisissant x = [Z, z, Z, etat, on obtient la repr esentation d etat suivante : 0 0 1 0 0 0 0 0 1 x + 0 x = K/M 1/M K/M f /M f /M K/m (K + k )/m f /m f /m 1/m y = K/M K/M f /M f /M x+ [1/M 0 0 0 0 1] u g u g 0 0 + 0 w 1/k +v .
(1.2) On obtient donc un mod` ele du type de (1.1) avec n = 4, m = 2, q = 1 et p = 1. 2
Page 8/74
finitions 1.2 Rappels et de
On reconna t dans les matrices (A, B, C, D) du mod` ele (1.1) la repr esentation 1 d etat classique du transfert entre lentr ee u et la sortie y : : F (s) = D + C (sI A)1 B . La r eponse de ce mod` ele ` a des entr ees d eterministes u(t) sur un horizon t [t0 , t] et ` a des conditions initiales x(t0 ) s ecrit : x(t) = eA(tt0 ) x(t0 ) + y (t) = Cx(t) + Du(t)
t t0
eA(t ) Bu( ) d
(1.3) (1.4)
D emonstration: voir annexe A dans la quelle gure egalement un exercice dapplication. Quen est-il de la r eponse du mod` ele (1.1) ` a un signal al eatoire w(t) ? Pour r epondre ` a cette question, nous allons : dabord rappeler comment on caract erise math ematiquement (ou stochastiquement) les signaux al eatoires (ou processus stochastiques), enoncer quelques hypoth` eses sur les caract eristiques stochastiques des bruits w(t)et v (t) (bruits blanc gaussiens) pour faciliter le calcul de la r eponse du mod` ele (1.1), calculer les caract eristiques stochastiques de cette r eponse (x(t) et y (t)). Les di erentes d enitions et rappels donn es ci-dessous sont extraits de la r ef erence [4] (chapitre II et annexe A.I).
1.2
1.2.1
Rappels et d enitions
Caract erisation dune variable al eatoire scalaire
Soit X une variable al eatoire scalaire prenant ses valeurs dans R. La fonction de r epartition F (x) associe ` a tout r eel x la probabilit e de l ev enement X < x. On note: F (x) = P [X < x] . Propri et es: x 1 < x 2 , P [x1 X < x2 ] = F (x2 ) F (x1 ),
limx+ F (x) = 1; limx F (x) = 0.

1
s d esigne la variable de Laplace. Page 9/74
10
atoires et syste `mes line aires 1. Signaux ale F (x) est monotone, non d ecroissante, et peut etre continue ou discontinue selon que X prenne des valeurs continues ou discr` etes.
Si F (x) est d erivable, alors sa d eriv ee est appel ee densit e de probabilit e et not ee p(x): dF (x) p(x) = soit : p(x)dx = P [x X < x + dx] . dx Pour caract eriser et manipuler math ematiquement une variable al eatoire X , on utilise egalement les moments de cette variable. Le moment dordre 1 est plus connu sous le nom de moyenne ou esp erance math ematique. Le moment centr e 2 dordre 2 est appel e variance que lon note varx = x ; x d esigne l ecart type. Soit : lesp erance math ematique ou moyenne:
+ +
E [X ] =
x p(x) dx =
x dF (x) ,
(1.5)
le moment dordre k : E [X ] =
k +
xk p(x) dx ,
(1.6)
le moment centr e dordre k :

k
2 +
E [(X E [X ]) ] =
(x E [X ])k p(x) dx .
Les moments dordre sup erieur ou egal ` a 3 sont tr` es peu utilis es car ils se pr etent mal au calcul th eorique. Lint er et (math ematique) des variables al eatoires gaussiennes est quelles sont enti` erement caract eris ees par leurs moments dordre 1 et 2. Soit X une variable al eatoire gaussienne de moyenne m et d ecart type , alors: (xm)2 1 p(x) = e 22 , E [x] = m, E [(x m)2 ] = 2 . 2 Exemple 1.2 (Loi uniforme) La variable al eatoire X est equir epartie entre deux valeurs a et b avec b > a.
b
E [X ] =
a
1 x 1 2 dx = x ba ba 2
b a
=
a 2
1 b2 a2 a+b = 2 ba 2 a+b x 2
3 b
1 varx = E [(X E [X ]) ] = ba
2
2
a+b x 2
1 1 dx = ba 3
Rappel: varx = E [X 2 ] E [X ]2 . Page 10/74

F(x) 1 p(x) 1/(ba)
11
a 0
a 0
Figure 1.2: Fonction de r epartition et densit e de probabilit e de la loi uniforme.
1 1 varx = 3ba
ba 2
(b a)2 . 12 2
Variable al eatoire discr` ete: si la variable al eatoire X prends ses valeurs dans un ensemble discret de N valeurs xi , i = 1, , N alors on ne parle plus de densit e de probabilit e et on d eni directement la probabilit e que X = xi que lon note P (X = xi ). La calcul des moments fais alors intervenir une somme discr` ete :
N
E [X ] =
i=1
xk i P (X = xi ) .
Exemple 1.3 La variable al eatoire X correspondante au lanc e dun d e: 1 P (X = i) = , i = 1, , 6 ; 6 E [X ] = 21 ; 6 varx = 35 . 12 2
1.2.2
Caract erisation dune variable al eatoire ` a plusieurs dimensions
Soit X = [X1 , , Xq ]T une variable al eatoire ` a q dimensions prenant ses valeurs dans Rq . Fonction de r epartition F (x1 , , xq ) = P (X1 < x1 et X2 < x2 et et Xq < xq ) . Densit e de probabilit e p(x1 , , xq ) = Introduction au ltre de Kalman q F (x1 , , xq ) . x1 xq
Page 11/74
12 Moments
On note: x = [x1 , , xq ]T et on ne sint eressera quau vecteur des moments dordre 1 (cest-` a-dire le vecteur moyen) et ` a la matrice des moments dordre 2 centr es (cest ` a dire la matrice de covariance). Moyenne: E [X ] = [E [X1 ], , E [Xq ]]T . Covariance: Covx = E [(X E [X ])(X E [X ])T ]. L el ement Covx (i, j ) de la ligne i et colonne j de cette matrice de covariance v erie: Covx (i, j ) =
R2
(xi E [Xi ])(xj E [Xj ]) dF (xi , xj ) .
La matrice de covariance est d enie, positive et sym etrique. Vecteur al eatoire gaussien de moyenne m et de covariance p(x) = 1 e 2 (xm)
1 T 1 (xm)
(2 )q/2 det
Le vecteur al eatoire gaussien de moyenne m et de covariance peut etre g en er e ` a partir du vecteur gaussien normalis e N (cest-` a-dire de moyenne nulle et de covariance unit e) de la fa con suivante: X = m + GN o` u G est une matrice v eriant: GGT = . Ind ependance Deux variables al eatoires X1 et X2 sont ind ependantes si et seulement si: F (x1 , x2 ) = F (x1 )F (x2 ) . Une condition n ecessaire dind ependance s ecrit : E [X1 X2 ] = E [X1 ]E [X2 ] . (1.7)
Exercice 1.1 On consid` ere 2 variables al eatoires X1 et X2 uniform ement r eparties X2 entre 1 et 1 et ind ependantes. On d enit la variable al eatoire Y = X1 + . 2 Calculer E [Y ], vary et covx1 ,y la matrice de covariance du vecteur: (X1 , Y )T . Correction : Dapr` es ce qui pr ec` ede (lois uniformes), on peut ecrire: E [X1 ] = E [X2 ] = 0; Introduction au ltre de Kalman varx1 = varx2 = 1 . 3
Page 12/74
finitions 1.2 Rappels et de X1 X2
13
Par d enition: Y = (1/2 1/2)
. On en d eduit : 1/2) E X2 ) X1 X2 1/2 1/2 =0, = (1/2 1/2)covx1 ,x2 1/2 1/2
E [Y ] = (1/2 vary = (1/2 1/2)E X1 X2 (X1
Du fait de lind ependance de X1 et X2 : covx1 ,x2 = On montre egalement que: covx1 ,y = Gcovx1 ,x2 GT , = 1/3 1/6 1/6 1/6 vary = 1 . 6
1/3 0 0 1/3
avec : G =
1 0 1/2 1/2
Remarque : on aurait pu egalement, au prix de calculs bien plus laborieux, caract eriser (compl` etement) la variable al eatoire Y par sa fonction de r epartition F (y ) et de densit e de probabilit e p(y ) pour en d eduire les moments dordre 1 et 2 : F (y ) = P X1 + X2 <y 2 = P (X1 < 2y X2 ) =
Dx2
P (X1 < 2y x2 )p(x2 )dx2 .
Pour une valeur donn ee de y , on peut ecrire (voir gure 1.2): P (X1 < 2y x2 ) = 0 x2 / 2y x2 < 1 x2 > 2y + 1, 2y x2 + 1 P (X1 < 2y x2 ) = x2 / 1 2y x2 < 1 x2 / 2y 1 < x2 2y + 1, 2 P (X1 < 2y x2 ) = 1 x2 / 2y x2 1 x2 2y 1. Et p(x2 ) = 1/2 si 1 x2 < 1, p(x2 ) = 0 sinon. Donc 3 : si y < 0
3
F (y ) =
1 0 dx2 + 2y +1 2
2y +1 1
2y x2 + 1 (y + 1)2 dx2 + 0 = , 4 2
En utilisant les probabilit es conditionnelles, on peut egalement ecrire: F (y ) =

Dx2
0 p(x2 |x2 > 2y + 1)dx2 +

Dx2
2y x2 + 1 p(x2 |2y 1 < x2 2y + 1)dx2 2
+
Dx2
1 p(x2 |x2 2y 1)dx2 .
Page 13/74
14
1
atoires et syste `mes line aires 1. Signaux ale 2y x2 + 1 dx2 + 4

2y 1 1
si y 0 F (y ) = 0 +
2y 1
1 y 2 + 2y + 1 dx2 = . 2 2
On en d eduit : p(y ) = y + 1 y [1, 0], p(y ) = y + 1 y [0, 1], 1 0 1 E [Y ] = 1 y p(y )dy = 1 y (y + 1)dy + 0 y (y + 1)dy = 0, 1 0 1 1 . vary = 1 y 2 p(y )dy = 1 y 2 (y + 1)dy + 0 y 2 (y + 1)dy = 6
F(y) 1 1 p(y)
Figure 1.3: Fonction de r epartition et densit e de probabilit e de Y .
2 Dans ce qui suit, on parle de variable (resp. signal) al eatoire quil sagisse dune variable (resp. signal) scalaire (q = 1) ou vectorielle ` a q composantes.
1.2.3
Signal al eatoire (processus stochastique)
Etant donn e une variable al eatoire X , le signal al eatoire ou processus stochastique x(t) est un signal fonction du temps t tel que pour tout t x e, x(t) corresponde ` a une valeur de la variable al eatoire X .
1.2.4
Moments dun signal al eatoire
Le moment dordre 2 dun signal al eatoire est appel e la fonction dauto-corr elation. Soit w(t) un signal al eatoire, alors: moment dordre 1: moment dordre 2: m(t) = E [w(t)] ww (t, ) = E [w(t)w(t + ) ] .
T
(1.8) (1.9)
Remarque 1.1 Si w(t) est un signal vectoriel ` a q composantes alors ww (t, ) est une matrice de taille q q d enie positive pour chaque valeur de t et de . Les termes de la diagonales sont les fonctions scalaires dauto-corr elation de chaque composantes et les termes hors-diagonaux sont les fonctions scalaires dinter-corr elation entre composantes. Introduction au ltre de Kalman
Page 14/74
15
Un signal al eatoire gaussien centr e, cest-` a-dire ` a moyenne nulle, est donc enti` erement d eni par sa fonction dauto-corr elation.
1.2.5
Stationnarit e
Un signal al eatoire est dit stationnaire ` a lordre 2 si sa moyenne est constante (m(t) = m) et si sa fonction dauto-corr elation ne d epend que de (ww (t, ) = ww ( )). La moyenne quadratique ou variance 4 dun signal al eatoire centr e stationnaire est la valeur de la fonction dauto-corr elation ` a lorigine:
2 w = ww ( )| =0
Exemple 1.4 Un signal al eatoire b(t) est g en er e de la fa con suivante: ` a partir de linstant initial t0 = 0, on bloque le signal b(t) toutes les dt secondes sur la valeur dune variable al eatoire gaussienne centr ee X d ecart type ; tous les tirages xi de la variable X sont ind ependants les uns des autres. Soit: b(t) = xi t [idt, (i + 1)dt[ (voir gure 1.4).
b(t) 2 dt 95 % de chance que le signal soit compris entre ces 2 limites
10 dt
20 dt
....
Figure 1.4: R eponse temporelle dune r ealisation du signal b(t).
Calculer la moyenne m(t) et la fonction dauto-corr elation bb (t, ) du signal b(t). b(t) est-il stationnaire ` a lordre 2?.
4
ou covariance sil sagit dun signal al eatoire vectoriel.
Page 15/74
16
atoires et syste `mes line aires 1. Signaux ale Reprendre les questions pr ec edentes en supposant maintenant que linstant initial est une variable al eatoire uniform ement r epartie entre 0 et dt.
ements de solution: El m(t) = E [b(t)] = E [X ] = 0, t ,
bb (t, ) = E [b(t)b(t + )] = 2 si t et t + sont dans le m eme intervalle de temps dt; 0 sinon (car les tirages sont ind ependants et centr es). La fonction dauto-corr elation d epend de t et ; le signal b(t) nest donc pas stationnaire ` a lordre 2. Par exemple, pour t = i dt + (i, [0, dt[) , la r eponse de bb (t, ) est donn ee sur la gure 1.5.
Figure 1.5: Fonction dautocorr elation du signal b(t).
dt
dt
Figure 1.6: Fonction dautocorr elation du signal b (t).
si linstant initial t0 est maintenant al eatoire uniform ement r eparti entre 0 et dt (on notera b (t) ce nouveau signal al eatoire) alors cela revient ` a consid erer que, dans le calcul pr ec edent, est uniform ement r eparti entre 0 et dt: 1 b b (t, ) = dt
dt
bb (i dt + , )d
0
pour dt > 0, bb (i dt + , ) = 2 ssi 0 < < dt , 0 sinon. Soit 2 b b (t, ) = dt Introduction au ltre de Kalman
dt
d =
0
2 (dt ) dt
/ 0 < dt .
Page 16/74
17
pour dt < < 0, bb (i dt + , ) = 2 ssi < < dt, 0 sinon. Soit : b b (t, ) = 2 dt
dt
d =
2 (dt + ) / dt < < 0 . dt
| On a donc b b (t, ) = 2 (1 |dt ) [dt dt], 0 sinon (voir gure 1.6) qui ne d epend plus que de , le signal est maintenant stationnaire ` a lordre 2.
1.2.6
Spectre complexe
On peut egalement caract eriser les signaux al eatoires, sils sont stationnaires, par leurs r eponses fr equentielles quon appelle densit e spectrale de puissance ou leurs spectres complexes (on passe de lun ` a lautre en rempla cant s par j ). On parle alors danalyse harmonique. Le spectre complexe dun signal al eatoire stationnaire est la transform ee de Laplace bilat erale de sa fonction dauto-corr elation 5 .
ww (s) = LII [ww ( )] =
ww ( )e s d .
(1.10)
Densit e spectrale de puissance (DSP): ww ( ) = ww (s)|s=j . Remarque 1.2 :
ww (s) =
0
ww ( )e s d +
0
ww (u)eus du
= + ww (s) + ww (s)
avec :
+ + ww (s) = L[ww ( )] et ww (s) = L[ww ( )] et
+ ww ( ) = ww ( ) si 0, ww ( ) = ww ( ) si 0,
+ ww ( ) = 0 sinon , ww ( ) = 0 sinon .
Si la fonction ww ( ) est paire alors + ww = ww et la spectre complexe ww (s) est une fonction bicarr ee en s. Le th eor` eme de la valeur initiale de la transform ee de Laplace (mono-lat erale) permet alors de calculer la variance directement ` a partir du spectre complexe : 2 w = ww ( )| =0 = lim s+ ww (s) . s
5
La transform ee de Laplace bilat erale inverse s ecrit :

1 ww ( ) = L II ww (s) =
1 2 j
+j j
ww (s)es ds
Page 17/74
18
atoires et syste `mes line aires 1. Signaux ale 2
Remarque 1.3 : A partir de la DSP on peut alors ecrire : 1 ww ( ) = 2

+
ww ( )e
et
2 w
1 = 2
ww ( )d .
La variance du bruit w est, ` a 2 pr` es, lint egrale de sa DSP. 2 Exemple 1.5 On reconsid` ere le signal b (t) de la section 1.4 dont la fonction dautocorr elation | | 2 s ecrit : b b (t, ) = (1 dt ) (fonction paire). Le spectre complexe de ce signal est : | | s + 2 (1 b b (s) = )e d = + avec: b b (s) + b b (s) dt + b b (s) = = =
dt 0
2 (1
s )e d dt
dt
2 dt 2 dt
dt s e s e s dt + s s2
0 dt
1 s
dt 0
e s d
(int egration par parties)
=
0 2 dt s2
2 s dt + es dt 1 . dt s2 es dt + es dt 2 .
b b (s) =
La densit e spectrale de puissance est donc : b b ( ) = 2 2 2 4 2 dt j dt j dt e + e 2 = (1 cos ( dt )) = sin2 ( ) 2 2 2 dt dt dt 2 b b ( ) = 2 dt sin2 ( 2dt )

dt 2 ( 2 )
La caract eristique de b b ( ) est pr esent ee gure 1.7 6 . Dun point de vue pratique, on pr ef erera la caract eristique en echelle logarithmique pr esent ee gure 1.8 qui met en evidence que la densit e spectrale de puissance peut- etre consid er ee comme constante pour des pulsations tr` es inf erieures ` a la pulsation d echantillonnage 2/dt. Ce signal pourra donc etre utilis e comme une source de bruit blanc de densit e spectrale R dans une plage de pulsations donn ee si lon choisi 2 /dt tr` es grand par rapport ` a cette plage de pulsations et si lon choisi 2 = R/dt (voir section suivante). Introduction au ltre de Kalman
Page 18/74
19
bb()
2
dt
4/dt 2/dt
2/dt 4/dt
Figure 1.7: Densit e spectrale de puissance du signal b (t).
bb() |dB
20 log10(2 dt)
2/dt 4/dt
Figure 1.8: Densit e spectrale de puissance du signal b (t) ( echelles logarithmiques).
Page 19/74
20
atoires et syste `mes line aires 1. Signaux ale 2
Cette repr esentation fr equentielle des signaux al eatoires est particuli` erement utile pour etudier la transmission des signaux al eatoires ` a travers les syst` emes lin eaires stationnaires (voir section 1.3.2).
1.2.7
Bruit blanc
Enn, un bruit blanc est un signal al eatoire de variance innie dont la fonction dauto-corr elation est proportionnelle ` a un dirac (cest-` a-dire un spectre complexe constant sur toute la plage des fr equences). Cela traduit que les valeurs du signal pris ` a deux instants, m eme tr` es proches, ne sont pas du tout corr el ees. Les bruits blancs gaussiens centr es w(t) et v (t) que nous allons utilis es dans le cadre du ltre de Kalman sont donc enti` erement d enis par leur densit es spectrales respectives W (t) et V (t): E [w(t)w(t + )T ] = W (t) ( ), E [v (t)v (t + )T ] = V (t) ( ) (1.11)
Les matrices W (t) et V (t) deviennent constantes dans le cas de bruits blancs stationnaires. Le bruit blanc gaussien normalis e est tel que W (t) = Iqq (q : nombre de composantes dans le bruit). Remarque 1.4 En pratique, on ne sait pas simuler sur un calculateur num erique un bruit blanc continu de densit e spectrale nie R (mais de variance et de puissance innies). On utilisera lapproximation pr esent ee dans lexemple 1.4 (voir gure 1.8) qui consiste ` a bloquer sur une p eriode dt (que lon choisira tr` es petite par rapport aux constantes de temps du syst` eme que lon veut simuler) une variable al eatoire 2 gaussienne de variance = R/dt (cest cela m eme qui est fait dans le bloc Bandlimited white-noise de Simulink).
1.3
1.3.1
Passage dun signal al eatoire dans un syst` eme lin eaire

Approche temporelle
Sous lhypoth` ese dun syst` eme lin eaire ( equation 1.1) et dun bruit w gaussien, nous pouvons armer que l etat x et la sortie y sont egalement des signaux (vectoriels) gaussiens qui sont donc enti` erement caract eris es par leurs moments dordre 1 et 2. Le th eor` eme suivant va nous permettre de calculer ces caract eristiques stochastiques. Nous supposons dans ce qui suit que lentr ee d eterministe est nulle (u(t) = 0).
6 x On rappelle que limx0 ( sin x )=1
Page 20/74
atoire dans un syste `me line aire 1.3 Passage dun signal ale Th eor` eme 1.1 Soit le syst` eme lin eaire: t) = Ax(t) + M w(t) . x(
21
(1.12)
w(t) est un bruit blanc gaussien stationnaire centr e de densit e spectrale de puissance W . On note m(t0 ) et P (t0 ) la moyenne et la covariance de l etat initial x(t0 ) (lui aussi al eatoire). Alors x(t) est un signal al eatoire gaussien : de moyenne: m(t) = E [x(t)] = eA(tt0 ) m(t0 ) de covariance P (t) = E [(x(t)m(t))(x(t)m(t))T ] v eriant l equation di erentielle : t) = AP (t) + P (t)AT + M W M T . P( (1.13)
Si le syst` eme est stable (toutes les valeurs propres de A sont ` a partie r eelle = 0 et P (t) = n egative) on tend vers un r egime permanent (stationnaire) : P P v erie alors l equation de Lyapunov continue : AP + P AT + M W M T = 0 . D emonstration: (voir annexe B.2). Remarque 1.5 Si lon consid` ere l equation de sortie y (t) = Cx(t) alors la matrice de covariance Py (t) de y (t) v erie : Py (t) = CP (t)C T (si lon consid` ere un bruit blanc de mesure, alors cette covariance est innie). (1.14)
1.3.2
Approche fr equentielle (spectrale)
Nous nous int eressons ici uniquement au r egime permanent. Th eor` eme 1.2 (Passage dun bruit dans un syst` eme lin eaire) La sortie y dun syst` eme lin eaire stable d eni par la matrice de transfert G(s)pq et attaqu e par un eatoire de spectre signal al eatoire w de spectre complexe ww (s)qq est un signal al complexe yy (s)pp = G(s)ww (s)GT (s) . D emonstration: (voir annexe B.3). Dans le cas dun transfert G mono-entr ee, mono sortie (SISO), cette relation est d ecrite par la gure 1.9. Introduction au ltre de Kalman
Page 21/74
22
G(s)
Figure 1.9: Cas SISO.
R eciproquement : consid erons un signal al eatoire w(t) de spectre complexe donn e ww (s) r eel (cest-` a-dire qui ne fait appara tre que des puissances paires de s; ce qui implique que le fonction dauto-corr elation associ ee ww ( ) est paire); alors 7 la d ecomposition : ww (s) = G(s)GT (s) o` u G(s) est le transfert qui regroupe tous les p oles stables de ww (s), permet de donner une repr esentation markovienne du signal w(t), cest-` a-dire une repr esentation d etat de G(s) not ee (on supposera G(s) strictement propre). xG(t) = AG xG (t) + BG b(t) w(t) = CG xG (t) o` u b(t) est un bruit blanc normalis e bb (s) = Iqq . Exemple 1.6 0n reprend lexemple 1.1 relatif au mod` ele 1/4 de v ehicule. Des essais en condition r eelle de roulement ont montr e que la densit e spectrale de puissance du bruit de roulement pour un v ehicule roulant ` a 90 km/h sur une d epartementale pouvait etre approch ee par la fonction suivante: ww ( ) = 2 + 104 4 1.75 104 2 + 108
a) Donner le spectre complexe du bruit w(t). b) Donner une repr esentation markovienne de w(t). c) Quel est la variance du signal w(t) (on suppose le signal centr e)? Solution : a) Par d enition: ww (s) =
s2 +104 . s4 +1.75 104 s2 +108
b) Une d ecomposition ww (s) = G(s)G(s) s ecrit : ww (s) =

7
102 + s s2 50s + 104
102 s s2 + 50s + 104
Une telle d ecomposition nest pas unique comme le montre lexemple 1.6. Page 22/74
1.4 Illustration sous Matlab
23
Il faut regrouper dans G(s) tous les p oles stables de ww(s), ce qui permet de xer le d enominateur de G(s) mais il ny a aucune condition sur la stabilit e des z eros de G(s). Il y a donc 2 ltres stables qui permettent, lorsquil sont attaqu es par un bruit blanc normalis e b(t), de g en erer un bruit de spectre ww (s): 102 s 102 + s G(s) = 2 et G ( s ) = s + 50s + 104 s2 + 50s + 104 Si lon choisi G(s) alors une repr esentation markovienne de w(t) s ecrit (on choisit par exemple une forme compagne horizontale) : 0 1 0 xG (t) = xG (t) + b(t) . (1.15) 104 50 1 2 w(t) = [10 1]xG (t) c) La variance peut etre calcul ee de 2 fa cons : ` a partir du spectre complexe et du th eor` eme de la valeur initiale (voir Remarque 1.2) : ww (s) = 1/50 s + 1/2 1/50 s + 1/2 + 2 = + ww (s) + ww (s) 2 4 4 s + 50s + 10 s 50s + 10
2 w = ww ( )| =0 = lim s+ ww (s) = 1/50. s
` a partir de la repr esentation markovienne et du th eor` eme 1.1 : en r egime permanent, la matrice de covariance P du vecteur d etat xG de la repr esentation 1.15 v erie l equation de Lyapunov : 0 1 4 10 50 P +P
2 w = [102
0 104 1 50
0 0 0 1
0 0 0 0
P =
106 0 0 102 102 1 = 1/50 . 2
1]
106 0 0 102
1.4
Illustration sous Matlab
Le chier bruit.m donn e ci-dessous permet dillustrer lutilisation de macro-fonctions Matlab pour r esoudre lexemple 1.6. Il montre egalement comment simuler sur un calculateur num erique un tel bruit color e` a partir dun bruit pseudo-blanc Introduction au ltre de Kalman
Page 23/74
24
echantillonn e-bloqu e` a la cadence dt et permet donc de valider les r esultats des exemples 1.4 et 1.5. Il utilise pour cela le chier Simulink simule_bruit.mdl repr esent e sur la gure 1.10 8 . Les r eponses du bruit pseudo-blanc b(t) et du bruit de roulement w(t) pr esent ees respectivement gures 1.11 et 1.12 mettent en evidence que la variance de w(t) est ind ependante de dt (du moment que dt est rapide par rapport ` a la dynamique du ltre G(s)) alors que la variance de b(t) est egale ` a 1/dt: on retrouve que cette variance tend vers linni si dt tend vers 0 tout comme la variance dun bruit blanc continu est innie (par contre la densit e spectrale de b(t) est ind ependante de dt et vaut 1). Lexemple B.1 en annexe compl` ete cette illustration par une analyse de la variance du bruit w en temps discret (` a la cadence dt). %============================================================= clear all close all % D efinition du filtre: G=tf([-1 100],[1 50 10000]) % Calcul dune r ealisation: [A,B,C,D]=ssdata(G); % Calcul de la variance par l equation de Lyapunov: P=lyap(A,B*B); var_w=C*P*C % ==> On retrouve bien le r esultat esp er e: variance=1/50. % ( ecart type: sigma=0.14) % Validation en simulation: voir fichier SIMULINK: simule_bruit.mdl
% Choix dun pas d echantillonnage rapide par rapport ` a % la dynamique du filtre (100 rd/s): dt=0.0001; % Simulation: sim(simule_bruit); % Trac e des r esultats: plot(b.time,b.signals.values,k); % Calcul num erique de la variance de b(t): var(b.signals.values) % On retrouve var_b=1/dt=10000. figure plot(w.time,w.signals.values,k) % Calcul num erique de la variance de w(t): var(w.signals.values) % On retrouve var_w=1/50 (` a peu pr es)
Merci denvoyer un mail ` a alazard@supaero.fr avec Introduction Kalman pour sujet si vous d esirez une copie des 2 chiers bruit.m et simule bruit.mdl.
8
Page 24/74
1.4 Illustration sous Matlab % On multiplie le pas d echantillonnage par 10: dt=0.001; sim(simule_bruit); figure(1) hold on plot(b.time,b.signals.values,g-); var(b.signals.values) % On retrouve var_b=1/dt=1000. figure(2) hold on plot(w.time,w.signals.values,g-) var(w.signals.values) % On retrouve var_w=1/50 (` a peu pr es). %=============================================================
25
b(t) BandLimited White Noise: Noise power =1 Sample time=dt
s+100 s2+50s+10000 G(s)
w(t)
w To Workspace
b To Workspace1
Figure 1.10: Fichier SIMULINK simule bruit.mdl pour la simulation du bruit de roulement.
400 300 200 100 b(t) 0 100 200 300 400 0
0.5
1 Temps (s)
1.5
Figure 1.11: R eponse du bruit blanc normalis e b(t) (avec dt = 0.0001 s: noir; avec dt = 0.001 s: gris).
Page 25/74
26
0.4 0.3 0.2 0.1 0 0.1 0.2 0.3 0.4 0.5 0 2w=0.28
w(t)
0.5
1 Temps (s)
1.5
Figure 1.12: R eponse du bruit de roulement w(t) (avec dt = 0.0001 s: noir; avec dt = 0.001 s: gris).
1.5
Conclusion
Ce chapitre a permis de pr esenter les outils math ematiques utilis es pour analyser les signaux al eatoires continus et leurs transmission dans les syst` emes dynamiques lin eaires. Il a egalement permis dintroduire la notion de bruit gaussien centr e (hypoth` ese requise dans le mod` ele de Kalman pr esent e dans le chapitre suivant) qui a lavantage d etre enti` erement caract eris e par sa fonction dautocorr elation (ou son spectre complexe sil est stationnaire). Le lecteur trouvera en annexe B des compl ements pour lanalyse des signaux al eatoires discrets.
Page 26/74
27
Chapter 2 Le ltre de Kalman

2.1
2.1.1
Principe du ltre de Kalman

Le mod` ele de Kalman
Nous reprenons le mod` ele pr esent e au d ebut du chapitre 1 qui fait appara tre des entr ees d eterministes u(t) et al eatoires w(t) et v (t). Nous supposerons donc que notre syst` eme perturb e peut etre mod elis e par le mod` ele d etat suivant appel e mod` ele de Kalman : t) = Ax(t) + Bu(t) + M w(t) x( equation d etat, x Rn , u Rm , w Rq y (t) = Cx(t) + Du(t) + v (t) equation de mesure, y Rp , v Rp (2.1) auquel nous adjoindrons les hypoth` eses suivantes.
2.1.2
Hypoth` eses
Nous supposerons que : H1: La paire (A, C ) est d etectable, cest-` a-dire quil ny a pas de mode instable et inobservable dans le syst` eme, H2: les signaux w(t) et v (t) sont des bruits blancs gaussiens centr es de Densit e Spectrale de Puissance (DSP) W et V respectivement, cest-` a-dire : E [w(t) w(t + )T ] = W ( ), E [v (t) v (t + )T ] = V ( ) E [w(t) v (t + )T ] = 0 (cette derni` ere relation traduit lind ependance stochastique des bruits w(t) et v (t) : cette hypoth` ese est introduite pour all eger les calculs qui vont suivre mais nest pas n ecessaire. On trouvera Introduction au ltre de Kalman
Page 27/74
28
2. Le filtre de Kalman dans la r ef erence [4] les formules qui prennent en compte une corr elation entre les bruits d etat et de mesure).
H3: V est inversible (il y a autant de sources de bruits blancs ind ependantes que 1 de mesures dans l equation de mesure) . Quelques remarques: Bien que toute la th eorie du ltre Kalman soit valable dans le cas nonstationnaire, nous supposerons que le syst` eme et les bruits sont stationnaires: les matrices A, B , M , C , D, W et V sont suppos es ici ind ependantes du temps. La moyenne dun signal al eatoire, que lon appelle aussi biais, est consid er ee comme d eterministe et doit etre, le cas ech eant, extraite du signal w(t) pour que celui-ci satisfasse lhypoth` ese de signal centr e (hypoth` ese H2). Par exemple si le signal al eatoire w(t) qui perturbe le syst` eme lin eaire d eni par (2.1) est biais e et si ce biais E [w(t)] est connu alors on appliquera le ltre de Kalman sur le mod` ele suivant: u(t) x(t) = Ax(t) + [B M ] + M (w(t) E [w(t)]) . E [w(t)] y (t) = Cx(t) + Du(t) + v (t) Le bruit d etat w (t) = w(t) E [w(t)] est maintenant centr e. Si le biais E [w(t)] est inconnu alors on pourra le mod eliser comme une condition initiale sur une variable d etat suppl ementaire et le ltre de Kalman permettra destimer ce biais (voir exemple de la section 2.3). Si les bruits sont des bruits color es et caract eris es par des spectres complexes alors les r esultats de la section 1.3.2 permettront de prendre en compte la couleur (ou r eponse fr equentielle) de ces bruits par un mod` ele de Kalman augment e de la repr esentation Markovienne des bruits. Par exemple: si on conna t le spectre complexe ww (s) du signal al eatoire w(t) centr e et color e qui intervient dans l equation d etat (2.1), la d ecomposition ww (s) = T G(s)G (s) permettra de d eterminer une repr esentation Markovienne du signal al eatoire w(t) cest-` a-dire une repr esentation d etat de G(s) (voir exemple 1.6) : xG(t) = AG xG (t) + BG b(t) w(t) = CG xG (t) o` u b(t) est maintenant un signal al eatoire de spectre complexe unitaire bb (s) = Iqq (cest-` a-dire un bruit blanc normalis e).
1
V est donc une matrice d enie positive et W est une matrice semi-d enie positive.
Page 28/74
2.1 Principe du filtre de Kalman Le mod` ele augment e: t) x( A M CG x(t) = 0 AG xG (t) xG (t) x(t) y (t) = [C 0] xG (t)
29
B 0
u(t) +
0 BG
b(t)
satisfait maintenant les hypoth` eses du mod` ele de Kalman. En fait toute linformation d eterministe que lon peut conna tre du syst` eme doit etre regroup ee dans le mod` ele (soit x = Ax + Bu, y = Cx + Du et la matrice M ); toute linformation al eatoire doit etre regroup ee dans les bruits w(t) et v (t). Le bruit d etat wx = M w repr esente les perturbations ext erieures (le vent dans le cas dun avion, les irr egularit es de la route dans le cas dune voiture, ...) et/ou egalement les erreurs de mod elisation ( ecart entre le mod` ele tangent et le mod` ele non-lin eaire qui appara t lors de la lin earisation, ph enom` enes dynamiques n eglig es,...): wx est un majorant de tout ce qui fait que l etat n evolue pas exactement comme le pr edit le mod` ele d eterministe x = Ax + Bu.
2.1.3
Structure dun estimateur non biais e
Un ltre de Kalman est un syst` eme dynamique avec 2 entr ees (vectorielles): la commande d eterministe u et la mesure y , cest-` a-dire tous les signaux connus du syst` eme. L etat x (ou la sortie) de ce ltre est un estim e de l etat x du syst` eme. Soit : (t) = Af x(t) + [Bf x u(t) y (t) = Af x(t) + Bf u(t) + Kf y (t) Kf ] (2.2) (2.3)
la repr esentation d etat de ce ltre. Bien entendu il faut initialiser ce ltre avec x(t0 ): lestim e de l etat du syst` eme ` a linstant initial t0 . On note (t) = x(t) x(t) lerreur destimation de l etat du syst` eme et (t0 ) = x(t0 ) x(t0 ) lerreur dinitialisation. En retranchant l equation (2.3) de l equation d etat (2.1) et en utilisant l equation de mesure, nous pouvons ecrire :
= Ax + Bu + M w Af x Bf u Kf (Cx + Du + v ) = (A Kf C )x Af x + (B Bf Kf D)u + M w Kf v = (A Kf C ) + (A Kf C Af )x + (B Kf D Bf )u + M w Kf v . (2.4)
Page 29/74
30
2. Le filtre de Kalman
Etant donn e que les bruits w et v sont gaussiens et le syst` eme est lin eaire, on peut armer que (t) est une variable al eatoire gaussienne. Nous allons maintenant nous int eresser ` a lesp erance math ematique (moyenne) de (t). Estimateur non biais e: avant tout, on souhaite que lestimateur soit non biais e, cest-` a-dire que: quel que soit le prol de commande u( ) appliqu e sur lhorizon [t0 , t], quel que soit linitialisation x(t0 ), on souhaite que la moyenne de lerreur destimation tende vers 0 lorsque t tend vers linni. Les bruits w et v etant centr es, nous pouvons ecrire : E [(t)] = E [ (t)] = (A Kf C )E [(t)]+(A Kf C Af )E [x(t)]+(B Kf D Bf )u(t) et limt E [(t)] = 0, u(t), E [x(t)], si et seulement si : (2.5) (2.6)
Af = A Kf C, et A Kf C
Bf = B K f D est stable.
En eet, dapr` es le th eor` eme 1.1 (page 21), on a alors : E [(t)] = e(AKf C )(tt0 ) (t0 ) et
t
lim E [(t)] = 0 .
Si lon reporte (2.5) dans (2.3), l equation du ltre de Kalman s ecrit: = (Ax + Bu) + Kf (y C x Du) . x (2.7)
On reconna t dans le premier terme du second membre de cette equation, le mod` ele du syst` eme (Ax + Bu) qui est exploit e pour pr edire l evolution de l etat du syst` eme ` a partir de lestimation courante x. Cette pr ediction est en fait une simulation en ligne du mod` ele du syst` eme. Le mod` ele etant faux, la pr ediction est recal ee en fonction de lerreur entre la mesure y et la mesure pr edite y = C x + Du et du gain du ltre Kf . Le signal derreur y y est aussi appel e linnovation. Le sch ema correspondant (dans le cas o` u D = 0) est repr esent e sur la gure 2.1. Cette structure garantit que lestimateur est non biais e quel que soient les matrices A, B , C , D du syst` eme et le gain Kf tel que A Kf C soit stable (cela justie en fait lhypoth` ese H1: la pr esence dun mode instable et inobservable ne permet pas de trouver de gain Kf stabilisant et donc de construire un estimateur non-biais e).
Page 30/74
` variance minimale 2.2 Estimateur a
31
Plant
Kf
B
+ + +
Z
^ x
A
^ x Kalman filter
Figure 2.1: Sch ema fonctionnel du ltre de Kalman (cas D = 0).
2.2
Estimateur ` a variance minimale
Le gain Kf est calcul e en fonction de la conance que lon a dans le mod` ele (exprim ee par la densit e spectrale W ) relativement ` a la conance que lon a dans la mesure (exprim ee par la densit e spectrale V ). Si le mod` ele est tr` es bon (W tr` es petit) et la mesure tr` es bruit ee (V tr` es grand) alors le gain Kf devra etre tr` es petit. En fait parmi tous les gains Kf satisfaisant la contrainte (2.6), nous allons choisir celui qui minimise la variance de lerreur destimation de l etat du syst` eme (t) (t). Nous rappelons (voir section pr ec edente) que (t) = x(t) x(t) est une variable al eatoire vectorielle (` a n composantes) centr ee (non-biais ee) gaussienne. Le caract` ere gaussien de cette variable permet darmer que si la variance de lerreur destimation est eectivement minimis ee, alors x(t) est vraiment le meilleur estim e de x(t).
2.2.1
Solution g en erale
On cherche donc Kf qui minimise :

n
J (t) =
i=1
E [i (t)2 ] = E [T (t)(t)] = trace E [(t)T (t)] = trace P (t) .
(2.8) (2.9) (2.10)
P (t) = E [(x(t) x(t))(x(t) x(t))T ]: matrice de covariance de lerreur destimation. En reportant (2.5) dans (2.4), l evolution de (t) est d ecrite par l equation d etat : w(t) (t) = (A Kf C )(t) + [M K ] , (2.11) v (t)
Page 31/74
32 avec : E
w(t) v (t)
[wT (t + ) v T (t + )] =
Wq q 0 q p 0pq Vpp
( ) .
On peut donc appliquer le th eor` eme 1.1 (page 21) et conclure que la covariance de lerreur destimation P (t) ob eit ` a l equation di erentielle : (t) = (A Kf C )P (t) + P (t)(A Kf C )T + [M P Kf ] W 0 0 V MT T Kf (2.12)
T = (A Kf C )P (t) + P (t)(A Kf C )T + M W M T + Kf V Kf .
t) : Pour minimiser trace P (t), il sut de minimiser trace P ( t)) (trace P ( = P (t)C T P (t)C T + 2Kf V Kf Kf (t) = P (t)C T V 1 . (2.13)
En reportant (2.13) dans (2.12), nous obtenons : (t) = AP (t) + P (t)AT P (t)C T V 1 CP (t) + M W M T . P (2.14)
Cette equation di erentielle de Riccati doit etre int egrer et initialiser avec P (t0 ) qui traduit la conance que lon a dans linitialisation du ltre avec x(t0 ): P (t0 ) = E [(x(t0 ) x(t0 ))(x(t0 ) x(t0 ))T ] . On obtient alors le gain Kf (t) ` a partir de P (t) et de l equation (2.13). Le ltre de Kalman est donc non-stationnaire. Les equations (2.7), (2.13) et (2.14) constituent les equations du ltre de Kalman continu quil faut int egrer ` a partir de linitialisation x(t0 ) et P (t0 ). Lint egration de (2.14) et le calcul de Kf (t) (2.13) peuvent etre eectu es en ligne ou hors ligne. Dans ce dernier cas, il faudra stocker dans le calculateur la loi Kf (t). En pratique, limplantation du ltre de Kalman se fera sur un calculateur num erique et donc en temps discret. On peut alors discr etiser l equation d etat (2.7) du ltre de Kalman (int egration par la formule des rectangles ou des trap` ezes, utilisation la transformation bilin eaire si lon ne sint eresse quau r egime permanent, ...). On peut aussi choisir de faire la synth` ese dun ltre de Kalman directement en discret (voir section 2.4.2). Enn, les equations du ltre sont enti` erement d enies par les donn ees du probl` eme, cest-` a-dire les matrices A, B , M , C , D, W et V .
Page 32/74
` variance minimale 2.2 Estimateur a
33
2.2.2
R egime permanent du ltre de Kalman
En r egime permanent, une fois pass e le r egime transitoire d u aux erreurs dinitialisation, lerreur destimation devient un signal al eatoire stationnaire (cela est vrai pour tous les signaux qui circulent dans le sch ema de la gure 2.1). On donc : (t) = 0 . P P , matrice constante d enie positive qui repr esente la covariance de lerreur destimation en r egime permanent, est la solution positive de l equation alg ebrique de Riccati : AP + P AT P C T V 1 CP + M W M T = 0 (2.15)
Le gain du ltre Kf = P C T V 1 devient egalement constant. On peut v erier que la positivit e de P implique la stabilit e du ltre, cest-` a-dire que toutes les valeurs propres de la matrice A Kf C sont ` a partie r eelle n egative (dapr` es le r egime permanent de l equation de Lyapunov (2.12)). On trouvera dans [1], une m ethode g en erale pour trouver la solution positive dune equation de Riccati. Sur la plan pratique, il faut savoir que tels solveurs sont disponibles dans les logiciels de CAO dautomatique : fonction lqe sous Matlab ou Scilab qui fournit directement P et Kf (voir aussi les fonctions care et kalman sous Matlab).
2.2.3
R eglage du ltre de Kalman
Pour un mod` ele donn e (matrices A, B , M , C , D), le gain du ltre Kf et son evolution en fonction du temps ne d ependent que de : W : la conance que lon a dans l equation d etat, V : la conance que lon a dans la mesure, P (t0 ): la conance que lon a dans linitialisation. En r egime permanent si le syst` eme et les bruits sont stationnaires, le gain de Kalman Kf est constant et sa valeur ne d epend plus que de W et V . Le gain Kf et la r eponse de lerreur destimation (t) d ependent du poids relatif de P (t0 ) par rapport ` a V (en r egime transitoire) et du poids relatif de W par rapport ` a V (en r egime permanent). Il est en eet ais e de v erier que le gain Kf (et donc le ltre) ne change pas si lon multiplie les trois matrices W , V et P (t0 ) par une constante . Par contre la covariance de lerreur destimation P sera egalement multipli ee par . Par cons equent il faudra v erier que ces 3 matrices sont r ealistes si lon souhaite analyser P pour juger de la qualit e de lestimation et conclure par Introduction au ltre de Kalman
Page 33/74
34
exemple: la probabilit e est de 95 % pour que la i` eme composante de l etat xi (t) soit comprise entre xi (t) 2 P(i,i) (t) et xi (t) + 2 P(i,i) (t) (propri et e des variables gaussiennes) . En pratique, il est donc n ecessaire de valider le ltre de Kalman sur un mod` ele de validation qui prend en compte des bruits de mesure r ealistes et surtout une mod elisation la plus ne possible des perturbations (non-lin earit es,...) que lon a major ees par un bruit d etat dans le mod` ele de Kalman. Quel que soit le r eglage, on peut v erier que la variance de lerreur destimation P (t) est toujours inf erieure ` a la variance du bruit d etat propag e dans l equation T T d etat (donn ee par P = AP + P A + M W M ) et la variance de lerreur destimation de la sortie non bruit ee yp = Cx + Du, cest-` a-dire : CP (t)C T , est toujours inf erieure ` a la variance du bruit de mesure (qui est innie en continu !! mais cette remarque reste vrai et egalement plus pertinente en discret, voir section 2.4). Il vaut mieux donc utiliser yp = C x + Du plut ot que la mesure brute y pour estimer la sortie r eelle du syst` eme yp . On pourra sappuyer sur les compromis suivants pour r egler qualitativement la r eponse de lerreur destimation. Inuence de P (t0 ) ./. V : en r egime transitoire lerreur destimation initiale 0 sera dautant plus vite recal ee (et la gain de Kalman sera dautant plus grand) que P (t0 ) est grand rapport ` a V . Mais lestim e sera alors pollu e par le bruit de mesure dans laquelle on accorde beaucoup de conance. Inuence de W ./. V en r egime permanent, le gain Kf sera tr` es faible et lestimation tr` es lisse si W est tr` es faible par rapport ` a V (on fait conance au mod` ele). Par contre, si le mod` ele est soumis ` a une perturbation que lon a sous estim ee en r eduisant trop W , lestim e ne suivra pas ou mettra beaucoup de temps ` a se recaler, la variance de lerreur destimation P ne sera pas non plus repr esentative de ce ph enom` ene. Ces comportements sont illustr es sur lexemple suivant qui, bien qu etant tr` es simple (mod` ele du premier ordre), permet de d egager des conclusions qui restent valables sur des cas plus complexes (continus ou discrets).
2.3
2.3.1
Exercices corrig es
Syst` eme du premier ordre
1 sa
Enonc e: on consid` ere un syst` eme stable du premier ordre G(s) = avec a < 0 .
La mesure y de la sortie de ce syst` eme est pollu ee par un bruit blanc v (t) de densit e spectrale unitaire. Lentr ee de ce syst` eme est soumise ` a un retard de transmission Introduction au ltre de Kalman
Page 34/74
s 2.3 Exercices corrige
35
de dur ee T inconnue et variable. Pour la synth` ese du ltre de Kalman que lon souhaite elaborer pour estimer la sortie de ce syst` eme, on propose de mod eliser (tr` es grossi` erement) cette perturbation de mod` ele comme un bruit blanc w(t), de densit e spectrale W et ind ependant de v , agissant directement sur le signal dentr ee selon la gure 2.2.
u + + w v 1 x sa + + y
Figure 2.2: Mod` ele de synth` ese pour le ltre de Kalman.
a) Donner le mod` ele de Kalman et calculer le ltre de Kalman permettant destimer la sortie x du syst` eme en fonction de a, W et P0 (la variance de lerreur dinitialisation: = x0 x0 ). Etudier le comportement asymptotique du gain du ltre pour W tendant vers 0. b) Construire un simulateur sous M atlab/Simulink an de v erier linuence des param` etres W et P0 sur la r eponse de lerreur destimation (t) et sur sa variance P (t). Application num erique: a = 1, x0 = 20, x0 = 0, T = 0, 0.1, 0.5, 1(s) (on essaiera ces diverses valeurs), u(t): cr eneau damplitude 10 et de p eriode 5 s, dt = 0.01 s (pas dint egration du simulateur). c) Que proposeriez vous de faire pour am eliorer la qualit e de lestim e dans le cas dun retard T important et connu (T = 1 s) ? Correction: Question a): le mod` ele de Kalman est imm ediat : x (t) = ax(t) + u(t) + w(t) y (t) = x(t) + v (t) (n = 1; m = 1; p = 1),
avec E [w(t)w(t + )] = W ( ), E [v (t)v (t + )] = ( ) et E [v (t)v (t + )] = 0. Le premi` ere etape dans le calcul du ltre de Kalman consiste ` a r esoudre l equation di erentielle de Riccati (2.14) qui est ici scalaire : (t) = 2aP (t) P (t)2 + W P On trouvera dans les formulaires de math ematiques ([6]) une m ethode g en erale de r esolution dune telle equation di erentielle scalaire. Cette m ethode consiste ` a: Introduction au ltre de Kalman
Page 35/74
36
2. Le filtre de Kalman chercher une solution particuli` ere constante (car les coecients de l equation di erentielle ne d ependent pas du temps) qui correspond en fait ` a la solution en r egime permanent p = p qui v erie :
2 P 2aP W = 0 .
La seule solution positive est P = a + chercher une solution g en erale du type:
a2 + W ,
P (t) = P +
1 . z (t)
(t) on trouve : Apr` es d eveloppement de P z (t) = 2 a2 + W z (t) + 1 . Lint egration de cette equation di erentielle par la m ethode de la variation de la constante donne :
1 2 2 z (t) = e2 a +W (tt0 ) 1 + e2 a +W (tt0 ) z0 2 2 a +W
o` u t0 est linstant initial et z0 = z (t0 ) est la condition initiale sur z (t) : z0 = 1 P0 P
avec P0 condition initiale sur P (t). En notant k = 2 a2 + W , il vient : P (t) = P + k (P0 P ) (P0 P )(ek(tt0 ) 1) + kek(tt0 ) k (P0 P ) = P + k(tt0 ) . e (P0 P + k ) + P P0
Enn: Kf (t) = P (t). Si W = 0 alors P = a + |a| = 0 si a < 0 (syst` eme stable)

t
lim Kf = 0 .
Une fois recal ee lerreur dinitialisation, on utilise plus que le mod` ele pour estimer x. Ceci est logique puisque lon a suppos e le mod` ele parfait (W = 0). Cela suppose egalement que le mod` ele soit stable. En eet si le mod` ele est instable (a > 0) alors : Introduction au ltre de Kalman
t
lim Kf = 2a .
Page 36/74
37
Cest la valeur du gain qui permet de stabiliser la dynamique du ltre (A Kf C ) tout en minimisant leet du bruit de mesure sur la variance de lerreur destimation de x. Question b): Le simulateur d evelopp e sous Matlab Simulink est repr esent e sur la gure 2.3. Les di erents param` etres dentr ee utilis es par ce chier Simulink (simuKalman.mdl) sont mentionn es ` a m eme le sch ema. La fonction Matlab Kf_t.m utilis ee pour implanter le gain Kf non stationnaire est donn ee en annexe C ainsi que le chier demoKalman.m d enissant les di erents param` etres et permettant le trac e 2 des r esultats . Di erents r esultats de simulation sont pr esent es sur les gures 2.4 ` a 2.8 : Figure 2.4 : La conance P0 dans linitialisation nest pas du tout repr esentative de lerreur destimation initiale qui est en fait tr` es importante (0 = x0 x0 = 20). Le transitoire de recalage de cette erreur initiale est donc long et lestimation de x ` a 2 (avec (t) = P (t)) ne permet pas de cadrer la valeur r eelle de x durent le r egime transitoire. Figure 2.5 : Si lon d egrade cette conance dans linitialisation (P0 = 100), ce transitoire devient plus rapide car le ltre exploite davantage les mesures. Lestim e est donc un peu plus bruit e durant ce transitoire. En r egime permanent, lestim e redevient plus lisse car on fait conance au mod` ele W = 1 (faible bruit d etat). On peut constater que lestim e est non biais e. Enn lestimation est bonne car pour cette simulation le mod` ele de validation (avec T = 0) correspond eectivement au mod` ele de synth` ese du ltre de Kalman. Figure 2.6 : Si maintenant on prends en compte un retard de 1 s dans le mod` ele de validation, la (bonne) conance que lon a dans le mod` ele (W = 1) ne permet pas de repr esenter les erreurs r eelles de mod` eles; le ltre fait conance au mod` ele (qui est faux), r eagi tr` es lentement et exploite mal les mesures. Figure 2.7 : Si lon sp ecie maintenant que le mod` ele nest pas bon (W = 100), le ltre fait davantage conance aux mesures : lestim e devient plus sensible au bruit de mesure. Figure 2.8 (r eponse ` a la question c)) : si lon sait eectivement quil y un retard de 1 s, on peut en tenir compte dans le mod` ele de Kalman soit en propageant directement ce retard dans la pr ediction soit, ce qui est propos e ici, en prenant compte dun ltre de Pade en s erie avec le mod` ele du syst` eme (voir chier demoKalman.m).
Merci denvoyer un mail ` a alazard@supaero.fr avec Introduction Kalman pour sujet si vous d esirez une copie de ces chiers.
2
Page 37/74
38
v SIMULATION PARAMETERS BandLimited White Noise: Noise power=V; Sample Time=dt; u Signal Generator x = Ax+Bu y = Cx+Du Transport Delay 1/(s+1) Time Delay=T A=1;B=1;C=1; D=0;X0 x Start time: 0s Stop time: 10s Solver: ode4 (Runge Kutta) Fixed step=dt y
Scope Clock output MATLAB Kf_t Function b 1 s Integrator Initial Condition=0 a c To Workspace StructureWithTime
Figure 2.3: Fichier SIMULINK simuKalman.mdl pour la simulation du ltre de Kalman.
50 y(t) x(t) u(t) xhat(t) xhat(t)+2 xhat(t)2
40
30
20
10
10
20
30
40
10
Figure 2.4: Simulation avec P0 = 1, W = 1, T = 0.
Page 38/74
39
40
30
20
10
10
20
30
40
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
Figure 2.5: Simulation avec P0 = 100, W = 1, T = 0 (zoom autour du transitoire) .
40
30
20
10
10
20
30
40
50
10
Page 39/74
40
40
30
20
10
10
20
30
40
50
10
30
20
10
10
20
30
40
50
10
Figure 2.8: Simulation du ltre de Kalman stationnaire calcul e sur un mod` ele tenant compte dune approximation de Pade ` a lordre 2 du retard (W = 1, T = 1 s).
Page 40/74
41
2.3.2
Estimation dun biais
Enonc e: un mobile se d eplace le long dun axe Ox. On mesure la vitesse x et la position x de ce mobile et on note vm et xm ces mesures. La mesure xm est entach ee dun bruit blanc gaussien centr e v (t) de densit e spectrale unitaire V = 1 (m2 /Hz ) : xm (t) = x(t) + v (t). La mesure vm est biais ee par un signal b(t) mod elis e comme un echelon damplitude b0 inconnue : vm (t) = x(t) + b(t). A partir des 2 mesures vm et xm , on d esire construire un ltre de Kalman permettant destimer la position x(t) et le biais b(t). 1) Donnez les equations d etat du mod` ele de Kalman avec x et b comme variable d etat, vm comme variable dentr ee, xm comme variable de sortie. 2) Interpr eter ce r esultat ` a laide dun sch ema fonctionnel. En fait le biais b(t) est susceptible de d eriver avec le temps. Pour tenir compte de ces variations eventuelles, on suppose que la d eriv ee du biais est pollu ee par un bruit blanc w(t) de densit e spectrale W = q 2 ind ependant de v (t) : b(t) = w(t) . 3) Donnez les nouvelles equations du mod` ele de Kalman, calculez le gain de Kalman en r egime permanent (en fonction de q ) et donner la repr esentation d etat du ltre permettant de calculer les estim es x et b ` a partir de xm et de vm . 4) Comment proc ederiez vous pour estimer la vitesse du mobile x ? 5) Calculer la matrice de transfert F (s) du ltre : X (s) (s) X = F (s) Xm (s) Vm (s) .
(s)/Vm (s)) en fonction q . Montrer que 6) Commentez ce transfert (notamment X ce ltre F (s) donne des estim es parfaits si les mesures sont parfaites. Correction: Question 1. On peut directement ecrire : vm (t) = x (t) + b(t) xm (t) = x(t) + v (t) soit: x = b + vm xm = x + v .
Page 41/74
42
= 0) avec une condition initiale b0 . Le biais est mod elis e comme un etat int egral (b On a donc : x 0 1 x 1 = + vm 0 0 b 0 b (2.16) x xm = [ 1 0 ] +v b avec E [v (t)v T (t + )] = ( ). Question 2. Le sch ema fonctionnel du mod` ele de Kalman est repr esent e sur la gure 2.9
b0 s + x
vm
+ +
xm
Figure 2.9: Sch ema fonctionnel du mod` ele.
Question 3. Si lon calcule le ltre de Kalman sur le mod` ele 2.16, on trouvera un gain nul en r egiment permanent car l equation d etat nest pas bruit ee: une fois le biais initial estim e, le ltre ne sera plus capable de d etecter une d erive eventuelle de ce biais. Pour pallier ` a ce probl` eme on introduit un bruit w sur b(t) pour sp ecier que ce biais nest pas constant. Le mod` ele s ecrit alors : x b = 0 1 0 0 0 ] x 1 + b 0 x +v b vm + 0 1 w (2.17)
xm = [ 1
avec E [w(t)wT (t + )] = q 2 ( ). On a bien un mod` ele de la forme de (2.1) et on peut identier les matrices A, B , C , M . p1 p12 La solution en r egime permanent s ecrit : Kf = P C T V 1 avec P = p12 p2 solution positive de l equation alg ebrique de Riccati:
0 0 1 0 p1 p12 p12 p2 + p1 p12 p12 p2 0 1 0 0 p1 p12 p12 p2 1 0 0 0 p1 p12 p12 p2 + 0 0 0 q2 = 0 0 0 0
soit :
2p12 + p2 1 = 0 p2 + p1 p12 = 0 . 2 p2 12 = q
Page 42/74
s 2.3 Exercices corrige la solution positive s ecrit: P = 2q q q q 2q Kf = 2q q .
43
L equation d etat du ltre s ecrit (dapr` es (2.7)) : x b =A x +Bvm +Kf b x b xm C x b = (AKf C ) + x +[Kf b xm vm B] xm vm ,
soit :
2q 1 q 0
x b
2q 1 q 0
Question 4. On a directement un estim e non-biais e de la vitesse en retranchant lestim e du biais b de la mesure de vitesse vm : x = vm b. Question 5. La repr esentation d etat du transfert recherch e s ecrit : x x 2q 1 2q 1 xm + = q 0 q 0 vm b b x x 1 0 0 0 xm = + 0 1 0 1 vm x b La matrice de transfert F (s) correspondante s ecrit donc : F (s) = 0 0 0 1 + 1 0 0 1 X (s) (s) X Question 6. = s 0 0 s 2q 1 q 0
1
2q 1 q 0
On en d eduit : 2qs + q s 2 qs s + 2qs s2 + 2qs + q Xm (s) Vm (s)
(2.18)
X s2 + 2qs . (s) = 2 Vm s + 2qs + q Cest un ltre passe haut du second ordre de pulsation q (rd/s) et damortissement 2/2 (q ) qui a un gain statique nul (on ltre les composantes continues). La fr equence de coupure est dautant plus grande que q est grand, cest-` a-dire que le biais est susceptible de d eriver. Si les mesures sont parfaites alors xm = x et vm = x . On a donc: Introduction au ltre de Kalman
Page 43/74
44
2. Le filtre de Kalman Xm (s) = X (s) et Vm (s) = sX (s). En reportant cela dans la premi` ere ligne de (2.18), on obtient: ( 2qs + q )X (s) + s2 X (s) X (s) = X (s) = X (s) . s2 + 2qs + q (s) et Xm (s) = 1/s X (s). En reportant cela dans la seconde ligne Vm (s) = X de (2.18), on obtient: (s) (s) + (s2 + 2qs)X X (s) = X (s) . X (s) = X s2 + 2qs + q
Le ltre nentra nent donc aucune d egradation de la qualit e de la mesure. On appelle un tel ltre un ltre compl ementaire. 2
2.4
2.4.1
Le ltre de Kalman discret

Le mod` ele de Kalman discret
Par analogie directe avec le cas continu le mod` ele de Kalman discret s ecrit : x(k + 1) = Ad x(k ) + Bd u(k ) + Md wd (k ) equation d etat, x Rn , u Rm , wd Rq y (k ) = Cd x(k ) + Du(k ) + vd (k ) equation de mesure, y Rp , vd Rp (2.19) Hypoth` eses : nous supposerons que : H1: La paire (Ad , Cd ) est d etectable, H2: les signaux wd (k ) et vd (k ) sont des bruits pseudo-blancs gaussiens centr es de matrices de covariance Wd et Vd respectivement, cest-` a-dire : E [wd (k ) wd (k + l)T ] = Wd (l), E [vd (k ) vd (k + l)T ] = Vd (l) E [wd (k ) vd (k + l)T ] = 0 (avec (l) = 1 si l = 0; 0 sinon). H3: Vd est inversible. Remarque 2.1 : Alors que dans le cas continu, les bruits blancs du mod` ele de Kalman sont d enis par des matrices de densit e spectrale de puissance (DSP) W et V (les variances sont innies), les bruits du mod` ele de Kalman discret sont d enis par leurs matrices de covariances Wd et Vd (les variances sont nies). Les densit es spectrales de puissance de ces bruits sont constantes (et egales ` a Wd et Vd ) mais sur une plage limit ee de la pulsation r eduite [ ] (voir annexe B.1); do` u le qualicatif de bruit pseudo-blanc. Introduction au ltre de Kalman
Page 44/74
2.4 Le filtre de Kalman discret
45
w(t) v(t) u(k) dt Bo(s) u(t) PLANT y(t) dt y(k)
Figure 2.10: Syst` eme continu avec commande echantillonn ee-bloqu ee et observation discr` ete.
u(k) u(k) dt 2 O 1 3 4 5 6 k 0 dt Bloqueur dordre 0 u(t) t u(t)
Figure 2.11: Le bloqueur dordre 0: Bo (s).
Remarque 2.2 : comme dans le cas continu, si le bruit wd est color e de spectre complexe ww (z ) la d ecomposition ww (z ) = G(z 1 )GT (z ) (voir section B.3.2) permet de trouver une repr esentation markovienne de wd que lon pourra prendre en compte dans un mod` ele de Kalman augment e.
2.4.2
Cas particulier dun syst` eme continu echantillonn e
Comme nous lavons d ej` a mentionn e, limplantation pratique des ltres de Kalman, m eme pour les syst` emes continus, ce fait en r` egle g en erale sur un calculateur num erique. Nous allons donc consid erer que la sortie du mod` ele continu (1.1) est echantillonn ee ` a la cadence dt (observation discr` ete) et nous supposerons que des bloqueurs dordre 0 sont plac es sur les signaux d eterministes u (voir gures 2.10 et 2.11) et nous allons chercher une repr esentation d etat discr` ete de ce mod` ele. On note x(k dt) = x(k ). Dapr` es la solution g en erale (1.3), lint egration de l equation d etat entre linstant t0 = k dt et t = (k + 1) dt s ecrit : x(k +1) = eAdt x(k )+
(k+1)dt k dt
eA((k+1)dt ) Bd
(k+1)dt
u(k )+
k dt
eA((k+1)dt ) M w( )d .
Le changement de variable : (k + 1)dt = v conduit au r esultat : x(k + 1) = eAdt x(k ) +

0 dt
eAv Bdv u(k ) +

0
dt
eAv M w((k + 1)dt v )dv .
L equation de sortie aux instants d echantillonnage s ecrit : y (k ) = Cx(k ) + Du(k ) + v (k ) Introduction au ltre de Kalman
Page 45/74
46
()
vv
/dt
/dt
Figure 2.12: Limitation en fr equence du bruit de mesure continu.
L equation d etat discr` ete est donc bien de la forme (2.19) avec : Ad = eAdt , Bd =
dt Av e Bdv, 0
Md = In ,
Cd = C,
(2.20)
Les bruits d etats et de mesure discrets s ecrivent :

dt
wd (k ) =
0
eAv M w((k + 1)dt v )dv,
vd (k ) = v (kdt) ,
il faut les caract eriser par leur matrices de covariances respectives. Du fait de l echantillonnage, la matrice de covariance du bruit de mesure discret devient : Vd = V /dt . (2.21) Justication: l equation de mesure continue y (t) = Cx(t) + Du(t) + v (t) fait intervenir un bruit blanc v (t) de variance innie et de DSP nie V . L echantillonnage de la mesure ` a ma cadence dt fournira donc une suite num erique y (k ) de variance innie et rendra singulier le calcul du ltre de Kalman. On doit limiter entre /dt et /dt la r eponse fr equentielle ou la DSP du bruit v (t) pour pouvoir echantillonner la mesure y (t) correctement. On retrouve, de fa con analogue ` a la condition de Shannon pour les signaux d eterministes, que l echantillonnage ne peut pas restituer les composantes du signal de pulsation sup erieure ` a /dt. La DSP du bruit de mesure continu limit e en fr equence est repr esent e sur la gure 2.4.2. La variance de ce signal est maintenant nie et vaut (voir remarque 1.3) :
2 v
1 = 2
V vv ( )d = 2
dt dt
d =
V . dt
L echantillonnage dun signal ne change pas sa variance donc l echantillonnage de la V mesure s ecrit : y (k ) = Cx(k ) + Du(k ) + vd (k ) avec E [vd (k ) vd (k + l)T ] = dt (l). Introduction au ltre de Kalman
Page 46/74
47 2
Calculons maintenant la matrice de covariance Wd du bruit d etat wd (k ) :

T Wd = E [wd (k )wd (k )] = E dt dt 0
eAv M w((k + 1)dt v )dv

0
dt
wT ((k + 1)dt )M T eA d
T
=
0 dt
eAv M E [w((k + 1)dt v )wT ((k + 1)dt )]M T eA dvd eAv M W ( v )M T eA dvd Wd =
T dt Av e M W M T eA v dv 0 T
=
0
(2.22)
Remarque 2.3 : on indique en annexe B (voir remarque B.2, equation (B.13)) comment calculer Wd mais on peut aussi utiliser lapproximation Wd dtM W M T si dt est petit par rapport au temps de r eponse du syst` eme. Enn on retiendra les macro-functions Matlab lqed ou kalmd qui englobent tous ces calculs et qui permettent dobtenir le ltre de Kalman discret en r egime permanent directement ` a partir des donn ees continues (A, B , C , D, M , W et V ) et dune cadence d echantillonnage dt (voir illustration Matlab en annexe C.3). Cette approche ne supporte pas des bruits d etat w et de mesure v corr el es.
2.4.3
Les equations r ecurrentes du ltre de Kalman
Le principe du ltre de Kalman discret est le m eme quen continu. Il utilise une pr ediction qui sappuie sur le mod` ele d eterministe et un recalage qui sappuie sur linnovation (di erence entre la mesure et la sortie pr edite) mais en discret on distinguera : l etat pr edit ` a linstant k + 1 connaissant toutes les mesures jusqu` a linstant k que lon note x(k + 1|k ) et auquel on associe la matrice de covariance de lerreur de pr ediction not ee : P (k + 1|k ) = E x(k + 1) x(k + 1|k ) x(k + 1) x(k + 1|k )
T
l etat estim e connaissant la mesure ` a linstant k + 1 (apr` es le recalage) que lon note x(k +1|k +1) auquel on associe la matrice de covariance de lerreur destimation not ee : P (k + 1|k + 1) = E x(k + 1) x(k + 1|k + 1) x(k + 1) x(k + 1|k + 1)
T
Page 47/74
48
Pr ediction: ` a linstant k , on conna t x(k |k ), on pr edit l etat ` a linstant k + 1 en utilisant le mod` ele d eterministe: x(k + 1|k ) = Ad x(k |k ) + Bd u(k ) . (2.23)
A linstant k , lerreur destimation etait caract eris e par P (k |k ). Le mod` ele de pr ediction etant faux, lerreur ne peut que cro tre et lerreur de pr ediction ` a linstant k + 1 sera caract eris ee par (voir annexe th eor` eme B.1) :
T P (k + 1|k ) = Ad P (k |k )AT d + Md Wd Md
(2.24)
Recalage: ` a linstant k + 1, on recale la pr ediction avec linnovation via le gain du ltre: x(k + 1|k + 1) = x(k + 1|k ) + Kf (k + 1) y (k + 1) Cd x(k + 1|k ) Du(k + 1) En utilisant l equation de mesure du mod` ele (2.19), on peut ecrire : x(k +1)x(k +1|k +1) = In Kf (k +1)Cd (x(k +1)x(k +1|k ))Kf (k +1)vd (k +1) et :
T
(2.25)
P (k + 1|k + 1) =
In Kf (k + 1)Cd P (k + 1|k ) In Kf (k + 1)Cd
T + Kf (k + 1)Vd Kf (k + 1)
T T = P (k + 1|k ) Kf (k + 1)Cd P (k + 1|k ) P (k + 1|k )Cd Kf (k + 1) T T + Kf (k + 1) Cd P (k + 1|k )Cd + Vd Kf (k + 1) .
(2.26)
Comme dans le cas continu, on cherche Kf (k +1) qui minimise trace(P (k +1|k +1)) : trace(P (k + 1|k + 1)) T T = 2P (k + 1|k )Cd + 2Kf (k + 1) Cd P (k + 1|k )Cd + Vd . Kf (k + 1) On en d eduit :
T T Kf (k + 1) = P (k + 1|k )Cd Cd P (k + 1|k )Cd + Vd 1
(2.27)
En reportant cette expression dans 2.26, on obtient : P (k + 1|k + 1) = In Kf (k + 1)Cd P (k + 1|k ) . (2.28)
Les equations (2.23), (2.24), (2.25), (2.27) et (2.28) constituent les equations du ltre de Kalman discret. On initialise (2.23) et (2.25) avec x(0|0), lestim e initial et on initialise (2.24), (2.27) et (2.28) avec P (0|0), la conance que lon a dans linitialisation. Introduction au ltre de Kalman
Page 48/74
49
Si le mod` ele et les bruits sont stationnaires, on peut int egrer (2.24), (2.27) et (2.28) hors ligne. En reportant (2.27) et(2.28) dans (2.24), on trouve une equation r ecurrente de Riccati en la covariance de lerreur de pr ediction :
T T P (k +1|k ) = Ad P (k |k 1)AT d Ad P (k |k 1)Cd Cd P (k |k 1)Cd +Vd T Cd P (k |k 1)AT d +Md Wd Md . (2.29) Enn, en r egime permanent, Kf (k + 1) = Kf (k ) = Kf , mais on distingue : 1
Pp = P (k + 1|k ) = P (k |k 1) = : la matrice de covariance de lerreur de pr ediction en r egime permanent qui v erie l equation alg ebrique discr` ete de Riccati :
1 T T T T Pp = Ad Pp AT d Ad Pp Cd (Cd Pp Cd + Vd ) Cd Pp Ad + Md Wd Md .
(2.30)
Pe = P (k + 1|k + 1) = P (k |k ) = : la matrice de covariance de lerreur destimation en r egime permanent qui v erie : Pe = (I Kf Cd )Pp . La repr esentation d etat du ltre de Kalman stationnaire en r egime permanent s ecrit alors (il sut de reporter (2.25) dans (2.23)) : x(k + 1|k ) = Ad (I Kf Cd )x(k |k 1) +[Ad Kf x(k |k ) = (I Kf Cd )x(k |k 1) +[Kf Bd Ad Kf D] Kf D ] y (k ) u(k ) y (k ) u(k )
(2.31) L etat de cette repr esentation correspond ` a l etat pr edit, la sortie correspond ` a l etat estim e. Remarque 2.4 : On a toujours 0 < Pe < Pp . En eet en reportant (2.27) dans (2.28), on obtient : T T Pe = Pp Pp Cd (Cd Pp Cd + Vd )1 Cd Pp . Le second terme du membre de droite est toujours positif donc : Pe < Pp , cest-` a-dire que la variance de lerreur destimation et toujours inf erieur ` a la variance de lerreur de pr ediction (ou la variance du bruit d etat propag e dans l equation d etat).
Page 49/74
50
2. Le filtre de Kalman Enn on peut estimer la sortie yp (k ) = Cd x(k )+ Du(k ) par yp (k ) = Cd x(k |k )+ Du(k ). La covariance de lerreur destimation de cette sortie (non-bruit ee) est :
T T T T T T T Cd Pe Cd = Cd Pp Cd Cd Pp Cd (Cd Pp Cd +Vd )1 Cd Pp Cd = Cd Pp Cd (Cd Pp Cd +Vd )1 Vd .
Do` u:
T T T T +Vd )1 Vd < 0 . +Vd )1 I Vd = Vd (Cd Pp Cd (Cd Pp Cd Vd = Cd Pp Cd Cd Pe Cd T < Vd cest-` a-dire que yp (k ) est un meilleur estim e On a donc toujours Cd Pe Cd de yp que la mesure directe.
Nous ne d etaillerons pas les techniques de r esolution de l equation de Riccati discrete (DARE: Discrete Algebraic Riccati Equation). Des macro-functions (lqe sous Scilab ou dlqe, dare, kalman sous Matlab) permettent de r esoudre de telles equations, de calculer le gain Kf et de fournir la repr esentation d etat (2.31) du ltre en r egime permanent. Lutilisation de ces fonctions est illustr ee dans la session Matlab pr esent ee en annexe C.3. Exercice 2.1 D emontrer que dans le cas dun syst` eme continu echantillonn e` a la cadence dt, le ltre de Kalman continu synth etis e` a partir des donn ees continues (A, B , M , C , W , V ) puis discr etis e ` a la cadence dt par la m ethode d Euler et le ltre de Kalman discret synth etis e ` a partir des donn ees des equations (2.20), (2.21) et (2.22) tendent vers la m eme solution lorsque dt tend vers 0 (calcul au premier ordre en dt). Solution : Le ltre de Kalman continu est d eni par les equations (2.7), (2.13) t) et x(t) par x(k+1)x(k) et et (2.14). La m ethode d Euler consiste ` a remplacer x( dt x(k ) respectivement. En reportant (2.13) dans (2.7), ce premier ltre discret est enti` erement d eni par : P (k + 1) = P (k ) + dt AP (k ) + P (k )AT P (k )C T V 1 C (k )P + M W M T x(k + 1) = x(k ) + dt Ax(k ) + Bu(k ) + P (k )C T V 1 y (k ) C x(k ) Du(k ) (2.32) La covariance de lerreur de pr ediction du ltre de Kalman discret est d enie par l equation (2.29), en notant P (k |k 1) = P (k ), en utilisant les relations (2.20), (2.21) et (2.22) et en remarquant quau premier ordre Wd dtM W M T , on obtient : P (k +1) eAdt P (k )eA
T dt
eAdt P (k )C T CP (k )C T +V /dt
CP (k )eA
T dt
+dtM W M T .
Soit en d eveloppant au premier ordre : P (k + 1) (I + Adt)P (k )(I + AT dt) + dtM W M T dt(I + Adt)P (k )C T I dtV 1 CP (k )C T V 1 CP (k )(I + AT dt) P (k ) + dt AP (k ) + P (k )AT P (k )C T V 1 CP (k ) + M W M T + dt2 ( ) . Introduction au ltre de Kalman
Page 50/74
51
On voit donc quau premier ordre on retrouve la premi` ere equation de (2.32). Le gain Kf devient : Kf (k ) = P (k )C T CP (k )C T V 1 dt + I
1
V 1 dt P (k )C T V 1 dt,
et l equation d etat du ltre ( equation (2.31)) devient (on note x(k |k 1) = x(k ) et on remarque que Bd dtB au premier ordre) : x(k + 1) = eAdt I dtP (k )C T V 1 C x(k ) + dteAdt P (k )C T V 1 y (k ) + Bd dt eAdt P (k )C T V 1 D u(k ) (I Adt) I dtP (k )C T V 1 C x(k ) + dt(I Adt)P (k )C T V 1 y (k ) + Bd dt(I Adt)P (k )C T V 1 D u(k ) x(k ) + dt Ax(k ) + Bu(k ) + P (k )C T V 1 y (k ) C x(k ) Du(k ) + dt2 ( ) .
Au premier ordre, on retrouve la seconde equation de (2.32). Les 2 ltres discrets sont donc equivalents lorsque dt tend vers 0.
2.4.4
Exemple
On reprend lexercice 2.3.2 sur lestimation dun biais. On souhaite implanter le ltre sur un calculateur num erique, les deux mesures vm et xm etant echantillonn ees ` a la cadence dt. Donner les equations d etat du mod` ele de Kalman discret avec [x(k ), comme vecteur d etat. b(k )]T
Calculer sous Matlab le gain Kf et la covariance de lerreur destimation en r egime permanent. Application num erique: dt = 0.01 s, V = 1, q = 1. Correction : Le calcul du mod` ele d etat discret consiste ` a appliquer les formules de discr etisation (2.20), (2.21) et (2.22) au mod` ele continu d eni par l equation (2.17). On suppose la mesure de vitesse vm (k ) constante sur une p eriode d echantillonnage; ce qui correspond bien ` a lintroduction dun bloqueur dordre 0. On a donc : 0 dt Ad = e 0 0

1 0 0 1
dt
0 dt 0 0 1 0
0 0 0 0 dt 0
+ =
1 dt 0 1
Bd =
0
1 v 0 1
dv =
Page 51/74
52 1 , dt
Cd = [1 0] ,
dt
Vd =
Wd =
0
1 v 0 1
0 0 0 q2
1 2 q dt3 3 1 2 2 q dt2
1 0 v 1 1 q 2 dt2 2 q 2 dt
dt
dv =
0
q 2 v 2 q 2 v q 2 v q2 .
dv
Wd =
0 0 2 0 q dt
Le mod` ele de Kalman discret s ecrit donc : x(k + 1) 1 dt x(k ) dt = + b(k + 1) 0 1 b(k ) 0 x xm (k ) = [ 1 0 ] + vd (k ) b
vm (k ) + wd (k ) (2.33)
On trouvera en annexe C.3 le chier Matlab demoKalmand permettant de calculer ces di erents param` etres du mod` ele et de d eterminer le gain Kf et la covariance de lerreur destimation en r egime permanent ` a partir de la fonction dlqe. On montre egalement comment utiliser la fonction lqed directement ` a partir des donn ees du mod` ele continu. 2
2.5
2.5.1
Exercices
Syst` eme du second ordre:
Une masse m se d eplace le long dun axe Ox sous leet dune commande en force u(t). Une force perturbatrice w(t) agit egalement sur cette masse. w(t) est mod elis e comme un bruit blanc gaussien centr e de densit e spectrale W . On mesure la position x(t) de cette masse. On note xm (t) cette mesure. La mesure xm (t) est entach ee dun bruit blanc gaussien centr e v (t) de densit e spectrale V . 2 A.N.: m = 1 (Kg ); W = 1 (N /Hz ); V = 2 (m2 /Hz ) . A partir de la mesure xm et de la commande u, on d esire construire un ltre t) de cette masse. de Kalman permettant destimer la position x(t) et la vitesse x( On note x(t) et x (t) ces estim es. 1) Donnez les equations d etat du mod` ele de Kalman. 2) Calculez le gain de Kalman en r egime permanent (en fonction de ). 3) Calculez la matrice de transfert F (s) du ltre : X (s) (s) X Introduction au ltre de Kalman = F (s) Xm (s) U (s)
Page 52/74
2.5 Exercices
53
4) Commentez ce transfert (r eponses fr equentielles des di erentes composantes, evolution en fonction de ). 5) On souhaite implanter le ltre sur un calculateur num erique ` a la cadence d echantillonnage dt. Donner les equations d etat du mod` ele discret et les equations r ecurrentes du ltre de Kalman discret. On note x0 et x 0 lestimation initiale et P0 la covariance de lerreur destimation initiale.
Page 53/74
PAGE SANS TEXTE
55
Chapter 3 A propos des unit es physiques

Il est important de donner des unit es physiques aux di erentes caract eristiques des signaux al eatoires que lon a ` a utiliser ou ` a simuler. Le but de ce chapitre est de clarier ce point. On rappelle avant tout que si on d esigne par u lunit e physique dune variable al eatoire X , alors sa fonction de r epartition F (x) est sans unit e et sa densit e de probabilit e f (x) a pour dimension u1 . Si on d esigne par u lunit e physique dun signal al eatoire continu w(t) alors les unit es physiques des di erentes caract eristiques stochastiques sont rappel ees dans le tableau 3.1 (on rappelle egalement les equations qui permettent de v erier lhomog en eit e des dimensions). Variable signal esp erance auto-corr elation (variance) spectre complexe (et DSP) Notation w(t) E [w(t)] ww (t, ) ww (s) Unit e physique Equations u u (1.8) et (1.5) 2 u (1.9) et (1.6) 2 2 u s ou (u /Hz ) (1.10)
Table 3.1: Unit es physiques des caract eristiques dun signal al eatoire continu.
Dans le cas dun signal al eatoire discret, on se reportera au tableau 3.2 (voir annexe B.1). On constate donc un facteur temps entre le spectre complexe dun signal continu et celui dun signal discret ou echantillonn e. L equation (2.21), utilis ee lors de la discr etisation de l equation de mesure dun syst` eme continu, respecte cette homog en eit e. Pour caract eriser un bruit continu sur une grandeur u , on sp ecie parfois egalement la racine carr ee de la DSP qui sexprime donc en u/ Hz (et que lon apparente abusivement ` a un ecart type) . Si lon consid` ere maintenant le mod` ele de Kalman continu (2.1) et si lon note Introduction au ltre de Kalman
Page 55/74
56 Variable signal esp erance auto-corr elation (variance) spectre complexe (et DSP)
s physiques 3. A propos des unite Notation w(n) E [w(n)] ww (n, k ) ww (z ) Unit e physique u u u2 u2
Table 3.2: Unit es physiques des caract eristiques dun signal al eatoire discret.
u lunit e physique de la variable d etat x(t), alors lunit e physique du bruit d etat wx (t) = M w(t) est en u/s et sa DSP Wx = M W M T sexprime en u2 /s. Dans le cas du mod` ele de Kalman discret (2.19), si lon note u lunit e physique de la variable d etat x(k ) (et donc de x(k + 1)) alors Md wd (k ) sexprime egalement T 2 en u et la DSP du bruit d etat Md Wd Md sexprime en u . L equation (2.22) utilis ee pour discr etiser un bruit d etat continu ou son approximation Wd = W dt respecte egalement cette homog en eit e.
Page 56/74
57
R ef erences
1 Robustesse et Commande Optimale: D. Alazard, C. Cumer, P. Apkarian, M. Gauvrit et G. Ferr` eres - C epadu` es Editions, 2 Commande num erique des syst` emes: application aux engins mobiles et aux robots: Ouvrage collectif publi e sous la direction de C. Fargeon, Editions Masson. 3 Linear optimal control systems: H. Kwakernaak and R. Sivan, Wiley interscience, John Wiley and Sons, 1972. 4 Le ltrage et ses applications: M. Labarr` ere, J. P. Krief et B. Gimonet, C epadu` es Editions. 5 Les processus stochastiques: J. L. Pac, polycopi e SUPAERO. 6 Formulaire de math ematiques sp eciales: G Flory, Ed. Vuibert
Page 57/74
PAGE SANS TEXTE
59
Appendix A Int egration de l equation l etat

A.1 Cas continu
x (t) = Ax(t) + Bu(t) y (t) = Cx(t) + Du(t) (A.1)
Consid erons le mod` ele d etat continu :
La r eponse de ce mod` ele ` a des entr ees d eterministes u(t) sur un horizon t [t0 , t] et ` a des conditions initiales x(t0 ) s ecrit : x(t) = eA(tt0 ) x(t0 ) + y (t) = Cx(t) + Du(t)
t t0
eA(t ) Bu( ) d
(A.2) (A.3)
D emonstration: la solution g en erale de l equation d etat dite sans second membre x (t) Ax(t) = 0 s ecrit: x(t) = eAt K . La recherche dune solution particuli` ere de l equation d etat par la m ethode de la variation de la constante (K K (t)) entra ne : (t) = AeAt K (t) + Bu(t) AeAt K (t) + eAt K (t) = eAt Bu(t) K
t
(A.4) (A.5) (A.6) (A.7)
K (t) =
t0
eA Bu( ) d + K0
t t0
x(t) = eAt K0 +
eA(t ) Bu( ) d .
La prise en compte de la condition initiale ` a t = t0 entra ne K0 = eAt0 x(t0 ) et permet de trouver la solution particuli` ere (A.2). Enn, on a la relation statique : y (t) = Cx(t) + Du(t). Introduction au ltre de Kalman
Page 59/74
60
gration de le quation le tat A. Inte 2
Remarque A.1 En appliquant (A.2) avec t0 =0, x0 = 0 et u(t) = (t) (le Dirac), la r eponse impulsionnelle g en erale du syst` eme d eni par le quadruplet (A, B , C , D) s ecrit donc: f (t) = CeAt B + D (t) t 0 La transform ee de Laplace: F (s) =
+ + 0
(f (t) = 0 si t < 0) .
f ( )e s d permet de retrouver le r esultat : (s domaine de convergence). (A.8)

Y (s) U (s)
F (s) =
0
(CeA B +D ( ))e s d = C (sI A)1 B +D
Exemple A.1 On consid` ere le syst` eme du second ordre: a) calculer la r eponse impulsionnelle du syst` eme,
1 . (s+1)2
b) calculer la r eponse du syst` eme ` a des conditions initiales y (0) = y0 et y (0) = y0 (on suppose t0 = 0). Correction: si lutilisation des tables de transform ees de Laplace permet de t r epondre directement ` a la question a): y (t) = te , il est recommand e dutiliser une repr esentation d etat et l equation (A.2) pour r epondre ` a la question b). Consid erons une repr esentation sous forme de Jordan du syst` eme : 1 1 0 x = x+ u (A.9) . 0 1 1 y= [ 1 0 ]x La matrice dynamique A n etant pas diagonalisable (une valeur propre double), le calcul de lexponentielle de matrice eAt peut etre eectu e par son d eveloppement limit e: A2 t2 A3 t3 A4 t4 + + + . eAt = I + At + 2! 3! 4! On obtient alors : t t e 0 t

1t+
t2 2!
t3 3!
+ t t2 + t t + 2! 3! 2 t t3 1 t + 2! 3! +
et tet 0 et
R eponse impulsionnelle: avec t0 = 0, x(t0 ) = 0 et u(t) = (t) (impulsion de Dirac), l equation (A.2) devient :
t
x(t) =
0
(t )e t e t
( )d =
tet et
et
y (t) = [1
0]x(t) = tet .
Page 60/74
A.2 Cas discret
61
R eponse ` a des conditions initiales (u(t) = 0): il faut maintenant etablir la relation T entre le vecteur d etat x = [x1 , x2 ] de la repr esentation (A.9) et le vecteur constitu e de la sortie y et de sa d eriv ee y sur lesquelles sont sp eci ees les conditions initiales. L equation de mesure et sa d erivation nous permet d ecrire : y y On en d eduit que : y (t) = CeAt x(t0 ) = [1 0] et tet 0 et 1 0 1 1 y0 y0 = et (1 + t)y0 + tet y0 . = 1 0 1 1 x x(t0 ) = 1 0 1 1 y0 y0 .
Remarque: nous aurions pu egalement consid erer une repr esentation sous forme compagne horizontale qui fait directement appara tre dans le vecteur d etat la sortie et sa d eriv ee : y 0 1 y 0 = + u. (A.10) y 1 2 y 1 Le calcul de lexponentielle de matrice donne alors : 0 t t 2t e

et (1 + t) tet tet et (1 t)
. 2
A.2
Cas discret
x(k + 1) = Ad x(k ) + Bd u(k ) y (k ) = Cd x(k ) + Du(k ) (A.11)
Consid erons le mod` ele d etat discret :
La r eponse de ce mod` ele ` a des entr ees d eterministes u(.) sur un horizon [k0 , k ] et ` a des conditions initiales x(k0 ) = x0 s ecrit :
k1
x( k ) =
k0 Ak x0 d
+
i=k0
k0 Ai Bd u(k 1 i + k0 ) d
(A.12) (A.13)
y (k ) = Cd x(k ) + Du(k )
la d emonstration est imm ediate en r esolvant la r ecurrence sur l equation d etat : x(k ) = Ad x(k 1) + Bd u(k 1) = A2 d x(k 2) + Ad Bd u(k 2) + Bd u(k 1) =
Page 61/74
62
gration de le quation le tat A. Inte
Remarque A.2 En appliquant (A.12) avec n0 =0, x0 = 0 et u(i) = (i) (le Dirac en discret: (0) = 1; (i) = 0 si i = 0), la r eponse impulsionnelle g en erale du syst` eme d eni par le quadruplet (Ad , Bd , Cd , D) s ecrit donc: f (0) = D,
1 f (i) = Cd Ai d Bd i=0
i 1 (f (i) = 0 si i < 0) .
La transform ee en Z : F (z ) =
f (i)z i permet de retrouver le r esultat : (z domaine de convergence). (A.14)
F (z ) =
i=1
1 i 1 Cd Ai d Bd z +D = Cd (zI Ad ) Bd +D
Page 62/74
63
Appendix B Passage dun bruit dans un syst` eme lin eaire

Les d emonstrations qui suivent sont extraites de la r ef erence [4] et adapt ees aux notations de ce document.
B.1
Compl ement: caract erisation des signaux al eatoires discrets
On etend ici au cas discret les notions introduites au chapitre 1. Soit w(n) une suite de variables al eatoires. Esp erance math ematique (moyenne) : m(n) = E [w(n)]. Fonction dautocorr elation : ww (n, k ) = E [w(n)wT (n + k )]. Stationnarit e` a lordre 2 : m(n) = m;
2 Variance : w = ww (k )|k=0 .
ww (n, k ) = ww (k ) n .
Spectre complexe :
ww (z ) =
k=
ww (k )z k , et inversement :

ww (k ) =
1 1 ww (z )z k1 dz = 2j cercle unit 2 e
ww (ej )ejk d .
Densit e spectrale de puissance : cette fonction de la pulsation suppose que lon introduise une echelle de temps. On introduit donc la p eriode d echantillonnage Introduction au ltre de Kalman
Page 63/74
64
`me line aire B. Passage dun bruit dans un syste dt entre les ev enements
1
w(n). ww ( ) = ww (z )|z=ejdt
On a alors : dt ww (k ) = 2
/dt /dt
ww ( )e
j dt k
et
2 w
dt = 2
/dt
ww ( )d .
/dt
La variance est ` a dt/2/ pr es, lint egrale de la densit e spectrale de puissance entre /dt et /dt.
B.2
B.2.1
Approche temporelle
Cas continu
t) = Ax(t) + M w(t) . x( (B.1)
Th eor` eme 1.1 (rappel de la page 21). Soit le syst` eme lin eaire continu:
w(t) est un bruit blanc gaussien centr e de densit e spectrale de puissance W . On note m(t0 ) et P (t0 ) la moyenne et la covariance de l etat initial x(t0 ) (lui aussi al eatoire gaussien mais ind ependant de w(t)). On montre que x(t) est un signal al eatoire gaussien : de moyenne: m(t) = E [x(t)] = eA(tt0 ) m(t0 ) de covariance P (t) = E [(x(t)m(t))(x(t)m(t))T ] v eriant l equation di erentielle de Lyapunov : t) = AP (t) + P (t)AT + M W M T . P( (B.2) Si le syst` eme est stable (toutes les valeurs propres de A sont ` a partie r eelle n egative) on tend vers un r egime permanent: P = 0 et P (t) = P v erie alors l equation de Lyapunov continue : AP + P AT + M W M T = 0 . (B.3)
D emonstration: lint egration de l equation (B.1) entre linstant initial t0 et le linstant courant t s ecrit : x(t) = eA(tt0 ) x(t0 ) +
t t0
eA(t ) M w( )d
x(t) est donc une combinaison lin eaire de signaux al eatoires gaussiens (x(t0 ) et w( )), x(t) est donc egalement un signal al eatoire gaussien. Calculons sa moyenne m(t) = E (x(t)] et sa matrice de covariance P (t) = E [(x(t) m(t))(x(t) m(t))T ].
1
On note = dt la pulsation r eduite. Page 64/74
B.2 Approche temporelle Moyenne m(t): m(t) = eA(tt0 ) E [x(t0 )] +

t t0
65
eA(t ) M E [w( )]d ,
or E [w( )] = 0 (bruit centr e) et E [x(t0 )] = m(t0 ) donc: m(t) = eA(tt0 ) m(t0 ) . Covariance P (t):
x(t) m(t) = eA(tt0 ) x(t0 ) m(t0 ) + = eA(tt0 ) x(t0 ) m(t0 ) +

T
t t0 t t0
eA(t ) M w( )d (B.4)
T
eA(t0 ) M w( )d .
x(t) m(t)
t
x(t) m(t)
= eA(tt0 )
T
x(t0 ) m(t0 ) d + d + d du eA
T (tt 0)
x(t0 ) m(t0 )
+
t0 t
eA(t0 ) M w( ) x(t0 ) m(t0 ) x(t0 ) m(t0 ) wT ( )M T eA

t
+
t0
T (t ) 0
+
t0
eA(t0 ) M w( )wT (u)M T eA

t t0
T (t
0 u)
(B.5)
P (t) = eA(tt0 ) P (t0 ) +

t
eA(t0 ) M E w( ) x(t0 ) m(t0 )

T (t 0 )
d +
+
t0
E
t
x(t0 ) m(t0 ) wT ( ) M T eA
d + d du eA
T (tt ) 0
+
t0
eA(t0 ) M E w( )wT (u) M T eA
T (t
0 u)
(B.6)
Du fait des hypoth` eses (x(t0 ) et w( ) sont ind ependants > t0 et w(t) est un bruit blanc centr e), on peut armer: E w( ) x(t0 ) m(t0 )
T
=0
E w( )wT (u) = W ( u). Introduction au ltre de Kalman

Page 65/74
66 On a donc:
`me line aire B. Passage dun bruit dans un syste
P (t) = eA(tt0 ) P (t0 ) + et t) = P(
t t0
eA(t0 ) M W M T eA
T (t
0 )
d eA
T (tt
0)
(B.7)
dP (t) T T = AeA(tt0 ) P (t0 ) + eA (tt0 ) + eA(tt0 ) P (t0 ) + eA (tt0 ) AT dt

T (t 0 t)
+eA(tt0 ) eA(t0 t) M W M T eA Soit:
eA
T (tt
0)
(B.8)
t) = AP (t) + P (t)AT + M W M T . P(
(B.9)
Remarque B.1 En r egime permanent, la solution g en erale de l equation T T AP + P A + M W M = 0 s ecrit :
P =
0
eAt M W M T eA t dt .
En eet: AP + P AT =
0
AeAt M W M T eA
Tt
+ eAt M W M T eA t AT dt
=
0 At
d[eAt M W M T eA t ] dt dt
T
T = [e M W M T eA t ] 0 = 0 MW M
(ssi A est stable) .
Si lon note h(t) = eAt B, on peut alors ecrire :
t 0 la r eponse impulsionnelle des etats x du syst` eme,
P =
0
h(t)W hT (t)dt =
1 = [L II xx (s)]s=0
1 H (j )W H T (j )d 2 avec: xx (s) = H (s)W H T (s) .
( egalit e de Parseval)
Ces derni` eres egalit es permettent de faire le lien avec lapproche fr equentielle qui suit (th eor` eme 1.2) et de montrer que la variance est egale (` a 2 pr` es) ` a lint egrale du carr e de la r eponse fr equentielle du bruit.
B.2.2
Cas discret
Th eor` eme B.1 Soit le syst` eme lin eaire discret: x(k + 1) = Ad x(k ) + Md wd (k ) . Introduction au ltre de Kalman
Page 66/74
(B.10)
B.2 Approche temporelle
67
wd (k ) est un bruit pseudo-blanc gaussien centr e de densit e spectrale Wd (cest-` a-dire T E [wd (k )wd (k + j ) ] = Wd (j )) . On note m(0) et P (0) la moyenne et la covariance de l etat initial x(k0 ) = x(0) (lui aussi al eatoire gaussien mais ind ependant de wd (k )). On montre que x(k ) est un signal al eatoire gaussien : de moyenne:
k0 m(k ) = E [x(k )] = Ak m(0) d
de covariance P (k ) = E [(x(k ) m(k ))(x(k ) m(k ))T ] v eriant l equation r ecurrente de Lyapunov :
T P (k + 1) = Ad P (k )AT d + Md Wd Md .
(B.11)
Si le syst` eme est stable (toutes les valeurs propres de Ad sont de module inf erieur ` a 1) on tend vers un r egime permanent: P (k + 1) = P (k ) = P v erie alors l equation de Lyapunov discr` ete:
T P = Ad P AT d + Md Wd Md .
(B.12)
D emonstration: (elle est en fait beaucoup plus simple que dans le cas continu). L equation (A.12), qui fait appara tre x(k ) comme une combinaison lin eaire de variables al eatoires gaussiennes, permet de conclure que x(k ) est eectivement une variable al eatoire gaussienne. wd (k ) etant centr e (k ), on peut conclure que E [x(k )] = k k0 Ad m(0). Enn, on peut remarquer que x(k +1) m(k +1) = Ad (x(k ) m(k ))+ Md wd (k ). Lind ependance, ` a tout instant k , des variables centr ees x(k ) m(k ) et wd (k ) T T permet de conclure que P (k + 1) = Ad P (k )Ad + Md Wd Md . Remarque B.2 A partir de l equation B.7 on retrouve l equation de Lyapunov discr` ete dans le cas dun syst` eme continu echantillonn e` a la cadence dt. Il sut de prendre t0 = n dt et t = (n + 1) dt o` u dt d esigne la p eriode d echantillonnage. On note alors P (t) = P (n + 1) et P (t0 ) = P (n), on obtient: P (n + 1) = eA dt P (n)eA On note : eAdt = Ad : la matrice dynamique discr` ete, = Wd la matrice de covariance du bruit int egr e sur une p eriode d echantillonnage,
T dt Au e M W M T eA u du 0 T dt
dt
+
0
eAu M W M T eA u du
Page 67/74
68 pour obtenir:
Pn+1 = Ad Pn AT d + Wd (dont le r egime permanent s ecrit: Pn = Ad Pn AT equation d +Wd ). On retrouve bien l (B.11) avec Md = I . On peut v erier que : si A est inversible, Wd v erie l equation de Lyapunov: AWd + Wd AT + M W M T eAdt M W M T eA
T dt
= 0,
(B.13)
Wd dtM W M T si dt est petit par rapport au temps de r eponse du syst` eme. Exemple B.1 (Illustration sous Matlab) On reprend lexercice 1.6 que lon compl` ete sous Matlab par une analyse de la variance du bruit w en temps discret : % D efinition du filtre: G=tf([-1 100],[1 50 10000]) % Calcul dune r ealisation: [A,B,C,D]=ssdata(G); % Calcul de la variance par l equation de Lyapunov continue: P=lyap(A,B*B); var_w=C*P*C % ==> var_w=1/50. % Analyse en discret: dt=0.001; A_d=expm(A*dt); Wd=lyap(A,B*B-A_d*B*B*A_d); %Dans cet exemple: W=I; M=B. Pd=dlyap(A_d,Wd);var_wd=C*Pd*C % ==> on retrouve exactement la variance de w(t): var_w=1/50. % Calcul aproximatif: Wd=dt*B*B Pdp=dlyap(A_d,dt*B*B);var_wdp=C*Pdp*C % ==> cela ne marche pas trop mal. 2
B.3
B.3.1
Approche fr equentielle
Cas continu
Th eor` eme 1.2 (rappel). La sortie y dun syst` eme lin eaire, continu et stable d eni par la matrice de transfert G(s)pq et attaqu e par un signal al eatoire stationnaire Introduction au ltre de Kalman
Page 68/74
quentielle B.3 Approche fre
69
w de spectre complexe ww (s)qq est un signal al eatoire stationnaire de spectre complexe yy (s)pp = G(s)ww (s)GT (s) . D emonstration: sans perte de g en eralit e la d emonstration est faite dans le cas dun syst` eme strictement propre (pas de transmission directe). On note (A, M , C ) la r ealisation du transfert G(s), cest-` a-dire : G(s) = C (sI A)1 M . Par d enition :
yy (s) =
yy ( )e s d =
E y (t)y T (t + ) e s d
(B.14)
Pour calculer y (t) nous allons utiliser la formule (A.2) dans la quelle nous choisirons x(t0 ) = 0 et t0 = car on ne sint eresse quau r egime permanent (r egime stationnaire) :
t
y (t) =
CeA(tu) M w(u)du =
0
CeAv M w(tv )dv
(changement de variable: t u = v ) .
yy (s) =
+
E
0
CeAv M w(t v )dv

0
wT (t + u)M T eA u C T du e s d
T
0 0
=
+
CeAv M E w(t v )wT (t + u) M T eA u C T du dv e s d CeAv M ev s ww ( + v u)e( +vu)s M T eA u C T eus du dv d

+
T
=
0
Ce M e
Av
vs
dv
T
ww ( + v u)e
( +v u)s
d
0
M T eA u C T eus du
= G(s)ww (s)G (s) dapr` es (A.8) et (B.14) .
B.3.2
Cas discret
Th eor` eme B.2 (Passage dun bruit dans un syst` eme lin eaire discret) La sortie y dun syst` eme lin eaire, discret et stable d eni par la matrice de transfert G(z )pq et attaqu e par un signal al eatoire w stationnaire de spectre ww (z )qq est un signal al eatoire stationnaire de spectre yy (z )pp = G(z 1 )ww (z )GT (z ) . D emonstration: sans perte de g en eralit e la d emonstration est faite dans le cas dun syst` eme strictement propre (pas de transmission directe). On note (Ad , Md , Cd ) la r ealisation du transfert G(z ), cest-` a-dire : G(s) = Cd (zI Ad )1 Md . Introduction au ltre de Kalman
Page 69/74
70 Par d enition :
yy (z ) =
i=
yy (i)z i =
i=
E y (k )y T (k + i) z i
(B.15)
Pour calculer y (k ) nous allons utiliser la formule (A.12) dans la quelle nous choisirons x0 = 0 et k0 = car on ne sint eresse quau r egime permanent (r egime stationnaire) :
k1 + k0 Cd Aj Md w(k 1 j + k0 ) d j =
y (k ) =
=
j =0
Cd Aj d Md w (k 1 j ) (j j k0 ) .
yy (z ) =
i= +
E
j =0
Cd Aj d Md w (k 1 j )
l=0 + +
T T T z i wT (k + i 1 l)Md Ad Cd
=
i= j =0 l=0 + + +
T T T T Cd Aj d Md E w (k 1 j )w (k + i 1 l ) Md Ad Cd
z i
=
i= j =0 l=0 +
j +1 T T T l1 Cd Aj ww (i l + j )z (il+j ) Md Ad Cd z d Md z + 1 j Cd Aj d Md z j =1 k= T +
ww (k )z
k l=1
T T Md Ad
l1
T l Cd z
(k = i l + j )
= G(z 1 )ww (z )G (z ) dapr` es (A.14) et (B.15) .
Page 70/74
71
Appendix C Code source Matlab des chiers de d emonstration

C.1 Fonction Kf t.m
function y=Kf_t(u) % y=Kf_t(u) % input: % * u(1): time (t), % * u(2:length(u)): innovation (y(t)-yhat(t)) % output: % * y = Kf(t)*(y(t)-yhat(t)) global a c W P0 % Compute P(t): Pinf=a+sqrt(a^2+W); k=2*sqrt(a^2+W); P=Pinf+k*(P0-Pinf)./(exp(k*u(1))*(P0-Pinf+k)+Pinf-P0); % Compute Kf(t): Kf=P*c; % Output y=Kf*u(2:length(u));
C.2
Fichier demoKalman.m
% Global Variable declaration Introduction au ltre de Kalman

Page 71/74
72
monstration C. Code source Matlab des fichiers de de
global a c W P0 % Kalman model data: a=-1; % state space date b=1; c=1; W=1; % Process noise spectral density V=1; P0=1; % Initial estimation error variance X0=20; % Initial condition for the process output % Simulation data dt=0.01; % Integration sampling period T=0; % Time delay in the validation model % Start simulation: sim(simuKalman); % Output plots: figure plot(output.time,output.signals.values(:,1),g-) hold on plot(output.time,output.signals.values(:,2),k-,LineWidth,2) plot(output.time,output.signals.values(:,3),k-) plot(output.time,output.signals.values(:,4),r-,LineWidth,2) % Compute state estimation error variance as a function of time: t=output.time; Pinf=a+sqrt(a^2+W); k=2*sqrt(a^2+W); Pdet=Pinf+k*(P0-Pinf)./(exp(k*t)*(P0-Pinf+k)+Pinf-P0); % plot estimation+2*sigma: plot(output.time,output.signals.values(:,4)+2*sqrt(Pdet),r-) % plot estimation-2*sigma: plot(output.time,output.signals.values(:,4)-2*sqrt(Pdet),r-) legend(y(t),x(t),u(t),xhat(t),... xhat(t)+2\sigma,xhat(t)-2 \sigma) return % Solution with a 2nd order Pade approximation of a 1 second delay % taken into account in the Kalman filter: T=1; sys=ss(a,b,c,0); [num,den]=pade(T,2); ret=tf(num,den); sysret=sys*ret; Introduction au ltre de Kalman
Page 72/74
C.3 Fichier demoKalmand.m [a,b,c,d]=ssdata(sysret); W=1; [Kf,P]=lqe(a,b,c,W,1); % Output estimation error variance: var=c*P*c; % Start simulation only in steady state behavior: X0=0; sim(simuKalman_p); % same as simuKalman.mdl but % the matlab Function Kf_t is % replaced by a static Gain % Output plots: figure plot(output.time,output.signals.values(:,1),g-) hold on plot(output.time,output.signals.values(:,2),k-,LineWidth,2) plot(output.time,output.signals.values(:,3),k-) plot(output.time,output.signals.values(:,4),r-,LineWidth,2) % plot estimation+2*sigma: plot(output.time,output.signals.values(:,4)+2*sqrt(var),r-) % plot estimation-2*sigma: plot(output.time,output.signals.values(:,4)-2*sqrt(var),r-) legend(y(t),x(t),u(t),xhat(t),... xhat(t)+2\sigma,xhat(t)-2 \sigma)
73
C.3
Fichier demoKalmand.m
% Continuous-time model data: A=[0 -1;0 0]; B=[1;0]; M=[0;1]; C=[1 0]; D=0; sysc=ss(A,B,C,D); V=1; q=1; W=q^2; % Continuous-time Kalman filter: [Kf,P]=lqe(A,M,C,W,V) dt=0.01; % Compute Discrete-time model: Introduction au ltre de Kalman
Page 73/74
74
monstration C. Code source Matlab des fichiers de de
sysd=c2d(sysc,dt,zoh); [A_d,B_d,C_d,D]=ssdata(sysd) Vd=V/dt Wd=[1/3*q^2*dt^3 -1/2*q^2*dt^2;-1/2*q^2*dt^2 q^2*dt] % Wd can not % be computed by a Lyapunov equation (equation B.13) since A % is not invertible !! (see how Wd can be computed in LQED) % Discrete-time Kalman filter using dlqe: [Kfd1,Pd1]=dlqe(A_d,eye(2),C_d,Wd,Vd) % Discrete-time Kalman filter using lqed (from continuous-time data): [Kfd2,Pd2]=lqed(A,M,C,W,V,dt) %==> same solution !
Page 74/74

Kalman

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Kalman

Uploaded by

Copyright:

Available Formats

Introduction au ltre de Kalman

Notes de cours Exercices corrig es Sessions Matlab

Octobre 2006 - version 0.2

PAGE SANS TEXTE

3 A propos des unit es physiques R ef erences

Introduction au ltre de Kalman

Introduction au ltre de Kalman

Introduction au ltre de Kalman

Chapter 1 Signaux al eatoires et syst` emes lin eaires

atoires et syste `mes line aires 1. Signaux ale

Figure 1.1: Mod` ele 1/4 de v ehicule.

(1.2) On obtient donc un mod` ele du type de (1.1) avec n = 4, m = 2, q = 1 et p = 1. 2

Introduction au ltre de Kalman

finitions 1.2 Rappels et de

limx+ F (x) = 1; limx F (x) = 0.

s d esigne la variable de Laplace. Page 9/74

Introduction au ltre de Kalman

le moment centr e dordre k :

Rappel: varx = E [X 2 ] E [X ]2 . Page 10/74

Introduction au ltre de Kalman

finitions 1.2 Rappels et de

Figure 1.2: Fonction de r epartition et densit e de probabilit e de la loi uniforme.

Exemple 1.3 La variable al eatoire X correspondante au lanc e dun d e: 1 P (X = i) = , i = 1, , 6 ; 6 E [X ] = 21 ; 6 varx = 35 . 12 2

Caract erisation dune variable al eatoire ` a plusieurs dimensions

atoires et syste `mes line aires 1. Signaux ale

(xi E [Xi ])(xj E [Xj ]) dF (xi , xj ) .

finitions 1.2 Rappels et de X1 X2

Par d enition: Y = (1/2 1/2)

E [Y ] = (1/2 vary = (1/2 1/2)E X1 X2 (X1

P (X1 < 2y x2 )p(x2 )dx2 .

En utilisant les probabilit es conditionnelles, on peut egalement ecrire: F (y ) =

0 p(x2 |x2 > 2y + 1)dx2 +

2y x2 + 1 p(x2 |2y 1 < x2 2y + 1)dx2 2

1 p(x2 |x2 2y 1)dx2 .

Introduction au ltre de Kalman

atoires et syste `mes line aires 1. Signaux ale 2y x2 + 1 dx2 + 4

Figure 1.3: Fonction de r epartition et densit e de probabilit e de Y .

Signal al eatoire (processus stochastique)

Moments dun signal al eatoire

finitions 1.2 Rappels et de

Figure 1.4: R eponse temporelle dune r ealisation du signal b(t).

ou covariance sil sagit dun signal al eatoire vectoriel.

Introduction au ltre de Kalman

ements de solution: El m(t) = E [b(t)] = E [X ] = 0, t ,

Figure 1.5: Fonction dautocorr elation du signal b(t).

Figure 1.6: Fonction dautocorr elation du signal b (t).

finitions 1.2 Rappels et de

2 (dt + ) / dt < < 0 . dt

ww (s) = LII [ww ( )] =

Densit e spectrale de puissance (DSP): ww ( ) = ww (s)|s=j . Remarque 1.2 :

La transform ee de Laplace bilat erale inverse s ecrit :

Introduction au ltre de Kalman

atoires et syste `mes line aires 1. Signaux ale 2

Remarque 1.3 : A partir de la DSP on peut alors ecrire : 1 ww ( ) = 2

(int egration par parties)

La densit e spectrale de puissance est donc : b b ( ) = 2 2 2 4 2 dt j dt j dt e + e 2 = (1 cos ( dt )) = sin2 ( ) 2 2 2 dt dt dt 2 b b ( ) = 2 dt sin2 ( 2dt )

finitions 1.2 Rappels et de

Figure 1.7: Densit e spectrale de puissance du signal b (t).

Figure 1.8: Densit e spectrale de puissance du signal b (t) ( echelles logarithmiques).

Introduction au ltre de Kalman

atoires et syste `mes line aires 1. Signaux ale 2

Passage dun signal al eatoire dans un syst` eme lin eaire

Introduction au ltre de Kalman

Approche fr equentielle (spectrale)