Brevet FR3028031A1 — publié le 20160506

Procédé d'estimation d'un état de navigation contraint en observabilité DOMAINE GEN ERAL 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 oeuvre 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 oeuvre 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 10 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 15 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 : 20 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, 25 le procédé étant caractérisé en ce qu'il comprend une étape d'ajustement de la matrice de transition et/ou de la matrice d'observation avant leur utilisation dans le filtrage de Kalman étendu, de sorte que chaque matrice ajustée vérifie 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 30 de Kalman de réduire la dimension de la région le long d'au moins un axe de l'espace d'état. 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 oeuvre 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 oeuvre 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 oeuvre avant l'étape de propagation, en l'état estimé précédent. La condition d'observabilité à vérifier par les matrices de transition et d'observation ajustées peut être la nullité du noyau d'une matrice d'observabilité qui leur est associée. L'ajustement peut en outre comprendre 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 et éventuellement celle du cycle précédent, décalage de chaque matrice à ajuster selon l'écart matriciel qui lui est associé de sorte à vérifier la condition d'observabilité. 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 oeuvre à 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 oeuvre 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 oeuvre 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 15 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 oeuvre du procédé selon le premier aspect de l'invention. DESCRIPTION DES FIGURES 20 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 oeuvre 25 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. 30 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 oeuvre 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 15 CI, des capteurs complémentaires CC, et des moyens E pour mettre en oeuvre des calculs d'estimation. Ces parties peuvent être physiquement séparées les unes des autres. Les capteurs inertiels CI sont typiquement des accéléromètres et/ou des gyromètres mesurant respectivement des forces spécifiques et des vitesses de 20 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, 25 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. 30 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é : fX(t) = f (X (t), u(t)) z(t) = 11(X (t)) 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 CI de 15 façon discrète dans le temps selon une période T1. L'état global X(t) 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. 20 L'estimateur E exploite une version discrète de ces équations à temps continu obtenue selon les règles de l'état de l'art : {Xic = Le filtre EKF fonctionne de manière itérative selon une étape de prédiction prenant en compte les mesures des capteurs CI et une étape de mise 25 à 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. Zk - h(Xk) Prédiction état global estimé : gkik-i = ek-iik-i, uk-i) Prédiction covariance associée :P - kik-i = Ok-1-4kr)k-1ik-1igc-1,k + Qk-1 OÙ ok_l,k 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 : .5"k - zk h(gklk-1) Covariance innovation : -s'Ic = Hkr)k-11k-1111: + Rk où Hk correspond à la matrice d'observation. Gain de Kalman : Kk = 13k-11k-11-11:S-1 k La mise à jour est mis en oeuvre comme suit : Mise à jour état global estimé : g/clic = gkik_i + Kk5,k Mise à jour covariance associée : Pkik = (I - KkHk)P kik-1 kT Matrice de transition théorique : CI5 T k-1->k = f(k-1)T exP(F (T))c1T Avec : F(t) = -ai« Matrice d'observation : a xlx(0,u(t) ah Hk = - aXIP -klic-i 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 oeuvre. L'estimation est mis en oeuvre 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, Îklk-1, .îkik, 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 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 Îklk, .îkik, ce dernier état provenant de la dernière mise à jour du filtre de Kalman 400. Si O, alors -f17.1(-11k-1 et 517.H,k 5(.1c1k-l- 302 803 1 13 Un premier point de linéarisation k-1 permet de calculer dans l'étape 300 la matrice de transition é k 1->k( k ,k 1) à la période de Kalman correspondant à la cadence 2. Ce point de linéarisation correspond à l'état global estimé après le dernier recalage. 5 Un deuxième point de linéarisation ,k permet de calculer au cycle de Kalman k dans l'étape 300 la matrice d'observation I kK ,k) - 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 Ok-1->k(g0,k-1) 10 sera notée désormais à k 1->k et la matrice d'observation Hk(gH,k) notée désormaisHk . é k-1->k et H1 s'expriment donc chacune en fonction d'un état global correspondant à leurs points de linéarisation respectifs go,k_i_ et gH,k. 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. 15 Ajustement des matrices de transition et d'observation L'étape de linéarisation mise en oeuvre dans chaque cycle de Kalman constitue un point faible de l'estimateur E. 20 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 25 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- 30 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 20 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 Bk générant le sous-espace vectoriel non observable au cycle k, ainsi qu'une base /3/,_, générant le sous-espace vectoriel non observable au cycle k-1. 25 Il 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 30 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 5 d'observation au cycle k sont ajustées d'après un modèle de non observabilité utilisant les vecteurs d'état globaux ÎH,k-1 et ÎH,k, 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 B. . Une telle base peut être calculée à chaque 10 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 15 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 20 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 25 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. 30 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 Bk 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 B est orthogonalisée en une base secondaire Bk2 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 20 d'ajustement des matrices présentée par la suite. Cette opération génère une matrice Tk kLq de coefficients ayant servi à l'orthogonalisation. Soit q le nombre de vecteurs de la base primaire non observable. On pose : B kl =1,2,k 121,q,k1 , et 25 B12t- /22,2,1 - - - V2 2,q,ki Au cycle k, l'étape d'orthogonalisation peut mettre en oeuvre le calcul suivant : V21 "'') 1,1,k , Tk [13111.1. Chaque écart matriciel élémentaire peut, d'après l'annexe 3, être calculé comme suit : Pour i compris entre 1 et q, 1 6H /k kl'22,i,k)V2,i,k V2,i,k1';2,i,k Par ailleurs, au cycle k, la base Bk1 et les coefficients Tk (mémorisés au cycle de Kalman précédent k-1) permettent de reconstruire une base non 20 observable Bk3ik 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é k/k-1 correspondant à l'instant k sachant les estimations à l'instant k-1. Les bases B121 302 803 1 18 et Bk3/k permettent de calculer l'écart matriciel 6k 1k sans inversion de matrice. On pose : Bk3/k-1 P3,1,k/k-1 123,2,k/k-1 - 3,g k-1] 5 Au cycle k, l'étape de reconstruction 304 peut mettre en oeuvre le calcul suivant : Coefficients a1_1 extraits de Tk = ,k /23,1,k/k-1 =1,1,k Puis : Vi 2, i q 19 3,i ,k I k -1 9 1,i ,k ,-=1 -1 3,j ,k I k -1 Dans une étape référencée 306, on calcule un écart matriciel relatif à la 10 matrice de transition. Il est possible de décomposer l'écart matriciel 365 1,k 1->k relatif à la matrice de transition 01, k 1->k en q écarts matriciels élémentaires : 60k-1->k I60i,k-1->k i=1 Chaque écart matriciel élémentaire peut, d'après l'annexe 3, être calculé 15 comme suit : Pour i compris entre 1 et q, 1 6i .k-1k T 3,i ,k1 k-1 k 2,i,k-1)V^ 21. ,i,k-1 v2,i ,k-11';2,i,k-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 10 d'observation. Le même type de calcul de métrique peut être mis en oeuvre 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 15 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 20 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 25 pondération go() et g-H( ). Si la matrice d'observation H,, est indépendante de l'état estimé, l'écart matriciel SH,, est 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. 30 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 oeuvre 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 oeuvre 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 oeuvre que si la métrique de l'écart matriciel élu est inférieure à un seuil prédéterminé : il peut en effet être 25 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 oeuvre par un programme d'ordinateur exécuté par les moyens de calcul de la centrale 30 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 (Zero 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 5 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 : [t] Repère terrestre. x=axe de rotation [m] Repère de mesure [v] Repère plateforme, en azimuth libre Tl Matrice de passage de [t] à [v] Matrice de passage de [m] à [v] a Angle de mécanisation S-2 Vitesse de rotation terrestre g Module de la gravité (gravité = pesanteur = gravitation) ge Module de la gravité à l'équateur ao, K1, K2 Constantes liées aux constantes géodésiques WGS84 R Rayon de courbure de l'ellipsoïde de référence dans la direction x de [v] Ry Rayon de courbure de l'ellipsoïde de référence dans la direction y de [v] RN Rayon de courbure de l'ellipsoïde de référence dans la direction Nord RE Rayon de courbure de l'ellipsoïde de référence dans la direction Est 1" Torsion géodésique sur l'ellipsoïde de référence L Latitude z Altitude Ax Erreur de position selon l'axe x de [v] Ay Erreur de position selon l'axe y de [v] Az Erreur de position selon l'axe z de [v] AV Erreur de vitesse selon l'axe x de [v] AV y Erreur de vitesse selon l'axe y de [v] AV z Erreur de vitesse selon l'axe z de [v] Erreur de rotation selon l'axe x de [v] Oy Erreur de rotation selon l'axe y de [v] Oz Erreur de rotation selon l'axe z de [v] Ab, Erreur de biais accéléro selon l'axe x de [m] 4b yll, Erreur de biais accéléro selon l'axe y de [m] Ab, Erreur de biais accéléro selon l'axe z de [m] Erreur de facteur d'échelle accéléro selon l'axe x de [m] AK yll, Erreur de facteur d'échelle accéléro selon l'axe y de [m] Erreur de facteur d'échelle accéléro selon l'axe z de [m] Ad, Erreur de dérive gyro selon l'axe x de [m] Ad yll, Erreur de dérive gyro selon l'axe y de [m] Ad, Erreur de dérive gyro selon l'axe z de [m] Les composantes d'états intervenant dans les équations sont présentées dans un ordre arbitraire qui est le suivant : N = Ax Ax Erreurs de position adns le repère plateforme [v] Autres Ay Ay composantes nulles Az Az Oy Oz Ab ym Ab AK xm AKym AK zm Adxm Adym Ad Aa : O., Erreurs d'attitude dans le repère plateforme [v] Oy Ab.'' Erreurs de biais accéléro ym dans le repère de mesure Ab [m] _ zm _ AKxm AK ym AK zm _ Erreurs de facteur d'échelle accéléro dans le repère de mesure [m] Adxm Erreurs de dérive gyro Ad ym dans le repère de mesure Ad zm [m] 4a Erreur d'angle de mécanisation en azimuth libre Les mesures acquises sont des vitesses. Le porteur est supposé parfaitement immobile en translation et rotation. Les défauts capteurs sont 5 supposés constants dans le temps. Accéléromètres gyromètres Abxm 0 d AKxm 0 Adxm 0 dt Ym dt Ym dt Ym Abm 0 _ AKm 0 Adm 0 _ z _ _ z _ _ _ _ z _ _ _ Ces équations forment un système stationnaire de la forme : Ax AV x d 67't AY = AV Y Az AV _ _ _ z _ La méthode de recherche des vecteurs non observables : Les axes non observables sont les suivants : Axe 1 : v1 Axe 2: v2 Axe 3: V3 Axe 4: y4 d dt + - _ _ - AK xm 0 0 - o AV), Ab,m, AVy = Tvm - Abym ± Tvm 0 - AK 0 Tmv o ym AVz - Ab zm _ 0 0 AK zm _ _g_ 0 nI21 - M31 AV,, 0 0 0 Ax - .71 nI21 0 - M32 AV y + 0 0 0 Ay + q2 Aa nI31 n132 0 AV P31 P32 P33 _ _AZ _ q3 - - 0 - g 0 g 0 0 0 0 0 al a2 a3 Ay b1 b2 b _ 3 _ AVy + S2i + 1 2 1 Rx 0 1 Ry 1 2 0 AV x + ^ =Tvm Adxm Ad ym Ad + A ( Cl C2 C _ 3 _ ) ^ d dt Aa 1 1 0 1 1 1 I II II I 0 ,-- 0 0 0 i i I 1 1 1 1 1 1 0 1 I I I 0 0 H 0 0 H ,- H H 0 0l 0 0 0 0 0 0 0 0 I i I I I I I I I I i E--, 1 1 1 1 1 I I 1 1 to c) c) , , I 0 0 I I 0 0 I I 0 I I 0 0 0 ,-- I H H I I i E---i E--i 1 1 1 0 1 1 1 I 0 0 I I 0 0 H 0 0 H 0 I I 0 0 0 0 ,- 0 , I , 0 i Il i I I I I 1 E---i E---i 1, 1 1 1 1 2 Axe 5 : v5 Axe 6 : V6 Axe 7 : V7 30C 1 0 0 Ay 0 1 0 Az 0 0 1 A Vx 0 0 0 A Vy 0 0 0 A Vz 0 0 0 Oy 0 0 0 Oz 0 0 0 Ab. 0 0 0 411 ym Tm, 0 Tm, 0 Tm, 0 Ab zm _ P31 P32 P33 AK' 0 0 0 AKym 0 0 0 AKzm 0 0 0 Ad. t 1 - S2,111 0 Ad, Tm, -S2,a2 Tm, -S2,112 0 Ad z _ -S2ta3 - S2tb3 0 _ m Aa 0 0 0 Axe 8: 128 Axe 9: y9 Axe 10: 1110 4x 0 0 0 Aty 0 0 0 Az 0 0 0 M;, 0 0 0 4 Vz 0 0 0 Oy 0 0 0 Oz 0 0 0 - gTm1,3 0 0 4 bym 0 - gTm2,3 0 Ab zm _ 0 0 0 AK xm 1 0 0 AK ym 0 1 0 AK zm 0 0 0 Ad xm 0 0 RT1 Ad ym 0 0 Tmv - glyiti Ad z _ 0 0 0 _ m Aa 0 0 1 ANNEXE 3 : mise en contrainte du filtre Des écarts matriciels sont appliqués aux matrices de transition et 5 d'observation à chaque cycle de Kalman d'après un modèle de q axes non observables 12.2,»H,k), ---^'; (5(H,k) obtenus à chaque cycle k selon des fonctions de l'état global " correspondant au point de linéarisation de la matrice d'observation à la période de Kalman. est remplacé par : 10 Ok* ->k+1(5 0,k XHk, H,k+1)- Ok->k+1(5(0,k)± 60k->k+1 Î H H,k+1) proche de et tel que pour i compris entre 1 et q : Equation 1 :é 1,k +1( k H ,k +1) - 1: ->k+l(b ,k H ,k H ,k +1)1 '/' 1,k( k H ,k), et b k->k+1K ,k Î H ,k+11 minimum vis-à-vis de toutes les solutions possibles. Hk (kHk) est remplacé par H k*(-71^(. H ,k) - H k()^(' H ,k)± 15H k()^(' H ,k) proche de , Hk(kHk), et tel que pour i compris entre 1 et q : , 5 Equation 2 : H 0, et per/A minimum vis-à-vis de toutes 'k(H ,k» kH ,k) - les solutions possibles. La norme des matrices considérée est la norme de Froboenius. Les matrices é î 0,k 5 I 5 - ,k+1) et Hk*(ÎH,k) sont alors utilisées dans l'étape de propagation de la matrice de covariance, et du calcul du gain de 10 Kalman. On adopte désormais les notations suivantes : 0k*->k+1(ÎO,k,5(1-1,k,5(1-1,k+1) est noté é k* ->k+1 60k->k+1(5 - ,k -71^(' H ,k+1) est noté 6é kk+1 Hk*(ÎH,k) est noté H k* 15 311k (5CH,k est noté 61/1 Résolution des équations 1 et 2 dans le cas où les vecteurs non observables modélisés 12 2,k --- q,k sont orthogonaux entre eux à chaque instant k : L'écart matriciel total de la matrice de transition correspond à la somme 20 de q écarts matriciels indépendants. Il en est de même pour l'écart matriciel de la matrice d'observation. 60 k-1k =160/ ,k-1->k i =1 ^51 k =1^51-1i k i =1 Et, pour i compris entre 1 et q : 601,k-1->k = (1 2 1,k I k -1 - k-1->ki 2 1,k-1) '2 1T. ,k -1 V ,k -1v ,k -1 T 6H1,k = T 1 fr ki i,k)ij 1,k V 1,kv 1,k 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 mo et m' 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 080k0à et Dalla à 011/1. Le but est de disposer d'une fonction croissante de 118°k et 110k-1->k 11 060- 06/1 1131-411 ou des constituants élémentaires I 'k 1->k et , ces normes pouvant 0/40 00k-1,k0 01-40 ê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 : z- . 2 m = 08gbk-1->kll Mo - ' 060k-1->k 11 \ N 06 °i,k -1->kll çb 00k -1->kll OU Il° k -1-> kll ) OUM° - 11°1' -1->kll ni, =0811k0 N 0611 I ,k11 illH -11311 kr2 - m - 011k11 OU 1111 11 ) OU il 01111 Des variantes sont possibles en faisant appel à des fonctions de 10 pondération go et g , 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. lIgg54k-1->k )11 - °gril (6/4)0 Par exemple : m et lu o = n Dg 0(46k-i->k)l H Og r II (II k )11 Une réalisation possible de ces fonctions de pondération consiste à 15 construire d'abord une fonction de pondération f d'un vecteur d'état, puis d'en déduire go et g , . Ainsi par cette méthode, au vecteur u = u7 UL f est associé par une fonction de pondération f un vecteur , tl' = f (t I ) = [al a2 ... aL]il =Plu, a2u2 au L1T ou les coefficients réels al 20 , a2 ,... a L forment les coefficients de pondération. Ces derniers permettent alors de pondérer la matrice de transition de la façon suivante : ( go 41 42 4L 121 122 _t Ll t L2 t LL _ aa a t t 11 t 12 IL a a2 a a2 t a2 a2 21 t 22 t2L a1 a2 a a a a L t L t L t Li L2 LL a a _ 1 a2 Car si v = Ou alors j(v)= g 0(5)f (u) 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. En effet, en posant t11 t12 t1L U = [Ui uL et t2I t22 t2L t LL _ _tL1 t L 2 V = [121 v2 vL a a a 1 t 1 t 1 t 12 IL a1 a2 a a2 t a2 t a2 t 21 22 2L a1 a2 a a a a L t L L t Li 72 L2 LL a1 a L _ On a g 04)f (U)= a1u1 a1t11 a1t12 a1t1L U1 a2 il 2 a2 t21 a2 t22 a2 t2L 112 aLttL _ aLtLl aLtL 2 aLtLL_ uL _ C'est-à-dire g 04)f (U) = [al Donc g 0(t5)f (0= [ai a2 - - - 10 Donc f (V) = g (0)f (u) a2 ... a L] t11 t21 t12 4L 1 tLl t22 t2L 2 tL 2 tLL _ _ = [a1 a2 . . . a L'OU ( t 1L 1 11 717t11 717tu -ti-tu 1 L 1 1 1 -til t22 al a22 a L t11 t12 t21 t22 t2L De façon similaire g H 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. . r(t) f(X(t),u(t)) Système à temps continu : ' , z(t)= h(X(t)) Système à temps discret : Ixk+i- f(xk,uk) z k = h(x k) Où X(t) et xk désignent l'état du système, u(t) et uk désignent la commande d'entrée, z(t) et zk 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 oeuvre 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(0). Par la suite, la fonction de propagation ainsi que la commande d'entrée font passer le système dans des états X(t), 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 X(0) d'une petite valeur 3X(0) 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 X(t)+3X(t). 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 3X(0). 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 3X(0) 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 trajectoire X(t) : leo= ±a (x(t),u(t))3x(t)=F(x(t),u(t))3x(t) ax 3z(t),( -dli 3X(t)=11(20))3X(t) variables dans le temps correspondant à la matrice d'évolution et la matrice 5 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 cez(t) que = 0 pour k=1,2,3,... et t > o dtk Les solutions forment un espace vectoriel d'états linéarisés. Il existe alors une base B(0) de cet espace formant des vecteurs non observables 10 B(0) = (3X1 (0), 3X2 (0), ..., (5Xq (0)) à 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 15 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 X1 X2 ... Connaissant ces états successifs, supposons que l'état initial s'écarte maintenant de X0 d'une petite valeur 6X0 appelé état linéarisé et que les commandes d'entrée soient les mêmes que dans la situation précédente. Les 20 états successifs suivants sont alors X1 + 6X1 X2 ± 6X2 ... 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 6X0. L'espace des états linéarisés à l'instant 0 est alors composé d'un sous-espace observable et un sous-espace non 25 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. où F et H sont des matrices L'espace non observable à l'instant 0 caractérisé par l'état X0 peut alors se déduire en considérant le système dynamique des états linéarisés 6X,, mettant en oeuvre la fonction d'évolution et la fonction d'observation linéa risées aux différents états X0, X1, X, ... La fonction d'évolution linéarisée à l'instant k est appelée « matrice de transition » notée do , k_>k+I(Xk ) - La fonction d'observation linéarisée à l'instant k est notée H k() k) . Une première observation de l'écart 6X0 autour de X0 est faite à l'aide de la matrice d'observation H0(X0) correspondant à la fonction d'observation linéarisée en X. Une prédiction de l'écart 6X1 autour de X1 est faite à partir de l'écart 6X0 autour de X0 à l'aide de la matrice de transition q501(X0) correspondant à la fonction d'évolution linéarisée en X. Une deuxième observation de l'écart 6X1 autour de X1 est faite à l'aide de la matrice d'observation HI(X1), etc... On sait alors que l'espace non observable du système décrivant la perturbation est le noyau de la matrice : H, (x, ) 111(X 1» 0->l(X 0) H 2(X 2» 1-> 2 (x, 0 (X0) L'espace non observable et l'espace observable dépendent donc de l'état de référence initial X0, 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 vo(X0) fasse partie du noyau deM(Xj. Posons : 11,1 (X1) = 00_>1 (X0 )V0 (X0 ) V2 (X2 ) = 01_>2 (XI )V1 (X1 ) , On a alors : V3 (X3 ) 02->3 (X2 )V2 (X2 ) - - - A un instant donné, une condition d'observabilité est alors formée pour une direction non observable par le système d'équations: J'k+1 - k->k+1(X k)Vk(X k) H k+1(X k+1)Vic+1(X k+1) Où vk(Xk) forme un vecteur non observable particulier à un instant discret k. Cette condition s'applique à chaque instant et est liée à la trajectoire X0 , X1, X2 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 à 10 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 X, X1, X2 ... est remplacée par une suite 517. H,0 512-. 0,1 517.H,1, 5(.0,2, 517.H,2, 5(.0,3 ,,--- Iy 1(X - H j) - 0 0->l(X 0 ,0 )V0 (XH,0 ) (X H,) 01->2 (X0 ,1 )V1 (XH ,1 ) V3 (X-H,3 ) 02->3 (X-0,2 )V2 (XH,2 ) Et : Avec : IH0 (x',0)V0(X H,0)= 0 H1(Xfu)V1(XHJ)=0 H 2(X H ,2)V2(X H ,2) = 0 15 La condition d'observabilité s'écrit alors : IvkA(H,k+1) Ok->k+lk,,k)vk(H,k) H k+1K ,k+1)Vk+1(H,k+1) o ANNEXE 8: démonstration de la formule d'ajustement de matrices pour une base orthogonale de vecteurs 20 Hypothèse : p matrices colonne Ui telles que : VO, Otq i # j, UirUi = 0 (1) Soit p matrices colonne Vi de même dimension que les Ui. Soit la matrice A. Problème : On recherche la matrice SA de norme minimum telle que : + 6A)(Ui U2 ... Uq) = (Vi V2 ... La matrice A + 6A peut se mettre sous forme de p matrices ligne en colonne, et les vecteur VI, sous forme de q scalaires en colonne : + \ (U1 U2 ... Uq) = /v11 V21 v12 Viq \ L2 + 6L2 \V v22 V2q \L P P Vcl2 v I qq On recherche donc les ak de norme minimum tels que : V(k,1) E {1,2, C1}2, (Lk ak)Ui = Vid (2) 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 : VO, Otq i # ,Ïif ± ri; La condition (2) se traduit par le produit scalaire suivant : V(k, E {1,2, ..., q}2, + 64). 11f1 = vkl La projection du vecteur 4 sur l'axe orienté défini par le vecteur unitaire a pour abscisse Lk.imil sur cet axe. C'est le cosinus directeur de k selon cet axe. On recherche 64 tel que la projection de 4 + 64 sur cet axe soit (f,k+ek)rli Vkl 64 peut être décomposé selon les vecteurs Ui qui génèrent un espace vectoriel de dimension q, et selon une composante résiduelle É>k orthogonale à cet espace : 6i>4( = 161k1 (f,k+ek1).11 Vkl Et donc - = IuiI PIF La figure 7 montre qu'il existe une infinité de solutions r(k1. La solution minimale correspond à un vecteur 641 colinéaire à rh. D'après la figure, on a : 641 = 1111111 1111111 1111111 1111111 vid 1hi Ék--fil 111 Vki Lk- U1 U1 6f-ki = ( -> , 'II' II ->Il Il u->ill) Il=uill 6t>kl - 1 2 (Vkl - t>k. u* 1)u* Ilifill On a donc É>k = Ô, et : ci f ,ST-'> 1 k - , 2 Vkl - t>k- iii)11>i 1-1 IlUill De retour en notation matricielle, on a donc : cl 1 ak = (Vkl - LkUi)Uir UTU 1 1 1=1 En reformant les matrices A et SA, après quelques manipulations, on obtient la relation : ci 1 SA = uTu (V1- AU1)Uir 1-15 REVENDICATIONS1. 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 5 les étapes de : acquisition de mesures d'au moins une des variables, filtrage (400) 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é 10 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 (310, 330) de la matrice de transition et/ou de la matrice d'observation avant leur utilisation dans le filtrage de Kalman étendu, de sorte que 15 chaque matrice ajustée vérifie 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 de l'espace d'état. 20 2. Procédé selon la revendication 1, dans lequel l'étape de filtrage de Kalman étendu comprend 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, 25 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. 3. Procédé d'estimation selon l'une des revendications 1 et 2, dans lequel 30 la condition d'observabilité à vérifier par les matrices de transition etd'observation ajustées est la nullité du noyau d'une matrice d'observabilité qui leur est associée. 4. Procédé d'estimation selon l'une des revendications 1 à 3, dans lequel 5 l'ajustement comprend les étapes de : calcul (301) d'au moins une base primaire de vecteurs non-observables à partir de l'état estimé précédent, pour chaque matrice à ajuster, calcul (306, 308) d'au moins un écart matriciel associé à la matrice à partir de la base primaire de 10 vecteurs, décalage (330) de chaque matrice à ajuster selon l'écart matriciel qui lui est associé de sorte à vérifier la condition d'observabilité. 5. Procédé d'estimation selon la revendication 4, comprenant en 15 outre une étape d'orthogonalisation (302) de la base primaire en une base secondaire de vecteurs, l'écart matriciel associé à la matrice d'observation étant calculé à partir de la base secondaire de vecteurs. 6. Procédé d'estimation selon la revendication 5, dans lequel l'écart 20 matriciel associé à la matrice d'observation est la somme de plusieurs écarts matriciels élémentaires, chaque écart matriciel élémentaire étant calculé à partir d'un vecteur de la base secondaire qui lui est propre. 7. Procédé d'estimation selon l'une des revendications 4 à 6 dont les 25 étapes sont répétées dans des cycles successifs, et dans lequel un cycle donné, dit cycle courant, comprend les étapes de: mémorisation (303) de la base secondaire de vecteurs et de coefficients d'orthogonalisation qui sont également produits par l'étape d'orthogonalisation, et- transformation (304) 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, le calcul (306) de l'écart matriciel associé à la matrice de transition étant 5 mis en oeuvre à 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. 8. Procédé d'estimation selon la revendication 7, dans lequel l'écart matriciel associé à la matrice de transition est la somme de plusieurs 10 écarts matriciels élémentaires, chaque écart matriciel élémentaire étant calculé à partir d'un vecteur de la base secondaire mémorisée au cours du cycle précédent et propre à l'écart matriciel élémentaire, et d'un vecteur de la base tertiaire calculée au cours du cycle courant et propre à l'écart matriciel élémentaire. 15 9. Procédé selon l'une des revendications 1 à 8, dans lequel, pour au moins une matrice à ajuster, on calcule : une pluralité d'écarts matriciels candidats, chaque écart matriciel candidat étant calculé à partir d'une base primaire de vecteurs 20 non-observables respective et d'une équation représentative d'un type d'observation respectif, et éventuellement d'une trajectoire respective du vecteur d'état, et une pluralité de métriques, chaque métrique étant représentative d'une amplitude d'ajustement induite par un écart matriciel 25 candidat respectif, l'étape de décalage (330) de la matrice à ajuster étant mise en oeuvre selon un écart matriciel élu (320) parmi les écarts matriciels candidats en fonction des métriques calculées. 10. Procédé selon la revendication 9, dans lequel l'écart matriciel élu est l'écart matriciel candidat associée à la métrique représentative de l'amplitude d'ajustement la plus faible. 11. Procédé selon l'une des revendications 9 ou 10, dans lequel le décalage n'est mis en oeuvre que si la métrique de l'écart matriciel élu est inférieure à un seuil prédéterminé. 12. Centrale inertielle (IN) comprenant une pluralité de capteurs (CI, CC) 10 et des moyens (E) pour estimer un état de navigation de la centrale inertielle par mise en oeuvre du procédé d'estimation selon l'une des revendications 1 à 11. 13. Produit programme d'ordinateur comprenant des instructions de code 15 de programme pour l'exécution des étapes du procédé selon l'une des revendications 1 à 11, lorsque ce programme est exécuté par des moyens (E) de traitement de données.