DOMAINE GENERAL

L'invention se rapporte au domaine des centrales inertielles de navigation.

L'invention concerne plus particulièrement un procédé d'estimation de l'état de navigation d'un porteur mobile utilisant un filtre de Kalman étendu (généralement abrégé EKF dans la littérature), ainsi qu'une centrale inertielle adaptée pour mettre en œuvre un tel procédé.

ETAT DE L'ART

Une centrale inertielle de navigation est un appareil destiné à fournir en temps réel des informations sur l'état d'un porteur : sa position, sa vitesse, son orientation, etc. Ces informations peuvent être utilisées pour piloter le porteur.

Une solution connue pour les estimer consiste à mettre en œuvre dans une centrale inertielle un filtre de Kalman étendu qui fournit un état estimé du porteur en fusionnant des données de capteurs inertiels (accéléromètres, gyromètres), et de capteurs non inertiels adaptés au type de porteur (odomètre, baro-altimètre, radar Doppler, récepteur GPS, etc). Les données inertielles jouent le rôle de commandes, et les données non inertielles celui d'observations.

Le filtre de Kalman est un algorithme comprenant typiquement une étape de prédiction et une étape de mise à jour répétées dans le temps : l'étape de prédiction calcule un état courant à partir d'un état précédent et de commandes d'entrée; l'étape de mise à jour affine l'état courant à l'aide d'observations. Les calculs de prédiction sont basés sur une équation de propagation modélisant la dynamique de l'état en fonction de l'état et de la commande. Les calculs de mise à jour sont basés sur une équation d'observation modélisant l'observation en fonction de l'état. Dans le filtre de Kalman, les équations de propagation et d'observation sont linéaires et prennent la forme de matrices ne dépendant pas de l'état estimé.

Dans le filtre de Kalman étendu, l'une au moins de ces fonctions est non linéaire. La fusion de données est obtenue en linéarisant ces fonctions aux étapes de propagation et de mise à jour. Les calculs sont donc effectués en utilisant des matrices qui dépendent de l'état estimé.

Le filtre de Kalman étendu peut alors être considéré comme un algorithme effectuant d'une part des calculs non linéaires et d'autre part des calculs linéaires effectués dans un filtre de Kalman à l'intérieur du filtre de Kalman étendu. L'état prédit par l'équation de propagation non linéaire sera dans la suite nommé « état global ». L'état estimé par le filtre de Kalman sera nommé « état linéarisé ». L'état global peut être corrigé par les informations portées par l'état linéarisé lors d'étapes de recalage.

Un filtre de Kalman (étendu ou non) calcule en outre une marge d'erreur associée à l'estimation de l'état en tenant compte d'une estimation de l'erreur statistique due à de nombreux facteurs tels que les erreurs initiales et les bruits des capteurs inertiels utilisés. L'erreur statistique à un instant donné peut se représenter sous forme d'un nuage de points dans l'espace d'état, chaque point correspondant à une réalisation possible. Pour estimer cette marge d'erreur, la centrale dispose d'un modèle statistique gaussien de l'erreur caractérisé à chaque instant par une moyenne nulle, et une matrice de covariance. Cette dernière définit l'enveloppe d'un volume ellipsoïdal dans l'espace d'état, centré sur l'erreur nulle, et contenant 99,97% des erreurs estimées dans chaque direction de l'espace d'état. Cette enveloppe est par la suite dénommée « enveloppe 3 sigma ».

La vraie loi statistique d'erreurs est inconnue et caractérisée par une autre enveloppe 3 sigma. En général, cette enveloppe n'est pas centrée sur zéro et sa forme est non ellipsoïdale.

La figure 1 représente l'évolution d'une enveloppe 3 sigma estimée et une enveloppe 3 sigma vraie à différentes itérations du filtre de Kalman. L'enveloppe 3 sigma vraie doit être contenue dans l'enveloppe 3 sigma estimée. Ceci constitue un critère de cohérence à respecter à tout instant et pour toutes les réalisations des variables aléatoires du système.

En général, le filtre de Kalman améliore au moyen des observations son estimation dans le temps, et les enveloppes 3 sigma ont tendance à s'aplatir le long de certains axes de l'espace d'état dits « observables », comme représenté en figure 2. Les axes qui sont orthogonaux à ces derniers sont dits « non observables ». L'enveloppe 3 sigma estimée prend alors une forme allongée selon les axes non observables, ce qui crée des corrélations entre les erreurs estimées.

Bien que l'imperfection du modèle utilisé par le filtre puisse jouer une part importante, il a été constaté que les non linéarités et les bruits du système empêchent souvent le critère de cohérence décrit ci-dessus d'être respecté lorsqu'un filtre de Kalman étendu est utilisé (par exemple, au temps T3 de la figure 2, l'enveloppe 3 sigma vraie n'est plus localisée à l'intérieur de l'enveloppe 3 sigma estimée le long d'un axe non observable).

Sur le long terme, ces incohérences peuvent devenir importantes et se propager aux autres axes de l'espace vectoriel de l'état à estimer.

Ces incohérences n'apparaissent généralement que dans des conditions particulières dépendant par exemple de la trajectoire du porteur et des conditions initiales. Ces conditions sont propres à l'observation et peuvent être très difficiles à prévoir. Elles sont d'autant plus fortes que l'observation est non linéaire.

La cause des incohérences est connue : l'étape de linéarisation, inhérente à un filtre de Kalman étendu, est bruitée par les erreurs d'estimation. En effet, cette linéarisation est effectuée à la dernière estimée produite par le filtre, estimée qui est par définition une donnée imprécise. Les modifications du second ordre produites sur les fonctions linéarisées par ce bruit d'estimation modifient alors les observations et sont la cause des incohérences.

Dans un premier cas, une variable observable peut être considérée par le filtre non observable mais ce cas est très improbable; et dans un deuxième cas, une variable non observable peut être considérée observable par le filtre. Cela réduit l'enveloppe 3 sigma estimée le long des axes non observables alors qu'elle devrait conserver sa valeur initiale. Or, cela ne réduit pas le nuage d'erreur vrai le long de ces mêmes axes puisque ces derniers sont réellement non observables. Une part de plus en plus importante du nuage d'erreur vrai risque alors de ne plus se trouver à l'intérieur de l'enveloppe 3 sigma estimée dès que celle-ci commence à se réduire. Les conséquences de cette incohérence peuvent être importantes et dépendent de l'enveloppe initiale de long des axes non observables en défaut.

