Filtrage de Kalman

Le ltrage de Kalman peut tre vu comme une e estimation rcursive de lerreur quadratique moyenne e minimale (EQMM).

INRS-EMT J. Benesty

Plan

Introduction Variables dtat et mod`le e e Formulation du probl`me du ltrage de Kalman e Drivation de lalgorithme en deux tapes e e Gain de Kalman et initialisation Filtrage de Kalman discret

INRS-EMT J. Benesty

Introduction

Supposons que nous avons: une observation: y(n) = x(n) + v(n); v(n) est un bruit blanc, un mod`le: x(n) = ax(n 1) + w(n); w(n) est un e bruit blanc. Comment estimer x(n) ` partir de lobservation y(n)? a Un estimateur peut tre le suivant: e x(n) = ax(n 1) + k[y(n) ax(n 1)]. (1)

On peut se poser la question: quelle valeur de k minimise lerreur quadratique moyenne? Dans ce qui suit, on va augmenter lordre du mod`le: e
P

x(n) =
p=1

apx(n p) + w(n).

(2)

INRS-EMT J. Benesty

Variables dtat et mod`le e e

Soit x(n) ltat du syst`me ` linstant n: e e a x(n) = x(n) x(n 1) x(n P + 1)

T

Ltat contient toutes les informations sur le syst`me e e pour gnrer la prochaine valeur, x(n+1), tant donne e e e e lentre w(n + 1). e On peut rcrire le mod`le: ee e 1 0 x(n) = Ax(n 1) + . w(n), . 0 et lobservation: y(n) =
INRS-EMT J. Benesty

(3)

1 0

x(n) + v(n),

(4)
3

o`: u A= a1 a2 1 0 0 1 . . . . 0 0 ... aP 1 aP 0 0 0 0 . . . . 1 0 . (5)

Une mani`re plus traditionnelle dexprimer e quations prcdentes est la suivante: e e e x(n + 1) = Ax(n) + w(n + 1), y(n) = Cx(n) + v(n), o`: u A (P P ) est la matrice transition dtat, e

les

(6) (7)

w(n) (P 1) est le vecteur processus bruit, C (N P ) est une matrice de mesures, v(n) (N 1) est un vecteur du bruit mesur. e On supposera que: w(n) est un bruit blanc de moyenne nulle avec E{w(n)wT (k)} = Rw (n)(n k),
INRS-EMT J. Benesty 4

 v(n) est un bruit blanc de moyenne nulle avec E{v(n)vT (k)} = Rv (n)(n k), et w(n) et v(n) sont dcorrls, c`d E{v(n)wT (k)} = e ee a 0N P . Etant donns les vecteurs dobservations y(i) pour e i = 1, 2, , n, le ltrage de Kalman consiste ` trouver a un estimateur linaire quadratique moyen minimal pour e x(n). On verra que lestimateur optimal du vecteur dtat e x(n), utilisant toutes les mesures jusqu` linstant n, a a la forme: x(n) = Ax(n 1) + K(n)[y(n) CAx(n 1)], o` K(n) est la matrice gain de Kalman. u (8)

INRS-EMT J. Benesty

Formulation du probl`me du ltrage de e Kalman

On suppose que les quantits A, C, Rw (n), et Rv (n) e sont connues. Soit x(n|i) lestimateur linaire quadratique moyen e de x(n) ` linstant n tant donnes les observations a e e y(1), y(2), , y(i). Soit e(n|i) lerreur destimation sur ltat: e e(n|i) = x(n) x(n|i), dont la matrice de covariance est: P(n|i) = E{e(n|i)eT (n|i)}. Le crit`re que lon minimisera est: e J(n) = tr[P(n|n)]. (11) (10) (9)

INRS-EMT J. Benesty

Dnition du probl`me: e e Etant donne x(0|0), lestimation de ltat ` n = 0, e e a et la matrice de covariance de lerreur de cette estimation, P(0|0), quand y(1) est disponible, on dsire mettre ` jour x(0|0) pour trouver x(1|1) qui e a minimise: J(1) = E{eT (1|1)e(1|1)} = tr[P(1|1)]
P 1

=
p=0

E{e2 (1|1)}. p

(12)

Une fois x(1|1) et P(1|1) trouvs, on recommence e le processus pour la nouvelle observation y(2). Solution: La solution se dduit en deux tapes: e e 1. Etant donn le vecteur x(n 1|n 1), lestimation e optimale non-biaise de x(n 1), on trouvera e lestimation optimale non-biaise, x(n|n 1), de x(n) e sans lobservation y(n). 2. Ensuite, avec y(n) et x(n|n 1), on estimera x(n), c`d on trouvera x(n|n). a
INRS-EMT J. Benesty 7

Drivation de lalgorithme en deux e tapes e

