Babyfoot Vision et Stratégie - Lausanne, 17 juin 2017 EPFL - master II Yoann moret
←
→
Transcription du contenu de la page
Si votre navigateur ne rend pas la page correctement, lisez s'il vous plaît le contenu de la page ci-dessous
Projet de semestre
Section Génie Mécanique
Automatic Control Laboratory
Babyfoot
Vision et Stratégie
Yoann moret
EPFL - master II
Lausanne, 17 juin 2017
Responsable de projet : Dr Christophe salzmann
Assistant Doctorant : Luca fabietti
Projet de semestre M2 - Févr/Juin 2017 Yoann moret
ii
Projet de semestre M2 - Févr/Juin 2017 Yoann moret
iv
Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Résumé
Ce document présente les différentes avancées que j’ai pu mener durant mon projet
de semestre sur le babyfoot robotisé du laboratoire d’automatique. Le procéssus
d’acquisition de la position de la balle est présenté dans un premier temps, puis ses
limitations sont identifiées. Un filtre de Kalman a ensuite été mis en place de manière
à pallier les difficultés rencontrées lors du suivi de la balle.
Malgré ces efforts l’amélioration de l’algorithme de tracking de la balle représente
un véritable enjeu pour l’avancée du projet, et fait l’objet de la troisième partie de
ce rapport. Pour finir, une dernière section est consacrée à une brève présentation
des modifications apportées au VI de stratégie.
Ce rapport se veut volontairement détaillé dans le but de transmettre au mieux les
connaissances acquises aux prochains étudiants. Chacune de ces sections est munie
d’un guide pour utlisateur permettant au lecteur intéressé de continuer le travail
mené jusqu’ici.
v
Projet de semestre M2 - Févr/Juin 2017 Yoann moret
viProjet de semestre M2 - Févr/Juin 2017 Yoann moret
Table des matières
1 Vision de la balle 1
1.1 Obtention de la position . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Analyse des positions mesurées . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3 Explication des mesures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.4 Amélioration du modèle de distorsion . . . . . . . . . . . . . . . . . . . . . . . . 5
1.4.1 Amélioration par re-calibration . . . . . . . . . . . . . . . . . . . . . . . . 5
1.4.2 Amélioration par création d’un nouveau modèle de distorsion . . . . . . . 5
1.5 Conclusions sur la vision de la balle . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.6 Guide pour utilisateur : ajuster le modèle de distorsion . . . . . . . . . . . . . . . 8
1.6.1 Régler les paramètres du filtre . . . . . . . . . . . . . . . . . . . . . . . . 8
1.6.2 Recalibrer la caméra . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.6.3 Améliorer le modèle de distorsion . . . . . . . . . . . . . . . . . . . . . . . 8
2 Filtre de Kalman 9
2.1 Objectifs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.2 Cahier des charges . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.3 Théorie sur le filtre . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.4 Démarche . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.5 Performances du filtre . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.6 Guide pour utilisateur : Comment travailler sur le filtre . . . . . . . . . . . . . . 16
2.6.1 Analyser les sorties du filtre . . . . . . . . . . . . . . . . . . . . . . . . . . 16
2.6.2 Code Matlab pour comprendre d’influence des paramètres . . . . . . . . . 17
3 Algorithme de tracking 17
3.1 Premier algorithme . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.2 Deuxième algorithme . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.3 Faiblesses de la méthode de tracking . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.4 Guide pour utilisateur : comment améliorer le tracking de la balle . . . . . . . . . 18
3.4.1 Modifier la zone de recherche du 1er algorithme . . . . . . . . . . . . . . . 18
3.4.2 Créer un algorithme intermédiaire . . . . . . . . . . . . . . . . . . . . . . 18
3.4.3 Trouver une alternative au pattern matching . . . . . . . . . . . . . . . . 19
4 Modifications du VI stratégie 19
4.1 Présentation du VI stratégie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
4.2 Calibration des joueurs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
4.3 Tirs croisés . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
4.4 Mécanique du tir . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
4.5 Stratégie de défense . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
4.6 Guide pour utilisateur : calibration et futurs travaux . . . . . . . . . . . . . . . . 22
4.6.1 Calibration des joueurs . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
4.6.2 Prochains travaux . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
viiProjet de semestre M2 - Févr/Juin 2017 Yoann moret
viiiProjet de semestre M2 - Févr/Juin 2017 Yoann moret
1 Vision de la balle
1.1 Obtention de la position
Ce paragraphe présente les différentes étapes qui permettent à l’algorithme de trouver la
position de la balle (Fig 1.1).
Figure 1.1
Etape 1
En premier lieu la caméra située sous le babyfoot prend une photographie en couleur. Le
Laboratoire d’Automatique posséde plusieurs caméras dont les principales caractéristiques sont
présentées tableau 1.1 [1].
Table 1.1 – Principales caractéristiques des caméras du laboratoire
Caméra Couleur Résolution Images par seconde
MQ003CG CM Couleur 648 x 488 >500
MQ022CG CM Couleur 2048 x 1048 170
MQ022RG CM Noir et blanc 2048 x 1088 170
La caméra actuellement employée est la MQ003CG CM de la marque Ximea. Il semble
cependant préférable de favoriser la fréquence d’acquisition à la qualité de l’image, les autres
caméras n’ont donc pas été utilisées lors de ce projet.
1Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Etape 2
De manière à identifier la balle plus facilement, un filtre est appliqué sur l’image acquise.
Les paramètres de ce filtre sont ajustés manuellement par l’utilisateur lors du lancement
du programme. Ce dernier devra alors faire en sorte que seule la balle apparaisse en blanc sur
l’image filtrée.
Cette étape était fortement influencée par la luminosité de la pièce, et les faibles variations
de luminosité sur la surface du terrain s’avéraient être problématiques. Un dispositif de LED a
alors été mis en place par Bartélémy tremeau afin d’obtenir une lumière forte et homogène sur
l’ensemble de la surface du terrain.
Etape 3
Arrivé à ce stade, l’algorithme possède une image noire sur laquelle la balle ressort en blanc.
Cette 3ème étape consiste alors à reconnaître la balle sur la photographie en la comparant au
motif sélectionné par l’utilisateur, au moment du lancement de l’algorithme.
Le programme va alors chercher le motif correspondant dans une zone restreinte autour d’une
position estimée. S’il ne la trouve pas, il cherchera la balle sur l’ensemble du terrain à l’aide
d’un 2nd algorithme de suivi.
Cette étape est fondamentale au bon fonctionnement de l’algorithme et ses limitations
peuvent ralentir fortement le programme. La 3ème partie de ce rapport est alors consacrée à
l’étude de ces deux algorithmes de tracking qui méritent d’être optimisés.
Etape 4
Après avoir détecté le motif de la balle, son centre est identifié mais ne correspond pas à la
position physique de la balle.
La 4ème étape consiste alors à compenser le biais que cause la distorsion de l’image sur la
position mesurée. Pour celà, le VI « IMAQ Convert Pixel to Real World » de National Instrument
est employé. Son fonctionnement est détaillé section 1.4.1.
Etape 5
Un dernier VI est appelé afin de prendre en compte de changement de direction de la lumière
lorsqu’elle traverse la plaque de plexiglass. Après cette correction, la position finale de la balle
est obtenue.
1.2 Analyse des positions mesurées
Dans cette section la qualité de l’acquisition des positions de la balle est étudiée. Les mesures
issues d’un tir le long d’une diagonale du babyfoot sont présentées Fig.1.2.
Lorsque l’on zoom sur ces données, il apparaît que les positions sont disposées en escalier.
Ce phénomène vient du fait qu’aucune valeur des ordonnées n’est prise en deux pixels, chaque
marche correspond donc à une succession de mesures ou la balle à été identifiée à la même
ordonnée.
2Projet de semestre M2 - Févr/Juin 2017 Yoann moret
La différence entre les données receuillies et la trajectoire physique de la balle vient donc de
la pixelisation du terrain, qui demeure inévitable. L’algorithme permettant de trouver la balle
est donc jugé performant lors d’un tir en diagonale.
(a) Aperçu global des mesures (b) zoom sur les mesures
Figure 1.2 – Mesures receuillies lors tir le long d’une diagonale du babyfoot
Plusieurs mesures ont ensuite été prises à différentes abscisses lors d’un tir droit le long de
la largeur du babyfoot, elles sont présentées Fig.1.3.
Figure 1.3 – Mesures prises lors d’un tir le long de la largeur du babyfoot.
3Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Le lecteur est invité à se pencher sur l’échelle graphique adoptée, à échelle réelle les données
semblent tracer une droite. La trajectoire receuillie peut être surprenante car elle est consti-
tuée d’une succession d’arcs de cercle alors que la trajectoire physique était rectiligne. Lorsque
les mêmes tests sont effectués dans la longueur du babyfoot, les résultats sont obtenus sont
semblables.
1.3 Explication des mesures
Les mesures de la position de la balle semblent donc être cohérentes lors d’un tir en diagonale,
mais correspondent mal à la trajectoire physique lors d’un tir droit.
Pour expliquer ce phénomène, il faut retracer les différentes étapes permettant d’obtenir les
mesures. Ces étapes sont illustrées Fig.1.4 et sont détaillées ci-dessous.
Figure 1.4 – Illustration du processus d’acquisition des mesures. Les valeurs sur ce graphique
ne correspondent pas à des mesures réelles.
— La trajectoire physique de la balle est une ligne droite, représentée en bleu Fig.1.4.
— L’effet de distorsion fait que la trajectoire vue par la lentille est courbée. Cette trajecoire
est représentée en rouge.
— Une fois la photographie prise, chaque point de l’espace est pixelisé. La courbe rouge est
alors discrétisée et adopte cette structure en escalier, représentée en jaune.
— L’étape n°4 du processus d’acquisition permet de corriger l’effet de distorsion de la lentille.
Appliquée aux mesures discrétisées, cette correction permet d’obtenir les mesures qui ont
été observées, en violet Fig.1.4.
Par ailleurs, cet effet ne se fait pas ressentir lorsque la trajectoire de la balle est diagonale sur le
4Projet de semestre M2 - Févr/Juin 2017 Yoann moret
terrain. Cela s’explique par le fait que dans ce cas, les vitesses de la balle selon chaque axe sont
élevées : entre chaque mesure la balle aura changé de pixel selon ~x et ~y . La trajectoire pixélisée
sera alors beaucoup plus fidèle à la trajectoire analogique et en particulier, ne contiendra que très
peu cette structure en escalier. Lorsque le VI corrigeant la distorsion sera appelé, il retournera
alors des données correctement alignées.
1.4 Amélioration du modèle de distorsion
Les mesures observées section 1.2 sont, à faible échelle, et seulement dans le cas de tirs droits,
de faible qualité. Lorsque le robot les utilise pour jouer, les quelques millimètres d’erreur sur la
position ne l’empêche pas de se placer devant la balle. Cependant, si ces mesures ne sont pas
améliorées, l’ordinateur sera toujours limité en précision et sera incapable de réaliser les mêmes
actions qu’un joueur humain.
Le but de cette section est donc d’améliorer la qualité des mesures de la position. L’effet
de distorsion dépend seulement de la lentille et la pixelisation est pour sa part régulée par la
résolution de la caméra. L’algorithme de correction de la distorsion est alors étudié dans le but
de pouvoir l’optimiser.
1.4.1 Amélioration par re-calibration
Le VI « IMAQ Convert Pixel to Real World » [2] est employé dans le programme afin de
redresser l’image retournée par la caméra. De manière à compenser la disorsion causée par la
lentille, une correction ∆→
−
x est alors appliquée à chaque position →
−
x :
→
−
x corrigé = →
−
x mesuré + ∆→
−
x
National Instrument fournie le VI « IMAQ Get Camera Parameters VI » [3] permettant de
déterminer la correction à appliquer en chaque point de l’image. Ainsi plusieurs modèles de
correction ∆→ −x sont disponibles dont les coefficients sont déterminés expérimentalement. Pour
celà une image à priori connue est comparée avec une photographie de cette même image ayant
subi une distorsion.
Cette étape de calibration a été réalisée avec les différents modèles possibles. Il en ressort que
le modèle polynomial à 3 termes est le plus adapté à notre problème. En recalibrant la caméra,
les coefficients du polynôme ont été recalculés mais la qualité des mesures n’a cependant pas pu
être améliorée.
1.4.2 Amélioration par création d’un nouveau modèle de distorsion
Malgré la calibration du VI de national Instrument, l’effet de la distorsion sur les mesures
reste prononcé et mériterait d’être minimisé. Le modèle implémenté dans ce VI étant de faible
degré, plusieurs essais ont été menés afin d’établir un modèle plus complet qui permettrait
d’améliorer la qualité des mesures.
Pour celà, les mesures d’un tir droit ont été extraites et une régression linéaire (qui correspond
supposément à la trajectoire physique) à été tracée. Pour un certain modèle de distorsion, tous
5Projet de semestre M2 - Févr/Juin 2017 Yoann moret
les coefficients du modèle ont été essayés. Sont conservés les coefficients qui minimisent l’erreur
entre les mesures corrigées et la régression linéaire.
Cette procédure a été répétée pour de nombreux modèles de distorsion à savoir :
— Des modèles de distorsion radiale :
n
X
∆~x = ki ri n = 1, 2....6
i=1
Avec :
p
r = (x − x0 )2 + (y − y0 )2
— Des modèles de distorsion elliptique :
n i
∆x X r/l
= ki n = 1, 2....6
∆y r/L
i=1
Avec : l : longueur du babyfoot
L : largeur du babyfoot
— D’autres modèles variés (logarithmiques, correction multiplicative ect ...).
Le meilleur modèle de correction est comparé avec celui du VI de National Instrument Fig.1.5.
Il apparaît que la différence est à peine perceptible, et donc que la distorsion de l’image était
correctement corrigée.
6Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Figure 1.5 – Comparaison des mesures corrigées par le VI de National Instrument et de celles
corrigées avec nos modèles
1.5 Conclusions sur la vision de la balle
Le processus d’acquisition de la position de la balle, bien qu’imparfait, permet au robot
d’aopter une stratégie relativement simple. Des imprécisions de l’ordre de quelques millimètres
empêchent cependant le robot de jouer avec la même justesse qu’un joueur humain. À l’avenir,
lorsque des stratégies plus poussées seront mises en place, il sera intéressant d’affiner la mesure
de la position de la balle.
Pour celà, il est tout d’abord possible de choisir une caméra avec une meilleure résolution, à
condition que le temps de traitement de l’image reste toutefois raisonnable. Une autre possibilité
pourrait être de trouver une lentille qui impose une plus faible distorsion. Les algorithmes utilisés
semblent quand à eux être déjà performants. En particulier la correction de la distorsion basée
sur une comparaison expérimentale n’a pas pu être améliorée.
Une ultime solution serait de repenser entièrement le système de vision en remplaçant par
exemple, la caméra par des capteurs laser.
7Projet de semestre M2 - Févr/Juin 2017 Yoann moret
1.6 Guide pour utilisateur : ajuster le modèle de distorsion
1.6.1 Régler les paramètres du filtre
Lors du lancement du programme, les paramètres du filtre doivent être entrés manuellement
par l’utilisateur. Le système « Hue/Saturation/Lightness » doit alors être ajusté de manière
à faire ressortir au mieux la balle sur le terrain. Des valeurs typiques de ces facteurs sont
présentées ci-dessous. Il est vivement conseillé d’utiliser les LED et de les alimenter avec un
courant d’environ 0.7 ampères.
1.6.2 Recalibrer la caméra
La calibration de la caméra permet d’ajuster le modèle de distorsion en se basant sur la
comparaison d’une image à priori connue, et d’une image ayant subi la distorsion. Il est important
pour l’utilisateur de réajuster ce modèle en suivant la procédure de calibration à chaque fois que
la table de jeux ou la caméra est déplacée.
La procédure à suivre est décrite par National Instrument [4] mais est notamment détaillée
étape par étape dans le rapport « Upgrade of the babyfoot vision control system » [5]. Le
laboratoire d’Automatique possède la grille d’étalonnage nécessaire à cette calibration.
(a) Grille d’étalonnage avant distorsion (b) Grille d’étalonnage après distorsion
Figure 1.6 – Grille d’étalonnage à utiliser
1.6.3 Améliorer le modèle de distorsion
La procédure décrite section 1.4.2 peut être répétée si l’utilisateur pense connaître un meilleur
modèle de distorsion. Pour celà, il peut utiliser le code Matlab fourni en annexe en corrigeant
les données selon un nouveau modèle. Le code Matlab retournera alors les meilleures valeurs des
coefficients multipliant chaque terme du modèle.
8Projet de semestre M2 - Févr/Juin 2017 Yoann moret
2 Filtre de Kalman
2.1 Objectifs
A l’heure actuelle, l’algorithme a accès à la position de la balle mais ne connaît pas son
vecteur vitesse. Cette information permettrait d’anticiper les positions futures de la balle, et
donc d’augmenter la réactivité du robot. Connaître la direction qu’emprunte la balle ouvrirait
également la porte à de nouvelles stratégies plus avancées que celles actuellement implémentées.
Le calcul de la vitesse n’est toutefois pas si commode. En se penchant sur les mesures de la
position, il est aisé de se convaincre que des différences finies ne peuvent pas être adoptées. Un
filtre de Kalman a alors été mis en place de manière à pallier cette difficulté.
2.2 Cahier des charges
Le filtre de Kalman devra remplir le cahier des charges suivant pour pouvoir être efficace :
— Ne pas être sensible à la structure particulière des données qu’il reçoit. Le filtre doit être
robuste aux changements brusques de direction qui sont intrinsèques aux mesures de la
position.
— Être capable de s’adapter aux multiples discontinuités que contient la trajectoire d’une
balle de babyfoot. Le filtre doit cette fois être souple lorsqu’il détecte un changement de
direction réel.
9Projet de semestre M2 - Févr/Juin 2017 Yoann moret
— Le filtre doit savoir comment se comporter lorsq’il perd la balle. Cette situation apparaît
si la balle se situe sur les rebords du terrain (zone en rouge).
— Il arrive fréquemment que la caméra renvoie plusieurs fois d’affilé la même position.
Le filtre doit pouvoir identifier cette situation et la différencier du cas où la balle est
physiquement arrêtée.
2.3 Théorie sur le filtre
Le filtre de Kalman est un estimateur récursif permettant de traiter des mesures bruitées.
Une des hypothèses nécessaire à l’utilisation de ce filtre est que le bruit de mesure soit blanc
et gaussien. Cette hypothèse n’est cependant pas respectée car les positions de la balle sont
disposées « en escalier ». Le filtre de Kalman sera tout de même utilisé puisque l’expérience
montre qu’il est malgré tout capable de converger correctement. Son fonctionnement se déroule
en deux étapes.
La première d’entre elles consiste à établir une prédiction de la position mesurée et de la
matrice P [6] :
→
−
x =F →−x + B ~u (1)
k|k−1 k k−1|k−1 k k
Pk|k−1 = Fk Pk−1|k−1 FkT + Qk (2)
Ici, la notation Ai|j désigne la valeur de la matrice A à l’instant i, estimée avec les informa-
tions connues à l’instant j. L’indice « k | k − 1 » correspond donc à une prédiction.
La 2nde étape, dite de mise à jour, est la suivante [6] :
→
−
yk=→
−
z k − Hk →
−
x k|k−1 (3)
Sk = Hk Pk|k−1 HkT + Rk (4)
Kk = Pk|k−1 HkT Sk−1 (5)
→
−
x k|k = →
−
x k|k−1 + Kk →
−
yk (6)
Pk|k = [I − Kk Hk ] Pk|k−1 (7)
10Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Avec pour notations :
~x Estimation de l’état. Dans notre cas c’est un vecteur contenant les positions et les vitesses.
F Matrice qui relie l’état au temps k − 1 à l’état au temps k.
~u Entrée de commande.
B Matrice qui relie l’entrée de commande → − u à l’état →
−
x.
P Matrice de la covariance de l’erreur.
Q Matrice de covariance du bruit de process.
~z Mesures du process au temps k.
H Matrice qui relie l’état →
−
x à la mesure ~z.
R Matrice de covariance du bruit de mesure.
Cet algorithme compare donc une prédiction de l’état avec une mesure qu’il reçoit et estime
a posteriori le vecteur d’état en question. Les matrices de covariance de l’erreur lui permettent
de pondérer ces deux informations en fonction de leur fiabilité respective.
2.4 Démarche
Etape de prédiction
L’étape de prédiction (equations 1 & 2) nécessite un modèle de la trajectoire de la balle. Ce
modèle est cependant difficile à déterminer étant donné que les rebonds sur le terrain ne sont
pas modélisables (géométrie des joueurs complexe et précision insuffisante sur la position).
Il a donc été choisi de considérer que la trajectoire de la balle était une succession le lignes
droites interrompues par des rebonds. Les entrées de commande ne sont alors pas modélisées
(~u = 0, B = 0) et la matrice F prend pour valeur :
1 0 δt 0
0 1 0 δt
F = 0 0 1 0
0 0 0 1
Où δt désigne le temps écoulé entre deux mesures.
Initialisation du filtre
Le filtre de Kalman étant un estimateur récursif, a besoin d’une première valeur de l’état.
Puisque l’état contient x y ẋ ẏ tandis que la mesure ne contient que les positions
x y , enclencher le filtre après la prise de la 2ème mesure n’est pas suffisant. Bien que
les valeurs de xk−1 et yk−1 seront connues, il faudra aussi fournir les valeurs des vitesses.
Cette étape qui peut paraître anecdotique est en réalité importante car elle aura lieu à
chaque fois que la balle quitte la surface visible du terrain. De plus, fournir une vitesse initiale
erronnée altérera fortement le filtre car il la considérera comme acquise et prendra du temps à
s’en détacher.
À ce stade il est nécessaire de garder à l’esprit que les mesures retournées par la caméra ne
permettent pas d’estimer une première vitesse par différences finies. Deux solutions sont alors
envisageables :
11Projet de semestre M2 - Févr/Juin 2017 Yoann moret
— Fixer →
−
v 0 = 0.
Ce choix neutre permet de ne pas donner une direction préférentielle au filtre mais lui
fait sous-estimer l’avancée de la balle pour les premières estimations.
— Donner une incertitude quasi-infinie à → −
v 0.
→
−v 0 est calculé par différences finies mais le filtre comprend que cette information n’est
pas fiable
Ces deux possibilités ont été essayées sur les mêmes mesures, Fig 2.1.
Figure 2.1 – Filtre de Kalman appliqué lors d’un tir droit avec les deux méthodes d’initialisa-
tion.
Il apparaît alors que fixer →
−
v 0 = 0 est préférable car cela permet au filtre de ne pas suivre
aveuglément la structure « en escalier » des premières mesures qu’il reçoit. Ce choix a donc été
adopté.
Doublons des mesures
Il arrive fréquemment que l’algorithme de vision retourne plusieurs fois la même position alors
que la balle est en mouvement. Cette situation peut être détectée simplement en comparant
les positions au temps k et au temps k − 1. Si elles sont identiques, aucune opération n’est
appliquée sur le filtre mais le temps écoulé entre deux mesures doit en revanche être accumulé.
Cette solution revient donc à ne pas considérer une mesure si elle est identique à la précédente
car l’algorithme la considère comme erronnée.
12Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Celà peut cependant poser problème dans le cas où la balle est physiquement arrêtée. Le
filtre devrait alors pouvoir identifier ce cas précis et se réinitialiser car les données avant et après
l’arrêt de la balle ne sont pas corrélées.
Pour cette raison une condition a été ajoutée au VI. Si la position mesurée reste la même
pendant plus d’un certain temps (appelé Reset Time), alors le filtre se réinitialisera au prochain
changement de position.
La valeur de ce Reset Time peut être ajustée par l’utilisateur.
Sortie de la partie visible du terrain
Lorsque la balle est poussée sur les rebords du terrain elle sort du champ de vision de la
caméra. L’algorithme de détection de la balle renvoie alors sa dernière position visible jusqu’à
ce qu’elle réapparaisse. On observe ainsi un large saut dans les valeurs des positions.
Dans ce cas de figure, les trajectoires de la balle avant et après sa disparition ne sont que
très rarement corrélées. Il est en effet très fréquent que la balle heurte le rebord du terrain et
qu’elle change de direction.
Cette situation étant facile à identifier, le filtre est réinitialisé à chaque fois que la balle sort
du terrain visible. Pour celà, la réinitialisation a lieu si la différence entre la position prédite et
la mesure est jugée trop élevée. Cette condition se traduit par :
xk|k−1 − xmesuré
> Reset distance
xk|k−1 − ymesuré
La valeur de Reset distance peut être ajustée par l’utilisateur.
Attention : Dans le programme du babyfoot, le booléen « ball found » est faux si le 1er
algorithme de suivi n’a pas trouvé la balle, mais aucune information n’est donnée sur le 2ème .
Ce booléen ne peut donc pas être directement utilisé pour détecter la perte de la balle.
Rebond sur le terrain
L’étape de prédiction du filtre nécessite d’établir un modèle de trajectoire de la balle. Il a été
choisi précédemment de considérer que la balle se déplace selon une succession de lignes droites.
Cependant, si un rebond a lieu sur le terrain, le filtre va essayer de corréler les mesures d’avant
et d’après le rebond comme si elles faisaient partie d’une même droite, Fig.2.2.
13Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Figure 2.2 – Filtre de Kalman initial appliqué à une trajectoire avec rebond. La balle s’est
déplacée de la droite vers la gauche du terrain. Le filtre est donc biaisé après le rebond.
Il est nécessaire dans ce cas de relaxer le modèle de prédiction en informant le filtre qu’il doit
faire d’avantage confiance aux mesures acquises plutôt qu’à sa prédiction. Pour celà, la matrice
de covariance de l’erreur est corrigée :
P ∗ = P + Padd
Un rebond sur le terrain n’est cependant pas facile à détecter car il n’y a pas de saut distinct
entre deux mesures consécutives de la position. Une déviation de la trajectoire pourrait tout à
fait provenir de la structure des données.
La solution adoptée est alors d’appliquer une correction sur la matrice P lorsque la différence
entre la position prédite et la mesure est jugée trop élevée. Cette condition se traduit par :
xk|k−1 − xmesuré
> Incertitude distance
xk|k−1 − ymesuré
La valeur du paramètre Incertitude distance peut être ajustée par l’utilisateur mais doit
être inférieure à celle de Reset distance.
Il est à présent nécessaire de choisir quelle matrice Padd permettra de relaxer la prédiction.
Deux constatations ont permis de faire ce choix :
— L’incertitude sur les vitesses doit être plus élevée que sur les positions : lors d’un rebond,
le vecteur vitesse change fortement de direction tandis que les positions restent proches
les unes des autres.
— Lors d’un rebond sur une paroie horizontale, seule la vitesse et la position verticale
changent. Il est donc préférable que l’incertitude sur le modèle soit calculée indépendam-
ment de la différence entre prédiction et observation sur chaque axe.
La matrice Padd est alors calculée de cette façon 1 :
1. L’indice mes signifie mesuré
14Projet de semestre M2 - Févr/Juin 2017 Yoann moret
| xk|k−1 − xmes |· α 0 0 0
0 | yk|k−1 − ymes |· α 0 0
Padd = +Pf ixe
0 0 | xk|k−1 − xmes |· β 0
0 0 0 | yk|k−1 − ymes |· β
Avec pour notations :
α Facteur d’incertitude position
β Facteur d’incertitude vitesse
Pf ixe Une incertitude indépendante de ces différences est ajoutée.
La valeur de ces 3 paramètres peut être modifiée. Il est tout de même conseillé de garder
α < β.
2.5 Performances du filtre
Le filtre implémenté à été testé sur une séquence de mesures où la balle est tout d’abord
sortie du terrain visible, puis a rebondi sur un joueur. Les résultats sont présentés Fig.2.3. Les
points où la matrice Padd est ajoutée au modèle sont appelés « Relaxation » sur la figure.
(a) Positions du filtre et des mesures (b) Vecteur vitesse calculé par le filtre
Figure 2.3 – Analyse de la fiabilité du filtre de Kalman. La balle est sortie du terrain en x = 100
et a renbondi sur un joueur en x = −200.
Le choix des paramètres du filtre présentées a été fait de manière empirique et est présenté
Tableau 2.1.
15Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Table 2.1 – Valeurs des paramètres du filtre de Kalman
40 mm
Reset distance 2 0 0 0
Incertitude distance 12 mm 0 2 0 0
Pf ixe
Facteur d’incertitude position 0.5
0 0 10 0
Facteur d’incertitude vitesse 6 0 0 0 10
Il apparaît alors que la position calculée par le filtre n’a pas de difficultés à suivre les mesures
(Fig 2.3.a). La distance les séparant ne dépasse d’ailleurs 1cm qu’une seule fois sur cette séquence.
L’objectif du filtre de Kalman étant d’obtenir le vecteur vitesse, nous porterons un plus grand
intérêt aux résultats Fig 2.3.b. Il apparaît alors que le vecteur vitesse reconnaît la direction de
la balle après avoir convergé. La première discontinuité (en x = 100 ) ne pose pas de problèmes
au filtre car il identifie que la balle est sortie du terrain et se détache des anciennes mesures
en se réinitialisant 2 . En revanche, en x = −200, un rebond dévie la trajectoire de la balle et le
filtre a besoin de plusieurs mesures avant de converger.
Pour pallier cette difficulté, une option a été ajoutée au filtre. Si l’utilisateur active le booléen
« différences finies », les vitesses calculées après chaque relaxation seront évaluées par différences
finies. Le risque étant que la structure des données fausse le résultat, la vitesse est calculée sur
une moyenne des 3 derniers schémas d’Euler centrés.
2.6 Guide pour utilisateur : Comment travailler sur le filtre
2.6.1 Analyser les sorties du filtre
L’utilisateur cherchant à optimiser les paramètres du filtre peut extraire les valeurs des
mesures de la caméra et des estimations du filtre. Pour celà, il est nécessaire d’ouvrir le VI
« KalmanVisionTestKalman » dans lequel un tableau contient les valeurs suivantes :
— Les positions x et y mesurées par la caméra
— Les positions x et y estimées par le filtre
— Les vitesses ẋ et ẏ estimées par le filtre
— Un booléen pouvant être choisi, tel que : les points de réinitialisation, les points de
relaxation du filtre ect...
Une fois les données acquises, elles doivent être copiées, puis collées dans un tableau (ayant
la même structure) présent dans le VI « DisplayPositions ». L’utilisateur exécute ensuite le
programme et les données sont affichées dans les graphes du même VI. Depuis ces graphiques,
les données sont exportées à l’aide de la commande : Data operations - Export data - Export
to Clipboard. Il est alors possible de les récupérer en dehors de Labview en les collant dans le
bloc-notes.
2. La réinitialisation n’est pas visible Fig 2.3.1 car elle est cachée par un point de relaxation qui a lieu au
même moment
16Projet de semestre M2 - Févr/Juin 2017 Yoann moret
2.6.2 Code Matlab pour comprendre d’influence des paramètres
Un code Matlab permettant de réaliser les opérations du filtre de Kalman (eq. 1 à 7) est
donné en annexe. Ce code ne permet pas de gérer les discontinuités sur le terrain mais constitue
un outil pour que l’utilisateur prenne connaissance de l’influence des paramètres du filtre.
3 Algorithme de tracking
L’algorithme de tracking permet de détecter le motif de la balle sur l’image en noir et blanc
(3ème étape, Fig.1.1). Cette courte section présente son fonctionnement car cette partie du
programme est particulièrement coûteuse en terme de temps de calcul. Il semblerait en effet
qu’à ce jour, ce soit cet algorithme qui limite les performances du robot.
3.1 Premier algorithme
Une fois l’image filtrée obtenue, un premier algorithme de tracking est appelé (IMAQ Track
Objects VI) [7].
Ce VI cherche à identifier la balle sur une zone de recherche restreinte par pattern matching.
Celà signifie qu’il va comparer une partie de l’image acquise avec le motif sélectionné au lance-
ment du programme et, si le « score de corrélation » dépasse la tolérance « Minimum Score »,
va considérer que la balle est trouvée.
L’information indiquant à l’algorithme dans quelle zone chercher est contenue dans l’entrée
« Object Tracking Session ». Il semblerait que la balle soit recherchée autour de son ancienne
position, ou autour d’une position estimée.
Chercher la balle dans une zone restreinte permet au VI de s’executer en environ 2ms mais
implique que si la balle a trop bougé d’une image à l’autre, l’algorithme ne la trouve plus.
Ce VI est donc un outil très rapide permettant de trouver la balle, mais ne la détecte pas
dans tous les cas.
3.2 Deuxième algorithme
Lorsque la balle se déplace trop vite sur le terrain, elle sort donc de la zone d’inspection du
VI présenté ci-dessus, et un 2nd algorithme de tracking est appelé.
17Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Le VI « IMAQ Match Pattern 4 VI » [8] utilise également la méthode de pattern matching,
mais cette fois-ci sur tout le terrain. Son fonctionnement est donc plus coûteux en puissance de
calcul, il nécessite au minimum 30ms pour trouver la balle.
Une fois identifiée, la position est transmise au VI « IMAQ Set Initial Object Tracking
Position VI » afin d’ajuster la zone de recherche du 1er algorithme pour la prochaine itération.
3.3 Faiblesses de la méthode de tracking
La méthode utilisée pour identifier le motif de la balle est efficace dans la mesure où elle
permet de trouver sa position dans la plupart des cas. Cependant, l’expérience montre que
lorsque le robot joue, la balle est presque toujours trop rapide pour le 1er algorithme. La position
est alors détectée par le 2nd VI et n’est obtenue que 30 fois par seconde avec un délai de 30ms
entre la capture de l’image et l’acquisition de la position.
En pratique le robot est donc très peu réactif et encaisse des buts facilement (voir vidéo
fournie avec le rapport).
Le tracking de la balle mériterait fortement d’être amélioré car, de part ses limitations, il
empêche d’exploiter la rapidité de la caméra et des moteurs.
3.4 Guide pour utilisateur : comment améliorer le tracking de la balle
Cette section présente des suggestions sur lesquelles les prochains étudiants pourraient se
pencher dans le but d’améliorer la méthode de tracking de la balle. Acune de ces solutions
n’ayant été implémentée, il est possible que certaines d’entre elles se révèlent être utopistes.
3.4.1 Modifier la zone de recherche du 1er algorithme
Le principal défaut de la méthode de tracking utilisée est que le 1er algorithme perd la balle
trop facilement. Pour y remédier, il serait intéressant de mieux définir la zone de recherche du
1er algorithme.
En agrandissant cette zone de recherche et en la centrant sur la position prédite par le filtre de
Kalman, le 2ème algorithme serait employé moins souvent. Il serait donc nécessaire de modifier
l’entrée « Object Tracking Session In », ce qui n’est peut être pas possible.
3.4.2 Créer un algorithme intermédiaire
Cette solution consisterait à ne pas passer directement au 2ème algorithme (peu viable en
terme de temps d’exécution) lorsque la balle n’est pas trouvée par le 1er . Ainsi, un VI inter-
médiaire serait utilisé pour chercher dans une zone plus large autour d’une position prédite.
18Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Cette méthode serait donc utilisée dans le cas où la solution proposée section 3.4.1 ne serait pas
implémentable.
La mise en place de cet algorithme pourrait se faire à l’aide du 2ème VI de tracking. L’entrée
« Region Of Interest » permet en effet d’ajuster directement la zone de recherche.
3.4.3 Trouver une alternative au pattern matching
Jusqu’ici, seuls des algorithmes utilisant le pattern matching ont été présentés. Bien que très
fiable, cette méthode s’avère être coûteuse en temps de calcul, et ralentie donc le robot.
Etant donné que la balle ressort très nettement sur l’image filtrée, d’autres algorithmes
pourraient se montrer efficaces. Au lieu de calculer de nombreuses corrélations entre l’image
de référence et l’image acquise, il pourrait être possible d’identifier les pixels blancs sur la
photographie. Le logiciel NI Vision Builder propose par exemple la fonction « Detect Objects ».
Il est possible d’ajuster des paramètres de manière à ce qu’un seul objet soit détecté. Le
temps d’exécution de cet algorithme a été évalué à 7ms pour une recherche sur l’intégralité de la
photographie. Il serait donc envisageable de remplacer le 2ème VI de tracking par cette fonction,
si elle s’avère fiable et peu coûteuse en temps de calcul.
4 Modifications du VI stratégie
4.1 Présentation du VI stratégie
Le VI de stratégie permet, à partir des positions de la balle et des joueurs sur le terrain,
de déterminer les consignes à donner au controlleur. La stratégie implémentée à ce jour reste
simple et mérite d’être améliorée. Les étudiants se penchant sur ce VI sont invités à tout d’abord
prendre connaissance des limitations liées à la vision de la balle.
4.2 Calibration des joueurs
Le VI de stratégie permet aux joueurs de chaque rangée de se placer devant la balle. Le choix
de ce joueur dépend de la position de la balle le long de l’axe ~y ( dans la largeur du babyfoot ).
Une fois que ce choix est fait, la position du joueur sélectionné est obtenue selon la formule :
yjoueur = ybarre + ∆y (8)
Ici, ∆y correspond à la position de joueur sur la barre, et est donc propre à chaque joueur.
Il est alors nécessaire de réajuster cette valeur manuellement à chaque fois que la barres sont
19Projet de semestre M2 - Févr/Juin 2017 Yoann moret
retirées des accouplements. Si ce ∆y n’est pas exacte, les joueurs seront légérement décallés par
rapport à la balle.
La deuxième valeur nécessitant d’être calibrée est la position de chaque barre selon l’axe
~x (dans la longueur du babyfoot). Le VI compare cette position à celle de la balle et, si elles
sont est assez proches, décide d’enclencher un tir. Il est donc nécessaire cette l’information soit
correcte afin que le robot tire lorsqu’il en a la possibilité.
4.3 Tirs croisés
En positionnant chaque joueur exactement devant la balle, le robot réalisera un tir droit
même s’il n’est pas en face des cages adverses. De manière à ce qu’il cadre plus souvent ses tirs,
un petit offset a été ajouté à la commande sur la position linéaire des joueurs :
yballe
0
yjoueur = yjoueur + (9)
k
Cet offset permet de décaller les joueurs vers l’extérieur du terrain et, au moment de tirer,
de taper la balle avec le coin de la figurine (Fig.4.1) de manière à réaliser un tir croisé.
L’offset augmente donc linéairement avec la position de la balle (pour croiser d’avantage son
tir si la balle et plus loin des cages) et dépend d’un facteur k propre à chaque barre. Plus une
barre est loin du but adverse, moins l’angle de tir doit être important, et plus la valeur de k est
élevée.
(a) Position du joueur sans offset (b) Position du joueur avec offset
Figure 4.1 – Offset permettant de réaliser un tir croisé
En implémentant ce VI, il nous a semblé que le robot n’était pas capable de réaliser un
mouvement en translation et en rotation en même temps. L’offset permettant de croiser les
tirs du robot ne peut alors pas être appliqué qu’au moment de tirer, sinon le déacallage serait
appliqué après le tir.
20Projet de semestre M2 - Févr/Juin 2017 Yoann moret
4.4 Mécanique du tir
Jusqu’ici lorsque le robot tirait, il faisait un mouvement de va-et-vient vers l’avant. Il arrivait
cependant que de façon imprévue, le robot tape la balle vers son camp au moment où il ramenait
ses joueurs à la verticale. Cette situation menait alors très fréquemment à des auto-goals.
Il a alors été décidé de changer sa mécanique de tir en faisant toujours tourner les joueurs
vers l’avant. Celà signifie que lors d’un tir, le robot revient à sa position verticale en faisant un
tour entier. Le fait de faire un tour est autorisé par les règles du babyfoot à condition que le
robot ne fasse qu’un tour. Il est possible de régler manuellement le nombre de tours maximal
que le robot exécute.
Cette mécanique de tir a été appliquée à tous les joueurs sauf au goal. Il était en effet
intéressant de garder ce mouvement de va-et-vient pour le gardien car il permet d’incliner le
joueur vers l’arrière dans le cas où la balle viendrait de côté, et donc de l’empêcher d’entrer dans
les cages.
4.5 Stratégie de défense
Lorsque le joueur adverse possédait la balle avec ses attaquants, il était aisé de marquer
contre le robot. Sa stratégie de défense a donc été adaptée de manière à éviter les buts facilement
encaissés.
La première situation étudiée permet de se défendre si le joueur adverse détient la balle avec
un des attaquants latéraux. Le robot essayait de positionner tous ses joueurs devant la balle
(Fig.4.2.a) et il suffisait de réaliser un tir croisé quelque peu rapide pour marquer systématique-
ment. Il a donc été décisé de décallé le défenseur vers l’intérieur du terrain de manière à fermer
l’angle de tir (Fig.4.2.b). La position du défenseur est ajustée selon la formule :
yballe
y déf enseur = yballe + (10)
k2
Où la valeur de k2 a été déterminée empiriquement. Cette stratégie semble être très efficace
indépendamment du style de jeu de l’attaquant.
21Projet de semestre M2 - Févr/Juin 2017 Yoann moret
(a) Ancienne position de défense, mal adaptée. (b) Nouvelle position de défense, plus performante.
Figure 4.2
La deuxième situation analysée consiste à se défendre lorsque la balle est possédée par l’at-
taquant central de l’adversaire. Ici, le risque n’est pas d’encaisser des tirs croisés, mais est que
l’attaquant décalle la balle rapidement et tire dans les buts avec son joueur central. Dans ce
cas précis, il est fréquent que le 1er algorithme de tracking perde la balle et que le robot ne
réagisse pas assez vite à ce décallage. De manière à bloquer ces tirs tout en étant en retard sur
la position de la balle, il a été décidé que le défenseur suive l’attaquant adverse. La position de
la barre adverse étant mesurée avec des lasers, le robot est très réactif et ne se laisse pas prendre
de vitesse. Afin d’éviter toute sorte de feintes, le gardien se positionne quand à lui devant la
balle.
Cette 2ème stratégie s’est révélée être très efficace lorsque le robot a affronté le champion.
Elle n’est cependant pas très utile contre un joueur débutant qui n’effectuera pas de décallage,
et préférera tirer sans controller la balle. Il est donc envisageable qu’à l’avenir, le robot adapte
sa stratégie en fonction de son adversaire.
4.6 Guide pour utilisateur : calibration et futurs travaux
4.6.1 Calibration des joueurs
La valeur de la position des joueurs sur la barre présentée section 4.2 doit être ajustée à
chaque fois que les accouplements sont enlevés. Sans celà, les joueurs ne seront plus positionnés
correctement par rapport à la balle, et les tirs croisés ne seront plus cadrés.
Pour celà, il est tout d’abord nécessaire de désactiver l’offset permettant de réaliser ces
tirs croisés. Les joueurs sont alors sensés se positionner exactement en face de la balle, ce qui
permet de savoir s’ils sont ajustés. Les seules valeurs à changer sont les décallages ∆y (eq.8), les
conditions sur la valeur de la position de la balle n’étant pas successibles d’évoluer. Une fois les
∆y ajustés indépendamment sur chaque joueur, il est possible de réactiver l’offset pour croiser
les tirs du robot.
22Projet de semestre M2 - Févr/Juin 2017 Yoann moret
4.6.2 Prochains travaux
L’amélioration du VI de stratégie présente un grand intérêt pour l’avancée du projet mais
doit se faire en connaissance des limitations du robot. De nombreuses possibilités deviendraient
réalisables en augmentant par exemple la vitesse d’exécution de l’algorithme de tracking.
Il serait premièrement possible de réaliser un contrôle de la balle au cours du jeu. Cette
technique a été étudiée par Bartélémy tremeau et semble implémentable pour peu que la
réactivité du robot soit améliorée. À partir de cette étape, il serait intéressant d’apprendre
différents tirs au robot (présentés dans les vidéos jointes à ce projet), qui s’avéreraient être très
efficaces grâce à la rapidité des moteurs.
Sans avoir recours au contrôle de la balle mais en admettant que le robot soit plus réactif,
il serait également très utile de pouvoir contrer les tirs du défenseur adverse. En orientant
l’attaquant correctement et en implémentant un tir « à la volée » (qui se différenciera du tir
en tour complet), l’adversaire aura beaucoup de mal à dégager la balle lorsqu’il la possède en
défense.
Remerciements - Je souhaitais remercier le Dr. Christophe salzmann, qui m’a proposé
ce projet, m’a accueilli dans son laboratoire, et m’a apporté son très précieux soutien ainsi que
son enthousiamse contagieux. Je remercie également l’ensemble du Laboratoire d’Automatique,
et plus particulièrement M. Luca fabietti pour m’avoir guidé tout au lond de ce semestre.
23Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Bibliographie
[1] https ://www.ximea.com/files/brochures/xiQ-USB3-Vision-2015-brochure.pdf
[2] http ://zone.ni.com/reference/en-XX/help/370281P-01/imaqvision/imaq_convert_pixel_to_real_world/
[3] http ://zone.ni.com/reference/en-XX/help/370281AC-01/imaqvision/imaq_get_camera_parameters/
[4] http ://zone.ni.com/reference/en-XX/help/370281M-01/nivisionlvbasics/perspective_and_nonlinear_distortion_calibration/
[5] Léo Sibut, Upgrade of the babyfoot vision control system, EPFL.
[6] https ://fr.wikipedia.org/wiki/Filtre_de_Kalman
[7] https ://zone.ni.com/reference/en-XX/help/370281AC-01/imaqvision/imaq_track_objects/
[8] http ://zone.ni.com/reference/en-XX/help/370281AC-01/imaqvision/imaq_set_initial_object_tracking_position/
24Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Annexes
25Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Code Matlab pour optimiser un modèle de distorsion
x=[......]; % Positions x extraites de Labview
y=[......]; % Positions y extraites de Labview
%% Suppression des doublons
xnew(1)=x(1); ynew(1)=y(1); j=2;
for i=2:length(x)
if x(i)~=x(i-1) || y(i)~=y(i-1)
xnew(j)=x(i);
ynew(j)=y(i);
j=j+1;
end
end
%% Paramètres géométriques
xc=0; % Position x du centre de la caméra (différent de 0 si mal calibrée)
yc=0; % Position y du centre de la caméra (différent de 0 si mal calibrée)
%% Initialisation des paramètres de la boucle
Meilleurbruit=10^9; % Initialisation du meilleur bruit
k1=0; % Coefficient de distorsion en r^2 à optimiser
k2=0; % Coefficient de distorsion en r^4 à optimiser
%% Boucle permettant de tester les coefficients du modèle de distorsion
for k=1:1:15
for l=1:1:15
k1 = 10^-k; % Les paramètres sont testés sur une échelle logarithmique
k2 = 10^-l;
%% Calcul des positions corrigées pour un modèle défini
r = (((xnew-xc)).^2+((ynew-yc)).^2).^(0.5);
dx = (xnew-xc).*(k1*r.^2+k2*r.^4);
dy = (ynew-yc).*(k1*r.^2+k2*r.^4);
xf = xnew+dx;
yf = ynew+dy;
%% Régression linéaire
p = polyfit(xf,yf,1);
fit = (p(2)+xf*p(1));
%% Calcul de l'erreur
erreur = yf-fit;
Sommeerreur = sum(abs(erreur));
%% Vérification : les coefficients sont-ils optimaux?
if Sommeerreur < Meilleurbruit
Meilleurbruit = Sommeerreur;
k1opt = k1;
k2opt = k2;
end
end
end
26Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Code Matlab du Filtre de Kalman (sans gestion du cahier des charges)
%% Données des positions mesurées
x = [...];
y = [...];
dt = [...];
%% Suppression des doublons
x_new(1) = x(1) ; y_new(1) = y(1) ; dt_new(1) = dt(1);
j=2;
for i=2:length(x)
if x(i)~=x(i-1) || y(i)~=y(i-1)
x_new(j)=x(i);
y_new(j)=y(i);
dt_new(j)=dt(i);
j=j+1;
end
end
%% Opérations de Kalman
Z(1,:)=x_new; % Mesure du process pour x
Z(2,:)=y_new; % Mesure du process pour y
H=[1 0 0 0; 0 1 0 0]; % Matrice qui relie l'état x à la mesure
R=[2.25 0;0 2.25]; % Covariance du bruit de mesures (à optimiser)
P_old=[2 0 0 0;0 2 0 0;0 0 5 0;0 0 0 5]; % Initialisation de P (à optimiser)
X_filtre(:,2)=[Z(1,2);Z(2,2);0;0]; % Initialisation du filtre (voir section 2.4)
for k=3:length(x_new)
% Etape 1 : Prédiction
F=[1 0 dt(k) 0; 0 1 0 dt(k); 0 0 1 0; 0 0 0 1]; % Matrice reliant les états k et k-1
X_pred(:,k)=F*X_filtre(:,k-1); % Etat prédit
P_pred=F*P_old*F.'; % Prédiction de la covariance
% Etape 2 : Mise à jour
e2(:,k)=Z(:,k)-H*X_pred(:,k); % Innovation
S=H*P_pred*H.'+R; % Covariance de l'innovation
K=P_pred*H.'*inv(S); % Gain de Kalman
X_filtre(:,k)=X_pred(:,k)+K*e2(:,k); % Etat mis à jour
P_new=(eye(4)-K*H)*P_pred; % Covariance mise à jour
P_old=P_new; % Préparation à l'itération suivante
end
figure (1)
plot(x_new,y_new,'+'); hold on ; plot(X_filtre(1,2:end),X_filtre(2,2:end),'linewidth',3);
legend('Mesures','Filtre de Kalman'); xlabel('x') ; ylabel('y')
set(gca,'FontSize',15)
title('Filtre de Kalman sans gestion du cahier des charges'); hold off
27Projet de semestre M2 - Févr/Juin 2017 Yoann moret
VI général du filtre de Kalman
28Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Sous VI « Kalman operations »
Sous VI « Uncertainty Pos »
29Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Sous VI « Finite Differences »
30Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Filtre de Kalman : Différents choix de la matrice P0
λ 0 0 0
0 λ 0 0
P0 =
0
, λ = 0.1, 1, 10
0 λ 0
0 0 0 λ
31Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Filtre de Kalman avec Padd fixe : Premier choix de Padd
5 0 0 0
0 5 0 0
Padd =
0
, Incertitude distance = 12
0 10 0
0 0 0 10
32Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Filtre de Kalman avec Padd fixe : Deuxième choix de Padd
5 0 0 0
0 5 0 0
Padd =
0
, Incertitude distance = 15
0 20 0
0 0 0 20
33Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Filtre de Kalman avec Padd dépendant de la mesure : Premier choix de
Padd
α = 0.5 Facteur d’incertitude position
β=3 Facteur d’incertitude vitesse
Pf ixe = 0 Une incertitude indépendante de ces différences est ajoutée.
34Projet de semestre M2 - Févr/Juin 2017 Yoann moret
Filtre de Kalman avec Padd dépendant de la mesure : Deuxième choix
de Padd
α = 0.5 Facteur d’incertitude position
β=6 Facteur d’incertitude vitesse
Pf ixe = 0 Une incertitude indépendante de ces différences est ajoutée.
35Vous pouvez aussi lire
