Method for checking a correction of a system model in a kalman filter
Patent Information
- Authority / Receiving Office
- EP · EP
- Patent Type
- Applications
- Current Assignee / Owner
- ROBERT BOSCH GMBH
- Filing Date
- 2024-06-27
- Publication Date
- 2026-05-13
AI Technical Summary
Current Kalman filter systems lack a method for real-time validation of their system models and parameters, which is crucial for maintaining high precision and confidence in localization data, especially in highly automated and autonomous driving applications, where age-related changes and sensor errors can significantly impact accuracy.
A method that utilizes standstill phases to validate the self-correction capabilities of the Kalman filter by comparing corrected parameters with comparison parameters from a secondary data source, involving a calibration function and error detection mechanism, allowing for continuous self-calibration and error compensation within the filter network.
This approach enables real-time validation of the Kalman filter's system model, detecting age-related changes and sensor errors, thereby improving the precision and reliability of localization data used in navigation systems, even in the field, and ensuring high-quality data for automated driving functions.
Smart Images
Figure EP2024068093_16012025_PF_FP_ABST
Abstract
Description
[0001] Description
[0002] title
[0003] Method for testing a correction of a system model in a Kalman filter
[0004] State of the art
[0005] The invention relates to a novel method for testing a parameterization of a system model in a Kalman filter.
[0006] The Kalman filter (also Kalman-Bucy filter, Stratonovich-Kalman-Bucy filter or Kalman-Bucy-Stratonovich filter) is a mathematical filter model for the iterative estimation of system states based on faulty input data, which is in particular sensor data.
[0007] The Kalman filter is used to estimate non-directly measurable system variables while optimally reducing observation errors. Based on the input data, the Kalman filter typically maintains an internal mathematical model as a constraint. This model is incorporated into the estimation of parameters and takes dynamic relationships between the system variables into account. For example, the mathematical model contains equations of motion that relate the changing positions and velocities that flow into the Kalman filter as input data. Thus, precise estimates can be created jointly based on (incorrect) input data for positions and velocities.
[0008] Kalman filters are particularly useful for the iterative estimation of system states based on observations that are usually subject to error. Kalman filters have proven particularly advantageous in this context, especially for applications where sensor information from different sensors must be combined (or fused) with model information. Furthermore, Kalman filters are frequently used in embedded systems because their calculations are advantageous, accurate, and robust. Furthermore, microcontrollers can execute the calculations of a Kalman filter with great efficiency.
[0009] Kalman filters are used in particular to fuse data from various sensors used to determine vehicle positions in order to obtain highly precise localization data. Localization data here refers in particular to position data, as well as data relating to speed and acceleration. Sensors whose data can be processed or fused using Kalman filters include, for example, GNSS sensors for determining localization using GNSS satellites, inertial sensors, and, for example, wheel sensors and steering angle sensors used in motor vehicles to monitor the movement of a vehicle via the vehicle's chassis. Such sensors thus determine a type of precursor data for localization data, which is used as input data in a Kalman filter to determine highly precise localization data.By jointly considering these described data in a Kalman filter using the system models, parameters and / or system states stored in the Kalman filter, the generation of high-precision localization data from the described input data becomes possible.
[0010] Kalman filters are used in particular to obtain high-precision localization data for highly automated driving functions of motor vehicles and in particular for autonomous driving functions.
[0011] In a Kalman filter, the internal parameters and system states are preferably continuously corrected. The term "corrected parameters" is used here generally for the corrections that take place in the Kalman filter. The quality of the internal parameters and / or system states in the Kalman filter are crucial for improving the localization data through the use of the Kalman filter.
[0012] However, there is a fundamental need for improvement regarding the precision and confidence of the localization data determined using Kalman filters, because very high precision and high confidence of the localization data used is desirable for applications of highly automated and autonomous driving.
[0013] Disclosure of the invention
[0014] Based on this, a particularly advantageous method for testing the design of the system model and the parameters of the system model in a Kalman filter will be described.
[0015] The invention relates to a method for testing a design of a system model in a Kalman filter, which is part of a filter network for determining localization data in a motor vehicle and uses a first set of sensor data to determine localization data, wherein the method comprises at least the following steps: a) detecting a standstill situation of the motor vehicle during operation with a standstill detection, so that a standstill signal is generated; b) performing a calibration function for parameters of the system model during the standstill situation in the Kalman filter, wherein corrected parameters are determined;c) performing a test of the corrected parameters by comparing the corrected parameters with comparison parameters when a standstill signal is present, wherein the comparison parameters are determined using another data source, wherein the other data source creates comparison parameters using a second set of sensor data, wherein the second set of sensor data is reduced compared to the first set of sensor data, d) performing an error function if a test performed in step c) is negative;
[0016] The basic approach of the method described here is to use vehicle standstill phases to test or validate the self-correcting capabilities of the Kalman filter. The self-correcting capabilities of the Kalman filter specifically relate to the ability of the Kalman filter to self-correct its internal parameters.
[0017] Standstill phases are typically used by navigation systems for the isolated calibration of individual sensors (e.g., for calibrating an inertial sensor). This approach is based on the assumption that, for example, an inertial sensor should not output any acceleration values during a standstill phase. This allows faulty signals from an inertial sensor to be detected during standstill phases. Sensor zero point drifts, for example, can be detected, and offset parameters can be introduced if necessary to correct such zero point drifts.
[0018] The method described here goes one step further. According to step c), the filter's own system models are checked or validated in the Kalman filter as a whole when a standstill phase occurs.
[0019] To date, the system model of a Kalman filter has been validated, particularly during development, using reference systems (carried on board). A review of the variances estimated by the filter (and thus of the system model) is therefore only performed during development and not for navigation filters in the field. The method presented here allows for runtime validation for every navigation device in the field. This allows for an improved detection of both age-related changes in the measuring devices and sensor errors.
[0020] The calibration of a Kalman filter is typically performed continuously due to its properties. The calibration function described in step b) is preferably a (conventional) self-calibration of the Kalman filter, which is performed continuously during operation of the Kalman filter. The calibration according to step b) is a process of the Kalman filter itself, which the Kalman filter performs itself when the system is at a standstill and preferably also during other operating phases (non-standstill phases). Calibration is, in particular, an automatic error compensation that results from the structure of the Kalman filter itself. In other words: errors that occur are implicitly detected, lead to changes in the system model, are stored in the system model, and used in the future to correct sensor signals and generate improved localization data.
[0021] Due to the structure of the Kalman filter, standstill phases play an important role because the relationship between the individual signal sources processed by the Kalman filter is different during these phases than during phases of movement. For example, inertial sensors typically provide no data here, or only data due to sensor inaccuracies. For this reason, system states of the system model in the Kalman filter change, particularly during standstill phases, so that system model parameters are corrected. System model parameters that change due to the internal correction mechanisms in the Kalman filter are referred to here as corrected parameters.
[0022] The parameters of the Kalman filter (and thus also the corrected parameters) are usually the individual values of the following matrices and vectors:
[0023] - a covariance matrix of the Kalman filter;
[0024] - an estimated state vector (x) of the Kalman filter ;
[0025] - parameters and / or a structure of a system model of the Kalman filter; and
[0026] - a noise vector that describes a system noise of the Kalman filter.
[0027] Sets of sensor data within the meaning of the first set and the second set are each data from sensors. Sets of data are preferably further processed using filters, e.g., the Kalman filter. The signals flowing into the first set preferably overlap with the signals flowing into the second set. The second set is particularly preferably a subset of the first set.
[0028] Compared to the calibration described in step b), the test performed in step c) does not interfere with the Kalman filter. The test does not change the internal parameters (system states) of the Kalman filter. Rather, the internal (corrected) parameters of the Kalman filter are compared with other parameters. The test is preferably binary and can produce either a positive or negative result. If necessary, the test can also output more than two (binary) result values.
[0029] Preferably, however, there are negative result values that trigger the error function in step d), as well as positive result values that do not trigger the error function in step d). If the test is positive, the filter network operates as intended. The error function in step d) then does not need to be triggered. If the test is negative, the filter network does not operate as intended. In particular, the self-correction of the Kalman filter, which constantly corrects parameters or outputs corrected parameters, is then impaired.
[0030] The method aims to trigger the error function explained in step d) if, according to step c), it is detected that the Kalman filter is not sufficiently self-correcting. For example, the error function can detect that the localization data determined with the Kalman filter is impaired overall and should be discarded or not considered for highly automated or autonomous driving functions.
[0031] For the comparison according to step c), comparison parameters are preferably determined. This is done using a data source other than the Kalman filter. In the simplest case, comparison parameters can be parameters determined directly from sensors (e.g., inertial sensors). Inertial sensors should normally not register any movements and / or accelerations or velocities of the vehicle during a (reliably detected) standstill phase. For this reason, deviations can be detected (if data from inertial sensors is used directly as comparison parameters). Such deviations can allow conclusions to be drawn about errors in the system model of the Kalman filter.
[0032] Preferably, the other data source is set up in such a way that the comparison parameters available from this data source ensure easy comparability with the corrected parameters.
[0033] The basis of the described procedure is that in step a) a real and reliable standstill detection takes place.
[0034] It has been found that wheel sensor signals alone are usually not sufficient to reliably detect a standstill using the described procedure. According to the described procedure, the corrected Kalman filter parameters are only checked in standstill situations.
[0035] The method is particularly advantageous if the first set of sensor data used by the Kalman filter comprises at least GNSS signals from at least one GNSS sensor and inertial sensor signals from inertial sensors.
[0036] Particularly preferably, the first set of sensor data also includes WSS signals from wheel sensors and, particularly preferably, also data from sensors with which a motor vehicle's steering angle can be monitored. The method is also advantageous if age-related changes in GNSS sensors and / or inertial sensors are detected by checking the corrected parameters in step c).
[0037] In particular, the described method can detect age-related changes in sensors. In particular, it can also detect age-related changes in sensors that were not yet taken into account in the design of the Kalman filter.
[0038] Furthermore, the method is advantageous if data from GNSS sensors are not taken into account for detecting a standstill situation in step a).
[0039] Furthermore, the method is advantageous if sensor data from at least one inertial sensor and / or sensor data from wheel rotation sensors are used to detect a standstill situation in step a).
[0040] To detect a standstill, a plurality of sensor data is preferably fused. For example, the following data can be considered to detect a standstill:
[0041] Camera images, whereby, for example, a completely or partially stationary camera image can be assessed as an indication of a standstill;
[0042] Time-based data, where, for example, certain times can be evaluated as an indication of a standstill;
[0043] Detecting the standstill of a workshop situation when the vehicle is in a workshop; and / or
[0044] Data relating to the condition of the vehicle, such as internal system flags indicating a deactivated ignition.
[0045] In principle, this list is not exhaustive. There are many ways to detect a vehicle's standstill. For the described method, it is important that the various data considered are combined or linked in such a way that a standstill is detected with a high degree of confidence. Furthermore, the method is advantageous if, when checking the corrected parameters tested in step c), at least the following parameters are checked:
[0046] - a covariance matrix of the Kalman filter;
[0047] - an estimated state vector of the Kalman filter;
[0048] - parameters and / or a structure of a system model of the Kalman filter; and
[0049] - a noise vector that describes a system noise of the Kalman filter.
[0050] The method is also advantageous if the other data source is a strapdown filter provided in the filter network in addition to the Kalman filter, wherein the strapdown filter uses data from at least one inertial sensor to generate comparison parameters for testing the corrected parameters in step c).
[0051] The strapdown filter is a filter present in addition to the Kalman filter in the filter network, which preferably performs processing of the second set of sensor data in parallel to the Kalman filter.
[0052] Preferably, the strapdown filter also receives data from the Kalman filter as input data. Particularly preferably, the strapdown filter receives as input data output data from the Kalman filter that was previously determined (at a previous point in time) using the Kalman filter.
[0053] The core of the invention is a validation approach at standstill that uses a parallel strapdown filter to validate filter variances. The advantage arises from the ability to detect and react to sensor errors or deviations from the filter model (e.g., due to sensor aging).
[0054] The strapdown filter preferably only processes data from an inertial sensor system. The strapdown filter preferably has a system model whose structure corresponds to the system model of the Kalman filter, which is regularly adapted to the system model in the Kalman filter and, starting from a previous point in time, is updated or propagated only using the data from the second set (preferably exclusively data from inertial sensors). The term "propagation" here means that the system model is updated only using the data from an inertial sensor system, starting from the previous point in time. The term "propagation" here therefore means in particular that, at standstill, only the inertial sensor data is used to check how the state vector changes when it is updated with the strapdown filter. Changes are likely to occur only within an expected framework.In particular, a synchronous increase in variances and their respective associated parameters is to be expected. In principle, the variances should always encompass the associated parameters. Preferably, the test of the corrected parameters includes a comparison of parameters and their variances. A drift of errors or parameters beyond the variances triggers the error function described in step d). The system models in the strapdown filter and the Kalman filter each model the drift of the error in the absence of new input data. This drift can be measured during a standstill phase because it occurs without additional position changes influencing the parameters.With the described method, according to step c), it is essentially determined whether the actual runaway of the fault at standstill (which inevitably always occurs due to inaccuracies in the inertial sensors and their data processing) fits the system model or is correctly implemented in the system model. This allows it to be determined whether the system model is suitable for correctly accounting for the behavior of the inertial sensors during operation.
[0055] In preferred embodiments of the method, the strapdown filter operates continuously and independently of the standstill situation. However, the test described in step c) is then only performed in the standstill situation, which is preferably detected independently of the strapdown filter.
[0056] Strapdown is a fixed term for an algorithm related to fixed acceleration and angular rate sensors.
[0057] Furthermore, it is advantageous if, in parallel to the propagation of the state vector of the Kalman filter with the strapdown filter, the at least one covariance matrix of the Kalman filter is propagated with the strapdown filter as a propagated covariance matrix.
[0058] The strapdown filter preferentially corrects the inertial sensor signals (if present) with the estimated sensor errors from the state vector x of the Kalman filter. The corrected inertial sensor values are used in the strapdown filter to propagate the initial position, velocity, and attitude over time. In parallel, the covariance matrix is propagated using the system model, the inertial sensor data, and the system noise.
[0059] The propagated covariance matrix is then compared with the propagated states of position, velocity and attitude.
[0060] The initialization errors, as well as the measurement errors (inaccuracies) of the inertial sensors, cause the propagation errors of the states to increase. At standstill, the propagation error can be calculated by assuming constant position and orientation as well as zero velocity, subtracting the initial values from the current propagation state. For an ideally designed Kalman filter, three times the standard deviation (from the filter's covariance matrix) should encompass the estimation error. This is not verifiable in conventional systems without a reference system.
[0061] A comparison of the propagation errors described above and the propagated variance (also described above) enables such a check. If this condition is violated over a longer period of time (tuning factor), it can be assumed that the system model no longer describes the sensors with sufficient accuracy. Depending on the magnitude of the deviation, this can be addressed by adjusting the system model parameters at runtime or, in the case of larger deviations, by issuing an error signal.
[0062] Furthermore, it is advantageous if, for the testing of the corrected parameters according to step c), a comparison of the covariance matrix determined with the Kalman filter and the state vector determined with the Kalman filter is carried out with the covariance matrix propagated with the strapdown filter and the state vector propagated with the strapdown filter.
[0063] The comparison of covariance matrices and state vectors does not have to be complete, meaning that not every element of the matrices and vectors needs to be compared for the method. Individual values can also be compared, for example, only diagonal values of the matrices. The diagonal values of the covariance matrix typically contain variances. By comparing the variances, the test according to step c) can be performed. Also described here is a control unit comprising a processor that is adapted / configured to execute the described method.
[0064] Preferably, the described sensor data sources (GNSS sensors, inertial sensors, wheel sensors, etc.) are connected to the control unit
[0065] Preferably, the control unit is configured to generate high-precision localization data from sensor data and to regularly execute the described method in order to ensure a particularly high quality of the localization data and to execute the error function described in step d) if the quality of the localization data cannot be ensured.
[0066] Also to be described is a computer program product comprising instructions which, when the computer program product is executed by a computer, cause the computer to carry out the described method.
[0067] The computer program product is preferably installed on the described control unit.
[0068] A computer-readable storage medium is also to be described, comprising instructions which, when executed by a computer, cause the computer to carry out the described method or the steps of the described method.
[0069] The invention and the technical context of the invention are explained in more detail below with reference to the figures. The figures show preferred embodiments to which the invention is not limited. It should be noted in particular that the figures, and in particular the proportions depicted in the figures, are only schematic. They show:
[0070] Fig. 1 : a filter network for carrying out the described method;
[0071] Fig. 2: a flow diagram of the described method; Fig. 3: an example of a deviation occurring between a state vector of a Kalman filter and a state vector propagated with a strapdown filter; and
[0072] Fig. 4: another example of a deviation occurring between a state vector of a Kalman filter and a state vector propagated with a strapdown filter.
[0073] Fig. 1 shows a filter network 2 with a Kalman filter 1 that processes a first set 3 of input data to determine localization data. The first set 3 of input data includes at least data from GNSS sensors 8, data from inertial sensors 9, and optionally additional data from wheel rotation sensors 11.
[0074] The Kalman filter 1 has an internal system model with a multitude of parameters, which are typically present in the form of a state vector X, a covariance matrix 14, a noise vector 15, and possibly in the form of further parameters. The system model of the Kalman filter 1 serves to generate precise localization data from the input data. The Kalman filter continuously executes a calibration function with which the parameters of the system model are corrected, so that precise localization data can continue to be generated even in the event of systematic deviations in the input data. The Kalman filter 1 preferably produces permanently corrected parameters 5 of the system model, which are located in particular in the state vector X, the covariance matrix 14, and the noise vector 15.
[0075] In addition to the Kalman filter 1, the filter network 2 includes a strapdown filter 10, which operates in parallel with the Kalman filter 1 and processes a second set 4 of input data. The second set 4 of input data includes data from inertial sensors 9 and, in particular, does not include data from GNSS sensors 8.
[0076] The strapdown filter 10 propagates the parameters of the Kalman filter 1 based on the second set 4 of input data. This means that the strapdown filter 10 propagates the parameters of the Kalman filter 1, in particular independently of the data from the GNSS sensors 8. This results in comparison parameters 6 propagated by the strapdown filter 10, namely, in particular, a propagated state vector X, a propagated covariance matrix 16, and a propagated noise vector 17. Preferably, the propagation of the comparison parameters 6 with the strapdown filter 10 occurs continuously and in parallel with the operation of the Kalman filter 1.
[0077] The filter network 2 further comprises a standstill detection function 12, which detects standstill situations based on data (not defined in more detail here in Fig. 1) and outputs a standstill signal 13. When a standstill situation occurs and the standstill signal 13 is output, a test function 20 is activated, which compares the corrected parameters 5 with the comparison parameters 6. In a standstill situation, only expected deviations should occur between the corrected parameters 5 and the comparison parameters 6, and in particular, no deviations that exceed limit values. In the case of unforeseen deviations, the test function 20 would have a negative result, and an error function 7 would be executed.If necessary, errors 19 estimated by the Kalman filter 1 can be transmitted to the strapdown filter 10 and with the aid of the strapdown filter 10, corrections for inertial sensor signals 18 can be generated, which are available for further processing.
[0078] Fig. 2 shows a flow diagram of the described method. Step a) can be seen, in which a standstill situation is detected. If a standstill situation has been detected, the result of step b) (corrected parameters of the Kalman filter 1) is then subjected to a test according to step c). Step c) is preferably divided into sub-steps c1), c2), and c3). Both step b) and step c1) are optionally also carried out permanently, independently of step a), whereby the results of these steps (corrected parameters 5 of the Kalman filter 1 as the result of step b) and corresponding comparison parameters 6 of the strapdown filter 10 as the result of step c1) for steps c2) and c3) are preferably only used if step a) has shown that a standstill situation exists.
[0079] In step c2), a comparison of the corrected parameters 5 with the comparison parameters 6 is preferably carried out. Preferably, both the corrected parameters 5 and the comparison parameters 6 each comprise a plurality of individual parameters, wherein the comparison comprises in particular the comparison of corresponding individual parameters from the corrected parameters 5 and from the comparison parameters 6.
[0080] In step c3), parameter variances are preferably compared. If steps c2) and c3) reveal that a comparison was unsuccessful, an error function 7 is preferably executed in step d).
[0081] Fig. 3 and Fig. 4 each show how a deviation between corrected parameters 5 and comparison parameters 6 can develop during a standstill situation and which deviation can lead to the detection of an error or to a negative test result of the test of the corrected parameters 5 based on the comparison parameters 6.
[0082] On the parameter axis 22, a variance 23 and the parameter 24, which is monitored for an error, are plotted across the time axis 21. For easier understanding: The parameter 24 is, for example, a position estimate. The variance 23 describes the uncertainty of this position estimate 24. It can be seen that, according to Fig. 3, the parameter 24 behaves according to the expected variance 23 and remains below the variance 23. This shows that the parameter 24 can be estimated here based on the variance 23. Correct or permissible behavior can be determined. The test according to step c) of the described method would result in a positive test. According to Fig. 4, a deviation 25 occurs, which indicates a problem or a systematic error in the filter model, or which can mean a negative test result for the test of the corrected parameters 5 based on the comparison parameters 6.Here, the parameter 24 grows faster than the variance 23. The test according to step c) of the described procedure would result in a negative test.
Claims
Patent claims 1 . Method for testing a correction of a system model in a Kalman filter (1), which is part of a filter network (2) for determining localization data in a motor vehicle and uses a first set (3) of sensor data to determine localization data, wherein the method comprises at least the following steps: a) detecting a standstill situation of the motor vehicle during operation with a standstill detection (12), so that a standstill signal (13) is generated; b) performing a calibration function for parameters of the system model during the standstill situation in the Kalman filter (1), wherein corrected parameters (5) are determined;c) carrying out a test of the corrected parameters (5) by comparing the corrected parameters (5) with comparison parameters (6) when a standstill signal (13) is present, wherein the comparison parameters (6) are determined using another data source (10), wherein the other data source (10) creates comparison parameters (6) using a second set (4) of sensor data, wherein the second set (4) of sensor data is reduced compared to the first set (3) of sensor data, d) executing an error function (7) if a test carried out in step c) is negative; 2. The method according to claim 1, wherein the first set (3) of sensor data used by the Kalman filter (1) comprises at least GNSS signals from at least one GNSS sensor (8) and inertial sensor signals from inertial sensors (9).
3. Method according to one of the preceding claims, wherein age-related changes of GNSS sensors (8) and / or inertial sensors (9) are detected by checking the corrected parameters (5) in step c).
4. Method according to one of the preceding claims, wherein data from GNSS sensors (8) are not taken into account for detecting a standstill situation in step a).
5. Method according to one of the preceding claims, wherein for the detection of a standstill situation in step a) sensor data from at least one inertial sensor (9) and / or sensor data from wheel rotation sensors (11) are used.
6. Method according to one of the preceding claims, wherein, when checking the corrected parameters (5) checked in step c), at least the following parameters are checked: a covariance matrix (14) of the Kalman filter (1); an estimated state vector (x) of the Kalman filter (1); Parameters of a system model of the Kalman filter (1); and a noise vector (15) describing a system noise of the Kalman filter (1).
7. The method according to claim 6, wherein the other data source (10) is a strapdown filter provided in the filter network (2) in addition to the Kalman filter (1) (10), wherein the strapdown filter (10) uses data from at least one inertial sensor (9) to generate comparison parameters (6) for testing the corrected parameters (5) in step c).
8. The method according to claim 7, wherein the comparison parameters (6) generated with the strapdown filter (10) comprise at least one propagated state vector (y) which corresponds to the state vector (x) estimated with the Kalman filter (1) and propagates this state vector (x).
9. The method according to any one of claims 6 to 8, wherein, in parallel with the propagation of the state vector (x) of the Kalman filter (1) with the strapdown filter (10), the at least one covariance matrix (14) of the Kalman filter (1) is propagated with the strapdown filter (10) as a propagated covariance matrix (14).
10. Method according to one of claims 6 to 9, wherein for the testing of the corrected parameters (5) according to step c) a comparison of the parameters determined with the Kalman filter (1) determined covariance matrix (14) and the state vector (x) determined with the Kalman filter (1) with the covariance matrix (14) propagated with the strapdown filter (10) and the state vector (y) propagated with the strapdown filter (10).
11. Control device comprising a processor adapted / configured to carry out the method according to one of claims 1 to 10.
12. A computer program product comprising instructions which, when the computer program product is executed by a computer, cause the computer to carry out the method according to one of claims 1 to 10.
13. A computer-readable storage medium comprising instructions which, when executed by a computer, cause the computer to carry out the steps of the method according to any one of claims 1 to 10.