Fusion de capteurs
Transcript of Fusion de capteurs
![Page 1: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/1.jpg)
Philippe Giguère
Fusion de capteurs
![Page 2: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/2.jpg)
Retour sur l’examen
• GLO-4001 : moyenne de 78 %
• GLO-7021 : moyenne de 82 %
• Explication sur l’examen
– Ce vendredi de 10h30 à midi, local du laboratoire
• Donc pas de laboratoire cette semaine
GLO-4001/7021 Introduction à la robotique mobile 2
![Page 3: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/3.jpg)
Bourses CRSNG à la maîtrise
GLO-4001/7021 Introduction à la robotique mobile 3
• Date limite d’application : 1er décembre
• Bourse de 17,500$ (sur 12 mois)
• Moyenne de 3,67 dans les 2 dernières années
• http://www.nserc-crsng.gc.ca/Students-Etudiants/PG-CS/CGSM-BESCM_fra.asp
![Page 4: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/4.jpg)
Concept #1Fusion de capteurs
GLO-4001/7021 Introduction à la robotique mobile 4
![Page 5: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/5.jpg)
Fusion de capteurs/information
• Robot est équipé de différents capteurs, travaillant dans différentes modalités / grandeurs physiques :– scanneur laser (temps de vol lumière, mètre)– odomètre (pulsations, mètre)– caméra (intensité lumineuse, pixel)– centrale inertielle (accél., gyro, magnéto)
• On a souvent aussi des estimés préalables de pose X
• Comment combiner l’information de ces différentes sources de façon optimale*, en tenant compte des incertitudes (bruits)
5*quand c’est possible/calculable en un temps raisonnable
![Page 6: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/6.jpg)
Fusion de capteurs : cas simple
• Soit un robot équipé de deux capteurs de distance (A et B) ayant des bruits différents (sA
2et sB2)
GLO-4001/7021 Introduction à la robotique mobile 6
AzA
0 x
mu
r BzB
![Page 7: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/7.jpg)
Fusion de capteurs
• Quel est le meilleur estimé de la position du robot à partir des deux mesures zA et zB,
en tenant compte des bruits/incertitudes sA2 et sB
2?
GLO-4001/7021 Introduction à la robotique mobile 7
xzA zB3x 2x
1x 4x
x
pro
bab
lemen
t ici
?
![Page 8: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/8.jpg)
Fusion de capteur : moyenne pondérée
• Combiner avec une moyenne pondérée les i mesures (évidences) obtenues, basée sur l’inverse des variances s2
i
• On veut donner plus d’importance à la mesure (évidence) ayant une variance s2
i plus petite.
GLO-4001/7021 Introduction à la robotique mobile 8
2
1information
is
![Page 9: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/9.jpg)
Fusion de capteurs : moyenne
GLO-4001/7021 Introduction à la robotique mobile 9
11
2 210 (0, 1 )Nz s 22
2 210 (0, 1 )Nz s 13
2
2
zz
z
capteur 1 capteur 2moyenne
Capteurs identiques, s12 = s2
2
1
2
2(0, )i iz x N s
modèle du capteur
-parfaitement linéaire-bruit sans biais-portée illimitée
x=10
écart-type écart-type
![Page 10: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/10.jpg)
Fusion de capteurs : moyenne
10
11
2 210 (0, 1 )Nz s 22
2 210 (0, 1 )Nz s 13
2
2
zz
z
capteur 1 capteur 2moyenne
1
2
2 2 2
1 2 1 2 1 2 1 2
1 1 1 1 1 1 1 1 1 1var var var var var (1)
2 2 2 2 4 4 4 4 2 2moyenne z z z z z zs s s
Capteurs identiques, s12 = s2
2 x=10
écart-type écart-typeécart-type
![Page 11: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/11.jpg)
Fusion de capteurs : moyenne
11
11
2 210 (0, 1 )Nz s 22
2 210 (0, 0.5 )z N s 13
2
2
zz
z
capteur 1 capteur 2moyenne
Capteurs non-identiques, s12 > s2
2 x=10
écart-type écart-type
?
![Page 12: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/12.jpg)
Fusion de capteurs : moyenne
12
11
2 210 (0, 1 )Nz s 22
2 210 (0, 0.5 )z N s 13
2
2
zz
z
capteur 1 capteur 2moyenne
smoyenne est pire!Capteurs non-identiques, s1
2 > s22
Puis-je quand même soutirer de l’info du capteur 1?
x=10
écart-type écart-typeécart-type
![Page 13: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/13.jpg)
Fusion de capteurs : moyenne pondérée
13
capteur 1 capteur 2
22
2 210 (0, 0.5 )z N s 11
210 (0, 1)z N s 13 2(1 )z w z zw
Cap
teu
rsn
on
-id
enti
qu
es, s
12
> s
22
0.5
1.0
w
(même qu’avant) (même qu’avant)0 1w
![Page 14: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/14.jpg)
Fusion de capteurs : moyenne pondérée
14
capteur 1 capteur 2
22
2 210 (0, 0.5 )z N s 11
210 (0, 1)z N s 13 2(1 )z w z zw
Cap
teu
rsn
on
-id
enti
qu
es, s
12
> s
22
0.5
1.0
w
(même qu’avant) (même qu’avant)
w=0.50
3var z
0 1w
z 3
![Page 15: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/15.jpg)
Fusion de capteurs : moyenne pondérée
15
2
1
2 2
1 2
optws
s s
capteur 1 capteur 2
3var z
22
2 210 (0, 0.5 )z N s 11
210 (0, 1)z N s 13 2(1 )z w z zw
w=0.80minimum
0 1w
Cap
teu
rsn
on
-id
enti
qu
es, s
12
> s
22
z 3
![Page 16: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/16.jpg)
Preuve poids w optimal
GLO-4001/7021 Introduction à la robotique mobile 16p.103 du manuel
3 1 2(1 )z w z wz
2 2 2 2 2
3 1 2(1- )w ws s s
Pour minimiser fonction, cherche
2 2 2 2 2 2 2
3 1 2 1 2(1- ) 2(1- ) ( 1) 2 0d d
w w w wdw dw
s s s s s
2
1
2 2
1 2
ws
s s
2
3 0d
dws
variances
Poids optimal minimisant variance z3 :
2 2
1 22( 1) 2 0w ws s
(x1 et x2 sont indépendants)
![Page 17: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/17.jpg)
Fusion de capteurs : moyenne pondérée
GLO-4001/7021 Introduction à la robotique mobile 17
2
1
2 2 2
1 2
1 10.8
1 0.5 1.25optw
s
s s
22
2 210 (0, 0.5 )z N s 11
2 210 (0, 1 )Nz s 13 2(1 )z w z zw
z 3
![Page 18: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/18.jpg)
• J’ai deux mesures : z1 et z2
• J’ai les variances associées : et
GLO-4001/7021 Introduction à la robotique mobile 18
Fusion de capteurs : résumé
2
1s 2
2s
2
112 2
1 1
2
222 2
2 2
3 zzzs
s
s
s s s
combinaison optimale
On fait plus confiance à la mesure ou l’évidence avec la plus petite variance s2
![Page 19: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/19.jpg)
Concept #2Moyenne au fil du temps
20
![Page 20: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/20.jpg)
1
1 2
1 2 3
1 2 3 4
ˆ(1)
ˆ(2)2
ˆ(3)3
ˆ(4)4
d z
z zd
z z zd
z z z zd
Moyenne au fil du temps
• Après chaque mesure zt, avoir le meilleur estimé disponible de la position d d’un robot immobile
• Prendre plusieurs mesures du même capteur
21d
z1
0 z1
![Page 21: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/21.jpg)
Moyenne au fil du temps
• Après chaque mesure zt, avoir le meilleur estimé disponible de la position d d’un robot immobile
• Prendre plusieurs mesures du même capteur
22d
z2
0
1
1 2
1 2 3
1 2 3 4
ˆ(1)
ˆ(2)2
ˆ(3)3
ˆ(4)4
d z
z zd
z z zd
z z z zd
z1z2
![Page 22: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/22.jpg)
1
1 2
1 2 3
1 2 3 4
ˆ(1)
ˆ(2)2
ˆ(3)3
ˆ(4)4
d z
z zd
z z zd
z z z zd
Moyenne au fil du temps
• Après chaque mesure zt, avoir le meilleur estimé disponible de la position d d’un robot immobile
• Prendre plusieurs mesures du même capteur
23d
z3
0 z1 z3z2
![Page 23: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/23.jpg)
1
1 2
1 2 3
1 2 3 4
ˆ(1)
ˆ(2)2
ˆ(3)3
ˆ(4)4
d z
z zd
z z zd
z z z zd
Moyenne au fil du temps
• Après chaque mesure zt, avoir le meilleur estimé disponible de la position d d’un robot immobile
• Prendre plusieurs mesures du même capteur
24d
0 z1 z3z2
etc…
z4
![Page 24: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/24.jpg)
Moyenne au fil du temps
• Avec le temps, la série allonge… et le temps de
calcul/espace de stockage pour les zi aussi!
25d
0
1
ˆ 1( ) i
T
iTzd t
1
1 2
1 2 3
1 2 3 4
ˆ(1)
ˆ(2)2
ˆ(3)3
ˆ(4)4
d z
z zd
z z zd
z z z zd
z1 z3z2z4
![Page 25: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/25.jpg)
Moyenne récursive• Garder en mémoire 2 variables :
– estimé moyen
– estimé de la variance de cette moyenne de :
• On connait la variance s2z du capteur
• Après nouvelle mesure z(k+1), on la combine avec pour avoir un nouvel estimé
ˆ( )x k
( )P kˆ( )x k
ˆ( )x k
2
1
2 2
1 2
ws
s s
3 1 2(1 )z w z wz
on a vu précédemment
2
(
( )
( )
( 1
)ˆ ˆ ˆ( 1) ( ) ( )
) ( )(
1
1 )
( )
Z
zx k
P k
P k
P k
x k
P k
w x kk
w
w
s
variance diminue avec le temps
simple reformulation en posant
(estimé de x à l’instant k)
3 1 2var var (1 )z w z wz
1ˆ( )z x k
2 ( 1)z z k
3ˆ( 1)z x k
,ˆ( 1)1) (Px kk
![Page 26: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/26.jpg)
Preuve sur la variance P(k)
27
2
( )
( ) Z
P k
kw
P s
ˆ ˆ ˆ( 1)( 1) ( )() ( )wx k x kk kz x
2 2{ } (1 ) { ( 1ˆ ˆ( 1 } {() }( ) )x k x kVar w Var w r ka zV
ˆ( )) }( {Var xP kk
2
2
1 , donc : ( 1)
( )( )
( )Z
Z
P kP kw
wP ks
s
ˆ ˆ(1 ( 1)( 1) ( ))x k x kw k wz
{ } {(1 ( 1)ˆ ˆ( 1) ( )})Var Var w kx k k wzx
2 2 2{ } (1ˆ )( 1) ( ) zx kV r P wka w s
2 2ˆ( 1) ( )1
{ } (1 ( 1() ))V P kP kw wx karw
2
2 2{ } (1 2 ) ( ) (ˆ )( ) )1 (P kw
Var w w ww
x k P k P k
{ }ˆ( 1) ( ) )1 (Var x kwk P
On choisi le poids optimal :
![Page 27: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/27.jpg)
Exemple d’exécution
GLO-4001/7021 Introduction à la robotique mobile 28
2
1 2 3{ 10, 12, 11} 1zZ z z z s Soit les mesures suivantes :
2
10(1)
(1)
)
1
ˆ(1
zP
x z
s
1
3
2
(1)
(1
1
1 2)
1
1z
wP
P s
2
(
( )
( )
( 1
)ˆ ˆ ˆ( 1) ( ) ( )
) ( )(
1
1 )
( )
Z
zx k
P k
P k
P k
x k
P k
w x kk
w
w
s
(21
( ) 10 (12 1ˆ 0) 1ˆ ˆ(2) (1) (1 1)2
)x x xzw
1 1(1 )
2(2) (1)
2P P
2
2
(2)
(2)
1/ 2 1
1/ 2 1 3z
wP
P s
(31
( ) 11 (11 1ˆ 1) 1ˆ ˆ(3) (2) (2 1)3
)x x xzw
1 1(1 )
3(3) (2)
3P P
On peut donc oublier les mesures zi après
les mises-à-jour
1(2) 2z
1(3) 1z
![Page 28: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/28.jpg)
Implémentation filtre récursif matlab
GLO-4001/7021 Introduction à la robotique mobile 29
Notez comment la moyenne se rapproche de la valeur
véritable, au fil des données
% Boucle de capture d'echantillons
for k=1:nStep
z(k) = 5 + Scapteur*randn; % Z bruitee
if (k==1)
% Premier echantillon, on initialise
xhat = z(k);
P = Scapteur^2;
else
% Equation (4.4) manuel p.104
w = P(k-1)/(P(k-1)+Scapteur^2);
xhat(k) = xhat(k-1) + w*(z(k)-xhat(k-1));
P(k) = (1-w)*P(k-1);
end
mz(k) = mean(z(1:k));
end
![Page 29: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/29.jpg)
Moyenne si la valeur réelle change…
GLO-4001/7021 Introduction à la robotique mobile 30
MoyenneRecursive.m
• Si le robot se déplace… la moyenne perd de son sens
![Page 30: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/30.jpg)
Combiner mesures différents endroits
GLO-4001/7021 Introduction à la robotique mobile 31
résultat de la commande ut envoyée aux moteurs
pasDx
z(1)
z(2)
2( ) (0,( ) )laserxz Nk k s
![Page 31: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/31.jpg)
Combiner mesures différents endroits
GLO-4001/7021 Introduction à la robotique mobile 32
résultat de la commande ut envoyée aux moteurs
pasDx
z(1)
z(2)
2( ) (0,( ) )laserxz Nk k s
J’ai donc z1:2={z(1), z(2)}
et u1:1={u(1)}
![Page 32: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/32.jpg)
2
(2 |1)
(2 |1)
ˆ ˆ ˆ(2) (2 |1) ( (2) (2 |1))
(2) (1 ) (2 |1)
laser
Pw
P
x x w z x
P w P
s
2(2 |1) (1) pasP P s
Combiner mesures différents endroits
GLO-4001/7021 Introduction à la robotique mobile 33
ˆ(1) (1)x z
ˆ ˆ(2 |1) (1)x x x D
2(1) laserP s
avance d’un pas
prend mesure
estimé au temps k=2, à partir de l’information à k=1
2un pas ( , )pasN x sD
pasDx
z(1)
z(2)
2( ) (0,( ) )laserxz Nk k s
mo
yen
ne
récu
rsiv
einnovation
![Page 33: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/33.jpg)
Combiner mesures différents endroits
GLO-4001/7021 Introduction à la robotique mobile 34
ˆ(1) (1)x z 2(1) laserP s
2(2 |1) (1) pasP P s
avance d’un pas
prend mesure
2
(2 |1)
(2 |1)
ˆ ˆ ˆ(2) (2 |1) ( (2) (2 |1))
(2) (1 ) (2 |1)
laser
Pw
P
x x w z x
P w P
s
pas
2un pas ( , )pasN x sD
z(1)
z(2)
ˆ ˆ(2 |1) (1)x x x D
mo
yen
ne
récu
rsiv
einnovation
![Page 34: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/34.jpg)
Combiner mesures différents endroits
GLO-4001/7021 Introduction à la robotique mobile 35
ˆ(1) (1)x z 2(1) laserP s
2(2 |1) (1) pasP P s
avance d’un pas
prend mesure
2
(2 |1)
(2 |1)
ˆ ˆ ˆ(2) (2 |1) ( (2) (2 |1))
(2) (1 ) (2 |1)
laser
Pw
P
x x w z x
P w P
s
Prédiction
pas
2un pas ( , )pasN x sD
z(1)
z(2)
ˆ ˆ(2 |1) (1)x x x D
mo
yen
ne
récu
rsiv
e
Mise à jour
![Page 35: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/35.jpg)
Estimation d’état du robot
• On estime l’endroit le plus probable où le robot se situe à l’instant k:
• Il faut aussi la confiance de cet estimé (variance), pour savoir comment incorporer les mesures z :
36
ˆ( )x k
var ˆ( ) ( )P k x k
![Page 36: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/36.jpg)
Estimation d’état du robot
• Quelles sont les variables qui nous intéressent ?
– pose statique du robot : [x y q]
– état dynamique du robot :
– certains paramètres d’un capteur qui évoluent lentement dans le temps (bias de gyro)
– l’endroit et la signature des objets dans l’environnement
• Idéalement l’état est choisi pour être complet : propriété Markov
– rarement vrai, mais souvent est une bonne approximation
37
x y x yq q
traités p
rincip
alemen
t d
ans le co
urs
![Page 37: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/37.jpg)
Estimation d’état :Filtrage Bayésien
![Page 38: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/38.jpg)
Problème de localisation
• Je suis parti de x0
• J’ai exécuté les commandes de déplacement / mesures odométriques* u1…ut
• J’ai pris les mesures z1…zt (extéroceptives en général)
• Où suis-je à l’instant t? (xt) (filtrage)
*les mesures odométriques sont traitées comme des commandes de déplacement
![Page 39: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/39.jpg)
Problème de localisation
GLO-4001/7021 Introduction à la robotique mobile 42
1: 1:( ) ( | , )t t t tb p xe uxl z
bel( ) : belief ou croyance (raccourci de notation)
xt : pose du robot au temps présent
u1:t : toutes les commandes passées envoyées aux actionneurs
u1:t={u1, u2, …, ut}
z1:t : toutes les mesures passées provenant des capteurs
Simple en apparence, mais imaginez 1000 commandes ut et 1000 mesures zt… une équation avec au moins 2000 termes?
Solution : va falloir ajouter des simplifications!
![Page 40: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/40.jpg)
Simplification : État x complet (Markovien)
43
x0
u1
x1
z1
x0 x1
u1z1
Graphe des dépendances
B Adépend de…
![Page 41: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/41.jpg)
z2
44
x0
u1
x1
z1
u2
x2
z2
x0 x1
u2
x2
Graphe des dépendances
B Adépend de…
et ainsi de suite…
Ces dépendances vont nous permettre de résoudre bel(xt) de façon récursive
Simplification : État x complet (Markovien)
![Page 42: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/42.jpg)
Simplification par indépendances des variables
46
x : état du robot (position, angle, etc.)u : commandes envoyées aux actionneursz : mesures provenant des capteurs
0: 1: 1 1:( | , , | )) ( t tt t t tp z x p zz u x
0: 1 1: 1 1: 1( | , , ) ( | , )t t tt t t tp x x z u p x x u
( | ) ( )p A B p A
si B n’apporte aucune information
sur A
(Après tout, les mesures z ne dépendent que de la pose du robot)
(après la commande ut , avant la mesure zt)
motion model/modèle de déplacement
sensor model/modèle de capteur
xt-1
ut
xt
zt
ut+1
xt+1
zt+1
Incertitude augmente
Incertitude diminue
![Page 43: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/43.jpg)
Filtrage Bayésien : formule générique
47
Algorithme FiltreBayes(bel(xt-1), ut, zt)
for all xt do
endfor
return bel(xt)
1 1 1( | , )( ) ( )pre t tt t ttdbel x belp x xx xu d | ))( (()t pre tt t dp z xbel x bel x
(1)
(2)
• On doit avoir :
– belief original bel(x0)
– probabilité de transition d’état : p(xt|ut, xt-1)
– modèle du capteur p(zt | xt)
![Page 44: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/44.jpg)
État xt complet (Markov)
1: 1 1:,( | , ) ( | )t t ttttp pz uz zx x
1: 1:( ) ( | , )t t t tbel x p x z uOn cherche :
Théorème de Bayes : ( | , ) ( | ) ( | , ) ( | )p C C Cp p pAB B BA A C(avec extra conditionnelle C)
1: 1 1: 1: 1 1: 1: 1 1: 1: 1 1:( | , ) ( | ) ( | , ) ( |, , , , )t t t t t t tt tt tt t tp z u z u z u zpx x xz z p pz u
1: 1 1: 1: 1 1: 1: 1 1:( | , ) ( | , ), ,( | ),t t t tt tt t t ttz u z u zzx x x up p pz
1: 1 1: 1: 1 1:( | , ) ( | ) ( ,| ),t tt tt ttt tx x xz zz u zp p p u Probabilités totales
( | ) ( | , ) ( | )p p pA AC C CB B Bd
1: 1 1: 1: 1 1: 1: 11 1 11:( | ) ( | ( |, ,, ) ), tt t ttt t tt ttp z u zx xp u zx xdx up
Preuve filtrage Bayésien récursif
État xt-1 complet (Markov)
1: 1 1:1 1( | , ) ( |, , )t tt tt ttzx xxp ux pu
1 11: 1 1: 1: 1 1: 1( | ) ( ( | ), | ) ,,t t tttt t tttp p ux x x p dz u z ux x Théorème d’indépendance
( | ) ( ) si A, B indép.p A B p A
1 11: 1 1 1: 1 1 1 1 1 1: 1:( | ) ( | , ) ( |, , ) ( | , ) ( )t t ttt t t t t tt t tt tx x x x x xp p u p z u d p u bexux dz lx
xt-1 et ut sont indépendants car ut arrive après xt-1
constante, car ne dépend pas de xtextrait la dernière mesure
(1)Belpred(xt)
(2)
![Page 45: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/45.jpg)
Filtrage Bayésien
49
Algorithme FiltreBayes(bel(xt-1), ut, zt)
for all xt do
endfor
return bel(xt)
bel( ) : belief ou croyance (raccourci de notation)
ut : commande au temps t
zt : mesure au temps t, après la commande ut
xt : pose du robot au temps t, après ut et zt
: facteur de normalisation
Prédiction
Mise-à-jour
Difficile à implémenter exactement car pas toujours de solution analytique de l’intégrale (1)/produit (2)
(1)
(2)| ))( (()t pre tt t dp z xbel x bel x
1 1 1( | , )( ) ( )pre t tt t ttdbel x belp x xx xu d
distribution
Algorithmerécursif
![Page 46: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/46.jpg)
Quand aura-t-on une solution analytique?
• Les équations à calculer sont
• Solution analytique à ces équations si :
– bel( ) est gaussien
– les équations du systèmes sont linéaires
– les bruits sont gaussiens
• Dans les autres cas, il faudra approximer
GLO-4001/7021 Introduction à la robotique mobile 50
1 1 1( | , )( ) ( )pre t tt t ttdbel x belp x xx xu d
| ))( (()t pre tt t dp z xbel x bel x
![Page 47: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/47.jpg)
Exemple 1D représentation du bel(xt)
51
continue, 1 gaussienne
pro
ba
bil
ité
xt
continue, plusieurs gaussiennes
pro
ba
bil
ité
xt
continue, par échantillons
pro
ba
bil
ité
xt
poids ~ prob
discret, carte topologique
pro
ba
bil
ité
xt
PLT
-37
92
PLT
-37
94
PLT
-37
96
Cafétéria
Co
rrido
r Est
Ascen
seur
discret, carte « quadrillée »
pro
ba
bil
ité
xt10 12 14 18 208642
La réalité va dicter la représentation de Bel(xt)Cette représentation dictera le type de filtre Bayésien utilisé
![Page 48: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/48.jpg)
Filtre Bayésien avec gaussiennes + équations linéaires :
Filtre de Kalman
52
![Page 49: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/49.jpg)
Différents filtres de Kalman
• Fusion de capteurs
– combiner évidences de façon optimal
• Fusion de mesures dans le temps
– moyenne récursive
• Fusion de capteurs avec déplacement : estimation d’état (pour distributions Gaussiennes)
– Filtre de Kalman (linéaire)
– Filtre de Kalman étendu (non-linéaire)
– Filtre de Kalman non-parfumé (non-linéaire)
• Ce sont tous des filtres Bayésiens53
concept clef #1
concept clef #2
#1+ #2
estimés + mesures
![Page 50: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/50.jpg)
• « Boîte noire » d’équations (toujours les mêmes, peu importe le système)
• Paramètres qui décrivent le comportement du système
• Système dynamique, mesures incomplètes* ou bruitées
• En entrée : commandes u(t) et mesures z(t) + estimé précédent et sa covariance estimée P(t-1)
• En sortie : nouvel estimé de la pose + covariance P(t)
• Pourquoi? Pour avoir plus
précis qu’avec seulement z(t)
Filtre de Kalman
54
ˆ( )x t
Filtre de Kalman
u(t)
z(t) ˆ( )x t
P(t)
fcapteur
-1 ˆ( )x tz(t)ˆ( )x t
F G L CwCv
phi gamma lambda
mie
ux
qu
e
matrices :* Je mesure (observe) x, ymais je veux connaitre q.
ˆ( 1)x t
ˆ( 1)x t P(t-1)
![Page 51: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/51.jpg)
Équations du filtre de Kalman (aperçu)
55
( (( 1) )1) ( 1| )I K kP k P k k L
ˆ ˆ (( 1) ( 1) () 1)1|x k x k k K k r k
ˆ( 1) ( 1) ( 1| )r k z k z k k 1( 1| ) ( 1( 1) { ( 1)}| )T T
wP k k P k kK k C k L L L
ˆ( 1| ) ˆ( 1| )z k x k kk L
ˆ ˆ( 1| ) ( )) (x k k x k u k F G
( 1| ) ( ) T
vP Ck k P k F F
(1)
(2)
(3)
(4)
(5)
(6)
(7)
Vo
ici l
a «
bo
îte
no
ire
» (suite d’équations matricielles : système linéaire)
Exécuté à chaque itération k :• 1 vecteur commande u(k)
• un vecteur de mesures z(k)
![Page 52: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/52.jpg)
Filtre de Kalman
• Utiliser la connaissance de la dynamique du système pour réduire l’impact du bruit
• L’état+incertitude du système seront représentés par des gaussiennes multidimensionnelles
GLO-4001/7021 Introduction à la robotique mobile 56
centre
matrice de covariance
2D1D
m
s
![Page 53: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/53.jpg)
Gaussienne multivariée dimension k
57
sou
rces
: w
ikip
edia
11 1( ) exp ( ) ( )
2(2 )
TX
kf x x xm m
1 1
,
k k
x
x
x
m
m
m
matrice carrée des covariances
2
1 1 2 1 3
2
1 2 2 2 3
2
1 3 2 3 3
x x x x x
x x x x x
x x x x x
s s s
s s s
s s s
(symétrique, définie positive)
cov( , ) [( )( )]i j ii i jj jX X E X Xm m
déterminant
![Page 54: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/54.jpg)
Filtre Kalman : estimateur optimal
• Modèle du système est linéaire
• Modèle du capteur est linéaire
• Bruits sont gaussiens, avec moyennes nulles.
• État est caractérisé par une moyenne et covariance distribution unimodale
• Covariance est un bon indicateur de la confiance
• État x(k) est complet : propriété de Markov
GLO-4001/7021 Introduction à la robotique mobile 59
Pré-requis:ˆ ˆ( 1| ) ( )) (x k k x k u k F G
ˆ( 1| ) ˆ( 1| )z k x k kk L
capteurnon-biaisé
![Page 55: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/55.jpg)
Estimation d’état
GLO-4001/7021 Introduction à la robotique mobile 60
u(t) z(t)
un tour de la boucle par cycle déplacement-mesure
![Page 56: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/56.jpg)
Modèle linéaire du système
• Modèle de la dynamique du système
• Exemple
GLO-4001/7021 Introduction à la robotique mobile 61
( 1) ( )( ) ( ) ( ) ( )k k u kx k kx k v F G
21 = 1 v N(0, )passF G
d
État : x=[d] (mètres)Commande : u=[dd] (mètres)
( 1) ( ) ( ) ( )dd k d k k v kd
( 1) ( ) ( ) ( )x k u k v kk x
( ( ) signifie que peut varier en fonction du temps)kF F
modèle matriciel de transition d’état
modèle matriciel des commandes
Bruit Gaussien
actions 2( ) (0, )vv k N s
(Ceci n’est pas l’équation du filtre, mais comment le système fonctionne. On n’ajoute pas du bruit dans le filtre. Il en va de même pour les acétates suivantes sur les modèles).
![Page 57: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/57.jpg)
Modèle linéaire du système
• Modèle de la dynamique du système
• Exemple
GLO-4001/7021 Introduction à la robotique mobile 62
( 1) ( )( ) ( ) ( ) ( )k k u kx k kx k v F G
21 = 1 N(0, )pas sF G
Bruit Gaussien
actions
d
État : x=[d] (mètres)Commande : u=[dd] (mètres)
( 1) ( ) ( ) ( )dd k d k k v kd
( 1) ( ) ( ) ( )x k u k v kk x
![Page 58: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/58.jpg)
Modèle linéaire du système (2)
• Modèle dynamique système
63
( 1) ( )( ) ( ) ( ) ( )k k u kx k kx k v F G
Bruit Gaussienactions
d( 1) ( ) ( ) ( )rd k d k d k t v k D
État : d
xd
Commande : u d
rayon r
vitesse angulaire moteur
( )( 1) 1 ( ) 0
( 1) ( ) ( )( 1) 0 0 ( )
rv kd k t d kx k k
rv kd k d k r
D
F Gx u
roches…
( 1) ( ( ) ( ))d k r k v k
rad/sm/s
m
![Page 59: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/59.jpg)
Modèle linéaire du capteur
GLO-4001/7021 Introduction à la robotique mobile 64
( ) ) ( ))( (xk k kz w k L Bruit Gaussien
d
z
z (V
olt
)
d (mètres)
( ) 0.5 (( ))x kz k w k
État : x=[d] (mètres)
(en Volt)
0
2= 0.5 N(0, )laserw sL
![Page 60: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/60.jpg)
Modèle linéaire du capteur (2)
( ) ) ( ))( (xk k kz w k L Bruit Gaussien
dz 1
(Volt
)
d (mètres)
z1(en Volt)
États : d
xd
z2 (en pulse/s)
encodeur optique
z 2(p
uls
e/s)
d (mètres/s).
.
1 1
2 2
0.5 0
0 5000
z wd
z wd
d
z n’est pas en mètres
(en Volt)
(en pulse/s)
0
z peut avoir une variété d’unités
L
![Page 61: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/61.jpg)
Modèle linéaire du capteur (3)
( ) ) ( ))( (xk k kz w k L Bruit Gaussien
dz 1
(Volt
)
d (mètres)
z1(en Volt)
États : d
xd
1 10.5 0d
z wd
d
z n’est pas obligé d’avoir la même dimension que x
0
2 vs. 1
![Page 62: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/62.jpg)
Modèle linéaire du capteur (4)
( ) ) ( ))( (xk k kz w k L
dz 1
(Volt
)
d (mètres)
z1(en Volt)
États : d
xd
1 1
2 2
3 3
0.5 0
1.2 0
9.1 0
z wd
z wd
z w
d
z2(en Volt)
z3(en Volt) z 2(V
olt
)
d (mètres)
z2=1.2(Volt/m)d
z 3(V
olt
)d (mètres)
z3=9.1(Volt/m)d
0
2 vs. 3
![Page 63: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/63.jpg)
Filtre Kalman
• Ce que l’on cherche à estimer :
– estimé de la pose au moment k :
– estimé de la covariance de la pose au moment k : P(k)
• Ce que l’on doit connaître de notre système
– Variance sur déplacement Cv(k)
– Variance sur capteurs Cw(k)
– Modèle transition d’état F
– Modèle des commandes G
– Modèle du capteur L
GLO-4001/7021 Introduction à la robotique mobile 68
ˆ( )x k
fourni par le manufacturier / vous le mesurez
c’est votre modèle physique du système
fourni par le manufacturier / calibration manuelle
![Page 64: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/64.jpg)
Notation
GLO-4001/7021 Introduction à la robotique mobile 69
estimé de la pose au moment
estimé de la pose, après déplacement ( 1), mesure ( 1)
estimé
ˆ(
d
avan
e la pose, m
t
apr
)
ˆ( 1| )
ˆ( esure ès1 ( 1) )
x k
x k k
x
k
u k z k
zk k
ˆ( 1)x k ˆ( )x k
ˆ( 1| )x k k
u(k+1) z(k+1)
![Page 65: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/65.jpg)
Filtre Kalman : prédiction sur x
GLO-4001/7021 Introduction à la robotique mobile 70
ˆ ˆ( ) ( ) ( )( 1| ) ( )x k k x u kkk k F Gavant mesure z(k+1)
pour ( ) 1 , ( ) 1 ˆ ˆ( | ) ( ) 1x xk xk k kk F G D
ˆ( )x k
cas
cha
rio
t 1
D
![Page 66: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/66.jpg)
Filtre Kalman générique : prédiction sur P
• Variance estimée sur la prédiction :
• Par exemple, 1 dimension et F=[1]
• Par exemple, 1 dimension et F=[a]
GLO-4001/7021 Introduction à la robotique mobile 71
( 1| ) ( )( ) ( ) ( )T
vP k k P kk k C k F F
{ (ˆ ˆ( ) ) ( )) (( ) }T
E x k xx kP k xk k
( 1| ) ( ) ( )T
v vP k k P k P kC C F F
Bruit sur déplacement,perturbations
ˆ( )x k
Position réelle (ettoujours inconnue)
2( 1| ) ( ) ( ) ( )T
v v vC a a C a CP k k P k P k P k F F
![Page 67: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/67.jpg)
Filtre Kalman : prédiction sur P
72
( 1| ) ( )( ) ( ) ( )T
vP k k P kk k C k F F
( ) 1kF 2( ) pasP k s
cas
cha
rio
t 1
D
![Page 68: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/68.jpg)
( )u k
Estimation d’état : prédiction
GLO-4001/7021 Introduction à la robotique mobile 73
note : pour alléger la notation, F=F(k), G=G(k)
ˆ( 1| )x k k
ˆ( )x k
( 1| ) ( ) T
vP Ck k P k F F
ˆ ˆ( 1| ) ( )) (x k k x k u k F G
( 1)z k
![Page 69: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/69.jpg)
( )u k
Estimation d’état : mise-à-jour
GLO-4001/7021 Introduction à la robotique mobile 74
note : pour alléger la notation, F=F(k), G=G(k)
ˆ( 1| )x k k
ˆ( )x k
( 1| ) ( ) T
vP Ck k P k F F
ˆ ˆ( 1| ) ( )) (x k k x k u k F G
( 1)z k
![Page 70: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/70.jpg)
Filtre Kalman : prédire mesure z
• Prédire la mesure du capteur :
• Compare avec mesure prise z(k+1) : innovation r
GLO-4001/7021 Introduction à la robotique mobile 75
ˆ( 1| ) ˆ( 1) | )( 1z k k k x k k L
État que l’on pense être
fonction linéaire du capteur
ˆ( 1) ( 1) ( 1| )r k z k z k k
mesure obtenue
mesure prédite
![Page 71: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/71.jpg)
Calcul poids optimal : gain K
• K est similaire à wopt vu précédemment
• La valeur K (gain) permet de combiner de façon optimalel’estimé de position avec l’innovation r(k+1), en tenant compte de :
– estimé de la covariance de l’estimé de la pose : P(k+1|k)
– covariance de la mesure : matrice Cw(k+1)
ˆ( 1| )x k k
1( 1) ( 1) { ( 1( ) ( 1) ( 1)}1| ) ( 1| )T T
wK P k k Pk k k kk kk C L L L
2( 1) 1 , w laserk C sL exemple :
2 1
2( 1) 1 { 1
( 1| )( 1| ) ( 1| 1 })
( 1| )
T T
laser
laser
P k kPK k k P k k
P k kk s
s
(voir moyenne récursive)
![Page 72: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/72.jpg)
Calcul poids optimal : gain K
• La comparaison se fait dans l’espace des mesures (via L), et non pas dans l’espace de l’état
• On n’inverse donc jamais la fonction de capteur• (Avantage : Si la fonction de capteur n’est pas bijective, on ne peut pas l’inverser.
Dans ce cas-ci linéaire, ce ne sera jamais un problème. Mais pour des cas non-linéaires (EKF) cela pourrait arriver : capteur infrarouge!)
77
1( 1) ( 1) { ( 1)( 1| ) ( 1| ( 1) ( 1) )}T T
wK k k kP k k P k k k C k L L L
« passe » la covariance sur l’état à travers la fonction de capteur
covariance des bruits dans les
unités des capteurs
![Page 73: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/73.jpg)
Combiner innovation + estimé
GLO-4001/7021 Introduction à la robotique mobile 79
ˆ ˆ (( 1) ( 1) () 1)1|x k x k k K k r k
Estimé avant mesure
Estimé après mesure
innovation : information du
capteur
gain Kalman : matrice distribue les corrections
![Page 74: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/74.jpg)
Espace des mesures vs. espace d’état
80
2
V
2
ˆ( 1| ) 4.3
(0.2 ) 0.04
3
0.3
( ) 13.8 (x=4
m
m
V/m
V
.6 )V m
x k k
z
P
k
m
s
L
1( 1| )( 1) { ( 1 ) }|T T
wP k k P kK kk C L L L
2 2 12( 1) 0.04 3 {3 0.04 3m V/m V/m m V/ 0.09m }VK k
( 1) 0.2667 m/V K k
ˆ ˆ( 1) ( 1| ) ( 1) ( 1) 4.3 + 0.2667 m m/ 0.9 =4.V 5V 40 mx k x k k K k r k
ˆˆ( 1| ) ( 1| ) V/m3 4.3 = 12.9 m Vz k k x k k Lprédiction de mesure :
innovation : ˆ( 1) ( 1) ( 1| ) 13.8 - 12.9 = 0.V V V9 r k z k z k k
calcul du gain :
V0.30.1
3 V/m
Vz m
ss
L
4.64.3 x (m)
2 m0.xs
Rapporté dans l’espace de l’état2 2
2 2
méthode 4.6 4.3 4.6 0.04 4.3 0.01: 4.5400
fusion 0.05
x Z
x z
s s
s s
4.54
même chose
mise-à-jour:
estimé directement selon mesure z
![Page 75: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/75.jpg)
Ajuster la covariance P
• Nous avons acquis de la précision sur la pose
• La covariance P va nécessairement diminuer
GLO-4001/7021 Introduction à la robotique mobile 81
( ( 1) (( 1) (1)) 1| )I K k kP k P k k L
![Page 76: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/76.jpg)
Équations du filtre de Kalman
• Prédiction (lors du déplacement)
• Mise-à-jour (mesure par capteur extéroceptif)
82
( (( 1) )1) ( 1| )I K kP k P k k L
ˆ ˆ (( 1) ( 1) () 1)1|x k x k k K k r k
ˆ( 1) ( 1) ( 1| )r k z k z k k 1( 1| ) ( 1( 1) { ( 1)}| )T T
wP k k P k kK k C k L L L
ˆ( 1| ) ˆ( 1| )z k x k kk L
ˆ ˆ( 1| ) ( )) (x k k x k u k F G
( 1| ) ( ) T
vP Ck k P k F F
note : pour alléger la notation, F=F(k), G=G(k), L=L(k)
déplacement du robot
augmentation covariance P, après déplacement
prédire ce que je devrait mesurer
compare avec mesure : innovation r(k+1)
gain optimal
combine estimé pose + mesure
ajuste covariance P, après gain info
(1)
(2)
(3)
(4)
(5)
(6)
(7)
![Page 77: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/77.jpg)
Exemples de filtre de Kalman linéaires
GLO-4001/7021 Introduction à la robotique mobile 83
![Page 78: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/78.jpg)
2 Exemples de filtre Kalman
• 1er exemple : 1 variable d’état
• 2ème exemple : 2 variables d’état
GLO-4001/7021 Introduction à la robotique mobile 84
![Page 79: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/79.jpg)
1er exemple : état 1 variable
• Chariot sur roues avance pas-à-pas, laser mesure en mètre
85
pas
laser est moins précis
No
te :
l’od
om
étri
e es
t so
uve
nt
un
e m
esu
re
loca
lem
ent
très
pré
cise
, m
ais
dér
ive.
![Page 80: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/80.jpg)
1er exemple : équations
• Propagation
86
ˆ ˆ( 1| ) ( )x k k x sk pa
2( 1| ) ( ) pasP k k P k s
1
1
1
F
G
L
commande
prop. d’état
mesure
![Page 81: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/81.jpg)
1er exemple : équations
• Propagation
• Mise-à-jour
87
ˆ ˆ( 1| ) ( )x k k x sk pa
2( 1| ) ( ) pasP k k P k s
( ( 1( 1) 1) ( |) )I K kP k P k k
ˆ ˆ (( 1) ( 1) () 1)1|x k x k k K k r k
ˆ( 1) ( 1) ( 1| )r k z k z k k
1
2( 1) { ( 1
( 1| )( 1| ) ( 1| )
( 1|)}
)w
laser
P k kP k k P kK C
kk k
P kk
s
ˆ( 1| ) ˆ( 1| )xz k k kk
1
1
1
F
G
L
(on retrouve les mêmes équations qu’avec l’exemple sur la moyenne récursive!)
commande
prop. d’état
mesure
![Page 82: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/82.jpg)
Filtre Kalman code matlab
• Code pour la boucle
GLO-4001/7021 Introduction à la robotique mobile 88
un pas = V*dt
% Simulation du systeme
xVrai = xVrai + V*dt + Sx*randn;
z = xVrai + Slaser*randn; % mesure laser
% Propagation
x = x + V*dt;
P = P + Cv;
% Mise-a-jour (update)
K = P/(P+Cw);
x = x + K*(z-x);
P = (eye(1)-K)*P;
Kal
man V constante
![Page 83: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/83.jpg)
Résultat simulation Kalman
• 1ère itération : on initialise la position du filtre à la mesure laser, P=slaser
GLO-4001/7021 Introduction à la robotique mobile 89
![Page 84: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/84.jpg)
Résultat simulation Kalman
• 1ère itération : on initialise la position du filtre à la mesure laser, P=slaser
• Au fil du temps, l’erreur du filtre diminue : moins grande dispersion que les mesures laser
GLO-4001/7021 Introduction à la robotique mobile 90
![Page 85: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/85.jpg)
Résultat simulation Kalman
GLO-4001/7021 Introduction à la robotique mobile 91
réduction d’un facteur 5 à 10 pour ce système
sur l’erreur en position, par rapport au laser
s2laser
![Page 86: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/86.jpg)
2ème Exemple : gyro + compas magnétique
• Deux capteurs placé sur un objet quelconque:
– gyroscope à taux N(0.1,0.22) deg/s (avec biais)
– compas magnétique N(0,102) deg (sans biais)
• Rotation dans un plan
• Veut fusionner les deux ensemble :
– gyro est peu bruité, mais dérive
– compas ne dérive pas, mais est bruité
GLO-4001/7021 Introduction à la robotique mobile 92
bon à court terme
bon à long terme
Proprio.
Exterio.
![Page 87: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/87.jpg)
Modèle du système : propagation
• État : angle q et vitesse angulaire q : x=[q q ]T
• Gyro : proprioceptif, équivaut à une commande
93
1 0
0 0 1gyro
tq
q q
q q
D
1
0 0
tD F
0
1
G
état à k+1
état àk
bruitcommande
sur-estime, pour tenir compte biais
.
surestimer augmente la robustesse des algos aux violations
.
2
0 0
0 16v
gyro
Cs
![Page 88: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/88.jpg)
Modèle capteur : mise-à-jour
• Je ne «mesure » qu’avec le compas extéroceptif
• Autrement dit, je ne veux pas que le gyro intervienne dans la mise-à-jour, car il est proprioceptif
94
x(k+1|kz(k+1)= = 1 0)q
L
2
w compasC s
ce que je m’attends à mesurer…
1 0 L
Notez la dimension de L : (1x2) • 1 capteur• 2 variables d’état
![Page 89: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/89.jpg)
Code matlab
95
en
bo
ucl
e
F
LG
équ
ation
s K
alman
Cv = [0 0; 0 (4*SGyro)^2];
Cw = SCompas^2;
F = [1 dT; 0 0];
H = [1 0];
B = [0 1]’;
Paramètres angle dérivée de l’angle
simu
lation
![Page 90: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/90.jpg)
Résultat simulation (20 mesures/sec)
GLO-4001/7021 Introduction à la robotique mobile 96
40 sin(0.2 )o tq trajectoire :
![Page 91: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/91.jpg)
Résultat simulation (20 mesures/sec)
GLO-4001/7021 Introduction à la robotique mobile 97
Au tout début…
Plus tard…
L’intégration du gyroscope nous donne une mesure peu bruitée et proche de la réalité
![Page 92: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/92.jpg)
Résultat simulation (20 mesures/sec)
GLO-4001/7021 Introduction à la robotique mobile 98
le gyro diverge
Au tout début…
Plus tard…
Mais l’intégrale finie par s’éloigner de la réalité, à cause de l’accumulation du biais
![Page 93: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/93.jpg)
Résultat simulation (20 mesures/sec)
GLO-4001/7021 Introduction à la robotique mobile 99
on obtient le meilleur des deux mondes : système
précis et sans dérive
Au tout début…
Plus tard…
magnétique
le gyro diverge
![Page 94: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/94.jpg)
Estimation du biais du gyroscope
• On ajoute une entrée Gbiais dans l’état :
• On le soustrait à la mesure du gyroscope :
• On ajuste la vitesse qu’on estime que
le biais change, via son « bruit » :
• Matrice de mesure :
1 0 0
0 0 11
0 0 1 0biais biais
gyro
t
G G
q q
q q q
D
biais
X
G
q
q
2
20.003
0 0 0
0 0
0 0
gyroC s
1 0
0 0
0 0 1
1
tD
F
1 0 0L utilise maintenant la valeur exact du
bruit de gyro
random walk
![Page 95: Fusion de capteurs](https://reader035.fdocument.pub/reader035/viewer/2022072212/62d99d39635eed7dc44e9874/html5/thumbnails/95.jpg)
Estimation biais : résultats
GLO-4001/7021 Introduction à la robotique mobile 102
biais réel
erreur au début (biaisé)
erreur moins biaisée