Method and device for locating a mobile carrier using an extended Kalman filter

Abstract

A method for localizing a moving carrier, in which an extended Kalman filter (EKF) estimates, over successive iterations, a carrier state, an iteration of the filter (EKF) implementing a propagation (300) and a recalibration (500) of a carrier state and a location uncertainty as a function of measurements intended to be fused, these measurements being acquired by at least one inertial sensor and / or at least one navigation sensor, the determinations implemented for this propagation and recalibration being a function of different filter parameters, these parameters comprising quantities characteristic of a kinematic model and quantities characteristic of an observational model, an adjustment of the filter parameters, this adjustment being implemented by a backpropagation calculation in a time window, this calculation comprising a determination of the gradients of a loss function with respect to the filter parameters,an optimization of the parameters that minimize said loss function. The parameters with respect to which the determination of the gradients of the loss function is determined include at least the following list: Kalman gain, Jacobian of the observation model, observation, covariance of the observation, Jacobian of the dynamic model, covariance of the dynamic model. The parameter values ​​thus obtained are used, along with the propagated carrier state obtained at the end of step a) and an update of the measurements by the sensors, in the implementation of the next iteration. Figure 4,

Description

Title of the invention: Method and device for localizing a moving carrier using an extended Kalman filter technical field

[0001] The present invention relates to the field of localization of at least one mobile carrier.

[0002] The localization of a mobile carrier is often carried out from physical measurements which are then merged.

[0003] The invention relates more particularly to systems implementing a fusion between dynamic data (typically data provided by an inertial measurement unit) and data from other sensors (odometers, GPS, etc.). The invention is advantageously applicable to all localization systems (terrestrial, aeronautical, naval), and in particular, but not limited to, aircraft navigation systems. More specifically, the invention focuses on navigation systems that use an extended Kalman filter for localization. State of the art

[0004] The task of merging data from different sensors is difficult because of measurement noise. To account for the random nature of the measurements, the merging process must weight the measurements according to their uncertainties.

[0005] Kalman filters are well-known tools for addressing this difficulty and for merging inertial data and data from other sensors in real time, thus enabling localization for a carrier such as a ship, aircraft, land vehicle or any other moving object.

[0006] Classically, a Kalman filter estimates, through successive iterations, a navigation state of the carrier via linear equations applied to noisy measurements provided by navigation sensors. The navigation equations are linearized, which constitutes a restrictive limitation.

[0007] For nonlinear cases, there is no optimal method. Numerous variants have been proposed to extend Kalman filters to dynamical systems governed by nonlinear equations. One method used is that designated by the expression "extended Kalman filter" (EKF). This development proposes an additional step consisting of linearizing, at each new iteration of the filter, the equations governing the nonlinear system at a point in the vector space, this point being typically a state estimated during a previous iteration. The matrices resulting from this linearization can then be used to compute a new state estimated according to the classical Kalman filter method.

[0008] Methods using an extended Kalman filter, like most other known methods, work by implementing the following two steps. • The first step is propagation. During this step, the estimated state of the system is propagated according to the propagation equation from measurements of the dynamics (example: inertial increments). The associated uncertainty of the system is also propagated. • The second step is a recalibration (also called "correction"). During this step, the system state estimate and the location uncertainty are corrected through a total or partial measurement of the system state. The relationship between the measurement and the system state is described by the observation equation.

[0009] Thus, to implement an extended Kalman filtering method, it is necessary to model the dynamics of the system as well as its observation function.

[0010] However, in some cases, the dynamics of the system as well as the link between the state of the system and the measurement are difficult to model, particularly in the case of complex systems.

[0011] In addition, the uncertainties associated with the dynamics and observation models can change significantly over time.

[0012] Furthermore, modeling the observation function can also be problematic with certain sensors.

[0013] Typically, when using an odometer, the shape of the terrain can have a significant impact. Modeling the effect of landslides also proves difficult.

[0014] To try to resolve these difficulties, it is known to evolve the parameters of the filter as navigation progresses (adaptive filtering).

[0015] The literature often focuses on estimating covariances (of the dynamics and the observation), because they have a huge impact on the performance of the filter.

[0016] Thus, there are statistical methods which allow online determination of covariance estimates.

[0017] [1] AH Mohamed and KP Schwarz, “Adaptive Kalman Filtering for INS / GPS”, Journal of Geodesy, vol. 73, no. 4, p. 193-203, May 1999, doi: 10.1007 / s001900050236.

[0018] However, these approaches neglect the dependence of the Jacobians on the state of the system.

[0019] Another type of approach is the adaptation by numerical optimization of a loss function. To perform the optimization, it is necessary to calculate the gradient of the loss function with respect to the parameters. Traditionally, sensitivity equations are used to perform this calculation.

