Navigation support method and device based on a Kalman filter

Description

Previous technique

[0001] The invention relates to the general field of measurements made using sensors, such as the position and speed of a vehicle.

[0002] It specifically concerns the control of a vehicle's navigation using a Kalman filter. This Kalman filter is used to estimate a quantity necessary for implementing a navigation maneuver for that vehicle. This quantity represents, for example, a characteristic of the vehicle itself (e.g., position, speed, orientation, etc.), or it can also represent a characteristic related to the environment (the presence of an object near the vehicle, wind speed, etc.). Subsequently, such characteristics, whether internal or external to the vehicle, will be called "states."

[0003] Note that a Kalman filter also estimates uncertainties, in the form of covariance matrices, in the estimation of states. These estimated uncertainties can also be used to implement a maneuver. The term "magnitude" can therefore also refer to such uncertainties estimated by the Kalman filter.

[0004] As is well known, in a navigation system a Kalman filter is frequently used as a state estimator. The Kalman filter evaluates the state xn recursively, at each time n, from a numerical model (in particular a model describing how the different states evolve in relation to each other, and how the measurements depend on the states) and information collected by sensors, hereinafter referred to as "measurements".

[0005] For example, in a navigation system that generates an estimate of the position, orientation, and velocity of a target object, the model includes kinematic equations governing the trajectory of a vehicle based on its position, orientation, and velocity. The model may involve parameters whose values ​​are known a priori (e.g., the acceleration due to gravity) and / or whose values ​​can vary according to the time step n.

[0006] In the remainder of the request, to refer to the temporalization n, the expressions "moment", "stage" and "given instant" will be used interchangeably.

[0007] The time interval between two of these successive moments is usually a parameter that can be adjusted by the user.

[0008] In the field of navigation systems, sensors are often associated with an inertial measurement unit.

[0009] For example, in navigation, it is common to use an inertial measurement unit (IMU) coupled with a GPS receiver to estimate a vehicle's position and orientation. The IMU, which includes gyroscopes and accelerometers, evaluates changes in orientation and successive forces acting on the moving vehicle, respectively. Through a simple mathematical integration operation, these measurements allow us to estimate the vehicle's state in terms of position, speed, and orientation. The information provided by the GPS allows us to calculate the vehicle's position. A Kalman filter then merges this information to estimate the vehicle's state. The quality of the fusion depends, among other things, on properly calibrating the filter parameters. The models and methods of data fusion are well-established and will not be discussed in further detail.

[0010] The quantity or quantities estimated by the Kalman filter, and the use of these estimates, vary depending on the application. In particular: The estimation relates, for example, to the state of the vehicle and the estimation can be used to update a road map displayed to the driver of the vehicle and / or to decide which directions will be given to him to reach the goal of his journey; in a system equipped with radar sensors, the estimation can relate to the states of objects near the vehicle and this estimation can serve as an indication of the position of these objects so that the trajectory of the vehicle is adapted; in an autonomous vehicle, the estimation relates, for example, to the vehicle and can inform a control module which pilots the vehicle allowing the latter to decide on the next maneuver, etc.

[0011] The Kalman filter recursively determines an estimate xn|n of the current state of the target object using the previous estimate as well as measurements provided by sensors up to time n. This recursive calculation is schematically illustrated by the figure 1 .

[0012] Even with a suitable mathematical model, the quality of the measurements provided by the sensor(s) has an important influence on the quality of the estimates by the Kalman filter.

[0013] To appreciate the importance of the quality of measurements, it is first necessary to recall the classic operation of a Kalman filter.

[0014] We will denote by xn the state estimated by the Kalman filter at a given step n. In many applications the state is expressed in terms of a plurality of variables and, therefore, xn constitutes a vector.

[0015] 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 x n-1 of the previous instant n-1 according to the equation [Math. 1] below: x n = F n x n − 1 + B n u n + w n Where F n is a known square matrix, un is a control input vector (known to the user), i.e., an action applied to the system, B n is the matrix representing the model that links the state xn to this control input, and wn is a centered Gaussian random variable representing covariance noise represented by a matrix Q n.

[0016] In equation [Math.1] and the following equations, the side-by-side notation of two quantities corresponds to: a simple product if both quantities are numbers, a matrix product with a vector if one of these quantities is a matrix and the other quantity is a vector (for example F nx n-1 in equation [Math. 1]), or a matrix product if both quantities are matrices (for example F n P n-1|n-1 in equation [Math. 4] below).

[0017] The measurement(s), in particular the one(s) delivered by the sensor(s), at time n, will be denoted yn and depend on the state xn according to the equation [Math. 2] below: y n = H n x n + v n

[0018] Où Hn is a known observation matrix, and vn is a centered Gaussian random variable representing noise with covariance Rn. Again, in many applications, a plurality of measurements is delivered to the Kalman filter at each time n, and in this case, yn and vn will be vectors and Rn a matrix.

[0019] When implementing a Kalman filter, the noise covariance matrices, Qn and Rn, must be chosen. This is often done by first studying how these covariances relate to the data obtained in the domain in question. A typical approach for estimating the covariances involves applying the ALS algorithm (for "autocovariance least-squares").

[0020] The noise covariance matrices Qn and Rn correspond to uncertainties in the measurements performed by the sensors. Thus, in this application, and for the sake of brevity, we will refer to these quantities as measurement uncertainty yn.

