FR3023623A1 - Procede d'estimation de l'etat d'un systeme mobile et centrale inertielle correspondante. - Google Patents

Abstract

Il est proposé un procédé d'estimation de l'état d'un système mobile, le procédé mettant en œuvre un filtre particulaire et comprenant les étapes suivantes mises en œuvre à partir d'une particule de référence parmi plusieurs particules précédemment calculées, la particule de référence offrant une estimation de l'état et comprenant un premier jeu de variables et un deuxième jeu de variables: - mutation aléatoire (101), par le filtre particulaire, du premier jeu de variable en un premier jeu de variable muté, - paramétrage (102) d'un filtre de Kalman étendu à l'aide du premier jeu de variable muté, - mise en œuvre (103) du filtre de Kalman étendu paramétré pour produire une nouvelle particule à partir du deuxième jeu de variable et de données mesurées par au moins un capteur, le procédé étant caractérisé en ce que le filtre de Kalman étendu est du type invariant.

Description

Filtre particulaire invariant DOMAINE GENERAL L'invention se rapporte au domaine des procédés estimant l'état 5 d'un système dynamique à partir de mesures incomplètes ou bruitées. L'invention concerne plus particulièrement un procédé mettant en oeuvre un filtre particulaire de type particulier. ETAT DE L'ART 10 Le filtre de Kalman est un outil bien connu pour estimer l'état d'un système dynamique régi par des équations matricielles, donc linéaires, au moyen de mesures bruitées. Pour étendre le filtre de Kalman à des systèmes dynamiques régis par des équations non-linéaires, il a été proposé un procédé désigné sous 15 l'expression de - filtre de Kalman étendu » (EKF). Cette évolution propose une étape supplémentaire consistant à linéariser à chaque nouvelle itération du filtre les équations régissant le système non-linéaire en un point de l'espace vectoriel qui est un état estimé au cours d'une itération précédente. Les matrices issues de cette linéarisation peuvent être ainsi 20 utilisées pour calculer un nouvel état estimé selon ta méthode du filtre de Kalman classique. Toutefois, te filtre de Kalman étendu s'avère peu efficace lorsque le bruit des mesures est non gaussien. Dans te cadre particulier de mesures effectuées par un récepteur GNSS, on constate que ces mesures 25 présentent un biais sujet à des - sauts » brusques imprévisibles et susceptibles de faire diverger irrémédiablement l'estimation de l'état. Le filtrage particulaire, également connu sous le nom de méthode de Monte-Carlo séquentielle, est un autre procédé pour estimer un système dynamique non-linéaire, lequel supporte des mesures bruitées 30 selon un bruit non gaussien.

Le principe général d'un filtre particulaire consiste à explorer l'espace de l'état considéré à estimer à l'aide d'une pluralité d'échantillons conventionnellement appelés « particules », chaque particule offrant une estimation plus ou moins vraisemblable de l'état.

Selon une variante de base bien dite « bootstrap », les étapes suivantes sont mises en oeuvre par le filtre particulaire : une « mutation », au cours de laquelle chaque particule est propagée aléatoirement dans l'espace d'état, une - pondération », au cours de laquelle on calcule pour chaque particule propagée un poids associé à partir d'un nouveau jeu de mesures, le poids indiquant le degré de vraisemblance de ta particule par rapport à l'état réel du système compte tenu de ces mesures, une - sélection », au cours de laquelle des particules assorties d'un poids faible sont éliminées tandis que tes particules assorties d'un poids élevé, plus vraisemblables, sont conservées. D'itération en itération, les particules explorent l'espace d'état de façon indépendante tes unes les autres. Le filtre particulaire est donc d'autant plus coûteux en ressources 20 de calcul qu'il existe un nombre élevé de particules à maintenir. De plus, la variante - bootstrap » est sujette à un problème de dégénérescence des poids : une seule particule finit par présenter un poids très élevé, tandis que tes autres particules présentent un poids proche de zéro, ce qui rend l'estimation peu efficace en plus d'être 25 coûteuse. Une variante améliorée de filtre particulaire a ainsi été récemment proposée pour pallier cet inconvénient : le filtre particulaire de RaoBlackwell (« Rao-Blackwellized particle filter », ou RBPF en anglais). Le filtre RBF propose de décomposer un état de dimension K en un 30 premier jeu de variables et un deuxième jeu de variables, ces deux jeux subissant des traitements différents au cours d'une itération donnée.

