Method for Determining at Least One System State by Means of a Kalman Filter

Abstract

A method for determining at least one system state by way of a Kalman filter assembly, wherein at least one measured value measured by at least one sensor of the system is supplied to the Kalman filter assembly is disclosed. The method includes (a) performing a first estimation of the system state by way of a first Kalman filter of the Kalman filter assembly, a first estimation result and at least one associated first item of information about the reliability of the first estimation result being output, (b) performing a second estimation of the system state by way of a second Kalman filter of the Kalman filter assembly, a second estimation result and at least one associated second item of information about the reliability of the second estimation result being output, the second Kalman filter differing from the first Kalman filter in at least one setting parameter, and (c) fusing the first estimation result and the second estimation result to produce an overall estimation result for the system state, and fusing the first item of information about the reliability of the first estimation result and the second item of information about the reliability of the second estimation result to produce an overall item of information about the reliability of the overall estimation result.

Description

The invention relates to a method for determining at least one system state by means of a Kalman filter, a computer program for performing the method, a machine-readable storage medium on which the computer program is stored, as well as a system for determining the position of a mobile object, such as in particular a vehicle, configured to perform the method. The invention can in particular be applied in the context of at least partially automated or autonomous driving.

PRIOR ART

Kalman filters are used to iteratively estimate system states on the basis of observations which are typically prone to error. In this context, Kalman filters have proven to be particularly advantageous in particular for applications in which sensor information from various sensors has to be combined (or fused) with model information in particular. Moreover, since their calculations are advantageously accurate and robust, Kalman filters are often used in embedded systems. In addition, microcontrollers can advantageously perform the calculations of a Kalman filter in an advantageously efficient manner.

The Kalman filter equations can be described in matrix notation as follows:

$\begin{array}{cc}{\hat{x}}_k=F_k{\hat{x}}_{k-1}+B_k\overrightarrow{u_k}& \left(\mathrm{GL1}\right)\end{array}$ $\begin{array}{cc}{P}_k=F_k{P}_{k-1}{F}_k^T+Q_k& \left(\mathrm{GL2}\right)\end{array}$ $\begin{array}{cc}\underset{K}{\underbrace{H_k{K}^{\prime }}}=\underset{{\sum }_0}{\underbrace{H_k{P}_k{H}_k^T}}{\left(\underset{{\sum }_0}{\underbrace{H_k{P}_k{H}_k^T}}+\underset{{\sum }_1}{\underbrace{R_k}}\right)}^{-1}& \left(\mathrm{GL3}\right)\end{array}$ $\begin{array}{cc}\underset{{\mu }^{\prime }}{\underbrace{H_k{\hat{x}}_k^{\prime }}}=\underset{{\mu }_0}{\underbrace{H_k{\hat{x}}_k}}+\underset{K}{\underbrace{H_k{K}^{\prime }}}\left(\underset{{\mu }_1}{\underbrace{\overrightarrow{z_k}}}-\underset{{\mu }_0}{\underbrace{H_k{\hat{x}}_k}}\right)& \left(\mathrm{GL4}\right)\end{array}$ $\begin{array}{cc}\underset{{\sum }^{\prime }}{\underbrace{H_k{P}_k^{\prime }{H}_k^T}}=\underset{{\sum }_0}{\underbrace{H_k{P}_k{H}_k^T}}-\underset{K}{\underbrace{H_k{K}^{\prime }}}\underset{{\sum }_0}{\underbrace{H_k{P}_k{H}_k^T}}& \left(\mathrm{GL5}\right)\end{array}$

The explicit equations comprising the mathematical symbols K, Σ′, Σ₀, Σ₁, μ′, μ₀, and μ₁, can in particular be used if a corresponding measurement variable with the same scaling exists for each model variable and/or, vice versa, a corresponding model variable at the same scaling exists for each measurement variable. If this is not the case, then e.g. Equations GL3 to GL5 with the mathematical symbols H, K′, P, R, {circumflex over (x)}, and z can be used. For a numerical calculation, these equations can be expressed beforehand in explicit form, which can in particular be achieved by dividing H or H^T on both sides of the equation(s).

Equations GL1 and GL2 describe the iterative estimation process of the Kalman filter, and Equations GL3 to GL5 describe the correction or fusion of the iteratively estimated model values with measured values acquired by means of sensors. For a more detailed explanation, reference is made to the description of a typical Kalman filter design in the context of FIG. 1.

Kalman filters are comparatively complex, however, and there are numerous setting options (in particular the system matrix F_k, the variance matrix R_k of the measurement noise, and the variance matrix Q_k of the system noise) that have to be selected and/or set for the respective system behavior being described. This complicates the use of the Kalman filter for new applications and/or maintenance of existing applications.

