Method for determining a dynamic alignment trajectory for the estimation by extended Kalman filtering of the state of a carrier

Abstract

Method for determining a dynamic alignment trajectory (P) of a carrier by extended Kalman filtering, comprising the steps: - (S3) determining a first gradient of a scalar with respect to pilot commands, being a final covariance matrix, the determination comprising: - a calculation of a second gradient of the scalar with respect to transition matrices from point n-1 to point n, - a calculation of a third gradient of the scalar with respect to action matrices of a noise in the transition from point n-1 to point n, and - a calculation of a fourth gradient of the scalar with respect to an observation matrix of the state at point n, the determination of the first gradient comprising a backpropagation of the first, second, third and fourth gradients, - (S5) determining a sequence of commands which minimizes the scalar and operational constraints, and - (S6) determining the trajectory.Figure to be published for the abstract: Figure 2.

Description

Title of the invention: Method for determining a dynamic alignment trajectory for the estimation by extended Kalman filtering of the state of a carrier DOMAIN

[0001] The invention relates to a method for determining a trajectory of a dynamic alignment to improve the estimation by extended Kalman filter of the state of a carrier, in particular a carrier equipped with an inertial unit and other sensors. STATE OF THE ART

[0002] The use of extended Kalman filtering is known for estimating the state of a carrier or a dynamic system equipped with an inertial measurement unit (also known as an Inertial Measurement Unit abbreviated as IMU) and other sensors such as a satellite geo-positioning system (also known as a Global Positioning System abbreviated as GPS) or odometers. The uncertainty of the estimates during navigation depends strongly on the initial uncertainty about the state of the system. In order to reduce this, it is possible to carry out a dynamic alignment, i.e. to carry out a maneuver of the carrier along a particular trajectory during which relevant information is acquired.

[0003] Known methods for determining the trajectory have various limitations, such as the sole application to land vehicles (i.e., flat systems), thus excluding more complex aerodynamic systems such as aeronautical systems. Another common limitation is a high computational complexity that does not allow solving an optimization problem over only a few discretization steps.

[0004] There is therefore a need for a method for determining an optimized trajectory for dynamic alignment applicable to an aeronautical system and not requiring excessive computing time. EXPOSED

[0005] An aim of the present presentation is to propose a method for determining an optimized trajectory for a dynamic alignment applicable to an aeronautical system and requiring less calculation time than in the prior art.

[0006] The aim is achieved by means of a method for determining a dynamic alignment trajectory of a mobile carrier followed by extended Kalman filtering, the method comprising the following steps:

[0007]

[0008]

[0009]

[0010]

[0011]

[0012]

[0013]

[0014]

[0015]

[0016]

[0017]

[0018]

[0019]