L'étape de mutation du filtrage particulaire est alors mise en oeuvre de façon sélective sur le premier jeu de variable, ce qui permet de réduire le nombre de particules à maintenir au fit des itérations par rapport à ta variante « bootstrap » de base.

Par ailleurs, te deuxième jeu de variables est considéré comme un état de dimension inférieur à K que l'on estime au moyen d'un filtre de Kalman étendu. Or, tes calculs mis en oeuvre par le filtre de Kalman étendu comprennent des inversions de matrices particulièrement coûteuses en charge de calcul. Cette augmentation de charge de calcul est de plus multipliée par le nombre de particules à traiter par te filtre de RaoBlackwell. PRESENTATION DE L'INVENTION L'invention vise à proposer un estimateur de l'état d'un système dynamique non linéaire et sujet à des bruits de mesures non-gaussiens dont ta charge de calcul est réduite par rapport à un filtre particulaire de Rao-Blackwelt conventionnel. Dans ce but, il est proposé selon un premier aspect un procédé d'estimation de l'état d'un système mobile, le procédé mettant en oeuvre un filtre particulaire et comprenant les étapes suivantes mises en oeuvre à partir d'une particule de référence parmi plusieurs particules précédemment calculées, la particule de référence offrant une estimation de l'état et comprenant un premier jeu de variables et un deuxième jeu de variables: mutation aléatoire, par le filtre particulaire, du premier jeu de variable en un premier jeu de variable muté, paramétrage d'un filtre de Kalman étendu à l'aide du premier jeu de variable muté, - mise en oeuvre du filtre de Kalman étendu paramétré pour produire une nouvelle particule à partir du deuxième jeu de variable et de données mesurées par au moins un capteur, le procédé étant caractérisé en ce que le filtre de Kalman étendu est du 5 type invariant. Une propriété intéressante du filtre de Kalman étendu invariant est que les particules ont toutes une même covariance. En conséquence, l'utilisation d'un filtre de Kalman invariant dans le contexte particulier d'un filtrage particulaire faisant évoluer k particules 10 permet de s'affranchir du calcul de k covariances, et donc de réduire considérablement la charge de calcul nécessaire à l'estimation de l'état, par rapport à un filtre particulaire de Rao-Blackwell classique. L'invention peut également être complétée par les caractéristiques suivantes, prises seules ou en une quelconque de leurs combinaisons 15 techniquement possibles. Le deuxième jeu de variables peut comprendre une covariance représentative d'une incertitude au sens de Kalman des autres variables du deuxième jeu. Les étapes du procédé selon le premier aspect peuvent être 20 répétées pour chacune des particules, le filtre de Kalman utiliser une équation de Ricatty commune pour toutes les particules. L'étape de mutation tient compte de mesures de l'état pour produire le premier jeu de variables muté. Les étapes du procédé selon te premier aspect étant répétées pour 25 chacune des particules, l'étape de mutation peut comprendre une décomposition de Cholesky commune pour toutes les particules. Le filtre de Kalman étendu invariant peut être mis en oeuvre pour une pluralité de particules, ladite mise en oeuvre comprenre une propagation du deuxième jeu de variables par intégration d'une équation 30 dynamique, l'intégration comprenant, pour une particule de référence : La détermination d'une solution générale présentant une forme à au moins une inconnue, le choix de conditions initiales pour déterminer une solution complète propre à la particule de référence, la détermination de la solution générale étant mise en oeuvre une seule fois pour toutes tes particules. Le procédé d'estimation selon te premier aspect trouve avantageusement application dans le domaine de ta navigation satellite. Le premier jeu peut alors contenir des variables représentatives 10 d'un biais dans des mesures de radionavigation satellite. Le deuxième jeu peut en outre contenir des variables de navigation d'un porteur mobile. Il est proposé, selon un deuxième aspect, une unité de traitement 15 de données mesurées adaptée pour mettre en oeuvre le procédé selon te premier aspect. Il est en outre proposé selon un troisième aspect, un produit programme d'ordinateur comprenant des instructions de code de programme pour l'exécution des étapes du procédé selon te premier 20 aspect, lorsque ce programme est exécuté par cette unité de traitement de données. Il est également proposé, selon un quatrième aspect, un procédé d'estimation de l'état d'un système mobile mis en oeuvre par une centrale 25 de navigation comprenant au moins un récepteur de signaux de radionavigation satellite, au moins un capteur inertiel, te procédé comprenant les étapes suivantes : - acquisition de mesures par les capteurs, mise en oeuvre des étapes du procédé selon te premier aspect pour 30 produire un ensemble de particules à partir des données mesurées, estimation de l'état du système mobile à partir de l'ensemble de particules. Selon un quatrième aspect, il est en outre proposé une centrale de navigation comprenant : au moins un récepteur de signaux de radionavigation satellite, au moins un capteur inertiel, une unité de traitement de données de mesure pour estimer un état de navigation de ta centrale à partir de mesures acquises par le récepteur et par le capteur inertiel.