In part, so-called extended Kalman filters are also used to be able to model non-linear dynamic processes and/or so-called adaptive Kalman filters (or ROSE filters) in order to be able to adjust the numerous setting possibilities in particular the matrix values R_k and Q_k for the measurement and system noise automatically during the runtime by means of additional filters. However, a comparatively large amount of effort is needed to design or configure the additional filters, wherein the ROSE filter, given its additional filters, is even more difficult to configure.

Kalman filter implementations, such as the ensemble Kalman filter, are also known. Said ensemble typically uses a matrix of possible state vectors instead of one state vector and its associated covariance matrix, and it uses a matrix of possible measurement vectors instead of one measurement vector and its associated covariance matrix. As with the standard Kalman filter, prediction of the state {circumflex over (x)}_i,k of an ensemble element is formed using the transition matrix F_k. However, in deviation from the latter, a noise vector w_i,k is added whose distribution corresponds to a normal distribution according to (0,Q_k), wherein the same process noise Q_k is selected for all ensemble elements. Likewise, for the formation of a measurement vector {right arrow over (z_i,k)} of an ensemble element, a noise vector v_i,k is added, the distribution of which corresponds to a normal distribution (0,R_k), wherein the same covariance matrix R_k of the measurement noise is used for all ensemble elements.

DISCLOSURE OF THE INVENTION

Proposed here according to claim 1 is a method for determining at least one system state by means of a Kalman filter assembly, wherein at least one measured value measured by at least one sensor of the system is supplied to the Kalman filter assembly, said method comprising at least the following steps:

• • a) performing a first estimation of the system state by means of a first Kalman filter of the Kalman filter assembly, a first estimation result and at least one associated first item of information about the reliability of the first estimation result being output,
  • b) performing a second system state estimation using a second Kalman filter of the Kalman filter assembly, a second estimation result and at least one associated second item of information about the reliability of the second estimation result being output, the second Kalman filter differing from the first Kalman filter in at least one setting parameter,
  • c) fusing the first estimation result and the second estimation result to produce an overall estimation result for the system state, and fusing the first item of information about the reliability of the first estimation result and the second item of information about the reliability of the second estimation result to produce an overall item of information about the reliability of the overall estimation result.

The specified sequence of steps a), b) and c) is by way of example and can be performed in the sequence thus specified, e.g., at least once during a normal operating process in order to perform the method. Alternatively or additionally, steps a), b), and c), in particular steps a) and b), can be performed at least partially in parallel or simultaneously. The method can be performed by a control device, e.g. a (micro)controller which can, e.g., be a part of the system also described here.

The method advantageously enables errors or large inaccuracies in model formation and/or configuration and/or deviations from the observed reference situations by distorted measured values to be detected or (automatically) corrected, even if in a given case the assignment of an expected error to a model error and/or a measurement error is not known. The latter is in particular true since, in the Kalman filter assembly, variously adjusted Kalman filters using the measured values, in particular the same measured values, first perform an estimation independently of each other, and these estimation results are then fused to produce an overall estimation result. This advantageously helps to ensure that setting-specific sources of errors, i.e., errors that are based on a Kalman filter setting, can be compensated for as much as possible, in particular by at least one additional Kalman filter of the assembly with a different setting.

The system can, e.g., be a system for determining the position of an object, e.g., a (motor) vehicle. The system can be arranged at least partially in or on the object, e.g. the vehicle. The vehicle can advantageously be configured for at least partially automated and/or autonomous driving operation, e.g., by means of an appropriately configured control device. The control device can be connected to the system in order to receive position data from the system. The system can comprise multiple sensors, in particular various sensors or various types of sensors, or it can be connected to sensors of the vehicle. The sensors can, e.g., comprise at least one GNSS sensor and an (optical or acoustic) environmental sensor, e.g., a camera sensor, LIDAR sensor, RADAR sensor, ultrasonic sensor, or the like. The measured values from the sensors can be fused by means of the method described here or by means of the Kalman filter assembly.

The method contributes in particular to the (continuous) determination of system states by means of an assembly of Kalman filters and depending on sensor data. The Kalman filter assembly can be provided with measured values from multiple different sensors or different types of sensors of the system in order to perform an estimation by taking these measured values into account. The at least one system state can, e.g., include the (current) (ego) position of a mobile object or mobile unit, in particular one that can be moved along the earth's surface, e.g., a (motor) vehicle (car), a ship, an aircraft, a smartphone or a smartwatch. The at least one system state can further include a (current) speed, a (current) direction of movement, and/or a (current) acceleration of the object. Use of the method described is generally advantageous for any sensor data fusion tasks, e.g., position determination, object detection, and/or driving dynamics control of a vehicle. In addition, the method described can also be used for modeling transmission characteristics of a sensor, e.g., in order to reduce measurement noise and other disruptive influences on the sensor signal.