[0020] - determination of an initial covariance matrix of an initial estimate of a state of the carrier's movement at the start of the trajectory, the initial covariance matrix representing an uncertainty of the initial estimate, - determination of carrier navigation equations from measurements provided by an inertial unit and carrier sensors, - determination of a first gradient of a scalar L(P^ with respect to carrier piloting commands, the scalar LiP^ being constructed from a final covariance matrix Pp of a final estimate of the state of the carrier's movement at the end of the trajectory, the scalar L(P^ representing an uncertainty of the final estimate, the first gradient representing an influence of the piloting controls on the uncertainty of the final estimate, the trajectory being discretized according to a plurality of points n, the determination of the first gradient including for each point n: - a calculation of a second gradient of the scalar L(Pf} with respect to transition matrices from an estimated value xn-i of the carrier state at point n-1 to an estimated value xn of the carrier state at point n, - a calculation of a third gradient of the scalar LiP^ with respect to action matrices of a noise in the transition from the estimated value to the estimated value And - a calculation of a fourth gradient of the scalar L(P^ With respect to an observation matrix of the state of the carrier at point n, determining the first gradient comprising using back-propagation equations of the first gradient, the second gradient, the third gradient and the fourth gradient, - determination of a gradient of operational constraints of the carrier in relation to the carrier controls, - determination of a sequence of commands which minimizes a sum of the scalar L(P^ and the operational constraints, the determination using a projected gradient descent method, and - determination of the trajectory from the sequence of commands. Such a method is advantageously and optionally supplemented by the following different characteristics taken alone or in combination: - the determination of the first gradient includes the use of the following equations: sf„

[0021] dUP J f ' _ O lr r' dG„ ~

[0022]

[0023]

[0024]

[0025]

[0026] in which Fn denotes the transition matrix from an estimated value of the carrier state at point n-1 to an estimated value x» of the carrier state at point n, denotes the action matrix of a noise on the estimated value xn, the noise being a Gaussian white noise of zero expectation and covariance matrix Q , P is the covariance matrix of the estimated value xn which takes into account an observation made at point n, P is the covariance matrix of the estimated value xn which does not take into account the observation made at point n, H„ denotes an observation matrix of the estimated value x» and denotes a Gaussian measurement noise of zero expectation and covariance matrix Rft; - the determination of the first gradient of the scalar LiP^ includes a step of determining for each point n a fifth gradient of the scalar LiP^ with respect to the components j of the estimated value xn and noted xi; - the determination of the fifth gradient includes a backpropagation of the values ​​of the partial derivative of the scalar as a function of the components j of the estimated value G and noted; - the determination of the partial derivatives of the scalar L(P^ as a function of the matrices P includes a backpropagation of the covariance matrices P from the partial derivative of the scalar UP^ as a function of the matrix Pp;

[0027] - the determination of the first gradient comprises a determination step, for each point n of the trajectory, of partial derivatives MG) of the scalar ipp j in r function of components li^ of the command un at point n according to the following relation 100281

[0029] in which is a Jacobian of a propagation function f with respect to the controls so that _ 4Ç, the propagation function f allowing Jn~3Un to estimate the orientation, speed and position of the carrier at point n+1, from the orientation, speed and position of the carrier at point n, the commands at time n and the noise at time n, and ei is a vector whose components are all zero except the j-th which is 1; and

[0030] - the scalar IJjP^ is the trace or the largest eigenvalue of the matrix Pp.

[0031] The disclosure further relates to a method for navigating a carrier using an extended Kalman filter estimating a navigation state of the carrier, the method comprising the following steps:

[0032] - determination of a trajectory of a dynamic alignment such as has just been to present,

[0033] - piloting the carrier so that the carrier follows the trajectory,

[0034] - measurement record of the inertial unit and the carrier's sensors during the path,

[0035] - determination of the covariance matrix Pp of the carrier states at the end of tra jectory, and

[0036] - replacement of the initial matrix Po of covariance of the states of the carrier by the Pp matrix.

[0037] The disclosure also relates to a computer program product comprising program code instructions for executing the steps of one of the methods just described, when this program is executed by a computer.

[0038] The disclosure also relates to a device for tracking the trajectory of a mobile carrier, the device comprising:

[0039] - a receiving interface configured to receive measurements acquired by a inertial unit and sensors,

[0040] - a processor configured to implement a determination of a trajectory dynamic alignment of the wearer, the device being configured to implement the following steps:

[0041] - determination of an initial covariance matrix Pq of an initial estimate of a state of the carrier's motion at the start of the trajectory, the initial matrix of co variance representing an uncertainty of the initial estimate,

[0042] - determination of carrier navigation equations from measurements provided by an inertial unit and carrier sensors,

[0043] - determination of a first gradient of a scalar HP^ with respect to carrier piloting commands, the scalar L(P^ being constructed from a final covariance matrix Pp of a final estimate of the state of the movement of the carrier at one end of the trajectory, the scalar representing an uncertainty of the final estimate, the first gradient representing an influence of the piloting controls on the uncertainty of the final estimate,

[0044] the trajectory being discretized according to a plurality of points n,

[0045] the determination of the first gradient comprising for each point n:

[0046] - a calculation of a second gradient of the scalar L(P^ Pæ" with respect to matrices of transition from an estimated value x»-i of the carrier state at point n-1 to a value estimate xn of the state of the carrier at point n,

[0047] - a calculation of a third gradient of the scalar LiP^ with respect to matrices of action of a noise in the transition from the estimated value to the estimated value -¼ and

[0048] - a calculation of a fourth gradient of the scalar LÇP^ with respect to a matrix observation of the state of the carrier at point n,

[0049] determining the first gradient comprising using back-propagation equations of the first gradient, the second gradient, the third gradient and the fourth gradient,

[0050] - determination of a gradient of operational constraints of the carrier with respect to at the carrier's controls,

[0051] - determination of a sequence of commands which minimizes a sum of the scalar L(P^ ct operational constraints, the determination using a projected gradient descent method, and

[0052] - determination of the trajectory from the sequence of commands.

[0053] The presentation finally relates to a navigation center for a mobile carrier, comprising:

[0054] - an inertial unit,

[0055] - sensors, and

[0056] - at least one trajectory tracking device such as has just been presented. DESCRIPTION OF FIGURES

[0057] Other characteristics and advantages will emerge from the following description, which is purely illustrative and non-limiting, and must be read in conjunction with the attached drawings in which:

[0058] [Fig.l] [Fig.l] is a schematic representation of a trajectory tracking device; and

[0059] [Fig.2] [Fig.2] is a schematic representation of a tra tracking method jectory. DETAILED DESCRIPTION OF THE INVENTION Navigation center for mobile carrier

[0060] A mobile carrier can be a land vehicle, a ship or an aircraft.

[0061] The mobile carrier comprises 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.

[0062] With reference to [Fig.l], the navigation unit 1 comprises an inertial unit 11 and 12 sensors.

[0063] An inertial unit 11 is also referred to as an “Inertial Measurement Unit” abbreviated to IMU. The inertial unit comprises three gyrometers measuring the three components of an angular acceleration (in radians per square second) of the mobile carrier and three accelerometers configured to estimate the three components of a metric acceleration (in meters per square second) of the carrier.

[0064] The sensors 12 or complementary sensors 12 are for example a satellite geo-positioning system (also known by the English expression “Global Positioning System” abbreviated to GPS) or an odometer.

[0065] The navigation unit 1 comprises a trajectory tracking device 2 for the mobile carrier. The trajectory tracking device 2 comprises a reception interface 21 configured to receive measurements acquired by the inertial unit 11 and the sensors 12.

[0066] The trajectory tracking device 2 comprises at least one processor 20 configured to implement an extended Kalman filter (generally designated by the acronym EKF in the literature). The extended Kalman filter EKF is an algorithm capable of being coded in the form of a computer program executable by the processor 20.

[0067] The trajectory tracking device 2 further comprises an output 23 for delivering output data calculated by the processor 20. Extended Kalman Filter

[0068] As is known, an extended Kalman filter EKF is a recursive estimator of a state representative of the navigation of the carrier, hereinafter called the navigation state. This state is a state of movement of the mobile carrier.

[0069] This navigation state may comprise at least one navigation variable of the carrier (position, speed, acceleration, orientation, etc.). The navigation state may in any event be represented in the form of a vector, each component of which is a navigation variable of the carrier.

[0070] We consider in the following an embodiment in which the navigation state notably comprises the following navigation variables: 1. a position vector x of the carrier of dimension 3, 2. a velocity vector v of the carrier of dimension 3, 3. An orientation matrix G of the carrier, defined as the rotation matrix allowing the change of reference frame from the carrier's reference frame to an inertial reference frame. The inertial reference frame can for example be a terrestrial reference 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 reference frame because of the rotation of the Earth) and whose y axis points in the direction of the vector zxx, x denoting the vector product).

[0071] The navigation state may also include additional variables which may be, for example, the noise associated with the operation of the accelerometers and gyrometers (vectors of dimension 3), lever arms between 1TMU and the complementary sensors (vectors of dimension 3).

[0072] Method for determining a dynamic alignment trajectory of the carrier mobile

[0073] A trajectory tracking device as just presented makes it possible to implement a method P to determine a dynamic alignment trajectory of the mobile carrier. In relation to [Fig.2], the steps of the method P are described.

[0074] In a first step SI, the localization is initialized with an estimate of the navigation state to which a covariance matrix PO is associated. The covariance matrix represents the uncertainty that is associated with the estimate of the initial state of the carrier.

[0075] The extended Kalman filter EKF 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 state of the carrier. The extended Kalman filter is therefore implemented by successive iterations so as to determine xn which is an estimated value of a state of the carrier at a step n (or point n or iteration n) from x«-i which is an estimated value of a state of the carrier at the previous step n-1 (or previous point n-1 or previous iteration n-1).

[0076] In a second step S2, navigation equations of the inertial unit are determined.

[0077] The dynamics of 1TMU can be locally linearized around zero according to the following equations:

[0078] t Xn = F^ + GnWn ( 1) I yn — Hnxn + sn

[0079] Fn is a transition matrix from to xn,

[0080] Gn is a noise action matrix on xn, with Q ) i.e. the noise wn is a white Gaussian noise of zero expectation and covariance matrix Q ,

[0081] y„ is a measure of the carrier's navigation state at step n,

[0082] Hn is an observation matrix of xn, and

[0083] sn is a measurement noise which is a white Gaussian noise of zero expectation and covariance matrix Rn, in~N(0, Rit).

[0084] It should be noted that white noise corresponds to a character independent of noise by

[0085]

[0086]

[0087]

[0088]

[0089]

[0090]

[0091]

[0092]

[0093]

[0094]

[0095]

[0096] relation to time: two noises at two different times are independent. The first numbered equation (1) represents the transition from xnA to xn. The second numbered equation (2) represents the observation made during the trajectory at step n. This observation depends on the estimated value xn of the state of the system at this step n. Both the transition and the observation are noisy with white Gaussian noises sn and wn respectively, which are all of zero expectation. It should be noted that, given the linearization, the matrices F„ and Gn depend on and on the order The command is the carrier's piloting command that is sent to the carrier's motorization system at point n. The command is a vector, each component of which corresponds to a specific command signal to be transmitted to the carrier to ensure its piloting, for example, command one is a vector comprising three metric accelerations and three angular accelerations. Like the other quantities, the command is discretized. At each step, the uncertainty in the estimate of the carrier state is evaluated. For this, the covariance matrix Pn of the estimated value xn of the carrier state at step n is used. The matrix Pn is called the posterior covariance matrix P^ when it takes into account the observation yn made at point n. It is called the prior covariance matrix P^\ when it does not take into account the observation yn made at point n. The propagation and correction equations of the covariance matrix in extended Kalman filtering are: ' P,^ = + G„Qt Gl (3) S„ = H„P,^Hl + R„ (4) (5) . P^p-KMP,^ (6) The term fF denotes the transpose matrix of the matrix Fn. Equation (3) allows us to calculate the prior covariance matrix Pf^tp without taking into account the observation yn. This covariance estimates the uncertainty on the prior state. Knowing this uncertainty, equation (4) allows us to calculate the observation covariance matrix Sn at step n by combining the a priori uncertainty P^p on the state and the covariance Rn of the measurement noise. Knowing both the prior uncertainty P^pa on the state and the uncertainty Sn on the observation, equation (5) allows us to calculate the matrix Kn, called the Kalman gain. This gain matrix Kn is notably used to update the estimated value of

[0097]

[0098]

[0099] the state of the system by adding the product of the gain matrix with the difference between the actual observation and the observation estimated from the a priori estimate of the state (i.e. without knowledge of the observation). Following this update of the estimated value of the state, equation (6) makes it possible to calculate a new covariance P„^ of the estimated value of the updated state, taking into account the previous covariance and the gain. The trajectory sought to perform the dynamic alignment is discretized according to a plurality of points n (or steps n). 0 designates the initial point of the curve and F designates the end point of the trajectory In a third step S3, the gradient of a scalar L(P ] is determined with respect to the carrier's piloting commands, this gradient quantifying the variation of when the vary. The scalar LiP^ being constructed from a covariance matrix PF of an estimated value of the carrier's state at the end of the trajectory and a scalar function L.

[0100] The scalar function L can in particular be the trace or the largest eigenvalue of the matrix Pp.

[0101] The quantity L(P^ makes it possible to represent the uncertainty of an estimate of the state of the carrier by Kalman filtering extended by a scalar.

[0102] The gradient of the scalar L^P^ is calculated with respect to the commands un for piloting the carrier throughout the trajectory, that is to say that for each step n of the trajectory and for each component U'n of the command , we determine the partial derivative ^Up^ , H.

[0103] The gradient of the scalar HP^ Pæ" relative to the carrier piloting commands is a first gradient of the scalar LiP^-

[0104] The determination of this first gradient comprises for each point n a calculation of a second gradient of the scalar L(P^ Pæ" relative to the transition matrices of the estimated value x«-i of the state of the carrier at point n-1 towards the estimated value of the state of the carrier at point n. The transition matrices are the matrices For each step n of the trajectory, the term iïp] is determined. dF„

[0105] The determination of the first gradient comprises for each point n a calculation of a third gradient of the scalar L(P^ Pæ" relative to action matrices of a noise in the transition from the estimated value xn-\ to the estimated value The action matrices of the noise are the matrices Gn. For each step n of the trajectory, we determine the term •

[0106] The determination of the first gradient comprises for each point n a calculation of a fourth gradient of the scalar L(P^ with respect to an observation matrix of the state of the system at point n. The observation matrices are the Hn matrices. For each step n of the trajectory, the term dUP is determined).

[0107] The determination of the first gradient comprises the use of backpropagation equations from the partial derivatives of the scalar as a function of the piloting commands 11 f at the end of the trajectory. In other words, at step n of the trajectory, the terms ^PF) indexed by i are determined on the basis of the terms ^UPp) indexed by i. 34 34^

[0108] The determination of the first gradient also includes the use of backpropagation equations of the second gradient, the third gradient and the fourth gradient from the end of the trajectory. In other words, at step n of the trajectory, the term dppj is determined on the basis of the term ^Up\ the term MPr) on the basis of the èF„ dF„+} dG„ term dL(Pf) and the term dUP^] based on the term ^Fpf). dH„

[0109] By proceeding in this way and in particular by using backpropagation, it is possible to obtain the first gradient of L(Pf) with a less significant calculation time than in the prior art when taking into account the a priori uncertainties of the state of the system noted

[0110] Advantageously, to determine the first gradient of the scalar L^P^, we use the following equations [YES]

[0112] Equation (7) corresponds to the second gradient of the scalar LÇP^ Pæ" with respect to the transition matrices. Equation (8) corresponds to the third gradient of the scalar L(P^ P31 rPort to the action matrices of a noise. Equation (9) corresponds to the fourth gradient of the scalar LÇP^ Relative to the observation matrices.

[0113] The determinations of the expressions of the partial derivatives of the function L(PF) with respect to the matrices Fn, Gn and Hn, as given in equations (7), (8) and (9), are intermediate calculations which make it possible to establish the first gradient of the function L(PF). The matrices Fn and Gn depend on the commands according to a

[0114]

[0115]

[0116]

[0117]

[0118]

[0119]

[0120]

[0121]

[0122]

[0123]

[0124]

[0125]

[0126]

[0127]

[0128]

[0129]

[0130] propagation function denoted f . The Jacobian of f with respect to the orders one (that is, the derivative df„ ) is denoted j“. An example of a propagation function j can be given as follows. The commands are represented by the vector = ( a„ ) where represents the angular velocity given as the instruction (the vector is of size 3), and the acceleration given as the instruction (the vector an is of size 3). So, the noise is represented by the vector MJ where is the noise on the angular velocity (the vector is size 3) and is the noise on the acceleration (the vector Vt^ is of size 3). An example of a function f can then be written as follows: ( i ) fn ( Xn, ) ■ In this equality, M+i represents the orientation of the carrier at time n+1, 1 «+i represents the velocity of the carrier at time n+1, A«+i represents the position of the carrier at time n+1, represents the orientation of the carrier at time n, vn represents the velocity of the carrier at time n, and xn represents the position of the carrier at time n. The propagation function f allows to estimate the orientation ^l+j, the speed v«+i and the position x«+i of the carrier at time n+1, from the orientation Qw, the speed and the position of the carrier at time n, the commands at time n and the noise wn at time n. This equality (Q„+I, v„+1, vn, x„ wn) can be written as form of the following equations: j ^„+1 — 4* ( ^n~ ) + l -J+l ~ Xn "f" where S represents gravity and the time elapsed between point n-1 and point n. For a vector v of size 3, the operator fQ is defined as where is the identity matrix, and 6 = || y || is the norm of the vector v. The term (y) denotes the matrix of size 3*3 written:

[0131]

[0132]

[0133]

[0134]

[0135]

[0136]

[0137]

[0138]

[0139]

[0140] where the scalars vi' v2' are the components of the vector v: / V1 v = 1¼ The determination of the first gradient of the scalar L(P May include a step S3a of determining partial derivatives of the scalar IXP^ as a function of components j of the estimated value xn (we denote these components xi). In other words, the determination of the first gradient of the scalar L(Pf.j includes the step (S3a) of determining mination for each point n of a fifth gradient of the scalar LÇP^ Pæ" relative to the components j of the estimated value xn and noted xJn. The partial derivatives, denoted düj), can be determined in particular by 34, using the relation ^pj / j \ct a backpropagation of the gradient of L( Pp ) with respect to the carrier state In other words, at step n of the trajectory, we determine the term 3Ppf) on the basis of the term dHPF). The partial derivatives, denoted dL(Prj, can be determined using a retro-34 propagation of intermediate calculations (7), (8) and (9). Similarly, the determination of the partial derivatives of the scalar L(P} as a function of the matrices Pet P „41 uses a backpropagation of the gradient of LpP^ with respect to the covariance matrices Pn^ and P^. [ from the partial derivative of the scalar L(P^ as a function of the matrix Pp. For this purpose, the following backpropagation equations can be used: __- pT__Ip in! . TÔLiP.,] . . I \ = (11) WJ, / J Lf„ LJ \ / Wj LLV WJ TT- = ^ TT- VT +zr TT- TT +?r T--TT + ^ / T— M ôx^ j 1 dP» 0:4 J 1 dGl( dxi / l vil ni dxi I l JI dx,-, I with ei which corresponds to the vector whose components are all zero except the j- th which is worth 1. Equations (10) to (12) correspond to the backpropagation of the second gradients of L(PF) with respect to the covariance matrices (covariance matrix P 4.4 a priori and covariance matrix P^ a posteriori) and the state of the system. Equations (10) to (11) link the second gradient of the scalar LÇP^ P31 with respect to the covariance P,^ estimated at time n with the second gradient of the scalar HP^ By relative to the Pn-fo-i covariance estimated at time n-1. This allows a recursive calculation.

[0141] Equation (12) links the gradient of the scalar £(p ) with respect to the component x / ' to FJ the instant n with the gradient of the scalar IAP^ With respect to the component xC^ at time n-1.

[0142] This allows for recursive calculation.

[0143] Equation (12) is enriched with terms taking into account the impact of linearization and whose calculation is allowed by equations (7), (8) and (9).

[0144] The determination of the gradient of the scalar lAP^ with respect to the carrier commands may comprise a step S3b of determining, for each point n of the trajectory, partial derivatives of the scalar of the order one (noted uln) according to the following relation [°145] / daPj P \ J dL(Pj T du» I dF„ dul„ I î 3G„ da’„

[0146] where is the Jacobian of the propagation function fn with respect to the control Ujp) as a function of components i

[0147] In a fourth step S4, a gradient of operational constraints of the carrier is determined with respect to the carrier controls. The operational constraints designate practical conditions for achieving the optimized trajectory. They may, for example, correspond to a maximum energy consumption for achieving the trajectory, or the regularity of the controls applied to achieve the trajectory. These constraints are functions of the controls along the trajectory, i.e. each constraint can be expressed as a function of the variables üi , w2' • • ■ uf . The set of constraints can be denoted AC( .... itF), where C denotes a column vector listing the different constraints and 2 denotes a row vector which contains the weights of each constraint so that AC ( W j, ..., UP ) denotes a scalar which is a linear combination of the different constraints or in other words a sum of the weighted constraints. It is assumed that the gradient of the AC term ( .... liF ) is quick to calculate.

[0148] In a fifth step S5, a sequence of commands is determined which minimizes a sum of the scalar and operational constraints, determining it mination using a projected gradient descent method applied to the gradient of the scalar and to the gradient of the operational constraints.

[0149] This involves solving the following optimization problem S: finding the u\, ..., u'F which minimize L ( PF ) + 26 ( uj, ..., llF ) with V i, iq e K. The problem S can also be written in the form:

[0150] (S) minUt.MpL(Pf) ..., uF) st yi, U; e K

[0151] K corresponds to the set of controls admissible by the mobile carrier, such as for example an acceleration lower than a maximum acceleration between two iterations, a speed lower than a maximum speed, etc.

[0152] Determining the minimum of the sum of the function L(Pp) and the operational constraints as a function of the commands does not require a significant amount of computation time because the gradient of the function L(Pf) and the gradient of the operational constraints can be easily calculated at each gradient descent iteration from the previously determined equations.

[0153] In a sixth step S6, the trajectory is determined from the sequence of commands determined in step S5. Knowledge of the u'^ ■ - ■ » u'n which minimize L(Pn ) + UN ) withV i, u,Æ K makes it possible to find the tra optimized target sought.

[0154] Thus, if the carrier follows this trajectory, it is possible to obtain an estimated value xf of the final state and the covariance matrix PF of the estimated value xf. By construction, the uncertainty associated with the covariance matrix PF is less than the uncertainty associated with the initial covariance matrix Pq of the estimated value of the initial state of the mobile carrier. In this way, by following the optimized trajectory, it is possible to carry out a dynamic alignment which reduces the estimation uncertainty of the extended Kalman filter.

[0155] The method described allows determination of an optimized trajectory for dynamic alignment requiring less calculation time than in the prior art.

[0156] Furthermore, the method is applicable to a larger class of systems. Navigation method of a carrier

[0157] In relation to [Fig.2], a method Q for navigating a carrier comprises all of the steps S1 to S6 of the method P. The method Q further comprises the following steps.

[0158] During a seventh step S7, the carrier is piloted so that the carrier follows the optimized trajectory determined in step S6.

[0159] In an eighth step S8, the measurements of the inertial unit and the sensors of the carrier are recorded during the trajectory.

[0160] During a ninth step S9, the covariance matrix PF of the states of the carrier at the end of the optimized trajectory is determined using an extended Kalman filter.

[0161] In a tenth step S10, the initial covariance matrix Po of the carrier states is replaced by the matrix PF. This concludes the alignment procedure and allows starting the mission with an initial uncertainty on the state of the system which is reduced compared to the previous situation.

Claims

Claims

1. Method for determining a dynamic alignment trajectory (P) of a mobile carrier tracked by extended Kalman filtering, the method comprising the following steps: - (SI) determination of an initial covariance matrix Pq of an initial estimate of a state of the movement of the carrier at the start of the trajectory, the initial covariance matrix representing an uncertainty of the initial estimate, - (S2) determination of carrier navigation equations from measurements provided by an inertial unit and carrier sensors, - (S3) determination of a first gradient of a scalar Pæ" in relation to carrier piloting commands, the scalar HP^ being constructed from a final covariance matrix P f of a final estimate of the state of the carrier's movement at the end of the trajectory, the scalar HP^ representing an uncertainty of the final estimate, the first gradient representing an influence of the piloting commands on the uncertainty of the final estimate, the trajectory being discretized according to a plurality of points n, the determination of the first gradient comprising for each point n: - a calculation of a second gradient of the scalar L(P^ Pæ" relative to transition matrices from an estimated value of the state of the carrier at point n-1 to an estimated value x» of the state of the carrier at point n, - a calculation of a third gradient of the scalar LÇP^ Pæ" relative to action matrices of a noise in the transition of the estimated value towards the estimated value xn, and - a calculation of a fourth gradient of the scalar LÇP^ Pæ" relative to a observation matrix of the carrier state at point n, the determination of the first gradient including a use of backpropagation equations of the first gradient, of the second gradient, of the third gradient and of the fourth gradient, - (S4) determination of a gradient of operational constraints of the carrier in relation to carrier controls, - (S5) determination of a sequence of commands which minimizes a sum of the scalar HP^ and the operational constraints, the determination - (S6) determination of the trajectory from the sequence of commands.

2. The method of claim 1 wherein determining the first gradient comprises using the following equations: dGn “ dP^ ^{P^ T . FJ qp FJ p tj J ni dHl “ ^'àP^t 1 in which F„ denotes the transition matrix from an estimated value of the state of the carrier at point n-1 to an estimated value xn of the state of the carrier at point n, Gn denotes the action matrix of a noise wn on the estimated value xn, the noise being a Gaussian white noise of zero expectation and covariance matrix Q , P n\n is the covariance matrix of the estimated value xn which takes into account an observation made at point n, P is the covariance matrix of the estimated value xn which does not take into account the observation made at point n, Hn denotes an observation matrix of the estimated value xn and £n denotes a Gaussian measurement noise of zero expectation and covariance matrix Rn.

3. Method according to claim 2 in which the determination of the first gradient of the scalar L(P^ comprises a step (S3a) of determining for each point n a fifth gradient of the scalar L(P^ with respect to the components j of the estimated value xn and noted

4. Method according to claim 3 in which the determination of the fifth gradient comprises a backpropagation of the values ​​of the partial derivative of the scalar L(Pp} as a function of the components j of the estimated value xp and noted x^.

5. Method according to any one of claims 2 to 4 in which the determination of the partial derivatives of the scalar HP as a function of the matrices P n\n comprises a backpropagation of the covariance matrices P from the partial derivative of the scalar HP} in function of the matrix Pp.

6. Method according to any one of claims 2 to 5, in which the determination of the first gradient comprises a step (S3b) of determining, for each point n of the trajectory, partial derivatives ^PF) of the scalar p(pj as a function of components 14 of the p) command a at point n according to the following relation. T \ / T \ t 'T / WJ dFn \ dGn \ \ "K" in which is a Jacobian of a propagation function f with respect to the commands so that m _ , the function of Jn~ of a propagation f making it possible to estimate the orientation, the speed and the position of the carrier at point n+1, from the orientation, the speed and the position of the carrier at point n, the commands at time n and the noise at time n, and ei is a vector all the components of which are zero except the j-th which is equal to 1.

7. A method according to any one of claims 1 to 6, wherein the scalar is the trace or largest eigenvalue of the matrix p

8. Pp- Method for navigating (Q) a carrier using an extended Kalman filter estimating a navigation state of the carrier, the method comprising the following steps: - determining a trajectory of a dynamic alignment according to one of claims 1 to 7, - (S7) piloting the carrier so that the carrier follows the trajectory, - (S8) recording measurements from the inertial unit and the sensors of the carrier during the trajectory, - (S9) determining the covariance matrix Pp of the states of the carrier at the end of the trajectory, and - (S 10) replacing the initial covariance matrix Pn of the states of the carrier with the matrix Pp.

9. Computer program product comprising program code instructions for executing the steps of the method according to one of the preceding claims, when this program is executed by a computer.

10. Device for tracking the trajectory of a mobile carrier, the device comprising: - a reception interface configured to receive measurements acquired by an inertial unit and sensors, - a processor configured to implement a determination of a dynamic alignment trajectory of the wearer, the device being configured to implement the following steps: - (SI) determination of an initial covariance matrix Pq of an initial estimate of a state of the movement of the carrier at the start of the trajectory, the initial covariance matrix representing an uncertainty of the initial estimate, - (S2) determination of carrier navigation equations from measurements provided by an inertial unit and carrier sensors, - (S3) determination of a first gradient of a scalar L(P^ Pæ" relative to the carrier's piloting commands, the scalar L(P^ being constructed from a final covariance matrix Pp of a final estimate of the state of the carrier's movement at the end of the trajectory, the scalar LÇP^ representing an uncertainty of the final estimate, the first gradient representing an influence of the piloting commands on the uncertainty of the final estimate, the trajectory being discretized according to a plurality of points n, the determination of the first gradient comprising for each point n: - a calculation of a second gradient of the scalar L(P^ P^ relative to transition matrices from an estimated value of the state of the carrier at point n-1 to an estimated value of the state of the carrier at point n, - a calculation of a third gradient of the scalar LÇP^ relative to action matrices of a noise in the transition from the estimated value 1 towards the estimated value and - a calculation of a fourth gradient of the scalar L{P^ With respect to a observation matrices of the carrier state at point n, determining the first gradient including use of backpropagation equations of the first gradient, of the second gradient, of the third gradient and of the fourth gradient, - (S4) determination of a gradient of operational constraints of the carrier in relation to carrier controls, - (S5) determination of a sequence of commands which minimizes a sum of the scalar HP^ and the operational constraints, the determination - (S6) determination of the trajectory from the sequence of commands.

11. Navigation unit for mobile carrier, comprising: - an inertial unit, - sensors, and - at least one trajectory tracking device according to the preceding claim.

Images