DESCRIPTION DES FIGURES D'autres caractéristiques, buts et avantages de l'invention ressortiront de ta description qui suit, qui est purement illustrative et non limitative, et qui doit être tue en regard des dessins annexés sur lesquels : - La figure 1 représente schématiquement une centrale inertielle selon un mode de réalisation de l'invention. La figure 2 représente les étapes d'un procédé de filtrage particulaire selon un mode de réalisation de l'invention. La figure 3 détaille une étape particulière de la figure 2 mise en oeuvre par un filtre de Kalman invariant. Sur l'ensemble des figures, tes éléments similaires portent des références identiques. DESCRIPTION DETAILLEE DE L'INVENTION La description qui suit comprend trois parties : une première partie (I) décrivant le principe général d'un filtre particulaire invariant selon un mode de réalisation de l'invention, une deuxième partie (II) décrivant un mode de réalisation particulier appliqué à l'estimation de l'état d'une central de navigation, une troisième partie (III) détaillant des astuces d'implémentation particulières et optionnelles du mode de réalisation selon ta deuxième partie.

I. Principe général de fonctionnement d'un filtre particulaire invariant En référence à la figure 1, on considère dans un premier temps une unité de traitement U de données de mesures. L'unité de traitement U est par exemple un processeur ou un 10 ensemble de processeur(s) adapté(s) pour exécuter un programme d'ordinateur comprenant des instructions de code de programme. L'unité U est configurée pour mettre en oeuvre un estimateur qualifié dans la suite de « filtre particulaire invariant », cet estimateur ayant pour objectif d'estimer le mouvement d'un système mobile. 15 Le filtre particulaire invariant sollicite un filtre particulaire de Rao- Blackwell combiné à un filtre de Kalman d'un type particulier : un filtre de Kalman étendu invariant. A ce stade, on suppose également que l'unité de traitement U comprend une entrée recevant des données mesurées par un ou plusieurs 20 capteurs adaptés pour mesurer des grandeurs physiques liées au système mobile (vitesse, position, accélération, etc.). Ces mesures sont également dénommées « observations - dans la suite. On considère tout d'abord un jeu de variables régies par un système 25 linéaire susceptible d'être traité par un filtre de Kalman défini par des équations matricielles. Ces équations matricielles, connues à l'avance dans te cas d'un filtre de Kalman classique, dépendent ici d'un ou plusieurs paramètres supplémentaires inconnus évoluant aléatoirement au cours du temps. 30 On utiliser donc un filtre particulaire pour produire différents échantillons (ou particules) de ces paramètres supplémentaires. Chaque particule fournit un jeu d'équations linéaires spécifique utilisé par le filtre de Kalman pour élaborer une nouvelle estimation du jeu de variable précédemment mentionné. Le filtre de Kalman fournit également une valeur de vraisemblance 5 de chaque paramètre testé. On sélectionne alors les paramètres supplémentaires d'après leur vraisemblance. Dans le cadre de la présente invention, le filtre de Kalman utilisé est du type étendu invariant. L'utilisation de ce filtre étendu invariant rend possible le 10 traitement d'un jeu de variables régies par un système cette fois non-linéaire, te jeu de variables présentant une propriété d'invariance. Cette propriété d'invariance est notamment exposée dans l'article « Invariant Extended Kalman Fitter: theory and application to a velocity-aided attitude estimation problem », par S. Bonnabel, P. Martin et E. Salaun, 15 publié dans In Decision and Control, 2009 held jointly with the 2009 28th Chinese Control Conference, CDC/CCC 2009. Proceedings of the 48th IEEE Conference on (pp. 1297-1304). IEEE. L'unité U de traitement met en oeuvre le filtre particulaire invariant dans des itérations successives, ou cycles, pour estimer des états 20 successifs du système mobile, l'évolution desdits états étant représentative du mouvement du système mobile. On considère dans la suite que le terme « état » peut désigner un ensemble de variables représentatives de grandeurs physiques liées au système mobile (vitesse, position, accélération, etc. de ce système 25 mobile). Chaque itération prenant en entrée des données calculées par l'unité U dans une itération précédente, ainsi que des mesures externes fournies par des capteurs, chaque itération produisant des nouvelles estimations de l'état désignées sous le terme conventionnel de particules 30 comme on l'a vu en introduction.