The Kalman filter assembly can comprise multiple Kalman filters (each operating autonomously) able to operate independently of each other. The Kalman filters can indeed operate independently, but preferably using the same measured values. In particular, the Kalman filters of the assembly can operate at least in part in parallel with one another, or at the same time as one another. In other words, this can also be described as the Kalman filters being arranged or connected parallel to one another within the assembly.

The equations for the individual Kalman filters of the Kalman filter assembly can be specified in a manner similar to Equations (1) to (5), as described hereinafter. In this context, the index i denotes the number of the respective Kalman filter:

{circumflex over (x)} _i,k =F _k {circumflex over (x)} _i,k−1 +B _i,k{right arrow over (u_k)}  (6)

P _i,k =F _k P _i,k−1 F _k ^T +Q _i,k   (7)

K′ _i =P _i,k H _k ^T(H_kP_i,kH_k^T+R_i,k)⁻¹   (8)

{circumflex over (x)}′ _i,k ={circumflex over (x)} _i,k +K′ _i({right arrow over (z_k)}−H_k{circumflex over (x)}_i,k)   (9)

P′ _i,k =P _i,k −K′ _i H _k P _i,k   (10)

In step a), a first estimation of the system state is performed by means of a first Kalman filter (index: i=1) of the Kalman filter assembly, wherein a first estimation result (mathematical symbol: {circumflex over (x)}′_1,k) and at least one associated first item of information (mathematical symbol: P′_1,k) about the reliability of the first estimation result are output. In step b), a second estimation of the system state is performed by means of a second Kalman filter (index: i=2) of the Kalman filter assembly, wherein a second estimation result (mathematical symbol: {circumflex over (x)}′_2,k) and at least one associated second item of information (mathematical symbol: P′_2,k) about the reliability of the second estimation result are output. In principle, more than two Kalman filters can be provided or can belong to the Kalman filter assembly. The first estimation and the second estimation are typically performed in the same chronological step (index: k).

The second Kalman filter differs from the first Kalman filter in at least one setting parameter. Accordingly, if more than two Kalman filters are provided, then each further Kalman filter can, e.g., differ from the other Kalman filters in at least one setting parameter. Individual or multiple parameters of the setting options specified hereinabove (in particular the system matrix F_k, the variance matrix of the measurement noise R_k, and the variance matrix of the system noise Q_k) are conceivable as setting parameters. The setting parameters can, e.g., include entries of variance matrices or covariance matrices of the Kalman filters. It is further conceivable that the setting parameters relate to factors regarding the scaling of the corresponding matrices and thus influence the variance matrices or covariance matrices, or they assist in adjusting the relevant Kalman filter.

In particular, in steps a) and b), multiple independent Kalman filters comprising at least the first Kalman filter and the second Kalman filter in parallel, in particular having multiple individual covariance matrices (mathematical symbol: P_i,k) in the same chronological step, but given different assumptions about possible incorrect sensor and/or model variables, in particular given individual assumptions about process noise (mathematical symbol: Q_i,k), and/or given individual assumptions about the covariance matrix for the measurement noise (mathematical symbol: R_i,k) can be calculated. Their independent state vectors (mathematical symbol: {circumflex over (x)}′_i,k) and covariance matrices (mathematical symbol: P′_i,k) are fused together before output, or in step c).

For example, one or more of the following variables can be (selected as) the same for multiple or all Kalman filters of the Kalman filter assembly:

• • i: index of a Kalman filter,
  • j: number of Kalman filters,
  • F_k: transition matrix propagating the system state from time t_k−1 to time t_k, e.g., using motion equations,
  • H_k: an observation matrix mapping the n values of the system state onto the m observations, and/or
  • {right arrow over (z_k)}: new observations provided at time t_k.

Preferably, at least one or more of the following variables (of the second Kalman filter) differ from the variables of another Kalman filter (in particular the first Kalman filter):

• • {circumflex over (x)}_i,k: system state at time t_k derived from the state at the previous time t_k−1 prior to application of the new observations {right arrow over (z_k)},
  • {circumflex over (x)}′_i,k: system state following application of the new observations {right arrow over (z_k)} (a posteriori),
  • P_i,k: covariance matrix of the errors in {circumflex over (x)}_i,k prior to application of the new observations {right arrow over (z_k)} (a priori),
  • P′_i,k: covariance matrix of the errors in {circumflex over (x)}′_i,k after application of the new observations {right arrow over (z_k)} (a posteriori),
  • R_i,k: covariance matrix of the measurement noise, and/or
  • Q_i,k: covariance matrix of the process noise or process noise with which it is possible to make the covariance matrix P′_i,k maintain the correct long-term relation to the system state failures {circumflex over (x)}′_i,k.

In step c), a fusing the first estimation result (mathematical symbol: {circumflex over (x)}′_1,k) and the second estimation result (mathematical symbol: {circumflex over (x)}′_2,k) to an overall estimation result (mathematical symbol: {circumflex over (x)}′_fus,k) for the system state as well as the first item of information (mathematical symbol: P′_1,k) on the reliability of the first estimation result and the second item of information (mathematical symbol: P′_2,k) on the reliability of the second estimation result to an overall information (mathematical symbol: P′_fus,k) concerning the reliability of the overall estimation result takes place.

