METHOD FOR SUPPORTING THE NAVIGATION OF A VEHICLE

Description

DOMAINE DE L'INVENTION

[0001] The present invention relates to the field of vehicle navigation methods. It relates more particularly to so-called hybrid navigation methods. ETAT DE LA TECHNIQUE

[0002] Hybrid navigation methods are methods in which measurements from several sensors (accelerometers, gyroscopes, GPS, etc.) are fused in order to determine variables representative of a state of a device implementing the method.

[0003] These variables include kinematic variables which are, for example, a position, a velocity or an orientation matrix of the device representing the change of coordinates from a moving frame attached to the carrier to a reference frame.

[0004] These variables also include sensor fault variables whose value is representative of faults in the measurement of one or more sensors (e.g., a bias or incorrect sensor positioning).

[0005] In the following, kinematic variables and sensor error variables are referred to indiscriminately as variables.

[0006] These methods use primary measurements that are motion measurements, for example, inertial measurements, such as those obtained from accelerometers and gyroscopes. Accelerometers and gyroscopes allow us to obtain the specific force and angular velocity. The specific force is the sum of all external forces other than gravitational forces divided by the mass. This quantity therefore has the dimensions of an acceleration.

[0007] These methods also use additional measurements that may come from another sensor (GPS, odometer, etc.) or from an algorithm that detects when the device has stopped. In the latter case, the vehicle's speed is detected as zero by an algorithm, or indicated as zero by the user, and is not actually measured. However, this does not affect the operation of the navigation system, and in this document, we will refer to stop detection as a "measurement" of speed. These additional measurements are combined with the main measurements to determine the kinematic variables.

[0008] Hybrid navigation methods employ classical extended Kalman filtering. This filtering is characterized by propagation stages using the primary measurements and update stages using the additional measurements. Extended Kalman filtering maintains estimated values ​​of the system variables and a matrix P n , known as covariance, assesses the uncertainty of the estimated values. This uncertainty is defined as the covariance of an error variable e X< defined as e X< = X - X̂ Or X̂ is the estimate of one of the vector variables and where X is the real value of this vector variable. Since one of the kinematic variables is generally an orientation represented by a rotation matrix (therefore a non-vector variable), the formula above cannot be used for the orientation, which is not represented by a vector but generally by a unit quaternion or an orientation matrix and a rotation error variable. e T< is defined by the formula R ( e T< ) = TT̂ t< Or R ( e ) = T̂ t< T, R(·) denoting the function returning a rotation matrix from a rotation vector, T designating the orientation matrix, T̂ the estimated value of this matrix and the symbol M t< denoting the transposition of a matrix M .

[0009] However, the classical extended Kalman filter is not capable of correcting large estimation errors and requires a preliminary phase called alignment.

[0010] We are familiar with the invariant Kalman filter, in which specific error variables are used to achieve better performance on certain nonlinear systems (particularly in navigation). For a vector X written in a given coordinate system and an orientation matrix T representing the change of coordinates from a moving frame attached to the carrier to the given frame used to write the vector X These so-called "invariant" error variables are of two types: Left-invariant error variables in this given frame of reference: R e T = T ^ t T , e X = T ^ t X − X ^ Right-invariant error variables in this given coordinate system: R e T = T T ^ t , e X = X − T T ^ t X ^

[0011] However, the performance of the Kalman invariant filter is guaranteed by theoretical properties only when all the variables it estimates are defined in the same frame of reference. If it simultaneously estimates values ​​defined in different frames of reference, such as an inertial frame and a frame attached to the carrier, it can lose consistency. Loss of consistency occurs when the actual estimation errors made by the filter are significantly greater than the uncertainties defined by the covariance matrix calculated by the filter. This is the case when some values, such as the position or velocity of the device, are defined in a reference frame (for example, a geocentric frame of reference) while other values, such as defects on one or more sensors, are defined in a moving frame (for example, one centered on the device).Furthermore, the particular error variables used by an invariant Kalman filter are sometimes sources of numerical instabilities, in the sense that the estimated variables gradually move away from their expected value because of the approximations made by the computer during arithmetic operations on floating numbers.

[0012] There is therefore a need for a new type of navigation process that allows the state of a system to be estimated in a consistent and numerically stable way, this state including values ​​defined in different reference frames.

[0013] Document US 2017314928 A1 proposes a method for estimating a navigation state with several variables of a moving carrier according to the extended Kalman filter method. EXPOSE DE L'INVENTION

[0014] In this respect, the invention proposes a method according to claim 1.

[0015] This method allows us to determine the values ​​of the navigation system's variables. These variables include kinematic variables and sensor error variables. Therefore, this method can determine variables that are not all expressed in the same coordinate system. In particular, the kinematic variables are expressed in a different coordinate system than the sensor error variables.

[0016] In an extended Kalman filter, we know the use of so-called "system" linearization points which allow us to obtain a linear approximation of the system: at each time step, a system linearization point is used during the current value determination step and another system linearization point is used during the update step.

[0017] In this invention, the linearization points of the error variables are linearization points that allow for changes in error variables. They will simply be called "linearization points".

[0018] The error variable change used in the invention involves transferring all variables to a single reference frame followed by the application of an invariant error. This error variable change is immediately followed by an inverse error variable change, but applied to a different linearization point. This second operation does not cancel out the first because the linearization point used is different. The complete step, defined as the sequence of these two operations, does not exist in the prior art and improves the filter's performance by giving it certain properties of invariant filtering. This step particularly improves filter performance in the case of significant sensor biases.

[0019] In addition, the use of two error variable systems helps to avoid certain numerical problems induced by invariant filtering that appear in long-distance navigation applications.

[0020] In one embodiment, the current and previous values ​​of the variables are defined in a reference frame. The first change of error variable is a change to error variables that are invariant in an auxiliary frame known with respect to the reference frame. The second change of error variable is a change from error variables that are invariant in the auxiliary frame known with respect to the reference frame. The auxiliary frame may or may not coincide with the reference frame.

[0021] In one embodiment, the first linearization point is determined from the current values ​​before the update, and the second linearization point is determined from the current values ​​after the update.

[0022] In one embodiment, the first linearization point is a system linearization point used during the update step, and the second linearization point is a system linearization point used during the step of determining the current values ​​of the next instant.

[0023] In one embodiment, a product is determined of a first transformation matrix performing the first transformation and a second transformation matrix performing the second transformation, and the first and second transformations are performed jointly using the product.

[0024] In one embodiment, the variables include: an orientation of the navigation device, a speed of the navigation device, a position of the navigation device, and a fault on a sensor of the navigation device, of which a current value is a current fault vector or matrix, The first change in error variable is a change in error variable: of a current value of the orientation, a current value of the velocity, a current value of the position, and the current fault vector or matrix, towards invariant error variables, in an auxiliary frame known with respect to a reference frame.

