NAVIGATION SYSTEM AND METHOD FOR ERROR CORRECTION

Abstract

A system corrects errors of a measurement system. The system comprises base measurement system BS and a correction system. BS sensor data is acquired and provided to an algorithm. The data is stored associated with a time stamp indicating time of sensing. Output values are calculated and stored with time stamps indicating time of the sensor data upon which the output values are calculated. Data having corresponding time stamps are supplied to a filter where correction values and correction increments are calculated. The correction increments reflect the change of error in a base system output value over time due to integration or summing up BS sensor data errors in the processing algorithm. The correction values are applied to the BS data and corrected base navigation output values are calculated. These output values are corrected by the correction increments. Output values of the processing algorithm are further processed in succeeding applications.

Claims

1. A method for correcting errors of measurement system output, the system comprising a base measurement system BS and a correction system, the method comprising the steps of: acquiring BS sensor data and providing the BS data to a processing algorithm, storing BS sensor data associated with a time stamp indicating the time of sensing, calculating BS output values by processing the BS sensor data in the processing algorithm, storing the calculated BS output values with associated time stamp, acquiring correction system sensor data associated with a time stamp are indicative of the time of sensing, supplying BS sensor data, BS output values and correction system sensor data with corresponding time stamps to a fusion filter, calculating correction values and correction increments, wherein the correction increments reflect the change of an error in an BS output value over time due to integration and/or summing up BS sensor data in the processing algorithm, applying respective correction values to BS sensor data and processing these corrected BS sensor data by the processing algorithm, applying the correction increments to the respective BS output values and storing these recalculated BS output values, and outputting the most recent output values of the processing algorithm for further data processing.

2. The method according to claim 1, wherein a plurality of correction system sensor data is collected by storing the correction system sensor data in storage associated with their respective time stamps.

3. The method according to claim 1, wherein correction system sensor data having a lag time exceeding a predetermined threshold are discarded.

4. The method according to claim 1, wherein the latest calculated correction values are stored.

5. The method according to claim 4, wherein in case that no correction system sensor data with a lag time shorter than the predetermined threshold is available a correction is performed only on the basis of the stored correction values.

6. The method according to claim 1, wherein the processing algorithm is a strap-down algorithm in a base navigation system.

7. The method according to claim 1, wherein the fusion filter is an Extended Error Space Kalman-Filter.

8. An integrated navigation system, comprising: a base navigation system BS, and a correction system, wherein the base navigation system and the correction system are configured to perform the steps of acquiring BS sensor data and providing the BS data to a processing algorithm, storing BS sensor data associated with a time stamp indicating the time of sensing, calculating BS output values by processing the BS sensor data in the processing algorithm, storing the calculated BS output values with associated time stamp, acquiring correction system sensor data associated with a time stamp are indicative of the time of sensing, supplying BS sensor data, BS output values and correction system sensor data with corresponding time stamps to a fusion filter, calculating correction values and correction increments, wherein the correction increments reflect the change of an error in an BS output value over time due to integration and/or summing up BS sensor data in the processing algorithm, applying respective correction values to BS sensor data and processing these corrected BS sensor data by the processing algorithm, applying the correction increments to the respective BS output values and storing these recalculated BS output values, and outputting the most recent output values of the processing algorithm for further data processing.

9. The integrated navigation system according to claim 8, wherein the base navigation system comprises a inertial measurement unit, a processing unit configured to carry out a strap-down algorithm, and a fusion filter.

10. The integrated navigation system according to claim 6, wherein the correction system comprises at least one of GNSS, odometry, barometer, magnetometer, radars, stereo vision, laser scanners, ultrasonic rangefinders, landmarks, radio location.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0023] FIG. 1 shows a block diagram to illustrate the components of the integrated navigation system as one embodiment of the invention; and

[0024] FIG. 2 a diagram to illustrate the method steps of the invention and in particular their time relation.

DETAILED DESCRIPTION

[0025] In FIG. 1 a block diagram is shown which illustrates the main functional blocks of a navigation system 1 which is exemplary for an embodiment of the present invention. The entire navigation system 1 consists of a base system 2 and a correction system 3. The base system 2 comprises base sensors 4 such as three dimensional accelerometers and gyroscopes that measure three dimensional accelerations and angular rates. Base system sensor data include naturally an error associated with each of the base sensors 4, for example a sensor offset.

[0026] The sensor data that is output by the base sensors 4 is supplied to a processing algorithm unit 6, for example a strap-down algorithm. By the strap-down algorithm output values are calculated from the base sensor data. These output values comprise for example values for velocity, position and pose. By processing the base system sensor data for at least some of the BNS output values sensor data is integrated. This of course results in an error of the respective output value that changes over time and results in increasing inaccuracy of the output data with time.