According to one advantageous configuration, it is proposed that the same measured values be supplied to the first Kalman filter and the second Kalman filter. For example, the first Kalman filter and the second Kalman filter can have measured value inputs connected to the same sensor output. In other words, this can also be described as, regarding the first Kalman filter and the second Kalman filter, the (new) observations (measurement vector: {right arrow over (z_k)}) are equal or the same (in the relevant chronological step).

It is proposed according to a further advantageous configuration that, in step c), the first estimation result is weighted using the first item of information about the reliability of the first estimation result, and the second estimation result is weighted using the second item of information about the reliability of the second estimation result. In particular, the respective estimation results (mathematical symbol: {circumflex over (x)}′_i,k) can be weighted using the associated covariance matrices (mathematical symbol: P′_i,k).

For example, an average {circumflex over (x)}′_fus,k of the system state weighted with the covariance matrices P′_i,k from all j Kalman filters can be calculated as follows:

$\begin{array}{cc}{\hat{x}}_{\mathrm{fus},k}^{\prime }=P_{\mathrm{fus},k}^{\prime }\sum _{i=1}^j{P}_{i,k}^{\prime }{ }_{}^{-1}{\hat{x}}_{i,k}^{\prime }& \left(11\right)\end{array}$

The fused covariance matrix P′_fus,k from all Kalman filters can thereby be calculated as follows:

$\begin{array}{cc}{P}_{\mathrm{fus},k}^{\prime }={\left(\sum _{i=1}^j{P}_{i,k}^{\prime }{ }_{}^{-1}\right)}^{-1}& \left(12\right)\end{array}$

According to a further advantageous configuration, it is proposed that the overall item of information (mathematical symbol: P′_fus,k) about the reliability of the overall estimation result be corrected using a measurement for the discrepancy (mathematical symbol: D) between the first estimation result (mathematical symbol: {circumflex over (x)}′_1,k) and the second estimation result (mathematical symbol: {circumflex over (x)}′_2,k). In the calculation of the measurement for the discrepancy, the overall estimation result (mathematical symbol: {circumflex over (x)}′_fus,k) and/or the respective item of information (mathematical symbol: P′_i,k) about the reliability of the relevant estimation result can also be taken into account.

For example, the discrepancy D between the different estimation results or the Kalman filter model values {circumflex over (x)}′_i,k, can be calculated as follows:

$\begin{array}{cc}D=P_{\mathrm{fus},k}^{\prime }\sum _{i=1}^j{P}_{i,k}^{\prime }{ }_{}^{-1}\left({\hat{x}}_{i,k}^{\prime }-{\hat{x}}_{\mathrm{fus},k}^{\prime }\right){\left({\hat{x}}_{i,k}^{\prime }-{\hat{x}}_{\mathrm{fus},k}^{\prime }\right)}^T& \left(13\right)\end{array}$

Accordingly, the discrepancy D can, e.g., be calculated based on the covariance matrices P′_i,k using the previous variance calculation P′_fus,k, the weighted sum based on the system states {circumflex over (x)}′_i,k of the Kalman filters, and the weighted average {circumflex over (x)}′_fus,k of the system state based on the covariance matrices P′_i,k of the Kalman filters.

The measurement for the discrepancy can (subsequently) further be weighted using a weight E_D for the discrepancy and/or optionally added to the weight E_P of the covariance for the weighted and fused covariance matrix P′_fus,k, e.g. as follows:

P′ _out,k =E _P P′ _fus,k +E _D D   (14)

For example, the weight E_D for the discrepancy or the weight E_P for the covariance in the form of matrices can also be multiplied element-wise or be a scalar variable. In this case, P′_out,k describes the output for the overall item of information about the reliability of the overall estimation result from the Kalman filter assembly.

In particular, the measurement for the discrepancy can be determined or calculated using the following elements (see Equation 13):

• • the first estimation result,
  • the second estimation result,
  • the overall estimation result,
  • the item of information about the reliability of the first estimation result,
  • the item of information about the reliability of the second estimation result, and
  • the overall item of information about the reliability of the overall estimation result.

It can further be provided that the discrepancy or the measurement for the discrepancy be temporally filtered and/or scaled element-wise.

Alternatively or in addition to the calculations according to Equations (11) to (14), the weighted average {circumflex over (x)}′_fus,k of the system state can be calculated with weights π_i, e.g. as follows:

$\begin{array}{cc}{\hat{x}}_{\mathrm{fus},k}^{\prime }=\sum _{i=1}^j{\pi }_i{\hat{x}}_{i,k}^{\prime }& \left(15\right)\end{array}$