L'état est construit à partir du jeu de variables invariant et des paramètres supplémentaires susmentionnés. L'état peut être représenté par un vecteur comprenant un premier jeu de variables (les paramètres supplémentaires) et un deuxième jeu de variables (le jeu de variable 5 invariant). On va maintenant décrire une itération courante du filtre particulaire mis en oeuvre par l'unité U de traitement de données selon un mode de réalisation illustré en figure 2. On suppose que k particules pondérées ont été calculées au cours 10 d'une itération précédente du filtre particulaire invariant. Par définition, chaque particule est un vecteur comprenant : un vecteur contenant le jeu de variables invariantes, un vecteur contenant les paramètres supplémentaires, - la covariance des variables invariantes. 15 Chaque particule fait l'objet de plusieurs traitements au cours de L'itération courante. On considère, parmi les k particules, une particule donnée, dite particule de référence, pour laquelle on décrit ces traitements. 20 a) Mutation Dans une étape dite de - mutation » (étape 101), les paramètres supplémentaires de ta particule de référence sont propagés aléatoirement dans leur espace, à la manière d'un filtre particulaire classique. Le résultat obtenu est un jeu de paramètres supplémentaire modifié, dit dans 25 la suite - muté ». Selon une variante particulière, lorsqu'une observation est disponible (étape d'observation 200), l'étape de mutation 101 peut tenir compte de cette observation en plus du modèle de propagation aléatoire utilisé. Cette correction permet ainsi de rapprocher tes particules des 30 paramètres supplémentaires vrais.

On notera que cette correction est avantageuse dans le cas où l'observation est sujette à des sauts brusques (phénomène rencontré dans le cas où l'observation est issue d'un récepteur GNSS/GPS, comme on le verra dans une application particulière du procédé selon l'invention dans la partie II). On pourra par exemple utiliser pour mettre en oeuvre cette correction la technique dénommée - optimal sampling ». D'autres heuristiques sont néanmoins utilisables. b) Propagation I Mise à jour Les équations du filtre de Kalman invariant comprennent une équation dynamique, représentant le comportement a priori du système, et une équation d'observation, représentant le comportement des capteurs utilisés pour faire obtenir des observations, également appelées mesures. Les équations du filtre de Kalman sont paramétrées en fonction des des valeurs du jeu de paramètres supplémentaires mutés obtenues à L'issue de l'étape de mutation 101. Dans le cas particulier où l'équation dynamique ne dépend pas des 20 paramètres supplémentaires, on peut faire l'économie de l'étape de mutation lorsqu'il n'y a pas d'observation disponible. Le jeu de variables invariantes de la particule de référence est ensuite estimé au moyen du filtre de Kalman étendu invariant. En référence à ta figure 3, le vecteur du jeu de variables 25 invariantes et la covariance des variables invariantes sont propagées à la manière d'un filtre de Kalman invariant, en utilisant l'équation dynamique (étape dite de propagation 301). La covariance des variables invariantes fait également l'objet d'une propagation au moyen de l'équation de Riccati associée au filtre de 30 Kalman invariant (étape 303), construite en utilisant l'équation dynamique.

