Method for navigating a vehicle using a Kalman filter to estimate the navigation state of the vehicle

Abstract

A method for navigating a vehicle by estimating a navigation state of a vehicle using a Kalman filter, the method comprising the steps of: obtaining (102) incremental ranges measured between the vehicle and a satellite and other measured kinematic data associated with the satellite based on a signal transmitted by the satellite and then received by the vehicle; generating (104) estimated incremental ranges between the vehicle and the satellite based on position data of the vehicle and the satellite in the navigation state; using the incremental ranges, calculating (106) incremental range updates associated with the satellite; performing a test (108) on the incremental range updates to produce a test result indicating whether the signal is a multipath signal; and if the test result indicates that the signal is not a multipath signal, using (114) the kinematic data as observations by the filter to update the navigation state.

Description

Technical Field

[0001] The invention relates to the field of navigation methods using Kalman filters. Background Art

[0002] A Kalman filter is a recursive estimator of data included in a state vector that evolves over time.

[0003] This filter is often used in the context of navigating a mobile vehicle in space. In this case, the state vector contains the dynamic data of the vehicle (e.g. position, velocity, acceleration, etc.). The method works by successive iterations, with each iteration taking as input the state vector produced by the previous iteration.

[0004] Each iteration consists of two basic steps: a prediction step (also called a propagation step) and an update step (also called a rescaling step in some applications).

[0005] The updating step is based on the observations provided by the sensors.

[0006] In particular, it has been proposed to use a GPS receiver as such a sensor. In this case, the Kalman filter uses positioning signals transmitted from different satellites and then received by the GPS receiver as observation data.