Une solution de l'état de l'art permettant dans la plupart des cas de compenser la réduction de l'enveloppe 3 sigma estimée consiste à appliquer un bruit de modèle. L'algorithme EKF comporte en effet une opération consistant à augmenter la covariance en phase de prédiction à l'aide d'une matrice de bruits de modèles afin de tenir compte des variables et leur dynamique qui n'ont pas été modélisées dans le filtre. Dans ce problème de cohérence, la matrice de bruit est détournée de sa fonction car la réduction de l'enveloppe estimée n'est pas due à un problème de modélisation mais à un problème d'observabilité. De plus, pour des raisons d'architecture et de précision numérique de calcul, les bruits sont souvent appliqués sur la diagonale de la matrice de bruits de modèle : Tout comme la matrice de covariance, la diagonale de cette matrice correspond aux composantes de l'état linéarisé suivies par le filtre. En conséquence, le bruit n'est pas appliqué uniquement dans la direction des axes non observables. Il en résulte que le problème de cohérence risque de ne pas être totalement résolu, et que de plus cela nuit à la précision des estimations.

En effet, le bruit appliqué comporte une projection sur des axes non observables et une projection sur des axes observables, ces projections pouvant évoluer au cours du temps selon l'évolution des axes non observables. Plus la projection selon les axes observables est grande, et plus la valeur asymptotique de l'erreur est grande selon ces directions, et donc moins la précision des estimations est bonne. La valeur du bruit à appliquer quant à elle n'est pas connue. Une valeur empirique est choisie, généralement grande afin de tenir compte de tous les cas connus posant problème. Cela aggrave le problème de la précision.

D'autre part, comme le bruit de modèle n'est pas appliqué dans la bonne direction, l'enveloppe estimée s'allonge selon une direction qui s'éloigne de l'axe non observable vrai. En conséquence, cette enveloppe ellipsoïdale, qui devient certes plus importante selon ses axes principaux, risque de ne plus recouvrir le nuage d'erreurs vraies. Il risque donc de subsister une incohérence.

Le problème d'observabilité est donc traité selon l'état de l'art par une solution qui ne résout pas complètement le problème, et qui de plus nuit à la précision des estimations.

PRESENTATION DE L'INVENTION

L'invention vise à estimer l'état global d'un porteur mobile régi par un modèle non-linéaire, tout en minimisant l'apparition des incohérences définies en introduction.

Il est dès lors proposé un procédé d'estimation d'un état de navigation à plusieurs variables d'un porteur mobile selon la méthode du filtre de Kalman étendu, comprenant les étapes de :

- acquisition de mesures d'au moins une des variables,

- filtrage de Kalman étendu produisant un état estimé courant et une matrice de covariance délimitant dans l'espace de l'état de navigation une région d'erreurs, à partir d'un état estimé précédent, d'une matrice d'observation, d'une matrice de transition et des mesures acquises, le procédé étant caractérisé en ce qu'il comprend une étape d'ajustement de la matrice de transition et de la matrice d'observation avant leur utilisation dans le filtrage de Kalman étendu, de sorte que les matrices ajustées vérifient une condition d'observabilité qui dépend d'au moins une des variables de l'état du porteur, la condition d'observabilité étant adaptée pour empêcher le filtre de Kalman de réduire la dimension de la région le long d'au moins un axe non- observable de l'espace d'état, dans lequel la condition d'observabilité à vérifier par les matrices de transition et d'observation ajustées est la nullité du noyau d'une matrice d'observabilité qui leur est associée, et

dans lequel l'ajustement comprend les étapes de :

- calcul d'au moins une base primaire de vecteurs non-observables à partir de l'état estimé précédent,

- pour chaque matrice à ajuster, calcul d'au moins un écart matriciel associé à la matrice à partir de la base primaire de vecteurs,

décalage de chaque matrice à ajuster selon l'écart matriciel qui lui est associé de sorte à vérifier la condition d'observabilité.

Le procédé selon l'invention permet non seulement d'améliorer la cohérence des estimations mais permet en outre d'améliorer la précision des estimations, car la part de bruits de modèle utilisée selon l'état de l'art pour résoudre les problèmes d'observabilité peut être annulée.

De plus, ce procédé ne modifie que très peu l'architecture logicielle du filtre EKF, et peut être mis en œuvre sous forme d'une mise à jour logicielle légère sur des matériels déjà en opération. Cette mise à jour consiste en l'ajout d'une fonction d'ajustement de 2 matrices, et en une réduction des réglages de bruits de modèle.

L'invention peut également être complétée par les caractéristiques suivantes, prises seules ou en une quelconque de leurs combinaisons techniquement possibles.

L'étape de filtrage de Kalman étendu peut comprendre les sous-étapes de :

- propagation de l'état estimé précédent en un état prédit au moyen de la matrice de transition ajustée,

- linéarisation en l'état prédit d'un modèle non-linéaire pour produire la matrice d'observation avant ajustement,

- ajustement de la matrice d'observation produite par linéarisation.

Le fait de mettre en œuvre la linéarisation après l'étape de propagation permet de faire sorte que cette linéarisation soit effectuée en un point de l'espace vectoriel plus vraisemblable : l'état prédit produit par l'étape de propagation. Dès lors, l'ajustement de la matrice d'observation tient compte de cette propagation, et fournit des résultats plus précis que dans le cas où la linéarisation est mise en œuvre avant l'étape de propagation, en l'état estimé précédent.

L'ajustement peut en outre comprendre une étape d'orthogonalisation de la base primaire en une base secondaire de vecteurs, celle-ci étant mémorisée d'un cycle à l'autre, et l'écart matriciel associé à la matrice d'observation étant calculé à partir de la base secondaire de vecteurs.

L'écart matriciel associé à la matrice d'observation peut être la somme de plusieurs écarts matriciels indépendants, chaque écart matriciel étant calculé à partir d'un vecteur de la base secondaire qui lui est propre.

Les étapes du procédé peuvent être répétées dans des cycles successifs. Un cycle donné, dit cycle courant, peut alors comprendre les étapes de:

