2007 développement du logiciel embarqué d un module de localisation fine (2)
1. Cycle de formation des ingénieurs en Télécommunications
Option :
Architecture des Systèmes de télécommunication
Rapport de Projet de fin d’études
Thème :
Développement du logiciel embarqué
d’un module de localisation fine
Réalisé par :
Amel KLAA
Encadré par :
M. Samuel TARDIEU
M. Khaled GRATI
Mlle Manel BEN ROMDHANE
Travail proposé et réalisé en collaboration avec
Ecole Nationale Supérieure des Télécommunications de Paris
Année universitaire :
2006/2007
2. Dédicaces
Je dédie ce travail à tous ceux que j’aime.
Une première pensée s’adresse tout particulièrement à mes parents,
qui m’ont tant soutenue durant mon cursus, veillant à mon bien être et à mon succès.
A mes soeurs qui ont su, durant toutes ces années de travail,
me motiver et m’inviter à croire en moi.
A mon frère qui m’a tant poussée à aller de l’avant.
A ma petite Myriam, que j’aime beaucoup.
A tous mes amis qui ont toujours été présents pour moi.
A mes professeurs de Sup’Com, à qui j’exprime tout mon respect
pour le travail qu’ils font.
A tous ceux qui m’ont aidée à réaliser ce travail.
I
3. Avant propos
Le travail de ce projet de fin d’études a été réalisé à l’Ecole Nationale Supérieure des
Télécommunications (Télécom Paris), au sein du département Informatique et Réseau.
Je tiens à remercier mon maître de stage, M. Samuel Tardieu, de m’avoir accueillie
dans son département et surtout de m’avoir proposé un projet industriel qui m’initie à la
vie professionnelle.
J’adresse ensuite ma profonde gratitude à mon encadrant à Sup’Com et mon profes-seur,
M. Khaled Grati, pour sa disponibilité, son soutien et sa compréhension.
Je remercie également Mlle Manel Ben Romdhane, qui a contribué dans l’encadre-ment
de ce travail, pour ses remarques pertinentes et sa rigueur.
Mes remerciements s’adressent aussi à tous mes professeurs à Sup’Com, pour ces
trois années de partage, d’enrichissement, d’échange, me projettant aujourd’hui dans une
carrière que j’ai hâte de conquérir. Nombreux sont mes enseignants auxquels je porte
cette gratitude, je ne me retiendrai pas de citer M. Adel Ghazel, M. Fethi Tlili, M. Chiheb
Rebai et M. Hichem Besbes, qui m’ont tant apporté durant toute cette dernière année
d’école d’ingénieur.
Enfin, Merci à l’équipe qui m’a accompagnée tout au long de la période que j’ai passé
à Télécom Paris. Je m’adresse particulièrement à Raja, Khaled, César, Olivier, Béchir,
Hoa, Dorra, Irfan...et bien d’autres, avec qui j’ai partagé des moments exceptionnels de
ma vie.
Finalement, je remercie toutes les personnes qui ont contribué, de près ou de loin, à la
réalisation de ce travail.
II
4. Résumé
A une époque où les technologies de communication ne cessent de se développer, les
défis de confort et de sécurité des personnes sont, de nos jours, l’une des problématiques
les plus préoccupantes des laboratoires de recherches, aussi bien chez les industriels, que
dans les écoles et universités. D’énormes moyens, en R&D, sont déployés pour répondre
au mieux aux besoins incessants des personnes. Notre projet, s’inscrivant dans un cadre
de partenariat avec d’autres acteurs, s’intéresse à la localisation fine d’un mobile, à savoir
une personne se déplaçant dans un espace physique ouvert.
Les systèmes de localisation sont multiples, certes, notamment avec la démocratisa-tion
du récepteur GPS, qui est devenu un appareil accessible par tous. Mais, la précision
d’un tel composant demeure peu fine, quoi qu’elle réponde parfaitement à certains besoins
de localisation exprimés par les utilisateurs. D’autres applications sont, en effet, plus exi-geantes,
et requièrent une précision centimétrique. Tel est le cas de notre travail, là où le
besoin de raffiner la localisation est impératif pour que l’information de positionnement
soit pertinente. Pour ce faire, l’idée consiste à combiner différents systèmes de positionne-ment,
et d’exploiter les avantages de l’un pour compenser les faiblesses des autres. Avec
le développement des capteurs inertiels, nous nous intéressons à réaliser le traitement adé-quat
qui, ayant à l’entrée les mesures inertielles et la position délivrée par le GPS, permet
d’aboutir à des performances meilleures que celles propres à chaque capteur.
Mots clés : localisation fine, récepteur GPS, système de positionnement, capteurs
inertiels.
III
7. Table des figures
1.1 Trois types d’erreurs de mesure classiques . . . . . . . . . . . . . . . . . 6
1.2 Référenciel Géodésique Local . . . . . . . . . . . . . . . . . . . . . . . 9
2.1 Les techniques de Prédiction et de Lissage . . . . . . . . . . . . . . . . . 11
2.2 Filtre de Kalman, contrôle feedback . . . . . . . . . . . . . . . . . . . . 12
2.3 Trajectoire nominale et réelle pour un filtre linéarisé de Kalman . . . . . 18
3.1 Représentation polaire dans l’espace des quaternions . . . . . . . . . . . 36
3.2 Estimation de l’altitude du mobile . . . . . . . . . . . . . . . . . . . . . 38
3.3 Estimation de la vitesse du mobile . . . . . . . . . . . . . . . . . . . . . 39
3.4 Estimation de la position du mobile . . . . . . . . . . . . . . . . . . . . 39
3.5 Estimation de la vitesse en X du mobile, pour w » N(0; 1) . . . . . . . . 40
3.6 Estimation de la vitesse en X du mobile, pour w » N(0; 10¡2) . . . . . . 41
3.7 Estimation de la vitesse en X du mobile, pour w » N(0; 10¡4) . . . . . . 41
3.8 Estimation de la vitesse en Y du mobile, pour w » N(0; 1) . . . . . . . . 42
3.9 Estimation de la vitesse en Y du mobile, pour w » N(0; 10¡2) . . . . . . 42
3.10 Estimation de la vitesse en Y du mobile, pour w » N(0; 10¡4) . . . . . . 43
3.11 Erreur d’estimation de la vitesse en X du mobile, pour différentes valeurs
de l’erreur de mesure . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
3.12 Estimée de la position du mobile . . . . . . . . . . . . . . . . . . . . . . 44
B.1 ADXL202 Specification . . . . . . . . . . . . . . . . . . . . . . . . . . 51
B.2 L’accéléromètre MEMSIC . . . . . . . . . . . . . . . . . . . . . . . . . 52
B.3 Le gyroscope CRS-03 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
B.4 Le gyroscope CRS-04 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
VI
8. Liste des acronymes
DCM Direction Cosine Matrix
DGPS Differencial Global Positioning System
ECEF Earth Centered Earth Fixed
EKF Extended Kalman Filter
GPS Global Positioning System
GRV Gaussian Random Variable
IMU Inertial Measurement Unit
INS Inertial Navigation System
LLA Longitude, Latitude, Altitude
MEMS Micro Electro Mechanical System
NED North-East-Down
RMS Root Mean Square
RTK Real Time Kinematics
USDoD United States Department of Defense
WGS-84 World Geodetic System 1984
VII
9.
10. Introduction générale
NOtre projet constitue une part d’un grand projet industriel, sous le nom de SOUND
DELTA. La définition globale du projet, dans un contexte partenarial, consiste en la créa-tion
d’un Dispositif d’interaction temps réel pour l’exploration simultanée et corrélée
d’un espace physique et d’un espace musical. Les enjeux de ce projet se résument à assu-rer
la navigation d’une composante musicale dans un espace urbain, de manière à ce que
la composition, à sa reception par les spectateurs, soit optimale en terme d’interactivité et
de qualité de service (rendu, latence, précision).
Compte tenu de la diversité des enjeux, le programme du projet global a été partagé
entre six partenaires, dont l’ENST constitue un acteur responsable de la localisation de
la personne mobile dans l’espace physique, afin d’assurer une meilleure navigation de la
composition musicale. Ainsi se définit le cadre général de notre projet, qui, faisant abs-traction
de la composante artistique confiée à d’autres spécialistes, consiste à réaliser le
module embarqué responsable de la localisation fine du spectateur en mouvement. Ce
traitement, prenant en entrées les mesures délivrées par les différents systèmes de posi-tionnement,
sera dans une étape ultérieure implémenté sur un calculateur embarqué.
Pour répondre aux besoins de vérification, et ensuite de validation du traitement de
localisation demandé, un travail de simulation s’avère indispensable.
Dans une première partie, nous étudions les algorithmes qui permettent de réaliser
le traitement des mesures. Le filtre de Kalma, dont nous détaillerons le principe, est un
estimateur performant, bien adapté aux systèmes dynamiques.
Ensuite, Il s’agit de modéliser notre système. Une telle phase doit obéir à la nature du
mouvement, aux contraintes de l’environnement ainsi qu’à la précision des instruments
de mesures utilisés pour le positionnement. La connaissance du modèle d’évolution et
d’observation du système étudié est indispensable pour la conception de l’estimateur.
Une fois la modélisation formulée, nous nous proposons d’implémenter le traitement
appliqué à une trajectoire générée manuellement.
1
11. Chapitre 1
Introduction aux systèmes de
localisation fine
1.1 Introduction
Les problèmes traitant des besoins incessants de sécurité et de confort constituent de
nos jours des défis pour les chercheurs et les développeurs. De nombreuses études s’ins-crivent
dans cette thématique, et plus particulièrement, le problème de positionnement.
Pour répondre à ces défis technologiques, extrêmement utils pour la vie des personnes, de
nouveaux systèmes de positionnement ont été développés, offrant, à leur intégration, des
performances de fonctionnement importantes.
L’objet de ce premier chapitre consiste à introduire le principe de localisation fine, en
explicitant l’intérêt de combiner différents systèmes de positionnement. Ensuite, à pré-senter
brièvement les principaux instruments de mesure qui seront utilisés pour estimer la
position du mobile, à savoir capteurs inertiels et systèmes de positionnement par satellites.
1.2 Principe de la localisation fine
Dans le cadre de notre projet, nous nous proposons de combiner capteurs inertiels et
récepteur GPS, dans l’objectif de réaliser un traitement optimal des différentes données
mesurées, estimant au mieux la position et l’orientation du mobile [1], [2].
L’intérêt d’effectuer cette intégration s’explique par la complémentarité des différents
instruments de positionnement [3]. En effet, Pour le GPS, La fréquence de fonctionne-ment
est de l’ordre de quelques Hertz, alors que les capteurs inertiels fonctionnent à des
fréquences beaucoup plus élevées, généralement entre 20 et 200 Hz. Cette différence de
fréquence permet d’augmenter le rythme de mesure, et par conséquent augmenter la quan-
2
12. 1.3. Les sytèmes de positionnement par capteurs inertiels 3
tité d’information disponible. En outre, l’utilisation des capteurs inertiels permet de dimi-nuer
le besoin aux références externes, tel employé par un récepteur GPS. Par ailleurs,
alors que les mesures du GPS se dégradent peu à long terme, celles des capteurs iner-tiels
présentent une dérive. La combinaison des deux permet donc de compenser la dérive
causée par le gyroscope et l’accéléromètre.
L’intégration des différents composants a donc pour principal objectif d’aboutir à une
performance de l’ensemble, meilleure que les performances individuelles des capteurs.
1.3 Les sytèmes de positionnement par capteurs inertiels
Les capteurs inertiels sont de plus en plus exploités dans diverses applications de posi-tionnement.
En vue d’acquérir une information complète et fiable sur la position et l’orien-tation
du mobile, la combinaison acc¶el¶eromµetre + gyroscope retourne une information
complète sur le localisation du mobile. Etudions de plus près chacun de ces deux capteurs.
1.3.1 Les accéléromètres
Un accéléromètre est un appareil qui fournit un positionnement absolu du mobile au-quel
il est fixé. Cette mesure retournée correspond à l’accélération non gravitationnelle
¡! accnongravit subie par une masse m. Il faut lui ajouter la valeur du champ local de gravita-tion
~g pour connaître l’accélération absolue ¡! accabs.
¡! accabs = ¡! accnongravit +~g (1.1)
Compte tenu du principe de mesure, ils existent différents types d’accéléromètres :
les accéléromètres piézoélectriques, les piézorésistifs, les capacitifs et les asservis. Ils
couvrent à eux quatre la grande majorité des applications. Ces accéléromètres fonctionnent
tous selon le même principe : ils intègrent une masse liée au bâti du capteur par une lame
flexible ou un ressort ; l’accélération se déduit alors de la force appliquée à la masse.
En revanche, le principe de mesure de la force change d’une technologie à une autre.
Dans la plupart des capteurs piézoélectriques, la masse sismique exerce des efforts de
compression ou de cisaillement sur un matériau piézoélectrique qui génère alors une
charge électrique proportionnelle à la force qui lui est appliquée. Dans les capteurs pié-zorésistifs,
la masse sismique est le plus souvent solidaire d’une poutre dont on mesure
la déformation avec des jauges piézorésistives. Avec les accéléromètres capacitifs, par
ailleurs, on s’intéresse généralement au déplacement de la masse sismique sous l’effet de
l’accélération. La masse se déplace en effet entre deux électrodes. La variation de tension
aux bornes de celles-ci traduit alors l’accélération. D’autres accéléromètres capacitifs in-
3
13. 1.3. Les sytèmes de positionnement par capteurs inertiels 4
tègrent une sorte de diaphragme dont le mouvement varie en fonction de l’accélération.
Enfin, dans le cas des accéléromètres asservis, il n’y a pas de déplacement de la masse
sismique : une force, égale et opposée à celle induite par l’accélération, est crée pour
maintenir la masse sismique dans sa position initiale. La force appliquée est alors propor-tionnelle
à l’accélération à mesurer.
Dans notre projet, et compte tenu des contraintes de disponibilité et de coût, le choix a
été fixé sur deux modèles d’accéléromètres, à deux axes, couplés afin de mesurer l’accé-lération
selon les trois axes : un ADXL202 et un MEMSIC 2125. Présentons brièvement
ces deux modèles.
L’ADXL202 :
Le circuit intégré ADXL202 d’Analog Devices intègre deux accéléromètres capaci-tifs
: une masse sismique m, solidaire de l’armature centrale d’un condensateur plan dif-férentiel,
traduit le déplacement des armatures par une variation de la capacité électrique.
Il mesure les accélérations dynamiques et statiques.
Les principes de fonctionnement exploités s’appuient tous sur le principe fondamental
de la dynamique. En effet, selon la deuxième loi de Newton, l’accélération subit par la
masse m, dans un repère galiléen, est proportionnelle à la résultante des forces qui lui
sont appliquées ~F.
~F = m:~a (1.2)
Selon la loi de Hooke "Telle extension, Telle force ", l’allongement ~x est proportionnel
à la force ~F.
~F = k:~x (1.3)
En considérant ce couple d’équations, on peut déduire l’accélération de (m).
Cet accéléromètre permet aussi d’assurer la fonction d’inclinomètre. En effet, ce type
d’accéléromètre est configuré par rapport à la gravité de telle sorte que le tilt A calculé
à partir de l’accélération mesurée constitue l’inclinaison du mobile. Cependant, ces deux
fonctions distinctes ne peuvent pas être assurées en même temps.
Certains accéléromètres à détection capacitive à masse sismique pendulaire permettent
aussi d’assurer la fonction d’inclinomètre. Cette dernière est rendue possible par la confi-guration
mécanique des accéléromètres capacitifs pendulaires et la gravité terrestre. Ce-pendant,
la fonction d’accéléromètre ne peut pas être utilisée en même temps.
Aangle d’inclinaison
4
14. 1.3. Les sytèmes de positionnement par capteurs inertiels 5
Le MEMSIC 2125 :
À la différence de ceux d’Analog Devices qui emploient un faisceau mobile, les ac-céléromètres
de Memsic mesurent l’accélération par le biais d’un gaz chaud à l’intérieur
d’une chambre. La chambre est équipée d’un ensemble de capteurs qui surveille le mou-vement
du gaz.
En changeant le tilt, le gaz est décalé, et c’est ainsi que les valeurs des sondes changent
proportionnellement.
1.3.2 Les gyroscopes
Un gyroscope est un appareil qui permet de mesurer la vitesse angulaire du mobile.
Deux composants sont également à présent disponibles : Le Silicon Sensing CRS-03 et le
Silicon Sensing CRS-04. Les deux modèles CRS-03 et CRS-04 sont des produits de Sili-con
Sensing Systems B. Il s’agit de gyroscopes uni-axiaux, délivrant une sortie analogique
proportionnelle à la mesure de la vitesse angulaire du mobile considéré, et qui diffèrent
toutefois par quelques spécifications.
Le tableau (1.1) explique les caractéristiques les plus importantes de ces deux gyro-scopes
:
Gyroscope CRS-03 CRS-04
Tension d’alimentation 5V § 0; 5 5V
Coût(en euros) 119 111
Etendue de mesures §100=sec §150=sec
Bande Passante (¡3dB) 10 Hz De 50 à 85 Hz
Bruit à la sortie 1 mV rms 0,75°/sec rms
Sensibilité/Facteur d’échelle 20 mV/º/sec 12.75 mV/º/sec
Précision du facteur d’échelle §3% §3%
Non linéarité du facteur d’échelle §1% §0:6%
Biais initial (à 0°/sec) §60mV 2,5 V
Biais (Variation selon la température) §3% §10%
Biais de la dérive §0:55=sec §0:55=sec
Tableau 1.1 – Les caractéristiques des deux gyroscopes
Pour une spécification complète des ces quatre composants décrits dans le paragraphe
(1.3), les deux datasheets présentés en AnnexeB décrivent explicitement chacun des
capteurs.
Les capteurs inertiels, à eux seuls, constituent ainsi toute une théorie basée principa-lement
sur les notions fondamentales de la mécanique.
Bhttp ://www.siliconsensing.com/
5
15. 1.3. Les sytèmes de positionnement par capteurs inertiels 6
1.3.3 Modèles d’erreur dans les capteurs inertiels
Un capteur inertiel présente souvent des erreurs de mesure. Il s’agit en effet de la
déviation de la valeur obtenue à la sortie du capteur par rapport à la vraie valeur de la
grandeur à l’entrée. La figure (1.1) illustre les trois formes d’erreur les plus communes.
Figure 1.1 – Trois types d’erreurs de mesure classiques
Le biais :
le biais, qu’on désigne par ¹, est l’erreur la plus évidente, c’est une simple valeur qui
vient s’additionner au signal de mesure.
asortie = aentr¶ee + ±abiais (1.4)
!sortie = !entr¶ee + ±!biais (1.5)
D’après cette forme que prend le biais, il est tout aussi systématique de penser que
le biais est déterminé simplement en appliquant une entrée nulle au capteur. Cependant,
la valeur du biais diffère généralement chaque fois que le capteur est mis en marche. En
outre, le biais varie aussi dans le temps. Cette evolution, désignée par dérive "drift", est
entre autres due aux variations de la température.
Il existe plusieurs modèles qui permettent de formuler cette dérive du biais. Pour ce
faire, certains auteurs favorisent une démarche aléatoire. Compte tenu du contexte proba-biliste
dans lequel est étudié notre système, nous pouvons considérer que le biais est une
variable aléatoire dont l’espérance ne dépend pas du temps : le modèle le plus commun
à utiliser est celui d’un bruit blanc gaussien ±(t) ajouté à une moyenne. Par souci de se
rapprocher de la réalité, il peut convenir de mettre en oeuvre un modèle d’erreur un peu
plus complexe : le modèle "Random Walk"[4], qui décrit une évolution de premier ordre.
6
16. 1.3. Les sytèmes de positionnement par capteurs inertiels 7
Certaines études élaborées sur le comportement des capteurs inertiels ont montré que l’er-reur
prépondérante de ces capteurs est due à un bruit de type "Random Walk". Ce modèle
constitue, en effet, un processus stochastique de type chaîne de Markov, possédant une
dynamique discrète composée d’une succession de pas aléatoires.
Le premier modèle prend ainsi la forme d’une simple évolution de premier ordre ex-primée
par la formule (1.6) :
¹_ = ±(t) (1.6)
Ce modèle est généralement utilisé dans la cas où les capteurs choisis sont de bonne
qualité, par rapport à la précision de l’estimation recherchée.
Le deuxième modèle est régie par l’expression (1.7).
¹_ = ¡
1
¿¹
:¹ + ±(t) (1.7)
Où ¿¹ s’écrit selon l’équation (1.8).
1
¿¹
=
2
64
1
¿x
0 0
0 1
¿y
0
0 0 1
¿z
3
75
(1.8)
Le facteur d’échelle :
le facteur d’échelle est un terme multiplicatif K dû aux erreurs de fabrication, certes.
Mais, il n’évolue pas considérablement dans le temps, ce qui permet de le compenser par
une simple calibration.
Généralement, le facteur d’échelle est décrit par le modèle (1.9).
asortie = Ka:aentr¶ee (1.9)
!sortie = K!!entr¶ee (1.10)
La non-linéarité :
la non linéarité est une contrainte de fonctionnalité présente dans tout capteur. Ils
existent plusieurs modèles permettant de décrire cette imperfection des appareils. L’ap-proche
la plus commune consiste à développer la mesure sous forme d’une série de type
décrit dans les équations (1.11) et (1.12).
entr¶ee + ::: (1.11)
asortie = K0 + K1:aentr¶ee + K2:a2
7
17. 1.4. Les systèmes de positionnement par satellites 8
entr¶ee + ::: (1.12)
!sortie = L0 + L1!entr¶ee + L2!2
Les coefficients Ki et Li sont calculés en effectuant des mesures dynamiques sur le
capteur.
1.4 Les systèmes de positionnement par satellites
Le système de positionnement par satellites permet de calculer la position tridimen-sionnelle
(latitude, longitude et altitude) d’un utilisateur, de manière continue et instanta-née,
en tout endroit sur la terre, excepté sur les pôles.
Le premier satellite artificiel à orbiter autour de la Terre, a été lancé en octobre 1957
par l’Union soviétique. Cet événement marquant a donné le départ d’une course effrénée
à la conquête de l’espace.
Au début des années Soixante dix, le Département de la Défense des Etats-Unis a
développé le système de positionnement GPS. Les quatre premiers satellites prototypes
ont été lancés en 1978. Une constellation de 24 satellites opérationnels a été complétée en
1993.
A cette époque, seules les applications militaires bénéficiaient des avantages de po-sitionnement
par GPS. Les récepteurs civils sont, par ailleurs, soumis à une dégradation
volontaire des performances. C’est uniquement à partir de l’année 2000 que le gouverne-ment
américain a décidé de supprimer cette dégradation, afin d’améliorer la compétitivité
du système GPS face aux prochaines constellations de satellites concurrentes comme Ga-lileo
[5], pour des raisons politiques et stratégiques.
Lorsqu’un récepteur de positionement par satellites est mobile, sa vitesse et la direc-tion
de son mouvement peuvent être également déterminées. Cette information temporelle
tient sa relativité du fait qu’elle utilise une référence, appelée référentiel géodésique. En
effet, les systèmes géodésiques utilisés historiquement pour la cartographie diffèrent d’un
pays à un autre. La constitution de ces systèmes géodésiques locaux s’appuie sur un mo-dèle
de surface terrestre raccordé à la surface réelle en un ou plusieurs points de référence
connus. Le modèle de surface terrestre utilisé est dit ellipsoïde et correspond à une sphère
légèrement aplatie aux pôles afin de mieux représenter la véritable forme de la surface
terrestre.
Les coordonnées en longitude, latitude et altitude sont déterminées par rapport à cet
ellipsoïde de référence. La figure (1.2) illustre le principe de constitution d’un référentiel
géodésique local, constitué d’un ellipsoïde de référence raccordé à la surface terrestre au
niveau d’un point de référence.
La mise en place du système GPS a imposé l’avènement d’un autre type de système
géodésique, dit spatial, appelé système WGS-84. Ce système WGS (World Geodetic Sys-
8
18. 1.5. Conclusion 9
Figure 1.2 – Référenciel Géodésique Local
tem) a été réalisé par le département de la Défense américain à partir des positions de
bases propres au système GPS et de constantes fondamentales. Toutes les coordonnées
GPS prennent leur signification dans ce système géodésique.
Les systèmes de navigation par satellites sont donc conçus pour fournir des positions
compatibles à moins d’un mètre près. Les champs d’application de la localisation par
satellite utilisant le GPS ne cesseront de s’élargir du fait du développement de bases de
données géographiques et cartographiques de plus en plus précises.
1.5 Conclusion
Ainsi, les systèmes de positionnement délivrent une information de position, de vitesse
où encore de temps absolu, pourvu que l’association soit optimale et bien étudiée.
Dans notre projet, nous nous intéressons au traitement qui suit l’acquisition des me-sures
des différents systèmes de positionnement, en vue de localiser la personne en mou-vement.
Nous sommes alors ramenés à trouver le bon système qui réalisera au mieux cette
mission de localisation.
9
19. Chapitre 2
Technique d’estimation par filtre de
Kalman
2.1 Introduction
L’objet de notre travail consiste à combiner les données provenant des différents sys-tèmes
de positionnement utilisés, à savoir un récepteur GPS, un accéléromètre et un gy-roscope.
L’intégration efficace de ces informations requiert un outil mathématique perfor-mant
pour effectuer les compensations.
Pour ce faire, le filtre de Kalman constitue un algorithme candidat qui présente les
performances et les exigences requises pour répondre à notre problème.
Dans ce chapitre, nous expliciterons le traitement de Kalman, aussi bien dans sa ver-sion
traditionnelle, que dans d’autres approches plus élaborées et supportant les contraintes
de non linéarité des systèmes dynamiques.
2.2 Introduction au filtre de Kalman
En 1960, Rudolph E. KalmanA a évoqué un algorithme d’estimation approprié pour
l’intégration de données de différents types et à des différentes fréquences, et ceci en
tenant compte de la cinématique de l’objet considéré.
Cependant, ce filtre a fait l’objet de plusieurs publications, faisant ainsi, à cette époque,
une controverse dans la communauté scientifique [6].
Plusieurs chercheurs ont participé à son développement, son implémentation et sa
validation. La première application du filtre a permis l’estimation de la trajectoire pour
le programme Apollo. Ensuite, vue les performances réalisées de l’estimateur, il a été
Ahttp ://www.cs.unc.edu/ welch/kalman/kalmanBio.html
10
20. 2.2. Introduction au filtre de Kalman 11
exploité dans l’ordinateur de navigation. Plus tard, son évolution a permis son intégration
dans d’autres applications de positionnement [7] [8].
Une grande variété de filtres a été, depuis, développé à partir de la formulation origi-nale
dite "filtre de Kalman classique". Stanley Schmidt, qui a réalisé la première implé-mentation
du filtre, a ensuite dévelpppé la version étendue [9]. Ensuite, Bierman, Thorn-ton
et bien d’autres ont développé toute une gamme de filtres "racine carrée". Le filtre
le plus utilisé est vraisemblablement la boucle à verrouillage de phase "Phase-Locked
Loop", largement répondu dans les radios, ordinateurs, équipements de communication,
etc.
L’approche de Kalman, qui modélise le système étudié comme étant dynamique, cor-respond,
comme toute méthode de filtrage, à résoudre des problèmes paramétrés par le
temps. Une telle approche stipule que l’état du système à l’instant t n’est autre que la
conséquence des observations et de l’état du système aux instants précédents. De ce fait,
cette méthode utilise la prédiction, qui est l’estimation d’une inconnue à partir des obser-vations
passées, et se distingue du lissage, qui estime la meilleure solution par l’utilisation
des observations passées et futures. La figure (2.2) présente la comparaison entre les deux
techniques dans le domaine temporel.
Figure 2.1 – Les techniques de Prédiction et de Lissage
Le filtrage, fait ainsi intervenir le principe de la navigation à l’estime, dont la notion
est plus connue par la désignation anglaise le "dead reckoning". Il s’agit d’une technique
de discrétisation de la trajectoire, procédant par intégration des vitesses ou double inté-gration
des accélérations afin de déduire le changement de position entre deux points.
Considérons l’exemple simple d’un véhicule en mouvement, si l’on connaît sa position,
sa vitesse et son accélération à l’instant t, il est possible de prédire sa position, sa vitesse
et même son accélération à l’instant t + 1. La compensation prend en compte non seule-ment
les mesures à l’instant t + 1 (correction), mais aussi l’évolution dans le temps des
paramètres (prédiction). La figure (2.2) souligne cette continuité dans le temps entre les
deux phases : la phase de prédiction permet de calculer l’estimée a priori de l’état, qui
sera par la suite corrigée par le biais des observations disponibles.
Ils existent plusieurs types du filtre de Kalman, et ceci selon la linéarité du système.
Lorsque la relation décrivant le système à l’instant t en fonction de son état à l’instant t¡1
est linéaire, on parle du filtre de Kalman traditionnel [10]. D’autres algorithmes, reposant
11
21. 2.3. Le filtre de Kalman : approche linéaire 12
sur le même principe, décrivent plutôt un système non-linéaire. Une introduction au filtre
de Kalman fait l’objet d’un article publié par Greg WelchAet Gary BishopB [11].
Figure 2.2 – Filtre de Kalman, contrôle feedback
2.3 Le filtre de Kalman : approche linéaire
Le système d’équations utilisé dans le filtre de Kalman repose sur la définition de deux
modèles qui sont le processus et la mesure. Le modèle du processus est la représentation
de l’évolution de l’état du système. Il est utilisé pour estimer l’état d’un système dyna-mique
observé. L’équation (2.1) décrit Le modèle de processus d’un système linéaire.
xk = Ák¡1:xk¡1 + vk¡1 + Gk:uk (2.1)
xk : Vecteur d’état à l’instant tk
Ák¡1 : Matrice de transition,
Elle fait le lien entre les paramètres du système entre deux étapes successives.
Gk : Matrice de commande.
Elle fait le lien entre les valeurs optionnelles de contrôle et l’état du système.
uk : Vecteur de commande
vk : Bruit du modèle ou dit aussi bruit de l’état, blanc gaussien
Le modèle de mesure décrit l’information fournie par le ou les capteurs en une équa-tion
liant les paramètres de l’état de la mesure et du bruit. L’équation de mesure ou d’ob-servation
est donnée par (2.2).
zk = Hk:xk + wk + Dk:uk (2.2)
Awelch@cs.unc.edu, http ://www.cs.unc.edu/ welch
Bgb@cs.unc.edu, http ://www.cs.unc.edu/ gb
12
22. 2.3. Le filtre de Kalman : approche linéaire 13
zk : vecteur de mesure à l’instant tk
Hk : Matrice de mesure
Dk : Matrice d’observation de la commande
wk : bruit de mesure, bruit blanc
Les conditions initiales se résument à :
– intitialement, le vecteur d’état est estimé par son espérence mathématique selon la
formule (2.3).
^x0 = E(x0) (2.3)
– La covariance de l’erreur d’estimation de l’état est initialisée à la variance de x0,
comme décrit par l’équation (2.4).
P0 = E((^x0 ¡ x0):(^x0 ¡ x0)T ) (2.4)
On note aussi la covariance de l’erreur d’estimation de l’état xk par l’expression (2.5).
Pk = E((^xk ¡ xk):(^xk ¡ xk)T ) (2.5)
Lors de l’initialisation du filtre, l’hypothèse est faite telle que les bruits d’état et de me-sure
sont des variables aléatoires dont les distributions gaussiennes sont connues a priori
(doivent être estimées à l’avance), blanches et indépendantes de l’état initial du système.
Cette indépendance des bruits permet de simplifier le formalisme des équations d’évolu-tion
et d’observation.
Les bruits de mesures et de l’état sont modélisés par un processus aléatoire gaussien
de moyenne nulle.
vk » N(0;Rk) (2.6)
wk » N(0;Qk) (2.7)
Rk et Qk sont respectivement les matrices de covariance du bruit de modèle et du bruit
de mesure.
Les covariance des bruits du modèle et de mesure sont respectivement exprimées dans
13
23. 2.3. Le filtre de Kalman : approche linéaire 14
les équations (2.8) et (2.9).
Où ±kj est le symbole de kronecker.
j ) = Rk±kj (2.8)
E(vk:vT
j ) = Qk±kj (2.9)
E(wk:wT
L’hypothèse d’indépendance des bruits de mesure et de modèle est prise, ce qui se
traduit explicitement par l’expression (2.10).
j ) = ±kj (2.10)
E(wk:vT
En posant cette hypothèse d’indépendance entre les bruits, le calcul réalisé dans le dé-veloppement
du filtre est simplifié, ce qui exige de satisfaire cette condition tout au long
du fonctionnement du filtre. Le filtre de Kalman consiste à calculer un certain nombre
de paramètres, de manière récursive et incrémentale. Ce traitement comporte deux étapes
principales. L’étape de prédiction, qui consiste à estimer a priori l’état du système ainsi
que la matrice de covariance de l’erreur d’estimation. Puis, l’étape dite de correction per-met
de réaliser une mise à jour par le biais des observations. La figure (2.2) illustre ces
deux phases du filtre.
Les étapes du filtre de Kalman dans son approche linéaire se résument comme suit :
Prédiction temporelle :
Cette étape permet la prédiction de l’état du système (2.11) et de sa précision (2.12)
à l’instant tk en partant de ces mêmes paramètres à l’instant tk¡1, ainsi que du modèle
d’évolution du système. On parle alors de mise à jour temporelle des paramètres.
^x¡
k = Ák¡1:x+
k¡1 + Gk:uk (2.11)
P¡
k = Ák¡1:P+
k¡1:ÁTk
¡1 + Rk¡1 (2.12)
^z¡
k = Hk:^x¡
k + Dk:uk (2.13)
^x¡
k étant l’estimée a priori du vecteur d’état, ^x+
k est l’estimée a posteriori du vecteur
d’état et ^z¡
k la mesure prédite a priori.
14
24. 2.3. Le filtre de Kalman : approche linéaire 15
Correction :
Cette étape récupère les observations disponibles à l’instant tk, et les utilise pour corri-ger
l’estimation a priori de l’état du système. En effet, l’état prédit dans l’étape précédente
est alors corrigé par sa variance associée P¡
k ainsi que l’innovation ^s¡
k , pondéré par le gain
du filtre Kk. On en déduit ainsi l’estimation de l’état ^x+
k à l’aide de la matrice de cova-riance
P+
k .
^s¡
k = zk ¡ ^z¡
k (2.14)
S¡
k = Hk:P¡
k :HT
k + Qk (2.15)
Kk = P¡
k :(S¡
k :HT
k )¡1 (2.16)
^x+
k = ^x¡
k + Kk:^s¡
k (2.17)
P+
k = (Ik ¡ Kk:Hk):P¡
k (2.18)
Où Ik est la matrice identité d’ordre k et S¡
k est la covariance de l’innovation a priori.
Il est important de noter que le résiduel zk ¡ ^z¡
k reflète l’écart entre la mesure prédite
et la mesure réelle. Un résiduel à zéro signifie que les deux mesures sont équivalentes.
Cette phase de mise à jour consiste, en un premier lieu à calculer le gain de Kalman,
de façon à minimiser la covariance de l’erreur a posteriori. A son optimum, ce gain est
égal à la covariance de prédiction d’état P¡
k , divisée par la covariance d’innovation
k :HT
S¡
k . Sa valeur converge alors vers 1 (Kk
»=
1) lorsque la covariance de prédiction de l’état
P¡
k devient équivalente à la covariance d’innovation S¡
k .
Ainsi, dans ce cas, l’examen de l’équation (2.16) stipule l’interprétation suivante
Si les mesures réalisées sont plus précises que l’erreur du système ; en d’autres termes
si Qk ¿ Pk et la gain de Kalman s’approche de 1 (Kk
»=
1), alors l’équation (2.17) est
k = ^x¡
k +S¡
k , ce qui donnerait plus de poids à la mesure
approximativement identique à ^x+
dans l’estimation de l’état.
Les tableaux (2.1) et (2.2) récapitulent le traitement réalisé lors des deux phases du
filtre.
15
25. 2.4. Linéarisation des problèmes non linéaires 16
Tableau 2.1 – Mise à jour Temporelle : "Prédiction"
Tk
(1) Estimée a priori de l’état
^x¡
= Ák¡1:x+
+ k k¡1 Gk:uk
(2) Estimée a priori de la covariance de l’erreur d’estimation
P¡
= Ák¡1P+
Á+ k k¡1¡1 Rk¡1
Tableau 2.2 – Mise à jour par les observations : "Correction"
(1) Calcul du gain de Kalman
Kk = P¡
k (HkP¡
k HT
k + Qk)¡1
k HT
(2) Estimée a posteriori de l’état par le biais des observations
^x+
k = ^x¡
k + Kk(zk ¡ Dkuk ¡ Hk^x¡
k )
(3) Estimée a posteriori de la covariance de l’erreur d’estmation
P+
k = (Ik ¡ KkHk)P¡
k
2.4 Linéarisation des problèmes non linéaires
La formulation du traitement de Kalman, telle qu’elle est décrite précédemment, ne
représente qu’une approche linéaire. Elle ne peut s’appliquer que sur un modèle parti-culier
de systèmes dynamiques. Elle est désignée par formulation traditionnelle ou clas-sique.
Dans la réalité, la plupart des systèmes présentent la propriété contraignante de non
linéarité, ce qui représente une limite à la formulation précédente. De ce fait, un certain
nombre de méthodes a été développé pour répondre à cette problématique. L’idée consiste
à linéariser d’abord le modèle, puis d’appliquer le filtre de Kalman standard afin d’obte-nir
l’état du système. L’objet de la linéarisation est un critère permettant de classifier les
différents types de filtres linéarisés. En effet, si la linéarisation est effectuée autour d’une
trajectoire nominale dans l’espace d’état qui ne dépend pas des données de mesure, le
filtre résultant s’appelle le filtre linéarisé de Kalman [13]. Dans le cas d’une linéarisation
autour d’une trajectoire estimée, continuellement mise à jour avec les estimations d’état
résultant des mesures, le filtre de Kalman est dit étendu [14]. Une autre approche de linéa-risation
consiste à utiliser un ensemble de points échantillons choisis, ce qui est désigné
par le filtre de Kalman non parfumé (Unscented Kalman Filter ou Sigma Point Kalman
Filter). Cette approche, un peu plus élaborée que les autres, est surtout utilisée pour les
systèmes fortement non linéaires.
Nous nous intéresserons dans ce qui suit à présenter uniquement le filtre de Kalman
linéarisé et le filtre de Kalman étendu.
16
26. 2.4. Linéarisation des problèmes non linéaires 17
2.4.1 Filtre de Kalman linéarisé
Dans cette première approche, la linéarisation se fait autour d’une certaine trajectoire
nominale, qui ne dépend pas des données de mesure. En d’autres termes, le développe-ment
en série de Taylor, utilisé pour la linéarisation, est évalué à un certain point nominal
connu.
Soit un modèle non linéaire décrit par le couple d’équations (2.19) et (2.20).
xk = f(xk¡1; uk) + vk¡1 (2.19)
zk = h(xk; uk) + wk (2.20)
Les deux modèles d’évolution et d’observation ne sont donc pas linéaires. Ils sont
représentés, respectivement par les deux fonctions connues et différenciables f (décrite
dans l’équation 2.19) et h (décrite dans l’équation 2.20). Toutefois, la non-linéarité peut
résider soit dans le modèle du processus soit dans le modèle de mesure, soit dans les deux
modèles régis respectivement par les fonctions f et h.
La figure (2.3) illustre un exemple de trajectoire nominale avec une erreur de ±xk par
rapport à la trajectoire réelle.
Supposons que la trajectoire approximative est obtenue par un quelconque moyen.
Elle constitue une trajectoire de référence et est désignée par la trajectoire nominale. La
trajectoire réelle peut alors être exprimée en fonction de celle de référence et d’une cer-taine
erreur ±xk, comme décrit par l’équation (2.26).
k + ±xk (2.21)
xk = xnom
En tenant compte du modèle d’évolution et de mesure du système, l’expression (2.19) se
transforme en (2.22).
xnom
k + ±xk = f(xnom
k¡1 + ±xk¡1; uk) + vk¡1 (2.22)
La linéarisation étant basée sur le développement en série de Taylor, et en supposant
que ±xk est petit, le développement de f autour de xnom
k ,donne l’expression (3.2.2).
xk = xnom
k + ±xk
= f(xnom
k¡1 ; uk) +
@f
@x
jx=xnom
k¡1
:±xk¡1 +
@2f
@x2
jxk=xnom
k¡1
:
(±xk¡1)2
2!
+ ::: + vk(¡21.23)
17
27. 2.4. Linéarisation des problèmes non linéaires 18
Figure 2.3 – Trajectoire nominale et réelle pour un filtre linéarisé de Kalman
Compte tenu de la nature de la trajectoire nominale, xnom
k =f(xnom
k¡1 ; uk)
En limitant le développement en série de Taylor au premier ordre, la prédiction de
l’état est ainsi propagée par des équations non linéaires, alors que les erreurs d’état le sont
par un système linéaire.
Ainsi, l’équation (2.24) exprime la dynamique linéarisée :
±xk ¼ ¨k¡1±xk¡1 + vk¡1 (2.24)
Où,
¨k¡1 =
·
@f
@x
¸
xk=xnom
k¡1
=
2
6666664
@f1
@x1
@f1
@x2
@f1
@x3
:::
@f2
@x1
@f2
@x2
@f2
@x3
:::
@f3
@x1
@f3
@x2
@f3
@x3
:::
:::
:::
3
7777775
xk=xnom
k¡1
(2.25)
Par analogie, la linéarisation du modèle de mesures est formulée selon l’équation
(2.26) :
k + ±zk (2.26)
zk = znom
18
28. 2.4. Linéarisation des problèmes non linéaires 19
En tenant compte du modèle d’évolution et de mesure du système, l’équation (2.20)
prend la forme de l’expression (2.27).
k + ±xk; uk) + wk (2.27)
zk = h(xnom
La linéarisation étant basée sur l’expansion en série de Taylor, et en supposant que ±zk
est petit, le développement de f autour de znom
k , donne l’expression (3.2.2).
k + ±zk (2.28)
zk = znom
= h(xnom
k ; uk) +
@h
@x
jx=xnom
k
:±xk +
@2h
@x2
jx=xnom
k
:
(±xk)2
2!
+ ::: + wk (2.29)
Ainsi, et compte tenu de l’approximation ±zk ¼ zk¡h(xnom
k ; uk), la dynamique linéa-risée
s’exprime selon l’équation (2.30). Une telle approximation s’explique par la nature
non bruitée, voire non réelle, de la trajectoire nominale.
±zk ¼ Hk±xk + wk (2.30)
Où,
Hk =
·
@h
@x
¸
x=xnom
k
=
2
6666664
@h1
@x1
@h1
@x2
@h1
@x3
:::
@h2
@x1
@h2
@x2
@h2
@x3
:::
@h3
@x1
@h3
@x2
@h3
@x3
:::
:::
:::
3
7777775
x=xnom
k
(2.31)
Ainsi, les deux équations (2.24) et (2.30) constituent une formulation linéarisée du
filtre de Kalman appliqué à notre système, en temps discret. Au terme de ce traitement, les
relations de la dynamique de l’erreur d’estimation et de l’erreur de mesure sont devenues
linéaires. Cette transformation est essentiellement basée sur la construction des matrices
Jacobiennes ¨ et H , qui ne sont autres que les dérivées partielles de f et h le long de la
trajectoire nominale. Il est à noter que ces matrices peuvent évoluer au cours du temps,
du fait de l’évolution récursive du vecteur d’état. De ce fait, à chaque étape du processus
récursif, les matrices ¨ et H sont recalculées.
Cependant, au fur et à mesure que le système évolue dans le temps, la déviation de la
trajectoire nominale par rapport à la trajectoire réelle tend à augmenter. Cette déviation
se traduit également par l’importance du poids des termes d’ordre supérieur dans le déve-loppement
en séries de Taylor. D’où la notion précédemment évoquée, dite d’innovation.
En d’autres termes, cette déviation de trajectoire traduit exactement le résiduel entre la
mesure réelle et celle prédite par la trajectoire nominale, en l’absence de bruit.
19
29. 2.4. Linéarisation des problèmes non linéaires 20
Ce risque de voir le système diverger au bout d’un certain nombre d’itérations, limite
l’utilisation du filtre de Kalman linéarisé dans certaines missions prolongées.
Toutefois, nous supposerons, dans le cas de notre système, que cette déviation reste
relativement petite durant toute la période d’intérêt.
Enfin, ce type de filtre présente l’avantage d’une exécution en temps réel, quoiqu’il
manque de robustesse vis à vis des approximations non linéaires des erreurs du modèle
et de mesure. Pour remédier à cette problématique, une autre version du filtre de Kalman
a été développée, c’est le filtre de Kalman étendu, qu’on explicitera dans le paragraphe
(2.4.2).
2.4.2 Filtre de Kalman étendu "EKF"
Le filtre de Kalman étendu "EKF", développé par Stanley F.Schmidh [14], représente
une évolution du filtre de Kalman linéarisé, dans le sens où il ramène la modélisation d’un
système non linéaire à un problème plutôt linéaire. Il diffère du filtre de Kalman linéarisé,
par le fait que le processus de linéarisation se fait autour d’une trajectoire estimée plutôt
qu’autour d’une trajectoire nominale. En d’autres termes, les dérivées partielles sont cal-culées
le long de la trajectoire qui a été mise à jour avec l’estimation du filtre. Il s’agit
donc d’exploiter les mesures disponibles à chaque itération pour calculer les paramètres à
évaluer, entre autres le gain de Kalman. L’idée est alors basée sur le principe de linéariser
toutes les équations non linéaires, afin de ramener le système à une approche linéaire, de
telle sorte que la version traditionnelle du filtre de Kalman soit applicable. La formulation
initiale est bien conforme à celle décrite précédemment par le filtre de Kalman linéarisé,
telle est décrite par le couple d’équations (2.32) et (2.33).
xk = f(xk¡1; uk; vk¡1) (2.32)
zk = h(xk; uk;wk) (2.33)
où vk¡1 et wk représentent respectivement les bruits du modèle et de mesure.
La fonction f est utilisée pour l’état prédit à partir de l’état estimé à l’itération précé-dente,
et semblablement, la fonction h servira à calculer l’observation prédite de l’état pré-dit.
Autrement, un passage aux matrices de covariance requiert le calcul des jacobiennes
respectives de f et h.
En pratique, nous ne disposerons bien évidemment pas des valeurs exactes du bruit à
chaque itération. Toutefois, nous pouvons approximer les vecteurs d’état et de mesure xk
et zk, en annulant ces termes relatifs au bruit.
20
30. 2.4. Linéarisation des problèmes non linéaires 21
~xk = f(^xk¡1; uk; 0) (2.34)
~zk = h(~xk; uk; 0) (2.35)
Le processus de linéarisation des équations (2.32) et (2.33) est effectué en utilisant
l’approximation décrite par les équations (2.34) et (2.35).
xk = ~xk + A(xk¡1 ¡ ^xk¡1) + vVk¡1 (2.36)
zk = ~zk + H(xk ¡ ~xk) + wWk (2.37)
^xk : une estimée a posteriori de l’état à l’instant tk
~xk : une approximation de l’état, obtenue en ignorant le bruit du modèle.
A : la matrice jacobienne des dérivées partielles de f par rapport à x :
A[i;j] =
@f[i]
@x[j]
(^xk¡1; uk; 0) (2.38)
V : la matrice jacobienne des dérivées partielles de h par rapport à v :
V[i;j] =
@h[i]
@v[j]
(~xk; uk; 0) (2.39)
H : la matrice jacobienne des dérivées partielles de h par rapport à x :
H[i;j] =
@h[i]
@x[j]
(~xk; uk; 0) (2.40)
W : la matrice jacobienne des dérivées partielles de h par rapport à w :
W[i;j] =
@h[i]
@w[j]
(~xk; uk; 0) (2.41)
Notons que les matrices Jacobiennes A, V, H etWpeuvent évoluer au cours du temps ;
l’omission de l’indice k de chaque étape n’est autre qu’une forme de simplification des
expressions.
Une fois l’approximation est faite, on définit une nouvelle forme de l’erreur de pré-
21
31. 2.4. Linéarisation des problèmes non linéaires 22
diction qui est décrite par l’expression (2.42).
~exk ´ xk ¡ ~xk (2.42)
Ainsi que le résiduel relatif à la mesure par l’équation (2.43) :
~ezk ´ zk ¡ ~zk (2.43)
Dans les équations (2.42) et (2.43), le terme xk étant inconnu puisqu’il représente
la valeur actuelle de l’état qu’on cherche à estimer, nous disposons plutôt de la mesure
actuelle zk. Compte tenu de la disponibilité des observations, nous aboutissons aux ex-pressions
(2.44) et (2.45).
~exk ¼ A(xk¡1 ¡ ^xk¡1) + ²k (2.44)
~ezk ¼ H~exk + ´k (2.45)
Où ²k et ´k représentent deux variables aléatoires indépendantes, de moyenne nulle et
de covariances respectives V RV T et WQWT , avec R et Q les covariances d’erreur des
bruits du modèle et de mesure.
Ainsi, au terme de ces expressions, nous aboutissons à un système d’équations li-néaires,
décrit par (2.44) et (2.45). L’avantage de cette linéarisation réside dans le fait
qu’elle nous ramène à un système linéaire, comme celui décrit initialement par les équa-tions
(2.1) et (2.2). Compte tenu de cette nouvelle formulation, nous nous proposons
d’utiliser le résidu des mesures ~ezk en vue d’estimer l’erreur de prédiction ~exk décrite
par l’équation (2:44). Cette estimation d’erreur, désignée par ^ek , sera par la suite intégrée
dans l’équation (2.42), ce qui permettra de retourner une estimée a posteriori du processus
initial , à savoir celui décrivant le système non linéaire de départ.
^xk = ~xk + ^ek (2.46)
Les variables aléatoires présentées dans les équations (2.44) et (2.45) sont régies par
une loi de probabilité gaussienne, de moyenne nulle
k ]) (2.47)
~exk » N(0;E[~exk ~eTx
²k » N(0; V RkV T ) (2.48)
´k » N(0;WQkWT ) (2.49)
22
32. 2.4. Linéarisation des problèmes non linéaires 23
En ajoutant l’hypothèse d’annuler la valeur prédite de ^xk, l’application du filtre de
Kalman traditionnel pour estimer l’erreur retourne ainsi l’équation (2.50).
^ek = Kk~ezk (2.50)
En substituant les égalités (2.50) et (2.43) dans l’équation (2.46), nous aboutissons à
l’expression (2.52).
^xk = ~xk + Kk~ezk (2.51)
= ~xk + Kk(zk ¡ ~zk) (2.52)
Cette dernière équation sera ainsi utilisée par le filtre de Kalman étendu, dans la phase
de mise à jour des observations, et ceci en faisant appel aux équations (2.34) et (2.35),
ainsi que le gain de Kalman Kk décrit dans l’équation (2.16), en substituant la valeur de
la covariance de l’erreur de mesure.
Le filtre de Kalman étendu peut alors être récapitulé par les deux tableaux (2.3) et
(2.4). Notons la substitution de ~xk par ^x¡
k afin de se conformer à la notation de l’estimée a
priori utilisée dans l’approche linéaire. Nous avons aussi souligné la variation temporelle
des matrices jacobiennes en les indexant par l’indice k, ils seront ainsi recalculés à chaque
itération.
Tableau 2.3 – Mise à jour Temporelle : "Prédiction"
Tk
(1) Estimée a priori de l’état
^x¡
= 0)
k f(^xk¡1; uk; (2) Estimée a priori de la covariance de l’erreur d’estimation
P¡
= + T
k AkPk¡1AVkRkV k
Cette première phase du filtre de Kalman étendu décrite par le tableau (2.3) est conforme
à la phase de prédiction dans l’approche classique du filtre de Kalman, dans le sens où
elle permet de prédire l’état et la covariance d’erreur à la kme itération à partir de l’état à
l’itération précédente et du modèle d’évolution du système décrit par la fonction f.
Toujours conformément au filtre de Kalman dans son approche linéaire, le tableau
(2.4) rejoint la phase corrective qui consiste à corriger l’estimée a priori de l’état x¡
k et de
la covariance de l’erreur P¡
k par le biais des observations disponibles à l’instant actuel.
Ainsi, le filtre de Kalman étendu est applicable pour un nombre important de pro-blèmes
d’observation, notamment pour les missions prolongées. Cependant, comme tout
algorithme exploitable, ce type de filtre présente une certaine limite :
23
33. 2.5. Conclusion 24
Tableau 2.4 – Mise à jour par les observations : "Correction"
(1) calcul du gain de Kalman
Kk = P¡
k (HkP¡
k HT
k + (WkQkWT
k ))¡1
k HT
(2) Estimée a posteriori de l’état par le biais des observations
^xk = ^x¡
k + Kk(zk ¡ h(^xk; uk; 0))
(3) Estimée a posteriori de la covariance de l’erreur d’estmation
P+
k = (Ik ¡ KkHk)P¡
k
– Tout au long du fonctionnement du filtre, une variable aléatoire gaussienne est pro-pagée
par la dynamique du système. Cette propagation est effectuée lors de la li-néarisation
du premier ordre du système non linéaire. Les termes négligés dans la
linéarisation peuvent être relativement grands, ce qui risque d’augmenter les erreurs
dans l’estimation des paramètres, et par conséquent mener à une performance sub-optimale,
voir même à la divergence du filtre.
– Le filtre de Kalman étendu est relativement coûteux en temps de calcul. En effet,
contrairement au filtre linéarisé, la linéarisation est effectuée autour des estima-tions
obtenues à partir du filtre, nous ne pouvons donc pas estimer les paramètres à
l’avance.
– Les dérivations partielles effectuées lors du calcul des matrices Jacobiennes consti-tuent
des opérations lourdes à implémenter, au sens de complexité de calcul, pour
certaines applications.
2.5 Conclusion
Le filtre de Kalman est ainsi connu sous plusieurs versions, sachant nous n’avons
détaillé que certaines d’entres elles. L’approche étendue répond le plus à notre système, du
fait qu’elle soit bien adaptée aux contraintes de non linéarité. D’autres approches, encore
plus élaborées, ont été développés, certes. Mais, leur exploitation est notamment utile
lorsqu’il s’agit de systèmes fortement non linéaires et dont le modèle est plus contraignant.
Par ailleurs, une fois le principe global du filtre explicité, il faudra modéliser notre
système dynamique, en retrouvant les modèles d’évolution et d’observation du mobile
considéré.
24
34. Chapitre 3
Implémentation du module embarqué et
résultats
3.1 Introduction
Notre projet s’inscrit dans le cadre du partenariat "Sound Delta". La mission consiste
à la mise en oeuvre d’une carte de localisation fine par GPS. Pour ce faire, une phase
de simulation du traitement numérique des données s’impose, en vue de vérifier le bon
fonctionnement du filtre de Kalman, et de conclure sur sa validation, une fois la préci-sion
de localisation recherchée est atteinte. Dans ce dernier chapitre, nous présenterons
les formulations analytiques de notre système de navigation, en tenant compte de la na-ture
du mouvement considéré. Par la suite, une fois l’implémentation de l’ensemble de la
description analytique faite, nous illustrerons les résultats de la simulation, vérifiant ainsi
le fonctionnement du filtre de Kalman simulé.
3.2 Modélisation du système
L’étape de modélisation est d’une importance majeure dans notre travail. Les pro-blèmes
de localisation sont, en effet, dépendant de certaines contraintes liées directement
à la dynamique du système. D’où s’exprime le besoin de décrire cette problématique ana-lytiquement
de manière à pouvoir appliquer le filtre de Kalman dans sa version adéquate.
3.2.1 Transformations entre les systèmes de coordonnées
Dans un système là où il s’agit de combiner système de navigation terrestre utilisé
par le GPS et système de navigation inertielle associé aux capteurs, il est important de
25
35. 3.2. Modélisation du système 26
maintenir, tout au long de l’acquisition des différentes mesures, l’information d’orienta-tion.
En d’autres termes, afin d’assurer la bonne maîtrise de l’information d’orientation
dans un système de type "Strapdown" (là où les capteurs sont fixés rigidement au mobile),
nous sommes ramenés à connaître à chaque instant le rapport entre les axes de mesures
et ceux du système de localisation. Mathématiquement, ceci revient à exprimer la matrice
de transformation de la base du mobile vers la base terrestre.
Il existe plusieurs outils mathématiques permettant de faire cette transformation, parmi
lesquels on distingue les angles d’Euler et les quaternions (voir Annexe C). Dans notre
travail, nous avons opté particulièrement pour le choix des quaternions pour un intérêt
de simplicité et d’efficacité [15]. En effet, ceux-ci présentent, d’une part, une propriété
particulière de rotation qui en fait un outil complet de description des trois orientations
possibles. D’autre part, la dérivation des quaternions revient à réaliser des multiplications
de matrices d’ordre 4, où d’ordre 3 dans la cas d’un quaternion unitaire, ce qui est net-tement
plus simple que la dérivation des angles d’Euler faisant appel à des expressions
beaucoup plus complexes en cosinus et sinus, et surtout coûteuse en temps de calcul.
Ainsi, les formules de quaternions donnent la Matrice des Cosinus Directeurs "DCM"
décrite dans l’équation (3.1).
C =
2
64
q2
0 + q2
1 + q2
2 + q2
3 2(q1q2 ¡ q0q3) 2(q0q2 + q1q3)
2(q1q2 + q0q3) q2
0 ¡ q2
1 + q2
2 ¡ q2
3 2(q2q3 + q0q1)
2(q1q3 ¡ q0q2) 2(q0q1 + q2q3) q2
0 ¡ q2
1 ¡ q2
2 + q2
3
3
75
(3.1)
Cependant, une des particularités du mouvement étudié est qu’il est associé à un mo-bile
rigide, c’est-à-dire ne se déforme pas lors de son déplacement. Le quaternion est alors
unitaire.
3.2.2 Formulation du vecteur d’état
Le projet consiste à combiner récepteur GPS et capteurs inertiels, en vue d’estimer
la position et l’orientation du mobile. Pour ce faire, il est important de bien fixer les
paramètres à introduire dans le vecteur d’état.
Compte tenu du fait que le réajustement va se faire avec les données GPS, les pa-ramètres
de position et de vitesse doivent nécessairement figurer dans le vecteur d’état.
En outre, les quaternions, qui sont particulièrement responsables de l’orientation du mo-bile,
doivent aussi être recalculés à chaque itération, afin de tenir compte du changement
d’orientation. Finalement, les biais causés par le gyroscope et l’accéléromètre, du fait de
leur évolution dans le temps, permettent de corriger la mesure fournie par chacun des cap-teurs.
Leur mise à jour constitue alors un moyen d’améliorer la précision de l’estimation
des éléments du filtre.
26
36. 3.2. Modélisation du système 27
La formulation du vecteur d’état résultant prend alors la forme (3.2).
XT = (|u {vzw}
V
|x {yz z}
¥
|q0 q1{zq2 q3}
q
|ºx {ºzy ºz}
º
|¹x ¹{zy ¹z}
¹
) (3.2)
Avec :
V la vitesse exprimée dans la base mobile
¥ la position en coordonnées terrestres projetées à plat
q le quaternion des altitudes
º le vecteur biais du gyroscope
¹ le vecteur biais de l’accéléromètre
3.2.3 Etude de la dynamique du système
Le système étudié présente une certaine dynamique, qui, compte tenu de sa non linéa-rité,
requiert la formulation adéquate des fonctions d’évolution des différents paramètres
à estimer, dans le temps. Cette première étape est cruciale, dans le sens où elle précise
explicitement le modèle dynamique. Dans le cas de navigation inertielle, la fonction f de
l’équation (3.3) est construite en faisant appel à la matrice de transformation C.
_X
(t) = f(t;X(t); u(t)) (3.3)
En effet, la matrice DCM, comme explicité dans l’expression (3.1), permet de décrire
la dynamique du système. Elle constitue une matrice de passage de la base du mobile à la
base terrestre.
Les deux bases, étant orthonormées, la DCM vérifie alors la propriété d’orthogonalité
C¡1 = CT .
Exprimons maintenant par groupes d’états les diverses équations de la dynamique du
système.
3.2.3.1 Evolution de la vitesse
L’expression de la vitesse du mobile est régie par l’équation de l’accélération dans
le repère du mobile. En effet, l’accéléromètre nous fournit la valeur de l’accélération
non gravitationnelle, exprimé dans le système inertiel (voir Annexe A). Transposons cette
accélération dans la base du mobile, comme explicité dans l’égalité (3.32).
A =
2
64
ax
ay
az
3
75
inertiel
=
·
@V
@t
¸
mobile
+ (gyro ¡ º) ^ V ¡ CT :
2
64
0
0
g
3
75
(3.4)
27
37. 3.2. Modélisation du système 28
Avec gyro = (wxwywz) est la vitesse angulaire du mobile, mesurée par le gyroscope.
L’opération ^ désigne le produit vectoriel.
La dynamique de la vitesse prend alors la forme (3.5).
·
@V
@t
¸
mobile
=
2
64
u_
v_
w_
3
75
=
2
ax
ay
az
64
3
75
inertiel
+ CT :
2
64
0
0
g
3
75
¡ (gyro ¡ º) ^ V (3.5)
Ainsi, la vitesse du mobile n’est autre qu’une forme d’intégration de la mesure accé-léromètrique,
corrigée par le biais du gyrocospe. L’équation (3.5) montre la contribution
de chaque appareil de mesure dans l’expression de la dynamique de la vitesse du mobile.
3.2.3.2 Evolution de la position
l’évolution de la position du mobile est régie par l’équation de la dynamique (3.6).
·
@¥
@t
¸
mobile
= C:V (3.6)
3.2.3.3 Evolution des quaternions
Compte tenu de la dépendance des éléments de positionnement du vecteur quaternion,
celui-ci constitue un paramètre extrêmement important à estimer à chaque itération.
En effet, la dérivation des quaternions par rapport au temps fait apparaître une partie
arbitraire dans la direction du vecteur quaternion à dériver. Pour ce faire, nous définissons
un facteur k (en seconde¡1) qui, multiplié par ¸, determine la direction du quaternion,
tel que k <= 1
¢t , où k désigne le temps qui s’écoule entre deux calculs consécutifs de la
dérivée, et ¸ = 1 ¡ (q2
0 + q2
1 + q2
2 + q2
3).
@q
@t
= fonction(gyro ¡ º)
=
1
2
2
¡q1 ¡q2 ¡q3
q0 ¡q3 q2
q3 q0 ¡q1
¡q2 q1 q0
66664
3
77775
:
2
64
!x ¡ ºx
!y ¡ ºy
!z ¡ ºz
3
75
+ k:¸:
2
66664
q0
q1
q2
q3
3
77775
(3.7)
Toutefois, comme le quaternion est unitaire dans le cas de notre système, le multipli-cateur
¸ annulera le terme en Q.
28
38. 3.2. Modélisation du système 29
3.2.3.4 Evolution des biais
Le biais est une forme d’erreur qui est d’autant plus évidente que contraignante. En
effet, comme décrit dans le paragraphe (1.3.3), le biais peut être modélisé selon la com-plexité
de son évolution. Pour l’accéléromètre, nous avons choisi le premier modèle, qui
repose sur une variable aléatoire gaussienne. Le gyroscope, étant beaucoup plus sen-sible
aux contraintes de la température et aux conditions de mesure, exige l’utilisation
du deuxième modèle "random Walk" qui répond mieux à la qualité de l’appareil de me-sure.
3.2.4 Linéarisation autour de l’état estimé
Cette phase de linéarisation, comme précédemment décrite dans le filtre de Kalman
étendu (§2.4.2), a pour objectif de réaliser une mise à jour temporelle. Cette phase de
pédiction consiste à calculer une estimée a priori du vecteur d’état, en partant de l’état du
système à l’instant précédent ainsi que du modèle d’évolution du système.
Cette Mise à jour temporelle est régit par l’équation (C.2).
_X
=
2
6666664
_V
_¥
q_
º_
¹_
3
7777775
=
2
6666664
gyro ¡ º 0 Aºq AVº ¡I3
C 0 A¥q 0 0
0 0 Aq Aqº 0
0 0 0 0 0
0 0 0 0 0
3
7777775
:
| {z }
A
2
6666664 V
¥
q
º
¹
3
7777775
(3.8)
Explicitons les différents termes intervenant dans la matrices d’évolution du vecteur
d’état X de l’équation (3.2).
(gyro ¡ º) ^ V =
2
64
!x ¡ ºx
!y ¡ ºy
!z ¡ ºz
3
75
^
2
64
u
v
w
3
75
(3.9)
=
2
64
0 ¡(!z ¡ ºz) !y ¡ ºy
3
75
!z ¡ ºz 0 ¡(!x ¡ ºx)
¡(!y ¡ ºy) (!x ¡ ºx) 0
:V (3.10)
=
2
64
(!y ¡ ºy):w ¡ (!z ¡ ºz):v
(!z ¡ ºz):u ¡ (!x ¡ ºx):w
(!x ¡ ºx):v ¡ (!y ¡ ºy):u
3
75
(3.11)
Cette matrice traduit l’évolution du vecteur vitesse.
29
40. 3.2. Modélisation du système 31
Aq =
1
2
@
@q
2
66664
¡q1(wx ¡ ºx) ¡ q2(wy ¡ ºy) ¡ q3(wz ¡ ºz) + 2k¸q0
q0(wx ¡ ºx) ¡ q3(wy ¡ ºy) + q2(wz ¡ ºz) + 2k¸q1
q3(wx ¡ ºx) + q0(wy ¡ ºy) ¡ q1(wz ¡ ºz) + 2k¸q2
¡q2(wx ¡ ºx) + q1(wy ¡ ºy) + q0(wz ¡ ºz) + 2k¸q3
3
77775
=
1
2
2
66664
2k¸ ¡(wx ¡ ºx) ¡(wy ¡ ºy) ¡(wz ¡ ºz)
3
77775
(wx ¡ ºx) 2k¸ (wz ¡ ºz) ¡(wy ¡ ºy)
(wy ¡ ºy) ¡(wz ¡ ºz) 2k¸ (wx ¡ ºx)
(wz ¡ ºz) (wy ¡ ºy) ¡(wx ¡ ºx) 2k¸
=
1
2
2
66664
q1 q2 q3
¡q0 q3 ¡q2
¡q3 ¡q0 q1
q2 ¡q1 ¡q0
3
77775
(3.18)
Ainsi, au terme de cette mise à jour, nous aboutissons à une estimée a priori du vecteur
d’état, il suffit d’intégrer le _X
.
3.2.5 Mise en oeuvre du filtre
3.2.5.1 La phase de propagation
La phase de propagation est la première phase dans la mise en oeuvre du filtre. Elle
exige la bonne connaissance de la dynamique de l’état du système. Ainsi, elle prend en
entrée la dérivée du vecteur d’état calculée dans la section (3.2.4). L’objectif de cette étape
est de calculer une estimée a priori du vecteur d’état Xk et de la covariance d’erreur d’es-timation
Pk. Pour ce faire, il suffit d’intégrer les dérivées obtenues à l’issue de la phase
précédente. Ils existent plusieurs approches réalisant cette opération d’intégration. Tou-tefois,
plus les capteurs utilisent des hautes fréquences, plus la dynamique du système
global est importante, plus le calcul d’intégrale doit être précis. En d’autres termes, nous
travaillons à des fréquences relativement hautes, entre 20 Hz et 200 Hz, surtout par rap-port
à la fréquence d’un GPS qui est de l’ordre de quelques Hz. Cette haute dynamique
requiert l’utilisation d’algorithmes d’intégration, plus élaborés qu’une simple méthode
des rectangles, telle explicitée par la formule (3.19).
Xi+1 = Xi + _X
i:dt; (3.19)
Pour ce faire, il est possible d’utiliser la méthode Runge-Kutta, d’ordre 2 ou 4 (voir
Annexe C).
31
41. 3.2. Modélisation du système 32
Cette phase de propagation commence généralement par estimer le vecteur d’état X,
ensuite il convient d’utiliser cette estimée a priori de l’état pour propager la covariance de
l’erreur d’estimation P.
Les équations de propagation qui régissent cette phase prennent la forme de l’expres-sion
(3.20).
P¡
k = Pk¡1 + (Ak¡1:Pk¡1 + (Ak¡1:Pk¡1)T + R(t)):dt (3.20)
Où R(t) est la matrice de covariance du bruit de modèle.
L’estimée retournée après propagation, désignée par P¡
k est un paramètre qui quantifie
la confiance que l’on peut apporter à l’estimée du vecteur d’état X. Cette relation traduit
l’importance d’évaluer la covariance d’erreur au fur et à mesure de l’estimation de l’état.
Ainsi, au terme de ces opérations, nous aboutissons aux estimées a priori de l’état du
système et à la convariance d’erreur associée. Ces estimées constitueront les entrées de la
phase de mise à jour par réajustement avec le GPS.
3.2.5.2 Mise à jour : phase de correction par les mesures GPS
Cette dernière étape consiste à corriger les estimées a priori des éléments du filtre, en
introduisant les mesures fournies par le récepteur GPS. Généralement, le filtre effectue
une telle correction à chaque acquisition des données GPS, c’est à dire à une cadence
égale à la fréquence de fonctionnement du GPS. Cette fréquence est souvent de l’ordre du
Hertz, parfois au plus de quelques Hertz.
La phase de correction est effectuée sur le vecteur d’état, et plus précisément sur la
position et la vitesse du mobile, qui seront recalés par les données GPS. Compte tenu
du traitement explicité dans le tableau (2.4), nous avons opté pour un traitement plutôt
séquentiel, qui opère sur les différents paramètres séparément. L’intérêt d’un tel choix
consiste à alléger la complexité de calcul en ramenant le calcul effectué sur des matrices
d’ordre 16 (du fait de la taille du vecteur d’état), à des opérations matricielles d’ordre 3. En
outre, si la capacité de calcul du calculateur ne supporte pas ce traitement séquentiel, il est
possible de découpler chacun des axes et passer à des manipulations scalaires, plutôt que
matricielles. Ces méthodes d’optimisation des ressources sont d’une importance majeure
dans l’implémentation des modules embarqués. Dans notre cas, nous avons opté pour la
première méthode, à savoir un traitement séquenciel par paramètre, sachant que chaque
paramètre est projeté sur les trois axes du système de coordonnées (voir annexe A).
1. Mise à jour de la vitesse :
Nous commençons par calculer l’innovation, en soustrayant la mesure prédite de la
mesure réelle. Le résiduel est noté Zvit.
32
42. 3.2. Modélisation du système 33
Zvit = ¡
2
Vgpsx ¡ u
Vgpsy ¡ v
Vgpsz ¡ w
64
3
75
(3.21)
Où Vgpsx, Vgpsy et Vgpsz sont les mesures délivrées par le récepteur GPS.
Ensuite, la matrice d’observation H prend simplement la forme de l’expression
(3.22).
Hvit =
2
64
3
1 0 0 0 ::::: 0
0 1 0 0 ::::: 0
0 0 1 0 ::::: 0
75
(3.22)
Par la suite, on calcule le gain de Kalman associé à la vitesse du mobile, en intro-duisant
le bruit de mesure de la vitesse Qvit.
Kvit = P¡
vit:(Hvit:P¡
k :HT
vit + Qvit)¡1 (3.23)
k :HT
Enfin, il convient de corriger le vecteur d’état, plus précisément la composante "vi-tesse"
dans le vecteur d’état, ainsi que la covariance associée.
k ¡ Kvit:Hvit:P¡
P+
k = P¡
k (3.24)
k = X¡
X+
k ¡ Kvit:Zvit (3.25)
Cette méthode de recalage par mesures GPS permet de calculer l’estimée a poste-riori
des éléments du filtre, désignées dans le tableau (2.4) par ^xk et Pk. La valeur de
cette vitesse est ainsi exprimée selon les trois axes, dans le système de coordonnées
terrestre, en mètres par seconde, vers le Nord terrestre.
2. Mise à jour de la position :
L’approche suivie pour réaliser la mise à jour de la position est une simple extension
de celle abordée dans le cas de la vitesse. La seule différence réside dans le calcul
de l’innovation Zpos qui obéit à la forme décrite dans (3.26).
33
43. 3.2. Modélisation du système 34
Zpos = ¡
2
64
(¸GPS ¡ ¸0):k ¡ px
(¹GPS ¡ ¹):cos¸GPS:k ¡ py
Zalt ¡ pz
3
75
(3.26)
La matrice H correspondante à la position est régie par l’expression (3.27).
Hpos =
2
64
0 0 0 1 0 0 0 ::::: 0
0 0 0 0 1 0 0 ::::: 0
0 0 0 0 0 1 0 ::::: 0
3
75
(3.27)
Par la suite, on calcule le gain de Kalman associé à la position du mobile, en intro-duisant
la covariance du bruit de mesure de la position Qpos.
Kpos = P¡
pos:(Hpos:P¡
k :HT
pos + Qpos)¡1 (3.28)
k :HT
Ainsi, une fois le gain de Kalman calculé, l’étape finale consiste à calculer les esti-mées
recherchées (equations 3.29 et 3.30).
k ¡ Kpos:Hpos:P¡
P+
k = P¡
k (3.29)
k = X¡
X+
k ¡ Kpos:Zpos (3.30)
Au terme de ce traitement, le filtre de Kalman retourne ses estimations des différents
paramètres.
Récapitulons tout le traitement réalisé, en énumérant les différents phases de l’estima-teur
:
1. Initilisations des éléments du filtre :
Les quaternions,
^X
0,
P0,
Le biais du gyroscope : ( ºx ºy ºz ),
Le biais de l’accéléromètre : ( ¹x ¹y ¹z ).
34
44. 3.3. Les initialisations 35
2. Mise en oeuvre du filtre :
ÃAcquisition des signaux GPS
For (Compteur incrémenté avec l’acquisition des mesures GPS)
{
ÃAcquisition des signaux accéléromètre+Gyroscope
For (Compteur incrémenté avec l’acquisition des mesures inertiels)
{
Etude de al dynamique du système :
Evolution de la vitesse (eq 3.5)
Evolution de la position (eq 3.6)
Evolution des quaternions (eq 3.7)
Evolution des biais
Linéarisation autour de l’état estimé : (eq 3.8
Phase de propagation
(eq 3.20)
(eq 3.19)
}=) Les estimées a priori Pk et ^X
k
Mise à jour par les observations GPS :
Mise à jour de la vitesse (3.25)
Mise à jour de la position (3.30)
}=) Les estimées a posteriori Pk et ^X
k
3.3 Les initialisations
Cette étape est importante, voire cruciale pour le fonctionnement du filtre. En effet,
une fois l’expression des équations régissant le déroulement des étapes de l’estimation
du processus d’état faite, il ne reste plus qu’à initialiser notre système afin de pouvoir
démarrer le calculateur. Voici la liste des paramètres à initialiser :
35
45. 3.3. Les initialisations 36
3.3.1 Le vecteur d’état
Etant composé de 16 composantes, comme déjà explicité au §3.2.2, le vecteur d’état
est initialisé en tenant compte de la position initiale du mobile, son orientation ainsi que les
biais gyroscopique et accélérométrique, tels qu’ils sont données par les modèles choisis.
Plus précisément, le mobile étant initialement à une position bien déterminée, ses vitesses
linéaires et angulaires sont mises à zéro. Les quaternions sont initialisés grâce à la mesure
de l’accélération. En effet, étant au repos, le mobile perçoit la réaction de la terre à son
poids, ce qui permet de déterminer exactement, à l’instant initial, la valeur du quaternion
des attitudes. La mesure de l’accéléromètre résulte essentiellement de la mesure de la
force gravitationnelle selon la formule (3.31).
mesureacc¶el¶eromµetre =
0
B@
ax
ay
az
1
CA
' CT :
0
B@
0
0
¡ k g k
1
CA
(3.31)
La matrice C des Cosinus directeurs s’écrit en fonction des quaternions, on obtient direc-tement
la formule (3.32) permettant d’initialiser les altitudes.
mesureacc¶el¶eromµetre = ¡ k g k :
0
B@
¡2q1:q3 + 2:q0:q2
¡2q0:q1 ¡ 2:q2:q3
¡q2
0 + ¡q2
1 + ¡q2
2 ¡ q2
3
1
CA
(3.32)
Compte tenu du fait que le quaternion est unitaire, notre système à trois équations
présente alors 3 inconnues. En plus, en supposant que le mobile est orienté vers l’axe des
x selon la représentation en coordonnées polaires par la figure (3.1), on fixe déjà l’angle
à à zéro. De ce fait, il ne reste plus qu’à déduire les angles µ et Á.
Figure 3.1 – Représentation polaire dans l’espace des quaternions
36
46. 3.4. Résultats de simulation 37
q = cosµ + (i® + j¯ + k°)sinµ (3.33)
Où :
® = cosÁ
¯ = sinÁcosÃ
° = sinÁcosÃ
Le développement de l’équation (3.32) nous permet de déduire les expressions (3.34)
et (3.35).
Á = cot(¡ax= ¡ ay) (3.34)
µ = 0:5 £ arccos(¡az=g) (3.35)
3.3.2 Les paramètres initiaux d’estimation
Il est important d’initialiser l’estimée du vecteur d’état, appelée ^X
0 ainsi que la cova-riance
d’erreur d’estimation P0.
Pour permettre au filtre de converger, éventuellement, lors de son fonctionnement, il
est indispensable d’éviter de fixer P0 à 0A. Nous avons choisi d’initialiser les éléments
diagonaux à la valeur 10¡5.
3.3.3 Les paramètres intrinsèques aux capteurs utilisés
Il reste ainsi à introduire la covariance de l’erreur de mesure Q, ainsi que les biais de
l’accéléromètre et du gyroscope.
3.4 Résultats de simulation
L’algorithme d’estimation, qui a été décrit dans la section (3.2), a été implémenté en
langage C ANSI. Le travail réalisé consiste à simuler le fonctionnement du filtre en vue
de vérifier quelques performances. L’intérêt est alors de parvenir à estimer la position du
mobile, avec la meilleure précision possible.
Pour ce faire, nous avons généré une trajectoire, qui additionnée par un bruit gaussien,
forme la sortie des différents instruments de positionnement. Une fois les signaux injectés,
il est important de bien initialiser le filtre, afin d’éviter une éventuelle divergence, comme
nous l’avons décrit dans la section (3.3).
Nous avons choisi de tester le filtre pour différents types de mouvement du mobile.
A0 ici, est l’élément absorbant de l’espace matriciel <m:n
37
47. 3.4. Résultats de simulation 38
Tout d’abord, compte tenu du fait que seul le récepteur GPS nous informe sur le pa-ramètre
"altitude", nous présentons séparément son estimée, dont la précision reste inva-riable,
avec ou sans les capteurs inertiels. Comme le mobile se déplace dans un milieu
urbain, nous avons considéré que son altitude garde la valeur (207.48m). La figure (3.2)
illuste l’estimation de l’altitude du mobile.
0 10 20 30 40 50 60 70 80 90 100
220
218
216
214
212
210
208
206
204
202
200
itération
altitude (en m)
signal GPS
estimée de l’altitude
Figure 3.2 – Estimation de l’altitude du mobile
3.4.1 Cas d’un mouvement uniforme
Dans ce cas de figure, seul le récepteur GPS fournit des mesures. L’estimée de la
vitesse et de la position du mobile n’est alors rien d’autre qu’une simple interpolation des
données GPS. Le filtre d’intervient pas vraiment pour faire de traitement util, puisque les
condition de son optimalité ne sont pas vérifiées.
L’estimation retourne les courbes (3.4.1) et (3.4.1) , qui traduisent simplement la pré-cision
de l’information GPS, sans aucune amélioration ajoutée.
3.4.2 Cas d’un mouvement non uniforme
Considérons un mobile qui se déplace avec une accélération constante, et une vitesse
angulaire constante.
38
48. 3.4. Résultats de simulation 39
0 10 20 30 40 50 60 70 80 90 100
1.5
1
0.5
itération
0 10 20 30 40 50 60 70 80 90 100
1
0.9
0.8
0.7
0.6
0.5
itération
vitesse en X (en m/s)
signal GPS
estimée de la vitesse en X
vitesse en Y (en m/s)
signal GPS
estimée de la vitesse
Figure 3.3 – Estimation de la vitesse du mobile
0 10 20 30 40 50 60 70 80 90 100
3
2.8
2.6
2.4
2.2
2
itération
0 10 20 30 40 50 60 70 80 90 100
4.8
4.6
4.4
4.2
4
itération
Position en X (en m)
signal GPS
estimée de la Position en X
Position en Y (en m)
signal GPS
estimée de la position en Y
Figure 3.4 – Estimation de la position du mobile
39
49. 3.4. Résultats de simulation 40
La trajectoire générée, est bien évidemment connue. On commence par générer la
mesure accéléromètrique, qui une fois intégrée et additionnée par un bruit gaussien, nous
permet de déduire les données GPS. Comme un récepteur GPS est généralement non
précis à long terme, les mesures qu’il délivre dans son repère inertiel permettent deduire
la mesure du GPS.
0 10 20 30 40 50 60 70 80 90 100
1.6
1.4
1.2
1
0.8
0.6
0.4
0.2
0
−0.2
−0.4
itération
vitesse en X (en m/s)
signal GPS
estimée de la vitesse en X
Figure 3.5 – Estimation de la vitesse en X du mobile, pour w » N(0; 1)
Afin d’examiner le comportement du filtre, nous avons représenté l’estimée de la vi-tesse,
en X et en Y, pour trois valeurs différentes de l’erreur de mesure Q.
Les figures (3.5), (3.6) et (3.7) représentent les estimées de la vitesse du mobile se-lon
l’axe X, pour trois valeurs différentes de l’erreur de mesures, respectivement, w »
N(0; 1), w » N(0; 10¡2) et w » N(0; 10¡4).
De la même manière, nous avons représenté ces estimée pour la vitesse du mobile
selon l’axe des Y par les figures (3.5), (3.6) et (3.7).
Ainsi, notons que l’estimée de la vitesse du mobile est d’autant plus précise que l’er-reur
de mesure est faible. En effet, plus la mesure est bonne, plus le filtre répond mieux
au système et estime au mieux la valeur de la vitesse.
Évaluons ceci par la valeur de l’erreur d’estimation. Il s’agit en effet de représenter
l’évolution des élements diagonaux P11 et P22 de la matrice de covariance de l’erreur
d’estimation P.
40
50. 3.4. Résultats de simulation 41
0 10 20 30 40 50 60 70 80 90 100
3
2.5
2
1.5
1
0.5
0
−0.5
itération
vitesse en Y (en m/s)
signal GPS
estimée de la vitesse
Figure 3.6 – Estimation de la vitesse en X du mobile, pour w » N(0; 10¡2)
vitesse en X (en m/s)
signal GPS
estimée de la vitesse en X
0 10 20 30 40 50 60 70 80 90 100
1.6
1.4
1.2
1
0.8
0.6
0.4
0.2
0
−0.2
−0.4
itération
Figure 3.7 – Estimation de la vitesse en X du mobile, pour w » N(0; 10¡4)
41
51. 3.4. Résultats de simulation 42
0 10 20 30 40 50 60 70 80 90 100
3
2.5
2
1.5
1
0.5
0
−0.5
itération
vitesse en Y (en m/s)
signal GPS
estimée de la vitesse
Figure 3.8 – Estimation de la vitesse en Y du mobile, pour w » N(0; 1)
vitesse en Y (en m/s)
signal GPS
estimée de la vitesse
0 10 20 30 40 50 60 70 80 90 100
3
2.5
2
1.5
1
0.5
0
−0.5
itération
Figure 3.9 – Estimation de la vitesse en Y du mobile, pour w » N(0; 10¡2)
42
52. 3.4. Résultats de simulation 43
0 10 20 30 40 50 60 70 80 90 100
3
2.5
2
1.5
1
0.5
0
−0.5
itération
vitesse en Y (en m/s)
signal GPS
estimée de la vitesse
Figure 3.10 – Estimation de la vitesse en Y du mobile, pour w » N(0; 10¡4)
La figure (3.11) représente l’évolution de l’erreur d’estimation de la vitesse du mobile
suivant l’axe X.
Une estimée de la position de mobile à été également calculée, selon les deux axes X
et Y . Les courbes de la figure (3.12) en illustrent l’evolution.
L’évolution de la position du mobile est ainsi corrigée à chaque itération. Alors que les
observations disponibles sont bruitées au fur et à mesure du déplacement du mobile, l’ac-céléromètre
fournit, avec une dynamique plus importante, les mesures accéléromètriques.
Ces mesures sont alors recalées avec les mesures du GPS, pour corriger la dérive issue de
la double intégration des mesures de l’accéléromètre.
43
53. 3.4. Résultats de simulation 44
0 10 20 30 40 50 60 70 80 90 100
0.2
0.15
0.1
0.05
0
−0.05
−0.1
−0.15
−0.2
Itération
Erreur d’estimation de Vx
Q=1
Q=1e(−2)
Q=1e(−4)
Figure 3.11 – Erreur d’estimation de la vitesse en X du mobile, pour différentes valeurs
de l’erreur de mesure
0 10 20 30 40 50 60 70 80 90 100
400
300
200
100
0
−100
itération
Position en X (en m)
signal GPS
estimée de la Position en X
0 10 20 30 40 50 60 70 80 90 100
100
50
0
−50
−100
itération
Position en Y (en m)
signal GPS
estimée de la position en Y
Figure 3.12 – Estimée de la position du mobile
44
54. 3.5. Conclusion 45
3.5 Conclusion
Ainsi, l’estimation des paramètres composant le vecteur d’état souligne l’amélioration
de la précision en recalant les mesures inertiels par les données GPS. Cette méthode de
navigation hybride repose sur la complémentarité entre les différents appareils de mesure.
Le traitement de Kalman constitue un observateur performant qui permet d’extraire l’in-formation
recherchée du bruit additionnel. En plus, la modélisation des bruits de mesure
et du modèle par des processus gaussiens fait que le filtre atteint son optimalité.
En outre, notons que la précision de l’estimateur peut être encore plus amélioré en
agissant sur les biais relatifs à chaque capteur. Ces biais, qui figurent déjà dans le vecteur
d’état, varient égalment dans le temps, ce qui requiert leur mise à jour à chaque itération.
45
55. Conclusion générale
DAns ce projet, nous avons présenté un système de localisation temps réel basé sur
la fusion hybride de différents systèmes de positionnement. Le système élaboré fait inter-venir
un récepteur GPS, un accéléromètre et un gyroscope. L’ensemble a permis de tirer
profit des avantages de chaque capteur. Le but de ce travail consiste à simuler le fonction-nement
du filtre de Kalman, qui, ayant à son entrée les signaux délivrés par les différents
systèmes de positionnement, permet d’améliorer la précision de la position de la personne
en mouvement.
Pour ce faire, nous avons explicité le traitement de l’information et de l’intégration
des trois systèmes de positionnement. L’intérêt de cette intégration hybride est de cher-cher
dans les spécificités de chaque capteur ce qui peut être compensé par les autres cap-teurs.
Les capteurs inertiels sont par exemple caractérisés par une certaine dérive des
mesures que le récepteur GPS parvient à compenser, du fait de sa meilleure précision à
long terme. En revanche, le GPS fonctionne à une fréquence beaucoup moins élevée que
celle d’acquisition des mesures inertielles. Le rythme de mesure est alors augmenté grâce
à la disponibilité plus fréquente des mesures accéléromètriques et gyroscopiques. Enfin,
le récepteur GPS est peu autonome du fait de sa dépendance aux référenciels terrestres,
alors que les capteurs inertiels ne présentent pas cette dépendance. A l’examen de cette
complémentarité entre les différents systèmes de positionnement, l’idée de concevoir un
système hybride s’avère d’un intérêt indéniable.
Dans une première partie, nous avons explicité le traitement de Kalman. Ce filtre pré-sente
des performances indéniables, mais requiert le respect de certaines conditions qui
aboutissent à son optimalité. Plusieurs approches de ce filtre ont été élaborées, chacune
répondant à un aspect spécifique du système considéré. Nous nous sommes focalisés, en
particulier, sur la version étendue, qui s’adapte au système dynamique, tel qu’il est défini
par le projet. Dans une deuxième partie, nous avons présenté les modèles d’évolution et
d’observation qui régissent notre système. Cette modélisation est très importante, dans le
sens où elle définie deux principaux aspects du système : un aspect décrivant l’évolution
dynamique de l’état du système, et un deuxième décrivant la variation de l’observation
46
56. 3.5. Conclusion 47
disponible à l’instant actuel en fonction de l’état au même instant. Une fois la modélisa-tion
faite, nous sommes passé, dans une dernière étape, à l’implémentation du filtre et au
test de son fonctionnement.
Les résultats auxquels nous avons abouti mettent en oeuvre la finesse apportée à la
localisation du mobile. L’estimée des éléments du filtre témoigne du réajustement effectué
par les mesures GPS, offrant ainsi une bonne amélioration de la précision d’estimation.
47
57. Annexe A
Les systèmes de référence et de
coordonnées
Les systèmes de référence constituent des un outil mathématique pour exprimer l’orien-tation,
le module et implicitement la position d’une entité physique représentée souvent
par un vecteur. Changer de système de coordonnées ne change pas le vecteur mais seule-ment
sa représentation mathématique.
D’autre part,tout mouvement, qu’il soit de translation ou de rotation, est relatif au
système de référence dans lequel il est exprimé. d’où l’importance du choix du référenciel.
Ainsi, toute mesure physique est la valeur d’une quatité relative entre deux systèmes de
référence.
Pour mieux exprimer les equations de navigation inertielle, nous présentons dans cet
annexe les différents sytèmes de référence utilisé :
le système attaché au mobile
Ce système est propre au mobile. Les capteurs lui étant attachés, nous considérons que
c’est la même que celui des capteurs inertiels. L’origine de ce repère est, le centre de la
carte électronique sur laquelle seront soudés les différents composants. Les axes prennent
les directions suivantes :
l’axe x (roulis) : vers l’avant, l’axe y (tangage) : vers la droite ; l’axe z(lacet) : en bas.
le système de navigation
Ce système est défini dans le plan tangent local "LTP" , son origine se trouve à la
position actuelle du mobile et son plan x-y (North-East) est tangent à la surface de la
48
58. 49
terre, où plus précisément à l’ellipsoide. Il existe deux conventions de systèmes LTP qui
sont bien usuelles dans la navigation :
– NED : North,East,Down(vers le bas ou vers le vecteur gravité)
– ENU : East,North,Up (vers le haut)
C’est dans ce référenciel que le récepteur GPS va retourner les mesures de position et de
vitesse.
le système inertiel
Les lois de Newton sont applicables dans ce référenciel.Il ne suit pas la rotation de la
terre et donc ne tourne pas par rapport aux étoiles. L’origine de ce système est le centre
de la Terre.Le système des coordonnées correspondant est un système cartésien avec ses
axes notés :
– Axe X : Vers le "Vernal Equinoxe" (étoile distante)
– Axe Z : East,North,Up (vers le haut)
– Axe Y : pour compléter le repère direct
C’est dans ce référenciel que les capteurs inertiels (accéléromètres et gyroscopes ) re-tournent
leurs mesures.
49
59. Annexe B
Les Capteurs inertiels
Les accéléromètres
L’ADXL202
Features :
2-Axis Acceleration Sensor on a Single IC Chip Measures Static Acceleration asWell
as Dynamic Acceleration Duty Cycle Output with User Adjustable Period Low Power
<0.6 mA Faster Response than Electrolytic, Mercury or Thermal Tilt Sensors Bandwidth
Adjustment with a Single Capacitor Per Axis 5 mg Resolution at 60 Hz Bandwidth +3 V
to +5.25 V Single Supply Operation 1000 g Shock Survival APPLICATIONS 2-Axis Tilt
Sensing Computer Peripherals Inertial Navigation Seismic Monitoring Vehicle Security
Systems Battery Powered Motion Sensing.
General Description
The ADXL202 is low cost, low power, complete 2-axis accelerometer with a measu-rement
range of either §2g. The ADXL202 can measure both dynamic acceleration (e.g.,
vibration) and static acceleration (e.g., gravity). The outputs are digital signals whose duty
cycles (ratio of pulsewidth to period) are proportional to the acceleration in each of the 2
sensitive axes. These outputs may be measured directly with a microprocessor counter, re-quiring
no A/D converter or glue logic. The output period is adjustable from 0.5 ms to 10
ms via a single resistor (RSET). If a voltage output is desired, a voltage output proportio-nal
to acceleration is available from the XFILT and YFILT pins, or may be reconstructed
by filtering the duty cycle outputs. The bandwidth of the ADXL202/ADXL210 may be set
from 0.01 Hz to 5 kHz via capacitors CX and CY. The typical noise floor is 500 mg/ÖHz
allowing signals below 5 mg to be resolved for bandwidths below 60 Hz. The ADXL202 is
50
60. 51
available in a hermetic 14-lead Surface Mount CERPAK, specified over the 0°C to +70°C
commercial or -40°C to +85°C industrial temperature range.
ADXL202 specifications
Figure B.1 – ADXL202 Specification
51
61. 52
Memsic 2125
The Memsic 2125 is a low cost, dual-axis thermal accelerometer capable of measuring
dynamic acceleration (vibration) and static acceleratiion (gravity) with a range of §2 g.
For integration into existing applications, the Memsic 2125 is electrically compatible with
other popular accelerometers. What kind of things can be done with the Memsic 2125 ac-celerometer
? While there are many possibilities, here’s a small list of ideas that can be rea-lized
with a Memsic 2125 and the Parallax BASIC Stamp® microcontroller : . Dual-axis
tilt sensing for autonomous robotics applications (BOE-Bot, Toddler, SumoBot) . Single-axis
rotational position sensing . Movement/Lack-of-movement sensing for alarm systems
Packing List Verify that your Memsic 2125 Demo Kit is complete in accordance with the
list below : . Parallax Memsic 2125 Demo PCB (uses Memsic MXD2125GL) . Documen-tation
Note : Demonstration software files may be downloaded from www.parallax.com.
Features . Measure 0 to §2 g on either axis ; less than 1 mg resolution . Fully temperature
compensated over 0° to 70° C range . Simple, pulse output of g-force for X and Y axis
. direct connection to BASIC Stamp . Analog output of temperature (TOut pin) . Low
current operation : less than 4 mA at 5 vdc Connections Connecting the Memsic 2125 to
the BASIC Stamp is a straightforward operation, requiring just two IO pins. If single-axis
tilt of less than 60 degrees is your requirement, only one output from the Memsic 2125
need be connected. Figure (??) illustrates connections details.
Figure B.2 – L’accéléromètre MEMSIC
Les gyroscopes
CRS-03 / 04
The CRS-03/04 is robust and affordable mass-produced gyroscope for automotive and
commercial customers. Angular rate sensors are used wherever rate of turn sensing is re-quired
without a fixed point of reference. The sensor will output a DC voltage proportional
to the rate of turn and input voltage. High performance motion sensing even under severe
shock and vibration. Whatever your application, the unique silicon ring technology, cou-pled
with closed loop electronics, gives advanced and stable performance over time and
52
62. 53
temperature, overcoming the mount sensitivity problems experienced with simple beam
or tuning fork based sensors.
Figure B.3 – Le gyroscope CRS-03
Figure B.4 – Le gyroscope CRS-04
Le récepteur GPS
Le récepteur GPS fournit un positionnement absolu, sur un ellipsoïde de référence
représentant le globe terrestre (figure 1.2). Cependant, un tel positionnement est peu ex-ploitable
dans notre application, et l’on serait plus intéressé par un positionnement relatif
en mètres plutôt que par une position en Longitude/Latitude en radians.
La formule de conversion est la suivante :
l =
1
2
ln
1 + sin(')
1 ¡ sin(')
¡
e
2
ln
1 + esin'
1 ¡ esin'
(B.1)
R = Cexp(¡nl) (B.2)
° = n(¸ ¡ ¸0) (B.3)
X = Xs ¡ Rsin° (B.4)
Y = Ys ¡ Rcos° (B.5)
Où :
53
63. 54
¸ et ' sont les longitude et lattitude initiales.
n, C , Xs et Ys sont des constantes de la projection.
¸0 , est la longitude su méridien central.
e , est l’excentricité de l’ellipsoîde
LAMBERT I LAMBERT II LAMBERT III LAMBERT IV LAMBERT 93
N 0,7604059656 0,7289686274 0,6959127966 0,6712679322 0,7256077650
C 11603796,98 11745793,39 11947992,52 11947992,52 11754255,426
Xs 600000,000 600000,000 600000,000 234,358 700000,000
Ys 5657616,674 6199695,768 6791905,085 7239161,542 12655612,050
Tableau B.1 – Les constantes de la projection Lambert
¸0 = 0 grades à Paris (= 2° 20’ 14,025" E Greenwich)
e=0,08248325676
54