[0020] [2] N. Gupta and R. Mehra, “Computational aspects of maximum likelihood estimation and reduction in sensitivity function calculations”, IEEE Transactions on Automatic Control, vol. 19, no. 6, p. 774-783, Dec. 1974, doi: 10.1109 / TAC. 1974.1100714.

[0021] Optimization by calculating the gradients of a loss function with respect to the parameters according to this method is, however, particularly computationally intensive.

[0022] Another more efficient approach in terms of algorithmic complexity and therefore computation time is the use of analytical backpropagation equations.

[0023] [3] C. Parellier, A. Barrau, and S. Bonnabel, “Speeding-Up Backpropagation of Gradients Through the Kalman Filter via Closed-Form Expressions”, IEEE Transactions on Automatic Control, vol. 68, no. 12, p. 8171-8177, Dec. 2023, doi: 10.1109 / TAC.2023.3297879.

[0024] However, the equations used are only valid for a Kalman filter and are not transposable for an extended Kalman filter.

[0025] It has already been proposed to use analytical backpropagation equations in the case of extended Kalman filtering

[0026] [4] J. Benhamou, S. Bonnabel, and C. Chapdelaine, “Backpropagation-Based Analytical Derivatives of EKF Covariance for Active Sensing. arXiv, February 27, 2024. Accessed: February 28, 2024. [Online]. Available from: http: / / arxiv.org / abs / 2402.17569

[0027] However, backpropagation equations are very specific and limited to a certain type of loss criterion, as well as to the parameters taken into account. Presentation of the invention

[0028] A general object of the invention is to further increase the accuracy of localization systems and to overcome the drawbacks of known systems.

[0029] The invention proposes a method for localizing a moving carrier, in which an extended Kalman filter estimates, during successive iterations, a state of the carrier, one iteration of the filter implementing a. a propagation and recalibration of a carrier state and a location uncertainty as a function of measurements intended to be fused, these measurements being acquired by at least one inertial sensor and / or at least one navigation sensor, the determinations implemented for this propagation and recalibration being a function of different filter parameters, these parameters comprising quantities characteristic of a kinematic model and quantities characteristic of an observation model, b. an adjustment of the filter parameters, this adjustment being implemented by a backpropagation calculation within a time window, this calculation comprising • a determination of the gradients of a loss function with respect to the filter parameters, • an optimization of the parameters that minimize said loss function,

[0030] in which

[0031] The parameters with respect to which the determination of the gradients of the loss function is determined are linked to each other in such a way that knowledge at a time n of the state of a system and of these parameters makes it possible to backpropagate said parameters by analytical calculation to a previous time n-1, these parameters including at least the parameters of the following list of parameters: - Kalman gain, - Jacobian model of observation, - observation, - covariance of the observation, - Jacobian of the dynamic model, - covariance of the dynamic model,

[0032] the parameter quantities thus obtained being used, with on the one hand the propagated carrier state obtained at the end of step a) and on the other hand an update of the measurements by the sensors, in the implementation of the next iteration.

[0033] Such treatment has the advantage of taking into account the impact of the point where the linearization of the EKF filter is implemented and of refining the parameters of the extended Kalman filter.

[0034] The accuracy of the localization system is thereby increased.

[0035] Thus, the method makes it possible to deal with situations in which the observation model is not valid, for example: - when attempting to implement an inertia / odometry fusion on different terrains - or when the covariances (of the dynamics and / or the observation) are not known; - or also, in the case of sensors that may be very noisy and difficult to use, in particular odometers where the measured speed differs from the true speed because of slippage.

[0036] The proposed method also has the advantage of using analytical calculations and not requiring complex calculations.

[0037] It allows for short calculation times.

[0038] The proposed method is advantageously complemented by the following various features taken individually or in technically feasible combinations:

[0039] - the parameters with respect to which the determination of the gradients of the function of The loss is determined at a time n and includes the parameters from the following list of parameters:

[0040] - estimated state at time n

[0041] - estimation of the correction at time n,

[0042] - Kalman gain at time n,

[0043] - corrected covariance at time n,

[0044] - Covariance of innovation,

[0045] - observation matrix at time n,

[0046] covariance of the estimate; - Backpropagation on filter parameters includes the use • of a relation (1) which links the gradient of the loss function with respect to the state estimated at time n with the gradient of the loss function with respect to the correction term applied to the estimated state; • of a relation (2) which links the gradient of the loss function with respect to the Kalman gain at time n with • the gradient of the loss function with respect to the corrected covariance at time n and • the gradient of the loss function with respect to the correction estimate at time n; • of a relation (3) which links the gradient of the loss function with respect to the covariance of innovation with • the gradient of the direct loss function at time n with respect to the covariance of the innovation and • the gradient of the loss function with respect to the Kalman gain;