In this context, the covariance matrix output P′_fus,k or P′_out,k can be calculated as follows:

$\begin{array}{cc}{P}_{\mathrm{out},k}^{\prime }=\left(\sum _{i=1}^j{\pi }_i\left(P_{i,k}^{\prime }+{\hat{x}}_{i,k}^{\prime }{\hat{x}}_{i,k}^{\prime T}\right)\right)-{\hat{x}}_{\mathrm{fus},k}^{\prime }{\hat{x}}_{\mathrm{fus},k}^{\prime T}& \left(16\right)\end{array}$

In contrast to Equations (11) to (14), the error detection and compensation cannot be quite as pronounced (the deviations from errors can lead to somewhat smaller or small variances). However, Equations (15) and (16) have the advantage that no inverse matrices need be calculated, so computational time can be advantageously saved.

The weights π_i can be statically selected or dynamically calculated, wherein, for the dynamic calculation of the weights π_i, the covariance matrices P′_i,k, the fused system state, {circumflex over (x)}′_fus,k and the system states {circumflex over (x)}′_i,k can be used, and wherein the sum of all weights π_i at any given time is 1.

In particular, it can be provided that (see Equations 15 and 16):

• • the overall estimation result is calculated using the estimation results from the first and second Kalman filters and with weights, and
  • the overall item of information about the reliability of the overall estimation result is calculated using the weighted estimation results from the first and second Kalman filters, the (weighted) item of information about the reliability of the estimation results of the first and second Kalman filters, and the (unweighted) estimation results from the first and second Kalman filters and the weights.

According to a further advantageous configuration, it is proposed that the second Kalman filter differs from the first Kalman filter in at least one setting parameter of the covariance matrix for the measurement noise (mathematical symbol: R_i,k). The setting parameter can, e.g., be an entry in the covariance matrix for the measurement noise or a factor used for scaling the covariance matrix for the measurement noise. The setting parameter is particularly chosen such that the covariance matrix for the measurement noise from the first Kalman filter is different from the covariance matrix for the measurement noise from the second Kalman filter (in the same chronological step).

According to a further advantageous configuration, it is proposed that the second Kalman filter differ from the first Kalman filter in at least one setting parameter of the covariance matrix for the process noise (mathematical symbol: Q_i,k). The setting parameter can, e.g., be an entry of the covariance matrix of the process noise or a factor used for scaling the covariance matrix of the process noise. The setting parameter is in particular chosen such that the covariance matrix of the process noise of the first Kalman filter is different from the covariance matrix of the process noise of the second Kalman filter (in the same chronological step).

In this context, the configuration of the Kalman filters of the Kalman filter assembly can differ primarily in the choice of the covariance matrix R_i,k of the measurement noise and/or the process noise Q_i,k. In particular, for a sensor value whose error cannot in advance be so reliably detected and compensated that its influence on the Kalman filter calculation can be disregarded, a separate Kalman filter with an individual covariance matrix R_i,k of the measurement noise can be determined, or added to the arrangement. In this context, for example, a diagonal element entry of the covariance matrix R_i,k of the measurement noise (in the Kalman filter provided for this purpose) can be increased by the square of the error value. Furthermore (alternatively), e.g., based on known reference values {right arrow over (z)}_ref,k and measured values from a situation with the incorrect signal, an individual covariance matrix R_i,k of the measurement noise can be calculated over a number of n measurement steps as follows:

$\begin{array}{cc}{R}_{i,k}=\frac{1}{n}\sum _{k=1}^n\left({\overrightarrow{z}}_k-{\overrightarrow{z}}_{\mathrm{ref},k}\right){\left({\overrightarrow{z}}_k-{\overrightarrow{z}}_{\mathrm{ref},k}\right)}^T& \left(17\right)\end{array}$

By way of example, for each model value whose errors cannot be reduced strongly enough in advance by means of corresponding modeling, the corresponding diagonal element for the process noise Q_i,k can be increased in a similar manner.

For example, if only a potentially erroneous sensor value or only a potentially erroneous model value is to be compensated for, then it can be advantageous or sufficient to design the Kalman filter assembly using, e.g., only two Kalman filters, wherein the configuration of the first Kalman filter is designed with the assumption that no interference is present, and the configuration of the second Kalman filter is designed with the assumption of interference. For example, if erroneous values of more than one sensor value and/or model value are to be detected, then it can be advantageous to implement a Kalman filter for any possible combination of multiple erroneous sensor values in addition to each Kalman filter for each potentially erroneous sensor value.

According to a further advantageous configuration, it is proposed that at least the first item of information about the reliability of the first estimation result or the second item of information about the reliability of the second estimation result be corrected using a discrepancy between at least one model value associated with the relevant estimation and at least one measured value associated with the relevant estimation. Doing so can advantageously help improve the covariance matrix P′_i,k calculated by means of the Kalman filters for the calculation of the assembly (e.g., to compensate as much as possible for an incorrect assumption of an existing or non-existent interference).