[0021] The Kalman filter maintains an estimate of the state xn at a given time n, and this estimate will be denoted xn|n-1 when it is calculated (predicted) from the measurements y n-1 available at time n-1, and xn|n when the calculation integrates the measurements yn obtained at time n. The Kalman filter also estimates a covariance matrix P n representing the uncertainty of the estimate of the state xn and this matrix will be denoted P n|n-1 when it applies to the estimate xn|n-1, and P n|n when it applies to the estimate xn|n.

[0022] At each step n, the filter operates in two stages.

[0023] Initially, the Kalman filter predicts the prior state estimate xn|n-1 of the target object, as well as the covariance matrix P n|n-1, from the state estimate x n-1|n-1 and the covariance matrix P n-1|n-1, according to equations [Math. 3] and [Math. 4] below: x n n − 1 = F n x n − 1 n − 1 + u n P n n − 1 = F n P n − 1 n − 1 F n T + Q n

[0024] Where F n T< is the transpose of the matrix F n .

[0025] In a second step, the Kalman filter updates the prior estimate of the target object's state using the prediction made in the previous step and taking into account the current measurements yn with a certain weighting represented by a coefficient K n called "the Kalman gain". The update is performed according to equations [Math. 5] and [Math. 6] below: x n n = x n n − 1 + K n y n − H n x n n − 1 = x n n − 1 + K n z n P n n = I − K n H n P n n − 1 where K n is calculated according to equation [Math. 7] below: K n = P n n − 1 H n T S n − 1 where S n , which represents the covariance of the measures yn, is calculated according to the equation [Math. 8] below: S n = H n P n n − 1 H n T + R n S n -1< is the inverse matrix of S n , and zn is defined according to the equation [Math. 9] below: z n = y n − H n x n n − 1

[0026] The Kalman filter outputs the current estimate xn|n of the current state and the uncertainty P n|n associated with this estimate, these quantities being delivered, depending on the application, to a control system or other.

[0027] As long as the Kalman filter assumes that the measurements from the GPS are reliable, even a small measurement error will cause the estimate to deviate more or less strongly from the real situation.

[0028] Thus, the data of uncertainty values ​​associated with an estimate is not sufficient to evaluate the reliability of measurements and estimates by the Kalman filter, nor does it allow us to detect which measurement or sensor is responsible for poor robustness of this estimate.

[0029] The invention aims to overcome these drawbacks.

[0030] Furthermore, the document "ERIC ABBOTT ET AL: "Land-Vehicle Navigation Using GPS", PROCEEDINGS OF THE IEEE, IEEE. NEW YORK, US, vol. 87, no. 1, January 1, 1999" describes a sensitivity analysis method which consists of analyzing the individual contributions of different input data (i.e., measurements from different sensors) to the estimation errors of a quantity (a vehicle position, for example) by a Kalman filter by comparing the estimate by the filter and the estimate of this quantity by a reference model.

[0031] The document "KARKEE M ET AL: "Local and global sensitivity analysis of a tractor and single axle grain cart dynamic system model", BIOSYSTEMS ENGINEERING, ELSEVIER, AMSTERDAM, NL, vol. 106, no. 4, 1 August 2010" illustrates the principle of using partial derivatives in general to analyze the sensitivity of an estimate with respect to parameters on which said estimate depends. Description of the invention

[0032] Thus, according to a first aspect, the invention proposes a method for controlling the navigation of a vehicle for carrying out a navigation maneuver, according to claim 1. The method comprises, among other things, the following steps: estimating a quantity using a Kalman filter based on parameters, a digital model of the vehicle in its environment, and at least one measurement performed by at least one sensor and associated with an uncertainty on that measurement, said quantity being used to implement said navigation maneuver; determining at least one partial derivative value of said quantity with respect to said at least one measurement; obtaining at least one criticality value of said at least one measurement for said estimation, based on said at least one partial derivative value; sending at least one criticality signal representative of said at least one criticality value; receiving at least one instruction in response to said sending step; and applying said at least one instruction; said instruction indicating a control command for the implementation of said navigation maneuver.

[0033] Correspondingly, the invention proposes a vehicle navigation control system for implementing a navigation maneuver, according to claim 9. The system comprises, among other things: a module for estimating a quantity using a Kalman filter based on parameters, a digital model of the vehicle in its environment, and at least one measurement taken by at least one sensor and associated with an uncertainty on that measurement, said quantity being used to implement said navigation maneuver; a module for determining at least one partial derivative value of said quantity with respect to said at least one measurement; a module for obtaining at least one criticality value of said at least one measurement for said estimation, based on said at least one partial derivative value; a module for sending at least one criticality signal representative of said at least one criticality value; a module for receiving at least one instruction in response to said sending step; and a module for applying said instruction; said instruction indicating a control command for the implementation of said navigation maneuver.

[0034] According to one method of implementing the control process, the said instruction indicates to add measurements for a subsequent estimation of the said quantity.

[0035] It should be noted here that the use of the term "partial derivative" encompasses the notion of gradient. Indeed, a measurement or uncertainty can contain several quantities (velocity, position, etc.) and can therefore be represented in the form of a vector.

[0036] In this case, for the sake of brevity, we will also use the term partial derivative of the magnitude with respect to a quantity. In other words, in this application, if a quantity is in the form of a vector, the partial derivative of the magnitude with respect to that quantity denotes the gradient of that magnitude along the vector representing the quantity.

[0037] Also for the sake of brevity, we will call the value of a partial derivative of the quantity with respect to a measure the set of values ​​that includes the gradient of the quantity along the vector representing this measure.

[0038] Let us also specify that in the present application, the value of a partial derivative of a quantity with respect to a first matrix (for example a matrix representing uncertainties on a measurement) corresponds to a second matrix whose components are equal to the values ​​of partial derivatives of this quantity with respect to each of the components of the first matrix.

