Tutorial on Aided Inertial Navigation Systems: A Modern Treatment Using Lie-Group Theoretical Methods

Ce tutoriel présente une introduction orientée contrôle aux systèmes de navigation inertielle assistée en utilisant une formulation géométrique basée sur le groupe étendu SE₂(3) pour fusionner les mesures inertielles avec des données d'aide, tout en explicitant les rôles de l'invariance et de la symétrie.

L'essentiel

🧭 Le Guide de l'Explorateur : Comment ne pas se perdre dans le noir ?

Imaginez que vous êtes un explorateur perdu dans une forêt dense, sans boussole, sans carte et sans signal GPS. Vous avez juste un podomètre (qui compte vos pas) et un gyroscope (qui sent quand vous tournez). C'est exactement ce que fait un système de navigation inertielle (INS) dans un drone, une voiture autonome ou un robot.

Le problème ? Si vous marchez pendant 10 minutes en comptant vos pas, une petite erreur de 1% à chaque pas va vous faire croire que vous êtes à 100 mètres de là où vous êtes réellement. C'est ce qu'on appelle la dérive. Avec le temps, votre estimation de la position devient complètement fausse.

Pour corriger cela, on utilise des "aides" extérieures : un GPS, une caméra qui voit des arbres, ou un magnétomètre qui sent le champ magnétique de la Terre.

Ce rapport technique, écrit par Soulaimane Berkane, explique comment faire la fusion entre ces deux mondes (vos pas internes et les repères externes) d'une manière mathématiquement élégante et robuste, en utilisant une idée appelée la théorie des groupes de Lie.

---

🎭 Le Problème : La "Méthode du Miroir Brisé"

Pendant des décennies, les ingénieurs ont essayé de corriger ces erreurs en utilisant des formules mathématiques classiques (comme le "Filtre de Kalman").

L'analogie : Imaginez que vous essayez de dessiner une courbe parfaite (un cercle) en utilisant uniquement des règles droites. Plus le cercle est grand, plus vous devez utiliser de petits segments droits, et plus votre dessin devient moche et imprécis. C'est ce qui arrive quand on essaie de modéliser la rotation d'un objet (comme un drone qui tourne) avec des formules linéaires simples. Les mathématiques "cassent" la réalité, créant des erreurs bizarres et des instabilités.

C'est comme si vous essayiez de mesurer la température avec une règle : ça ne colle pas !

🌟 La Solution : La "Danse sur le Cercle" (Groupe de Lie)

L'auteur propose une nouvelle approche : au lieu de forcer la rotation dans une boîte rectangulaire (les formules classiques), on accepte qu'elle vive sur sa propre forme naturelle : une sphère ou un cercle.

En mathématiques, on appelle cela un Groupe de Lie (spécifiquement un groupe appelé SE2(3)).

L'analogie créative :
Imaginez que votre robot est un danseur sur une piste de danse circulaire.

• L'ancienne méthode (EKF classique) : Elle essaie de décrire la position du danseur en disant "il a avancé de 2 mètres vers la droite". Mais si le danseur fait un tour complet, la "droite" change ! La description devient confuse et le danseur trébuche.
• La nouvelle méthode (InvEKF / Lie-Group) : On décrit le mouvement du danseur en respectant la forme de la piste. On dit : "Il a fait un demi-tour sur lui-même". Peu importe où il est sur la piste, la règle du mouvement reste la même.

C'est comme si on utilisait un GPS interne qui comprend la géométrie de l'espace, plutôt qu'un simple compteur de pas.

🛠️ Comment ça marche en pratique ?

Le rapport détaille trois étapes clés, que l'on peut comparer à la conduite d'une voiture :

1. La Propagation (Le moteur) :
   Entre deux signaux GPS, le robot utilise ses capteurs internes (gyroscopes, accéléromètres) pour avancer.

   • L'astuce : Au lieu de faire des calculs approximatifs, le rapport montre comment faire un calcul exact sur la forme géométrique du mouvement. C'est comme si le robot savait exactement comment tourner sans jamais dériver, même si le moteur vibre un peu.

2. La Correction (Le GPS) :
   Quand le robot reçoit un signal GPS ou voit un arbre, il doit corriger sa position.

   • L'astuce : Au lieu de simplement "tirer" la position vers le GPS (ce qui peut créer des secousses), le filtre utilise la symétrie du mouvement. Il ajuste la trajectoire en respectant les lois de la physique. C'est comme si le GPS disait : "Tu es ici", et le robot répondait : "Ah, j'ai fait une petite erreur de rotation, je me corrige en glissant doucement sur ma trajectoire naturelle" plutôt que de sauter brusquement.