[0047] - backpropagation on the filter parameters includes the use of a relationship (4) which relates the gradient of the loss function with respect to the Jacobian of the observation matrix with • the gradient of the loss function with respect to the covariance of innovation, • the gradient of the loss function with respect to the corrected covariance at time n, and • the gradient of the loss function with respect to the Kalman gain at time n transposed.

[0048] - backpropagation on the filter parameters includes the use

[0049] - of a relation (5) which relates the gradient of the loss function with respect to the vector observation with • the gradient of the direct loss function at time n with respect to the observation vector and • the gradient of the loss function with respect to the correction estimate at time n; • of a relation (6) which links the gradient of the loss function with respect to the covariance matrix of the observation with • the gradient of the direct loss function at time n with respect to the covariance matrix of the observation and • the gradient of the loss function with respect to the covariance of the innovation; • of a relation (7) which links the gradient of the loss function with respect to the covariance of the system state at time n before correction with • the gradient of the direct loss function with respect to the covariance of the system state at time n before correction, • the gradient of the loss function with respect to the covariance of the innovation at time n, • the gradient of the loss function with respect to the Kalman gain at time n.

[0050] - backpropagation on the filter parameters includes the use of relations (8), (9) for calculating the gradients of the loss function with respect to the Jacobians of the observation model and the dynamic model with respect to the system state and the use of a relation (10) which links the gradient of the loss function with respect to the noise covariance with the gradient of the loss function with respect to the covariance matrix of the system state estimate at time n before correction,

[0051] - the gradient of the loss function with respect to the system state component before correction to other quantities, as well as relations (12), (13) for the determination of the gradient of the loss function with respect to the corrected covariance, as well as the determination of the gradient of the loss function with respect to the corrected state at state n-1.

[0052] The invention further relates to a computer program product comprising code instructions for executing the steps of the process, when this program is executed by a computer.

[0053] It also relates to a device for locating a mobile carrier, the device comprising:

[0054] - a receiving interface configured to receive measurements acquired by the minus one inertial sensor and / or at least one navigation sensor,

[0055] - a processor configured to implement a location determination of the carrier, the device being configured to implement the steps of the process

[0056] It also relates to a navigation unit for a mobile carrier, comprising:

[0057] - an inertial measurement unit,

[0058] - sensors (GPS, odometer, ...) Presentation of the figures

[0059] Other features, purposes and advantages of the invention will become apparent from the following description, which is purely illustrative and not limiting, and which should be read in conjunction with the accompanying drawings on which:

[0060] [Fig.1] is a schematic representation of a mobile carrier;

[0061] [Fig.2] is a schematic representation of a position tracking device;

[0062] [Fig. 3] is a schematic representation of a position tracking method by iteration of an EKF filter;

[0063] [Fig.4] illustrates the principle of adaptive filtering;

[0064] Fig. 5 illustrates the relationships between backpropagated parameters in a possible implementation of the invention. Detailed description

[0065] Mobile carrier location / navigation system

[0066] With reference to [Fig. 1], a carrier A is mobile in an inertial frame Ri. The carrier A is here a ship but can alternatively be an aircraft, or a land vehicle and more generally any moving object.

[0067] An example of an inertial frame of reference Ri is the frame centered on the center of the Earth, whose z-axis points towards the North Pole, whose x-axis points towards the intersection of the Greenwich meridian and the equator at time t=0 (the point thus defined will then move in our frame of reference because of the rotation of the Earth) and whose y-axis points in the direction of the vector zxx, x denoting the cross product).

[0068] Figure 1 also shows a frame of reference Rp attached to the carrier A. Rp is a measurement frame in the following. The axes of this frame are conventionally directed forward, to the right, and downward from the center of gravity. Its origin is a fixed point on the carrier.

[0069] As illustrated in [Fig.2], the mobile carrier includes a navigation unit 1 which is fixed to the structure of the carrier so that the navigation unit 1 is stationary relative to the mobile carrier.

[0070] The navigation system 1 includes an inertial measurement unit 11 and sensors 12.

[0071] An inertial measurement unit 11 is also referred to by the English expression "Inertial Measurement Unit" abbreviated as IMU. The inertial measurement unit comprises three gyroscopes measuring the three components of an angular velocity (in radians per second) of the moving carrier and three accelerometers configured to estimate the three components of a metric acceleration (in meters per second squared) of the carrier.

[0072] The sensors 12 or complementary sensors are for example a satellite geopositioning system (also known by the English expression "Global Positioning System" abbreviated as GPS) or an odometer or a linear speed sensor.

[0073] The navigation system 1 further includes a processor 20 with a receiving interface 21 configured to receive measurements acquired by the inertial measurement unit 11 and the sensors 12.