[0007] A signal transmitted by one of these satellites may reflect off obstacles (such as Figure 1 This obstacle can also be the ground, a building, or even the fuselage of the aircraft itself, which is called a "ground bounce". In this case, the signal is said to experience multipath.

[0008] However, this multipath phenomenon introduces errors in the data used as observations by the Kalman filter. These errors affect the accuracy of the update performed by the Kalman filter to switch from the a priori state vector to the a posteriori state vector. Summary of the invention

[0009] It is an object of the invention to estimate the navigation state of a vehicle based on data sent from satellites in a manner that is more robust to errors.

[0010] To this end, according to a first aspect, a method for navigating a vehicle using a Kalman filter to estimate the navigation state of the vehicle is proposed, the method comprising the following steps:

[0011] - obtaining incremental distances measured between said vehicle and said satellite and other measured kinematic data associated with said satellite, from positioning signals emitted by said satellite and then received by said vehicle;

[0012] - generating an estimated incremental distance between the vehicle and the satellite based on the position data of the vehicle and the satellite contained in the navigation state of the vehicle;

[0013] - calculating an incremental range innovation associated with the satellite using the estimated incremental range and the measured incremental range;

[0014] - performing a test on the incremental range innovation to generate a test result, the test result indicating whether the positioning signal follows a multipath; and

[0015] - If the test result indicates that the positioning signal does not follow a multipath, using the kinematic data measured as observations by the Kalman filter to update the navigation state of the vehicle.

[0016] The method according to the first aspect may further include the following optional features, which may be used alone or in combination where technically feasible.

[0017] Preferably, said kinematic data associated with said satellite is a pseudo-range between said vehicle and said satellite.

[0018] Preferably, the Kalman filter does not use the measured incremental distance to update the navigation state of the vehicle.

[0019] Preferably, the method further comprises the following steps:

[0020] - calculating second information associated with said satellite, said second information being related to said measured kinematic data; and

[0021] - performing a test on the second innovation associated with the satellite to produce a second test result, the second test result being indicative of a likelihood level of the measured kinematic data, the second test result further determining a condition for using the measured kinematic data by the Kalman filter to update the navigation state of the vehicle.

[0022] Preferably, said testing performed on said second information associated with said satellite, or even said calculation of said second information, is only carried out if said testing performed on said incremental range information produces said test result indicating that said positioning signal is not following a multipath.

[0023] Preferably, performing a test on the incremental distance innovation comprises the following steps:

[0024] - calculating a term as a function of the difference between the incremental range innovation associated with the satellite and the covariance of the incremental range innovation associated with the satellite, and

[0025] - A comparison is made between the calculated term and a predefined threshold value, the test result being dependent on the comparison.

[0026] Preferably, said generating said estimated incremental distance comprises:

[0027] - calculating the distance between the vehicle and the satellite at time t from the position data of the vehicle and the satellite contained in the navigation state of the vehicle associated with the time t;

[0028] - The Kalman filter performs inverse prediction based on the navigation state of the vehicle at the time t to obtain the navigation state of the vehicle with respect to time The associated navigation state;

[0029] - According to the time contained in the carrier The position data of the vehicle and the satellite in the associated navigation state are calculated at time The distance between the vehicle and the satellite; and

[0030] - Calculate the distance between the vehicle and the satellite at the time t and the distance between the vehicle and the satellite at the time The difference between the distances between the vehicle and the satellite.

[0031] Preferably, the Kalman filter implements tight inertial / satellite coupling.

[0032] According to a second aspect, a computer program product is also proposed, comprising program code instructions for performing the steps of the method according to the first aspect when the program is executed by a computer.

[0033] According to a third aspect, a navigation system is also proposed, comprising:

[0034] - a processing unit configured to execute the program according to the second aspect; and

[0035] - a receiver configured to receive the positioning signal. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Other features, objects and advantages of the present invention will be presented through the following description. The following description is entirely illustrative and non-limiting and should be read in conjunction with the accompanying drawings. In the accompanying drawings:

[0037] Figure 1 A navigation system according to an embodiment is schematically illustrated.

[0038] Figure 2 is a block diagram describing the general working principle of the Kalman filter.

[0039] Figure 3 is a flow chart of steps of a navigation method according to one embodiment.

[0040] In all the drawings, similar elements have the same reference numerals. DETAILED DESCRIPTION

[0041] Navigation system

[0042] Reference Figure 1 , a navigation system 1 for a mobile vehicle in space includes a receiver 2 and a processing unit 4.

[0043] The vehicle may be of any type: it may be a land vehicle, a watercraft or an aircraft.

[0044] The receiver 2 is suitable for receiving and processing positioning signals previously transmitted by satellites 3 organized in a constellation positioning system (GPS, GALILEO, etc.). The receiver 2 is known per se.

[0045] The receiver 2 generally comprises at least one antenna for receiving signals and a chain for processing these signals, the chain operating in three phases: an acquisition phase, a code error tracking phase (DLL) and a phase error tracking phase (PLL). In particular, the processing chain comprises a loop filter and uses a replica signal related to the received signal.

[0046] The processing unit 4 is configured to implement a vehicle navigation method using a Kalman filter.

[0047] For this purpose, the processing unit 4 comprises at least one processor configured to execute a program comprising code instructions to implement the navigation method.

[0048] The navigation system further comprises an inertial measurement unit 6 configured to provide inertial data from the vehicle to the processing unit 4. The inertial measurement unit 6 is conventional: it comprises a gyroscope and accelerometers (typically three of each type).

[0049] Discussion of the form of the received signal and the concepts of delta range and pseudo-distance

[0050] The signals received by the receiver 2 are C / A signals for civil use on the frequency band L1 = 1575.42 MHz. Each signal is encoded by a PRN code and comprises information samples. More specifically, the characteristics of these different components of the signal are as follows:

[0051] The PRN code is defined by a code length of 1023 bits (chips), a frequency of 1.023 Mcps, and each bit is modulated by a rectangular waveform. Therefore, the duration of the PRN code is 1 ms. This time is expressed as .

[0052] The information bits are periodic and modulated by a rectangular waveform.

[0053] The signal transmitted by the satellite is in the following form:

[0054]

[0055] in:

[0056] . A represents the amplitude of the signal

[0057] . d(t) represents the information carried by the signal

[0058] .c(t) represents the PRN code

[0059] . Represents the frequency of band L1, the frequency of the carrier

[0060] The corresponding signal received by receiver 2 is as follows:

[0061]

[0062] in:

[0063] . represents the wave propagation delay,

[0064] . Represents the phase of a signal which depends on the Doppler shift and wave propagation delay.

[0065] In order to calculate the distance between the receiving antenna and the satellite 3, the delay must be estimated. This delay estimation is usually done on the PRN code. However, this delay characterizes the path of the wave, but is also affected by several errors, such as ionospheric errors, tropospheric errors, multipath, measurement errors, clock drift and even thermal noise. Therefore, the distance resulting from calculating these delays is the true distance plus several errors, and is therefore called "pseudorange".

[0066] The pseudorange may be determined using the PRN component of the signal or using the phase of the signal. In the present disclosure:

[0067] . Code pseudo distance is the following measurement:

[0068]

[0069] . Phase pseudorange is the measurement of:

[0070]

[0071] in:

[0072] . Represents the actual distance

[0073] . Represents the speed of light

[0074] . represents the drift of the clock of receiver 2

[0075] . Represents the drift of the satellite clock

[0076] . Represents ionospheric delay

[0077] . Represents the tropospheric delay

[0078] . and Represents phase error and code error of multipath

[0079] . and Represents code deviation (bias) and phase deviation

[0080] . Representative noise

[0081] . represents the ambiguity associated with the phase measurement ( is the wavelength of the signal).

[0082] The receiver 2 is therefore configured to generate, based on the signals received by the receiver 2 from the satellite 3 , a pseudorange of one of the two types mentioned above, typically a code pseudorange, between the vehicle and the satellite 3 .

[0083] Furthermore, in a manner known per se, the "delta distance" between vehicle and satellite is a datum closely related to the Doppler frequency of the positioning signal in question received by receiver 2 and emitted from satellite 3. In practice, the delta distance is comparable to the phase pseudo distance.

[0084] In fact, the following formula is obtained:

[0085]

[0086]

[0087]

[0088] In the above formula, is the Doppler frequency of the signal received by the receiver 2, ID represents the known concept of “integrated Doppler”, and is the observation period of the incremental distance. In addition, is the start time of phase tracking.

[0089] There is also a relationship between the incremental distance and the distance between the vehicle in question and the satellite 3, because:

[0090]

[0091] in, is the distance between satellite 3 and the antenna at time t, is the wavelength of the signal received by receiver 2.

[0092] General Principles of Kalman Filter

[0093] refer to Figure 2 , Kalman filtering is a method for recursively estimating data contained in a state vector and evolving over time (this state vector is also more simply called "state"). These data are dynamic data (position, velocity, acceleration, etc.). The method works by successive iterations, each of which takes as input the state vector produced by the previous iteration.

[0094] Each iteration consists of two basic steps: a prediction step (also called a propagation step) and an update step (also called a rescaling step in some applications).

[0095] The prediction step transforms the state vector X(k / k) into a priori state vector X(k+1 / k) by using a transition model that models the motion of the vehicle between time k and time k+1.

[0096] Also associated with the state vector X(k / k) is the covariance matrix P(k / k). The covariance matrix can be viewed as a representation of the uncertainty of the estimate made up of the state vector X(k / k). The prediction step also transforms the covariance matrix P(k / k) into a covariance matrix P(k+1 / k) based on a transformation model.

[0097] The update step transforms the prior state vector X(k+1 / k) into the posterior state vector X(k+1 / k+1) based on external measurements called "observations" and by combining these observations with an observation model that models the noise to which these observations are subject. In the update step, the covariance matrix P(k+1 / k) is transformed into the covariance matrix P(k+1 / k). The observations provide additional information that improves the estimate formed by the prior data X(k+1 / k) and P(k+1 / k). This is how the posterior state X(k+1 / k+1) can also be more trustworthy. This can be visualized by the "reduction of P" between the prior and the posterior state.

[0098] The temporal behavior of the actual state vector and its observations is modeled by the following linear formula:

[0099]

[0100] in:

[0101] .X k represents the actual state vector for which an estimate is sought

[0102] . Y k Represents the measurement vector

[0103] . F k Represents the state transition matrix

[0104] . U k Represents the control vector

[0105] . G k Represents the control matrix

[0106] .H k Represents the observation matrix

[0107] .W k represents the state noise (covariance Q k = Cov(W k )

[0108] . V k represents the measurement noise vector (covariance R k =Cov(V k )

[0109] Therefore, in this case, the prior and posterior states described above are obtained due to the following relationship:

[0110] predict:

[0111]

[0112]

[0113] Kalman gain:

[0114]

[0115] renew:

[0116]

[0117]

[0118] in: represents the estimated state vector.

[0119] Many times, the system cannot be modeled with a linear formula. In this case, the traditional Kalman filter cannot be applied. Then, the Extended Kalman Filter (Extended Kalman Filter or EKF) is used. In this case, the Extended Kalman Filter method proposes to return the system to the traditional Kalman filter case by linearizing and discretizing the formula near the estimated solution.

[0120] Configuration of Kalman filter used in navigation system

[0121] The navigation state estimated by the Kalman filter used by system 1 is at time (Navigation filter processing time) calculated vector, which includes the following data:

[0122] . Navigation status, such as:

[0123] - Represents at time the estimated position of the vehicle (more specifically, the antenna of receiver 2);

[0124] - Represents at time estimated speed of the vehicle;

[0125] - Represents at time estimated height of the vehicle;

[0126] Optionally, states describing the accuracy and behavior of the reference navigation device, e.g. for an inertial reference:

[0127] - Accelerometer bias

[0128] - Gyro drift

[0129] . Observe the status, for example:

[0130] - Represents at time The estimated clock bias of

[0131] - Represents the channel deviations associated with different satellites.

[0132] In addition, the Kalman filter of system 1 uses at least two different data of each satellite in the constellation positioning system as the Observations of the "delta distance" between the vehicle and Satellite 3, and other kinematic data associated with Satellite 3. The dates of these observations are often different from The observation date and time are expressed as . It is the time when the radio navigation data is received.

[0133] In the following, an embodiment is described in more detail in which this other kinematic data is the pseudorange between the vehicle in question and the satellite 3 .

[0134] The data processed as observations by a conventional Kalman filter are used to update the a priori state vector, that is, to generate a posterior state vector taking into account these observations.

[0135] Unconventionally, the pseudo-range between the vehicle and the satellite, which has been measured by the receiver 2, is not always used by the Kalman filter of the system 1 to update the a priori state vector. It can be seen that the use of such an update depends on the results of an innovation test involving an incremental distance between the vehicle and the same satellite, which incremental distance is also measured by the receiver 2.

[0136] Another (optional) feature of the Kalman filter used by system 1 is that the incremental range between the vehicle and a given satellite measured by receiver 2 is not used to implement such an update. The incremental range then plays only a role in determining the conditions for using the pseudorange in the update.

[0137] The Kalman filter is further configured to achieve a tight satellite / inertial coupling. In other words, the Kalman filter uses not only the data sent from the receiver 2, in particular the pseudoranges, but also the inertial data provided by the inertial measurement unit 6. These inertial data mainly make it possible to maintain the navigation to be readjusted by the observations.

[0138] Navigation method using Kalman filter

[0139] refer to Figure 3 , the method for implementing the system includes the following steps.

[0140] It is assumed that during the implementation of the prediction step by the Kalman filter discussed above, a time-dependent The associated prior state vector.

[0141] Processing unit 4 according to the time The contents of the associated prior state vector, generated at time The estimated incremental distance between the vehicle and satellite 3 is calculated (step 102). Without loss of generality, it is assumed below that the navigation filter is synchronized with the radio navigation data. Therefore, the navigation date and time Considered to be equal to the observation data reception time .

[0142] Considering the satellite's signal transmission time The estimated position of , the estimated incremental distance is calculated as follows:

[0143]

[0144] Here, Δt is the duration between two Kalman cycles (two readjustments).

[0145] It should be noted here that some terms are involved in the calculation of the estimated incremental distance and the time spent The associated past data is relevant.

[0146] To obtain these past data, we will The associated state vector is used as the object of inverse prediction by the Kalman filter, thereby obtaining the past at time The state vector of .

[0147] Using the Kalman filter formula, the prediction formula is written as:

[0148]

[0149] matrix is called the transition matrix. Therefore, in order to go from state Switch to status , by multiplying the inverse transformation matrix To perform inverse prediction:

[0150]

[0151] The processing unit 4 also obtains the incremental distance measured between the vehicle and the satellite 3 at time t (step 104). The measured incremental distance is provided by the receiver 2, which determines the measurement value according to a known method based on the positioning signal in question received by the receiver 2 and sent from the satellite. In particular, the receiver 2 determines the measured incremental distance based on the phase of the received signal.

[0152] During step 104, the processing unit 4 also obtains the The pseudoranges measured between the vehicle and the satellite 3 are provided by the receiver 2, which determines the measurements according to known methods based on the positioning signals received by the receiver 2 and emitted from the satellite in question.

[0153] At this stage, the prior state vector has not been updated by the Kalman filter, and for a given satellite and time Two observations have been obtained: the incremental range between the vehicle and satellite 3, and the pseudorange between the vehicle and satellite 3.

[0154] Before performing the update, the following steps are performed by the processing unit 4 .

[0155] The components of the observation matrix H are determined by the processing unit 4 by calculating the Jacobian matrix of:

[0156]

[0157] Then, the processing unit 4 calculates the incremental range information associated with the satellite 3 (step 106). Using the prior state vector dX, the observation matrix H and the previous term , the new information is calculated as follows:

[0158]

[0159]

[0160] The processing unit 4 also calculates the covariance of the incremental range innovation associated with the same satellite as the "Inno" data. The covariance of the innovation is calculated as follows:

[0161]

[0162] Among them, H is the observation matrix, P is the state covariance matrix, and R is the observation error covariance.

[0163] The processing unit 4 performs a test on the "Inno" delta-range innovation to find out if the signal received from the associated satellite has experienced multipath (step 108).

[0164] The test 108 for the innovation includes the calculation by the processing unit 4 of a term that is or depends on the associated covariance of the "Inno" incremental distance innovation and the incremental distance innovation. The difference between.

[0165] The obtained terms are compared with a predefined threshold.

[0166] The processing unit 4 generates during a test step 108 a test result indicating whether the received satellite signal from which the measured incremental distance originates has experienced multipath.

[0167] If the term is above a predefined threshold, this means that the innovation is inconsistent with the associated covariance.The test result generated by the processing unit 4 is then positive, in the sense that it indicates that the received signal from which the measured incremental distance originates has experienced multipath.

[0168] In the first case, during the updating step, the pseudoranges measured for the same satellite are not used as observations by the Kalman filter to update the navigation state of the vehicle.

[0169] Conversely, if the term is not greater than a predefined threshold, the innovation is considered consistent with the associated covariance.The test result generated by the processing unit 4 is then negative, in the sense that it indicates that the received signal from which the measured incremental distance originates does not experience multipath.

[0170] In the second case, the processing unit calculates second information associated with the satellite, the second information being related to the measured pseudorange (step 110 ).

[0171] The processing unit then performs a test on the second innovation (step 112) which (except for the work done on the pseudorange) follows the same logic as described for the test on the incremental range innovation. This test on the second innovation produces a second test result which indicates the likelihood level of the measured pseudorange. This test forms part of the known operation of the Kalman filter technique.

[0172] If the second result indicates that pseudoranges are possible, then during an update step, the measured pseudoranges are used as observations by the Kalman filter to update the navigation state of the vehicle (step 114). Otherwise, the pseudoranges are not used in this way (as is the case if the test result on the incremental range innovation indicates multipath).

[0173] Finally, for the pseudorange measured for satellite 3 to be used as an observation by the Kalman filter to update the a priori state vector, two conditions must be met: the first condition comes from the test 108 performed on the incremental range information obtained for satellite 3, and the second condition comes from the test 112 performed on the information related to the pseudorange itself associated with the same satellite 3.

[0174] Furthermore, in order to save computational effort, it is advantageous to perform the test 112 on the innovation related to the pseudorange only if the result of the test on the incremental range innovation proves to be negative.

[0175] It should be noted that steps 100 to 114 are repeated for signals from different satellites. For example, let us consider the case of tracking N satellites. N pseudoranges and N incremental ranges are measured, N incremental range innovations are calculated, and each innovation is tested.

[0176] Other variants of the above method are possible. The pseudoranges between the vehicle and the satellite (the use of which during the updating step is conditional) may be replaced by other kinematic data associated with the satellite.

Claims

1. A method for navigating a vehicle by estimating the navigation state of the vehicle using a Kalman filter, the method comprising the steps of: obtaining (102) incremental distances measured between the vehicle and the satellite and other measured kinematic data associated with the satellite based on positioning signals transmitted by the satellite and then received by the vehicle; generating (104) an estimated incremental distance between the vehicle and the satellite based on position data of the vehicle and the satellite contained in the navigation state of the vehicle; calculating (106) incremental range innovation associated with the satellite using the estimated incremental range and the measured incremental range; Performing a test (108) on the incremental range innovation to generate a test result, the test result indicating whether the positioning signal follows a multipath; as well as If the test result indicates that the positioning signal does not follow a multipath, the kinematic data measured as observations are used (114) by the Kalman filter to update the navigation state of the vehicle.

2. The method according to claim 1, wherein: The kinematic data associated with the satellite are pseudoranges between the vehicle and the satellite.

3. The method according to claim 2, wherein: The Kalman filter does not use the measured incremental distance to update the navigation state of the vehicle.

4. The method according to claim 1, wherein: The method further comprises the following steps: calculating (110) second information associated with the satellite, the second information being related to the measured kinematic data; and A test (112) is performed on the second innovation associated with the satellite to produce a second test result, the second test result indicating a likelihood level of the measured kinematic data, the second test result also determining a condition for using (114) the measured kinematic data by the Kalman filter to update the navigation state of the vehicle.

5. The method according to claim 4, wherein: The test (112) performed on the second information associated with the satellite, or even the calculation (110) of the second information, is only performed if the test performed on the incremental range information produces a test result indicating that the positioning signal does not follow a multipath.

6. The method according to any one of claims 1 to 5, wherein: The performing of the test on the incremental distance innovation (108) includes: calculating a term based on the difference between the incremental range innovation associated with the satellite and the covariance of the incremental range innovation associated with the satellite, and A comparison is made between the calculated term and a predefined threshold value, the test result being dependent on the comparison.

7. The method according to any one of claims 1 to 5, wherein: The generating the estimated incremental distance (104) comprises: calculating the distance between the vehicle and the satellite at time t based on the position data of the vehicle and the satellite contained in the navigation state of the vehicle associated with time t; The Kalman filter performs inverse prediction based on the navigation state of the vehicle at the time t to obtain the navigation state of the vehicle with respect to time t. The associated navigation state; According to the time The position data of the vehicle and the satellite in the associated navigation state are calculated at the time The distance between the vehicle and the satellite; and Calculate the distance between the carrier and the satellite at the time t and the distance between the carrier and the satellite at the time The difference between the distances between the vehicle and the satellite.

8. The method according to any one of claims 1 to 5, wherein: The Kalman filter achieves tight inertial / satellite coupling.

9. A computer program product comprising program code instructions for executing the steps of the method according to any one of claims 1 to 8 when the program is executed by a computer.

10. A navigation system (1), comprising: A processing unit (4) configured to execute the method according to any one of claims 1 to 8; as well as A receiver (2) is configured to receive the positioning signal.

Images