3. La Robustesse (Le pare-chocs) :
   C'est le plus gros avantage. Si le robot commence avec une très mauvaise estimation (par exemple, il pense être au Canada alors qu'il est en France), les anciennes méthodes paniquent et échouent.

   • La magie : La méthode de ce rapport reste calme. Comme le système est construit sur des symétries mathématiques pures, il peut "rattraper" une grosse erreur initiale sans s'effondrer. C'est un parachute qui fonctionne même si vous sautez de l'avion en courant.

🚀 Les Nouvelles Frontières (SE5(3) et au-delà)

Le rapport ne s'arrête pas là. Il explore des versions encore plus avancées (comme SE5(3)).

L'analogie :
Si SE2(3) est comme conduire une voiture sur une route plate, SE5(3) est comme conduire un avion qui doit aussi gérer son altitude, sa vitesse et sa direction dans un espace 3D complexe, tout en ayant des capteurs imparfaits.
Ces nouvelles méthodes ajoutent des "variables fantômes" (des états auxiliaires) pour aider le système à rester stable même dans des situations extrêmes, garantissant que le robot ne se perdra jamais, même s'il tourne sur lui-même pendant des heures.

💡 En résumé

Ce rapport est un manuel pour construire des systèmes de navigation qui ne paniquent jamais.

• Avant : On utilisait des approximations qui fonctionnaient bien tant que tout allait bien, mais qui échouaient si le robot était perdu ou si les capteurs étaient bruyants.
• Maintenant (grâce à ce rapport) : On utilise la géométrie pure pour créer des filtres qui comprennent la nature même du mouvement. C'est plus précis, plus stable, et surtout, cela permet aux robots de naviguer de manière autonome dans des environnements complexes (sous-marins, espaces, forêts) avec une confiance absolue.

C'est un peu comme passer d'une boussole en bois qui tremble à une boussole quantique qui pointe toujours vers le Nord, peu importe les tempêtes.

Résumé technique

Voici un résumé technique détaillé du rapport technique « Tutorial on Aided Inertial Navigation Systems: A Modern Treatment Using Lie-Group Theoretical Methods » de Soulaimane Berkane.

1. Problématique

Les systèmes de navigation inertielle (INS) constituent l'épine dorsale des solutions de positionnement pour les véhicules autonomes, la robotique et la défense. En intégrant les mesures d'accéléromètres et de gyroscopes, un INS peut estimer la position, la vitesse et l'orientation sans infrastructure externe. Cependant, ces estimations sont intrinsèquement corrompues par le bruit des capteurs et les biais, entraînant une dérive (drift) non bornée au fil du temps.

Pour corriger cette dérive, les INS sont aidés par des capteurs externes (GNSS, caméras, magnétomètres, etc.). Le défi central réside dans la représentation et l'estimation de l'orientation. Contrairement à la position ou à la vitesse, l'orientation évolue sur une variété non linéaire (le groupe de rotation $SO(3)$).

• Limites des approches classiques : L'application naïve du Filtre de Kalman Étendu (EKF) avec des corrections additives (angles d'Euler ou quaternions) conduit à des singularités, des erreurs de normalisation et une incohérence des dynamiques d'erreur linéarisées.
• Limites de l'EKF Multiplicatif (MEKF) : Bien que le MEKF améliore la robustesse numérique en préservant la contrainte de norme unitaire, ses dynamiques d'erreur linéarisées dépendent encore de la trajectoire estimée. Cela peut entraîner une incohérence ou une divergence en cas d'erreurs initiales importantes ou de périodes d'assistance faibles.

2. Méthodologie

Le rapport propose une approche unifiée basée sur la théorie des groupes de Lie, centrée sur le groupe spécial euclidien étendu $SE_2(3)$. Cette formulation géométrique permet de traiter l'orientation, la vitesse et la position comme un seul objet mathématique cohérent.

A. Modélisation sur $SE_2(3)$

L'état de navigation est défini comme un élément du groupe $SE_2(3)$, qui unifie la rotation $R \in SO(3)$, la vitesse $v$ et la position $p$.

• Dynamiques : Les équations de mouvement sont écrites intrinsèquement sur le groupe, exploitant la structure affine du groupe. Cela permet de séparer les termes cinématiques, la gravité et les mesures IMU de manière géométrique.
• Discrétisation exacte : Contrairement aux méthodes classiques qui approximent l'intégration, le rapport dérive une propagation discrète exacte sur le groupe en utilisant l'exponentielle de Lie et les jacobiens gauches/droits ($J_\ell, Q_\ell$). Cela préserve la géométrie du mouvement (ex. : trajectoire circulaire parfaite) même avec des pas de temps importants.

B. Définition de l'Erreur Invariante

Au lieu d'utiliser une erreur euclidienne ($X - \hat{X}$), le rapport définit l'erreur d'estimation directement sur le groupe (erreur à droite ou à gauche).