[0027] The correction system 3 comprises correction system sensors 8. These correction system sensors 8 in case of a GPS measurement for correcting the output data deliver GPS raw data measurements that contain the measured distance between satellites and the receiver antenna. Other sources for obtaining correction system sensor data are also possible, of course.

[0028] The base system sensor data which is output by the base system sensors 4 includes a time stamp indicating the time of sensing. The base system sensor data is stored in a first memory 5. Advantageously, only a certain amount of base system sensor data is stored in the first memory 5. The time interval for which the data is stored in the first memory 5 depends on a maximum lag time for example, which will be explained afterwards in more detail. Also the base navigation system output values that are produced by processing the base navigation sensor data by this strap-down algorithm in the processing unit 6 are stored in a second memory 7. Again, the output values are associated with a time stamp. This time stamp in that case does not indicate the time of calculating the output values but the time corresponding to the sensing that is the basis for the calculation for the output values. It is to be noted that due to the low computational cost that the processing of the input base navigation sensor data requires, the time lag between inputting the data into the processing algorithm and outputting the output values can be neglected.

[0029] Furthermore, time stamps are also associated with the correction system sensor data, again indicating the point in time when the respective sensing took place. The correction system sensor data is stored in a third memory 9 associated with its corresponding time stamp.

[0030] The correction algorithm unit 10 is established by a fusion filter such as an extended error state space Kalman-filter. Such extended error state space Kalman-filter per se is already known in the art and is suitable of calculating correction values from input data. In the present case the correction algorithm unit 10 is supplied with data stored in the first memory 5, the second memory 7 and the third memory 9. It is to be noted that distinguishing between the first, the second and the third memory 5, 7, 9 is only made for illustrative reasons. Of course, only a single memory may be used to store all the different values.

[0031] The selection of the data and values that are supplied to the correction algorithm unit 10 is controlled by the selection and control unit 11 that selects base navigation system data from the first memory 5, base navigation output values from the second memory 7 and correction sensor data from the third memory 9 having corresponding time stamps. Thus, the error correction calculation performed by the Kalman filter is performed on the basis of timely corresponding data only which results in time consistency of the output values.

[0032] It might occur that no fully corresponding set of data can be achieved. In that case, an interpolation is performed in interpolation unit 12. The interpolation unit 12 therefore receives from the first memory 5 and the second memory 7 values that allow interpolating values that then match the time stamp of the correction sensor data that is supplied to the correction algorithm unit 10.

[0033] On the basis of the thus corresponding base navigation sensor data, base navigation output values and correction system sensor data then correction values and correction increments are calculated. The correction increments are used to correct a change of the error over time whereas the correction values are applied as a constant value. Thus, the correction value corrects errors that have low dynamic. Such errors are for example sensor offset or scale factor errors. Contrary, the correction increments correct errors that are mainly caused by processing data containing yet uncorrected constant errors. One example is the correction of the calculated velocity that requires the integration of accelerometer data. The calculation of such correction increments can be easily done since the change of such errors is systematically known from the processing algorithm of the strap-down algorithm.

[0034] It is furthermore to be noted that because of such integration or summing up of residual errors on the input side of the strap-down algorithm, also random errors like for example noise may contribute to an overall error.

[0035] In FIG. 2 there is illustrated the method according to the invention as it is implemented in an integrated navigation system. In order to indicate that the correction algorithm is running “in the past” whereas the base navigation system and thus the strapdown algorithm runs “in the present” a time line 15 is shown. The terms “in the past” and “in the present” indicate the time of the data that are used for the method steps of the correction system and the base navigation system as it is explained in the following sections. Thus, it refers to the time stamps of the base navigation system sensor data, the base navigation system output values and the correction system sensor data. As already explained at first sensor data from the base navigation system sensors 4 are generated and provided with time stamps. These sensor data are stored for further use in the correction system 3 as relate to by the thick arrow 16. But the base navigation system sensor data is not only stored but also immediately processed by the processing algorithm unit 6 in a strap-down algorithm there. The output values of the strap-down algorithm are output for further use in external applications or the like, but are also stored as indicated by black arrow 25 for later use in the correction system 3. It is to be noted that any time processing of data is mentioned it is evident that such processing is performed in a processing unit that can comprise either a plurality of CPU's or a single one.

[0036] Furthermore, a maximum lag time as indicated by interval 17 is defined. Any new data that is generated by the base navigation sensors 4 are added to the first memory 5 so that for anytime of the interval with the maximum lag time any sensor data of the base navigation system sensors 4 is available or can be interpolated. During the maximum lag time interval 17 also correction system sensor data from the respective sensors 8 are available. The correction system sensors also provide the data with time stamps that correspond to the time of sensing. The correction system sensor data is also stored as soon as it is available to the correction system 3. With arrows 18, 19 it is indicated that these data may be available for further processing only at a later point in time. It can be seen that the original sensing of the correction sensor data by the correction system sensor 8 and the base navigation system sensors 4 occurs at the same time, to be more precise at the current point in time according to the time line 15. Nevertheless the time lag being the delay in availability of the different data for further processing is the reason why the correction algorithm has to await until the correction system sensor data may be processed. Thus, in addition to the base navigation system sensor data also the correction sensor system data that are available within the maximum lag time are stored in the third memory 9 as indicated in FIG. 1.