[0039] In the present application, if a sum between two vectors (for example, two partial derivatives in vector form) is invoked, this sum corresponds to a vector equal to the element-wise sum of those two vectors.

[0040] The evaluation of these partial derivatives advantageously provides important information on the robustness of the estimation of a quantity necessary for the implementation of a navigation maneuver, in particular by allowing the criticality of the different measurements in this estimation to be quantified.

[0041] The information obtained on the robustness of the magnitude estimation indicates whether it is relevant or not to add a measurement to estimate the magnitude, from among the set of available measurements, or to apply a control command, which corresponds for example to the cancellation of the planned maneuver.

[0042] Indeed, on the one hand, one or more partial derivatives of a quantity with respect to a measurement made at a given instant, or with respect to measurements made during a given time range, makes it possible to evaluate the influence of the measurements on the estimation of the quantity over time.

[0043] For example, if a partial derivative with respect to a measurement takes a large value for the measurement taken at the time before the estimation of the quantity, but takes values ​​close to zero for the measurement taken at earlier times, this means that only the measurement taken at the time before the estimation is taken into account in a meaningful way in this estimation.

[0044] This scenario may indicate a lack of robustness in the estimate, in cases where a robust estimate requires the consideration of several successive measurements.

[0045] In this example, the estimate can be improved by increasing the frequency of measurements taken over time.

[0046] Thus, according to one method of implementing the control process: measurements are obtained successively, and during said determination step, for each of these measurements, a value of partial derivative of the quantity is determined with respect to that measurement.

[0047] In this embodiment, the calculation of partial derivatives is performed a posteriori. In other words, the quantity is obtained after obtaining the measurements against which the values ​​of the partial derivative of the quantity are calculated.

[0048] For example, the partial derivatives of an estimate xn|n at time n are calculated with respect to the series of measurements taken into account between times 0 and n; thus, the calculation of these partial derivatives is carried out following the obtaining of the estimate xn|n.

[0049] On the other hand, one or more partial derivatives of a quantity with respect to a measurement made by a given sensor, or with respect to measurements made by a set of given sensors, make it possible to evaluate the influence of the sensor or set of sensors on the estimation.

[0050] For example, if a partial derivative with respect to a measurement takes a small value, while the knowledge of a person skilled in the art indicates that the estimated quantity is usually very influenced by that measurement (e.g., the estimation of the speed of a vehicle is very influenced by the measurement of its acceleration), then this indicates that the estimation is not robust.

[0051] According to one implementation method of the control process, for at least one given instant, a said partial derivative value is determined during said determination step, with respect to a measurement, this measurement comprising at least two measurements each carried out by separate sensors at that given instant.

[0052] Recall that, as explained above, a measurement can correspond to a vector whose components are measurements of different states. For example, a measurement at a given instant could include a measurement of the vehicle's acceleration at that instant as well as a measurement of the vehicle's velocity at that instant.

[0053] It is also worth recalling that in this implementation, a partial derivative of the quantity with respect to this measurement is a vector whose components are equal to the partial derivatives of the quantity with respect to each of the measurements of the different states. For example, the partial derivative of the quantity with respect to a measurement of acceleration and velocity includes the partial derivative of the quantity with respect to the velocity measurement as well as the partial derivative of the quantity with respect to the acceleration measurement.

[0054] Calculating the partial derivatives of a quantity estimated by a Kalman filter is, advantageously, inexpensive in terms of computing power and storage capacity.

[0055] The calculation of the partial derivatives of the estimated quantity utilizes certain values ​​that are already calculated during the operation of a Kalman filter as a state estimator. Therefore, implementing the invention requires only the addition of a few inexpensive extra means in terms of time and computing power.

[0056] From the partial derivative values, the process yields a criticality value to characterize the criticality of a measurement for estimating the quantity. In other words, a criticality value quantifies the influence of one or more measurements on the quantity estimation. In a first example, one or more criticality values ​​can be equal to the partial derivative values, since these values ​​characterize the criticality of measurements for quantity estimation.

[0057] Thus, according to one method of implementing the control process, at least one criticality value is equal to at least one partial derivative value determined during said determination step.

[0058] In particular, in the case of a system with a single sensor that only provides one measurement at a time, the partial derivatives of the quantity with respect to the measurements made over time by this single sensor allow us to estimate the robustness of the estimation of the quantity.

[0059] However, depending on the navigation maneuver to be performed, it may be advantageous to define the criticality value(s) differently.

[0060] For example, according to a particular mode of the control process, the criticality value(s) are equal to the absolute values ​​of the partial derivative values.

[0061] Indeed, depending on the navigation maneuver to be implemented, the amplitude of the criticality of a measurement on the estimation of the quantity, quantified by the absolute value of the partial derivative with respect to this measurement, is relevant.

[0062] In another example, the criticality values ​​are equal to the absolute values ​​of the normalized partial derivative values, such that the sum of these criticality values ​​is equal to 1.

[0063] The normalization of the absolute values ​​of the partial derivative values ​​with respect to different measurements makes it possible in particular to compare the criticality of a measurement for the estimation of a quantity, relative to the criticalities of the other measurements used for this same estimation.

[0064] In another example, where several partial derivative values ​​are determined with respect to measurements taken successively over time, the criticality values ​​are equal to the differences between two partial derivative values ​​with respect to two successive measurements. Such criticality values ​​make it possible to detect a potentially excessive variation, at a given instant, in the criticality of a measurement for estimating the quantity, and thus to detect poor robustness of the estimate at that given instant.

