WO2016005532A1 - Filtre particulaire invariant - Google Patents

Filtre particulaire invariant Download PDF

Info

Publication number
WO2016005532A1
WO2016005532A1 PCT/EP2015/065769 EP2015065769W WO2016005532A1 WO 2016005532 A1 WO2016005532 A1 WO 2016005532A1 EP 2015065769 W EP2015065769 W EP 2015065769W WO 2016005532 A1 WO2016005532 A1 WO 2016005532A1
Authority
WO
WIPO (PCT)
Prior art keywords
variables
particles
kalman filter
invariant
state
Prior art date
Application number
PCT/EP2015/065769
Other languages
English (en)
Inventor
Axel BARRAU
Silvère BONNABEL
Original Assignee
Sagem Defense Securite
Association Pour La Recherche Developpement Des Methodes Et Processus Industriels - A.R.M.I.N.E.S.
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Sagem Defense Securite, Association Pour La Recherche Developpement Des Methodes Et Processus Industriels - A.R.M.I.N.E.S. filed Critical Sagem Defense Securite
Priority to US15/324,255 priority Critical patent/US10379224B2/en
Publication of WO2016005532A1 publication Critical patent/WO2016005532A1/fr

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/13—Receivers
    • G01S19/35—Constructional details or hardware or software details of the signal processing chain
    • G01S19/37—Hardware or software details of the signal processing chain
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Definitions

  • the invention relates to the field of methods estimating the state of a dynamic system from incomplete or noisy measurements.
  • the invention relates more particularly to a method implementing a particulate filter of particular type.
  • the Kalman filter is a well-known tool for estimating the state of a dynamic system governed by matrix equations, thus linear, by means of noisy measurements.
  • EKF Extended Kalman Filter
  • the extended Kalman filter is not very effective when the noise of the measurements is non-Gaussian.
  • these measurements have a bias subject to sudden "jumps" unpredictable and likely to irremediably diverge the estimate of the state.
  • the invariant filter described in this article is configured to take advantage of certain invariance properties filled with orientation and velocity variables.
  • This filter configuration consists in linearizing an error variable with the same invariances as these orientation and speed variables, instead of linearizing the error variables that can be encountered in a classical extended Kalman filter.
  • the difference between an extended Kalman filter and an invariant extended Kalman filter consists of a particular choice for the error variable.
  • the filter described in this article works on the basis of speed measurements acquired by a speed sensor.
  • Such a filter is not suitable for systems working on the basis of position measurements (certain satellite navigation systems, for example).
  • Particulate filtering also known as the sequential Monte Carlo method, is another method for estimating a non-linear dynamic system, which supports noisy measurements using non-Gaussian noise.
  • the general principle of a particulate filter is to explore the space of the considered state to be estimated using a plurality of samples conventionally called "particles", each particle offering a more or less probable estimate of the state.
  • the following steps are implemented by the particulate filter: a “mutation”, during which each particle is propagated randomly in the state space,
  • weighting during which a weight is calculated for each propagated particle from a new set of measurements, the weight indicating the degree of likelihood of the particle relative to the actual state of the system, taking into account of these measures,
  • the particles explore the state space independently of each other.
  • the particulate filter is therefore all the more expensive in computing resources that there is a high number of particles to maintain.
  • the "bootstrap" variant is prone to a problem of weight degeneration: a single particle ends up presenting a very high weight, while the other particles have a weight close to zero, which makes the estimation ineffective in more than being expensive.
  • Rao-Blackwell particulate filter (“Rao-Blackwellized particle filter", or RBPF in English), in particular described in the document:
  • the RBF filter proposes to decompose a state of dimension K into a first set of variables and a second set of variables, these two games undergoing different treatments during a given iteration.
  • the step of changing the particulate filtering is then implemented selectively on the first set of variables, which makes it possible to reduce the number of particles to be maintained over the iterations compared to the basic "bootstrap" variant.
  • the second set of variables is considered to be a state of dimension smaller than K that is estimated by means of an extended Kalman filter.
  • the calculations implemented by the extended Kalman filter include inversions of matrices which are particularly expensive in computing load. This calculation load is further multiplied by the number of particles to be treated by the Rao-Blackwell filter.
  • the aim of the invention is to provide an estimator of the state of a non-linear dynamic system and subject to noise from non-Gaussian measurements whose computing load is reduced compared to a conventional Rao-Blackwell particle filter.
  • a method for estimating a state representative of the movement of a mobile system the method implementing a particulate filter and comprising the following steps implemented from a reference particle among a plurality of previously calculated particles, the reference particle providing an estimate of the state and comprising a first set of variables and a second set of variables:
  • the extended Kalman filter is of the invariant type, and in that the second set of variables comprises at least variables of orientation, velocity and position of the mobile system, the invariant extended Kalman filter being configured under the assumption that:
  • the pair of orientation and position variables has an invariance property by applying a rotation or a translation to said pair
  • the pair of orientation and speed variables has an invariance property by applying a rotation or a translation to said pair.
  • the method according to the present invention thus proposes to configure an invariant extended Kalman filter under two invariance hypotheses, one filled with a pair of orientation and speed variables, and the other filled with a pair of the variables variables. orientation and position.
  • the invention may also be supplemented by the following features, taken alone or in any of their technically possible combinations.
  • the second set of variables may include a covariance representative of Kalman uncertainty of the other variables of the second set.
  • the steps of the method according to the first aspect can be repeated for each of the particles, the Kalman filter using a common Ricatti equation for all the particles.
  • the mutation step considers state measurements to produce the first set of mutated variables.
  • the mutation step may comprise a common Cholesky decomposition for all the particles.
  • the invariant extended Kalman filter can be implemented for a plurality of particles, said implementation comprising a propagation of the second set of variables by integration of a dynamic equation, the integration comprising, for a reference particle:
  • the estimation method according to the first aspect is advantageously applicable in the field of satellite navigation.
  • the first set can then contain variables representative of a bias in satellite radionavigation measurements.
  • a measured data processing unit adapted to implement the method according to the first aspect. It is further provided according to a third aspect, a computer program product comprising program code instructions for executing the steps of the method according to the first aspect, when this program is executed by this data processing unit.
  • a navigation unit comprising at least one satellite radionavigation signal receiver, at least one inertial sensor, the process comprising the following steps:
  • a navigation unit comprising:
  • At least one satellite radionavigation signal receiver at least one inertial sensor,
  • FIG. 1 shows schematically an inertial unit according to one embodiment of the invention.
  • FIG. 2 represents the steps of a particulate filtering method according to one embodiment of the invention.
  • FIG. 3 details a particular step of FIG. 2 implemented by an invariant Kalman filter.
  • a processing unit U of measurement data is initially considered.
  • the processing unit U is for example a processor or a set of processor (s) adapted (s) for executing a computer program comprising program code instructions.
  • the unit U is configured to implement a qualified estimator in the sequence of "invariant particulate filter", this estimator having for objective to estimate the movement of a mobile system.
  • the invariant particulate filter solicits a Rao-Blackwell particle filter combined with a Kalman filter of a particular type: an invariant extended Kalman filter.
  • the processing unit U comprises an input receiving data measured by one or more sensors adapted to measure physical quantities related to the mobile system (speed, position, acceleration, etc.).
  • a particulate filter is used to produce different samples (or particles) of these additional parameters.
  • Each particle provides a set of specific linear equations used by the Kalman filter to develop a new estimate of the previously mentioned variable set.
  • the Kalman filter also provides a likelihood value for each parameter tested.
  • the additional parameters are then selected according to their likelihood.
  • the Kalman filter used is of the invariant extended type.
  • this invariant extended filter makes it possible to process a set of variables governed by a non-linear system, the set of variables having an invariance property.
  • This property of invariance is notably exposed in the article "Invariant Extended Kalman Filter: theory and application to a velocity-aided attitude estimation problem", by S. Bonnabel, P. Martin and E. Salaun, published in 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. It should be noted here that the addition of a position variable prevents the invariance hypothesis exposed in these articles. But the invariant filter configured as if it were verified, however, has advantageous properties which the invention takes advantage.
  • the processing unit U implements the invariant particulate filter in successive iterations, or cycles, to estimate successive states of the mobile system, the evolution of said states being representative of the movement of the mobile system.
  • state can designate a set of variables representative of physical quantities related to the mobile system (speed, position, acceleration, etc. of this mobile system).
  • Each iteration takes as input data computed by the unit U in a previous iteration, as well as external measurements provided by sensors, each iteration producing new state estimates referred to as the conventional term of particles as it has been seen in introduction.
  • the state is constructed from the set of variables - attitude / position / speed and the additional parameters mentioned above.
  • the state can be represented by a vector comprising a first set of variables (the additional parameters) and a second set of variables (at least attitude / position / speed).
  • each particle is a vector comprising:
  • step 101 the additional parameters of the reference particle are randomly propagated in their space, in the manner of a conventional particulate filter.
  • the result obtained is an additional set of parameters modified, said in the following "mutated".
  • the mutation step 101 may take account of this observation in addition to the random propagation model used. This correction thus makes it possible to bring the particles closer to the true additional parameters.
  • the invariant Kalman filter equations include a dynamic equation, representing the prior behavior of the system, and an observation equation, representing the behavior of the sensors used to obtain observations, also called measurements.
  • the equations of the Kalman filter are parameterized as a function of the values of the set of additional mutated parameters obtained at the end of the mutation step 101.
  • the second set of variables of the reference particle is then estimated using the invariant extended Kalman filter.
  • the vector of the second set of variables is propagated in the manner of an invariant Kalman filter, using the dynamic equation (so-called propagation step 301).
  • the covariance of the variables constituting the second set of variables is also propagated by means of the Riccati equation associated with the invariant Kalman filter (step 303), constructed in using the dynamic equation.
  • the vector of the second set of variables is updated in the manner of an invariant Kalman filter, using the observation equation of this filter is set using the additional parameters. Likewise, the covariance obtained at the end of propagation steps 303 is updated using the invariant Kalman filter.
  • Update 302 by the invariant extended Kalman filter also provides a likelihood for the new particle.
  • a weighting step 104 the weight of the particle is multiplied by the exponential of the likelihood.
  • weighting step which is usually implemented in a bootstrap-type particulate filter, is here implemented. from the data produced by the invariant extended Kalman filter.
  • the weights can be normalized so that their sum is 1.
  • the current iteration may also include a selection step 105, in which low weight matched particles are removed while larger, more likely, weighted particles are duplicated. This selection can be triggered on a weight dispersion criterion, in order to minimize the problem of degeneration of the mentioned weights.
  • the selection 105 produces k new weighted particles which are then used as input data for a next iteration of the invariant particulate filter.
  • Step 102 is therefore implemented only once in the current iteration with respect to the propagation of the common covariance.
  • an estimate of the state (or function of the state) can be calculated by performing a weighted average of the k new particles. It is not necessary to make such an estimate at the end of each iteration, but only after some of them.
  • the estimate of the state obtained is the output of the estimator.
  • a navigation unit IN comprises at least one satellite radionavigation signal receiver R, at least one inertial sensor C1, and a unit U of data processing adapted to implement the defined invariant particulate filter. in part I.
  • the processing unit U is more particularly configured to implement an estimation of a navigation state of the plant, from measurements acquired by the receiver and the inertial sensor, and by means of the invariant particulate filter. above described.
  • Inertial sensors comprise in one embodiment of the three gyrometers (one per axis) and three accelerometers (one per axis).
  • the receiver R is of the GNSS / GPS type.
  • the observations are: a measurement of position Y n supplied at times t n by the receiver R (vector of dimension 3)
  • V n is the unbiased part of the noise associated with the measurement of the GPS receiver and g the gravity vector at the location given by the GPS.
  • the variance of V n is denoted r 2 .
  • b t is constant, except at random instants where it undergoes a jump and takes a Gaussian random value, of average 0 and of variance ⁇ .
  • the duration between two jumps is independent of the other variables and follows an exponential law of parameter ⁇ .
  • N (y, ⁇ , x) dImT ⁇ ) exp (- (y - x) T ⁇ - 1 (y -))
  • the set of variables processed by invariant filtering (second set of variables) is formed by T t , v t , x t .
  • the additional parameters are: the bias b t .
  • the second set of variables (attitude / position / velocity) is initialized according to a density a priori
  • the weights of the k particles are initialized to 1 / k
  • the values of the biases are drawn randomly according to a density a priori
  • the covariance common to all the particles is initialized to a value estimated a priori.
  • the bias b is randomly propagated in its space, in the manner of a conventional particulate filter.
  • the dynamic equation does not depend on the bias b t : we can save the mutation step when there is no observation available.
  • the mutation step is implemented by the following equations.
  • X (Y n , T tn HPH T n + ⁇ 3 + r 2 I 3 , x tn ) is the interval separating the arrival of the last two is the realization of a real random variable following a uniform on [0,1].
  • the vector of the second set of variables and its covariance are then propagated (propagation steps 301, 303) by means of an invariant Kalman filter, using the following dynamic e uation.
  • the covariance P is also updated, by the following formula:
  • the updating step 304 of the covariance is also performed once for all the k particles.
  • a new particle, derived from the reference particle, is obtained by applying the steps above.
  • Update 302 also provides a likelihood for the new particle, from which the weight of the new particle is calculated. c) Selection
  • the selection stage is implemented according to the general principle (see Part I). d) Estimate
  • the mutation comprises a Cholesky decomposition for each particle, a rather expensive decomposition in computing load.
  • propagation step 302 of the second set of variables was implemented for each of the k particles in the current iteration, and that this propagation step comprises the integration of a linear differential equation.
  • invariant particulate filter it is possible to calculate only one general form of the solution, for all the particles, since this general form is common to them.
  • the general method described in part I is not limited to the specific application described in part II.
  • the invariant particulate filter is intended to be used for the estimation of any state, especially those whose measurements are subject to jumps similar to those encountered with GPS receivers.
  • the invariant particle filter can be used to estimate the position of a robot using a depth camera.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Signal Processing (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Gyroscopes (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

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, et en ce que le deuxième jeu de variables comprend au moins des variables d'orientation, de vitesse et de position du système mobile, le filtre de Kalman étendu invariant étant configuré sous l'hypothèse que: le couple des variables d'orientation et de position présente une propriété d'invariance par application d'une rotation ou d'une translation audit couple, et que -le couple des variables d'orientation et de vitesse présente une propriété d'invariance par application d'une rotation ou d'une translation audit couple.

Description

Filtre particulaire invariant
DOMAINE GENERAL
L'invention se rapporte au domaine des procédés estimant l'état d'un système dynamique à partir de mesures incomplètes ou bruitées.
L'invention concerne plus particulièrement un procédé mettant en œuvre un filtre particulaire de type particulier.
ETAT DE L'ART
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 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 utilisées pour calculer un nouvel état estimé selon la méthode du filtre de Kalman classique.
Toutefois, le filtre de Kalman étendu s'avère peu efficace lorsque le bruit des mesures est non gaussien. Dans le cadre particulier de mesures effectuées par un récepteur GNSS, on constate que ces mesures présentent un biais sujet à des « sauts » brusques imprévisibles et susceptibles de faire diverger irrémédiablement l'estimation de l'état.
On connaît par ailleurs un type particulier de filtre de Kalman étendu, dit « filtre de Kalman étendu invariant », décrit notamment dans l'article :
« Invariant Extended Kalman Filter : theory and application to a velocity-aided attitude estimation problem », Silvère Bonnabel et al. - Décision and control, 2009 - Proceedings of the 48 IEEE conférence - 15 décembre 2009 - pages 1297-1304.
Le filtre invariant décrit dans cet article est configuré de façon à tirer parti de certaines propriétés d'invariance remplies par des variables d'orientation et de vitesse.
Cette configuration de filtre consiste à linéariser une variable d'erreur présentant les mêmes invariances que ces variables d'orientation et de vitesse, au lieu de linéariser la variables d'erreur que l'on peut rencontrer dans un filtre de Kalman étendu classique.
En d'autres termes, la différence entre un filtre de Kalman étendu et un filtre de Kalman étendu invariant consiste en un choix particulier pour la variable d'erreur.
Cependant, le filtre décrit dans cet article fonctionne sur la base de mesures de vitesses acquises par un capteur de vitesse. Un tel filtre n'est pas adapté à des systèmes travaillant sur la base de mesures de position (certains systèmes de radionavigation par satellite par exemple).
Par ailleurs, dans ce document il est fait l'hypothèse d'invariance selon laquelle l'équation décrivant l'évolution du système est inchangée par l'action d'un certain groupe de transformations agissant sur le système et ses entrées (cf. « Définition 2 », page 1298, colonne de gauche).
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 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 œuvre 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 la 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 les 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 les unes les autres.
Le filtre particulaire est donc d'autant plus coûteux en ressources 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 les autres particules présentent un poids proche de zéro, ce qui rend l'estimation peu efficace en plus d'être coûteuse.
Une variante améliorée de filtre particulaire a ainsi été proposée pour pallier cet inconvénient : le filtre particulaire de Rao-Blackwell (« Rao-Blackwellized particle filter », ou RBPF en anglais), notamment décrit dans le document :
- CAPPE O et al. - "An Overview of Existing Methods and Récent Advances in Sequential Monte Carlo"- Proceedings of the IEEE, vol. 95, n°5 - 1er Mai 2005 - pages 899-924
Le filtre RBF propose de décomposer un état de dimension K en un 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 œuvre de façon sélective sur le premier jeu de variable, ce qui permet de réduire le nombre de particules à maintenir au fil des itérations par rapport à la variante « bootstrap » de base.
Par ailleurs, le 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, les calculs mis en œuvre par le filtre de Kalman étendu comprennent des inversions de matrices particulièrement coûteuses en charge de calcul. Cette charge de calcul est de plus multipliée par le nombre de particules à traiter par le filtre de Rao-Blackwell.
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 la charge de calcul est réduite par rapport à un filtre particulaire de Rao-Blackwell conventionnel.
Dans ce but, il est proposé selon un premier aspect un procédé d'estimation d'un état représentatif du mouvement 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, 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 œuvre 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, et en ce que le deuxième jeu de variables comprend au moins des variables d'orientation, de vitesse et de position du système mobile, le filtre de Kalman étendu invariant étant configuré sous l'hypothèse que :
- le couple des variables d'orientation et de position présente une propriété d'invariance par application d'une rotation ou d'une translation audit couple, et que
- le couple des variables d'orientation et de vitesse présente une propriété d'invariance par application d'une rotation ou d'une translation audit couple.
Une propriété intéressante du filtre de Kalman étendu invariant est que les particules peuvent avoir 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 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.
On peut noter que le filtre de Kalman étendu invariant décrit dans le document « Invariant Extended Kalman Filter: theory and application to a velocity-aided attitude estimation problem » discuté en introduction ne fait pas intervenir la position du système mobile considéré mais seulement son orientation et sa vitesse ; de plus, l'hypothèse d'invariance décrite dans cet article n'est pas généralisable à un système incluant en plus une variable de position.
Le procédé selon la présente invention propose ainsi de configurer un filtre de Kalman étendu invariant sous deux hypothèses d'invariances, l'une remplie par un couple de variables d'orientation et de vitesse, et l'autre remplie par un couple des variables d'orientation et de position. L'invention peut également être complétée par les caractéristiques suivantes, prises seules ou en une quelconque de leurs combinaisons 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 répétées pour chacune des particules, le filtre de Kalman utiliser une équation de Ricatti 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 le premier aspect étant répétées pour 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 œuvre pour une pluralité de particules, ladite mise en œuvre comprendre 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 la solution générale étant mise en œuvre une seule fois pour toutes les particules.
Le procédé d'estimation selon le premier aspect trouve avantageusement application dans le domaine de la navigation satellite.
Le premier jeu peut alors contenir des variables représentatives d'un biais dans des mesures de radionavigation satellite.
Il est proposé, selon un deuxième aspect, une unité de traitement de données mesurées adaptée pour mettre en œuvre le procédé selon le 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 le premier 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 œuvre par une centrale de navigation comprenant au moins un récepteur de signaux de radionavigation satellite, au moins un capteur inertiel, le procédé comprenant les étapes suivantes :
- acquisition de mesures par les capteurs,
- mise en œuvre des étapes du procédé selon le premier aspect pour 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 la 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 la description qui suit, qui est purement illustrative et non limitative, et qui doit être lue 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 œuvre par un filtre de Kalman invariant.
Sur l'ensemble des figures, les é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 la deuxième partie.
/. 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 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 œuvre un estimateur qualifié dans la suite de « filtre particulaire invariant », cet estimateur ayant pour objectif d'estimer le mouvement d'un système mobile.
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 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 linéaire susceptible d'être traité par un filtre de Kalman défini par des équations matricielles. Ces équations matricielles, connues à l'avance dans le 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.
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 de chaque paramètre testé. On sélectionne alors les paramètres supplémentaires d'après leur vraisemblance.
Le filtre de Kalman utilisé est du type étendu invariant.
L'utilisation de ce filtre étendu invariant rend possible le traitement d'un jeu de variables régies par un système cette fois non- linéaire, le 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 Filter: theory and application to a velocity-aided attitude estimation problem », par S. Bonnabel, P. Martin et E. Salaun, publié dans In Décision and Control, 2009 held jointly with the 2009 28th Chinese Control Conférence, CDC/CCC 2009. Proceedings of the 48th IEEE Conférence on (pp. 1297-1304). IEEE. On notera ici que l'ajout d'une variable de position empêche d'obtenir l'hypothèse d'invariance exposée dans ces articles. Mais le filtre invariant configuré comme si elle était vérifiée présente cependant des propriétés avantageuses dont l'invention tire parti.
L'unité U de traitement met en œuvre le filtre particulaire invariant dans des itérations successives, ou cycles, pour estimer des états 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 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 comme on l'a vu en introduction.
L'état est construit à partir du jeu de variables - attitude/position/vitesse 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 (attitude/ position /vitesse au moins).
On va maintenant décrire une itération courante du filtre particulaire mis en œuvre 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 d'une itération précédente du filtre particulaire invariant. Par définition, chaque particule est un vecteur comprenant :
- un vecteur contenant au moins attitude, position et vitesse,
- un vecteur contenant les paramètres supplémentaires,
- la covariance des variables du deuxième jeu de variables. 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. a) Mutation
Dans une étape dite de « mutation » (étape 101 ), les paramètres supplémentaires de la 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 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 les particules des 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 œuvre 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 paramètres supplémentaires, on peut faire l'économie de l'étape de mutation lorsqu'il n'y a pas d'observation disponible.
Le deuxième jeu de variables de la particule de référence est ensuite estimé au moyen du filtre de Kalman étendu invariant.
En référence à la figure 3, le vecteur du deuxième jeu de variables est propagé à la manière d'un filtre de Kalman invariant, en utilisant l'équation dynamique (étape dite de propagation 301 ).
La covariance des variables constituant le deuxième jeu de variables (contenant au moins attitude, position et vitesse) fait également l'objet d'une propagation au moyen de l'équation de Riccati associée au filtre de 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 deuxième jeu de variables 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, le 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 la particule de référence.
On notera que l'étape de pondération, qui est ordinairement mise en œuvre dans un filtre particulaire de type « bootstrap », est ici mise en œuvre à 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 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. 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 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 la covariance est souvent commune pour les 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 œuvre les étapes de propagation et de mise à jour, qui comprennent des inversions de matrices particulièrement coûteuses. L'étape 102 n'est donc mise en œuvre 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 (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 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 à la fin de chaque itération, mais seulement à l'issue de certaines d'entre elles.
L'estimation de l'état obtenue est la donnée de sortie de l'estimateur.
//. Application du filtre particulaire invariant à une centrale de navigation
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 Cl, et une unité U de de traitement de données adaptée pour mettre en œuvre 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 œuvre une estimation d'un é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).
Le récepteur R est du type GNSS/GPS.
Variables
Les variables suivantes sont utilisées :
- Instant t considéré - 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).
Entrées du système
Les entrées du système sont les suivantes :
- vitesse angulaire œt (vecteur de dimension 3, données par le gyromètre)
- force spécifique ft (vecteur de dimension 3, donné par les 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 dimension 3)
Equations mises en œuvre
Les équations suivies par ces variables sont
— d Tt = Tt ùit + ννω)χ
d
—vt = g + Tt(ft + wf)
d
dt τ τ
Yn = Xtn + btn + Vn
Où ww est le bruit (de variance Çw ) associé à la mesure du gyroscope, wf est le bruit (de variance Qf ) associé à la mesure de l'accéléromètre, Vn 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 Vn 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 σξ . 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
N(y,∑, x) = dîmT∑) exp(- (y - x)T∑-1(y - ))
(27Γ) 2 |∑| l/2
Figure imgf000018_0001
(densité gaussienne de moyenne x et de covariance∑). Pour tout vecteur u G E3 on notera (u)x la matrice 3 x 3 antisymétrique telle que pour tout autre vecteur x G M3 on ait (u) y X — 1A. X OC * le symbole x désignant le produit vectoriel.
Le jeu de variables traitées par filtrage invariant (deuxième jeu de variables)est formé par Tt, vt, xt.
En effet, si le biais est connu on peut reformuler le problème en introduisant les variables suivantes :
Figure imgf000018_0002
Le système se réécrit alors sous la forme
Où gU) f : une fonction
Figure imgf000018_0003
dépendant des incréments œt et ft . Elle prend en argument une matrice 5 x 5, rend une matrice 5 x 5 et vérifie :
9a>,f (X1X2 = Χΐ9ω,β Χ2 + 9ω,β Χΐ)Χ2 ~ XlhX -
- Les paramètres supplémentaires sont : le biais bt.
- On considère donc ici des particules comprenant (ft, vt, xt, bt). On va maintenant détailler à nouveau les étapes de la figure 2 appliquées à la centrale inertielle.
Dans une étape préliminaire d'initialisation, le deuxième jeu de variables (attitude/ position /vitesse) est initialisé selon une densité a priori, les poids des k particule sont initialisées à 1 /k, les valeurs des biais sont tirées aléatoirement selon une densité a priori, la covariance commune à toutes les particules est initialisée à une valeur estimée a priori. a) Mutation
Dans l'étape de mutation 101 , le biais b est propagé aléatoirement dans son espace, à la manière d'un filtre particulaire classique.
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 correspondant, l'étape de mutation est mise en œuvre par les équations suivantes.
On calcule et ΙΊ2 d'après les définitions suivantes :
= 1 - e- t)X(Yn, TtnHPHTn + σξΐ3 + r2I3, xtn)
Figure imgf000019_0001
est l'intervalle séparant l'arrivée des deux dernières est la réalisation d'une variable aléatoire réelle suivant une uniforme sur [0,1].
Si s < le biais bt ne change pas. Sinon il est mis à jour comme suit :
Π!+Π2 Où NL suit une loi gaussienne de moyenne TtnKbîfn(Yn - xtn) et de variance ftnPbÎn, avec Kb = σξ (ΗΡΗτ + σξΐ3 + r2^)"1 et Pb = σ2 (/3 -
En d'autres termes le biais est mis à jour selon l'équation :
Figure imgf000020_0001
Où L vérifie la relation : L x LT = T, PbÎJ
On notera que dans le système dynamique selon la présente application particulière, l'équation dynamique ne dépend pas du biais, et l'équation d'observation dépend bien du biais b. b) Propagation I Mise à jour
Le vecteur du deuxième jeu de variables et sa covariance sont ensuite propagées (étapes de propagation 301 , 303) au moyen d'un filtre de Kalman invariant, en utilisant l'é uation dynamique suivante.
Figure imgf000020_0002
tet = g + Ttft
d
t = ϋ
Figure imgf000020_0003
Ces équations reviennent à intégrer une équation différentielle linéaire à 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 œuvre une seule fois pour toutes les particules à traiter au cours de l'itération courante.
SI l'observation GPS est disponible, le vecteur du deuxième jeu de variables et la covariance associée obtenus à l'issue de l'étape de propagation sont ensuite mis à jour à la manière d'un filtre de Kalman invari
Figure imgf000021_0001
Avec Kn = PtnHT(r2I3 + HP^y1
La covariance P est elle aussi mise à jour, par la formule suivante :
P <- (/9 - KnH)P
L'étape de mise à jour 304 de la covariance est également 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 également une vraisemblance pour la nouvelle particule, à partir de laquelle le poids de la nouvelle particule est calculé. c) Sélection
L'étape de sélection est mise en œuvre selon le principe général (voir partie I). 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 averaging rotations » par Gramkow, C. (2001 ), dans Journal of Mathematical Imaging and Vision, 75(1 -2), 7-16.
///. Astuces d'implémentation du filtre particulaire invariant dans une centrale de navigation 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 œuvre selon la partie II, la mutation comprend une 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 = ftnPbfn. Or la matrice Pb ne dépend pas de la particule considérée. On peut donc calculer une fois pour toutes la décomposition de Cholesky Pb = L0 * LT 0 puis poser pour chaque particule L = TtnLQ.
On dispose donc, en sortie de l'étape de mutation 101 , de particules plus vraisemblables car tenant compte de la dernière observation, pour une charge plus réduite. b) mise en commun de calculs de propagation
On a vu précédemment que l'étape de propagation 302 du deuxième jeu de variables était mise en œuvre pour chacune des k particules dans l'itération courante, et que cette étape de propagation comprenant l'intégration d'une équation différentielle linéaire.
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 la 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 la solution, pour toutes les particules, étant donné que cette forme générale 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 la présente application particulière:
Entre deux observations, dont les temps d'arrivée seront notés tn et tn+1 et l'écart At = tn+1 - tn, les grandeurs suivantes sont calculées une seule fois pour toutes les particules :
Figure imgf000023_0001
Δ η = J (J Rsfsdsj dt
Au moment où arrive l'observation les grandeurs suivantes sont mises à jour pour chaque particule :
1 Λ
Ûtn *~ Ûtn + Ttn vn + &t * g
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 particulaire 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, le filtre particulaire invariant peut être utilisé pour estimer la position d'un robot utilisant une caméra de profondeur.

Claims

REVENDICATIONS
1. Procédé d'estimation d'un état représentatif du mouvement 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 variables 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, et en ce que le deuxième jeu de variables comprend des variables d'orientation, de vitesse et de position du système mobile, le filtre de Kalman étendu invariant étant configuré sous l'hypothèse que :
- le couple des variables d'orientation et de position présente une propriété d'invariance par application d'une rotation ou d'une translation audit couple, et que
- le couple des variables d'orientation et de vitesse présente une propriété d'invariance par application d'une rotation ou d'une translation audit couple.
2. Procédé selon la revendication 1 , dans lequel le deuxième jeu de variables comprend une covariance représentative d'une incertitude au sens de Kalman des autres variables du deuxième jeu.
3. Procédé selon l'une des revendications 1 et 2, dont les étapes sont répétées pour chacune des particules, et dans lequel le filtre de Kalman utilise une équation de Ricatti commune pour toutes les particules.
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é.
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. Procédé selon l'une des revendications 1 à 5, dans lequel le filtre de Kalman étendu invariant est mis en œuvre pour une pluralité de particules, ladite mise en œuvre 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 la solution générale étant mise en œuvre une seule fois pour toutes les particules.
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.
8. Procédé d'estimation de l'état d'un système mobile mis en œuvre par une centrale de navigation comprenant au moins un récepteur (R) de signaux de radionavigation satellite, au moins un capteur inertiel (Cl), le procédé comprenant les étapes suivantes :
- acquisition de mesures (200) par les capteurs (R, Cl),
- mise en œuvre des étapes du procédé selon l'une des revendications 1 à 7 pour produire un ensemble de particules à partir des données mesurées,
- estimation (106) de l'état du système mobile à partir de l'ensemble de particules.
9. Unité de traitement de données mesurées adaptée pour mettre en œuvre le procédé selon l'une des revendications 1 à 8.
10. Centrale de navigation (S) comprenant :
- au moins un récepteur (R) de signaux de radionavigation satellite, - au moins un capteur inertiel (Cl),
- une unité (U) de traitement de données de mesure selon la revendication 9 pour estimer un état de navigation de la centrale à partir de mesures acquises par le récepteur et par le capteur inertiel.
1 1 . 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.
PCT/EP2015/065769 2014-07-09 2015-07-09 Filtre particulaire invariant WO2016005532A1 (fr)

Priority Applications (1)

Application Number Priority Date Filing Date Title
US15/324,255 US10379224B2 (en) 2014-07-09 2015-07-09 Invariant particle filtering

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
FR1401535A FR3023623B1 (fr) 2014-07-09 2014-07-09 Procede d'estimation de l'etat d'un systeme mobile et centrale inertielle correspondante.
FR1401535 2014-07-09

Publications (1)

Publication Number Publication Date
WO2016005532A1 true WO2016005532A1 (fr) 2016-01-14

Family

ID=51862347

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/EP2015/065769 WO2016005532A1 (fr) 2014-07-09 2015-07-09 Filtre particulaire invariant

Country Status (3)

Country Link
US (1) US10379224B2 (fr)
FR (1) FR3023623B1 (fr)
WO (1) WO2016005532A1 (fr)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106405433A (zh) * 2016-11-04 2017-02-15 首都师范大学 一种基于扩展卡尔曼粒子滤波的soc估计方法及系统
CN111964667A (zh) * 2020-07-03 2020-11-20 杭州电子科技大学 一种基于粒子滤波算法的地磁_ins组合导航方法
CN115041669A (zh) * 2022-06-30 2022-09-13 山东中衡光电科技有限公司 用于大型轮带切割设备的控制系统和控制方法

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10473466B2 (en) * 2016-12-09 2019-11-12 Honeywell International Inc. Apparatus and method for data-based referenced navigation
CN109253727B (zh) * 2018-06-22 2022-03-08 东南大学 一种基于改进迭代容积粒子滤波算法的定位方法
FR3084176B1 (fr) * 2018-07-23 2020-06-19 Safran Electronics & Defense Procede et dispositif d'aide a la navigation d'un porteur a l'aide d'un filtre de kalman invariant et d'un etat de navigation d'un deuxieme porteur
FR3084151B1 (fr) * 2018-07-23 2020-06-19 Safran Procede et dispositif d'aide a la navigation d'une flotte de vehicules a l'aide d'un filtre de kalman invariant
US11378403B2 (en) 2019-07-26 2022-07-05 Honeywell International Inc. Apparatus and method for terrain aided navigation using inertial position
FR3106885B1 (fr) * 2020-02-03 2021-12-24 Safran Procede d’aide à la navigation d’un porteur mobile

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2011064677A2 (fr) * 2009-11-24 2011-06-03 Yost Engineering, Inc. Combinaison de capteurs inertiels redondants afin de créer une sortie de capteur virtuelle

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8886394B2 (en) * 2009-12-17 2014-11-11 Bae Systems Plc Producing data describing states of a plurality of targets
JP5017392B2 (ja) * 2010-02-24 2012-09-05 クラリオン株式会社 位置推定装置および位置推定方法
FR2961897B1 (fr) * 2010-06-25 2012-07-13 Thales Sa Filtre de navigation pour un systeme de navigation par correlation de terrain
US9921306B1 (en) * 2015-05-27 2018-03-20 Lockheed Martin Corporation Active optimal reduced state estimator

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2011064677A2 (fr) * 2009-11-24 2011-06-03 Yost Engineering, Inc. Combinaison de capteurs inertiels redondants afin de créer une sortie de capteur virtuelle

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
BARRAU AXEL ET AL: "Invariant particle filtering with application to localization", 53RD IEEE CONFERENCE ON DECISION AND CONTROL, IEEE, 15 December 2014 (2014-12-15), pages 5599 - 5605, XP032733627, ISBN: 978-1-4799-7746-8, [retrieved on 20150211], DOI: 10.1109/CDC.2014.7040265 *
CAPPE O ET AL: "An Overview of Existing Methods and Recent Advances in Sequential Monte Carlo", PROCEEDINGS OF THE IEEE, IEEE. NEW YORK, US, vol. 95, no. 5, 1 May 2007 (2007-05-01), pages 899 - 924, XP011186717, ISSN: 0018-9219, DOI: 10.1109/JPROC.2007.893250 *
SILVERE BONNABEL ET AL: "Invariant Extended Kalman Filter: theory and application to a velocity-aided attitude estimation problem", DECISION AND CONTROL, 2009 HELD JOINTLY WITH THE 2009 28TH CHINESE CONTROL CONFERENCE. CDC/CCC 2009. PROCEEDINGS OF THE 48TH IEEE CONFERENCE ON, IEEE, PISCATAWAY, NJ, USA, 15 December 2009 (2009-12-15), pages 1297 - 1304, XP031620108, ISBN: 978-1-4244-3871-6 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106405433A (zh) * 2016-11-04 2017-02-15 首都师范大学 一种基于扩展卡尔曼粒子滤波的soc估计方法及系统
CN111964667A (zh) * 2020-07-03 2020-11-20 杭州电子科技大学 一种基于粒子滤波算法的地磁_ins组合导航方法
CN115041669A (zh) * 2022-06-30 2022-09-13 山东中衡光电科技有限公司 用于大型轮带切割设备的控制系统和控制方法
CN115041669B (zh) * 2022-06-30 2024-09-06 山东中衡光电科技有限公司 用于大型轮带切割设备的控制系统和控制方法

Also Published As

Publication number Publication date
FR3023623A1 (fr) 2016-01-15
FR3023623B1 (fr) 2023-05-05
US20170160399A1 (en) 2017-06-08
US10379224B2 (en) 2019-08-13

Similar Documents

Publication Publication Date Title
WO2016005532A1 (fr) Filtre particulaire invariant
EP3278061B1 (fr) Procédé de suivi de navigation d&#39;un porteur mobile avec un filtre de kalman étendu
EP3213033B1 (fr) Procédé d&#39;estimation d&#39;un état de navigation contraint en observabilité
EP3071934B1 (fr) Procédé d&#39;alignement d&#39;une centrale inertielle
EP3514571B1 (fr) Procede de pistage d&#39;une cible aerienne, et radar mettant en oeuvre un tel procede
EP2718670B1 (fr) Procede d&#39;estimation simplifie de l&#39;orientation d&#39;un objet et centrale d&#39;attitude mettant en oeuvre un tel procede
EP2297564A1 (fr) Procédé et dispositif d&#39;analyse fréquentielle de données
EP3827221B1 (fr) Procédé et dispositif d&#39;aide à la navigation d&#39;un porteur à l&#39;aide d&#39;un filtre de kalman invariant et d&#39;un état de navigation d&#39;un deuxième porteur
EP3655724B1 (fr) Procédé d&#39;estimation du mouvement d&#39;un objet évoluant dans un champ magnétique
EP3827220B1 (fr) Procédé et dispositif d&#39;aide à la navigation d&#39;une flotte de véhicules à l&#39;aide d&#39;un filtre de kalman invariant
WO2023180143A1 (fr) Procédé de détermination d&#39;au moins un rayon de protection associé a au moins un parametre de navigation et dispositif électronique de détermination associé
WO2022254163A1 (fr) Procede d&#39;aide a la navigation d&#39;un vehicule
FR3104704A1 (fr) Filtrage particulaire et centrale de navigation a correlation de mesure
EP4100696A1 (fr) Procede d&#39;aide à la navigation d&#39;un porteur mobile
EP2943935A1 (fr) Estimation de mouvement d&#39;une image
EP3639140A1 (fr) Dispositif de caracterisation et/ou de modelisation de temps d&#39;execution pire-cas
FR3104705A1 (fr) Filtrage particulaire et centrale de navigation a correlation de mesure
WO2023135379A1 (fr) Detection d&#39;anomalies de mesure par filtre de kalman appliquee a la navigation
WO2023166260A1 (fr) Procede et dispositif d&#39;aide a la navigation basee sur un filtre de kalman
EP3973249A1 (fr) Filtrage particulaire et centrale de navigation a correlation de mesure

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 15739214

Country of ref document: EP

Kind code of ref document: A1

WWE Wipo information: entry into national phase

Ref document number: 15324255

Country of ref document: US

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 15739214

Country of ref document: EP

Kind code of ref document: A1