[0037] After the maximum lag time elapsed the data corresponding with respect to the time stamps is supplied to the fusion filter. It is to be noted that for illustrating that the correction sensor data may be available only at different points in time the arrows 18, 19 are used. The storing itself may be performed within the correction system 3.

[0038] After the maximum lag time has expired, base navigation system data and correction system data are supplied to the processing unit executing the correction algorithm by means of a fusion filter. As indicated by reference numeral 20 in the algorithm there is calculated an error between the output values resulting from the base navigation sensor data and the correction system sensor data. Furthermore an interpolation unit 21 interpolates from the base navigation sensor data supplied to the fusion filter that exactly corresponds with respect to time to the time stamps of the correction sensor system data, in case that the time stamps of the base navigation sensor data and the time stamps of the correction system sensor data may not coincide precisely. For the correction algorithm then the interpolated data is used.

[0039] Using data in the correction algorithm that have corresponding sensing times ensures that all the measurements can be processed in the order of measurement instead of the order of their arrival. This is achieved by storing the respective sensor data until they are processed and results in time consistency.

[0040] The data are fed to a model 22 in which the distinction between the constant errors and the errors changing over time is performed. Examples for such constant or at least low dynamic errors may be sensor offsets or scale factor errors. Apart from that and as mentioned earlier there are also errors that are caused by processing data in the strap-down algorithms. Thus, if data still containing uncorrected errors are supplied to the strap-down algorithm such errors will add up or be integrated depending on the processing algorithms. But as the algorithms used in the strap-down algorithms are known also such errors are systematically known.

[0041] This allows calculating not only correction values for the constant errors but also correction increments describing the change of an error for a unit in time and with respect to a particular BNS output such as velocity. After the correction values and the correction increments have been computed they are applied. Firstly, the constant values are applied to the base navigation sensor data that is stored and that was collected in the past. Using corrected base navigation sensor data then the processing algorithm in unit 6 is run again. The base navigation output values are then corrected using the correction increments, thus taking care of the changing error over time. This means that an output value being calculated from earlier base navigation sensor data and output values calculated from later base navigation sensor data are corrected using the correction increments. This reflects the integration of an uncorrected error due to processing. The model that is executed in 22 by the fusion filter reflects for each of the different outputs of the strap-down algorithm in which way errors of input data of the strapped-down algorithm integrate or sums up.

[0042] After the corrected base navigation sensor data has been recalculated by the strap-down algorithm it is again stored, still with the respective time stamps, and is used for future correction cycles. This is integrated by the re-computation loop illustrated by the dashed error 23.

[0043] The constant values that are output by model 22 are also stored in a memory and thus are available if no current correction measurement can be used for calculating the correction increments. In that case, the base navigation system 2 runs in a stand-alone mode.

[0044] Anytime new correction values and new correction increments are available, the already stored base navigation system sensor data will be used to re-compute the base navigation output values. The corrected output values then overwrite the stored output values with matching time stamps. Thus, all the data that is available up to the most recent, present measurement are corrected on the latest results.

[0045] The algorithm is repeated for every base system measurement step. The recomputation is done if correction data was available.

[0046] It is to be noted that although the examples given are all directed to a navigation system, this is no limitation for application of the inventive method. The method is suitable for all multi-sensor fusion systems where the estimated properties are expected to have neglectable dynamics within the time lag of the slowest sensor defining the maximum lag time, and errors of such property may accumulate in another one. These assumptions are explicitly valid for systems which deal with error estimation such as error-state-filters.

[0047] Typically, for use in an integrated navigation system as described above, the base navigation system uses sensors like an IMU (Inertial Measurement Unit) as a base system and correction measurements performed by (differential) GNSS (Global Navigation Satellite System), odometry, barometer, magnetometer, radars, (stereo)vision, laser scanners, rangefinders, landmarks, radio location, etc, which are not only used as correction sensors, but also become mutually corrected. Such mutual correction is typically conducted in so-called tightly coupling architectures which do not only correct the base navigation system, but also its correction measurement sensors. The sensors as described above, typically are not permanently available and have different, asynchronous and possibly non-constant sampling rates and particularly a different and varying lag time. Since the system has a defined maximum lag time and takes account of the measurements in the correct time order, the system adapts itself to such varying lag times.

[0048] The system can also be used with service robots, for example autonomous lawn mowers, cleaning applications and the like. But also applications in error space and seafare are feasible as well.