θk+1 ωk+1 xk+1 • xk+1 •• xk+1 = θk+ ∆t.W (θ).ωk+ v1 k ωk+ v2 k xk+ ∆t.x•k +21∆t2.••xk +v3 k • xk+∆t.••xk+v4 k •• xk +v5 k (5.21)
Dans cette équation ∆t est la période d’échantillonnage entre deux mesures, W (θ) est la matrice de rotation liée à la vitesse angulaire et les vi
k, i ∈ [1..5] sont les distributions modélisant l’incertitude de l’état. Aux capteurs sont associées des équations de mesure qui convertissent les données fournies par ces derniers dans les mêmes grandeurs que celles utilisées dans l’équation d’état. Puisque l’objet de cette étude est la comparaison des performances d’un filtre particulaire avec un estimateur de Kalman étendu, nous allons supposer que les capteurs mesurent directement les mêmes grandeurs que celles de l’équation d’état. Le système de vision nous donnera la pose de la caméra (xm, θm)dans le repère associé à l’espace de travail. La centrale inertielle donnera les vitesses angulaires (ωm) par le biais de ses gyroscopes et les accélérations (••xm) par le biais des accéléromètres. Ces données issues des capteurs peuvent être représentées par le vecteur de mesure suivant :
zm = [θm, ωm, xm,••xm] (5.22)
L’équation de mesures sera donc :
Zk = H.Xk+ ek (5.23) Avec H = I3×3 0 0 0 0 0 I3×3 0 0 0 0 0 I3×3 0 0 0 0 0 I3×3 0 (5.24)
I3×3étant la matrice identité de taille 3 × 3 et ektraduisant l’incertitude de la mesure par rapport aux bruits.
Nos équations d’état et de mesure étant définies, nous pouvons appliquer les filtres particulaires ou de Kalman étendu pour estimer l’orientation et la position de la tête de l’opérateur grâce aux mesures fournies par le système de localisation par la vision couplé à une centrale inertielle. Nous allons à présent exposer les résultats obtenus en appliquant les deux algorithmes de filtrage sur les valeurs d’une simulation que nous avons effectuée. Nous en conclurons le type de filtrage nous devons employer.
5.4 Application du filtre particulaire à la prédiction du
mouve-ment de la tête
A notre connaissance, le filtre particulaire n’avait pas été employé dans le cadre de la prédiction du mouvement de la tête. Nous avons voulu voir quel était le comportement d’un tel filtre face à une base de comparaison à savoir le filtre de Kalman étendu qui est classiquement utilisé dans le cadre de la prédiction du point de vue en réalité augmentée [Ababsa et al., 2003].
Afin d’évaluer les performances de notre filtre particulaire, nous l’avons d’abord essayé sur un jeu de données en simulation. Une position et une orientation virtuelle de la caméra est décrite (voir
5.4. APPLICATION DU FILTRE PARTICULAIRE 153
FIG. 5.6 – Jeu de données virtuelles servant à la simulation
figure 5.6 page suivante), de même pour les données issues des accéléromètres et des gyroscopes. A ces mesures, nous ajoutons un bruit blanc gaussien d’une variance de 0.01 rad/s pour les vitesses angulaires, 1300 rad/s2 pour les accéléromètres et 0.016 m pour le système de localisation par la vision, en se basant sur les spécifications techniques de chaque capteur. De plus, le bruit de mesure et l’incertitude des états, respectivement, vket eksont considérés comme étant gaussiens, c’est à dire que vk ∈ N (0, Q) et ek ∈ N (0, R), N étant une distribution normale et Q et R étant des matrices de covariance. Dans notre expérience, nous avons fixé le pas d’échantillonnage à ∆t = 15.6ms ce qui donne une fréquence de 60 Hz environ.
Les matrices de covariances du filtres sont choisies empiriquement pour optimiser les perfor-mances du filtre. Ceci concerne R la matrice de covariance sur le bruit de mesure, Q la matrice de covariance décrivant l’incertitude sur la valeur d’un état interne et la matrice d’erreur sur la première estimation de la pose P0. Nous leur attribuons les valeurs suivantes :
P0 = diag([ 100I3×3 10I3×3 100I3×3 104I3×3 106I3×3 ]) (5.25)
Q = diag([ 0.01I3×3 0.1I3×3 10I3×3 105I3×3 106I3×3 ]) (5.26)
R = diag([ 0.001I3×3 0.01I3×3 I3×3 100I3×3 104I3×3 ]) (5.27)
Dans un premier temps, nous comparons le filtre particulaire (appelé aussi Sampling Importance Resampling ou SIR) avec le filtre de Kalman étendu (Extended Kalman Filter - EKF) en employant les équations 5.21 et 5.23 page ci-contre. Nous n’incluons pas ici les équations du filtre de Kalman étendu puisque ce dernier a déjà fait l’objet d’une présentation. Dans la figure 5.7 page suivante sont représentés les résultats de la prédiction du mouvement de la tête à l’aide du filtre particulaire. La position réelle et l’orientation sont représentées par des lignes en pointillés alors que l’estimation est représentée par des lignes continues.
Le figure 5.7(a) représente la position estimée suivant la coordonnée x du repère monde et la figure 5.7(b) fait de même pour l’orientation autour de ce même axe. Le filtre utilisé ici comporte N = 100
(a) Position x
(b) Orientation x
(c) Vue de la trajectoire dans l’espace dans le repère monde
5.4. APPLICATION DU FILTRE PARTICULAIRE 155
(a) Position x (b) Orientation x
FIG. 5.8 – Erreur quadratique moyenne de prédiction pour le filtre particulaire (SIR) et le filtre de Kalman étendu
Estimation Translation (m) Orientation (rad) Orientation (o)
Particulaire 0.01642 0.0003 0.0171
Kalman étendu 0.01986 0.0049 0.2807
TAB. 5.1 – Erreur quadratique moyenne pour le filtre particulaire et le filtre de Kalman étendu particules. Il apparaît que le filtre particulaire réussit à suivre fidèlement la trajectoire, ce qui implique un modèle de prédiction assez robuste. La figure 5.7(c) représente la trajectoire simulée dans le repère monde projetée sur le plan XY . La ligne continue donne la représentation réelle alors que les croix représentent les positions prédites.
Afin d’évaluer les performances du filtre, nous calculons l’erreur quadratique moyenne entre les positions réelles et les positions estimées. Cette erreur est donnée par l’équation :
ǫ(n) = 1 n n X k=1 kx(k) − ˆx(k)k2 (5.28)
Où x(k) est un des points de la trajectoire réelle et ˆx(k) est le même point de la trajectoire lorsqu’il est prédit.
A la figure 5.8, les erreurs quadratiques moyennes au cours du temps sont représentées pour le filtre particulaire et le filtre de Kalman étendu. Les figures 5.8(a) et 5.8(b) montrent l’erreur qua-dratique moyenne de la position et de l’orientation de la caméra suivant l’axe x du repère monde. Nous notons que, dans tous les cas, avec des conditions d’application similaires, le filtre particulaire donne une meilleure estimation que le filtre de Kalman étendu alors que nous sommes ici dans des hypothèses favorables à ce dernier (bruit gaussien).
Dans le tableau 5.1, nous avons résumé les résultats obtenus en moyenne par les deux méthodes : Nous avons ensuite étudié l’influence du nombre de particules sur les performances du filtre. Ces résultats sont présentés à la figure 5.9 page suivante. Comme nous pouvions nous y attendre, plus le nombre de particules augmente, plus l’erreur quadratique moyenne diminue. Toutefois, plus nous calculons de résultats sur un ensemble de particules, plus le temps de calcul pour établir une estimation va linéairement augmenter. Il est donc essentiel de trouver le nombre de particules optimal à employer en fonction du problème posé. Ce nombre doit être adapté aux contraintes liées au temps réel tout en assurant les meilleures performances possibles. Nous avons donc effectué plusieurs expériences en faisant varier le nombre de particules. La valeur optimale est d’environ une centaine de particules pour le problème posé.
FIG. 5.9 – Effet du nombre de particules sur les performances du filtre
Si nous voulons résumer et comparer les deux filtres, nous pouvons récapituler leurs caractéris-tiques par rapport à divers critères de comparaison comme nous l’avons fait dans le tableau 5.2 page suivante. Ainsi, même si le filtre de Kalman étendu est en théorie optimal et son exécution rapide, il n’en demeure pas moins que le filtre particulaire reste plus pratique à mettre en place, puisqu’il est robuste par rapport aux conditions initiales, sa vitesse de convergence vers la solution est supérieure et il permet de modéliser les non-linéarités qui peuvent intervenir dans les équations du filtre.
Filtre Kalman étendu Particulaire
Théorie Optimal Sous-optimal
Initialisation Sensible Robuste
Temps de calcul Rapide Dépend du nombre de particules
Convergence + ++
Modèle de bruit Blanc gaussien libre
Non-linéarité Linéarisation obligatoire oui
TAB. 5.2 – Comparaison du filtre de Kalman étendu et du filtre particulaire
Dans le but de contrôler l’efficacité de notre approche, nous avons simulé un système de réalité augmentée qui emploierait notre système de filtre particulaire. Nous avons généré une série d’images de synthèses prises par une caméra en mouvement. Pendant ce mouvement, la caméra observe un buste de statue (la Vénus) qui sera notre objet de référence. La Vénus est immobile par rapport au repère monde. Quand la pose de la caméra est prédite par le filtre particulaire, nous reprojetons le modèle filaire de la Vénus sur cette dernière, en tenant compte du retard que devrait avoir les mesures du capteur. La figure 5.10 page ci-contre montre le résultat visuel obtenu. Du point de vue qualitatif, les résultats donnés par le filtre particulaire permettent un recalage visuellement correct, mais qui reste perceptible. Ceci indique que nous pouvons améliorer la qualité du recalage. Pour se faire, nous allons coupler cette méthode de prédiction avec la méthode de post-rendering que nous avons développé.