Plan du cours
Le filtre de Kalman
I. Introduction II. Le filtre de Kalman discret : algorithme III. Le filtre de Kalman continu [Link] filtre de Kalman dans les cas non linaires
Module 5AS2-10 troisime anne ENSICA
Yves Briere ENSICA
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. [Link]/~ybriere/5AS2-10
3 4
I.0 Un exemple
Centrale dattitude : Gyroscope
I.0 Un exemple
Centrale 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?
I.0 Un exemple
Centrale 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 exemple
Centrale dattitude composants lis Strapdown
## %% %% "" $$ $ $ "" "" "" ""
&
''''
"
! !
A = Ca + v + g + b +
11 11
Bruits de mesure
% !! !! "" "" ! % " " 2 3 ! 2 "
"" ""
44
"" ""
""
% %
GPS : mesure de la position Dans un repre absolu Peu prcis Faible bande passante
" "
"
! !
'
"
" "
&
$
' # #
"
"
"
"
"
4 4
I.0 Un exemple
## ## $$ $
I.0 Un exemple
"
! "" "" " " !
" ! % " % 1 " %
" "
"
"
mes = vrai + b +
%%
(
""
" "
!
$$ $ $
""
"
% 2 "
"
!
"
"" ""
!
$$ $
% ! %
Bruits de mesure
"
"
""
%%
"
Centrale dattitude composants lis Strapdown
Centrale dattitude composants lis Strapdown
!! !!
"" ""
11
9
& $ " # " ## ## $$ $
I.0 Un exemple
& &
" "
11
%
"
M mes = M vrai + b +
$
" "
( ! !
En gnral
" "
%
"
"
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, etc
12
Une des plus grandes inventions du sicle (bigre)
Centrale dattitude composants lis Strapdown
"
"
10
I. Introduction I.2 La thorie de lestimation
I.2 La thorie de lestimation
Historique
1777-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 observateurs
Rappel sur lobservateur Identit
Permet 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 observateurs
Rglage de lobservateur Identit
Ce que lon connat du systme :
I.3 Rappel sur les observateurs
Rglage de lobservateur Identit
Mthode 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 = CX
PO = M O
X = MO XO
1 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 A
T 2 n 1 T
p1 = C T
Avec :
A - K.C : Dynamique de lobservateur
p 2 = A T + a n 1 I p1 p3 ...
+ a n 2 I p1
18
17
I.3 Rappel sur les observateurs
Rglage 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 observateurs
Rglage 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)
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 0
19
20
I.3 Rappel sur les observateurs
Rglage de lobservateur Identit
Mthode 2 : formule dAckerman
I.3 Rappel sur les observateurs
Rglage de lobservateur Identit
Mthode 3 : avec matlab
(p ) = p n +
n 1
p n 1 +
n 1
p n 1 + ... + 1p +
Polynme caractristique dsir de lobservateur (n ples)
O = CT , A T CT , A
( ) C ...(A ) C
2 T T n -1 T
1
>> 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 observateurs
Inconvnient de lobservateur Identit
Comment 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 discret
23 24
II. Le filtre de Kalman discret
II. Le filtre de Kalman discret : quations
Hypothses
On 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 ) = P0
T
E[x (0 )] = x 0
26
II. Le filtre de Kalman discret : quations
Notations
Ltat :
II. Le filtre de Kalman discret : quations
Etape 1 : prdiction de ltat
On 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 : quations
Etape 2 : filtrage de ltat
On utilise ltat prdit pour obtenir ltat estim :
II. Le filtre de Kalman discret : quations
Etape 2 : estimation de ltat
Lien 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) = E
T
] [
T
(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) =
] 1] ]
E[ E[
1 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
Lien :
Propri t fondamentale : minimiser J(k+1) est quivalent
J (k + 1) = E (k + 1) S (k + 1) , S > 0
T
minimiser J(k+1) avec :
J (k + 1) = trace(P(k + 1 | k + 1))
Il est plus simple de travailler sur la matrice de covariance
30
29
II. Le filtre de Kalman discret : quations
Etape 2 : estimation de ltat
Calcul de la matrice de covariance :
II. Le filtre de Kalman discret : quations
Etape 2 : estimation de ltat
Calcul 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 T
P(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
................................... + K (k + 1) R K (k + 1)
T
32
II. Le filtre de Kalman discret : quations
Etape 2 : estimation de ltat
Calcul du gain de Kalman Rappel : le gain de Kalman doit permettre de minimiser le critre quadratique :
II. Le filtre de Kalman discret : quations
Etape 2 : estimation de ltat
Calcul 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 positive
Ouf
33
34
II. Le filtre de Kalman discret : quations
Rsum : le filtre de Kalman discret
x (k + 1 | k ) = A x (k | k ) + B u (k )
Etat estim
Covariance du bruit sur ltat Covariance du bruit sur la mesure
II. Le filtre de Kalman discret : quations
Conclusion
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 + R
Gain de Kalman
Covariance de (tat estim tat rel)
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 filtr
35 36
II. Le filtre de Kalman discret stationnaire
II. Le filtre de Kalman discret stationnaire
On 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 T
37
] [R + C P
1
CT
C P A T + G.Q.G T = 0
38
II. Le filtre de Kalman discret stationnaire
II. Le filtre de Kalman discret stationnaire
A P A T P A P C T R + C P C T
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 stationnaire
Les bruits sont corrls !
E w (k ) v(i ) =
T
II. Le filtre de Kalman discret stationnaire
Les 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. Ouf
1
x (k + 1 | k ) = A x (k | k ) + B u (k )
P(k + 1 | k ) = A P(k | k ) A T + G Q G T
K (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 stationnaire
Exemple : constante alatoire
Mon acclromtre est biais, dune valeur biais imprvisible On rajoute un tat reprsentant ce biais :
II. Le filtre de Kalman discret stationnaire
Exemple : sinusode alatoire
Je 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 0
2
1 0
x +
0 1
y = (0 1) x
43
44
II. Le filtre de Kalman discret stationnaire
Exemple : marche alatoire
Processus 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 : algorithme
Apothose : lalgorithme
= A + Bu x x
Filtrage Diminution de P
II. Le filtre de Kalman discret : algorithme
Forme de Joseph
P = A P AT + G Q GT
Estimation
Augmentation 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
K = P CT C P CT + R
P = [I K C] P
47
48
II. Le filtre de Kalman discret : algorithme
Rduction de la complexit de lalgorithme en dcouplant les mesures
II. Le filtre de Kalman discret : algorithme
Algorithme avec mesures dcouples
K = P C1 C1 P C1 + R 1
T T
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 2
T T
P = [I K C 2 ] P
x = x + K (y 2 C 2 x )
Etc 49 50
II. Le filtre de Kalman discret : algorithme
Mise en uvre de lalgorithme
function [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 : algorithme
Mise en uvre de lalgorithme exemple
Donnes 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
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 : algorithme
Mise en uvre de lalgorithme Rglages
Initialisations 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 : algorithme
Mise en uvre de lalgorithme Vrifications
Vrifications 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 blancheur
53 54
II. Le filtre de Kalman discret : algorithme
Mise en uvre de lalgorithme
Test de blancheur
II. Le filtre de Kalman discret : algorithme
Mise en uvre de lalgorithme
Rjection 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 + R
T
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 ) = 0
2,17 RN(0 ) = 1, RN (i ) N
On dfini le terme :
(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,7
55
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 pas
Divergence ?
II.4 Ca marche pas
Et 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 pas
Divergence ?
Problme de mesure
Mesures errones intermittentes, cf. test de blancheur
Problme numrique
III. Le filtre de Kalman continu
59 60
Utilisation dalgorithmes volus
III. Le filtre de Kalman continu : Kalman-Bucy
Hypothses
On considre un processus linaire dcrit par lquation :
III. Le filtre de Kalman continu : Kalman-Bucy
Hypothses
Dfinitions 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
]
62
III. Le filtre de Kalman continu : Kalman-Bucy
Objectif
Obtenir un estim de ltat partir de la connaissance : - du modle - des entres et des sorties
III. Le filtre de Kalman continu : Kalman-Bucy
Equations 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 1
K(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-Bucy
Lien avec lobservateur optimal
Les quations prcdentes dcrivent un observateur optimal Il minimise tout critre quadratique du type
III. Le filtre de Kalman continu : Kalman-Bucy
Schma de lobservateur
J (t ) = (x (t ) x (t )) S (x (t ) x (t ))
O S est dfinie positive
65
66
III. Le filtre de Kalman continu : Kalman-Bucy
Filtre de Kalman-Bucy stationnaire
Hypothses - 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 1
Equation de Riccati continue
67
IV. Le filtre de Kalman dans les cas non linaires
68
IV. Le filtre de Kalman tendu
Hypothses
On considre un processus non linaire dcrit par lquation :
IV. Le filtre de Kalman tendu
Rsum : le filtre de Kalman tendu
x (k + 1 | k ) = (x (k | k ), u (k ))
Etat estim : intgration de lqua dif
Covariance 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 + R
Gain de Kalman
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 filtr
69 70
Etat
Temps
IV. Le filtre de Kalman tendu
Lalgorithme
x = (x, u )
Filtrage Diminution de P
P = F P F T + Q'
Estimation
Augmentation de P
= + K (y ) x x y
y = h (x )
K = P HT H P HT + R
P = [I K H ] P
V. Un exemple complet : reconstitution dattitude par centrale dattitude composants lis (Strapdown Inertial and Attitude System)
([Link] 2003)
71 72
V.1 Position du problme
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 GPS
Mag
ac c mc rc , vc
Acc
Gyro GPS
73
Dynamique
74
V.1 Position du problme
zECEF
Ple Nord N E D Greenwich
V.1 Position du problme
ya
vion
Orientations
Orientations reprsentes par les anglers dEuler Orientations reprsentes par les quaternions
Quaternion dorientation : Vecteur des vitesses angulaires :
xa
vion
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 q
xavion zavion
Matrice de vitesse angulaire :
75
p q r
0 r q
r q 0 p p 0
76
V.1 Position du problme
V.2 Equations du filtre de Kalman tendu
Localisation
Le modle cinmatique d'tat
Acc
Dynamique Relative
r + = r + Te v + v r v + = v + Te a + v v a+ = a + va
GPS
Statique 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
Acc + Mag
Orientation
Gyro
r q 0 p p 0
77
78
V.2 Equations du filtre de Kalman tendu
V.2 Equations du filtre de Kalman tendu
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 rzECEF
ECEF
r0ECEF x r0ECEF z cxy c yy czy
cxx czx
cxy c yy czy
cxz czz
NED
rxNED rzNED
v 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 mcz
79
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
vz
80
S = K+L
V.2 Equations du filtre de Kalman tendu
quations systme
rt +1 = rt + Te v t + w r v t +1 = v t + Te a t + w v a t +1 = a t + w a
t +1
V.2 Equations du filtre de Kalman tendu
quations mesures
R = 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 capteurs
Symbole r v a q 4x1 4x1 3x1 3x1 3x1 3x1 Sg 3x3 9x1 3x1 3x1 Sa 3x3 9x1 3x1 3x1 Sm 3x3 9x1
+w
Sa , t +1 = Sa , t + w Sa Q t +1 = Te t Q t + w Q
t +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
, t +1
=S
+ wS
t +1
+w
Incertitudes : mouvements browniens
81 82
S m, t +1 = S m, t + w Sm
V. 3 Simplification du problme
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 asynchrones
F=
NON
P + = F P FT + Q x+ = ( x)
Boucle de prdiction synchrone 40 ms
Symbole
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=
Boucle de correction larrive de chaque mesure
9+10=19
83
+ 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. 3 Simplification du problme
V. 4 Rsultats
viter linversion de matrices
yi = hi (x) + vi
Hi = hi x
PHT i H i PHT + ri i
Sous systme dorientation
K=
Calibrage du gyromtre: gains et dsalignements (19 tats) Poursuite avec estimation du biais (10 tats). Recherche dun jeu de mesures rduit. Donnes artificielles et relles
86
R est diagonale
Scalaire !!
Ne calculer que les composantes non nulles
K= k P(14,14) + k p ( P(14,17) + P(17,14) ) + P(17,17) + rp
2 p
k p P(:,14) + P(:,17)
12 produits 14 additions 10 divisions
85
V. 4 Rsultats
Attitude 1
V. 4 Rsultats
Attitude 1
-1
Calibration. Donnes artificielles Gxyz Axyz Mxyz
0 5 10 15 20 Angular25 rates 30 35 40 45
-1
Calibration. Donnes relles Gxyz Axyz Mxyz
0 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
V. 4 Rsultats
Attitude 1
V. 4 Rsultats
Attitude 1
-1
Poursuite. Donnes artificielles Gxyz Axy Mxy
0 5 10 15 20 Angular25 rates 30 35 40 45
-1 2 0 -2
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.2 0.1 0
-1 0.03 0.02 0.01
10
15
20 Variances 25
30
35
40
45
0 -3 x 10 15 10 5
10
20
30
40 Variances
50
60
70
10
15
20
25
30
35
40
45
10
20
30
40
50
60
70
89
90