[0074] The processor 20 is configured to implement an extended Kalman filter (usually referred to by the acronym EKF in the literature). The EKF extended Kalman filter is an algorithm that can be coded as a computer program executable by the processor 20.

[0075] An output 23 allows output data calculated by the processor 20 to be delivered. Extended Kalman filter

[0076] An EKF extended Kalman filter is a recursive estimator of a state representative of the mobile carrier's navigation.

[0077] This state includes at least one carrier navigation variable (position, speed, acceleration, orientation, etc.).

[0078] The navigation state can in any case be represented in the form of a vector in which each component is a navigation variable of the carrier.

[0079] We will consider in the following an embodiment in which the navigation state includes in particular the following navigation variables: • a 3-dimensional position vector of the carrier, • a 3-dimensional velocity vector of the carrier, • A carrier orientation matrix, defined as the rotation matrix allowing the change of reference frame from the carrier's frame to an inertial frame. The inertial frame can, for example, be a terrestrial frame centered on the center of the Earth whose z-axis points towards the North Pole, whose x-axis points towards the intersection of the Greenwich meridian and the equator at time t=0 (the point thus defined will then move in our frame due to the Earth's rotation) and whose y-axis points in the direction of the vector zxx, x denoting the cross product).

[0080] The navigation state may also include additional variables which may be, for example, biases associated with the operation of accelerometers and gyroscopes (vectors of dimension 3), lever arms between the IMU and complementary sensors (vectors of dimension 3).

[0081] The system evolves according to the numerical model and, therefore, we can consider that the state xn of the target object at a given time n depends on the state xn-1 of the previous instant

[0082] Thus, at a given step n, the system is described by the following model, of which the The first equation describes the dynamics of the carrier (navigation equation), and whose The second equation describes the observation model:

[0083]