[0065] It may be advantageous to characterize the criticality of a group of measurements for estimating the magnitude, for example measurements taken successively over a given time range, or measurements taken by a group of sensors.

[0066] For this purpose, a criticality value can be equal to a sum of the values ​​of partial derivatives with respect to the measurements among a group of measurements.

[0067] Thus, according to a particular embodiment of the control process: several partial derivative values ​​are determined during said determination step, said at least one criticality value is equal to a sum of said partial derivative values.

[0068] According to another embodiment of the control process: several partial derivative values ​​are determined during said determination step; said at least one criticality value is equal to a sum of products between: (i) said partial derivative value with respect to said measurement, and (ii) said uncertainty on that measurement.

[0069] In other words, in this implementation method, a criticality value is a sum of partial derivative values ​​with respect to measurements, this sum being weighted by the uncertainties on these measurements.

[0070] This embodiment offers a convenient and simple way to detect whether uncertain measurements have a high criticality for the estimation of the quantity. If such a criticality value is high, it means that the estimation of the quantity is not robust.

[0071] The invention also makes it possible to determine a criticality value similarly to the previous embodiment, from a single partial derivative value.

[0072] Thus, according to one embodiment of the control process, said at least one criticality value is equal to a product of: (i) a said partial derivative value with respect to a said measure, and (ii) said uncertainty on that measure.

[0073] To enable the use of the criticality value(s), the control process indicates the transmission of one or more criticality signals representing these criticality values. In the navigation control system implementing the process described above, this criticality signal(s) are sent to a receiving module, which then executes the process reception step.

[0074] By construction, this or these criticality signals contain one or more pieces of information on the robustness of the magnitude estimate, this information allowing a decision to be made regarding the implementation of the navigation maneuver.

[0075] In one embodiment of the invention, the criticality signal(s) have values ​​equal to the criticality values.

[0076] According to the invention, the criticality signal(s) are defined from the comparison of criticality values ​​with one or more thresholds.

[0077] Thus, according to the invention, the sending step comprises the following sub-steps: of comparison between said at least one criticality value and at least one threshold, said at least one threshold being defined in relation to said maneuver; of generation of said at least one criticality signal as a function of a result of said comparison.

[0078] Correspondingly, the invention proposes a control system in which said sending module comprises the following sub-modules: of comparison between said at least one criticality value and at least one threshold, said at least one threshold being defined in relation to said maneuver; of generation of said at least one criticality signal as a function of a result of the comparison by said comparison sub-module.

[0079] The knowledge by the person of the navigation maneuver to be implemented, in particular of the operation of the sensors performing the measurements, makes it possible to define one or more thresholds.

[0080] The criticality signal(s) resulting from the comparison of the criticality value(s) with this threshold(s) have the advantage of being easily readable indicators of the robustness of the quantity estimation.

[0081] In particular, such a criticality signal can be a binary number, indicating the good or bad robustness of the estimate.

[0082] The criticality signal(s) can be processed automatically or processed by a user to whom the criticality signal(s) are presented via this receiving module.

[0083] Consequently, one or more instructions, for example automatically determined or user-defined, are received by the receiving module and then applied. For example, the instruction(s) are determined based on the criticality signal(s).

[0084] An instruction may concern a control of the vehicle itself. In particular, if poor robustness of the magnitude estimation is detected, the instruction may indicate a temporary halt to the implementation of the navigation maneuver or its cancellation.

[0085] Such a temporary halt to the navigation maneuver helps to ensure the safety of navigation until a more robust estimate of the magnitude is obtained.

[0086] If, on the other hand, good robustness of the magnitude estimation is detected, an instruction may indicate the extension of the maneuver.

[0087] An instruction may also indicate, in a particular embodiment, that it is desirable to take into account one or more additional measurements from among the set of available measurements provided by the sensors, in order to make a more robust estimate of the magnitude.

[0088] The invention also proposes a vehicle navigation system, according to claim 10, said navigation system being adapted to perform the functions of the control system according to one of the embodiments described above, and comprising at least one sensor intended to obtain at least one measurement used for estimating a quantity, said quantity being used to implement a navigation maneuver.

[0089] According to one embodiment of this navigation system, at least one of said sensors is an inertial measurement unit.

[0090] In such a system, the sensor array may, for example, include at least one GPS sensor paired with an inertial measurement unit. This is a sensor configuration typically found on vehicles, drones, or other devices.

[0091] The invention provides a computer program according to claim 12, comprising instructions for executing the steps of a control process according to any one of the implementation methods described above.

[0092] It should be noted that the computer programs mentioned in this presentation can use any programming language, and be in the form of source code, object code, or code intermediate between source code and object code, such as in a partially compiled form, or in any other desirable form.

[0093] The invention provides a recording medium, according to claim 13, readable by computer equipment of a computer program comprising instructions for the execution of the steps of a process according to one of the embodiments described above.

[0094] The recording media referred to in this presentation can be any entity or device capable of storing the program and being read by a control device or by any computing equipment, including a computer.

[0095] For example, the medium may include a means of storage, or a means of magnetic recording, for example a hard drive.

[0096] Alternatively, the recording media may correspond to an integrated circuit in a computer or navigation system, a circuit in which the program is incorporated, and adapted to perform a process as described above or to be used in the performance of that process. Brief description of the drawings