Etape 1: Comme aucune nouvelle observation nest utilise dans lestimation de x(n), on doit prdire x(n) e e ` partir de x(n 1|n 1) en utilisant lquation dtat: a e e x(n) = Ax(n 1) + w(n). Le prdicteur est: e x(n|n 1) = Ax(n 1|n 1). Lerreur destimation est: e(n|n 1) = x(n) x(n|n 1) = Ae(n 1|n 1) + w(n). (15) (16) (14) (13)

= Ax(n 1) + w(n) Ax(n 1|n 1)

Comme w(n) est de moyenne nulle, si x(n 1|n 1) est non-biais, alors e E{e(n 1|n 1)} = E{e(n|n 1)} = 0P 1. (17)
INRS-EMT J. Benesty 8

Avec e(n|n 1) = Ae(n 1|n 1) + w(n), (18)

et comme e(n 1|n 1) est dcorrl avec w(n), alors e ee P(n|n 1) = AP(n 1|n 1)AT + Rw (n). (19)

Cest lquation de Ricatti qui compl`te cette premi`re e e e tape. e Etape 2: A prsent, on introduit la nouvelle e observation y(n) pour trouver x(n|n). Un estimateur linaire qui est bas sur x(n|n 1) et y(n) a la forme: e e x(n|n) = K (n)x(n|n 1) + K(n)y(n). (20)

On exige que x(n|n) soit non-biais, E{e(n|n)} = e 0P 1, et minimise lerreur quadratique moyenne J(n) = E{eT (n|n)e(n|n)}. On peut crire e(n|n) en fonction de e(n|n1) comme e
INRS-EMT J. Benesty 9

suit: e(n|n) = x(n) x(n|n) = x(n) K (n)[x(n) e(n|n 1)] K(n)[Cx(n) + v(n)] = [I K (n) K(n)C]x(n) + K (n)e(n|n 1) K(n)v(n). Comme E{v(n)} = 0N 1 et E{e(n|n 1)} = 0P 1, pour que x(n|n) soit non-biais if faut que: e I K (n) K(n)C = 0P P , do` u K (n) = I K(n)C. (23) (22) (21)

= x(n) K (n)x(n|n 1) K(n)y(n)

Avec la contrainte prcdente, on en dduit: e e e x(n|n) = K (n)x(n|n 1) + K(n)y(n) (24)

= x(n|n 1) + K(n)[y(n) Cx(n|n 1)].

INRS-EMT J. Benesty 10

Lerreur est: e(n|n) = [I K(n)C]e(n|n 1) K(n)v(n). (25) Comme v(n) est dcorrl avec w(n) et avec y(n 1), e ee alors v(n) sera dcorrl avec x(n) et x(n|n 1). Avec e ee e(n|n 1) = x(n) x(n|n 1), il sen suit que v(n) sera dcorrl avec e(n|n 1), e ee E{e(n|n 1)vT (n)} = 0P N . (26)

Do` la matrice de covariance de lerreur destimation: u P(n|n) = E{e(n|n)eT (n|n)} = [I K(n)C]P(n|n 1)[I K(n)C]T + K(n)Rv (n)KT (n). (27)

Il ne nous reste plus qu` dterminer la matrice gain a e de Kalman K(n) qui minimise lerreur quadratique moyenne: J(n) = tr[P(n|n)]. (28)

INRS-EMT J. Benesty

11

Gain de Kalman et initialisation

La seule quantit qui reste ` trouver est le gain de e a Kalman qui minimise lerreur quadratique moyenne: J(n) = tr[P(n|n)]. (29)

Nous allons utiliser les formules de direntiations e suivantes: tr[KA] = AT , K (30) (31)

tr[KAKT ] = 2KA. K

En direntiant J(n) par rapport ` K(n), on obtient: e a J(n) K(n) = 2[I K(n)C]P(n|n 1)CT + 2K(n)Rv (n), (32)

et en galant ` zro, on trouve le gain de Kalman: e a e K(n) = P(n|n 1)C

INRS-EMT J. Benesty

CP(n|n 1)C + Rv (n)

. (33)
12

On peut simplier lexpression de la matrice de covariance de lerreur destimation: P(n|n) = [I K(n)C]P(n|n 1) [I K(n)C]P(n|n 1)CT (34)

+ K(n)Rv (n)} KT (n).

Remarquer que le second terme est gal ` zro, ainsi: e a e P(n|n) = [I K(n)C]P(n|n 1). (35)

Initialisation: Lalgorithme est initialis de la mani`re e e suivante: x(0|0) = E{x(0)}, P(0|0) = E{x(0)xT (0)}. (36) (37)

Avec ce choix, x(0|0) est un estimateur non-biais e de x(0) et on sassure que x(n|n) sera toujours un estimateur non-biais quel que soit n. e

INRS-EMT J. Benesty

13

Filtrage de Kalman discret

Table 1: Filtrage de Kalman discret.

Equation dtat: e x(n + 1) = Ax(n) + w(n + 1) Equation dobservation: y(n) = Cx(n) + v(n) Initialisation: x(0|0) = E{x(0)} P(0|0) = E{x(0)x (0)} Pour n = 1, 2, calculer x(n|n 1) = Ax(n 1|n 1) P(n|n 1) = AP(n 1|n 1)A + Rw (n) K(n) = P(n|n 1)C
T T 1 T

CP(n|n 1)C + Rv (n)

x(n|n) = x(n|n 1) + K(n)[y(n) Cx(n|n 1)] P(n|n) = [I K(n)C]P(n|n 1)

INRS-EMT J. Benesty

14