- mémorisation de la base secondaire de vecteurs et de coefficients d'orthogonalisation qui sont également produits par l'étape d'orthogonalisation, et

- transformation de la base primaire de vecteurs en une base tertiaire de vecteurs au moyen de coefficients d'orthogonalisation mémorisés au cours d'un cycle précédent,

- l'écart matriciel associé à la matrice de transition étant mis en œuvre à partir d'une base secondaire mémorisée au cours du cycle précédent et de la base tertiaire calculée au cours du cycle courant.

L'écart matriciel associé à la matrice de transition peut en outre être la somme de plusieurs écarts matriciels élémentaires indépendants, chaque écart matriciel étant calculé à partir d'un vecteur secondaire de la base secondaire mémorisée au cours du cycle précédent et d'un vecteur tertiaire de la base tertiaire calculée au cours du cycle courant, les vecteurs secondaire et tertiaire étant propres à l'écart matriciel élémentaire.

Pour au moins une matrice à ajuster, on peut en outre calculer : - une pluralité d'écarts matriciels candidats, chaque écart matriciel candidat étant calculé à partir d'une base primaire de vecteurs non- observables respective, et

- une pluralité de métriques, chaque métrique étant représentative d'une amplitude d'ajustement induite par un écart matriciel candidat respectif, l'étape de décalage de la matrice à ajuster étant mise en œuvre selon un écart matriciel élu parmi les écarts matriciels candidats en fonction des métriques calculées.

L'écart matriciel élu peut être l'écart matriciel candidat associé à la métrique représentative de l'amplitude d'ajustement la plus faible.

Le décalage peut n'être mis en œuvre que si la métrique de l'écart matriciel élu est inférieure à un seuil prédéterminé.

Selon un autre aspect de l'invention, il est proposé une centrale inertielle comprenant une pluralité de capteurs et un module d'estimation configuré pour estimer un état de navigation de la centrale inertielle à plusieurs variables par mise en œuvre du procédé selon le premier aspect de l'invention.

DESCRIPTION DES FIGURES

D'autres caractéristiques, buts et avantages de l'invention ressortiront de la description qui suit, qui est purement illustrative et non limitative, et qui doit être lue en regard des dessins annexés sur lesquels :

- Les figures 1 et 2, déjà discutées, représentent deux évolutions d'enveloppes 3 sigma au cours du temps, au cours de la mise en œuvre d'un filtre de Kalman étendu.

- La figure 3 représente schématiquement un porteur embarquant une centrale inertielle selon un mode de réalisation de l'invention.

- La figure 4 est le schéma fonctionnel d'un estimateur selon un mode de réalisation.

- La figure 5 est le schéma fonctionnel d'un bloc de linéarisation et d'ajustement compris dans l'estimateur de la figure 2. - La figure 6 est le schéma fonctionnel d'un sous-bloc représenté sur la figure 5.

- La figure 7 illustre une justification géométrique de l'ajustement de matrices utilisées par le filtre de Kalman.

- La figure 8 représente un flux des données correspondant à la mise en œuvre des étapes illustrées sur la figure 3.

Sur l'ensemble des figures, les éléments similaires portent des références identiques. DESCRIPTION DETAILLEE DE L'INVENTION

En référence à la figure 3, une centrale inertielle IN est embarquée sur un porteur mobile P, tel qu'un véhicule terrestre, un hélicoptère, un avion, etc.

La centrale inertielle IN comporte plusieurs parties : des capteurs inertiels Cl, des capteurs complémentaires CC, et des moyens E pour mettre en œuvre des calculs d'estimation. Ces parties peuvent être physiquement séparées les unes des autres.

Les capteurs inertiels Cl sont typiquement des accéléromètres et/ou des gyromètres mesurant respectivement des forces spécifiques et des vitesses de rotation que subit le porteur par rapport à un référentiel inertiel. La force spécifique correspond à l'accélération d'origine non gravitationnelle. Lorsque ces capteurs sont fixes par rapport au porteur, la centrale est dite de type « strap down ».

Les capteurs complémentaires CC sont variables selon le type de porteur, sa dynamique, et l'application visée. Les centrales inertielles utilisent typiquement un récepteur GNSS (GPS par exemple). Pour un véhicule terrestre, il s'agit également d'un ou de plusieurs odomètres. Pour un bateau, il peut s'agir d'un « loch », donnant la vitesse du bateau par rapport à celle de l'eau ou du fond marin. Les caméras forment un autre exemple de capteurs.

Les moyens d'estimation E de navigation seront par la suite désignés sous le terme « estimateur ». Les données de sortie de l'estimateur E sont un état de navigation du porteur et éventuellement d'états internes de la centrale inertielle.

L'estimateur E comprend notamment un filtre de Kalman étendu EKF configuré pour fusionner les informations données par les capteurs complémentaires et les capteurs inertiels de façon à fournir une estimation optimale des informations de navigation.

