# Recalage de la navigation inertielle hybride par le filtrage de Kalman sans parfum paramétré à quaternions

Wassim Khoder¹, *

¹Université libanaise, Faculté des Sciences Économiques et de Gestion, Tripoli, Liban

**Abstract.** Dans ce papier, nous avons développé un algorithme d'hybridation (recalage) de la navigation inertielle, noté Q-SUKF, qui combine le filtre de Kalman sans parfum à paramètre (SUKF) et l'utilisation des propriétés de rotation et d'unicité des quaternions (Q) pour représenter l'attitude. L'utilisation des quaternions unités dans le calcul de la matrice de covariance d'erreurs prédite empêche les problèmes de singularité et la dérive des informations d'attitude. L'augmentation de l'incertitude dans les angles d'attitude, est modélisé par un vecteur de rotation pour garantir que la normalisation du quaternion est toujours maintenue dans le filtre. Le Q-SUKF proposé est bien adapté pour estimer récursivement les états de la navigation, quelque soient les valeurs initiales sur les angles d'attitude ou la dynamique des mouvements du mobile, à l'aide des capteurs externes qui sont complémentaires et/ou redondants.

## 1 Introduction

La précision de l'estimation des paramètres de navigation inertielle à bas coût se dégrade avec le temps. Donc il s'avère nécessaire d'effectuer un recalage des informations inertielles par un ou plusieurs moyens complémentaires [1]. L'intégration des mesures de ces moyens dans le calcul des états de navigation inertielle nécessite la mise en oeuvre d'un algorithme de recalage appelé filtre d'hybridation. Le filtrage de Kalman fournit un outil puissant pour créer le synergisme entre les différents capteurs à hybrider. Les filtres de kalman étendu (EKF) et sans parfum (UKF) sont des méthodes de filtrage non linéaire qui se focalisent principalement sur la conception des algorithmes d'intégration de l'attitude et de l'alignement initial. L'UKF s'impose aujourd'hui comme une alternative supérieure à l'EKF dans l'estimation des angles d'attitude si les erreurs des angles initiales d'attitude sont grandes [2]. Plusieurs paramètres ont été utilisés pour représenter l'attitude, comme les angles d'Euler, matrice de Cosinus directeur, quaternions, vecteur de rotation, Paramètres de Rodrigues, etc [2]. La méthode des quaternions est avantageuse car elle nécessite moins de calculs, donne une meilleure précision, évite la singularité et l'équation cinématique est bilinéaire [3]. Dans cet article, nous proposons un nouveau type de filtre UKF appelé filtre de Kalman sans parfum paramétré à quaternions « Quaternion-Scaled Unscented Kalman Filter » (Q-SUKF).

<footnote>* Corresponding author: wassim.khoder@ul.edu.lb</footnote>
## 2 Filtre Q-SUKF

Le filtre Q-SUKF modélise la distribution de l'état qui est approximée par une variable aléatoire gaussienne (GRV), par un ensemble minimal de points échantillons (« sigma-points ») soigneusement et attentivement choisis à partir de l'algorithme « Scaled Spherical Simplex transformation sans parfum ». L'algorithme Q-SUKF est détaillé dans [4]. Cet algorithme est bien adapté pour estimer récursivement les états de navigation à l'aide des capteurs externes (GPS et Magnétomètre) qui sont complémentaires et/ou redondants.

### 2.1 Modèles d'observation du système de navigation inertielle hybride

Les modèles d'observation des mesures du GPS, utilisées pour la mise à jour des mesures de position et de vitesse, sont représentés par:

$$h_{k-L_1}(p) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & -1 \end{bmatrix} \left[ C_e^n(q)(p_{INS}^e - p_{GPS}^e) + C_b^n(q)I_{GPS}^b \right]_{k-L_1} \quad (1)$$

$$h_{k-L_2}(v) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & -1 \end{bmatrix} \left[ (v^n - v_{GPS}^n) + C_b^n(q)I_{GPS}^b \right]_{k-L_2} \quad (2)$$