Si une nouvelle observation est rendue disponible par les capteurs utilisés (étape d'observation 200), le vecteur du jeu de variables invariantes est mis à jour à la manière d'un filtre de Kalman invariant, en utilisant l'équation d'observation de ce filtre paramétré à l'aide des paramètres supplémentaires. De même, la covariance obtenue à l'issue de des étapes de propagation 303 est mise à jour à l'aide du filtre de Kalman invariant. La mise à jour 302 par le filtre de Kalman étendu invariant fournit également une vraisemblance pour la nouvelle particule.

De retour à la figure 2, dans une étape de pondération 104, te poids de la particule est multiplié par l'exponentielle de la vraisemblance. On obtient en sortie du filtre de Kalman invariant une nouvelle particule pondérée, élaborée à partir de ta particule de référence. On notera que l'étape de pondération, qui est ordinairement mise 15 en oeuvre dans un filtre particulaire de type - bootstrap », est ici mise en oeuvre à partir des données produites par le filtre de Kalman étendu invariant. Les étapes ci-dessus sont répétées au cours de l'itération courante pour les autres k-1 particules, produisant ainsi k-1 autres nouvelles 20 particules assorties chacune d'un poids correspondant. Les poids peuvent être normalisés de façon à ce que leur somme valle 1. L'itération courante produit donc k nouvelles particules pondérées. 25 c) Sélection L'itération courante peut également comprendre une étape de sélection 105, au cours de laquelle des particules assorties d'un poids faible sont éliminées tandis que des particules assorties d'un poids élevé, plus vraisemblables, sont dupliquées. Cette sélection peut être 30 déclenchée sur un critère de dispersion des poids, afin de minimiser le problème de dégénérescence des poids mentionnés.

La sélection 105 produit k nouvelles particules pondérées qui sont ensuite utilisées comme données d'entrée d'une itération suivante du filtre particulaire invariant. Une propriété intéressante du filtre de Kalman invariant est que ta 5 covariance est souvent commune pour tes k particules. En conséquence, l'utilisation d'un filtre de Kalman invariant permet de s'affranchir du calcul de k covariances, et donc de réduire considérablement la charge de calcul nécessaire à la mise en oeuvre tes étapes de propagation et de mise à jour, qui comprennent des inversions de matrices particulièrement 10 coûteuses. L'étape 102 n'est donc mise en oeuvre qu'une seule fois dans l'itération courante en ce qui concerne la propagation de la covariance commune. En conséquence, le procédé proposé est donc particulièrement avantageux lorsque l'unité U de traitement est limitée en charge de calcul 15 (c'est typiquement le cas pour des équipements embarqués sur des aéronefs). d) Estimation Au terme d'une itération, une estimation de l'état (ou d'une 20 fonction de l'état) peut être calculée en effectuant une moyenne pondérée des k nouvelles particules. Il n'est pas nécessaire de procéder à une telle estimation à ta fin de chaque itération, mais seulement à l'issue de certaines d'entre elles. L'estimation de l'état obtenue est ta donnée de sortie de 25 l'estimateur. Il. Application du filtre particulaire invariant à une centrale de navigation 30 De retour à la figure 1, une centrale de navigation IN comprend au moins un récepteur R de signaux de radionavigation satellite, au moins un capteur inertiel CI, et une unité U de de traitement de données adaptée pour mettre en oeuvre le filtre particulaire invariant défini en partie I. Dans cette application, l'unité de traitement U est plus particulièrement configurée pour mettre en oeuvre une estimation d'un 5 état de navigation de la centrale, à partir de mesures acquises par le récepteur et le capteur inertiel, et au moyen du filtre particulaire invariant ci-dessus décrit. Les capteurs inertiels comprennent dans un mode de réalisation des trois gyromètres (un par axe) et trois accéléromètres (un par axe). 10 Le récepteur R est du type GNSS/GPS. Variables Les variables suivantes sont utilisées : - Instant t considéré 15 orientation du porteur Tt (matrice de rotation) vitesse du porteur vt (vecteur de dimension 3) position du porteur xt (vecteur dimension 3) biais GPS bt (vecteur de dimension 3). 20 Entrées du système Les entrées du système sont les suivantes : vitesse angulaire &Jt (vecteur de dimension 3, données par le gyromètre) force spécifique ft (vecteur de dimension 3, donné par les 25 accéléromètres Observations Les observations (au sens d'un filtre de Kalman sont : une mesure de position Yn fournie aux instants tn par le récepteur R (vecteur de 30 dimension 3) Equations mises en oeuvre Les équations suivies par ces variables sont : d -Tt = Tt(wt + wco)x dt d -dtvt = 9 +TtUt + wf) d -xt = vt dt 17.' = xtn + btn + Où wc' est le bruit (de variance Q) associé à la mesure du gyroscope, wf est te bruit (de variance (21) associé à la mesure de l'accéléromètre, V?, est la part non biaisée du bruit associé à la mesure du récepteur GPS et g le vecteur de gravité à l'endroit donné par le GPS. La variance de 1in est notée r2. bt est constant, sauf à des instants aléatoires où il subit un saut et prend une valeur aléatoire gaussienne, de moyenne 0 et de variance ai,. La durée entre deux sauts est indépendante des autres variables et suit une loi exponentielle de paramètre À.. Le notations suivantes seront de plus utilisées: H = (03 , M(y,E,x) = dm(E) exp(- (y - x)TE-1 (y - x)) 1 13 (270 2 1E1112 (densité gaussienne de moyenne x et de covariance E). Pour tout vecteur u E 11Z on notera (u)', la matrice 3 x 3 antisymétrique telle que pour tout autre vecteur x E i on ait (u)xx = u x x, le symbole x désignant le produit vectoriel. Le jeu de variables invariantes est formé par Tt, v, x.

En effet, si le biais est connu on peut reformuler le problème en introduisant les variables suivantes : X - (Tt 01)<3 °1X3 vt Xt w = (141w) x Wf °3X1 1 0 Oix3 o 0 0 1 01x3 0 0 Le système se réécrit alors sous ta forme : d -dt Xt = 9£0,f(Xt)+ Xtw Yr,= xt (V 0 ) 1 Où gcof Tt "ut xt -4 (Tco t(t)x 01)(3 Oix3 Ttft + .gt vt\ est une fonction (01x3 01x3 1 0 0 0 0 1 0 0 dépendant des incréments cot et ft. Elle prend en argument une matrice 5 x 5, rend une matrice 5 x 5 et vérifie : 9,0,f (XiX2) = X19w,f (X2) + ,geof (Xi )X2 - i5X2. Les paramètres supplémentaires sont : le biais bt. On considère donc ici des particules comprenant (1t, f3t, ijt). On va maintenant détailler à nouveau les étapes de ta figure 2 10 appliquées à ta centrale inertielle. Dans une étape préliminaire d'initialisation, le jeu de variables invariant est initialisé selon une densité a priori, tes poids des k particule sont initialisées à 1/k, tes valeurs des biais sont tirées aléatoirement selon une densité a priori, la covariance commune à toutes les particules est 15 initialisée à une valeur estimée a priori. Q) Mutation Dans l'étape de mutation 101, le biais b est propagé aléatoirement dans son espace, à la manière d'un filtre particutaire classique. 20 Dans le présent système, l'équation dynamique ne dépend pas du biais bt: on peut faire l'économie de l'étape de mutation lorsqu'il n'y a pas d'observation disponible. Si une observation est disponible à un instant tn, c'est-à-dire lorsque le récepteur R produit une nouvelle mesure GPS assortie d'un biais 25 correspondant, l'étape de mutation est mise en oeuvre par les équations suivantes.

On calcule Ili et 112 d'après les définitions suivantes : = (1 - e-À°t)X0',.'-ttnHPHT DT?, +a,13 +r2/3,î) ri2 = e-À°S(Y',ttnHPHTtTn + r2/31-îtn + brn) Où Att = -n+1 tn est l'intervalle séparant l'arrivée des deux dernières observations. s est ta réalisation d'une variable aléatoire réelle suivant une loi uniforme sur [0,1]. n2 Si s < - 1111-n2 te biais b, ne change pas. Sinon il est mis à jour comme suit : btn + Où N1 suit une loi gaussienne de moyenne tnKbDtTn(Yi, - 2t.) et de variance t.Pbttrn, avec Kb = oi(HPHT +4I3 +r213)-1 et Pb = (13 - 10 Kb). En d'autres termes te biais est mis à jour selon l'équation : htn + Dt7,417'(1'n - îtn) + L x Où L vérifie la relation : L x LT = tnPbttTn On notera que dans te système dynamique selon la présente application particulière, l'équation dynamique ne dépend pas du biais, et 15 l'équation d'observation dépend bien du biais b. b) Propagation / Mise à jour Le vecteur du jeu de variables invariantes et la covariance des variables invariantes sont ensuite propagées (étapes de propagation 301, 20 303) au moyen d'un filtre de Kalman invariant, en utilisant l'équation dynamique suivante. d. = pt(wt)x d = 9 + ttft d -dt2t = vt d dtPt = FtPt + PtFT + Q f-(tot)x 03x3 et Q ( Q. 03x3) Qf Avec Ft = -(ft)x 03x3 03x3 03x3 Ces équations reviennent à intégrer une équation différentielle affine à partir des mesures inertielles. Cette équation est la même pour toutes les k particules considérées en entrée de l'itération courante : son calcul peut donc être mis en oeuvre une seule fois pour toutes les particules à traiter au cours de l'itération courante. SI l'observation GPS est disponible, le vecteur du jeu de variables invariantes et la covariance des variables invariantes obtenus à l'issue de l'étape de propagation sont ensuite mis à jour à la manière d'un filtre de 10 Kalman invariant, au moyen des équations suivantes. 19tn 2t7,\ ttn etn 2tn\ 1 0 = 01x31 0 exp (KnIZ (Y - -1.'tn)) 0 / \01x3 0 1/ Avec K = Ptne(r213 + HPt.HT)-1 La covariance P est elle aussi mise à jour, par la formule suivante : P (19- KnII)P L'étape de mise à jour 304 de ta covariance est également 15 effectuée une fois pour toutes les k particules. On obtient une nouvelle particule, élaborée à partir de la particule de référence, par application des étapes ci-dessus. La mise à jour 302 fournit égaiement une vraisemblance pour la nouvelle particule, à partir de laquelle le poids de ta nouvelle particule 20 est calculé. c) Sélection L'étape de sélection est mise en oeuvre selon te principe général (voir partie I). 25 d) Estimation Certaines variables de l'état ne se prêtent pas une moyenne linéaire, comme par exemple une orientation prenant des valeurs entre -180° et 180°. Une moyenne quelconque sur une variété peut donc être employée, par exemple une de celles exposées dans l'article - On 5 averaging rotations » par Gramkow, C. (2001), dans Journal of Mathematical Imaging and Vision, 15(1-2), 7-16. III. Astuces d'implémentation du filtre particulaire invariant dans une centrale de navigation 10 a) optimisation de mutation On a vu précédemment que la mutation 101 pouvait tenir compte d'une nouvelle observation. Dans la mise en oeuvre selon la partie II, la mutation comprend une 15 décomposition de Cholesky pour chaque particule, décomposition assez coûteuse en charge de calcul. La décomposition de Cholesky peut être mise en commun pour les k particules, comme suit : on souhaite obtenir une matrice L vérifiant vérifiant L * LT = ttnPben. Or ta matrice Pb ne dépend pas de la particule 20 considérée. On peut donc calculer une fois pour toutes la décomposition de Cholesky Pb = L0 * LT0 puis poser pour chaque particule L = tnLo. On dispose donc, en sortie de l'étape de mutation 101, de particules plus vraisemblables car tenant compte de ta dernière observation, pour une charge plus réduite. 25 b) mise en commun de calculs de propagation On a vu précédemment que l'étape de propagation 302 du jeu de variables invariant était mise en oeuvre pour chacune des k particules dans L'itération courante, et que cette étape de propagation comprenant 30 l'intégration d'une équation différentielle affine.

De façon connue en elle-même, une telle équation affine est intégrée en deux temps : on détermine d'abord une forme générale de solution avec une inconnue, et on détermine cette inconnue au moyen de conditions initiales, propre à chaque particule. La forme générale de solution, complétée avec ta valeur de l'inconnue, devient une solution particulière pour chaque particule. Dans une implémentation avantageuse du filtre particulaire invariant, il est possible de ne calculer qu'une seule forme générale de ta solution, pour toutes tes particules, étant donné que cette forme générale 10 est commune à celles-ci. Le seul traitement spécifique à chaque particule est réduit à la détermination des conditions initiales. Ce principe peut être illustré par les calculs suivants dans ta présente application particulière: 15 Entre deux observations, dont les temps d'arrivée seront notés tn et t'1 et l'écart At = t tn, tes grandeurs suivantes sont calculées une seule fois pour toutes tes particules : 1 0 0 -dR = R (a)) dss s sx Rn - Rtn+1 Rtn = (0 1 0) , tn+, 0 0 1 Aria = Rs fsds tn tri+, f t Axn =j n itnRsfsds) dt Au moment où arrive l'observation les grandeurs suivantes sont mises à jour pour chaque particule : 1 i'tn +-2-At2g + At + tnAxn etn etn + j'tnvn + Lt * g ttn t'Rn 20 Le procédé général décrit en partie I n'est pas limité à l'application spécifique décrite en partie II. Le filtre particutaire invariant a vocation à être utilisé pour l'estimation de tout état, notamment ceux dont les mesures sont sujettes à des sauts similaires à ceux rencontrés avec des récepteur GPS. Par exemple, te filtre particulaire invariant peut être utilisé pour estimer la position d'un robot utilisant une caméra de profondeur.10

