Method for determining a dynamic alignment trajectory for the estimation, by extended Kalman filtering, of the state of a carrier
The method addresses the limitations of existing dynamic alignment techniques by using extended Kalman filtering to determine an optimized trajectory for aeronautical systems, reducing computational complexity and enhancing state estimation accuracy.
Patent Information
- Application Number
- FR2023014901
- Authority / Receiving Office
- FR · FR
- Patent Type
- Patents
- Current Assignee / Owner
- Filing Date
- 2023-12-21
- Publication Date
- 2025-11-28
- Estimated Expiration
- 2043-12-21
AI Technical Summary
Existing methods for determining dynamic alignment trajectories are limited to land vehicles and suffer from high computational complexity, making them unsuitable for aeronautical systems and impractical for real-time applications.
A method using extended Kalman filtering to determine an optimized trajectory for aeronautical systems, involving the determination of gradients and operational constraints, which minimizes uncertainty in carrier state estimation with reduced computational requirements.
The method enables efficient determination of an optimized trajectory for aeronautical systems with reduced computational time, improving state estimation accuracy and applicability to a broader range of mobile carriers.
Smart Images
Figure 00000021_0000 
Figure 00000022_0000
Abstract
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 the 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 measurement unit and other sensors. PRIOR TECHNIQUE
[0002] Extended Kalman filtering is known for estimating the state of a carrier or a dynamic system equipped with an inertial measurement unit (IMU) and other sensors such as a global positioning system (GPS) or odometers. The uncertainty of the estimates during navigation depends strongly on the initial uncertainty in the system's state. To reduce this uncertainty, dynamic alignment can be performed, i.e., maneuvering the carrier along a specific trajectory during which relevant information is acquired.
[0003] Known methods for determining trajectory have various limitations, such as their application only to land vehicles (i.e., flat systems), thus excluding more complex aerodynamic systems such as aircraft. Another common limitation is their high computational complexity, which makes it impossible to solve an optimization problem with only a few discretization steps.
[0004] There is therefore a need for a method of determining an optimized trajectory for dynamic alignment applicable to an aeronautical system and not requiring excessive computation time. EXPOSED
[0005] One aim of the present presentation is to propose a method for determining an optimized trajectory for dynamic alignment applicable to an aeronautical system and requiring less computation time than in the prior art.
[0006] The goal is achieved by means of a method for determining a dynamic alignment trajectory of a moving carrier tracked 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 motion at the beginning of the trajectory, the initial covariance matrix representing an uncertainty in the initial estimate, - Determination of carrier navigation equations from measurements provided by an inertial measurement unit and carrier sensors, - determination of a first gradient of a scalar L(P^ with respect to piloting commands of the carrier, the scalar LiP^ being constructed from a final covariance matrix Pp of a final estimate of the state of the carrier's motion at the end of the trajectory, the scalar L(P^ representing an uncertainty of the final estimate, the first gradient representing the influence of the pilot 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(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 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 carrier state at point n, the determination of the first gradient including the use of backpropagation equations for 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's controls, - determination of a command sequence that minimizes a sum of the scalar L(P^) and operational constraints, the determination using a projected gradient descent method, and - Determining the trajectory from the sequence of commands. Such a process is advantageously and optionally complemented by the following various characteristics, taken alone or in combination: - Determining the first gradient involves using 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 denoted 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 denoted; - 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 includes a determination step, for each point n of the trajectory, of partial derivatives MG) of the scalar ipp j at r function of components li^ of the control 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 commands such 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, etei is a vector whose components are all zero except the j-th, which is equal to 1; and
[0030] - the scalar IJjP^ is the trace or largest eigenvalue of the matrix Pp.
[0031] The exposition further relates to a method for navigating a carrier using an extended Kalman filter estimating a carrier navigation state, the method comprising the following steps:
[0032] - determination of a trajectory of a dynamic alignment such as we have just described to present,
[0033] - piloting the carrier so that the carrier follows the trajectory,
[0034] - recording of measurements from the inertial measurement unit and the carrier sensors during the path,
[0035] - determination of the covariance matrix Pp of the carrier states at the end of the tra jectory, and
[0036] - replacement of the initial covariance matrix Po of the carrier states by the matrix Pp.
[0037] The exposition also relates to a computer program product comprising program code instructions for executing the steps of one of the processes just described, when this program is executed by a computer.
[0038] The presentation 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 measurement unit and sensors,
[0040] - a processor configured to implement trajectory determination dynamic alignment of the carrier, the device being configured to implement the following steps:
[0041] - determination of an initial covariance matrix Pq of an initial estimate of a The state of the carrier's movement at the beginning of the trajectory, the initial matrix of co variance representing an uncertainty in the initial estimate,
[0042] - determination of carrier navigation equations from measurements provided by an inertial measurement unit and sensors on the carrier,
[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 motion of the bearing to an end of the trajectory, the scalar representing an uncertainty of the final estimate, the first gradient representing the influence of the pilot commands 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 bearer's state at point n-1 to a value estimated xn of the state of the bearer at point n,
[0047] - a calculation of a third gradient of the scalar LiP^ with respect to matrices 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 carrier's state at point n,
[0049] the determination of the first gradient including the use of backpropagation equations for 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 controls of the carrier,
[0051] - determination of a sequence of commands that minimizes a sum of the scalar L(P^ ct of 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 system for a mobile carrier, comprising:
[0054] - an inertial measurement unit,
[0055] - sensors, and
[0056] - at least one trajectory tracking device such as has just been presented. DESCRIPTION OF THE FIGURES
[0057] Other features and advantages will become apparent from the following description, which is purely illustrative and not limiting, and should be read in conjunction with the accompanying drawings on which:
[0058] [Fig. 1] [Fig. 1] 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 indifferently a land vehicle, a ship or an aircraft.
[0061] 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.
[0062] With reference to [Fig. 1], the navigation unit 1 comprises an inertial measurement unit 11 and sensors 12.
[0063] 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 acceleration (in radians per second squared) 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.
[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 as GPS) or an odometer.
[0065] The navigation unit 1 includes a trajectory tracking device 2 for the mobile carrier. The trajectory tracking device 2 includes a receiving interface 21 configured to receive measurements acquired by the inertial measurement unit 11 and the sensors 12.
[0066] The trajectory tracking device 2 includes at least one processor 20 configured to implement an extended Kalman filter (generally referred to by the acronym EKF in the literature). The extended Kalman filter EKF is an algorithm that can be coded as a computer program executable by the processor 20.
[0067] The trajectory tracking device 2 further includes 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 representative state of the carrier's navigation, referred to hereafter as the navigation state. This state is a state of motion of the moving carrier.
[0069] This navigation state may include at least one navigation variable of the carrier (position, speed, acceleration, orientation, etc.). The navigation state may in any case be represented as a vector, each component of which is a navigation variable of the carrier.
[0070] We will consider in the following an embodiment in which the navigation state includes in particular 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 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).
[0071] The navigation state may also include additional variables which may be, for example, the noise associated with the operation of the accelerometers and gyroscopes (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 such as the one just described makes it possible to implement a method P to determine a dynamic alignment trajectory of the moving carrier. With reference 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 associated with the estimate of the initial state of the carrier.
[0075] The extended Kalman filter EKF is initialized with an initial state, which serves as input for the 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. The extended Kalman filter is thus implemented by successive iterations so as to determine xn, which is an estimated value of a carrier state at step n (or point n or iteration n), from x'-i, which is an estimated value of a carrier state at the previous step n-1 (or previous point n-1 or previous iteration n-1).
[0076] In a second step S2, navigation equations for the inertial navigation system 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 ) that is to say that the noise wn is a Gaussian white 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 Gaussian white noise of zero expectation and covariance matrix Rn, en~N(0, Rit).
[0084] It should be noted that white noise corresponds to a character independent of the noise by
[0085]
[0086]
[0087]
[0088]
[0089]
[0090]
[0091]
[0092]
[0093]
[0094]
[0095]
[0096] Relationship to time: two noises at two different times are independent. The first numbered equation (1) represents the transition from xnA to xn. The second equation numbered (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. The transition and the observation are both noisy with Gaussian white noise respectively sn and wn which are all of zero expectation. It should be noted that, given the linearization, the matrices F„ and Gn depend on and the control The command is the control command for the carrier, which is sent to the carrier's drive system at point n. The command is a vector, each component of which corresponds to a specific control signal to be transmitted to the carrier to ensure its control; for example, command one is a vector comprising three metric accelerations and three angular accelerations. Like other quantities, the command is discretized. At each step, the uncertainty in the carrier state estimate is evaluated. This is done using the covariance matrix Pn of the estimated value xn of the carrier state at step n. 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 for 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 prior 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 to it the product of the gain matrix with the difference between the actual observation and the observation estimated from the a priori estimation of the state (i.e. without knowledge of the observation). Following this update of the estimated value of the state, equation (6) allows us to calculate a new covariance P„^ of the estimated value of the updated state, taking into account the previous covariance and the gain. The desired trajectory for performing the dynamic alignment is discretized into a plurality of points n (or steps n). 0 denotes the initial point of the curve and F denotes the final point of the trajectory In a third step S3, the gradient of a scalar L(P) is determined with respect to the carrier's control commands; this gradient quantifies the variation of L(P) as the control commands vary. The scalar L(P) is 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^ allows us to represent the uncertainty of an estimation of the state of the carrier by the Kalman filtering extended by a scalar.
[0102] The gradient of the scalar L^P^ cst calculated with respect to the piloting commands of 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æ" with respect to the carrier's piloting commands is a first gradient of the scalar LiP^-
[0104] The determination of this first gradient includes, for each point n, a calculation of a second gradient of the scalar L(P^Pæ" with respect to the transition matrices from the estimated value x«-i of the carrier state at point n-1 to the estimated value of the carrier state 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 includes, for each point n, a calculation of a third gradient of the scalar L(P^Pæ) with respect to noise action matrices in the transition from the estimated value xn to the estimated value. The noise action matrices are the matrices Gn. For each step n of the trajectory, the term is determined. •
[0106] The determination of the first gradient includes, 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 matrices Hn. For each step n of the trajectory, the term dUP is determined.
[0107] The determination of the first gradient involves the use of backpropagation equations from the partial derivatives of the scalar as a function of the end-of-trajectory control commands 11 f. 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 for the second, third, and fourth gradients 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 shorter computation time than in the prior art when taking into account the a priori uncertainties of the state of the system denoted
[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 Transition matrices. Equation (8) corresponds to the third gradient of the scalar L(P^ P31 raPPort to the action matrices of a noise. Equation (9) corresponds to the fourth gradient of the scalar LÇP^ With respect to the observation matrices.
[0113] Determining the expressions for 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 that allow us 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 commands un (i.e. 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 a setpoint (the vector is of size 3), and the acceleration given as a setpoint (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 size 3). An example of a function f can then be written in the following way: ( 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 us to estimate the orientation ^l+j, the velocity v«+i and the position x«+i of the carrier at time n+1, from the orientation Qw, the velocity and 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 in the 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 such that where is the identity matrix, and 6 = || y || is the norm of the vector v. The term (y) denotes the 3x3 matrix written as:
[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) can 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æ" with respect 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 state of the carrier. 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 Pp and P 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 in particular: __- 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- the second 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 (a priori covariance matrix P 4.4 and a posteriori covariance matrix P^) 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^ Par relative to the covariance Pn-fo-i estimated at time n-1. This allows a recursive calculation.
[0141] Equation (12) relates the gradient of the scalar £(p) with respect to the component x / ' to FJ at time n with the gradient of the scalar IAP^ With respect to the component xC^ at time n-1.
[0142] This allows for recursive computation.
[0143] Equation (12) is enriched with terms taking into account the impact of linearization and whose calculation is permitted by equations (7), (8) and (9).
[0144] The determination of the gradient of the scalar lAP^ With respect to the carrier's commands may include a step S3b of determining, for each point n of the trajectory, partial derivatives of the scalar of the command one (denoted 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's controls. The operational constraints refer to 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; that is, each constraint can be expressed as a function of the variables üi, w2', and uf. The set of constraints can be denoted AC(..., itF), where C 1 denotes a column vector listing the different constraints, and 2 denotes a row vector containing the weights of each constraint, such that AC ( W j, ..., UP ) denotes a scalar which is a linear combination of the different constraints, or in other words, a sum of weighted constraints. We assume 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, the determination mination using a projected gradient descent method applied to the scalar gradient and the operational stress gradient.
[0149] The aim is to solve the following optimization problem S: find the ui, ..., u'F that minimize L(PF) + 26(ui, ..., u1F) with Vi, ..., ui ∈ 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 moving carrier, such as for example an acceleration less than a maximum acceleration between two iterations, a regime less than a maximum regime, 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 significant 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. Knowing the u'^ ■ - ■ » u'n that minimize L(Pn ) + UN ) with Vi, u, Æ K allows us to find the tra Optimized jectory sought.
[0154] Thus, if the carrier follows this trajectory, it is possible to obtain an estimated value xf of the final state and the PF covariance matrix of the estimated value xf. By construction, the uncertainty associated with the PF covariance matrix is less than the uncertainty associated with the initial Pq covariance matrix of the estimated value of the initial state of the moving carrier. In this way, by following the optimized trajectory, one can perform a dynamic alignment that reduces the estimation uncertainty of the extended Kalman filter.
[0155] The method described allows for the determination of an optimized trajectory for dynamic alignment requiring less computation time than in the prior art.
[0156] Moreover, the process is applicable to a wider class of systems. Method for navigating a carrier
[0157] In relation to [Fig.2], a method Q for navigating a carrier comprises all the steps SI 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, measurements are taken from the inertial measurement unit and the sensors of the carrier during the trajectory.
[0160] During a ninth step S9, the PF covariance matrix of the carrier states at the end of the optimized trajectory is determined using an extended Kalman filter.
[0161] In a tenth step S10, the initial carrier state covariance matrix Po is replaced by the matrix PF. This concludes the alignment procedure and allows starting the mission with an initial uncertainty about the state of the system which is reduced compared to the previous situation.
Claims
Demands
1. Method for determining a dynamic alignment trajectory (P) of a moving 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 motion of the carrier at the beginning 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 measurement unit and carrier sensors, - (S3) determination of a first gradient of a scalar Pæ" with respect to carrier piloting commands, the scalar HP^ being constructed from a final covariance matrix P f of a final estimate of the carrier's motion state 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, Since the trajectory is discretized according to a plurality of points n, the determination of the first gradient includes, for each point n: - a calculation of a second gradient of the scalar L(P^Pæ) with respect to transition matrices from an estimated value of the carrier state at point n-1 to an estimated value x» of the carrier state at point n, - a calculation of a third gradient of the scalar L⇓P^Pæ with respect to noise action matrices 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æ" with respect 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 the carrier's controls - (S5) determination of a sequence of commands that minimizes a the sum of the HP^ scalar and the operational constraints, the determination - (S6) determination of the trajectory from the sequence of commands.
2. A method according to claim 1 wherein the determination of the first gradient comprises the use of the following equations: dGn “ dP^ ^{P^ T . FJ qp FJ p tj J ni dHl “ ^'àP^t 1 where F„ denotes the transition matrix from an estimated value of the carrier state at point n-1 to an estimated value xn of the carrier state 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 that takes into account an observation made at point n, P is the covariance matrix of the estimated value xn that 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 matrix of covariance Rn.
3. A method according to claim 2 wherein 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 denoted
4. Method according to claim 3 wherein the determination of the fifth gradient includes 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 denoted x^.
5. A method according to any one of claims 2 to 4 wherein the determination of the partial derivatives of the HP scalar as a function of the matrices Pn comprises a backpropagation of the covariance matrices P from the partial derivative of the HP scalar. function of the matrix Pp.
6. A method according to any one of claims 2 to 5, wherein 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 such that m _ , the function of Jn~ dun propagation f allowing estimation of the orientation, velocity and position of the carrier at point n+1, from the orientation, velocity 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.
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 of navigation (Q) of a carrier using an extended Kalman filter estimating a navigation state of the carrier, the method comprising the following steps: - determination of a trajectory of a dynamic alignment according to any one of claims 1 to 7, - (S7) steering of the carrier so that the carrier follows the trajectory, - (S8) recording of measurements of the inertial measurement unit and of the sensors of the carrier during the trajectory, - (S9) determination of the matrix Pp of covariance of the states of the carrier at the end of the trajectory, and - (S10) replacement of the initial matrix Pn of covariance of the states of the carrier by the matrix Pp.
9. 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.
10. A trajectory tracking device for a mobile carrier, the device comprising: - a receiving interface configured to receive measurements acquired by an inertial measurement unit and sensors, - a processor configured to implement the determination of a dynamic alignment trajectory for the carrier, 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 motion of the carrier at the beginning 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 measurement unit and carrier sensors, - (S3) determination of a first gradient of a scalar L(P^ Pæ" with respect to piloting commands of the carrier, the scalar L(P^ being constructed from a final covariance matrix Pp of a final estimate of the state of motion of the carrier at an 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 includes, for each point n: - a calculation of a second gradient of the scalar L(P^ P^ with respect to transition matrices from an estimated value of the carrier state at point n-1 to an estimated value of the carrier state at point n, - a calculation of a third gradient of the scalar L⇇P^ with respect to noise action matrices in the transition of 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, the determination of the first gradient including the use of backpropagation equations of the first gradient, 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 the carrier's controls - (S5) determination of a sequence of commands that minimizes a the sum of the HP^ scalar 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 measurement unit, - sensors, and - at least one trajectory tracking device according to the preceding claim.