[0097] Other features and advantages of the present invention will become apparent from the description below, with reference to the accompanying drawings which illustrate non-limiting examples of embodiments. In the figures: [ Fig. 1 ] there figure 1 schematically represents the recursive operation of a Kalman filter; Fig. 2 ] there figure 2 a navigation control method conforming to a first particular embodiment of the invention; [ Fig. 3 ] there figure 3 represents a navigation control system conforming to a first particular embodiment of the invention; [ Fig. 4 ] there figure 4 a navigation control method conforming to a second particular embodiment of the invention; [ Fig. 5 ] there figure 5 represents a navigation control system conforming to a second particular embodiment of the invention; [ Fig. 6 ] there figure 6 represents a navigation system conforming to a particular implementation method of the invention; [ Fig. 7 ] there figure 7 represents the hardware architecture of a navigation control system according to a particular implementation method of the invention; [ Fig. 8 ] there figure 8 represents partial derivative values ​​with respect to measurements taken successively, for two different time intervals between the successive measurements. Description of the implementation methods

[0098] We will now describe several embodiments of the invention. In general, and as mentioned previously, the invention proposes a method for controlling the navigation of a vehicle Veh for the implementation of a maneuver requiring the estimation of a quantity G.

[0099] There figure 1 represents the iterative process of estimating a quantity G at each instant n by a Kalman filter FK.

[0100] This quantity, at time n, is for example the value xn|n of a state estimated by the Kalman FK filter, or of an uncertainty Pn|n on the estimation of this state. In general, this quantity can correspond to any quantity estimated by the Kalman FK filter, and required for the implementation of a navigation maneuver Man j.

[0101] At a given instant n, the Kalman filter uses the parameters Par n corresponding to a numerical model of the vehicle in its environment, to estimate quantities whose magnitude, from quantities estimated by this Kalman filter at the previous instant n-1.

[0102] Note that on the figure 1 The estimation of the quantity at time n also uses measurements yn (and the uncertainties R n and Q n on these measurements) carried out at time n. In particular, the estimation of this quantity takes into account: a first estimation carried out without taking into account the measurements at time n, then an update of this estimate with the measurements yn and their uncertainties R n.

[0103] The first estimate is a prediction from time n-1, since it does not require observation of the vehicle and / or its environment at time n.

[0104] If the quantity corresponds to a state, the first estimate corresponds to equation [Math.3] which gives a first estimated value xn|n-1 of the state, and the update corresponds to equation [Math.5] which gives a value xn|n of the state.

[0105] In another example, the quantity G at time n does not take into account the measurements made at time n, and is therefore predicted from the n-1 measurements. In this example, if the quantity G corresponds to a state, its estimation corresponds to the first estimate of the value xn|n-1.

[0106] Such an estimate is useful in a case where the navigation maneuver Man j requires the prediction of the magnitude G at time n even before obtaining the measurements at time n.

[0107] Note that the parameters Par n-1 , Par n , Par n+1 of the numerical model can be modified between two successive instants.

[0108] There figure 2 represents the main steps of a navigation control process intended to improve the safety of the implementation of a navigation maneuver Man j of a vehicle Veh, in accordance with a particular embodiment of the invention.

[0109] During an ECap step, measurements y 1 -yn and their associated uncertainties R 1 -R n are obtained up to time n via one or more sensors Cap 1< -Cap k< .

[0110] During an estimation step E0, a quantity G is estimated by the Kalman filter from these measurements y1 -yn and uncertainties R1 -Rn. The estimation of this quantity is performed iteratively, as shown in the diagram. figure 1 and explained above.

[0111] In a particular implementation mode, this estimation of the quantity corresponds to a first estimation of the quantity without taking into account the measurements at time n followed by an update of this first estimate with the measurements at time n. In this mode, the estimation step corresponds to the estimation of the quantity at time n.

[0112] In another implementation mode, the magnitude estimation step includes the first estimation of the magnitude, followed by its update by the measurements yn and uncertainties R n, then an estimation of the magnitude at time n+1, from the updated estimate of the magnitude at time n. In this mode, the estimation step corresponds to the prediction of the magnitude at time n+1.

[0113] During a PDE step, partial derivative values ​​of the quantity G with respect to the measurements are calculated.

[0114] In one embodiment, the backpropagation technique is used to calculate the values ​​of partial derivatives with respect to measurements taken at times preceding n. This technique allows the partial derivatives to be calculated iteratively.

[0115] In particular, it allows us to calculate a partial derivative of the quantity with respect to a measurement y n-1 at time n-1 as a function of the partial derivative of the quantity with respect to a measurement at time n, then to calculate a partial derivative of the quantity with respect to a measurement at time n-2 as a function of the partial derivative of the quantity with respect to the measurement at time n-1, and so on.

[0116] In order to calculate the partial derivative of the quantity G with respect to a measurement taken at any given time, backpropagation requires combining: on the one hand, the formulas for the partial derivatives of the quantity G with respect to the measurements at time n: ∂ G ∂ y n = K n T ∂ G ∂ x n n on the other hand, the formulas for the partial derivatives with respect to the quantities estimated by the Kalman filter at the previous instant n: ∂ G ∂ P n n − 1 = I − K n H n T ∂ G ∂ P n n + 1 2 ∂ G ∂ x n n z n T R n − 1 H n + 1 2 H n T R n − 1 z n ∂ G ∂ x n n T I − K n H n ∂ G ∂ x n n − 1 = I − K n H n T ∂ G ∂ x n n ∂ G ∂ P n − 1 n − 1 = F n T ∂ G ∂ P n n − 1 F n ∂ G ∂ x n − 1 n − 1 = F n T ∂ G ∂ x n n − 1

[0117] Where the quantities appearing in these equations have been described previously.

