Kalman

Plan du cours

Le filtre de Kalman

I. Introduction II. Le filtre de Kalman discret : algorithme III. Le filtre de Kalman continu IV.Le filtre de Kalman dans les cas non linaires

Module 5AS2-10 troisime anne ENSICA

Yves Briere ENSICA

1

2

Plan du cours

Prrequis :Automatique ENSICA 1A et 2A

Outils :Matlab / Simulink

I. Introduction

Rfrences : Kalman Filtering : Theory and Practice , Mohinder S. Grewal, Angus P. Andrews, Prentice Hall ed. homepage.ensica.fr/~ybriere/5AS2-103 4

I.0 Un exempleCentrale dattitude : Gyroscope

I.0 Un exempleCentrale dattitude composants lis Strapdown Gyromtres Acclromtres GPS Pices mobiles maintient constant de lattitude Baromtre Calculateur Capteur de T Etc

Calculateur : Fusion de donnes

Pitot

! !

Attitude? Position?

5

6

I.0 Un exempleCentrale dattitude composants lis Strapdown Gyromtre : mesure de la vitesse de rotation Dans le repre local (li au capteur) Drive, biais, sensibilit la temprature Haute bande passante Acclromtre : mesure de lacclration Dans le repre local (li) Drive, biais, sensibilit la temprature Ne distingue pas lacclration inertielle et lacclration de la pesanteur (merci Einstein) Haute bande passante

I.0 Un exempleCentrale dattitude composants lis Strapdown ## %% %% "" $$ $ $ "" "" "" ""

&

