Mekki Ksouri 1

Filtre de Kalman
Mekki Ksouri 2
Rappel sur les variables
alatoires gaussiennes (1)
Soit vecteur alatoire X de dimension n, ayant
une moyenne et une matrice de
covariance
On dfinit sa fonction caractristique par:
X est distribu suivant une loi gaussienne si
sa fonction caractristique est
| | X E m =
( )( ) | |
T
m X m X E P =
|
.
|

\
|
= Pu u
2
1
m ju exp ) u (
T T

| |
X ju
t
e E ) u ( =
Mekki Ksouri 3
Rappel sur les variables
alatoires gaussiennes (2)
1. La probabilit multidimensionnelle p(X) sobtient par
transformation de Fourier de la fonction caractristique
2. p(X) nexiste que si la matrice de covariance P est
inversible
( )
( ) ( )
|
.
|

\
|
=

m X P m X
P
X p
T
n
1
2 /
2
1
exp
) det( 2
1
) (

Mekki Ksouri 4
Rappel sur les variables alatoires gaussiennes (3)
Variables alatoires gaussiennes
non corrles
Si les v. a., x
1
, x
2
, x
n
, composantes de X ne sont pas
corrles entre elles, P est diagonale. Il en est de mme
pour son inverse (si elle existe).
|
|
|
|
|
.
|

\
|
=
n
2
1
... 0 0
0 ... ...
0 ... 0
0 ... 0
P

O
( ) ( )
|
.
|

\
|
=

2 2
2
1
exp ) (
i i i i
u m u j u
) ( ) ( ) ( ) (
2 1 n
x p x p x p x p L =
) ( ) ( ) ( ) (
2 1 n
u u u u L =
Si P
-1
existe
Le non corrlation de v. a. gaussiennes
entrane leur indpendance (si P
-1
existe)
Mekki Ksouri 5
Rappel sur les variables alatoires gaussiennes (4)
Transformation de v. a. g.
Si X est un vecteur alatoire gaussien, de
moyenne m et de matrice de covariance P,
le vecteur alatoire Z dfini par la relation :
Z = AX + b
est aussi gaussien caractris par :
| | b Am Z E + =
| | ( ) | | ( ) | |
T
T
APA Z E Z Z E Z E =
Mekki Ksouri 6
Rappel sur les variables alatoires gaussiennes (5)
Transformation de v. a. g.
( )
| |
Z jv
Z
T
e E v = ) (
( ) ( ) | |
( )
| |
AX jv b jv T
Z
T T
e E e b AX jv E v = + = exp ) (
| |
|
.
|

\
|
= = Pu u m ju e E u
T T X ju
X
T
2
1
exp ) (
( )
|
.
|

\
|
= = v APA v Am jv e v A e v
T T T b jv T
X
b jv
Z
T T
2
1
exp ) (
( ) ( )
|
.
|

\
|
+ = v APA v b Am jv v
T T T
Z
2
1
exp ) (
cqfd
Mekki Ksouri 7
Rappel sur les variables alatoires gaussiennes (6)
Lois conditionnelles pour v. a. g.
Soit le vecteur alatoire J dfini par la
partition,
Si J est gaussien alors X et Y sont aussi
gaussiens
|
|
.
|

\
|
=
Y
X
J
Il suffit dcrire X et Y comme suit et dutiliser le
thorme prcdent:

=
=
J A Y
J A X
2
1
Mekki Ksouri 8
Soit le vecteur alatoire W dfini par la partition,
et tel que
Avec M une matrice choisie de faon que W
1
et W
2
soient non corrls.
Rappel sur les variables alatoires gaussiennes (7)
Lois conditionnelles pour v. a. g.
|
|
.
|

\
|
=
2
1
W
W
W

=
+ =
Y W
MY X W
2
1
Si X et Y sont supposs gaussiens, il vient :
1
=
YY XY
P P M
|
|
.
|

\
|

=

YY
YX YY XY XX
WW
P
P P P P
P
0
0
1
| | | |
1
1
/ W E Y P P Y X E
YY XY
+ =

| | | | | | ( ) Y E Y P P X E Y X E
YY XY
+ =
1
/
Mekki Ksouri 9
Rappel sur les variables alatoires gaussiennes (8)
Lois conditionnelles pour v. a. g.
Proprits qui dcoulent du
thorme prcdent :
1. Lesprance mathmatique conditionnelle du v.a.g. X
connaissant le v.a.g. Y est un v.a.g. combinaison linaire
des composantes de Y.
2. Le vecteur X-E[X/Y] est moyenne nulle et est
indpendant de Y et de toute combinaison linaire des
composantes de Y.
3. Si [X Y
1
Y
2
]
T
est un vecteur a.g. et Y
1
Y
2
sont des
vecteurs indpendants alors :
| | | | | | | | X E Y X E Y X E Y Y X E + =
2 1 2 1
/ / , /
Mekki Ksouri 10
A RETENIR
Si X et Y v. a. g.
Si Y=(Y
1
Y
2
)
T
v. a. g. alors Y
1
Y
2
v. a. g.
Si Z=(X Y
1
Y
2
)
T
v. a. g. et Y
1
Y
2
indpendants
| | | | | | ( ) Y E Y P P X E Y X E
YY XY
+ =
1
/
| | nulle moyenne et Y de t indpendan g. a. v. est Y / X E X
| | | | | | | | X E Y X E Y X E Y Y X E + =
2 1 2 1
/ / , /
Mekki Ksouri 11
Ralisation dobservateur
Filtre de Kalman-Bucy(1)

+ =
+ =
+
k k k k
k k k k k
w x H y
v x x
1
Modle discret :
| |
kj
T
j k
Q v v E =
| |
kj
T
j k
R w w E =
| | 0 =
k
v E | | 0 =
k
w E
v
k
et w
k
bbg tq:
| | j , k 0 w v E
T
j k
=
v
k
et w
k
ne sont pas corrls
( )( ) | |
0 0 0 0 0
P x x E
T
=
| |
0 0
= x E
Ltat initial est distribu
selon une loi gaussienne
| | 0 w x E
T
k 0
=
| | 0 v x E
T
k 0
=
x
0
, v
k
et x
0
, w
k
ne sont pas corrls

k
,
k
et H
k
sont parfaitement connus chaque instant
Mekki Ksouri 12
Ralisation dobservateur
Filtre de Kalman-Bucy (2)
Dterminer le meilleur estimateur de x
k
connaissant la suite de mesures
( )
k k
y y , y Y L
1 0
=
Critre :
( ) ( ) | |
k k
T
k k
x

x S x

x E min
S matrice dfinie positive
Mekki Ksouri 13
Ralisation dobservateur
Filtre de Kalman-Bucy (3)
Lestimateur optimal au sens du critre impos est
lestimateur de la moyenne conditionnelle
On dfinit le prdicteur un pas
On sera amen traiter 2 types de problmes :
Pb de prdiction ou dextrapolation si on cherche x

(k/i) avec i<k

Pb de lissage ou dinterpolation si on cherche x

(k/i) avec i>k

Daprs les hypothses la densit de probabilit p(xk/Yk)
est gaussienne, lestimateur du maximum posteriori est
donc confondu avec celui de la moyenne conditionnelle.
| | | |
k k k 1 0 k
Y / x E y , , y , y / x E ) k / k ( x

= = L
| |
1 k k
Y / x E ) 1 k / k ( x

=
Mekki Ksouri 14
| | ) 1 / (

) ( ) ( ) ( ) 1 / (

) / (

+ = k k x k H k y k K k k x k k x
| |
1
) ( ) 1 / ( ) ( ) ( ) 1 / ( ) (

+ = R k H k k P k H k H k k P k K
T T
2
me
tape : processus dinnovation
Le gain K(k) est donn par :
.
On calcule P(k/k) pour ltape suivante : P(k/k) = [I K(k)H(k)]P(k/k1)
3me tape : on reprend partir de ltape 1
On calcule :
) 1 / ( k k x
) 1 / 1 (

) 1 ( ) 1 / (

= k k x k k k x
) 1 ( ) 1 ( ) 1 ( ) 1 / 1 ( ) 1 ( ) 1 / ( + = k Q k k k k P k k k P
T T
1
re
tape : prdicteur un pas
Dans la notation qui prcde,
reprsente le vecteur dtat estim litration k en tenant compte des mesures
effectues jusqu litration (k1).
On dtermine la matrice de covariance de lerreur de prdiction un pas :
On calcule le prdicteur un pas de ltat :
Filtre de Kalman-Bucy (4)
Mekki Ksouri 15
Filtre de Kalman-Bucy (5)
1 ) 1 / 1 (

) 1 ( ) 1 / (

= k k x k k k x
| | ) 1 / (

) ( ) ( ) ( ) 1 / (

) / (

+ = k k x k H k y k K k k x k k x
| |
1
) ( ) 1 / ( ) ( ) ( ) 1 / ( ) (

+ = R k H k k P k H k H k k P k K
T T
) 1 ( ) 1 ( ) 1 ( ) 1 / 1 ( ) 1 ( ) 1 / ( + = k Q k k k k P k k k P
T T
( ) ( ) 1 k / k P H K I ) k / k ( P
k k
=
5
quation matricielle
de Riccati
Les quations 2, 3 et 5 peuvent tre calcules hors ligne
2
3
4
Mekki Ksouri 16
k / k
x

q
-1

k / 1 k
x

+
1 k / k
x

+
+
+

Processus dinnovation
+
+

q
-1

x
k+1
x
k y
k
+
+
+
+
w
k
Filtre de Kalman-Bucy (6)
v
k

MATLAB
Mekki Ksouri 17
Filtre de Kalman tendu
Cas de systme non linaire
Hypothse : on dispose dune solution
X
1
(t) de lquation dterministe :
On pose :
( )
( ) ) t ( w t , x h ) t ( y
) t ( v ) t ( u t , X f ) t ( X
+ =
+ + =
&
( ) ) t ( u t , X f ) t ( X
1 1
+ =
&
) t ( X ) t ( X ) t ( X
1
=
Mekki Ksouri 18
Filtre de Kalman tendu (2)
( ) ( ) ) t ( v t , X f t , X f ) t ( X
1
+ =
&

w X
X
h
w ) t , X ( h ) t , X ( h y y ) t ( y
1
X
1 1
+
|
.
|

\
|

= + = =
On dduit
Si X1 voisin de X, on peut crire :
Do :
On fait de mme pour lquation
de sortie
On obtient une EE linaire:
Filtre de Kalman
( ) ( ) X
x
f
t , X f t , X f
1
X
1

|
.
|

\
|

+ =
) t ( v X
x
f
X
1
X
+
|
.
|

\
|

=
&
w X H y
v X F X
+ =
+ =


&
Mekki Ksouri 19
Filtre de Kalman tendu (3)
Problme du choix de la solution X
1
Deux solutions:
Si les variances du bruit dtat et de la
condition initiale sont faibles, on prend
X
1
(t) solution de :
Sinon on utilise une structure du type
adaptative, la solution X
1
est rajuste
chaque pas de calcul
( ) ( ) | |
0 0 1 1 1
t X E ) t ( X ) t ( u t , X f ) t ( X = + = avec
&
Mekki Ksouri 20
Filtre de Kalman tendu (4)
structure adaptative
( ) ( ) ( ) | |
( ) ( ) ( )
( )
( )
( ) estime nouvelle de pas a n' on qu' ant
pour t

avec
1 - k
t ) 1 k / 1 k ( X

1 k / t X

t t 0 ) 1 k / t ( X

) 1 k ( X 1 k / t X

0 ) 1 k / 1 k ( X

) 1 k ( X 1 k / 1 k X

1 k / 1 k X

k / k X

k / k X

1 k / k X

) k ( H ) k ( y K 1 k / k X

k / k X

k 1
1
k
=
= =
= =
=
+ =

k-1, on pose :
Ce qui permet destimer la rfrence
linstant k partir des quations :
( ) 1 k / 1 k X

) 1 k ( X
1
=
Mekki Ksouri 21
Filtre de Kalman tendu (5)
structure adaptative. Algorithme
( ) 1 k / 1 k X

) 1 k ( X
1
=
( ) ( ) ( )
| |
1
T T
k
k
T
) k ( R ) k ( H ) 1 k / k ( P ) k ( H ) k ( H ) 1 k / k ( P K
) 1 k / k ( P ) k ( H K ) 1 k / k ( P ) k / k ( P
) t ( Q ) 1 k ( F 1 k / t P 1 k / t P ) 1 k ( F 1 k / t P

+ =
=
+ + =
&
Les quations du filtre :
Les matrices de covariance et gain sont
donnes par:
( )
( ) | | k ), 1 k / k ( X

h ) k ( y K ) 1 k / k ( X

) k / k ( X

) t ( u t ), 1 k / t ( X

f ) t ( X

k
+ =
+ =
&
Mekki Ksouri 22
Application du filtre de Kalman
lidentification de paramtres
Soit le systme, avec a un paramtre
inconnu mais constant :
On pose a(k+1) = a(k)
Do lEE augmente:
On utilise le FKE
( )
( ) ) k ( w a , k , X h ) k ( y
) k ( v a , k , X f ) 1 k ( X
+ =
+ = +
( )
( ) ) k ( w k , Z h ) k ( y
) k ( v
0
1
1 0
0 k , Z f
) 1 k ( Z
+ =
|
.
|

\
|
+ |
.
|

\
|
= +
Mekki Ksouri 23
Aide calcul
|
|
.
|

\
|
YY YX
XY XX
P P
P P
|
|
.
|

\
|
=
Y
X
J
( )( ) | |
T
XX
X E X X E X E P ) ( ) ( =
( )( ) | |
T
YX
T
XY
P Y E Y X E X E P = = ) ( ) (
( )( ) | |
T
YY
Y E Y Y E Y E P ) ( ) ( =

=
=
J A Y
J A X
2
1
( )( ) | | 0 ) ( ) (
2 2 1 1
=
T
W E W W E W E
) ( ) ( ) ( et
) ( ) ( ) (
2 1
2 1
2 1
W p W p W p
u u u
W W W
=
=
Mekki Ksouri 24
Aide calcul
( )( ) | | 0 ) ( ) (
2 2 1 1
=
T
W E W W E W E ( )( ) | | 0 ) ( ) ( = + +
T
Y E Y MY X E MY X E
0 = +
YY XY
MP P

=
=

Y W
Y P P X W
YY XY
2
1
1
( )( ) | |
YX YY XY XX
T
P P P P W E W W E W E
1
1 1 1 1
) ( ) (

=
1
1
W Y P P X
YY XY
+ =

Mekki Ksouri 25
Aide calcul
| | | |
1 1
/ W E W Y X E X =
|
|
|
.
|

\
|
2
1
Y
Y
X
| | | | | | | | X E Y X E Y X E Y Y X E + =
2 1 2 1
/ / , /
( )
2 1
XY XY XY
P P P =
|
|
.
|

\
|
=
2 2
1 1
0
0
Y Y
Y Y
YY
P
P
P
|
|
.
|

\
|
=
2
1
Y
Y
Y
| | | | | | | | ( ) Y E Y P P X E Y X E Y Y X E
YY XY
+ = =
1
2 1
/ , /
| | | | ( )
| |
| |
|
|
.
|

\
|

|
|
.
|

\
|
+ =

2 2
1 1
1
1
2 1
2 21
1 1
2 1
0
0
, /
Y E Y
Y E Y
P
P
P P X E Y Y X E
Y Y
Y Y
XY XY