[0118] Thus, for example, to calculate the partial derivative with respect to the measurements y n-1 taken at time n-1, we obtain on the one hand, according to equation [Math.10]: ∂ G ∂ y n − 1 = K n − 1 T ∂ G ∂ x n − 1 n − 1

[0119] And on the other hand, according to equations [Math.14] and [Math.12]: ∂ G ∂ x n − 1 n − 1 = F n T ∂ G ∂ x n n − 1 = F n T I − K n H n T ∂ G ∂ x n n

[0120] If, for example, the quantity G is the state xn|n estimated at time n, then we obtain, by combining [Math.15] and [Math.16]: ∂ G ∂ y n − 1 = K n − 1 T F n T I − K n H n T

[0121] To calculate the partial derivative with respect to the measurements y n-2 at time n-2, we use the same method, but replacing the n in the previous equations with n-1: ∂ G ∂ y n − 2 = K n − 2 T ∂ G ∂ x n − 2 n − 2 = K n − 2 T F n − 1 T I − K n − 1 H n − 1 T ∂ G ∂ x n − 1 n − 1

[0122] Where the quantity ∂ G / ∂ x n-1|n-1 has been calculated previously and therefore does not need to be recalculated.

[0123] If we continue the example in which G corresponds to the state xn|n estimated at time n, we finally obtain, using the result of the previous iteration: ∂ G ∂ y n − 2 = K n − 2 T F n − 1 T I − K n − 1 H n − 1 T F n T I − K n H n T

[0124] Calculating partial derivatives of the quantity G with respect to the measurements y 1 -yn is quite easy from the point of view of computational load and the storage of quantities during the estimation process, since: Matrix operations are not numerous; dimensions remain limited to matrix dimensions whose maximum dimensions are equal to the number of elements in the state vectors xn|n or to the number of elements in the measure vectors yn; terms calculated for an iteration n, for example IK n H n, can be stored and reused for the calculation of a partial derivative with respect to an element of an iteration preceding n; storage requirements during the estimation process by the Kalman filter FK can be limited to the three quantities below: (i) (IK n H n ), or K n and H n , or K n and H n (ii) R n or S n (iii) zn , H n and F n .

[0125] Recall that a measurement taken at a given time n yn can be a vector. Each of the components ynk< of this vector is itself a measurement taken by a sensor Cap k< at that given time n.

[0126] Thus, according to an embodiment according to the invention, a measurement yn comprises several measurements yn 1< -ynk< , each of these measurements being carried out by a separate sensor Cap 1< -Cap k< .

[0127] During an EOV step, one or more criticality values ​​VC are determined from these partial derivative values ​​DG< y 1 -DG< yn .

[0128] In a particular embodiment of the invention, the criticality values ​​VC are equal, for a given Man j maneuver, to the partial derivative values ​​DG< y 1 -DG< yn determined during the PDE step.

[0129] A particular example of Man j maneuver is to move a ship through a sea strait, and the magnitude needed to carry out this maneuver is the ship's position relative to the land coasts.

[0130] The yn measurements include, for example, acceleration measurements by a first sensor (an inertial measurement unit for example), and position measurements by a second sensor (a GPS for example), at a given instant n.

[0131] The partial derivative values ​​DG< yn , and therefore the criticality values ​​VC in this example, quantify the influence of each of the measurements, at each instant n, on the estimation of the ship's position.

[0132] One such example is illustrated on the figure 8 This figure shows the values ​​of the partial derivatives of the quantity G, with respect to measurements taken successively over time, over a period of 100 seconds (on the x-axis). In this example, the measurements are velocity measurements, and the quantity G is the estimated velocity at time 100.

[0133] At the top of the figure 8 The time interval between two measurements is 20 seconds: we therefore see the value of a partial derivative every 20 seconds. At the bottom of the figure 8 The time interval is 10 seconds.

[0134] There figure 8 shows that in this particular case, the larger the time interval between two successive measurements, the more critical the last measurement is for estimating the magnitude.

[0135] Indeed, at the top of the figure 8 We observe that the last partial derivative value is three times greater in absolute value than the penultimate partial derivative value, and more than twenty times greater than the preceding partial derivative values. Conversely, at the bottom of the figure, where the time interval between two measurements is half as long, the difference between the partial derivative values ​​for two successive measurements is much smaller.

[0136] Thus, on the top of the figure 8 Only the last measurement is critical for estimating the quantity in question. In other words, in this case, the estimation of the quantity relies primarily on the last measurement. Depending on the procedure to be performed, this may mean that the estimation of the quantity is not very reliable in this case.

[0137] As shown by the bottom of the figure 8 If the time interval between two successive measurements is reduced, multiple measurements significantly influence the estimation of the quantity, which then relies solely on a single measurement. Depending on the procedure, this may mean that the estimation of the quantity is more robust if the time interval between two measurements is decreased.

[0138] In another particular embodiment of the invention, the criticality value(s) are equal to the absolute values ​​of the partial derivative values.

[0139] In another particular embodiment of the invention, the criticality values ​​are equal to the absolute values ​​of the normalized partial derivative values, such that the sum of these criticality values ​​is equal to 1.

[0140] In another particular embodiment of the invention, in which several partial derivative values ​​are determined with respect to measurements taken successively over time, the criticality values ​​are equal to the differences between two partial derivative values ​​with respect to two successive measurements. Such criticality values ​​make it possible to detect a potentially excessive variation, at a given instant, in the criticality of a measurement for estimating the quantity, and thus to detect poor robustness of the estimate at that given instant.

[0141] In another particular embodiment of the invention, the criticality values ​​are sums over partial derivative values.