La fusion est faite d'après un système dynamique continu servant de modèle pour prédire l'état à chaque instant à l'aide d'une fonction de propagation f non linéaire, ainsi que la façon de l'observer à l'aide d'une fonction d'observation h pouvant être non linéaire également (voir à ce propos l'Annexe 5 rappelant quelques principes sur les systèmes dynamiques), cette fonction dépendant du type de capteur CC utilisé :

u(t) représente la commande d'entr e constituée de la force spécifique et de la vitesse angulaire. Ces 2 grandeurs sont mesurées par les capteurs Cl de façon discrète dans le temps selon une période 7^.

L'état global comporte entre autres les coordonnées de position, de vitesse, ainsi que l'orientation du porteur sous forme d'une ou plusieurs rotations, chacune d'elles représentée par exemple par une matrice ou un quaternion d'attitude.

L'estimateur E exploite une version discrète de ces équations à temps continu obtenue selon les règles de l'état de l'art :

Le filtre EKF fonctionne de manière itérative selon une étape de prédiction prenant en compte les mesures des capteurs Cl et une étape de mise à jour prenant en compte les mesures des capteurs CC :

L'étape de prédiction comprend la prédiction d'un état global estimé, et la prédiction d'une covariance associée. Prédiction état global estimé :

Prédiction covariance associée :

où correspond à la matrice de transition dépendant de la fonction de

propagation f, et Q la matrice de bruits de modèle.

L'étape de mise à jour utilise les variables suivantes :

Innovation :

Covariance innovation :

où H _k correspond à la matrice d'observation.

Gain de Kalman :

La mise à jour est mis en œuvre comme suit :

Mise à jour état global estimé :

Mise à jour covariance associée :

Matrice de transition théorique :

Avec : Matrice d'observation :

L'estimateur E bien que proche de cet algorithme en est légèrement différent. La mise à jour de l'état global peut ne pas se faire de façon totalement additive, notamment pour préserver les propriétés des matrices de rotation, et notamment également pour tenir compte du rythme rapide des mesures des capteurs Cl. La prédiction de l'état global estimé peut se faire alors à un rythme plus rapide que celui des autres opérations. La différence de rythme peut dépendre de la dynamique du porteur. D'autres différences sont possibles. Par exemple, une approximation peut être faite sur la matrice de transition.

Est illustré sur la figure 4 un estimateur E selon un mode de réalisation comprenant différents blocs fonctionnels correspondant à des étapes respectives de sa mise en œuvre.

L'estimation est mis en œuvre en itérations successives, chaque itération étant identifiée par un indice k. La prédiction de l'état global estimé selon une cadence 1 est effectuée dans l'étape 100.

Une mémoire sur 1 cycle, ainsi qu'une étape de recalage fonctionnent au même rythme. Cette étape de recalage prend en compte des données mises à jour à une cadence 2, plus lente.

Tous les autres traitements fonctionnent à la cadence 2. L'étape 200 convertit des états de cadence 1 en des états de cadence 2. A la cadence 2, ce dernier état provenant de la dernière mise à jour du

filtre de Kalman 400, représentent dans l'ordre l'état global prédit, l'état linéarisé prédit, l'état global mis à jour, l'état linéarisé mis à jour. L'état x peut dans certains cas être toujours à 0.

Les matrices de transition et d'observation sont calculées dans l'étape de linéarisation 300. Une sous-étape d'ajustement, propre à l'invention et détaillée plus loin, y est ajoutée.

L'innovation est calculée d'après l'état global prédit et d'après l'observation des capteurs CC représentée en bas de la figure 4.

Les autres étapes de l'EKF sont regroupées dans l'étape 400 réalisant des estimations à l'aide de matrices, et constituant un filtre de Kalman, ces estimations portant sur des états linéarisés et des covariances. Une particularité provient du fait que l'étape 400 ne comporte pas de prédiction d'état linéarisé, laquelle peut éventuellement être incorporée dans l'étape 100 à cadence 1.

L'étape 401 comporte la prédiction de la covariance associée à l'état global prédit et exploite la matrice de transition. L'étape 402 comporte le calcul de la covariance de l'innovation, des gains de Kalman, de la mise à jour de la covariance de l'état global, et de la mise à jour de l'état linéarisé. Cette étape exploite la matrice d'observation.

Les points de linéarisation dans l'étape 300 peuvent être calculés en fonction de ce dernier état provenant de la dernière

mise à jour du filtre de Kalman 400. Si , alors et

Un premier point de linéarisation X^ _k _ _x permet de calculer dans l'étape

300 la matrice de transition à la période de Kalman correspondant à

la cadence 2. Ce point de linéarisation correspond à l'état global estimé après le dernier recalage.

Un deuxième point de linéarisation x _{H k} permet de calculer au cycle de

Kalman k dans l'étape 300 la matrice d'observation Ce point de

linéarisation est obtenu d'après les calculs de l'étape 100. Il correspond au dernier état prédit avant le prochain recalage.

Afin de simplifier les notations, la matrice de transition

sera notée désormais et la matrice d'observation notée

désormaisH s'expriment donc chacune en fonction d'un état global correspondant à leurs points de linéarisation respectifs

Comme dit précédemment, les étapes sont répétées récursivement à l'itération suivante d'instant k+1 à la cadence 2, cadence du filtre de Kalman.

Ajustement des matrices de transition et d'observation

L'étape de linéarisation mise en œuvre dans chaque cycle de Kalman constitue un point faible de l'estimateur E.

Aussi, le procédé d'estimation comprend dans l'étape 300, une étape supplémentaire d'ajustement en fonction d'une condition d'observabilité, venant ajuster, avant leur utilisation par le filtre de Kalman, les matrices générées par cette étape de linéarisation : D'une part la matrice de transition servant à propager la matrice de covariance, et d'autre part la matrice d'observation.

La condition d'observabilité consiste en une contrainte imposée au noyau de la matrice d'observabilité (la définition de cette matrice est indiquée en annexe 6) au travers d'un ajustement des matrices de transition et d'observation de façon à inclure dans le noyau un modèle prédéterminé de sous- espace vectoriel non observable. Ce modèle de non-observabilité consiste en une équation dans l'espace d'état d'un sous-espace vectoriel représenté par une base, en fonction d'un état global, ce sous-espace vectoriel étant mis à jour à chaque cycle de Kalman. Le fait d'inclure ce sous-espace dans le noyau de la matrice d'observabilité le rend non observable et diminue le risque d'incohérence si le modèle est pertinent.

L'état global à partir duquel est calculé le sous-espace vectoriel rendu non observable correspond au point de linéarisation de la fonction d'observation (Voir annexe 6). Un vecteur non observable a en effet une image nulle au travers de la matrice d'observation, et ce vecteur correspond à un état linéarisé au voisinage d'un état global correspondant au point de linéarisation de la fonction d'observation.

Au cours de l'étape d'ajustement, on va calculer, pour chacune des deux matrices de transition et d'observation, un écart matriciel correspondant.

Par convention, on utilisera dans la suite des notations étoilées pour désigner les matrices de transition et d'observation obtenues par l'ajustement.

Le flux des données nécessaires au calcul d'ajustement est représenté de façon schématique sur la figure 8, mettant en évidence le fait que l'ajustement de la matrice d'observation dépend du point de linéarisation de la fonction d'observation dans le cycle de Kalman courant, et que l'ajustement de la matrice de transition dépend du point de linéarisation de la fonction d'observation dans le cycle courant et dans le cycle précédent.

Il est donc nécessaire d'exploiter une base B _k générant le sous-espace vectoriel non observable au cycle k, ainsi qu'une base B _k _ ₁ générant le sous- espace vectoriel non observable au cycle k-1 .

II est possible alors, sur un cycle de Kalman, de mémoriser le point de linéarisation et de générer 2 fois par cycle la base non observable, ainsi que cela est représenté sur la figure X. Il est possible de façon équivalente sur un cycle de Kalman, de mémoriser la base non observable de façon à ne la générer qu'une fois par cycle. Cette deuxième option correspond au schéma de principe de la figure 6. Celle-ci détaille les fonctions d'ajustement décrites sur la figure 5. La figure 6 détaille les calculs conduisant à l'obtention des deux écarts matriciels relatifs aux matrices de transition et d'observation au cycle de Kalman d'instant k.

La matrice de transition du cycle de Kalman k-1 au cycle k et la matrice d'observation au cycle k sont ajustées d'après un modèle de non observabilité utilisant les vecteurs d'état globaux et X points de linéarisation de la

fonction d'observation aux cycles k-1 et k. Ce modèle permet d'estimer, dans une étape 301 , l'espace non observable des états linéarisés sous forme d'une base primaire non observable . Une telle base peut être calculée à chaque

cycle de Kalman en utilisant une fonction analytique obtenue lors de la conception du filtre d'après la méthode présentée en Annexe 6 à l'aide d'un système dynamique à temps continu, puis appliquée au système dynamique à temps discret utilisé par la centrale de navigation.

Deux exemples de situations à risques pouvant être traitées selon le procédé de l'invention sont décrits en annexe 1 . Un modèle de base non observable propre à l'alignement statique est développé en Annexe 2.

Des hypothèses simplificatrices sont nécessairement faites pour élaborer ce modèle lors de la phase de conception car aucune équation contenant un nombre limité de variables ne peut représenter le monde vrai. Il est possible ainsi que le nuage d'erreur vrai se réduise lentement selon toutes les directions de l'espace d'état mais que l'EKF, en raison des non-linéarités et des bruits d'estimation, estime une décroissance plus rapide selon certains axes, ce qui crée une incohérence. Il est possible alors de modéliser la décroissance lente selon un certain axe par une décroissance nulle dans le temps et de considérer cet axe non observable afin de pouvoir y imposer une contrainte. Cette simplification réduit légèrement la précision mais garantit la cohérence des estimations. Mais comme il n'est plus nécessaire de recourir à des bruits de modèle pour empêcher l'enveloppe 3 sigma estimée de se réduire le long des axes non observables, il en découle finalement un gain en précision.

L'alignement statique décrit en annexe 1 est un exemple de situation dans laquelle le modèle ne prend pas en compte les petits mouvements imperceptibles qui, à court terme n'ont pas d'effet, mais qui peuvent être observés sur le long terme, ce qui provoque une décroissance lente de l'enveloppe 3 sigma vraie sur certains axes. Il est proposé, par soucis de simplification du modèle, de ne pas en tenir compte et de modéliser une base non observable en considérant que ces petits mouvements sont nuls, ce qui est un exemple de simplification. Dans le deuxième exemple de l'annexe 1 , le mouvement de houle peut au même titre être négligé pour modéliser la base non observable. Si la base est orthogonale, il existe une solution décrite dans l'annexe

3 permettant d'ajuster les matrices de façon simple. Cependant, cette condition, dans le cas général, n'est pas satisfaite. La solution proposée consiste à orthogonaliser la base puis à effectuer l'ajustement. Les traitements décrits ci-dessous sont justifiés par le paragraphe en fin de l'annexe 3 traitant de la base non orthogonale.

Dans une étape d'orthogonalisation 302, la base primaire est orthogonalisée en une base secondaire selon une variante de la méthode de

Gramm-Schmitt, la particularité provenant du fait que les vecteurs ne sont pas rendus unitaires car la normalisation est incluse implicitement dans la formule d'ajustement des matrices présentée par la suite. Cette opération génère une matrice de coefficients ayant servi à l'orthogonalisation.

Soit q le nombre de vecteurs de la base primaire non observable. On pose :

Au cycle k, l'étape d'orthogonalisation peut mettre en œuvre le calcul suivant :

Cette base ainsi que les coefficients d'orthogonalisation sont mémorisés à l'étape 303 pendant un cycle de Kalman.

La base permet de calculer dans une étape 308 un écart matriciel ôH _k relatif à la matrice d'observation et ce sans inversion d'une quelconque

matrice, ce qui peut permettre de réduire la charge de calcul. Cette étape peut être omise si la matrice d'observation est indépendante de l'état estimé. Dans ce cas, l'écart matriciel est nul.

Il est possible de décomposer l'écart matriciel ôH _k relatif à la matrice d'observation _k ,en q écarts matriciels élémentaires :

Cette décomposition permet ainsi de paralléliser le calcul de chaque écart matriciel élémentaire et ainsi de raccourcir le temps de calcul de l'étape 308.

Chaque écart matriciel élémentaire peut, d'après l'annexe 3, être calculé comme suit : Pour i compris entre 1 et

Par ailleurs, au cycle k, la base et les coefficients (mémorisés au cycle de Kalman précédent k-1 ) permettent de reconstruire une base non observable dans une étape de reconstruction référencée 304. Cette base reconstruite à l'instant k dépend de la base primaire à l'instant k, mais aussi des coefficients d'orthogonalisation à l'instant k-1 . Son indice est donc noté correspondant à l'instant k sachant les estimations à l'instant k-1 . Les bases et permettent de calculer l'écart matriciel sans inversion

matrice.

On pose :

Au cycle k, l'étape de reconstruction 304 peut mettre en œuvre le calcul suivant

Dans une étape référencée 306, on calcule un écart matriciel relatif à la matrice de transition.

Il est possible de décomposer l'écart matriciel relatif à la matrice

de transition en q écarts matriciels élémentaires :

Chaque écart matriciel élémentaire peut, d'après l'annexe 3, être calculé comme suit :

Pour i compris entre 1

Cette décomposition permet ainsi de paralléliser le calcul de chaque écart matriciel élémentaire et ainsi peut permettre de raccourcir le temps de calcul de l'étape 306.

Pour réduire l'occupation mémoire et les calculs, on peut ne considérer qu'une sous-matrice de la matrice de transition : Celle en correspondance avec les valeurs non nulles des vecteurs non observables. Les deux écarts matriciels obtenus permettent de vérifier une condition importante d'observabilité, condition qui est détaillée en Annexe 7.

Par ailleurs, l'Annexe 3 explique l'origine de la méthode de calcul, et l'annexe 8 en donne une démonstration mathématique.

L'étape de calcul 306 produit en outre une métrique représentative d'une amplitude d'ajustement induite par l'écart matriciel relatif à la matrice de transition.

L'étape de calcul 308 produit en outre une métrique représentative d'une amplitude d'ajustement induite par l'écart matriciel relative à la matrice d'observation.

Le même type de calcul de métrique peut être mis en œuvre pour la matrice de transition et la matrice d'observation. Par exemple, pour la matrice de transition, on calcule séparément le carré de la norme de l'écart matriciel, et celui de la matrice de transition. La métrique correspond alors au rapport de ces deux grandeurs, ou à la racine carrée du rapport de ces deux grandeurs.

La norme utilise éventuellement des coefficients de pondération pour normaliser les grandeurs intervenant dans le vecteur d'état. Par exemple, une erreur de position peut être de l'ordre de quelques mètres, alors qu'une erreur d'attitude peut-être de l'ordre de quelques milli-radians. La norme de matrice peut correspondre à la norme du vecteur que l'on formerait en mettant tous les termes de la matrice sous la forme d'un vecteur colonne (norme de Froebenius).

L'Annexe 4 explique comment exploiter des coefficients de pondération dans le calcul de métriques.

Le calcul de métriques fait appel éventuellement à des fonctions de pondération et Si la matrice d'observation est indépendante de

l'état estimé, l'écart matriciel es\. nul, et sa métrique est nulle. Il est

inutile de la calculer. Identiquement, si la matrice de transition est indépendante de l'état estimé, le calcul de la métrique associée peut être omise.

De retour à la figure 5, dans une étape de décalage 330, la matrice de transition est sommée à l'écart matriciel associé, de façon à obtenir une matrice de transition ajustée ; et la matrice d'observation est sommée à l'écart matriciel associé, de façon à obtenir une matrice d'observation ajustée.

La séquence 310 illustrée en figure 6 produisant une paire d'écarts matriciels et une paire de métriques peut être mise en œuvre n fois en parallèle.

On suppose que l'on connaît, pour n situations prédéterminées dans le domaine continu, un modèle de base non observable. Ce modèle provient de la résolution d'un système d'équations représentatives d'un type d'observation respectif et éventuellement d'une trajectoire respective du vecteur d'état, dont une méthode est donnée en Annexe 6.

Dans une étape de décision 320, l'une des n paires d'écart matriciels est élue (par exemple la paire obtenue par l'ajustement correspondant à la situation i), et l'étape de décalage est mise en œuvre seulement avec cette paire de matrices pour produire les matrices ajustées.

L'étape de décision 320 réalise un test de vraisemblance sur chaque paire d'écarts matriciels, en inspectant la valeur des métriques correspondantes. Ce test permet de prendre la décision de reconnaître telle ou telle situation.

L'écart matriciel élu est l'écart matriciel candidat associée à la métrique représentative de l'amplitude d'ajustement la plus faible. Ainsi, l'ajustement est réalisé avec les écarts matriciels les plus « vraisemblables ». Le critère de vraisemblance peut porter sur l'ajustement de la matrice de transition et/ou de la matrice d'observation.

Par ailleurs, le décalage n'est mis en œuvre que si la métrique de l'écart matriciel élu est inférieure à un seuil prédéterminé : il peut en effet être préférable de ne pas opérer de décalage des matrices de transition et d'observation, si ce décalage n'est pas considéré suffisamment fiable et introduit un bruit supplémentaire dans le système plutôt qu'il n'en corrige.

Le procédé d'estimation qui précède peut être mis en œuvre par un programme d'ordinateur exécuté par les moyens de calcul de la centrale inertielle IN. Par ailleurs, la carte 1 d'identification comporte un produit programme d'ordinateur comprenant des instructions de code de programme pour l'exécution des étapes du procédé décrit, lorsque ce programme est exécuté par la carte 1 d'identification.

ANNEXE 1 : Exemples de situations à risque d'une centrale inertielle de navigation exploitant un filtre EKF

Un premier exemple concerne une centrale inertielle de navigation en phase d'alignement statique au sol. Dans cette phase, la verticale locale est estimée en mesurant le vecteur de pesanteur à l'aide des accéléromètres, et l'orientation de la centrale est estimée en mesurant le vecteur de vitesse angulaire de rotation terrestre à l'aide des gyromètres. L'observation utilisée dans cette phase est typiquement la ZUPT (Zéro velocity Update) qui est une observation d'un capteur virtuel indiquant que la vitesse du porteur est nulle par rapport à la Terre. L'erreur de mesure est modélisée par un bruit blanc. Or, il arrive que les capteurs inertiels enregistrent des petits mouvements non aléatoires dus par exemple au vent ou à des effets de dilatation dus à la chaleur. Cela correspond à une erreur de mesure déterministe non prise en compte dans le modèle de capteur non inertiel produisant l'observation ZUPT. Il y a alors incohérence entre le système dynamique vrai et le système dynamique modélisé. Tant que la durée de l'alignement statique est courte, l'incohérence est négligeable. Il existe par contre de nombreuses applications nécessitant un alignement de plusieurs heures, ou de plusieurs jours, semaines ou mois. Dans ce cas, il est possible que l'incohérence devienne prépondérante.

Un deuxième exemple concerne une centrale inertielle de navigation montée sur un bateau de surface exploitant une observation de vitesse en projection dans le repère bateau. Cette mesure non inertielle est faite en général à l'aide d'un capteur « Loch » et il s'agit de plus d'une mesure relative par rapport à l'eau. Le fait qu'elle soit réalisée en projection dans le repère bateau crée une dépendance de la mesure vis-à-vis de l'attitude du bateau. Il est possible alors, en supposant que seule cette observation soit utilisée, qu'une incohérence apparaisse entre les estimations du filtre EKF et le monde vrai, et que cette incohérence produise des erreurs importantes au bout de plusieurs heures si le bateau navigue à cap constant.

ANNEXE 2 : Modèle d'axes non observables en alignement statique

Cette annexe présente la modélisation de la base non observable permettant de résoudre l'incohérence du premier exemple de l'annexe 1 à l'aide du procédé proposé.

Hypothèses : Le filtre de Kalman exploite le modèle d'erreurs en PHI et la mécanisation fonctionne en azimuth libre.

Les notations suivantes sont utilisées :

Les composantes d'états intervenant dans les équations sont présentées dans un ordre arbitraire qui est le suivant :

Les mesures acquises sont des vitesses. Le porteur est supposé parfaitement immobile en translation et rotation. Les défauts capteurs sont supposés constants dans le temps.

Ces équations forment un système stationnaire de la forme

thode de recherche des vecteurs non observables

Les axes non observables sont les suivants

ANNEXE 3 : mise en contrainte du filtre

Des écarts matriciels sont appliqués aux matrices de transition et d'observation à chaque cycle de Kalman d'après un modèle de q axes non observables obtenus à chaque cycle k selon des

fonctions de l'état global x _{H k} correspondant au point de linéarisation de la matrice d'observation à la période de Kalman. ^est remplacé par :

et tel que pour i compris entre 1 et q : Equation 1 :

minimum vis-à-vis de toutes les solutions possibles.

est remplacé par proche de

et tel que pour i compris entre 1 et q :

Equation 2 : minimum vis-à-vis de toutes

les solutions possibles.

La norme des matrices considérée est la norme de Froboenius.

Les matrices sont alors utilisées dans

l'étape de propagation de la matrice de covariance, et du calcul du gain de Kalman.

On adopte désormais les notations suivantes :

Résolution des équations 1 et 2 dans le cas où les vecteurs non observables modélisés sont orthogonaux entre eux à chaque

instant k :

L'écart matriciel total de la matrice de transition correspond à la somme de q écarts matriciels indépendants. Il en est de même pour l'écart matriciel de la matrice d'observation.

Et , pour i compris entre 1 et q :

La démonstration de ces relations est donnée en annexe 8 selon une méthode géométrique. Résolution des équations 1 et 2 pour une base non orthogonale, et pour q quelconque :

Cette base définit un modèle de l'espace vectoriel non observable : Toute combinaison linéaire des vecteurs de base est non observable.

L'image par la matrice d'observation ajustée de toute combinaison linéaire de ces vecteurs est donc nulle. Il est donc possible d'orthogonaliser la base, puis d'appliquer la méthode précédente pour calculer l'écart matriciel de la matrice d'observation.

D'autre part, l'image par la matrice de transition ajustée d'une combinaison linéaire de vecteurs non observables correspond à cette même combinaison linéaire appliquée aux images par la même matrice de transition de chacun des vecteurs non observables de départ. Il est donc possible de déterminer la combinaison linéaire permettant d'orthogonaliser la base de vecteurs non observables au cycle k-1 , et d'utiliser cette même combinaison pour transformer le modèle de base non observable au cycle k. Cela permet d'appliquer la méthode précédente pour calculer l'écart matriciel de la matrice de transition.

ANNEXE 4 : Métriques pondérées

Des scalaires ιη _φ et m _H appelés « métriques » accompagnent les matrices d'ajustement calculées et donnent une indication sur l'amplitude relative de l'ajustement, le but étant de modifier le plus faiblement possible les matrices de transition et d'observation. Différentes méthodes sont possibles afin de comparer

a et

des constituants élémentaires _ce s normes pouvant

être calculées de la même façon que celle d'un vecteur en rassemblant tous les coefficients de la matrice en une seule colonne.

Des exemples sont donnés ci-dessous :

Des variantes sont possibles en faisant appel à des fonctions de pondération et pour équilibrer le poids des différentes composantes dans

le calcul des normes de matrices. Les mêmes exemples que les précédents peuvent être repris en tenant compte de ces fonctions.

Une réalisation possible de ces fonctions de pondération consiste à construire d'abord une fonction de pondération d'un vecteur d'état, puis

d'en déduire

Ainsi par cette méthode, au vecteur est associé par une fonction de pondération un vecteur où les coefficients réels a _l

forment les coefficients de pondération. Ces derniers permettent alors

de pondérer la matrice de transition de la façon suivante :

Car et réciproquement. Ce sont 2 façons

équivalentes d'écrire la relation entre U et V, la deuxième permettant de calculer des normes de façon équilibrée.

effet, en posant

D

ANNEXE 5 : Notion de système dynamique et application à la navigation

C'est un système d'équations déterministe comportant une équation d'évolution et une équation d'observation. L'équation d'évolution décrit un vecteur d'état en fonction de son état antérieur et d'une commande. L'équation d'observation fournit un scalaire ou vecteur dépendant du vecteur d'état. Ces équations peuvent être non linéaires. Il existe des systèmes à temps continu et des systèmes à temps discret.

Système à temps continu :

Système à temps discret :

Où et x _k désignent l'état du système, u(t) et u _k désignent la commande d'entrée, et désignent l'observation à un instant continu t ou instant

discret k, selon le mode de représentation continu ou discret.

Les équations de la mécanique classique de Newton permettent de décrire l'état d'un repère mobile en fonction de son accélération et de sa vitesse angulaire à tout instant. Ces équations constituent un exemple de fonction d'évolution continue, le vecteur d'état rassemblant les informations de position, de vitesse, et d'orientation du repère mobile, et le vecteur de commande étant constitué de l'accélération et la vitesse angulaire.

Dans l'état de l'art d'une centrale inertielle de navigation, les calculs ne peuvent être réalisés que de façon discrète. Il en résulte que cette fonction d'évolution continue y est modélisée sous forme d'une fonction d'évolution discrète. Différentes méthodes connues permettent de convertir les équations d'évolution continues en équations d'évolution discrètes.

La fonction d'observation modélisée dans le système dynamique à temps discret de la centrale inertielle décrit les mesures effectuées par un capteur non inertiel particulier. Plusieurs fonctions d'observation peuvent alors être mises en œuvre successivement dans le temps en fonction des capteurs utilisés.

Le système dynamique à temps discret utilisé dans la centrale inertielle de navigation est donc un modèle le plus fidèle possible d'un autre système dynamique, à temps continu, correspondant au monde vrai. Les propriétés d'observabilité de ces deux systèmes sont donc différentes.

Les propriétés d'observabilité d'un système dynamique caractérisent les informations sur l'état du système à un instant donné qui peuvent être obtenues en tenant compte des observations effectuées ultérieurement et des fonctions d'évolution et d'observation. Ces propriétés dépendent notamment de la commande, c'est-à-dire des mouvements du porteur dans le cas de la centrale inertielle de navigation. ANNEXE 6 : Observabilité d'un système à temps continu

Considérons un système dynamique continu non linéaire qui, à l'instant 0 se trouve dans un état x(o) . Par la suite, la fonction de propagation ainsi que la commande d'entrée font passer le système dans des états définissant ainsi

une trajectoire particulière dans l'espace d'état.

Cette trajectoire étant connue, supposons que l'état initial s'écarte maintenant de d'une petite valeur appelée état linéarisé et que les

commandes d'entrée soient les mêmes que dans la situation précédente. Les états suivants sont alors La fonction d'observation permet de recueillir à chaque étape des informations sur l'état courant. Le problème posé est de savoir si ces informations du début à la fin du scénario considéré permettent de connaître L'espace des états linéarisés à l'instant 0 est

alors composé d'un sous-espace observable et un sous-espace non observable. L'espace non observable à l'instant 0 caractérisé par l'état δχ(θ) peut alors se déduire d'après un nouveau système dynamique permettant de décrire les états linéarisés en connaissant la tra ectoire où F et H sont des matrices

variables dans le temps correspondant à la matrice d'évolution et la matrice d'observation du système dynamique linéarisé pour une trajectoire donnée.

L'espace non observable est alors constitué d'états linéarisés 5X(t) tels que pour

Les solutions forment un espace vectoriel d'états linéarisés. Il existe alors une base B(O) de cet espace formant des vecteurs non observables à l'instant 0.

ANNEXE 7 : Observabilité d'un système à temps discret

Considérons un système dynamique discret non linéaire qui, à l'instant 0 se trouve dans un état X ₀ . A différents instants discrets successifs, la fonction de propagation ainsi que la commande d'entrée font passer le système dans des états Connaissant ces états successifs, supposons que l'état initial s'écarte maintenant de X ₀ d'une petite valeur δΧ ₀ appelé état linéarisé et que les commandes d'entrée soient les mêmes que dans la situation précédente. Les états successifs suivants sont alors La fonction

d'observation permet de recueillir à chaque étape des informations sur l'état courant. Le problème posé est de savoir si ces informations du début à la fin du scénario considéré permettent de connaître δΧ ₀ . L'espace des états linéarisés à l'instant 0 est alors composé d'un sous-espace observable et un sous-espace non observable. Un observateur fonctionnant sur le principe de la linéarisation, comme l'EKF ne pourra alors jamais estimer une erreur dans l'espace non observable des états linéarisés. L'espace non observable à l'instant 0 caractérisé par l'état X peut alors

se déduire en considérant le système dynamique des états linéarisés mettant en œuvre la fonction d'évolution et la fonction d'observation linéarisées aux différents états La fonction d'évolution linéarisée

à l'instant k est appelée « matrice de transition » notée La fonction

d'observation linéarisée à l'instant k est notée H

Une première observation de l'écart δΧ ₀ autour de X ₀ est faite à l'aide de la matrice d'observation correspondant à la fonction d'observation linéarisée en X ₀ . Une prédiction de l'écart δΧ _ι autour de X _l est faite à partir de l'écart δΧ ₀ autour de X ₀ à l'aide de la matrice de transition correspondant à la fonction d'évolution linéarisée en Une deuxième

observation de l'écart δΧ _ι autour de est faite à l'aide de la matrice d'observation etc..

On sait alors que l'espace non observable du système décrivant la perturbation est le noyau de la matrice :

L'espace non observable et l'espace observable dépendent donc de l'état de référence initial X ₀ , de la commande d'entrée (données inertielles dans le cas des centrales inertielles), et des fonctions d'évolution et d'observation.

Supposons que le vecteur fasse partie du noyau de ( )

Posons : A un instant donné, une condition d'observabilité est alors formée pour une direction non observable ar le système d'équations:

Où forme un vecteur non observable particulier à un instant discret k.

Cette condition s'applique à chaque instant et est liée à la trajectoire

Dans l'EKF, cette trajectoire est bruitée par les erreurs d'estimation. Ce bruit a tendance à réduire le noyau de la matrice d'observabilité, et donc à diminuer la dimension de l'espace non observable.

De plus les fonctions de transition et d'observation ne sont pas linéarisées au même point. La suite est remplacée par une suite

La condition d'observabilité s'écrit alors :

ANNEXE 8 : démonstration de la formule d'ajustement de matrices pour une base orthogonale de vecteurs

Hypothèse : p matrices colonne Ui telles que : | |

Soit p matrices colonne ¼ de même dimension que les Ui .

Soit la matrice A.

Problème : On recherche la matrice δΑ de norme minimum telle que : La matrice peut se mettre sous forme de p matrices ligne en

colonne, et les vecteur V _k sous forme de q scalaires en colonne :

On recherche donc les 6L _k de norme minimum tels que

Interprétation géométrique du problème : Les matrices ligne et colonne sont associées à des vecteurs dans un espace vectoriel de dimension supérieure à q. On garde les mêmes notations en ajoutant une flèche au-dessus des variables.

La condition (1 ) se traduit par :

La condition (2) se traduit par le produit scalaire suivant :

La projection du vecteur L _k sur l'axe orienté défini par le vecteur unitaire a pour abscisse sur cet axe. C'est le cosinus directeur de

selon cet axe.

On recherche 6L _k tel que la projection de sur cet axe soit

peut être décomposé selon les vecteurs Ui qui génèrent un espace vectoriel de dimension q, et selon une composante résiduelle ¾ orthogonale à cet espace

Et donc

La figure 7 montre qu'il existe une infinité de solutions La solution

minimale correspond à un vecteur 6L _kl colinéaire à

D'après la figure, on a :

On a donc , et

De retour en notation matricielle, on a donc

En reformant les matrices A et δΑ, après quelques manipulations, on obtient la relation :