Method for determining at least one system state by means of a Kalman filter

Abstract

A method determines at least one system state of a system using a Kalman filter. At least one measured value, measured by at least one sensor of the system, is supplied to the Kalman filter. The method includes estimating the at least one system state using the Kalman filter. An estimation result and at least one associated item of information relating to a reliability of the estimation result are output. The method further includes determining a discrepancy between at least one model value associated with the estimation result and at least one measured value associated with the estimation result, and correcting the at least one associated item of information relating to the reliability of the estimation result using the determined discrepancy.

Claims

1. A method of using a Kalman filter to determine at least one vehicle state of a vehicle, the method comprising: supplying at least one measured value measured by at least one sensor of the vehicle to the Kalman filter; estimating the at least one vehicle state using the Kalman filter by outputting an estimation result and at least one associated item of information relating to a reliability of the estimation result; determining a discrepancy between at least one model value associated with the estimation result and at least one measured value associated with the estimation result; correcting the at least one associated item of information relating to the reliability of the estimation result using the determined discrepancy; and performing, based on the estimation result, at least one of (i) a partially automated driving operation and (ii) an autonomous driving operation, using a controller of the vehicle.

2. The method according to claim 1, wherein the vehicle state includes a position of the vehicle.

3. The method according to claim 1, wherein correcting the at least one associated item of information relating to the reliability of the estimation result comprises: continuously correcting the at least one associated item of information relating to the reliability of the estimation result.

4. The method according to claim 1, further comprising: using the determined discrepancy to correct at least one confidence value of the at least one model value and/or of the at least one measured value.

5. The method according to claim 1, wherein determining the discrepancy comprises: determining the discrepancy using the following elements (i) the at least one model value, (ii) the at least one measured value, (iii) at least one fused model value, and (iv) at least one Kalman gain or, as an alternative to the at least one Kalman gain, using at least one covariance of the at least one measured value, and at least one covariance of the at least one model value.

6. The method according to claim 1, further comprising: weighting, using at least one weighting matrix, an influence of the determined discrepancy on the at least one associated item of information relating to the reliability of the estimation result.

7. The method according to claim 1, further comprising: filtering the determined discrepancy using a low-pass filter.

8. The method according to claim 1, wherein the method is carried out by a computer program stored on a non-transitory machine-readable storage medium.

9. A system for determining a position of a vehicle, comprising: at least one sensor; and a microcontroller operably connected to the at least one sensor and configured to use a Kalman filter to determine at least one vehicle state of the vehicle, the microcontroller configured to: supply at least one measured value measured by the at least one sensor to the Kalman filter, estimate the at least one vehicle state using the Kalman filter by outputting an estimation result and at least one associated item of information relating to a reliability of the estimation result, determine a discrepancy between at least one model value associated with the estimation result and at least one measured value associated with the estimation result, and correct the at least one associated item of information relating to the reliability of the estimation result using the determined discrepancy, wherein, based on the estimation result, a controller of the vehicle performs at least one of (i) a partially automated driving operation and (ii) an autonomous driving operation.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) The solution presented here and its technical environment are explained in more detail below on the basis of the figures. It should be pointed out that the disclosure is not intended to be restricted by the exemplary embodiments shown. In particular, unless explicitly described otherwise, it is also possible to extract partial aspects of the scenarios explained in the figures and to combine them with other parts and/or knowledge from other figures and/or the present description. In the drawings:

(2) FIG. 1: shows a typical signal propagation chart of a Kalman filter according to the prior art,

(3) FIG. 2: shows an exemplary sequence of the method presented here,

(4) FIG. 3: shows an exemplary system for determining the position of a vehicle,

(5) FIG. 4: shows an exemplary signal propagation chart of a Kalman filter,

(6) FIG. 5: shows a further exemplary signal propagation chart of a Kalman filter,

(7) FIGS. 6, 7: show illustrations of probability density distributions according to the prior art,

(8) FIG. 8: shows an illustration of probability density distributions which can be achieved with the exemplary embodiment from FIG. 4, and

(9) FIG. 9: shows an illustration of probability density distributions which can be achieved with the exemplary embodiment from FIG. 5.

DETAILED DESCRIPTION

(10) 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:

(11) $\begin{array}{cc}{\hat{x}}_k=F_k{\hat{x}}_{k-1}+B_k\stackrel{.fwdarw.}{u_k}& \left(\mathrm{EQ}18\right)\\ P_k=F_k{P}_{k-1}{F}_k^T+Q_k& \left(\mathrm{EQ}19\right)\\ \underset{\underset{K}{}}{H_k{K}^{}}=\underset{\underset{{}_0}{}}{H_k{P}_k{H}_k^T}{\left(\underset{\underset{{}_0}{}}{H_k{P}_k{H}_k^T}+\underset{\underset{{}_1}{}}{R_k}\right)}^{-1}& \left(\mathrm{EQ}20\right)\\ \underset{{}^{}}{\underset{}{H_k{\hat{x}}_k^{}}}=\underset{\underset{{}_0}{}}{H_k{\hat{x}}_k}+\underset{\underset{K}{}}{H_k{K}^{}}\left(\underset{\underset{{}_1}{}}{\stackrel{.fwdarw.}{z_k}}-\underset{\underset{{}_0}{}}{H_k{\hat{x}}_k}\right)& \left(\mathrm{EQ}21\right)\\ \underset{\underset{{}^{}}{}}{H_k{P}_k^{}{H}_k^T}=\underset{{}_0}{\underset{}{H_k{P}_k{H}_k^T}}-\underset{\underset{K}{}}{H_k{K}^{}}\underset{\underset{{}_0}{}}{H_k{P}_k{H}_k^T}& \left(\mathrm{EQ}22\right)\end{array}$

(12) In this case, equation (EQ1) describes the estimated state vector {circumflex over (x)}.sub.k on the basis of the state vector {circumflex over (x)}.sub.k-1 of the preceding time step (iterative estimation), the system matrix F.sub.k, the control matrix B.sub.k and the control vector {right arrow over (.sub.k)}. In this case, the state vectors generally describe mean values of Gaussian distributions. In other words, according to equation (EQ1), the new best estimation {circumflex over (x)}.sub.k is a prediction which was created from the previous best estimation {circumflex over (x)}.sub.k-1 plus a correction for known external influences.

(13) In this context, equation (EQ2) describes the covariance matrix P.sub.k associated with the Gaussian distribution of the estimated state vector {circumflex over (x)}.sub.k. This results on the basis of the covariance matrix P.sub.k-1 of the preceding time step (iterative estimation), the system matrix F.sub.k and the covariance matrix of the system noise Q.sub.k. In other words, according to equation (EQ2), the new (estimation) uncertainty P.sub.k is predicted from the old uncertainty P.sub.k-1 with an additional uncertainty from the environment.

(14) Equation (EQ3) describes the so-called Kalman gain K or the Kalman gain matrix K. This is formed on the basis of the covariance matrix P.sub.k, the observation matrix H.sub.k and the covariance matrix of the measurement noise R.sub.k. The covariance matrix P.sub.k can form, with the observation matrix H.sub.k, the covariance matrix .sub.0 of the model value vector .sub.0.

(15) Equation (EQ4) describes the correction of the estimated state vector {circumflex over (x)}.sub.k or of the model value vector .sub.0 using measured values which are represented by the measured value vector z.sub.k or .sub.1. A corrected or fused model value vector or a new state vector {circumflex over (x)}.sub.k, which can be used as an input for a subsequent estimation step, therefore results from equation (EQ4).

(16) Equation (EQ5) describes the determination of the corrected or fused covariance matrix P.sub.k or on the basis of the covariance matrix P.sub.k or .sub.0 of the state vector {circumflex over (x)}.sub.k or of the model value vector .sub.0. In this case, the covariance matrix R.sub.k or .sub.1 of the measured value vector z.sub.k or .sub.1 is concomitantly included using the Kalman gain K.

(17) Equations (EQ1) and (EQ2) therefore describe the iterative estimation process of the Kalman filter. This estimation process is indicated with the reference sign 10 in FIG. 1. Equations (EQ3) to (EQ5) describe the subsequent correction or fusion of the iteratively estimated model values with measured values which are captured using sensors. This correction or fusion is indicated with the reference sign 20 in FIG. 1. The corrected or fused (new) model values can be used in a subsequent iteration step in the estimation process 10. This is illustrated with the returning arrow in FIG. 1.

(18) FIG. 2 schematically shows an exemplary sequence of the method presented here. The method is used to determine at least one system state by means of a Kalman filter which is supplied with at least one measured value measured by at least one sensor of the system. The system may be, for example, a system for determining the position of a vehicle.

(19) The order of steps a), b) and c) represented with the blocks 110, 120 and 130 is exemplary and can be run through at least once in the illustrated order, for example, in order to carry out the method. In addition, steps a), b) and c) can also be at least partially carried out in a parallel manner or at the same time.

(20) In block 110, according to step a), the system state is estimated by means of the Kalman filter, wherein an estimation result and at least one associated item of information relating to the reliability of the estimation result are output. In block 120, according to step b), a discrepancy between at least one model value associated with the estimation and at least one measured value associated with the estimation is determined.

(21) For example, in this context, the discrepancy can be determined using the following elements: at least one model value, at least one measured value, at least one fused model value, and at least one Kalman gain or, as an alternative to the Kalman gain, using at least one covariance of the at least one measured value, and at least one covariance of the at least one model value.

(22) For example, the discrepancy can be determined in this case according to the following formula:

(23) $d={}^{}\left(\frac{{\left({}_0-{}^{}\right)}^2}{{}_0}+\frac{{\left({}_1-{}^{}\right)}^2}{{}_1}\right)$
in which d describes the discrepancy, .sub.0 describes the model value, .sub.1 describes the measured value, describes the fused model value, .sub.0 describes the variance of the model value, .sub.1 describes the variance of the measured value and describes the variance of the fused model value, and the variance of the fused model value can be determined according to the formula

(24) ${}^{}=\frac{1}{\frac{1}{{}_0}+\frac{1}{{}_1}}$

(25) In block 130, according to step c), at least one item of information relating to the reliability of the estimation is corrected using the determined discrepancy. In this case, the correction is advantageously carried out continuously. The discrepancy can also be advantageously used to correct at least one confidence value of at least one model value and/or of at least one measured value of the Kalman filter.

(26) Provision may also be made for the influence of the determined discrepancy on the at least one item of information relating to the reliability of the estimation to be weighted by means of a weighting matrix. Furthermore, the determined discrepancy may also be filtered by means of a low-pass filter, for example.

(27) FIG. 3 schematically shows an exemplary system 1 for determining the position of a vehicle 2. The system 1 is provided and configured to carry out a method described here.

(28) FIG. 4 shows an exemplary expansion of the signal propagation chart from FIG. 1. An embodiment of the method described herein can be implemented, for example, using the Kalman filter signal propagation chart according to FIG. 4. In this case, a discrepancy D between the measured values z and model values x increases the estimation uncertainty P. In this respect, equation EQ5 is replaced with equation EQ12, for example. In the case of model errors which result in a large deviation of the model values x from the measured values z, the model values x can be advantageously adjusted more quickly to the measured values z. This advantageously contributes to the Kalman filter model becoming more robust with respect to design errors.

(29) FIG. 5 shows a further alternative exemplary expansion of the signal propagation chart from FIG. 1. A further embodiment of the method described here can be implemented, for example, using the Kalman filter signal propagation chart according to FIG. 5. In this case, a discrepancy D between the measured values z and model values x reduces the influence of the measured values z on the model values. In this respect, the equation for calculating the Kalman gain K (equation EQ3 above) is replaced with equation EQ14, for example. As a result, the Kalman filter model becomes more robust with respect to measurement errors. In order to avoid any algebraic loops in the exemplary embodiments according to FIG. 5, recourse can be had, for example, to the D from the (immediately) preceding time step and/or computing steps of the current time step can be calculated repeatedly.

(30) FIG. 6 shows, by way of example, an illustration of probability density distributions according to the prior art, as may arise when using a Kalman filter illustrated in FIG. 1. In this context, FIG. 6 shows plausible probability density distributions. In a plausible probability density distribution, the areas of the Gaussian bells of the measurement z and the model X greatly overlap. The Gaussian bell x fused therefrom is between the two other Gaussian bells. It is also narrower and higher.

(31) FIG. 7 shows, by way of example, a further illustration of probability density distributions according to the prior art, as may arise when using a Kalman filter illustrated in FIG. 1. In this context, FIG. 7 shows non-plausible probability density distributions. In a non-plausible probability density distribution, both the measurement z and the model x are very certain that they are located correctly, but output values which differ greatly from one another. The areas of the Gaussian bells of the measurement z and the model x scarcely overlap, but the Kalman filter nevertheless calculates the fused Gaussian bell x in the conventional way.

(32) The Gaussian bell x fused therefrom is between the two other Gaussian bells. However, it is too narrow and/or too high. The fused Gaussian bell x must actually be broader and/or have a greater overlap with the other bells. The method described here can contribute to solving this problem.

(33) FIG. 8 shows an illustration of probability density distributions which can be achieved using the exemplary embodiment from FIG. 4. It can be seen that it is possible to achieve the situation in which the fused Gaussian bell x is broader and has a greater overlap with the other bells by taking into account the discrepancy D according to FIG. 4. This advantageously allows the fact that a non-plausible probability density distribution (of the Gaussian bells z and x; cf. FIG. 7) is present to now also be reflected in the fused Gaussian bell x.

(34) FIG. 9 shows an illustration of probability density distributions which can be achieved using the exemplary embodiment from FIG. 5. It can be seen that it is possible to achieve the situation in which the fused Gaussian bell x has a greater overlap with at least one of the other bells, here the Gaussian bell x of the model, by taking into account the discrepancy D according to FIG. 5. This advantageously allows the fact that a non-plausible probability density distribution (of the Gaussian bells z and x; cf. FIG. 7) is present to now also be reflected in the fused Gaussian bell x.

(35) The method described here and the system described here allow one or more of the following advantages, in particular: the Kalman filter is advantageously more robust with respect to interference signals, measurement errors, model inaccuracies and/or other design errors, costs can be advantageously saved when parameterizing the Kalman filter, and the discrepancy calculation can also be advantageously inserted in the expansions of the Kalman filter.