[0025] In one embodiment, the variables include: an orientation of the navigation device, whose current value is a current orientation matrix and whose previous value is a previous orientation matrix, a speed of the navigation device, whose current value is a current velocity vector and whose previous value is a previous velocity vector, a position of the navigation device, whose current value is a current position vector and whose previous value is a previous position vector, and a fault on a sensor of the navigation device, whose current value is a current fault vector and whose previous value is a previous fault vector, the current uncertainty matrix being representative of an uncertainty of the current orientation matrix, the current velocity vector of the current position vector, and the current sensor fault vector, the previous uncertainty matrix being representative of an uncertainty of the previous orientation matrix, the previous velocity vector, the previous position vector and the previous sensor fault vector.

[0026] In one embodiment, current values ​​are associated with a current time and previous values ​​are associated with a previous time, the determination of current values ​​comprising: a determination of the current velocity vector by adding to the previous velocity vector an integration, over a time interval between the previous instant and the current instant, of a sum of a specific force of the navigation device possibly corrected using the estimated values ​​of sensor defects and a model of terrestrial gravity experienced by the navigation device, a determination of the current position vector by adding to the previous position vector an integration, over the time interval, of the previous velocity vector, a determination of the current orientation matrix by multiplying the previous orientation matrix with a matrix representing a rotation of the navigation device possibly corrected using the estimated values ​​of sensor defects, and / or a determination of the current uncertainty matrix from the previous uncertainty matrix.

[0027] Thus in this embodiment if the sensor defect is a bias vector of the accelerometers, the specific force which is used to calculate the current velocity vector is corrected to take into account the estimated values ​​of the sensor defects.

[0028] Similarly, if the matrix representing a rotation of the navigation device is corrected to take into account the estimated values ​​of sensor defects.

[0029] In one embodiment, the measurement is a measurement taken in a moving frame attached to the navigation device of the speed of the navigation device relative to a reference frame; the determination of the correction includes: a subtraction from the velocity measurement of a multiplication of the transpose of the current orientation matrix and the current velocity vector, and a multiplication of the subtraction by a gain matrix.

[0030] In one embodiment, the correction is a vector of correction; the update includes: a substep of updating the current orientation matrix by multiplying the current orientation matrix and a rotation matrix associated with a rotation vector which constitutes a first part of the correction vector, a substep of updating the current velocity vector by adding to the current velocity vector a multiplication of the current orientation matrix and a second part of the correction vector, and / or a substep of updating the current position vector by adding to the current position vector a multiplication of the orientation matrix by a third part of the correction vector, and / or a substep of updating the current sensor fault vector by adding to the current sensor fault vector a fourth part of the correction vector.

[0031] In one embodiment, the current values ​​are first current values, the previous values ​​are first previous values, the correction is a first correction, the measurement is a first measurement, the gain matrix is ​​a first gain matrix, the uncertainty matrix is ​​a first uncertainty matrix, and the modified uncertainty matrix is ​​a first modified uncertainty matrix. The method further comprises: a determination of second current values ​​of the variables and a second current uncertainty matrix representing an uncertainty of the second current values, from previous second values ​​of the variables and a second previous uncertainty matrix representing an uncertainty of the previous second values; a determination of a second correction from: the second current values, the second current uncertainty matrix and a second measurement possibly identical to the first measurement; an update of the second current values ​​and the second current uncertainty matrix from the second correction and the second current uncertainty matrix; a first transformation of the second uncertainty matrix in order to obtain a modified second uncertainty matrix.the first transformation being a first change of error variable according to a third linearization point and a second transformation of the second modified uncertainty matrix, the second transformation being a second change of error variable according to a fourth linearization point, , the third linearization point being different from the fourth linearization point.

[0032] In one embodiment, the process further includes a determination of consolidated values ​​of the variables, from the first current values ​​and the second current values.

[0033] In one embodiment, the determination of respective consolidated values ​​of the variables includes a determination of a similarity or a difference between the first corrected values ​​and the second corrected values ​​and, where the similarity is greater than a similarity threshold or where the difference is less than a difference threshold, the determination of respective consolidated values ​​of the variables also includes an average of the first corrected values ​​and the second corrected values ​​or a weighted average of the first corrected values ​​and the second corrected values ​​or a selection of the first corrected values ​​or the second corrected values.

[0034] In one embodiment, the determination of respective consolidated values ​​of the kinematic variables includes a determination of a first deviation between the first corrected values ​​and the first measurements, a determination of a second deviation between the second corrected values ​​and the second measurements, and a selection of the first corrected values ​​when the first deviation is less than the second deviation or of the second corrected values ​​when the first deviation is greater than the second deviation.

[0035] In other words, in this embodiment, the determination of the respective consolidated values ​​of the variables includes determining a deviation indicator for the first set of values ​​and a deviation indicator for the second set of values. The determination involves selecting the values ​​with the lowest deviation indicator.

[0036] In one embodiment, a current value of a variable representing the environment of the navigation device is determined from a previous value of the variable representing the environment of the navigation device.

[0037] Another aspect of the invention relates to a navigation device comprising a processing unit, three accelerometers, three gyroscopes, and a device for measuring, for example, the speed of the navigation device. The processing unit is configured to implement the navigation assistance method.

[0038] Another aspect of the invention relates to a computer program product comprising program code instructions for executing the steps of the navigation assistance process. DESCRIPTION DES FIGURES

[0039] Other features and advantages of the invention will become apparent from the following description, which is purely illustrative and not limiting, and should be read in conjunction with the accompanying figures, in which: there figure 1 represents a navigation device of the invention, the figure 2 represents one embodiment of the navigation method of the invention, the figure 3 represents a second embodiment of the navigation method of the invention, and the figure 4 represents, in another way, the second embodiment of the navigation method of the invention. DESCRIPTION DETAILLEE DE L'INVENTION

[0040] There figure 1 This schematically represents a navigation device (DISP). This navigation device (DISP) includes a processing unit (UNIT). This processing unit (UNIT) includes a general-purpose or specialized processor or microcontroller and memory.