For example, it is advantageous to correct the covariance matrix P′_i,k before calculation of the assembly or the overall item of information about the reliability of the overall estimation result using a Kalman filter individual discrepancy D_i. For example, this discrepancy D_i can be calculated as follows:

D _i =W _i,0 +K′ _i(W_i,1−W_i,0)   (18)

The substitutions W_i,0 and W_i,1 can be calculated as follows:

$\begin{array}{cc}{W}_{i,0}=H_k\left({\hat{x}}_{i,k}-{\hat{x}}_{i,k}^{\prime }\right)\left({\hat{x}}_{i,k}^T-{\hat{x}}_{i,k}^{\prime T}\right)H_k^T& \left(19\right)\end{array}$ $\begin{array}{cc}{W}_{i,1}=\left(\overrightarrow{z_k}-H_k{\hat{x}}_{i,k}^{\prime }\right){\left(\overrightarrow{z_k}-H_k{\hat{x}}_{i,k}^{\prime }\right)}^T& \left(20\right)\end{array}$

Regarding calculation of the arrangement using Equations (11) to (13) or the overall item of information about the reliability of the overall estimation result, instead of the covariance matrix P′_i,k in this case it is possible to use the corrected covariance matrix P″_i,k, which can be calculated using the Kalman filter individual discrepancies D_i as follows:

P″ _i,k =P′ _i,k +E _i H _k ^T D _i H _k   (21)

For example, the weighting matrix E_i can be multiplied element-wise by the discrepancy.

In addition, it is also advantageous to filter the discrepancy element-wise PT1, whereby the covariance matrix output P″_i,k can advantageously be more stable and robust with respect to the measurement noise.

Proposed according to a further aspect is a computer program for performing a method presented here. In other words, this aspect relates in particular to a computer program (product) comprising instructions which, when the program is executed by a computer, cause the latter to perform a method described herein.

Proposed according to a further aspect is a machine-readable storage medium on which the computer program proposed here is stored or saved. The machine-readable storage medium is typically a computer-readable data carrier.

Proposed according to a further aspect is a system for determining the position of a mobile object, e.g. a vehicle, configured to perform a method described here. The system for determining the position of a mobile object can, e.g., be provided and configured to determine the ego position of a mobile object and/or to measure the position relative to other, in particular moving, mobile objects, e.g. road users. The system can, e.g., comprise a movement and position sensor configured to perform a method described here. The movement and position sensor can, e.g., moreover receive GNSS data and/or environmental sensor data (from environmental sensors of the mobile object or vehicle). In order to perform the method, the system can comprise, e.g., a computing means, e.g., a (micro)controller able to access the computer program described herein. In this context, the storage medium can, e.g., likewise be a part of or connected to the system.

The details, features, and advantageous configurations discussed in connection with the method can accordingly also be performed in the computer program presented herein, and/or the storage medium, and/or the system, and vice versa. In this respect, reference is made to the entirety of the statements made thereby for a more specific characterization of the features.

The solution presented here and the technical environment thereof are explained in greater detail hereinafter with reference to the drawings. It should be noted that the invention is not to be limited by the embodiment examples shown. In particular, unless explicitly stated otherwise, it is also possible to extract partial aspects of the factual subject matter explained in the drawings and to combine them with other components and/or insights based on other drawings and/or the present description. Shown schematically are:

FIG. 1: a typical signal flow diagram of a Kalman filter according to the prior art,

FIG. 2: an exemplary sequence of the method presented herein, and

FIG. 3: an exemplary, more detailed illustration of the method presented herein,

FIG. 4: an illustration of probability density that can be realized with the method presented here, and

FIG. 5: an exemplary system for determining the position of a vehicle.

FIG. 1 schematically shows a typical structure of a Kalman filter according to the prior art. The Kalman filter equations on which this structure is based can be described in matrix notation as follows:

$\begin{array}{cc}{\hat{x}}_k=F_k{\hat{x}}_{k-1}+B_k\overrightarrow{u_k}& \left(\mathrm{GL22}\right)\end{array}$ $\begin{array}{cc}{P}_k=F_k{P}_{k-1}{F}_k^T+Q_k& \left(\mathrm{GL23}\right)\end{array}$ $\begin{array}{cc}\underset{K}{\underbrace{H_k{K}^{\prime }}}=\underset{{\sum }_0}{\underbrace{H_k{P}_k{H}_k^T}}{\left(\underset{{\sum }_0}{\underbrace{H_k{P}_k{H}_k^T}}+\underset{{\sum }_1}{\underbrace{R_k}}\right)}^{-1}& \left(\mathrm{GL24}\right)\end{array}$ $\begin{array}{cc}\underset{{\mu }^{\prime }}{\underbrace{H_k{\hat{x}}_k^{\prime }}}=\underset{{\mu }_0}{\underbrace{H_k{\hat{x}}_k}}+\underset{K}{\underbrace{H_k{K}^{\prime }}}\left(\underset{{\mu }_1}{\underbrace{\overrightarrow{z_k}}}-\underset{{\mu }_0}{\underbrace{H_k{\hat{x}}_k}}\right)& \left(\mathrm{GL25}\right)\end{array}$ $\begin{array}{cc}\underset{{\sum }^{\prime }}{\underbrace{H_k{P}_k^{\prime }{H}_k^T}}=\underset{{\sum }_0}{\underbrace{H_k{P}_k{H}_k^T}}-\underset{K}{\underbrace{H_k{K}^{\prime }}}\underset{{\sum }_0}{\underbrace{H_k{P}_k{H}_k^T}}& \left(\mathrm{GL26}\right)\end{array}$