[0142] For example, a criticality value is the sum of measurements taken by a sensor over a given time interval. This quantifies the influence of the sensor's measurements on the estimation of the quantity during that time interval.

[0143] In another example, a criticality value is the sum of measurements taken by all sensors at a given time. Such a criticality value quantifies the influence of a set of measurements on the estimation of the quantity at that given time.

[0144] In another particular embodiment of the invention, the criticality values ​​are sums of partial derivative values ​​with respect to measurements, weighted by uncertainties in these measurements to homogenize (make unitless) the partial derivatives. Such criticality values ​​can be calculated using the formula: VC = ∑ i = m n R i ∂ G ∂ y i or the formula: VC = ∑ i = m n Q i ∂ G ∂ y i where m is an instant preceding instant n, or is instant n itself, in which case the sum of equation [Math.20] or equation [Math.21] comprises only one term.

[0145] Such criticality values ​​quantify the influence of measurements on the estimation of the quantity G, relative to the uncertainties on these measurements.

[0146] During the EE transmission step, one or more criticality signals (Sig) are obtained from the criticality values ​​determined during the EDP step. These signals are representative of the criticality values. In other words, they contain information about the influence of the measurements that the criticality values ​​quantify.

[0147] During this same EE step, this or these criticality signals are sent to a receiving module.

[0148] In a particular embodiment of the invention, the criticality signals are identical values ​​to the criticality values.

[0149] According to the invention, as shown in the figure 4 The criticality signal(s) are obtained by comparing criticality values ​​VC and thresholds Se during an EComp substep of the EE sending step, and then generated from the result of this comparison during an EGen substep of the sending step.

[0150] For example, if a criticality value VC is greater than a given threshold Se, the criticality signal Sig indicates a safety alert for the implementation of the Man j maneuver requiring estimation of the quantity. Conversely, if this criticality value VC is less than this threshold Se, the criticality signal Sig indicates that the estimation is robust.

[0151] There figure 5 represents an SCN navigation control system conforming to an embodiment of the invention, this system enabling the implementation of the control method represented in the figure 4 Thus, the SCN system represented on the figure 5 does not differ from the SCN system shown on the figure 3 that by the presence of the comparison sub-modules MComp and generation sub-modules MGen in the sending module ME, these sub-modules allow the implementation of the comparison sub-steps EComp and generation sub-steps of criticality signals Sig represented on the figure 4 .

[0152] The criticality signal Sig is then sent to a receiving module MR and analyzed by a user with access to the receiving module MR. This user then sends an Inst instruction back to the receiving module MR. This Inst instruction is then applied, during an application step EA, by an application module MA. In another embodiment, the analysis of the criticality signal and the determination of the instruction can be handled automatically.

[0153] The Inst instruction can correspond to a control command to be applied to the vehicle. For example, if the user deduces from the criticality signal(s) Sig that the estimation of the quantity G is not robust, they can send the Inst instruction to stop the vehicle Veh or to pause the maneuver Man j until a robust estimation of the quantity G is obtained.

[0154] The Inst instruction can also correspond to taking into account at least one additional measurement y 1 -yn among the set of available measurements made by at least one sensor Cap 1< -Cap k< associated with an uncertainty R n or Q n in order to improve the robustness of the estimation of the quantity G of the state of the vehicle Veh.

[0155] The specific implementation methods of the navigation control process shown on the figures 2 And figure 4 are implemented on SCN navigation control systems represented respectively on the figures 3 And figure 5 .

[0156] On the figure 3 , sensors Cap k< take measurements on the vehicle Veh and its environment Env. For example, a GPS can measure the position of the vehicle Veh, an inertial measurement unit can measure the speed and position of the vehicle, a radar can measure the position of objects near the vehicle etc.

[0157] These measurements and their associated uncertainties are used by an estimation module M0 to estimate a quantity G with a Kalman filter FK.

[0158] An MDP module then determines the partial derivatives of the quantity with respect to the measurements.

[0159] From the partial derivative values ​​determined by the MDP module, a MOV module obtains one or more criticality values ​​as described above.

[0160] An ME module then sends one or more criticality signals Sig, representative of these criticality values ​​VC, to a receiving module MR.

[0161] This MR module allows the criticality signal(s) to be displayed to a user who decides on an instruction as described above.

[0162] This instruction is received by the MR module and then applied by an MA application module.

[0163] If this instruction indicates a navigation control command, it is applied to the vehicle Veh. If it indicates to take into account more measurements, a new estimation of the target state is performed.

[0164] There figure 6 represents an SNav navigation system integrating an SCN navigation control system and a set of Cap 1< -Cap k< sensors

[0165] In a particular embodiment of this Nav system, this set of Cap 1< -Cap k< sensors includes at least one inertial measurement unit.

[0166] There figure 7 represents the hardware architecture of an SCN navigation control system conforming to a particular embodiment of the invention.

[0167] In the embodiment described here, the SCN navigation control system has a computer hardware architecture. It includes, in particular, a processor D1, a read-only memory D2, a random-access memory D3, a rewritable non-volatile memory D4, and communication means D5.

[0168] The read-only memory D2 of the SCN system constitutes a recording medium according to the invention, readable by the processor D1 and on which is recorded a computer program PGI according to the invention, this program comprising instructions for the execution of the steps of a control process according to the invention described above with reference to the figure 2 Or figure 4 in two embodiments.

[0169] The PGI computer program defines functional modules of the SCN control systems represented on the figure 3 Or figure 5 .

Claims