[0041] The processor or microcontroller can be an application-specific integrated circuit ( Application-Specific Integrated Circuit (for ASIC in English), it can also be a programmable logic circuit or programmable logic network ( Field-Programmable Gate Array (for FPGA in English).

[0042] The memory can be fixed or removable and may include different memory units, potentially combining volatile and non-volatile storage. The memory is configured to store software code usable by the processor or microcontroller to perform a process for determining the respective values ​​of kinematic variables of the DISP navigation device.

[0043] The values ​​of the kinematic variables allow the localization of the DISP navigation device and therefore the navigation of the wearer of this device.

[0044] The DISP navigation system also includes three accelerometers 101-a to 101-c and three gyroscopes 102-a to 102-c.

[0045] In addition, the DISP navigation device may optionally include a measuring device 103 for a position of the DISP navigation device.

[0046] The DISP navigation device may also include other devices for measuring a kinematic variable or a combination of several kinematic variables or additional variables also estimated by the Kalman filter of the DISP navigation device.

[0047] Furthermore, the DISP navigation device may also include a measuring device 104 for the distance traveled by the wearer of the DISP navigation device. This measuring device 104 is, for example, an odometer 104.

[0048] The three accelerometers 101-a to 101-c are capable of delivering specific force data. The three accelerometers are associated respectively with three axes that can be orthogonal to each other.

[0049] The three gyroscopes 102-a to 102-c are capable of providing angular velocity data. Each of the three gyroscopes is associated with three axes, which may be orthogonal to each other.

[0050] Sensor defects can include, for example, biases that are constant or proportional to a known electrical signal sent to the inertial sensors. In the case of biases proportional to an electrical signal, it is advantageous to send the same signal to all three accelerometers and / or all three gyroscopes.

[0051] More specifically, accelerometers measure a specific force f n The DISP navigation device and gyroscopes measure an angular velocity of the DISP navigation device. This angular velocity is then transformed into a rotation matrix. Ω n representative of the rotation of the device. The time interval between two measurements is noted dt.

[0052] In one embodiment the specific strength f n is corrected / modified using estimated sensor fault values.

[0053] In one embodiment the rotation matrix Ω n representative of the device's rotation is corrected / modified using the estimated values ​​of the sensor defects.

[0054] Accelerometers and gyroscopes can provide either specific forces and angular velocities, or directly variations in velocity and angle.

[0055] The measurement device 103 for a position of the DISP navigation device is, for example: a satellite navigation receiver, such as a GPS-type receiver for Global Positionning System in English or a Galileo-type receiver, a device performing triangulation using landmarks whose position is known, or a laser remote sensing device ( Light Detection And Ranging (for LIDAR in English) of a set of known landmarks allowing the vehicle's position to be calculated.

[0056] If the position measuring device 103 is not co-located with the navigation device DISP, the distance between the measuring device 103 and the navigation device DISP is considered a sensor defect. This sensor defect can be known or estimated at the time of implementation of the method of the invention.

[0057] The data delivered by the three accelerometers 101-a to 101-c, by the three gyroscopes 102-a to 102-c, and possibly by the position measuring device 103 and by the odometer 104 are received by the processing unit UNIT.

[0058] The processing unit UNIT is configured by implementing the navigation process through the determination of respective values ​​of variables in the navigation device DISP. This process is represented in figure 2 It enables the location of the DISP navigation device and therefore the navigation of the person carrying this device.

[0059] These variables include kinematic variables which are, for example, a position, a velocity or an orientation matrix of the device representing the change of coordinates from a moving frame attached to the carrier to a reference frame.

[0060] These variables also include sensor fault variables whose value is representative of faults in the measurement of one or more sensors (e.g., accelerometer or gyroscope bias).

[0061] Throughout this document, it is assumed that: The moving frame of reference is attached to the carrier; therefore, knowing this frame of reference requires knowing the carrier's orientation. The reference frame, considered fixed, is the one in which we want to know the carrier's coordinates. This is generally a terrestrial, inertial, or geographic frame of reference. The auxiliary frame of reference is the one used to write the error variables that remain invariant during the update. This frame of reference is known relative to the reference frame and can be moved regularly to bring it closer to the estimated position of the carrier.

[0062] The process of figure 2 includes the following steps: acquisition 201 of prior values ​​of kinematic variables of the navigation device and of prior values ​​of fault variables on one of the sensors of the navigation device DISP (this latter prior value advantageously being zero), determination 202 of respective current values ​​of the kinematic variables of the navigation device DISP, of respective current values ​​of the sensor fault variables and of a current uncertainty matrix representing an uncertainty of the respective current values ​​of the kinematic variables and of the sensor fault variables, from respective previous values ​​of the kinematic variables, of respective previous values ​​of the sensor fault variables and of a previous uncertainty matrix representing an uncertainty of the respective previous values ​​of the kinematic variables and of the sensor fault variables,Determination 203 of a correction from: the respective current values ​​of the kinematic variables and a measurement, for example, a measurement of one of the kinematic variables or one of the sensor error variables, an uncertainty matrix and a gain matrix, the gain matrix being determined from the uncertainty matrix, updating 204 of the respective current values ​​of the kinematic variables, the sensor error variables and the current uncertainty matrix from the correction, a first transformation 205 of the updated uncertainty matrix. This first transformation 205 is a first change of error variable applied to the updated uncertainty matrix. This first change of error variable allows us to move from a first system of error variables defined for the kinematic variables in a reference frame (for example, a geocentric geographic reference frame,inertial or heliocentric) and, for sensor error variables in a moving frame attached to the carrier, to a second system of error variables, defined as the error variables invariant in a single auxiliary frame used for both kinematic variables and sensor errors. This auxiliary frame can be moved over time (for example, regularly repositioned on the estimated position of the carrier). Error variables refer to the representation of estimation errors of the state variables of the DISP device.

[0063] Following the first change of error variable, the variables of the uncertainty matrix are invariant error variables (in the sense given in the state of the art section) in the auxiliary frame.

[0064] There are two types of invariant error variables, defined in the prior art section as left-invariant and right-invariant. Left-invariant error variables are chosen if measurements are taken in a fixed reference frame, and right-invariant error variables if measurements are taken in a moving frame attached to the carrier.

[0065] The process of figure 2 finally includes a second transformation 206 of the uncertainty matrix which has undergone the first transformation 205. This second transformation 206 corresponds to a second change of error variable, applied to the uncertainty matrix which has undergone the first transformation 205. This second change of error variable allows us to go back from the second system of error variables to the first system of error variables.

[0066] In an extended Kalman filter, we know the use of linearization points called system points which allow us to obtain a linear approximation of the system: at each time step, a linearization point is used to calculate the matrix F of the step of calculating the current value of the covariance matrix and another linearization point is used to calculate the matrix H of the update step.

[0067] The first linearization point of the error variables of the invention must coincide with the linearization point of the system used for the calculations of the update step, while the second linearization point of the error variables of the invention must coincide with the linearization point of the system used during the calculation step of the current uncertainty matrix of the following instant.

[0068] In one embodiment, the first linearization point is determined from the current values ​​before the update, and the second linearization point is determined from the current values ​​after the update.

[0069] Thus, in this embodiment, the first linearization point and the second linearization point are different.

[0070] The change of error variable in a covariance matrix P l< representing the uncertainty of an error variable e l< is a process for obtaining, using a first-order Taylor expansion of a second error variable e c< with respect to the error variable e l< , a second covariance matrix representing the uncertainty of the second error variable e c< It allows, for example, changing the coordinate system of the first uncertainty of the variable. Thus, in the case where the variable is a position in a plane, a first uncertainty of this position variable is expressed in polar coordinates having the form e r e θ and contains an uncertainty e r of the distance from the origin r and an uncertainty e θ from the angle θ We can write the same uncertainty using Cartesian coordinates. e x e y using the error variable change formula: e x e y = L r , θ e r e θ

[0071] With L r , θ = cos θ − r . sin θ sin θ r . cos θ Or ( r , θ ) are the polar coordinates of the point with respect to which the uncertainty is considered. This point is also called the linearization point of the change in the error variable. L r,θ is therefore a matrix of change of error variable, which allows the covariance matrix to be transformed P l< uncertainties defined in polar coordinates in a covariance matrix P c< in which the uncertainties are defined in Cartesian coordinates by the following formula: P c = L r , θ P l L r , θ t

[0072] We can see that the new uncertainty matrix depends on the old one, but also on the coordinates ( r , θ ) from a linearization point.

[0073] A change of error variable is therefore an operation that takes a covariance matrix, a first system of error variables, a second system of error variables, and a linearization point, and returns a new covariance matrix. A different linearization point leads to a different returned matrix.

[0074] Thus, since the linearization point of the first error variable change and the linearization point of the second error variable change are different, the second error variable change does not cancel out the first error variable change.

[0075] The first change of error variable can be achieved by a first matrix and the second change of error variable can be achieved by a second matrix.

[0076] The first and second matrices can be determined analytically in advance, as well as their product. Then, these two matrices, or their product, can be used to perform the first and second error variable changes.

[0077] A moving frame attached to the carrier is understood to be a frame aligned and centered on the DISP device. An auxiliary frame is understood to be a fully known frame used for calculations, which can be fixed or moved regularly near the estimated position of the carrier to avoid numerical problems. It can also be assigned a velocity close to the estimated velocity of the carrier.

[0078] The correction is determined by a transformation matrix or gain matrix (denoted K (later). This gain matrix K is calculated from an observation matrix (denoted H (subsequently). The observation matrix H In the prior art, a first-order relationship is established between an error in estimating the system state and an error in predicting the measurement. The gain matrix K uses the observation matrix Hto perform the reverse operation: determine a correction of the state from a prediction error of the observed measurement.

[0079] In a Kalman filter (linear, extended or invariant) an observation is defined as a function of the state predicting the measurement of a sensor, for example a GPS receiver providing a position observation or an odometer providing a speed observation in the carrier's frame of reference.

[0080] The kinematic variables of the device include: an orientation of the device, the value of which is a matrix T of current or previous orientation, of size 3 by 3, this orientation can also be represented by a quaternion, a velocity of the device, whose value is a vector V of current or previous velocity, of size 3, and a position of the device, whose value is a vector Xof current or previous position, of size 3. of sensor fault variables of which three will appear in the equations below: accelerometer measurement biases, gyroscope measurement biases and factors leading to gyroscope biases when multiplied by known electrical signals.

[0081] Furthermore, an uncertainty matrix P is used, representing the uncertainty of the kinematic variables and the sensor error variables. This matrix is ​​a covariance matrix.

[0082] In the rest of the document, variables (matrices or vectors) bearing a circumflex accent represent estimated variables, the corresponding real variables are noted without a circumflex accent.

[0083] The process includes determining the value of these variables, which are denoted respectively T̂ n | n , V̂ n | n and X̂ n |n for kinematic variables and d̂ n | n , d ^ n n u , b̂ n | n for sensor fault variables. The process also includes determining the covariance matrix P n | n representative of the uncertainty of the current estimate. We also assume that a covariance matrix P 0|0, representing the initial uncertainty, is available at the beginning of navigation.

[0084] The index n here represents the time step and, classically in Kalman filtering, the index n | n represents the estimated value at time n, taking into account the observation made at time n and the index n | n - 1 represents the estimated value at that moment n without taking into account the observation made at the moment n .

[0085] In the invention, the first system of error variables is arbitrary, but to arrive at the implementation that follows, the following first system of variables was chosen: e T = T ^ t T e V = T ^ t V − V ^ e X = T ^ t X − X ^ e d = d − d ^ e d u = d u − d ^ u e b = b − b ^

[0086] We can see that the error variables e b< , e d< , e du< are not invariant error variables.

[0087] Determination 202 uses the following equations: V ^ n n − 1 = V ^ n − 1 n − 1 + dt . T ^ n − 1 n − 1 f n − b ^ n n − 1 + g n X ^ n n − 1 = X ^ n − 1 n − 1 + dt . V ^ n − 1 n − 1 T ^ n n − 1 = T ^ n − 1 n − 1 R dt ω n − d ^ n n − 1 − U n d ^ n n − 1 u d ^ n n − 1 = d ^ n − 1 n − 1 d ^ n n − 1 u = d ^ n − 1 n − 1 u b ^ n n − 1 = b ^ n − 1 n − 1 P n n − 1 = F n P n − 1 n − 1 F n t + Q n With : V̂ n | n -1 is the current velocity vector, V̂ n -1| n -1 is the previous velocity vector, X̂ n | n -1 is the current position vector, X̂ n -1| n -1 is the previous position vector, T̂ n | n-1 is the current orientation matrix; thus, the current orientation matrix is ​​calculated from the angular velocities measured by the gyroscopes, modified using the estimated sensor fault variables. T̂ n -1| n -1 is the previous orientation matrix, b̂ n -1| n -1 represents a vector of three previous biases on the measurements of the three accelerometers, b̂ n | n -1 represents a vector of three common biases in the measurements of the three accelerometers, d̂ n -1| n -1 represents a vector of three preceding biases on gyroscope measurements, d̂ n | n -1 represents a vector of three current biases on the measurements of the three gyroscopes, thus in this embodiment the vector of three current biases is obtained by copying the preceding vector of three biases. d ^ n − 1 n − 1 u represents a vector containing three previous factors whose products are three known electrical signals u n − 1 1 , u n − 1 2 , u n − 1 3 three additional biases are introduced into the measurements of the three gyroscopes. d ^ n n − 1 u represents a vector containing three current factors whose products are three known electrical signals u n 1 , u n 2 , u n 3 three additional biases are introduced into the measurements of the three gyroscopes. g n is a model of the gravity experienced by the DISP navigation device, f n is the specific force derived from the accelerometers, dt is the time interval between the instants n - 1 and n , ω n is the angular velocity measurement provided by the gyroscope. U n = u n 1 0 0 0 u n 2 0 0 0 u n 3 is a 3 × 3 matrix containing on its diagonal the three known electrical signals u n 1 , u n 2 , u n 3 proportional to sensor defect components, Ω n = R dt ω n − d ^ n n − 1 − U n d ^ n n − 1 u is the rotation calculated between the orientation at the previous instant and the orientation at the current instant, R(·) denotes the function returning a rotation matrix from a rotation vector, − F n = Ω n t 0 3 0 3 − D R ω ^ n − D R w ^ n U n 0 3 − dt . Ω n t f ^ n × Ω n t 0 3 0 3 0 3 − dt . Ω n t 0 3 dt . Ω n t Ω n t 0 3 0 3 0 3 0 3 0 3 0 3 I 3 0 3 0 3 0 3 0 3 0 3 0 3 I 3 0 3 0 3 0 3 0 3 0 3 0 3 I 3 Or ω ^ n = ω n − d ^ n n − 1 − U n d ^ n n − 1 u is the angular velocity corrected for estimated drifts, f̂ n = f n - b̂ n | n -1 is the specific strength corrected for estimated biases and D R ( ω̂ n ) is defined by the following Taylor expansion in µ : R ( ω̂ n + µ ) = R( ω̂ n ) R ( D R ( ω̂ n ) µ ) +∘ (|| µ ||), Q n is a covariance matrix representing the uncertainty added by each propagation step of the kinematic variables and sensor error variables. The main sources of this uncertainty are the imprecision of measurements from accelerometers and gyroscopes and the uncertain evolution of the modeled bias vectors. The exact value to be given to Q n is determined using specifications provided by the manufacturer of the DISP navigation device, ( l ) × corresponds to an antisymmetric matrix constructed with the components of the vector l , this matrix is ​​such that for any vector v we have ( l ) × v = l × v where × is a cross product, P is a covariance matrix whose diagonal values ​​represent the uncertainties of each state variable and whose non-diagonal values ​​represent the cross uncertainties between the variables. Pn -1| n -1 is the previous uncertainty matrix, P n | n -1 is the current uncertainty matrix.

[0088] In one embodiment, step 203 of determining a correction ds is triggered when a DISP device stoppage is detected, and follows the following equations: z n = 0 − T ^ n n − 1 T V ^ n n − 1 K n = P n n − 1 H n t H n P n n − 1 H n t + R n − 1 ds = K n z n With : ds the correction. R n A covariance matrix is ​​used to represent measurement errors and unmodeled quantities. It may or may not depend on the estimated kinematic variables. H n = T ^ n n − 1 t V ^ n n − 1 × I 3 O 3 , 12 the observation matrix which allows linking the speed variations in the carrier frame to the error variables of the DISP navigation device, K n is a gain matrix that transforms the error on the velocity vector into a correction to be applied to the other kinematic variables and O3,12 is a zero matrix of size 3 × 12.

[0089] The zero appearing to the right of the "=" sign in the definition of z n is the speed observation, which here is always zero, because the additional measurement is a stop detection.

[0090] ds is a vector of size 18. The first three components ( ds 1:3 ) correspond to the rotational error. The following three components ( ds 4:6 ) correspond to the speed error. The following three components ( ds 7:9) correspond to the positional error, the following three components ds 10 :12 correspond to the bias errors of the gyroscopes, the following three components ds 13:15 correspond to the errors in factors to be applied to electrical signals u n 1 , u n 2 , u n 3 To calculate three additional biases to be applied to the three gyroscopes, the following three components ds 16:18 correspond to the bias errors of the accelerometers.

[0091] The determination 203 of a correction makes it possible to determine the estimation error of all the kinematic variables and sensor fault variables of the navigation device from the measurement, which in the particular case we describe here is a speed measurement of the device.

[0092] This determination of the gap is performed by the gain matrix K n , which takes into account the uncertainties on the kinematic variables of the DISP navigation device. If there is a small uncertainty, the zero speed measurement is taken into account in a small way and, if there is a large uncertainty, the zero speed measurement is taken into account in a large way.

[0093] Update 204 uses the ds correction to perform the following equations: T ^ n n = T ^ n n − 1 R ds 1 : 3 V ^ n n = V ^ n n − 1 + T ^ n n − 1 ds 4 : 6 X ^ n n = X ^ n n − 1 + T ^ n n − 1 ds 7 : 9 d ^ n n = d ^ n n − 1 + ds 10 : 12 d ^ n n u = d ^ n n − 1 u + ds 13 : 15 b ^ n n = b ^ n n − 1 + ds 16 : 18 P n n = I − K n H n P n n − 1

[0094] In another embodiment, the update uses the ds correction to perform the following modified equations: T ^ n n = T ^ n n − 1 R ds 1 : 3 V ^ n n = V ^ n n − 1 + R ds 1 : 3 V ds 1 : 3 ds 4 : 6 X ^ n n = X ^ n n − 1 + R ds 1 : 3 V ds 1 : 3 ds 7 : 9 d ^ n n = R ds 1 : 3 t I 3 + V ds 1 : 3 ds 1 : 3 × d ^ n n − 1 + R ds 1 : 3 t V ds 1 : 3 ds 10 : 12 d ^ n n u = R ds 1 : 3 t I 3 + V ds 1 : 3 ds 1 : 3 × d ^ n n − 1 u + R ds 1 : 3 t V ds 1 : 3 ds 13 : 15 b ^ n n = R ds 1 : 3 t I 3 + V ds 1 : 3 ds 1 : 3 × b ^ n n − 1 + R ds 1 : 3 t V ds 1 : 3 ds 16 : 18 P n n = I − K n H n P n n − 1 Avec V α = I 3 + 1 − cos α α 2 α × + α − sin α α 3 α × 2 .

[0095] The first change of error variable, using the state before the update as the linearization point, and the reference frame as the auxiliary frame, is written: P n n + = L n n − 1 P n n L n n − 1 t With L n n − 1 = T ^ n n − 1 0 3 0 3 0 3 0 3 0 3 V ^ n n − 1 × T ^ n n − 1 T ^ n n − 1 0 3 0 3 0 3 0 3 X ^ n n − 1 × T ^ n n − 1 0 3 T ^ n n − 1 0 3 0 3 0 3 0 3 0 3 0 3 T ^ n n − 1 0 3 0 3 0 3 0 3 0 3 0 3 T ^ n n − 1 0 3 0 3 0 3 0 3 0 3 0 3 T ^ n n − 1

[0096] L n | n -1 is a transformation matrix allowing the first change of error variable to be made.

[0097] The second change of error variable, taking the state after the update as the linearization point, is written: P n n + + = L n n − 1 P n n + L n n − 1 t With L n n = T ^ n n 0 3 0 3 0 3 0 3 0 3 V ^ n n × T ^ n n T ^ n n 0 3 0 3 0 3 0 3 X ^ n n × T ^ n n 0 3 T ^ n n 0 3 0 3 0 3 0 3 0 3 0 3 T ^ n n 0 3 0 3 0 3 0 3 0 3 0 3 T ^ n n 0 3 0 3 0 3 0 3 0 3 0 3 T ^ n n

[0098] L n | nis a transformation matrix that allows the second error variable change to be performed.

[0099] As indicated in one of the embodiments, it is also possible to calculate L n tot = L n n − 1 L n n − 1 and apply both error variable changes in a single operation: P n n + + = L n tot P n n L n tot t

[0100] The matrix L n tot is a change-of-reference matrix that allows for the simultaneous implementation of the first and second error variable changes. This matrix L n tot is particularly simple here: L n tot = R ds 1 : 3 T 0 3 , 3 0 3 , 3 0 3 , 3 0 3 , 3 0 3 , 3 − R ds 1 : 3 T s 4 : 6 × R ds 1 : 3 T 0 3 , 3 0 3 , 3 0 3 , 3 0 3 , 3 − R ds 1 : 3 T s 4 : 6 × 0 3 , 3 R ds 1 : 3 T 0 3 , 3 0 3 , 3 0 3 , 3 0 3 , 3 0 3 , 3 0 3 , 3 R ds 1 : 3 T 0 3 , 3 0 3 , 3 0 3 , 3 0 3 , 3 0 3 , 3 0 3 , 3 R ds 1 : 3 T 0 3 , 3 0 3 , 3 0 3 , 3 0 3 , 3 0 3 , 3 0 3 , 3 R ds 1 : 3 T

[0101] Steps 202 to 206 of the process are repeated throughout navigation.

[0102] In particular, the corrected velocity vector V̂ n | n becomes the next previous velocity vector, the corrected position vector X̂ n | nbecomes the next previous position vector, the corrected orientation matrix T̂ n | n becomes the next previous orientation matrix and the corrected sensor fault states of n | n , d n n u , b n | n become the next fault states of previous sensors.

[0103] This process allows the location of the DISP navigation device and therefore the navigation of the person carrying this device.

[0104] It is possible that any number of sensor fault error variables defined in the measurement frame can be estimated by the method of the invention. In addition to handling groups of three variables concatenated in a vector, the method can also handle groups of nine variables representing a 3 × 3 matrix denoted M but concatenated in a vector µof size 9 for the purposes of Kalman filtering. This vector can be written using the coordinates of M in the form: μ = M 1 , 1 M 2 , 1 M 3 , 1 M 1 , 2 M 2 , 2 M 3 , 2 M 1 , 3 M 2 , 3 M 3 , 3

[0105] The invariant error variables defined so far for vectors of size 3 must then be adapted to the vector µ of size 9 by replacing the matrix T of the transition from the moving frame to the auxiliary frame by the transformation matrix of the vector µ written in a moving frame of reference to the vector µ written in an auxiliary coordinate system. This formula is derived from the change of coordinate system formula. M a< = TM m< T T< of the matrix M which translates onto the vector µ by the formula µ a< = ( T ⊗ T ) µ m< where ⊗ denotes the Kronecker product (the matrix T ⊗ T is therefore of size 9 × 9) and where the exponents a And mdenote the quantities written respectively in an auxiliary frame or in a moving frame attached to the carrier. From this equation, we deduce an adaptation of the left-invariant error variable in the auxiliary frame, which is used in the first step of changing the error variable of the invention and in the second step of changing the error variable of the invention: e μ = T ^ ⊗ T ^ T μ a − μ ^ a = T ^ ⊗ T ^ T T ⊗ T μ m − μ ^ m

[0106] We can also deduce an adaptation of the right-invariant error variable in the auxiliary frame, which can also be used in the first and second error variable change steps of the invention: e μ = μ a − T ⊗ T T ^ ⊗ T ^ T μ ^ a = T ⊗ T μ m − μ ^ m

[0107] In the implementation described above, if such a vector µ is added following the other estimated quantities; from these adapted error variables, an adaptation to be made to the matrix is ​​deduced. L n tot which becomes a matrix L n tot ′ defined from L n tot by adding an additional 9×9 block associated with the vector µ : L n tot ′ = L n tot 0 18 , 9 0 9 , 18 R ds 1 : 3 T ⊗ R ds 1 : 3 T

[0108] It is also possible that parameters (contained in a vector) C r< ) describing the vehicle's environment in the reference frame are estimated simultaneously with the other variables. If these parameters are grouped into vectors of size 3, invariant error variables can be associated with them, and the steps of the invention can be followed. However, these parameters can be more complex and describe, for example, a magnetic field or a field of wind or current velocity vectors covering the entire space. The invention still applies, but the rotation T The change of reference frame appearing in invariant errors and changing from coordinates in a moving frame attached to the carrier to coordinates in an auxiliary frame must be replaced by a change of reference frame operation. ϕ T,V ,X passing the parameters C m< of the environment in a mobile reference frame attached to the wearer with parameters C a< of the environment in an auxiliary frame. From this, we deduce an adaptation of the left-invariant error variable in the auxiliary frame, which is used in the first error variable change step of the invention and in the second error variable change step of the invention: e C = ϕ T ^ , V ^ , X ^ − 1 C a − C ^ a

[0109] Where the exponent -1 denotes the inverse transformation. We also deduce an adaptation of the right-invariant error variable in the auxiliary frame, which can also be used in the first and second error variable change steps of the invention: e C = C a − ϕ T , V , X ∘ ϕ T ^ , V ^ , X ^ − 1 C ^ a

[0110] Where the exponent -1 denotes the inverse transformation and the symbol ∘ denotes the composition of two transformations.

[0111] For example, the environment could be a magnetic field B defined at every point P a< of the auxiliary coordinate system by a formula of the form B a P a = C 0 a + C 1 a P a : the parameters describing this environment are then the vector C 0 a of size 3 and the matrix C 1 of size 3 × 3. The same field is written in the moving frame attached to the carrier by the formula B m P m = C 0 m + C 1 m P m with C 0 a = TC 0 m − TC 1 m T T X a And C 1 a = TC 1 m T T The transformation of the coordinate system of the field is then: ϕ C 0 C 1 <mprescripts / > T , V , X <none / > = TC 0 − TC 1 T T X , TC 1 T T

[0112] This change of variable allows us to obtain the invariant error variables that constitute the second set of error variables, and thus to implement the invention. In particular, if we wish to supplement the implementation already described by estimating the parameters C r< of a magnetic field written in the reference frame, here coinciding with the auxiliary frame, using the error variable C r< - Ĉ r< in the first set of error variables and the invariant error variable e C = C r − ϕ T , V , X ∘ ϕ T ^ , V ^ , X ^ − 1 C ^ r In the second set of error variables, the adaptation of the propagation and update steps is conventional, but the matrix L n tot of the invention is augmented into a matrix L n tot ′ : L n tot ′ = L n tot 0 18 , c Dϕ C ^ n n r − Dϕ C ^ n n − 1 r Ad n n − 1 0 c , 9 I c , c

[0113] Where c is the dimension of the vector C r< magnetic field parameters and Dϕ C , is a matrix of size 9 × c defined by the following Taylor series expansion: ϕ R ξ , dv , dx C = Dϕ C ξ dv dx + ∘ ξ dv dx

[0114] And Ad n | n And Ad n | n -1 are defined by the formula: Ad n n = T n n 0 3 , 3 0 3 , 3 V ^ n n × T n n T n n 0 3 , 3 X ^ n n × T n n 0 3 , 3 T n n , Ad n n − 1 = T n n − 1 0 3 , 3 0 3 , 3 V ^ n n − 1 × T n n − 1 T n n − 1 0 3 , 3 X ^ n n − 1 × T n n − 1 0 3 , 3 T n n − 1

[0115] The environment described by the parameters of C is here a magnetic field but can be for example a binary function taking the value 1 for places occupied by an object and 0 for empty places, or taking a value related to a color observed at the place considered.

[0116] When the errors of a sensor group are defined as the dot products of two-dimensional vectors to be estimated and known vectors that may vary over time, the first change in the error variable can be preceded by applying a rotation of the plane by a different angle for each sensor in the group, and the second change can be followed by a reverse rotation. The angle of this rotation is the average angle between the known vector associated with that sensor and the vector associated with one of the sensors in the group chosen as the angular reference.

[0117] There figure 3 represents another embodiment of the method for determining the respective values ​​of the kinematic variables of the DISP navigation device and the sensor fault variables. In this embodiment, steps 202 to 206 are duplicated into two branches and carried out in parallel. Then, at different times, the estimates of the two branches are merged in a merging step 401 (shown in the figure 4 ). There figure 4 This represents, in another way, this implementation. At each time step, the estimates from the two branches are extracted and combined to provide a consolidated estimate, but this estimate never returns to the branches. It is only provided as output. Thus, the branches remain independent from beginning to end.

[0118] As depicted on the figure 4 Steps 202-a to 206-a form the steps of the first branch. Steps 202-b to 206-b are the steps of the second branch.

[0119] To be more precise, the figure 3 is a special case of the process shown on the figure 4 .

[0120] In this embodiment, the vehicle navigation assistance process comprises the following steps: determination 202-a of first current values ​​of the variables and of a first current uncertainty matrix representing an uncertainty of the first current values, from previous first values ​​of the variables and a previous first uncertainty matrix, determination 203-a of a first correction from: the first current values ​​of the variables, the first current uncertainty matrix and a first measurement, update 204-a of the first current values ​​and of the first current uncertainty matrix from the first correction and the first current uncertainty matrix, first transformation 205-a of the first current uncertainty matrix in order to obtain a first modified uncertainty matrix, this first transformation 205-a is a first change of variable according to a first linearization point, and second transformation 206-a of the first modified uncertainty matrix,The second transformation 206-a is a first change of error variable according to a second linearization point, the first linearization point being different from the second linearization point.

[0121] The process also includes: a determination 202-b of respective second current values ​​of the variables of a DISP navigation device and of a second current uncertainty matrix representing an uncertainty of the respective second current values ​​of the variables, from respective second previous values ​​of the variables and of a second previous uncertainty matrix representing an uncertainty of the respective second previous values ​​of the variables, a determination 203-b of a second correction from: ∘ the respective second current values ​​of the variables, ∘ the second current uncertainty matrix and ∘ a second measurement which is possibly identical to the first measurement, an update 204-b of the respective second current values ​​of the variables and of the second current uncertainty matrix from the second correction and the second current uncertainty matrix,a first transformation 205-b of the second uncertainty matrix to obtain a second modified uncertainty matrix, the first transformation 205-b being a first change of error variable according to a third linearization point, a second transformation 206-b of the second modified uncertainty matrix, the second transformation 206-b being a change of error variable according to a fourth linearization point, , the third linearization point being different from the fourth linearization point.

[0122] The 401 merge step then allows for obtaining a single estimate of the system state. It is also possible to have N different branches identified by an integer. i In this case, each branch i is accompanied by a deviation indicator from the measurements s n i (Or n(denotes a time step). This deviation indicator from measurements is used by the 401 merge step.

[0123] This indicator can, for example, be initialized to zero and updated as follows at each instant n : s n i = s n − 1 i + z n i T S n − 1 z n i Or z n i is the so-called "innovation" vector appearing in the branch equations i And S n i = H n i P n n − 1 i H n i T + R n i is the matrix called "innovation covariance" constructed from the matrices H n i , P n n − 1 i , R n i also appearing in the branch equations i .

[0124] In one embodiment, the 401 merge step allows the best branch to be selected, which will be branch i associated with the indicator s n i the weakest.

[0125] In one embodiment, the 401 merge step can perform a weighted average of the states returned by the different branches i, the weights p n i used depending on the indicators of deviation from the measurements s n i They can, for example, be calculated using the following formula: p n i = exp − s n i ∑ j exp − s n j

[0126] This implementation is particularly useful when the initial orientation of the carrier is unknown. In this case, each branch is implemented with a different initialization.

[0127] Step 401 can also implement a statistical test verifying the similarity or consistency of the two estimators, to obtain an estimator with better integrity than a classical Kalman filter. The likelihood test uses the matrix P n n ′ stemming from branch a and P n n " originating from branch b and verifying the following relationships p Δs " 0 , P n n " > α And p Δs ′ 0 , P n n ′ > α Or Δs ' (resp. Δs" ) is a vector representing the difference between the two navigation states from branches a and b expressed in the same system of error variables as P' (resp. P "), p ( s |0, P ) is the density of the centered multivariate normal distribution with covariance matrix P , evaluated at the point s, And α is a predetermined threshold. The difference Δ s This could be, for example, the logarithmic error characteristic of invariant filtering. This test can be summarized by saying that it verifies that each estimate assigns a high probability to the other estimate.

[0128] Another possible test is to calculate a difference between the two normal distributions returned by the two filters, this difference being, for example, the Kullback-Leibler divergence defined by the formula: 1 2 tr P ′ − 1 P ˜ " + Δ s ′ T P ′ − 1 Δ s ′ − k − ln det P ′ det P ˜ " Or tr() is the trace function, kis the dimension of the system state (k=18 if orientations, velocities and positions are estimated as well as the two types of gyroscope bias and accelerometer bias), In() denotes the logarithm function, det() the determinant function, P̃" is the matrix P" written in the same coordinate system as P' (The two Kalman filters of branches a and b can represent their errors in different error variable systems.) Alternatively, the roles of P ' And P" : 1 2 tr P " − 1 P ˜ ′ + Δ s " T P " − 1 Δ s " − k − ln det P " det P ˜ ′

[0129] The test will be positive if the difference thus defined remains below a previously fixed threshold.

[0130] If the test is positive, the estimate from one of the two branches is returned. Alternatively, the mean of the variables estimated by the two branches can be returned. Another possibility is to start from the state s 'and to define a correction δs' = P' [ P' + P̃" -1 < Δ s' which will be applied to s' using the same formulas as the correction ds' of the update step.

[0131] Conversely, we can start from the state ds" and define a correction δs" = P" [ P" + P̃' -1 < Δ s" which will be brought to s "using the same formulas as the correction ds" of the update step.

[0132] This embodiment is particularly advantageous if auxiliary measurements are taken by different sensors in different reference frames (for example, a GPS receiver providing a position measurement in a terrestrial reference frame and an odometer providing a speed measurement in a moving reference frame attached to the wearer). In this case, each of the two branches will use measurements from only one sensor, and the error variable systems involved in the invention may differ for the different branches.

[0133] In alternative embodiments, the linearization points of the extended Kalman filter improved by the invention can be chosen in advance, from another filter, from a consolidated estimation from several filters, obtained by modifying the value of the estimated state (for example, by correcting its altitude while preserving its latitude, longitude, and other estimated variables), or constructed in any other way from the variables estimated by the invention.

Claims

1. Method for assisting in the navigation of a vehicle, comprising the following steps: - acquiring (201) a priori values of variables of a navigation device (DISP) of the vehicle, - determining (202) current values of the variables and a current uncertainty matrix, based on previous values of the variables and a previous uncertainty matrix, - determining (203) a correction based on: - the current values of the variables, - the current uncertainty matrix, and - a measurement of a variable, the correction being intended to correct the current values and the current uncertainty matrix, - updating (204) the current values and the current uncertainty matrix based on the correction and the current uncertainty matrix, the method being characterized in that it comprises the following steps: - first transformation (205) of the current uncertainty matrix in order to obtain a modified uncertainty matrix, the first transformation (205) being a first change of error variable according to a first linearisation point, the first error variable change allowing the transition from a first system of error variables to a second system of error variables, so as to transfer all the error variables to the same reference frame, and - second transformation (206) of the modified uncertainty matrix, the second transformation being a second error variable change according to a second linearisation point, the second error variable change allowing transition from the second error variable system to the first error variable system, the first linearisation point being different from the second linearisation point.

2. Method according to claim 1, in which the current and previous values of the variables are defined in a reference frame, the first error variable change is a change to invariant error variables in an auxiliary reference frame known relative to the reference frame, the second change of error variables is a change from invariant error variables in the known auxiliary reference frame relative to the reference frame.

3. Method according to one of claims 1 to 2, in which the first linearisation point is determined from the current values before the update (204) and the second linearisation point is determined from the current values after the update (204).

4. Method according to one of claims 1 to 2, wherein the first linearisation point is a system linearisation point used during the update step and the second linearisation point is a system linearisation point used during the step of determining (202) the current values of the next instant.

5. Method according to one of claims 1 to 4, wherein a product of a first transformation matrix performing the first transformation (205) and a second transformation matrix performing the second transformation (206) is determined, and the first transformation (205) and the second transformation (206) are performed together using the product.

6. Method according to one of claims 1 to 5, the variables comprising: - an orientation of the navigation device (DISP), - a speed of the navigation device (DISP), - a position of the navigation device (DISP), and - a fault on a sensor of the navigation device (DISP), whose current value is a current fault vector or matrix, the first change in error variable is a change in the error variables: - a current value of the orientation, - a current value of the speed, - a current value of the position, and - the current fault vector or matrix, to invariant error variables, in an auxiliary coordinate system known relative to a reference coordinate system.

7. Method according to one of claims 1 to 5, the variables comprising: - an orientation of the navigation device (DISP), whose current value is a current orientation matrix and whose previous value is a previous orientation matrix, - a navigation device speed (DISP), whose current value is a current speed vector and whose previous value is a previous speed vector, - a position of the navigation device (DISP), whose current value is a current position vector and whose previous value is a previous position vector, and - a fault on a navigation device sensor (DISP), whose current value is a current fault vector and whose previous value is a previous fault vector, the current uncertainty matrix being representative of an uncertainty of the current orientation matrix, the current velocity vector, the current position vector and the current sensor fault variables, and the previous uncertainty matrix being representative of an uncertainty of the previous orientation matrix, the previous velocity vector, the previous position vector and the previous sensor fault variables.

8. The method according to claim 7, wherein the current values are associated with a current time and the previous values are associated with a previous time, the determination (202) of the current values comprising: - determination of the current velocity vector by adding to the previous velocity vector an integration, over a time interval between the previous instant and the current instant, of a sum of a specific force of the navigation device (DISP), possibly corrected using the estimated values of sensor faults, and a model of the Earth's gravity experienced by the navigation device (DISP), - a determination of the current position vector by adding to the previous position vector an integration, over the time interval, of the previous velocity vector, - a determination of the current orientation matrix by multiplying the previous orientation matrix by a matrix representing a rotation of the navigation device (DISP), possibly corrected using the estimated values of sensor faults, and / or - determining the current uncertainty matrix from the previous uncertainty matrix.

9. Method according to claim 7 or 8, the measurement being a measurement of the speed of the navigation device (DISP), the determination (203) of the correction comprising: - subtracting from the velocity measurement a multiplication of the transpose of the current orientation matrix and the current velocity vector, and - multiplying the subtraction by a gain matrix.

10. A method according to any one of claims 7 to 9, wherein the correction is a correction vector, the updating (204) comprising: - a sub-step of updating the current orientation matrix by multiplying the current orientation matrix by a rotation matrix associated with a rotation vector that constitutes a first part of the correction vector, - a sub-step of updating the current velocity vector by adding to the current velocity vector a multiplication of the current orientation matrix and a second part of the correction vector, - a sub-step of updating the current position vector by adding to the current position vector a multiplication of the orientation matrix and a third part of the correction vector, and / or - a sub-step of updating current sensor fault vectors by adding a fourth part of the correction vector.

11. Method according to one of claims 7 to 10, the current values being first current values, the previous values being first previous values, the correction being a first correction, the measurement being a first measurement, the gain matrix being a first gain matrix, the uncertainty matrix being a first uncertainty matrix, the modified uncertainty matrix being a first modified uncertainty matrix, the method further comprising: - determining (202-b) second current values of the variables and a second current uncertainty matrix representative of an uncertainty of the second current values, from previous second values of the kinematic variables and a previous second uncertainty matrix representative of an uncertainty of the previous second values, - determining (203-b) a second correction based on: - the current second values, - the second current uncertainty matrix, and - a second measurement, - an update (204-b) of the second current values and the second current uncertainty matrix based on the second correction and the second current uncertainty matrix, - a first transformation (205-b) of the second uncertainty matrix in order to obtain a modified second uncertainty matrix, the first transformation (205-b) being a first change of error variable according to a third linearisation point, - a second transformation (206-b) of the second modified uncertainty matrix, the second transformation being a second change of error variable according to a fourth linearisation point, the third linearisation point being different from the fourth linearisation point.

12. Method according to claim 11, further comprising determining (401) consolidated values of the kinematic variables from the first current values and the second current values.

13. Method according to claim 12, the determination (401) of respective consolidated values of the kinematic variables comprising a determination of a similarity or a deviation between the first corrected values and the second corrected values and, when the similarity is greater than a similarity threshold or when the deviation is less than a deviation threshold, the determination (401) of respective consolidated values of the kinematic variables also comprising: - averaging the first corrected values and the second corrected values, or - a weighted averaging of the first corrected values and the second corrected values, or - selecting the first corrected values or the second corrected values.

14. Method according to claim 12, the determination (401) of respective consolidated values of the kinematic variables comprising: - determining a first deviation between the first corrected values and the first measurements, - determination of a second deviation between the second corrected values and the second measurements, and - selecting the first corrected values when the first deviation is less than the second deviation, or the second corrected values when the first deviation is greater than the second deviation.

15. Method according to one of claims 1 to 14, in which a current value of a variable representing the environment of the navigation device (DISP) is determined from a previous value of the variable representing the environment of the navigation device (DISP).

16. Navigation device (DISP) comprising: - a processing unit (UNIT), - three accelerometers (101-a to 101-c), - three gyroscopes (102-a to 102-c) and - a measuring device (103-a), for example for measuring the speed of the navigation device (DISP), the processing unit (UNIT) being configured to implement the navigation assistance method according to one of claims 1 to 15.

17. Computer program product comprising program code instructions for executing the steps of the navigation assistance method according to one of claims 1 to 15 when executed by a processor.