• Propriété clé : Pour les systèmes affines de groupe comme l'INS, les dynamiques de l'erreur invariante (linéarisées) sont autonomes et indépendantes de la trajectoire estimée.
• Conséquence : Les matrices de transition et les Jacobiens de mesure ne dépendent pas de l'état estimé, ce qui élimine le problème de dépendance à la trajectoire inhérent à l'EKF classique.

C. Filtrage Invariant (InvEKF)

Le rapport détaille l'implémentation du Filtre de Kalman Étendu Invariant (InvEKF) :

1. Propagation : Mise à jour de l'état et de la covariance sur le groupe $SE_2(3)$ en utilisant les mesures IMU.
2. Correction : Fusion des mesures d'aide (GNSS, magnétomètre, etc.) via une innovation invariante. Les mesures sont transformées pour respecter la symétrie du filtre (ex. : transformation des mesures globales en mesures locales pour un filtre à erreur à droite).
3. Mise à jour de l'état : L'état corrigé est obtenu par rétraction multiplicative sur le groupe, garantissant que l'estimation reste sur la variété.

D. Extensions et Observateurs Avancés

Le rapport explore des développements récents pour améliorer la stabilité globale :

• $SE_5(3)$ : Extension de l'état pour inclure un cadre orthonormé auxiliaire, permettant d'obtenir une stabilité asymptotique presque globale (AGAS) pour la composante translationnelle.
• Observateurs Synchrones : Utilisation de groupes d'automorphismes pour découpler les erreurs de rotation et de translation, garantissant une convergence exponentielle globale sous certaines conditions d'excitation.
• Filtres Équivariants (EqF) : Cadre généralisé unifiant les méthodes invariantes et gérant les biais de capteurs via des produits semi-directs.

3. Contributions Clés

1. Unification Géométrique : Présentation d'un cadre unique ($SE_2(3)$) pour la modélisation, la propagation, l'analyse d'incertitude et la fusion de capteurs, rendant la structure géométrique transparente.
2. Propagation Discrète Exacte : Dérivation de formules de propagation discrète exactes sur le groupe, utilisant les jacobiens de Lie, éliminant les erreurs de discrétisation géométrique des méthodes classiques.
3. Analyse d'Observabilité Unifiée : Démonstration de comment différentes modalités de capteurs (GNSS, magnétomètre, points de repère) restaurent l'observabilité des modes non observables (comme le lacet) en exploitant la diversité spatiale ou temporelle, avec des Jacobiens constants ou dépendant uniquement de la mesure.
4. Comparaison MEKF vs InvEKF : Preuve théorique et simulation montrant que l'InvEKF offre une robustesse supérieure et une cohérence accrue, notamment en présence d'erreurs initiales importantes ou d'observabilité faible, là où le MEKF classique peut diverger.
5. Traitement des Biais : Discussion approfondie sur la difficulté de maintenir la stabilité globale tout en estimant les biais des capteurs, identifiant cela comme un défi de recherche ouvert.

4. Résultats

• Stabilité et Cohérence : Les simulations montrent que l'InvEKF maintient une convergence stable même sans aide magnétométrique (où l'observabilité du lacet est faible), tandis que le MEKF classique présente des amplifications d'erreurs transitoires et des divergences.
• Précision de la Trajectoire : L'utilisation de la propagation exacte sur $SE_2(3)$ préserve la géométrie des trajectoires (ex. : cercles parfaits) indépendamment du pas de temps, contrairement aux discrétisations classiques qui introduisent des distorsions géométriques.
• Formes d'Incertitude : La propagation de la covariance sur le groupe capture correctement la forme "en banane" de la distribution d'erreur (couplage rotation-position), que les modèles gaussiens euclidiens échouent à représenter fidèlement.
• Stabilité Globale : Les extensions vers $SE_5(3)$ et les observateurs synchrones démontrent théoriquement la possibilité d'atteindre une stabilité asymptotique presque globale, dépassant les limites de stabilité locale des filtres standards.

5. Signification et Impact

Ce rapport technique est une contribution majeure pour la communauté de la robotique et de la navigation autonome car il :

• Démocratise l'approche géométrique : Il rend accessible la théorie complexe des groupes de Lie pour les ingénieurs et chercheurs, en fournissant des formulations prêtes à l'emploi.
• Établit un nouveau standard : Il démontre que les méthodes invariantes ne sont pas seulement des alternatives théoriques, mais des solutions supérieures en termes de robustesse et de cohérence par rapport aux méthodes EKF traditionnelles.
• Clarifie les limites et l'avenir : Il identifie clairement les défis restants, notamment l'estimation simultanée des biais et la stabilité globale, orientant ainsi les recherches futures vers des cadres équivariants et des structures d'état augmentées.

En résumé, ce tutorial consolide une vaste littérature sur la navigation inertielle aidée en un récit géométrique cohérent, prouvant que la préservation des symétries du système est la clé pour obtenir des estimateurs robustes, précis et mathématiquement fondés.