1. A method for controlling the navigation of a vehicle (Veh) in order to implement a navigation manoeuvre (MANj), the method comprising the steps: - estimating (E0) a quantity (G) by means of a Kalman filter (FK) on the basis of parameters (Par1-Parn), a numerical model of the vehicle in its surroundings and at least one measurement (y1-yn) carried out by at least one sensor (Cap1-Capk) and associated with an uncertainty (R1-Rn, Q1-Qn) regarding said measurement (y1-yn), said quantity (G) representing a characteristic of the vehicle or relative to the surroundings of the vehicle and being used to implement said navigation manoeuvre (MANj); - determining (EDP) at least one partial derivative value (DGy1-DGyn) of said quantity (G) with respect to said at least one measurement (y1-yn); - obtaining (EOV) at least one criticality value (VC) of said at least one measurement (y1-yn) for said estimation (E0) on the basis of said at least one partial derivative value (DGy1-DGyn), the criticality value quantifying an influence of said at least one measurement on the estimation of the quantity; - applying (EA) at least one instruction (Inst) determined on the basis of at least one piece of information relating to the robustness of the estimation of the quantity, said piece of information being contained in at least one criticality signal (Sig) representative of said at least one criticality value (VC); said instruction indicating a control command for implementing said navigation manoeuvre (MANj), said at least one criticality signal (Sig) being generated (EGen) as a function of a result of a comparison (EComp) between said at least one criticality value (VC) and at least one threshold (Se), said at least one threshold (Se) being defined with respect to said manoeuvre (MANj).

2. The method according to claim 1, wherein one said instruction (Inst) indicates adding measurements for a subsequent estimation of said quantity (G).

3. The method according to claim 1 or 2, wherein: - successive measurements (y1-yn) are obtained; and - during said determination step (EDP), for each of these measurements, a partial derivative value (DGy1-DGyn) of said quantity is determined with respect to this measurement.

4. The method according to one of claims 1 to 3, wherein, for at least one given instant (n), one said partial derivative value (DGyn) is determined during said determination step (EDP), with respect to a measurement (yn), this measurement comprising at least two measurements (yn1-ynk) each carried out at this given instant (n) by distinct sensors (Cap1-Capk).

5. The method according to one of claims 1 to 4, wherein said at least one criticality value (VC) is equal to said at least one partial derivative value (DGy1-DGyn) determined during said determination step (EDP).

6. The method according to one of claims 1 to 4, wherein: - a plurality of partial derivative values (DGy1-DGyn) are determined during said determination step (EDP); and - said at least one criticality value (VC) is equal to a weighted sum of said partial derivative values (DGy1-DGyn).

7. The method according to one of claims 1 to 4, wherein: - a plurality of partial derivative values (DGy1-DGyn) are determined during said determination step (EDP); - said at least one criticality value (CV) is equal to a sum of products between: (i) one of said partial derivative value (DGy1-DGyn) with respect to one of said measurements (y1-yn); and (ii) said uncertainty (R1-Rn, Q1-Qn) on this measurement (y1-yn).

8. The method according to one of claims 1 to 4, wherein said at least one criticality value (CV) is equal to a product of: (i) one of said partial derivative values (DGy1-DGyn) with respect to one of said measurements (y1-yn); and (ii) said uncertainty (R1-Rn, Q1-Qn) on this measurement (y1-yn).

9. A navigation control system of a vehicle (Veh), for implementing a navigation manoeuvre (MANj), the system comprising: - a module (M0) for estimating a quantity (G) by means of a Kalman filter (FK) on the basis of parameters (Par1-Parn), a numerical model of the vehicle in its surroundings and at least one measurement (y1-yn) carried out by at least one sensor (Cap1-Capk) and associated with an uncertainty (Rn, Qn) regarding said measurement (y1-yn), said quantity (G) representing a characteristic of the vehicle or relative to the surroundings of the vehicle and being used to implement said navigation manoeuvre (MANj); - a module (MDP) for determining at least one partial derivative value (DGy1-DGyn) of said quantity (G) with respect to said at least one measurement (y1-yn); - a module (MOV) for obtaining at least one criticality value (VC) of said at least one measurement (y1-yn) for said estimation (E0) on the basis of said at least one partial derivative value (DGy1-DGyn), the criticality value quantifying an influence of said at least one measurement on the estimation of the quantity; - a module (MA) for applying at least one instruction (Inst) determined on the basis of at least one piece of information about the robustness of the estimation of the quantity, said piece of information being contained in at least one criticality signal (Siq) representative of said at least one criticality value (VC); said instruction indicating a control command for implementing said navigation manoeuvre (MANj), said transmission module (ME) comprising sub-modules: - (MComp) for comparing said at least one criticality value (VC) and at least one threshold (Se), said at least one threshold (Se) being defined with respect to said manoeuvre (MANj); - (MGen) for generating said at least one criticality signal (Sig) as a function of a result of the comparison by said comparison sub-module (MComp).

10. A navigation system (SNav) for a vehicle (Veh), said navigation system (SNav) being suitable for performing the functions of the control system (SCN) according to claim 9 and comprising at least one sensor (Cap1-Capk) intended to obtain at least one measurement (y1-yn) being used to estimate said quantity (G) being used to implement a navigation manoeuvre (MANj).

11. The navigation system (SNav) according to claim 10, wherein at least one said sensor (Cap1-Capk) is an inertial measurement unit.

12. A computer program (PGI) comprising instructions for executing the steps of a method according to at least one of claims 1 to 8.

13. A recording medium (D2), readable by computer equipment, for a computer program (PGI) comprising instructions for executing the steps of a method according to at least one of claims 1 to 8.