Equation (GL1) describes the estimated state vector {circumflex over (x)}_k based on the state vector {circumflex over (x)}_k−1 of the previous chronological step (iterative estimation), the system matrix F_k, the control matrix B_k, and the control vector {right arrow over (u_k)}. The state vectors usually describe mean values of Gaussian distributions. In other words, according to Equation (GL1), the new best estimation {circumflex over (x)}_k is a prediction made based on the previous best estimation {circumflex over (x)}_k−1 plus a correction for known external influences.

In this context, Equation (GL2) describes the covariance matrix P_k associated with the Gaussian distribution of the estimated state vector {circumflex over (x)}_k. The latter is obtained based on the covariance matrix P_k−1 of the previous chronological step (iterative estimation), the system matrix F_k, and the covariance matrix of the system noise Q_k. In other words, according to Equation (GL2), the new (estimation) uncertainty P_k is predicted based on the old uncertainty P_k−1 along with an additional uncertainty from the environment.

Equation (GL3) describes what is referred to as the Kalman gain K or the Kalman gain matrix K′. The latter is formed on the basis of the covariance matrix P_k, the observation matrix H_k, and the covariance matrix of the measurement noise R_k. The covariance matrix P_k along with the observation matrix H_k can form the covariance matrix Σ₀ for the model value vector μ₀.

Equation (GL4) describes the correction of the estimated state vector {circumflex over (x)}_k or model value vector μ₀ with measured values represented by the measured value vector z_k or μ₁. Equation (GL4) therefore produces a corrected or fused model value vector μ′ or a new state vector {circumflex over (x)}′_k, which can be used as the input for a subsequent estimation step.

Equation (GL5) describes the determination of the corrected or fused covariance matrix P′_k or Σ′ based on the covariance matrix P_k or Σ₀ the state vector {circumflex over (x)}_k or the model value vector μ₀. In this case, the covariance matrix R_k or Σ₁ the measured value vector z_k or μ₁ is included via the Kalman gain K.

Equations (GL1) and (GL2) therefore describe the iterative estimation process of the Kalman filter. This estimation process is identified in FIG. 1 using reference number 10. Equations (GL3) to (GL5) describe the subsequent correction or fusion of the iteratively estimated model values using measured values acquired by means of sensors. This correction or fusion is identified in FIG. 1 using reference number 20. The corrected or fused (new) model values can be used in a subsequent iteration step of the estimation process 10. This is indicated by the reversed arrow in FIG. 1.

FIG. 2 schematically shows an example of a sequence of the method presented herein. The method is used to determine at least one system state by means of a Kalman filter. At least one measured value measured by at least one sensor of the system is thereby supplied to the Kalman filter assembly. The system can for example be a system for determining the position of a vehicle.

The sequence of steps a), b), and c), as shown by blocks 110, 120 and 130, is by way of example and can, e.g., be gone through at least once in the sequence described in order to perform the method. Steps a), b), and c), in particular steps a) and b), can furthermore also be performed at least partially in parallel or simultaneously.

In block 110, according to step a), a first estimation of the system state is performed by means of a first Kalman filter of the Kalman filter assembly, wherein a first estimation result and at least one associated first item of information about the reliability of the first estimation result are output.

In block 120, according to step b), a second estimation of the system state is performed by means of a second Kalman filter of the Kalman filter assembly, wherein a second estimation result and at least one associated second item of information about the reliability of the second estimation result are output, wherein the second Kalman filter differs from the first Kalman filter by at least one setting parameter.

The first Kalman filter and the second Kalman filter can in this case be provided with the same measured values.

For example, the second Kalman filter can differ by at least one setting parameter of the covariance matrix for the measurement noise from the first Kalman filter.

Alternatively or cumulatively, the second Kalman filter can differ by at least one setting parameter of the covariance matrix of the process noise from the first Kalman filter.