''''

$

"

! !

!

A = Ca + v + g + b +11 11

(

Bruits de mesure % !! !! "" "" ! % " " 2 3 ! 2 "

0

)

%

!

!

"" ""

44

"" ""

""

4

% %

GPS : mesure de la position Dans un repre absolu Peu prcis Faible bande passante

" "

"

0

!

! !

%

'

%

!

!

7

8

%

"

!

" "

&

$

' # #

"

"

"

#

"

"

4 4

I.0 Un exemple## ## $$ $

I.0 Un exemple

"

! "" "" " " !

" ! % " % 1 " %

" "

"

"

mes = vrai + b +

%%

(

)

""

" "

!

$$ $ $

%

%

""

"

% 2 "

"

!

"

"" ""

$

3

!

$$ $

% ! %

Bruits de mesure

"

"

""

%%

2

"

Centrale dattitude composants lis Strapdown


!! !!

"" ""

11

9& $ " # " ## ## $$ $

I.0 Un exemple

& &

" "

11

%

!

"

M mes = M vrai + b +

$

)

" "

( ! !

%

En gnral

" "

%

"

2

"

!

3

!

Bruits de mesure

%

I.1 Quest-ce quun filtre de Kalman ?

Cest un estimateur qui permet de reconstituer les tats dun systme perturb en utilisant des mesures. Les mesures peuvent tre : - incompltes, - indirectes, - intermittentes, - bruites

Domaines dapplication : aronautique (centrales inertielles de navigation), spatial, etc12

Une des plus grandes inventions du sicle (bigre)

%


2

"

"

10

I. Introduction I.2 La thorie de lestimation

I.2 La thorie de lestimationHistorique1777-1855 : Carl Friedrich Gauss Mthode des moindres carrs : premire mthode destimation optimale

1894-1964 : Norbert Wiener Filtre de Wiener : utilisation des fonctions de corrlation et de densit spectrale

1960 : Kalman Filtre de Kalman : utilisation de la reprsentation dtat

13

14

I. Introduction I.3 Rappel sur les observateurs

I.3 Rappel sur les observateursRappel sur lobservateur IdentitPermet de reconstituer les tats du systme

Ce que lon connat du systme :

X = A X + Bu y = CX~ ~ X = A X + Bu ~ ~ = CX y ~ ~ X = A X + B u + K (y - ~ ) y ~ ~ = CX y

Observateur en boucle ouverte :

Observateur Identit :

Le rglage de K rduit lerreur y 15

16

I.3 Rappel sur les observateursRglage de lobservateur IdentitCe que lon connat du systme :

I.3 Rappel sur les observateursRglage de lobservateur IdentitMthode 1 : passage dans la base compagne dobservation

X = A X + Bu y = CX~ ~ y X = A X + B u + K (y - ~ ) ~ ~ = CX y

Observateur Identit :

X = A X + Bu y = CXPO = M O

X = MO XO1 2 n

X O = A O X O + BO u y = CO X O

~ ~ X - X = (A - K C ) X - X = (A - K C )

(

)

(

1 T

) = [p , p ,...p ][ ] = [(A ) + a AT 2 n 1 T

p1 = C TAvec :

A - K.C : Dynamique de lobservateur

p 2 = A T + a n 1 I p1 p3 ...

+ a n 2 I p118

]

17

I.3 Rappel sur les observateursRglage de lobservateur Identit a n 1 a n 2 A0 = . . a1 a0 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 B0 = b n 1 b n 2 . . b1 b0

I.3 Rappel sur les observateursRglage de lobservateur Identit a n 1 k n 1 a n 2 k n 2 A 0 - K 0 C0 = . . a1 k1 a0 k0 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0

C 0 = (1

0

0

0

0

0)

Coefficients du polynme caractristique de lobservateur Le rglage de lobservateur consiste placer n ples

y(p ) N(p) b p n 1 + b n 2 p n 2 + ... + b1p + b 0 = = n n 1 u (p ) D(p) p + a n 1p n 1 + a n 2 p n 2 + ... + a1p + a 019

20

I.3 Rappel sur les observateursRglage de lobservateur IdentitMthode 2 : formule dAckerman

I.3 Rappel sur les observateursRglage de lobservateur IdentitMthode 3 : avec matlab

(p ) = p n +

n 1

p n 1 +

n 1

p n 1 + ... + 1p +

0

Polynme caractristique dsir de lobservateur (n ples)

O = CT , A T CT , A

[

( ) C ...(A ) C2 T T n -1 T1

T

]

>> K=place(A,C,poles)

Matrice dobservabilit

K = [0,0,...0,1] O (A )Formule dAckerman ! Trs mauvais si n grand (problmes numriques en multivariable et en mono si n > 6)

!

Mthode numrique optimise

21

22

I.3 Rappel sur les observateursInconvnient de lobservateur IdentitComment placer les ples de manire optimale ?... Comment placer les ples si certains modes sont bruits ?...

X = A X + B u + G w (t ) y = C X + v (t )

II. Le filtre de Kalman discret23 24

II. Le filtre de Kalman discret

II. Le filtre de Kalman discret : quationsHypothsesOn considre un processus linaire dcrit par lquation :

II.1 Equations du filtre de Kalman

x (k + 1) = A x (k ) + B u (k ) + G w (k ) y(k ) = C x (k ) + v(k )

W(k) et v(k) sont des bruits blancs non corrls

E[w (k )] = 0

E w (k ) w (i ) =T

[

]

(k i ) Q(k )

E v(k ) v(i ) =T

E[v(k )] = 0

[

]

(k i ) R (k )

Condition initiale : x(0)

25

E x (0 ) x (0 ) = P0T

E[x (0 )] = x 0

[

]

26

II. Le filtre de Kalman discret : quationsNotationsLtat :

II. Le filtre de Kalman discret : quationsEtape 1 : prdiction de ltatOn utilise le modle du systme pour prdire ltat partir de la prdiction prcdente:

x (k ) x (k + 1 | k ) x (k + 1 | k + 1)

Ltat prdit :

E[x + | k = E x x | x (k (k1+ 1)]= A[A (k (k ) + B u (k ) + G w (k )]On utilise le modle du systme pour prdire la mesure suivante:

Moyenne nulle

Ltat filtr :

[ (( + | ] ) E[C x( ( ) Eyykk+ 11)k == C xkk| |kk ) + v(k )] On value lerreur ( innovation ) :

Moyenne nulle

y(k + 1) y(k + 1 | k )27 28

II. Le filtre de Kalman discret : quationsEtape 2 : filtrage de ltatOn utilise ltat prdit pour obtenir ltat estim :

II. Le filtre de Kalman discret : quationsEtape 2 : estimation de ltatLien entre critre quadratique et matrice de covariance : Critre quadratique :

x (k + 1 | k + 1) = x (k + 1 | k ) + K (k + 1) (y(k + 1) y(k + 1 | k ))K(k+1) est le gain de Kalman Comment choisir K(k+1) pour minimiser l erreur entre tat r el et tat estim ?

J (k + 1) = E (k + 1) (k + 1) = ET

[

] [T

1

(k + 1)2 + ... + n (k + 1)2 ]E[ E[ E[1 1

(k + 1) = x (k + 1) x (k + 1 | k + 1)

Matrice de covariance :

P(k + 1 | k + 1) = E (k + 1) (k + 1) =

[

]

2

] 1] ]

E[ E[

1 2

2

] 2]

.

Erreur au sens quadratique : on cherche

T 2 2 J (k + 1) = E[ (k + 1) (k + 1)] = E[ 1 (k + 1) + ... + n (k + 1) ]

minimiser le crit re :

n 1

. .

. . . . . . . E[ n

n

]

Lien :

Propri t fondamentale : minimiser J(k+1) est quivalent

J (k + 1) = E (k + 1) S (k + 1) , S > 0T

[

]

minimiser J(k+1) avec :

J (k + 1) = trace(P(k + 1 | k + 1))Il est plus simple de travailler sur la matrice de covariance30

29

II. Le filtre de Kalman discret : quationsEtape 2 : estimation de ltatCalcul de la matrice de covariance :

II. Le filtre de Kalman discret : quationsEtape 2 : estimation de ltatCalcul de la matrice de covariance (suite) :

x (k + 1) = A x (k ) + B u (k ) + G w (k ) x (k + 1 | k ) = A x (k | k ) + B u (k ) x (k + 1) x (k + 1 | k ) = A (x (k ) x (k | k )) + G w (k ) P(k + 1 | k ) = E (A (x (k ) x (k | k )) + G w (k )).(A (x (k ) x (k | k )) + G w (k )) ... ... P(k + 1 | k ) = A P(k | k ) A T + G Q G TP(k+1 | k+1) ?31

P(k + 1 | k + 1) = E (x (k + 1) x (k + 1 | k + 1)) (x (k + 1) x (k + 1 | k + 1)) P(k + 1 | k + 1) = [I K (k + 1) C] P(k + 1 | k ) [I K (k + 1) C]T T

[

T

]

................................... + K (k + 1) R K (k + 1)T

[

]

32

II. Le filtre de Kalman discret : quationsEtape 2 : estimation de ltatCalcul du gain de Kalman Rappel : le gain de Kalman doit permettre de minimiser le critre quadratique :

II. Le filtre de Kalman discret : quationsEtape 2 : estimation de ltatCalcul du gain de Kalman (suite)

min (J (k + 1)) = min (trace(P(k + 1 | k + 1)))K K

Autre criture de la relation dvolution de P :

J (k + 1) =0 K (k + 1)

P(k + 1 | k + 1) = [I K (k + 1) C] P(k + 1 | k )

trace K A K T = 2 K A K

(

(

))

(Comme tout le monde sait)

P(k + 1 | k + 1) = P(k + 1 | k ) K (k + 1) C P(k + 1 | k )Cette relation montre que le filtre converge car :

[I K (k + 1) C] P(k + 1 | k ) C T + K (k + 1) R = 0

K (k + 1) = P(k + 1 | k ) C T C P(k + 1 | k ) C T + R(Equation de Riccati)

[

]

K (k + 1) C P(k + 1 | k ) est dfinie positiveOuf

1

33

34

II. Le filtre de Kalman discret : quationsRsum : le filtre de Kalman discret x (k + 1 | k ) = A x (k | k ) + B u (k )Etat estimCovariance du bruit sur ltat Covariance du bruit sur la mesure

II. Le filtre de Kalman discret : quationsConclusion

P(k + 1 | k ) = A P(k | k ) A T + G Q G T

Systme linaire + Bruits blans Gaussiens

K (k + 1) = P(k + 1 | k ) C T C P(k + 1 | k ) C T + RGain de Kalman

Covariance de (tat estim tat rel)

[

]

1

Convergence + Filtre optimal

x (k + 1 | k + 1) = x (k + 1 | k ) + K (k + 1) (y(k + 1) y(k + 1 | k ))Etat filtr Prise en compte de la mesure

P(k + 1 | k + 1) = [I K (k + 1) C] P(k + 1 | k )Covariance de ltat filtr35 36

II. Le filtre de Kalman discret stationnaire

II. Le filtre de Kalman discret stationnaireOn considre un processus linaire dcrit par lquation :

x (k + 1) = A x (k ) + B u (k ) + G w (k )

II.2 Filtre stationnaire

y(k ) = C x (k ) + v(k )

Hypoth ses suppl mentaires : A, B et C constants Bruits blancs stationnaires (A,B) commandable (A,C) observable Le filtre de Kalman tend vers un r gime permanent :

x (k + 1 | k + 1) = A x (k | k ) + B u (k ) K (y(k + 1) C x (k | k )) K = P C T R + C P C T A P A T P A P C T37

[

] [R + C P1

CT

]

1

C P A T + G.Q.G T = 038



A P A T P A P C T R + C P C T

[

]

1

C P A T + G.Q.G T = 0

II.3 Les bruits ne sont pas honntes

Equation alg brique de Riccati discr te On conserve la solution d finie positive L erreur v rifie l quation diff rentielle :

(k + 1) = (A K C A ) (k ) + ...Constante de temps

Fonction Matlab : dare (discrete algebric riccati equation) >> Pinf = dare(A,C,G*Q*G,R)39 40

II. Le filtre de Kalman discret stationnaireLes bruits sont corrls !E w (k ) v(i ) =T

II. Le filtre de Kalman discret stationnaireLes bruits sont corrls dans le temps!Le bruit peut tre modlis comme dcrivant une quation dtat :

[

]

(k i ) QR (k )

Les quations sont

peine chang es :

v(k + 1) = A v v(k ) + w 1 (k )O w1 est maintenant un bruit blanc gaussien. Ouf1

x (k + 1 | k ) = A x (k | k ) + B u (k )

P(k + 1 | k ) = A P(k | k ) A T + G Q G TK (k + 1) = P(k + 1 | k ) C T + QR

(

)

C P(k + 1 | k ) C + R + ...T

On travaille alors sur la nouvelle reprsentation dtat :

...C QR + QR T C T

x (k + 1) v(k + 1)

=

A 0

0 Av x (k ) v(k )

x (k ) v(k )

+

B 0

u (k ) +

G 0 0 I

x (k + 1 | k + 1) = x (k + 1 | k ) + K (k + 1) (y(k + 1) y(k + 1 | k ))P(k + 1 | k + 1) = P(k + 1 | k ) K (k + 1) C P(k + 1 | k ) + QR T

w (k ) w 1 (k )

(

)

y(k ) = [C I]

41

42

II. Le filtre de Kalman discret stationnaireExemple : constante alatoireMon acclromtre est biais, dune valeur biais imprvisible On rajoute un tat reprsentant ce biais :

II. Le filtre de Kalman discret stationnaireExemple : sinusode alatoireJe suis perturb par le 50Hz : amplitude et phase alatoire de y On rajoute un tat x :

biais = x biais (k ) + w 1

x biais (k + 1) = 0

x=

0 02

1 0

x +

0 1

w

y = (0 1) x

43

44

II. Le filtre de Kalman discret stationnaireExemple : marche alatoireProcessus de Weiner ou mouvement Brownien

II. Le filtre de Kalman discret : algorithme

II.3 Algorithme du filtre de Kalman

On rajoute un tat x :

x (k + 1) = w 1 y = x (k )

45

46

II. Le filtre de Kalman discret : algorithmeApothose : lalgorithme = A + Bu x xFiltrage Diminution de P

II. Le filtre de Kalman discret : algorithmeForme de Joseph

P = A P AT + G Q GT

EstimationAugmentation de P

Equation mal conditionne :

P(k + 1 | k + 1) = [I K (k + 1) C] P(k + 1 | k )Forme de Joseph : mieux conditionne

= + K (y ) x x y y = Cx

P(k + 1 | k + 1) = [I K (k + 1) C] P(k + 1 | k ) [I K (k + 1) C] + K (k + 1) R K (k + 1)T

T

K = P CT C P CT + RP = [I K C] P

[

]

1

47

48

II. Le filtre de Kalman discret : algorithmeRduction de la complexit de lalgorithme en dcouplant les mesures

II. Le filtre de Kalman discret : algorithmeAlgorithme avec mesures dcouplesK = P C1 C1 P C1 + R 1T T

[

]

1

Inversions scalaires !

Condition dapplication : Bruits sur les mesures dcoupls : R diagonal Mthode On considre une quation de mesure par grandeur mesure

P = [I K C 1 ] P

x = x + K (y 1 C 1 x )K = P C2 C2 P C2 + R 2T T

[

]

1

P = [I K C 2 ] P

x = x + K (y 2 C 2 x )Etc 49 50

II. Le filtre de Kalman discret : algorithmeMise en uvre de lalgorithmefunction [xest,P]=estimation(u,xest,P) xest=A*xest+B*u P=A*P*A+G*Q*G end function [xest,P]=kalman1(y1,xest,P) K=P*C1*(C1*P*C1+R)^-1 P=P-K*C1*P yest=C1*xest xest=xest+K*(y-yest) end Main P=P0 x=x0 while (1) [xest,P]=estimation(u,xest,P) si (mesure(capteur1)==disponible) alors [xest,P]=kalman1(y,xest,P) end si (mesure(capteur2)==disponible) alors [xest,P]=kalman2(y,xest,P) end end end function [xest,P]=kalman2(y2,xest,P) K=P*C2*(C2*P*C2+R)^-1 P=P-K*C2*P yest=C2*xest xest=xest+K*(y-yest) end

II. Le filtre de Kalman discret : algorithmeMise en uvre de lalgorithme exempleDonnes A C G P0 Q R = = = = = = 1 1 1 50 10 20

x (k + 1) = A x (k ) + B u (k ) + G w (k ) y(k ) = C x (k ) + v(k )1 2 3 4 5

n= estimation P= A.P.A'+GQG kalman K= PC'/(CPC'+R) filtrage P= P-KCP P= 50

6

7

8

9

10

11

12

13

14

60,00 25,00 21,11 20,27 20,07 30,07 40,07 50,07 60,07 25,00 21,11 20,27 30,27 40,27 0,75 0,56 0,51 0,50 0,75 0,56 0,51 0,67 13,36

15,00 11,11 10,27 10,07

15,00 11,11 10,27

15,00 11,11 10,27 10,07 20,07 30,07 40,07 50,07 15,00 11,11 10,27 20,27 30,27 13,36

51

52

II. Le filtre de Kalman discret : algorithmeMise en uvre de lalgorithme RglagesInitialisations tat initial : x0 Covariance initiale : traduire la mconnaissance de x0 par P = a.I avec a grand Choix de Q et R Q traduit la perturbation sur ltat R traduit le bruit de mesure Possibilit de rglage la main

II. Le filtre de Kalman discret : algorithmeMise en uvre de lalgorithme VrificationsVrifications Vrifier chaque calcul que P est dfini positif Si P nest pas dfini positif, il y a un problme quelque part bug informatique matrice(s) mal conditionne(s) Etat non observable Etc. Notion dinnovation v(k) :

v(k + 1) = y(k + 1) C x (k + 1 | k )

Si le filtre est optimal, v(k) est un bruit blanc de valeur moyenne nulle Test de blancheur53 54

II. Le filtre de Kalman discret : algorithmeMise en uvre de lalgorithmeTest de blancheur

II. Le filtre de Kalman discret : algorithmeMise en uvre de lalgorithmeRjection de mesures errones Test de blancheur de linnovation

(i ) = y(i + 1) C x (i + 1 | i ) R (0 ) RN (0) = =1 R (0 ) R (i ) RN (i ) = R (0 )

1 N (k )2 R (0 ) = N k =1 1 N (k ) (k i ) R (i ) = N k =1

On montre que le terme S(k + 1) = C P(k + 1 | k ) C + RT

est la covariance de lerreur sur la prdiction de la mesure y Pour chaque mesure on calcule linnovation :T

Bruit blanc parfait : Test de blancheur :

RN (0) = 1, RN (i ) = 02,17 RN(0 ) = 1, RN (i ) N

On dfini le terme :

2

=

(i ) = y(i + 1) C x (i + 1 | i ) (k + 1) S(k + 1) (k + 1)m

Pour un filtre parfaitement modlis, avec des bruits blancs Gaussiens, alors ce terme a une distribution chi 2 Pour un Intervalle de confiance 3% on limine les mesures correspondant un chi2 suprieur 4,755

Intervalle de confiance 3%

En pratique : rglage empirique du seuil

56

II. Le filtre de Kalman discret : algorithme

II. Le filtre de Kalman discret : a marche pasDivergence ?

II.4 Ca marche pasEt pourtant a marchait en simulation

Problme de modlisation

Variables dtat non modlises Analyser la densit spectrale de linnovation : les dynamiques non modlises apparaissent Bruit sur la dynamique non modlise Amliorer la modlisation du bruit Erreur sur les coefficients dfinissant la dynamique

57

58

II. Le filtre de Kalman discret : a marche pasDivergence ?Problme de mesure

Mesures errones intermittentes, cf. test de blancheur

Problme numrique

III. Le filtre de Kalman continu59 60

Utilisation dalgorithmes volus

III. Le filtre de Kalman continu : Kalman-BucyHypothsesOn considre un processus linaire dcrit par lquation :

III. Le filtre de Kalman continu : Kalman-BucyHypothsesDfinitions Q est la covariance du bruit sur ltat Gnralement non mesurable R est la covariance du bruit sur les mesures

x (t ) = A x (t ) + B u (t ) + G w (t ) y(t ) = C x (t ) + v(t )

W(t) et v(t) sont des bruits blancs non corrls

E w (t1 ) w (t 2 ) =T

E[w (t )] = 0

[

]

(t1 t 2 ) Q(t )

E v(t1 ) v(t 2 ) =T

E[v(t )] = 0

[

]

Gnralement mesurable

(t1 t 2 ) R (t )

P est la covariance de lerreur sur ltat estim Permet de tester la qualit de lestimation

P(t ) = E (x (t ) x (t )) (x (t ) x (t ))61

[

T

]62

III. Le filtre de Kalman continu : Kalman-BucyObjectifObtenir un estim de ltat partir de la connaissance : - du modle - des entres et des sorties

III. Le filtre de Kalman continu : Kalman-BucyEquations du filtre de Kalman-Bucy (continu) x (t ) = A x (t ) + B u (t ) + K (t ) (y(t ) C x (t )) avec K (t ) = P(t ) C T R 1K(t) est le gain du filtre

P(t ) = A P(t ) + P(t ) A T + G Q G T K (t ) C P(t )

63

64

III. Le filtre de Kalman continu : Kalman-BucyLien avec lobservateur optimalLes quations prcdentes dcrivent un observateur optimal Il minimise tout critre quadratique du type

III. Le filtre de Kalman continu : Kalman-BucySchma de lobservateur

J (t ) = (x (t ) x (t )) S (x (t ) x (t ))O S est dfinie positive

T

65

66

III. Le filtre de Kalman continu : Kalman-BucyFiltre de Kalman-Bucy stationnaireHypothses - w et v sont des bruits blancs gaussiens stationnaires - w et v sont indpendants - La paire (A,C) est observable - La paire (A,B) est commandable Equations du filtre continu

0 = A P + P A T + G Q G T K C P K = P C T R 1Equation de Riccati continue67

IV. Le filtre de Kalman dans les cas non linaires68

IV. Le filtre de Kalman tenduHypothsesOn considre un processus non linaire dcrit par lquation :

IV. Le filtre de Kalman tenduRsum : le filtre de Kalman tendu x (k + 1 | k ) = (x (k | k ), u (k ))Etat estim : intgration de lqua difCovariance du bruit sur ltat : Q = TeQ Covariance du bruit sur la mesure

x (t ) = f (x (t ), u (t ), t ) + w (t ) y(t ) = h (x (t ), t ) + v(t )w(k) et v(k) sont des bruits blancs non corrls

P(k + 1 | k ) = F P(k | k ) F T + Q'

E[w ] = 0

E w wT = Q

[

]

E[v] = 0

Covariance de (tat estim tat rel)

E v vT = R

[

]

K (k + 1) = P(k + 1 | k ) H T H P(k + 1 | k ) H T + RGain de Kalman

[

]

1

Lalgorithme de filtre de Kalman tendu utilise les jacobiens pour les calculs du gain de Kalman et de la matrice de covariance

x (k + 1 | k + 1) = x (k + 1 | k ) + K (k + 1) (y(k + 1) y(k + 1 | k ))Etat filtr Prise en compte de la mesure

f (x ) x h (x ) H= x F=

Attention : Jacobien nest pas drive

P(k + 1 | k + 1) = [I K (k + 1) H ] P(k + 1 | k )Covariance de ltat filtr69 70

Etat

Temps

IV. Le filtre de Kalman tenduLalgorithme x = (x, u )Filtrage Diminution de P

P = F P F T + Q'

EstimationAugmentation de P

= + K (y ) x x y y = h (x )

K = P HT H P HT + RP = [I K H ] P

[

]

1

V. Un exemple complet : reconstitution dattitude par centrale dattitude composants lis (Strapdown Inertial and Attitude System)(J.Sola 2003)71 72

V.1 Position du problme


Les tats observer Localisation dans l'espace : Vitesse de translation : Acclration : Orientation : Vitesse d'orientation : r (m) v (m/s) a (m/s2) q (quaternion) (rad/s)

Les capteurs embarqus Prcision

Acclromtre 3D Gyromtre 3D Magntomtre 3D Rcepteur GPSMag

ac c mc rc , vcAcc

Gyro GPS73

Dynamique74

V.1 Position du problmezECEFPle Nord N E D Greenwich

V.1 Position du problmeyavion

Orientations Orientations reprsentes par les anglers dEuler Orientations reprsentes par les quaternionsQuaternion dorientation : Vecteur des vitesses angulaires :xavion

Les systmes de rfrence La rfrence ECEF GPS

za

vion

Equateur

yECEF xECEF

q = [q 0 , q1 , q 2 , q 3 ]T = [p, q, r ]T

La rfrence NED tat : r, v, a, q

La rfrence avion Acc, Mag, Gyro tat :

yavion

0 p qxavion zavion

r

T

Matrice de vitesse angulaire :75

=

p q r

0 r q

r q 0 p p 0

76


V.2 Equations du filtre de Kalman tendu

Localisation

Le modle cinmatique d'tatAccDynamique Relative

r + = r + Te v + v r v + = v + Te a + v v a+ = a + va

GPSStatique Absolue FUSION DE DONNES

Sparation en sous systmes Le sous systme de localisation Le sous systme d'orientation

q + = q + Te +

q + vq

=

+v

0 = p q r

p 0 r q

q

r

Acc + MagOrientation

Gyro

r q 0 p p 0

77

78



Les modles des mesures GPS Amplitude relle de ltat Rotation constante Translation constante

Les modles des mesures Acc, Mag et Gyro Amplitude relle avion Changement de repre variable C NED ( q ) Biais Gains: K Dsalignements: L (m, ou bien a+g) (pas pour le Gyro)

rxECEF rzECEFECEF

r0ECEF x r0ECEF z cxy c yy czy

cxx czx

cxy c yy czy

cxz czzNED

rxNED rzNEDv x v y v z

ryECEF = r0ECEF + c yx y vx cxx ECEF = c yx vy vzECEF czx

c yz ryNED

xr r + y zr

vx cxz NED + c yz v y czz vzNED

mcx kx mcy = s yx mcz79

sxy ky szy

sxz cxx s yz c yx kz czx

cxy c yy czy

cxz mx x vx c yz m y + x + v y czz mz

szx

x

vz80

S = K+L

V.2 Equations du filtre de Kalman tendu quations systmert +1 = rt + Te v t + w r v t +1 = v t + Te a t + w v a t +1 = a t + w at +1


quations mesuresR = r + vr V = v + vv A = S a C ( a g ) + vv M = S m C m + vm G = S + v

Augmentation du vecteur dtat avec les incertitudes des capteursSymbole r v a q 4x1 4x1 3x1 3x1 3x1 3x1 Sg 3x3 9x1 3x1 3x1 Sa 3x3 9x1 3x1 3x1 Sm 3x3 9x1

=

t

+w

Sa , t +1 = Sa , t + w Sa Q t +1 = Te t Q t + w Qt +1

Dimension 3x1 3x1 3x1 Sous 3x1 3x1 3x1 vecteur Indices 1-3 4-6 7-9

= = =

t t

+w +w,t

t +1

10-13 14-16 17-19 20-28 29-31 32-40 41-43 44-52

S

, t +1

=S

+ wS

t +1

t

+w

Incertitudes : mouvements browniens81 82

S m, t +1 = S m, t + w Sm

V. 3 Simplification du problme


Observateurs de poursuite: tat plus biais du gyromtre. Algorithmes embarqus diffrencis pour la localisation et lorientation. tats estimer : 3x1 3x1 5-7 3x1 3x1 8-10

Traitement des mesures asynchronesF=NON

x

x

P + = F P FT + Q x+ = ( x)

Boucle de prdiction synchrone 40 ms

Symbole

r

v

a

q 4x1 4x1 1-4

Nouvelle mesure ? OUI

Dimension 3x1 3x1 3x1 Sous 3x1 3x1 3x1 vecteur Indices 1-3 4-6 7-9

h Hi = hii H i = x x x K= K=

x

Boucle de correction larrive de chaque mesure

9+10=1983

+ K y xx ==xx+ K ((yii hi ( x ) )++

P ++= ((II KHii ) P P = KH

PHT PHiT i H i iPHTiT + rii H PHi +

84


V. 4 Rsultats

viter linversion de matricesyi = hi (x) + viHi = hi xPHT i H i PHT + ri i

Sous systme dorientationK=

Calibrage du gyromtre: gains et dsalignements (19 tats) Poursuite avec estimation du biais (10 tats). Recherche dun jeu de mesures rduit. Donnes artificielles et relles86

R est diagonale

Scalaire !!

Ne calculer que les composantes non nullesK= k P(14,14) + k p ( P(14,17) + P(17,14) ) + P(17,17) + rp2 p

k p P(:,14) + P(:,17)

12 produits 14 additions 10 divisions85

V. 4 RsultatsAttitude 1


0

-1

Calibration. Donnes artificielles Gxyz Axyz Mxyz0 5 10 15 20 Angular25 rates 30 35 40 45

0

-1

Calibration. Donnes relles Gxyz Axyz Mxyz0 50 Angular rates 100 150

2 0 -2 0 0.4 5 10 15 20 Gyro drifts 25 30 35 40 45

2 0 -2 0 0.15 50 Gyro drifts 100 150

0.2 0 0 1.5 1 0.5 0 0 5 10 15 20 25 30 35 40 45 5 10 15 Gyro gains and disalignements 20 25 30 35 40 45

0.1 0.05 0 0 1 0.5 0 0 50 100 150 50 Gyro gains and disalignements 100 150

87

88



0

-1

Poursuite. Donnes artificielles Gxyz Axy Mxy0 5 10 15 20 Angular25 rates 30 35 40 45

0

-1 2 0 -2

0

10

20

30

40 Angular rates

50

60

70

Poursuite. Donnes relles Gxyz Axy Mxy

2 0 -2 0 1 5 10 15 20 Gyro drifts 25 30 35 40 45

0 0.3

10

20

30

40 Gyro drifts

50

60

70

0

0.2 0.1 0

-1 0.03 0.02 0.01

0

5

10

15

20 Variances 25

30

35

40

45

0 -3 x 10 15 10 5

10

20

30

40 Variances

50

60

70

0

5

10

15

20

25

30

35

40

45

0

10

20

30

40

50

60

70

89

90

Documents

Kalman