Claims (12)

1. REVENDICATIONS1. Procédé d'estimation d'un état représentatif du mouvement d'un système mobile, le procédé mettant en oeuvre un filtre particulaire et comprenant les étapes suivantes mises en oeuvre à partir d'une particule de référence parmi plusieurs particules précédemment calculées, la particule de référence offrant une estimation de l'état et comprenant un premier jeu de variables et un deuxième jeu de variables: - mutation aléatoire (101), par le filtre particulaire, du premier jeu de variable en un premier jeu de variable muté, - paramétrage (102) d'un filtre de Kalman étendu à l'aide du premier jeu de variable muté, - mise en oeuvre (103) du filtre de Kalman étendu paramétré pour produire une nouvelle particule à partir du deuxième jeu de variable et de données mesurées par au moins un capteur inertiel, le procédé étant caractérisé en ce que le filtre de Kalman étendu est du type invariant.
2. 2. Procédé selon la revendication 1, dans lequel le deuxième jeu de 20 variables comprend une covariance représentative d'une incertitude au sens de Kalman des autres variables du deuxième jeu.
3. 3. Procédé selon l'une des revendications 1 et 2, dont tes étapes sont répétées pour chacune des particules, et dans lequel le filtre de Kalman 25 utilise une équation de Ricatty commune pour toutes les particules.
4. 4. Procédé selon l'une des revendications 1 à 3, dans lequel l'étape de mutation tient compte de mesures de l'état pour produire le premier jeu de variables muté. 30
5. 5. Procédé selon la revendication 4, dont les étapes sont répétées pour chacune des particules, l'étape de mutation comprenant une décomposition de Cholesky commune pour toutes les particules.
6. 6. Procédé selon l'une des revendications 1 à 5, dans lequel le filtre de Kalman étendu invariant est mis en oeuvre pour une pluralité de particules, ladite mise en oeuvre comprend une propagation du deuxième jeu de variables par intégration d'une équation dynamique, l'intégration comprenant, pour une particule de référence : - la détermination d'une solution générale présentant une forme à au moins une inconnue, - le choix de conditions initiales pour déterminer une solution complète propre à la particule de référence, la détermination de ta solution générale étant mise en oeuvre une seule 15 fois pour toutes les particules.
7. 7. Procédé selon l'une des revendications 1 à 6, dans lequel le premier jeu contient des variables représentatives d'un biais dans des mesures de radionavigation satellite. 20
8. 8. Procédé selon l'une des revendications 1 à 7, dans lequel le deuxième jeu contient des variables de navigation d'un porteur mobile.
9. 9. Procédé d'estimation de l'état d'un système mobile mis en oeuvre par 25 une centrale de navigation comprenant au moins un récepteur (R) de signaux de radionavigation satellite, au moins un capteur inertiel (CI), le procédé comprenant les étapes suivantes : - acquisition de mesures (200) par les capteurs (R, CI), - mise en oeuvre des étapes du procédé selon l'une des 30 revendications 1 à 8 pour produire un ensemble de particules à partir des données mesurées, 1 3023623 23 - estimation (106) de l'état du système mobile à partir de l'ensemble de particules.
10. 10. Unité de traitement de données mesurées adaptée pour mettre en 5 oeuvre le procédé selon l'une des revendications 1 à 8.
11. 11. Centrale de navigation (S) comprenant : - au moins un récepteur (R) de signaux de radionavigation satellite, - au moins un capteur inertiel (CI), 10 - une unité (U) de traitement de données de mesure selon la revendication 10 pour estimer un état de navigation de la centrale à partir de mesures acquises par le récepteur et par le capteur inertiel.
12. 12. Produit programme d'ordinateur comprenant des instructions de code de programme pour l'exécution des étapes du procédé selon l'une des revendications 1 à 9, lorsque ce programme est exécuté par une unité de traitement de données mesurées.20