Où $p_{GPS}^e$ et $p_{INS}^e$ sont les positions du GPS et de la centrale inertielle, mesurées dans le repère terrestre. $v_{GPS}^n$ et $v^n$ sont les vitesses du GPS et de la centrale inertielle, représentées dans le repère de navigation. $I_{GPS}^b$ est l'effet de bras de levier exprimé dans le repère body. $L_1$ et $L_2$ représentent la différence de temps entre les mesures du GPS et de la centrale inertielle en position et en vitesse respectivement. Le magnétomètre mesure le champ magnétique terrestre qui est utilisé pour déduire des informations sur les mesures de l'angle du lacet. L'équation d'observation de l'angle de lacet est présentée dans [5].

## 3 Simulation numérique et résultat expérimental

Le profil simulé de trajectoire de référence est généré par la fonction procencar du toolbox INS pour une automobile en coordonnées géographiques. La séquence dure 10 minutes. La trajectoire inertielle et les données GPS est déterminée, par la suite, en tenant compte d'une erreur initiale par rapport à la trajectoire de référence portant sur chacune des variables d'état. Les données simulées du champ magnétique terrestre ont été générées par le modèle magnétique mondiale 2005 (WMM-2005) puis corrompues par un bruit blanc gaussien de moyenne nulle et d'écart-type est égale à 0.02 Gauss/s¹/². Les données de la centrale inertielle du GPS et du magnétomètre sont fournies à des fréquences différentes (200, 10 et 50 Hz resp.). Pour tester notre filtre, les écart-type initiaux sur les angles d'attitude sont réglés à 60 degrés. Les figures 1, 2, 3 nous montrent les erreurs de position résultant de l'hyridation INS/GPS et INS/GPS/MAG. Dans ces figures, les erreurs de position restent à l'intérieur de la borne de covariance d'erreur fixée à ±3σ après la phase initiale de l'algorithme (inférieur à 80 sec). Puisque l'information de la position dérive de celle de la vitesse, On peut déduire que le Q-SUKF est tout à fait adapté pour estimer récursivement la position et la vitesse d'un mobile.
<figure><img src="image_1.png" /><figcaption>Fig. 1. Erreur de latitude.</figcaption></figure>

<figure><img src="image_2.png" /><figcaption>Fig. 2. Erreur de longitude.</figcaption></figure>

<figure><img src="image_3.png" /><figcaption>Fig. 3. Erreur de la hauteur.</figcaption></figure>

## 4 Conclusion

Les résultats de simulation indiquent que l’algorithme d’hybridation Q-SUKF proposé est capable de fournir un meilleur comportement de la convergence des angles d’Euler (du quaternion). Cette convergence mène à une meilleure estimation de la vitesse et de la position, quelque soient les valeurs initiales sur les angles d’attitude ou la dynamique des mouvements du mobile.

## References

1. J. Langelaan, S. Rock, Navigation of small uavs operating in forests, *AIAA guidance, navigation, and control conference*, Providence, Rhode Island, (2004).

2. J. L. Crassidis, F. L. Markley, Unscented filtering for spacecraft attitude estimation, J. of Guid., Contr., and Dyn., **26**, 4, (2003).

3. J. Waldmann, Attitude determination algorithms, computational complexity, and the accuracy of terrestrial navigation with strapdown inertial sensors, *XIV Congress Brasileiro de Auto*, (2002).

4. W. Khoder and al., Inertial Navigation Attitude Velocity and Position Algorithms using Quaternion Scaled Unscented Kalman Filtering, *IECON'08, the 34th annual Conference of the IEEE Industrial Electronics Society*, Orlando, Florida, USA, (2008).

5. S.-M. Oh, Nonlinear Estimation Techniques for Vision-Based Air-To-Air Tracking, Sch. of Aerospace Eng., Georgia Institute of Technology, Atlanta, GA 30332, (2007).