In this context, it can also be provided that the at least one different setting parameter of the second Kalman filter acts on a covariance matrix for the measurement noise of the second Kalman filter and/or on a covariance matrix of the process noise of the second Kalman filter, e.g., by the type of factor used for scaling the relevant matrix.

In block 130, according to step c), fusing the first estimation result and the second estimation result is performed to produce an overall estimation result about the state of the system, as well as fusing the first item of information about the reliability of the first estimation result and the second item of information about the reliability of the second estimation result to produce an overall item of information about the reliability of the overall estimation result.

The first estimation result can in this case be weighted using the first item of information about the reliability of the first estimation result, and the second estimation result can be weighted using the second item of information about the reliability of the second estimation result.

Further, the overall item of information about the reliability of the overall estimation result can be corrected using a measurement for the discrepancy between the first estimation result and the second estimation result.

Furthermore, it can be provided that at least the first item of information about the reliability of the first estimation result, or the second item of information about the reliability of the second estimation result, are corrected using a discrepancy between at least one model value associated with the relevant estimation and at least one measured value associated with the relevant estimation.

FIG. 3 schematically shows an exemplary detailed illustration of the method presented here. In the Kalman filter assembly, a Kalman filter estimation is respectively performed in step a) and step b), with the corresponding outputs: first estimation result {circumflex over (x)}′_1,k and associated first item of information P′_1,k about the reliability of the first estimation result and the second estimation result {circumflex over (x)}′_2,k and the associated second item of information P′_2,k about the reliability of the second estimation result. These are fused in step c) to produce the overall estimation result {circumflex over (x)}′_fus,k and the associated overall item of information P′_fus,k about the reliability of the overall estimation result.

The Kalman filters illustrated in FIG. 3 by way of example can differ from one another in their setting in that they have been set or designed given different assumptions regarding sensor accuracy. In this context, the second Kalman filter can, e.g., differ in at least one setting parameter in the covariance matrix R_k of the measurement noise from the first Kalman filter. In simple terms, in the first Kalman filter it could be assumed that there is no sensor error, and in the second Kalman filter it could be assumed that there is a sensor error.

FIG. 4 schematically shows an illustration of probability density distributions that can be realized with the method presented here. This illustration is based on the exemplary Kalman filter assembly shown in FIG. 3.

FIG. 5 schematically shows an exemplary system 1 for determining the position of a mobile object 2, e.g. a vehicle. The system 1 is provided and configured to perform a method described here.

The method described here and the system described here in particular enable one or more of the following advantages:

• • The Kalman filter assembly is advantageously more robust against model and measurement errors.
  • Costs can be saved in the development and parameterization of the Kalman filter, in particular in modeling and in the development and parameterization of sensor interference detection and compensation.
  • The discrepancy calculation can also be advantageously applied to the outputs from the EKF (extended Kalman filter) and UKF (unscented Kalman filter) Kalman filter extensions, as well as using the known EnKF (ensemble Kalman filter)

Claims

1. A method for determining at least one system state by way of a Kalman filter assembly, wherein at least one measured value measured by at least one sensor of the system is supplied to the Kalman filter assembly, said method comprising: a) performing a first estimation of the system state by way of a first Kalman filter of the Kalman filter assembly, a first estimation result and at least one associated first item of information about the reliability of the first estimation result being output,b) performing a second estimation of the system state by way of a second Kalman filter of the Kalman filter assembly, a second estimation result and at least one associated second item of information about the reliability of the second estimation result being output, the second Kalman filter differing from the first Kalman filter in at least one setting parameter, andc) fusing the first estimation result and the second estimation result to produce an overall estimation result for the system state, and fusing the first item of information about the reliability of the first estimation result and the second item of information about the reliability of the second estimation result to produce an overall item of information about the reliability of the overall estimation result.

2. The method according to claim 1, wherein the first Kalman filter and the second Kalman filter are supplied with the same measured values.

3. The method according to claim 1, wherein step c) includes: weighing the first estimation result using the first item of information about the reliability of the first estimation result, andweighing the second estimation result using the second item of information about the reliability of the second estimation result.

4. The method according to claim 1, further comprising correcting the overall item of information about the reliability of the overall estimation result using a measurement of the discrepancy between the first estimation result and the second estimation result.

5. The method according to claim 1, wherein the second Kalman filter differs in at least one setting parameter of the measurement noise covariance matrix from the first Kalman filter.

6. The method according to claim 1, wherein the second Kalman filter differs in at least one setting parameter of the process noise covariance matrix from the first Kalman filter.

7. The method according to claim 1, further comprising correcting at least the first item of information about the reliability of the first estimation result or the second item of information about the reliability of the second estimation result using a discrepancy between at least one model value associated with the relevant estimation and at least one measured value associated with the relevant estimation.

8. A computer program for performing a method according to claim 1.

9. A machine-readable storage medium on which the computer program according to claim 8 is stored.

10. A system for determining the position of a mobile object, said system being configured to perform a method according to claim 1.