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
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-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
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
## %% %% "" $$ $ $ "" "" "" ""
&

''''
"
! !
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

"
"
""

%%
"

!! !!

"" ""
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)

"
"
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

Rglage de lobservateur Identit
Ce que lon connat du systme :

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

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

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

Mthode 2 : formule dAckerman

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

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

Notations
Ltat :

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

Etape 2 : filtrage de ltat
On utilise ltat prdit pour obtenir ltat estim :

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

Calcul de la matrice de covariance :

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

Calcul du gain de Kalman Rappel : le gain de Kalman doit permettre de minimiser le critre quadratique :

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

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

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

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
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

Les bruits sont corrls !
E w (k ) v(i ) =
T

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

Exemple : constante alatoire
Mon acclromtre est biais, dune valeur biais imprvisible On rajoute un tat reprsentant ce biais :

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

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

Apothose : lalgorithme
= A + Bu x x
Filtrage Diminution de P

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

Rduction de la complexit de lalgorithme en dcouplant les mesures

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

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

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

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

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

Test de blancheur

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 : 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

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

Objectif
Obtenir un estim de ltat partir de la connaissance : - du modle - des entres et des sorties

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

Lien avec lobservateur optimal
Les quations prcdentes dcrivent un observateur optimal Il minimise tout critre quadratique du type

Schma de lobservateur
J (t ) = (x (t ) x (t )) S (x (t ) x (t ))
O S est dfinie positive
65
66

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 :

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

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)
(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 GPS

Mag
ac c mc rc , vc
Acc
Gyro GPS
73
Dynamique
74

zECEF
Ple Nord N E D Greenwich

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.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
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

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
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
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. 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

Kalman

Transféré par

Informations du document

Copyright

Formats disponibles

Partager ce document

Partager ou intégrer le document

Options de partage

Avez-vous trouvé ce document utile ?

Ce contenu est-il inapproprié ?

Droits d'auteur :

Formats disponibles

Kalman

Transféré par

Droits d'auteur :

Formats disponibles

Plan du cours

Module 5AS2-10 troisime anne ENSICA

Yves Briere ENSICA

Calculateur : Fusion de donnes

! "" ""  " " !

"   !   %   " %   1 " %

Centrale dattitude composants lis Strapdown

Centrale dattitude composants lis Strapdown

I.1 Quest-ce quun filtre de Kalman ?

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

Une des plus grandes inventions du sicle (bigre)

Centrale dattitude composants lis Strapdown

I. Introduction I.2 La thorie de lestimation

I.2 La thorie de lestimation

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

I. Introduction I.3 Rappel sur les observateurs

I.3 Rappel sur les observateurs

Ce que lon connat du systme :

Observateur en boucle ouverte :

Le rglage de K rduit lerreur y 15

I.3 Rappel sur les observateurs

I.3 Rappel sur les observateurs

A - K.C : Dynamique de lobservateur

I.3 Rappel sur les observateurs

I.3 Rappel sur les observateurs

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

I.3 Rappel sur les observateurs

I.3 Rappel sur les observateurs

Polynme caractristique dsir de lobservateur (n ples)

Mthode numrique optimise

I.3 Rappel sur les observateurs

II. Le filtre de Kalman discret

II. Le filtre de Kalman discret

II. Le filtre de Kalman discret : quations

II.1 Equations du filtre de Kalman

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

Condition initiale : x(0)

II. Le filtre de Kalman discret : quations

II. Le filtre de Kalman discret : quations

E[x + | k = E x x | x (k (k1+ 1)]= A[A (k (k ) + B u (k ) + G w (k )]

[ (( + | ] ) E[C x( ( ) Eyykk+ 11)k == C xkk| |kk ) + v(k )]

II. Le filtre de Kalman discret : quations

II. Le filtre de Kalman discret : quations

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

Erreur au sens quadratique : on cherche

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

minimiser J(k+1) avec :

II. Le filtre de Kalman discret : quations

II. Le filtre de Kalman discret : quations

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) = 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]

II. Le filtre de Kalman discret : quations

II. Le filtre de Kalman discret : quations

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

Autre criture de la relation dvolution de P :

(Comme tout le monde sait)

P(k + 1 | k + 1) = P(k + 1 | k ) K (k + 1) C P(k + 1 | k )

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

II. Le filtre de Kalman discret : quations

II. Le filtre de Kalman discret : quations

Systme linaire + Bruits blans Gaussiens

Covariance de (tat estim tat rel)

Convergence + Filtre optimal

! "" "" " " !

" ! % " % 1 " %