[0084] with the state of the system, one of the increments of the dynamics (for example, accelerations and angular velocities), Yn observations made from other sensors that measure inertia (for example, odometers), w u ~N(0, Qj a Gaussian noise of zero expectation and covariance matrix Qn, and £n~ Af(0, R^ a Gaussian noise of zero expectation and covariance matrix Rn.

[0085] In many applications, state, increments, observations, and noise are expressed in terms of a plurality of variables, so that wn, £n constitute vectors.

[0086] The dynamic model is described by the function f, which determines how the system evolves over time under the influence of a control. The fact that this evolution is not exact is modeled by adding a noise term.

[0087] The observation model is described by the function h which relates the state of the system xn to what is observed.

[0088] Implementation of EKF filtering by processor 20

[0089] The extended Kalman filter is implemented on the microprocessor 20 at each iteration n.

[0090] The filter is initialized with an initial state, which will serve as input for a first iteration of the filter. Each subsequent iteration of the filter takes as input a state estimated by a previous iteration of the filter, and provides a new estimate of the carrier state.

[0091] With reference to [Fig.3], an iteration of the extended Kalman filter classically comprises three steps: a linearization 100, a propagation 300, and a correction 500.

[0092] The linearization step 100 is implemented in a way known per se (expansion to order 1 at a given point of the vector space associated with the chosen state vector).

[0093] In this regard, reference may be made to the following publication

[0094] TEREJANU, Gabriel A., et al. Extended Kalman Filter Tutorial. University at Buffalo, 2008, vol. 27.

[0095] This step 100 uses a previous state as a linearization point, so as to approximate the propagation and observation functions by linear functions. These linear functions are then used during the propagation 300 and update 500 steps.

[0096] The propagation step 300 determines a propagated state of the carrier from the previous state of the carrier (or the initial state for the first iteration), using the linearized propagation function. It also produces a covariance matrix P representing an uncertainty in the acquired measurements.

[0097] The propagation equations may involve the primary measurements acquired 200 by the primary sensors 11.

[0098] Secondary measurements are taken by the secondary sensors 12 in step 400, these measurements being expressed in the carrier frame Rp. The measurements are received by the interface 21 which transmits them to the processor 20 for processing.

[0099] In the update step 500, the processor 20 corrects the propagated state x produced by the propagation step 300, by means of the linearized observation function and using the secondary measurements provided by the secondary sensors 12 in the step 400. The updated state produced by this step 500 is called x in the following.

[0100] The updated state is provided on output 23.

[0101] It is noted that, in the implementation of the extended Kalman filter considered here, two types of measurement take place respectively at the propagation 300 and update 500 stages (the measurements provided by the primary 11 and secondary 12 sensors).

[0102] This operation is typical of a hybrid power plant, in which secondary measurements are used to consolidate primary measurements. However, it is possible to foresee that the propagation stage does not use measurements, but only a kinematic model to propagate the previous state to the propagated state x. In this case, only the measurements provided by the secondary sensors 12 are used by the extended Kalman filter during the update 500.

[0103] Steps 100 to 500 are then repeated during each iteration of the extended Kalman filter executed by processor 20.

[0104] Detailed notations and equations:

[0105] The following notations are introduced: - at time n before the correction related to the measurement yn, the estimated state of the system is written: - at time n after the correction related to the measurement Yn, the estimated state of the system is written: x^.

[0106] Furthermore, we denote by Fn the Jacobian of f with respect to the state x, evaluated at and for wn = 0:

[0107] F (y ■ n ni rn ~ dx ( xn-jjn-p ujyuJ-

[0108]

[0109]

[0110] [YES]

[0112]

[0113]

[0114]

[0115]

[0116]

[0117]

[0118]

[0119]

[0120]

[0121]

[0122]

[0123] We also note Gn the Jacobian of f with respect to the noise w, evaluated in and un, for Wn = 0: G n =dw( 1' un, 0 ). Finally, we denote Hn the Jacobian of h evaluated in: TT d ll / \ dx h We also introduce Pn, the covariance matrix of errors on the state xn. State errors can be defined linearly or in a more complex way. In this regard, the following publication may be of particular use: [5] A. Barrau, “Non-linear State error based extended Kalman filters with applications to navigation”. Doctoral thesis, Mines Paristech, 2015. The covariance matrix Pn is estimated jointly with the system state. It gives the uncertainty associated with the state estimate. Similar to the state estimates x^ and X^, we denote: - at time n before the correction related to the measurement Yn, the estimated covariance of the errors on the state of the system is written: P^ai - at time n after the correction related to the measurement Yn, the estimated covariance of the errors on the state of the system is written: P^. To jointly estimate the system state and the covariance associated with the errors, extended Kalman filtering is performed recursively using the following equations: Propagation equations ^njn-l— ^ii' 0)-^n|nl— P nP n-$n.-lP n + GnQnGn The first equation propagates the system state estimate through the equation of motion. The second equation propagates the uncertainty of the system state estimate from time n-1 to time n by linearizing the dynamics. These equations describe how the mean and uncertainty at a given time evolve over time under the influence of the dynamics. Calculation of the innovation zn, the covariance Sa of the innovation and the Kalman gain Kn; zn = yn-h(x]^.1') Sa = H aP jj + Rn P n = Pi^n-1^n The first equation calculates innovation; this quantifies the gap between observation and dynamics.

[0124] The second equation calculates the covariance of the innovation; this quantity characterizes the uncertainty we have about the innovation resulting from the partial measurement of the state. The third equation calculates the Kalman gain. This gain corresponds to the ratio between the uncertainty present in the state propagated by the dynamics and the uncertainty present in the innovation resulting from the measurement. It allows us to trade off the uncertainty arising from the dynamics and the observation.

[0125] Correction of the covariance and estimation of the system state

[0126] ôxn = Knza [°1271 101281

[0129] Using the Kalman gain and an observation, the estimated state is corrected. The influence of this correction on the uncertainty of the system state estimate is then taken into account in the last equation. For the state correction leading to the calculation of x^, the function corresponds to the sum of the two arguments if the associated errors are defined linearly, or a more complex operation in the case where the errors are defined non-linearly. In this regard, reference may be made to the following publication:

[0130] [6] A. Barrau, and S. Bonnabel. The geometry of navigation problems. IEEE Transactions on Automatic Control, vol. 68, no. 2, pp. 689-704, 2023 - See in particular paragraph 38. Filter settings

[0131] The quality of an extended Kalman filter depends strongly on its parameters. The term "parameters" should be understood here in a broad sense, as including all the elements that allow an extended Kalman filter to function (covariances, dynamics model, observation model, etc.).

[0132] The parameters to be optimized, referred to as 0 in what follows, can be the covariance matrices Q and Rn for the inertial-odometer fusion, or the commands for performing an alignment. The Q parameters can also be preprocessing parameters, the inertial increments un, or observations Yn. Finally, they can be parameters of the dynamics model f, or of the observation model h.

[0133] At each iteration n, the following steps are performed: 1. Data collection: retrieval of data acquired by the sensors, of the old filter output and of the current filter parameters. 2. Estimation of the system state: the extended Kalman filter is used to estimate the current state of the system as described above. 3. Parameter adjustment: The filter parameters are adjusted to minimize the loss function. The optimization step must be very quick; in this case, it is implemented by calculating the gradient of the loss function and choosing the filter parameters that optimize said function. 4. Return to step 1: the adjusted parameter quantities thus obtained are used as current parameters in the estimation implemented during the next iteration; the estimation of the propagated carrier state obtained at the end of step 3) is the output of the filter used in step 1 of this new iteration. Loss function

[0134] For these steps, and as illustrated in Figure 4, the adaptation of the filter parameters is done by choosing parameters 9 which optimize a loss function (minimization of an error value or compliance with loss conditions).

[0135] The initialization of the filter parameters and in particular the noise covariance matrices is done using default values ​​or initial estimates based on prior knowledge of the system.

[0136] The adaptation treatment uses a loss function (or more generally a loss criterion) which depends directly or indirectly on the parameters that one seeks to estimate.

[0137] In practice, the estimation is done instantaneously on the microprocessor 20. The loss function can be an error function with respect to a ground truth, the negative log-likelihood (which has the advantage of not requiring ground truth), or any other function relevant to the application.

[0138] In the following example, the loss / criterion function is written in the following form:

[0139] A-VV /

[0140] with ln a scalar function that depends on the quantities involved in the Kalman filter at time n.

[0141] The adaptation then consists of determining the parameters 9 that minimize the loss function. The adaptation problem can then be written as:

[0142] = arg mjnA

[0143] where "arg min" is the minimum argument and represents the value of the variable for which the value of the function in question (in this case A) reaches its minimum.

[0144] To solve this optimization problem, it is necessary to know the gradient of the loss / criterion function with respect to the parameters 9.

[0145] Once the gradient is calculated by the microprocessor 20, a first-order optimization algorithm can be used to reach a local optimum.

[0146] The filter parameters are thus adjusted at each iteration so as to minimize the loss function. Backpropagation of gradients

[0147] The calculation of gradients by the microprocessor 20 is broken down into two steps:

[0148] - the first step is to apply the extended Kalman filter to the new available measurements, with the parameters determined during the previous iteration;

[0149] - in a second step, the microprocessor 20 performs backpropagation and to propagate the gradients in a propagation window going from time n to time n-1.

[0150] This backpropagation step begins with the initialization of dA _ and where we denote N the number of discretized instants that the The time window over which optimization is performed. This initialization of backpropagation depends on the choice of the loss function / criterion.

[0151] The discretized moments are those in which the iterations take place.

[0152] Next, using recursively the equations below, the microprocessor calculates the gradient of the loss / criterion function with respect to the different variables of interest.

[0153] In one possible implementation, for example, the microprocessor 20 calculates ...: these quantities are relevant depending on the parameters we want dRn dHa oy n vary.

[0154] The relationships which allow backpropagation of gradients in time are typically the following equations.

[0155] Equation 1 - gradient of the loss function with respect to the state estimated at time n with the gradient of the loss function with respect to the correction term applied

[0156] 1A / ml \ LJ — II _ 14 ! 36¾ 3¼ l1 /

[0157] This first equation relates the gradient of the loss function with respect to the state estimated at time n to the gradient of the loss function with respect to the correction term applied. The two terms are related by f, the Jacobian of with respect to J dx' at the end of the correction process.

[0158] This equation reflects the fact that a small perturbation of the correction term applied at time n to the loss function leads to a variation in the estimation of the state weighted by the Jacobian.

[0159] Equation 2 - gradient of the loss function with respect to the Kalman gain at time n with the gradient of the loss function with respect to the corrected covariance at time n and the gradient of the loss function with respect to the correction estimate at time n

[0160] jjA_ _ _ dA p zjT,_dA_

[0161] The above equation relates the gradient of the loss function with respect to the Kalman gain at time n to the gradient of the loss function with respect to the corrected covariance at time n and the gradient of the loss function with respect to the correction estimate at time n. This equation can also be understood in terms of perturbations. In this case, this equation shows that a perturbation of the Kalman gain on the loss function is the weighted sum of the perturbations of the loss function as the covariance n and the correction term vary.

[0162] Equation 3 - variation of the covariance of innovation and loss function as the weighted sum of the direct influence on the loss function and the influence on the Kalman gain.

[0163] dA _ dln ç-lrr p dA ç-l / ça dSn — dST1 ~ - The equation above that relates the gradient of the loss function to the covariance of innovation with • the gradient of the direct loss function ln at time n with respect to the covariance of the innovation and • the gradient of the loss function with respect to the Kalman gain.

[0164] It describes how a small variation in the covariance of the innovation affects the loss function as the weighted sum of the direct influence on the loss function and the influence on the Kalman gain.

[0165] Equation 4 - impact of a variation of the Jacobian of the observation matrix at time n on the loss function.

[0166] 3A / 3A _> 1 tt tj 9A tj. <-»-1 3A tj ( a\ 0H~a = te +

[0167] Similarly, the above equation allows us to calculate the impact of a variation of the Jacobian of the observation matrix (linearized version of the observation model) at time n on the loss function.

[0168] It relates the gradient of the loss function with respect to the Jacobian of the observation matrix with • the gradient of the loss function with respect to the covariance of innovation, • the gradient of the loss function with respect to the corrected covariance at time n, and • the gradient of the loss function with respect to the Kalman gain at time n transposed,

[0169] Equation 5 - variation of the observation and corrected state weighted by the Kalman gain

[0170] dA _ dlB dA LA dyn dya ~^-^-ndÔXn VJ

[0171] The above equation allows the gradient of the loss function with respect to the observation vector to be calculated.

[0172] It relates the gradient of the loss function with respect to the observation vector to • the gradient of the direct loss function ln at time n with respect to the observation vector and • the gradient of the loss function with respect to the correction estimate at time n.

[0173] It reflects the fact that a small variation in the observation has an influence on the corrected state weighted by the Kalman gain.

[0174] Equation 6 - loss function and covariance matrix of the observation

[0175] dA _ Mn , dA IA 0Rn dRn "T dSn

[0176] The calculation of the gradient of the loss function with respect to the covariance matrix of the observation is obtained by the above equation.

[0177] It relates the gradient of the loss function with respect to the covariance matrix of the observation with • the gradient of the direct loss function at time n with respect to the covariance matrix of the observation and • the gradient of the loss function with respect to the covariance of the innovation;

[0178] Equation 7 - gradient of the loss function with respect to the covariance matrix of the system state estimate at time n before correction

[0179] dA _ dln T JT TT \ T dA । ttT dA tt , dA dp, , — 3F i , + dP, . + os dK„ I j qzi-1 njn-1 qjt-1 \ /

[0180] This equation allows the microprocessor 20 to calculate the gradient of the loss function with respect to the covariance matrix of the estimation of the state of the system at the time before correction.

[0181] It relates the gradient of the loss function with respect to the covariance of the system state at time n before correction with • the gradient of the direct loss function with respect to the covariance of the system state at time n before correction, the gradient of the loss function with respect to the covariance of the innovation at time n, the gradient of the loss function with respect to the Kalman gain at time n

[0182] Equations 8 and 9 - gradient of the loss function with respect to the Jacobians Fa and G.

[0183] _aA_ _ / _£A_, _ta_ ï pp / oj

[0184] 7A _ / dA . 7AZ j dGn ~ dP,^ + dPp, /

[0185] The two equations above allow the microprocessor 20 to calculate the gradient of the loss function with respect to the Jacobians Fn and Gn. These equations describe how a variation in the Jacobians impacts the covariance matrix before the correction step.

[0186] Equation 10 - the influence of the covariance of the model noise on the loss function.

[0187] _ 11L . 3A z-, / 1 rd dQn - ôQn + M j

[0188] Using the above equation, the microprocessor can calculate the influence of the covariance of the model noise on the loss function.

[0189] It relates the gradient of the loss function with respect to the noise covariance to the gradient of the loss function with respect to the covariance matrix of the system state estimate at time n before correction,

[0190] In what follows, we denote the component of a vector by superscript. For example, the j-th component of the vector v is denoted yJ. Furthermore, we denote ^k the elementary vector composed of only 0s except for component k, which is equal to 1.

[0191] ___Ib-jr H p JHL. + fraCpl^TJ!E-\+frarpl^Td5&-\+[ fPp fl i) a6Xn + ti ace j aHn j + trace^ ,Kj; j tj

[0192] Using the above equation, the microprocessor 20 calculates the gradient of the loss function with respect to each component of the system state before correction. This equation is used for each component of the system state. Here, we introduce the trace(M) operator, which denotes the sum of the diagonal elements of the matrix M.

[0193]

[0194] Finally, as long as b > 0, the microprocessor 20 calculates the gradient of the loss function with respect to the corrected covariance as well as the gradient of the loss function with respect to the corrected state in state n-1. = '"A""1- + traceljp- 1 + trace-p"-

[0195] aA _ d]^ taa PL dP^ dP^,. rnrn

[0196] The set of nesting relationships between backpropagated parameters is represented in the graph in [Fig.5].

[0197] From the magnitudes of these parameters, it is possible for the microprocessor 20, thanks to the equations above, to backpropagate the gradients of each of them.

[0198] Other backpropagation equations are of course conceivable, especially depending on the parameters considered.

[0199] If parameter 0 appears directly in the extended Kalman filter, then the microprocessor 20 can directly use the gradient to perform the optimization. In the case where parameter 3 appears implicitly via a function Aü3, the microprocessor 20 is programmed to use the chain rule and calculate the gradient of the loss / criterion function with respect to the parameter according to the following equation:

[0200] SA _ xçN dAn dA dO ^22=1 30 dAn

[0201] Then, it uses the gradients calculated with the previous equations to evaluate the above expression.

[0202] The case of the implicitly used parameter arises for example if we want to find a preprocessing of the data.

Claims

1. Demands A method for locating a moving carrier, in which an extended Kalman filter (EKF) estimates a carrier state over successive iterations, with one iteration of the filter (EKF) implementing a. a propagation (300) and a recalibration (500) of a carrier state and a location uncertainty as a function of measurements intended to be fused, these measurements being acquired by at least one inertial sensor and / or at least one navigation sensor, the determinations implemented for this propagation and this recalibration being a function of different filter parameters, these parameters comprising quantities characteristic of a dynamic model and quantities characteristic of an observational model, b. an adjustment of the filter parameters, this adjustment being implemented by a backpropagation calculation within a time window, this calculation comprising • a determination of the gradients of a loss function with respect to the filter parameters, • an optimization of the parameters that minimize said loss function, in which The parameters with respect to which the determination of the gradients of the loss function is determined are linked to each other in such a way that knowledge at a time n of the state of a system and of these parameters makes it possible to backpropagate these parameters by analytical calculation to a previous time n-1, these parameters comprising at least the following list: - Kalman gain, - Jacobian model of observation, - observation, - covariance of the observation, - Jacobian of the dynamic model, - covariance of the dynamic model, the parameter quantities thus obtained being used, with on the one hand the propagated carrier state obtained at the end of step a) and on the other hand an update of the measurements by the sensors, in the implementation of the next iteration.

2. A method according to claim 1 wherein the parameters with respect to which the determination of the gradients of the loss function is determined at time n include the parameters from the following list of parameters: - estimated state at time n - estimation of the correction at time n, - Kalman gain at time n, - corrected covariance at time n, - innovation covariance, - observation matrix at time n, estimation covariance.

3. A method according to claim 2 wherein the backpropagation on the filter parameters comprises the use of - a relation (1) which links the gradient of the loss function with respect to the estimated state at time n with the gradient of the loss function with respect to the correction term applied to the estimated state; - a relation (2) which links the gradient of the loss function with respect to the Kalman gain at time n with • the gradient of the loss function with respect to the corrected covariance at time n and • the gradient of the loss function with respect to the estimate of the correction at time n; - a relation (3) which links the gradient of the loss function with respect to the innovation covariance with • the gradient of the direct loss function at time n with respect to the innovation covariance and • the gradient of the loss function with respect to the Kalman gain.

4. A method according to claim 3 wherein the backpropagation on the filter parameters comprises the use of a relation (4) which relates the gradient of the loss function with respect to the Jacobian of the observation matrix with • the gradient of the loss function with respect to the covariance of the innovation, • the gradient of the loss function with respect to the corrected covariance at time n, and • the gradient of the loss function with respect to the Kalman gain at time n transposed.

5. A method according to claim 4 wherein the backpropagation on the filter parameters comprises the use of - a relation (5) which relates the gradient of the loss function with respect to the observation vector with • the gradient of the direct loss function at time n with respect to the observation vector and • the gradient of the loss function with respect to the correction estimate at time n; • a relation (6) which relates the gradient of the loss function with respect to the covariance matrix of the observation with • the gradient of the direct loss function at time n with respect to the covariance matrix of the observation and • the gradient of the loss function with respect to the covariance of the innovation;• of a relation (7) which links the gradient of the loss function with respect to the covariance of the state of the system at time n before correction with • the gradient of the direct loss function with respect to the covariance of the state of the system at time n before correction, • the gradient of the loss function with respect to the covariance of the innovation at time n, • the gradient of the loss function with respect to the Kalman gain at time n.;

6. A method according to claim 5 wherein backpropagation on the filter parameters comprises the use of relations (8), (9) for calculating the gradients of the loss function with respect to to the Jacobians of the observation model and the dynamic model with respect to the state of the system and the use of a relation (10) which links the gradient of the loss function with respect to the noise covariance with the gradient of the loss function with respect to the covariance matrix of the estimation of the state of the system at time n before correction,

7. A method according to claim 6, wherein the gradient of the loss function with respect to the component of the system state before correction for other quantities, the gradient of the loss function with respect to the corrected covariance, and the gradient of the loss function with respect to the state corrected to state n-1 are calculated.

8. 11 1. Product computer program comprising program code instructions for carrying out the steps of the process according to any one of the preceding claims, when such program is executed by a computer.

9. A mobile carrier localization system comprising: - a receiving interface configured to receive measurements acquired by at least one inertial sensor and / or at least one navigation sensor, - a processor configured to implement a determination of the carrier's location, the system being configured to implement the following steps according to any one of claims 1 to 7.

10. System according to claim 9, further comprising: - an inertial unit, - sensors.

Images