Method of Determining an Uncertainty Estimate of an Estimated Velocity

Abstract

A method of determining an uncertainty estimate of an estimated velocity of an object includes, determining the uncertainty with respect to a first estimated coefficient and a second estimated coefficient of the velocity profile equation of the object. The first estimated coefficient being assigned to a first spatial dimension of the estimated velocity and the second estimated coefficient being assigned to a second spatial dimension of the estimated velocity. The velocity profile equation represents the estimated velocity in dependence of the first estimated coefficient and the second estimated coefficient. The method also includes determining the uncertainty with respect to an angular velocity of the object, a first coordinate of the object in the second spatial dimension, and a second coordinate of the object in the first spatial dimension.

Claims

1. A method comprising: determining, by at least one processor, an uncertainty estimate of an estimated velocity of an object, the uncertainty estimate representing an uncertainty of the estimated velocity with respect to a true velocity of the object, wherein the uncertainty estimate is determined by: determining a first portion of the uncertainty estimate, the first portion representing the uncertainty with respect to a first estimated coefficient and a second estimated coefficient of a velocity profile equation of the object, the first estimated coefficient being assigned to a first spatial dimension of the estimated velocity and the second estimated coefficient being assigned to a second spatial dimension of the estimated velocity, wherein the velocity profile equation represents the estimated velocity in dependence of the first estimated coefficient and the second estimated coefficient; determining a second portion of the uncertainty estimate, the second portion representing the uncertainty with respect to an angular velocity of the object, a first coordinate of the object in the second spatial dimension and a second coordinate of the object in the first spatial dimension; and determining the uncertainty estimate based on the first portion and the second portion.

2. The method according to claim 1, further comprising: controlling, by the processor, a vehicle in a vicinity of the object in dependence of the estimated velocity of the object, wherein the estimated velocity is processed in dependence of the uncertainty estimate.

3. The method according to claim 1, wherein the uncertainty estimate represents a dispersion of the estimated velocity, the first portion represents a dispersion of the first estimated coefficient and the second estimated coefficient, and the second portion represents a dispersion of the angular velocity of the object.

4. The method according to claim 1, wherein the uncertainty estimate and one or more of the first portion and the second portion of the uncertainty estimate are determined as a two-dimensional matrix, and the two-dimensional matrix represents a dispersion with respect to the first spatial dimension and the second spatial dimension.

5. The method according to claim 1, wherein the first portion is determined based on: a covariance portion representing a covariance matrix of the first estimated coefficient and the second estimated coefficient; and one or more of a bias portion representing a bias of the first estimated coefficient and the second estimated coefficient.

6. The method according to claim 5, wherein the bias portion is determined based on a plurality of detection points of the object and at least one constant, and each of the plurality of detection points comprises an estimated velocity assigned to a detection position on the object, the detection position on the object being represented at least by an angle.

7. The method according to claim 1, wherein the estimated velocity is assigned to a position of the object represented by the first coordinate of the object in the first spatial dimension and the second coordinate of the object in the second spatial dimension, and the uncertainty estimate is determined in dependence of the position of the object.

8. The method according to claim 1, wherein the second portion is determined based on an intermediate second portion representing the uncertainty with respect to the angular velocity only, and the intermediate second portion is predetermined.

9. The method according to claim 8, wherein the intermediate second portion is predetermined by a variance of a distribution of the angular velocity of the object.

10. The method according to claim 9, wherein the distribution is a uniform distribution with at least one predetermined extremum of the angular velocity of the object.

11. The method according to claim 8, wherein the intermediate second portion is predetermined by at least one parameter representing an extremum of the angular velocity of the object.

12. The method according to claim 1, wherein the uncertainty estimate is determined based on a sum of the first portion and the second portion.

13. A system comprising: at least one processor configured to determine an uncertainty estimate of an estimated velocity of an object for controlling a vehicle, the uncertainty estimate representing an uncertainty of the estimated velocity with respect to a true velocity of the object, wherein the processor is configured to determine the uncertainty estimate by: determining a first portion of the uncertainty estimate, the first portion representing the uncertainty with respect to a first estimated coefficient and a second estimated coefficient of a velocity profile equation of the object, the first estimated coefficient being assigned to a first spatial dimension of the estimated velocity and the second estimated coefficient being assigned to a second spatial dimension of the estimated velocity, wherein the velocity profile equation represents the estimated velocity in dependence of the first estimated coefficient and the second estimated coefficient; determining a second portion of the uncertainty estimate, the second portion representing the uncertainty with respect to an angular velocity of the object, a first coordinate of the object in the second spatial dimension and a second coordinate of the object in the first spatial dimension; and determining the uncertainty estimate based on the first portion and the second portion.

14. The system of claim 13, wherein the processor is further configured to: control a vehicle in a vicinity of the object in dependence of the estimated velocity of the object, wherein the estimated velocity is processed in dependence of the uncertainty estimate.

15. The system of claim 13, wherein the uncertainty estimate represents a dispersion of the estimated velocity, the first portion represents a dispersion of the first estimated coefficient and the second estimated coefficient, and the second portion represents a dispersion of the angular velocity of the object.

16. The system of claim 13, wherein the uncertainty estimate and one or more of the first portion and the second portion of the uncertainty estimate are determined as a two-dimensional matrix, and the two-dimensional matrix represents a dispersion with respect to the first spatial dimension and the second spatial dimension.

17. The system of claim 13, wherein the first portion is determined based on: a covariance portion representing a covariance matrix of the first estimated coefficient and the second estimated coefficient; and one or more of a bias portion representing a bias of the first estimated coefficient and the second estimated coefficient.

18. The system of claim 17, wherein the bias portion is determined based on a plurality of detection points of the object and at least one constant, and each of the plurality of detection points comprises an estimated velocity assigned to a detection position on the object, the detection position on the object being represented at least by an angle.

19. The system of claim 13, wherein the estimated velocity is assigned to a position of the object represented by the first coordinate of the object in the first spatial dimension and the second coordinate of the object in the second spatial dimension, and the uncertainty estimate is determined in dependence of the position of the object.

20. The system of claim 13, wherein the second portion is determined based on an intermediate second portion representing the uncertainty with respect to the angular velocity only, and the intermediate second portion is predetermined.

Description

BRIEF DESCRIPTION OF DRAWINGS

[0046] The invention is further described by way of example with reference to the drawings in which:

[0047] FIG. 1 shows a target coordinate system;

[0048] FIG. 2 shows a vehicle coordinate system;

[0049] FIG. 3 shows a sensor coordinate system;

[0050] FIG. 4 shows a target vehicle with respect to a host vehicle with detection points located on the target vehicle;

[0051] FIG. 5 illustrates how to calculate velocity vectors at the location of a detection point; and

[0052] FIG. 6 illustrates an embodiment of the method as described herein.

DETAILED DESCRIPTION

[0053] Generally, a host vehicle 4 (see FIG. 2) is equipped with a radar system 5′ (see FIG. 2) where reflected radar signals from a target 2 (FIG. 1) in the field of view of the radar system 5′ are processed to provide data in order to ascertain parameters used in the methodology.

[0054] In order to do this various conditions and requirements may be of advantage. The target 2 (rigid body, e.g. vehicle) is preferably an extended target, i.e., the target allows the determination of a plurality of points of reflection 6′ (see FIG. 4) that are reflected from the target 2 in real-time and that are based on raw radar detection measurements.

[0055] The target 2 is an example of an object in the sense of the general part of the description and the claims. However, other types of objects are also possible, in particular objects that appear in ordinary traffic scenes, like motorcycles, bicycles, pedestrians, large as well as small vehicles. Moreover, in principal objects can also be stationary objects.

[0056] As used herein, the term “extended target” is used to refer to targets 2 that are capable of providing multiple, i.e. two, three or more spaced-apart scattering-points 6′ also known as points of reflection 6′. The term “extended target” is thus understood as a target 2 that has some physical size. In this instance, it should be noted that the physical size can be selected, e.g., in the range of 0.3 m to 20 m in order to be able to detect points of reflection 6′ stemming from, e.g., a moving person to a moving heavy goods vehicle or the like.

[0057] The various scattering points 6′ are not necessarily individually tracked from one radar scan to the next and the number of scattering points 6′ can be a different between scans. Furthermore, the locations of the scattering points 6′ can be different on the extended target 2 in successive radar scans.

[0058] Radar points of reflection 6′ can be determined by the host vehicle 4 from radar signals reflected from the target 2, wherein a comparison of a given reflected signal with an associated emitted radar signal can be carried out to determine the position of the radar point of reflection 6′, e.g., in Cartesian or Polar coordinates (azimuth angle, radial range) with respect to the position of a radar-emitting and/or radar-receiving element/unit on the host vehicle, which can be the position of the radar sensor unit.

[0059] By using, e.g., Doppler radar techniques, the range rate is also determined as known in the art. It is to be noted that the “raw data” from a single radar scan can provide the parameters θ.sub.i (azimuth angle) and {dot over (r)}.sub.l (raw range rate, i.e., radial velocity) for the i-th point of reflection of n points of reflection. These are the parameters which are used to estimate the velocity of a (moving) target, wherein i=1, . . . , n.

[0060] It is also to be noted that the term instantaneous radar scan, single radar scan or single measurement instance can include reflection data from a “chirp” in Doppler techniques, which may scan over, e.g., up to 2 ms. This is well known in the art. In the subsequent description, the following conventions and definitions are used:

World Coordinate System

[0061] As a convention, a world coordinate system with the origin fixed to a point in space is used—it is assumed that this world coordinate system does not move and does not rotate. Conventionally, the coordinate system is right-handed; the Y-axis, orthogonal to the X-axis, pointing to the right; the Z-axis pointing into the page and a an azimuth angle is defined in negative direction (clock-wise) with respect to the X-axis; see FIG. 1 which shows such a coordinate system with origin 1 and a non-ego vehicle 2. FIG. 1 further shows the extended target 2 in the form of a vehicle, e.g. an object having a length of approximately 4.5 m.

Vehicle Coordinate System

[0062] FIG. 2 shows a vehicle coordinate system that in the present instance has its origin 3″ located at the center of the front bumper 3 of a host vehicle 4. It should be noted in this connection that the origin 3″ of the vehicle coordinate system can be arranged at different positions at the host vehicle 4.

[0063] In the present instance the X-axis is parallel to the longitudinal axis of the vehicle 4, i.e. it extends between the front bumper 3 and a rear bumper 3′ and intersects with the center of the front bumper 3 if the origin 3″ is located there. The vehicle coordinate system is right-handed with the Y-axis orthogonal to the X-axis and pointing to the right, the Z-axis pointing into the page. An (azimuth) angle is defined as in the world coordinate system.

Sensor Coordinate System

[0064] FIG. 3 shows a sensor coordinate system having the origin 5. In the example of FIG. 3 the origin 5 is located at the center of a radar system (in particular sensor unit) 5′, which can be a radome. The X-axis is perpendicular to the sensor radome, pointing away from the radome. The coordinate system is right-handed: Y-axis orthogonal to the X-axis and pointing to the right; Z-axis pointing into the page. An (azimuth) angle is defined as in the world coordinate system.

[0065] The velocity and the yaw rate of the host vehicle 4 are assumed to be known from sensor measurements known in the art. The over-the-ground (OTG) velocity vector of the host vehicle 4 is defined as:

V.sub.h=[u.sub.h v.sub.h].sup.t,

where u.sub.h is the longitudinal velocity of the host vehicle 4 (i.e., the velocity in a direction parallel to the X-axis of the vehicle coordinate system) and v.sub.h is lateral velocity of the host vehicle 4 (i.e., the velocity in a direction parallel to the Y-axis of the vehicle coordinate system). In more general terms, the longitudinal velocity and the lateral velocity are a first and a second velocity component of the host vehicle 4, respectively. The X-axis and the Y-axis generally correspond to a first spatial dimension and a second spatial dimension, respectively. Likewise, the longitudinal direction and the lateral direction correspond to the first spatial dimension and the second spatial dimension, respectively. These are preferably but not necessarily in orthogonal relation to each other.

[0066] The sensor mounting position and boresight angle with respect to the vehicle coordinate system are assumed to be known with respect to the vehicle coordinate system (VCS), wherein the following notations are used:

x.sub.s,VCS—sensor mounting position with respect to longitudinal (X−) coordinate
y.sub.s,VCS—sensor mounting position with respect to lateral (Y) coordinate
Y.sub.s,VCS—sensor boresight angle.

[0067] The sensor over-the-ground (OTG) velocities can be determined from the known host vehicle velocity and the known sensor mounting position. It is understood that more than one sensor can be integrated into one vehicle and specified accordingly.

[0068] The sensor OTG velocity vector is defined as:

V.sub.s=[u.sub.s v.sub.s].sup.T,

wherein u.sub.s is the sensor longitudinal velocity and v.sub.s is the sensor lateral velocity corresponding generally to first and second velocity components in the case of a yaw rate of zero.

[0069] At each radar measurement instance (scan) the radar sensor unit captures n (raw) detection points from the target. Each detection point i=1, . . . , n can be described by the following parameters expressed in the sensor coordinate system:

r.sub.i—range (or radial distance),
θ.sub.i—azimuth angle,
{dot over (r)}.sub.i—raw range rate (or radial velocity).

[0070] Target planar motion can be described by the target OTG velocity vector at the location of each raw detection:

V.sub.t,i[u.sub.t,i v.sub.t,i].sup.T,

wherein u.sub.t,i represents the longitudinal velocity of the target at the location of the i-th detection point and v.sub.t,i represents the lateral velocity of the target at the location of the i-th detection point, both preferably but not necessarily with respect to the sensor coordinate system.

[0071] Target planar motion can be described as well by:

V.sub.t,COR=[ω.sub.t x.sub.t,COR y.sub.t,COR].sup.T,

wherein ω.sub.t represents the yaw rate (angular velocity) of the target, x.sub.t,COR the longitudinal coordinate of the center of target's rotation and y.sub.t,COR the lateral coordinate of the center of target's rotation.

[0072] FIG. 4 illustrates target velocity vectors as lines originating from a plurality of detection points 6′ illustrated as crosses, wherein the detection points 6′ are all located on the same rigid body target 2 and wherein the detection points 6′ are acquired using a sensor unit of a host vehicle 4.

[0073] The general situation is shown in greater detail in FIG. 5 showing three detection points 6 located on a target (not shown) with a center of rotation 7. The vehicle coordinate system with axes X.sub.VCS, Y.sub.VCS is shown in overlay with the sensor coordinate system having axes X.sub.SCS, Y.sub.SCS. The velocity vector of one of the detection points 6 (i=1) is shown together with its components u.sub.t,i, v.sub.t,i.

[0074] The range rate equation for a single detection point 6 can be expressed as follows:

{dot over (r)}.sub.i+u.sub.s cos θ.sub.i+v.sub.s sin θ.sub.i=u.sub.t,i cos θ.sub.i+v.sub.t,i sin θ.sub.i,

wherein {dot over (r)}.sub.i represents the range rate, i.e., the rate of change of the distance between the origin of the sensor coordinate system and a detection point 6, as illustrated in FIG. 5. The location of the detection point 6 can be described by the azimuth angle θ.sub.i=1 and the value of the radial distance r.sub.i=1 (range of detection point, i.e. distance between origin and the detection point).

[0075] To simplify the notation the compensated range rate can be defined as:

{dot over (r)}.sub.i,cmp={dot over (r)}.sub.i+u.sub.s cos θ.sub.i+v.sub.s sin θ.sub.i

with {dot over (r)}.sub.i,cmp representing the range rate of the i-th detection point compensated for the velocity of the host vehicle 4.

[0076] The compensated range rate can also be expressed as:

{dot over (r)}.sub.i,cmp=u.sub.t,i cos θ.sub.i=v.sub.t,i sin θ.sub.i.

[0077] The compensated range rate can also be expressed in vector notation as:

{dot over (r)}.sub.i,cmp=[cos θ.sub.t,i sin θ.sub.i][.sub.v.sub.t,i.sup.u.sup.t,i].

[0078] The so called velocity profile equation (or range rate equation) is defined as:

{dot over (r)}.sub.i,cmp=c.sub.t cos θ.sub.i+s.sub.t sin θ.sub.i,

wherein c.sub.t represents the first, e.g. longitudinal, coefficient or component of the range rate and s.sub.t represents the second, e.g. lateral, coefficient or component of the range rate equation. Note that the coefficients c.sub.t, s.sub.t are preferably invariant with respect to the azimuth angle at least for a range of azimuth angles corresponding to the location of the target to which a plurality of detection points refer to and on which basis the coefficients have been determined. This means that the velocity profile equation is assumed to be valid not only for specific detection points but for a range of azimuth angles. Therefore, the range rate can readily be determined for any azimuth angle from a specific angle range using the range rate equation. The range rate is an example of an estimated velocity in the general sense of the disclosure.

[0079] As the skilled person understands, in practice, the “true” coefficients c.sub.t, s.sub.t are usually estimated from a plurality of detection points. These estimates are denoted {tilde over (c)}.sub.t and {tilde over (s)}.sub.t and are estimated using, e.g., an iteratively (re-) weighted least squares methodology. In the following, an example method for estimating the coefficients c.sub.t, s.sub.t is described.

[0080] Step 1: In an initial step, the method comprises emitting a radar signal and determining, from a plurality of radar detection measurements captured by said radar sensor unit, a plurality of radar detection points at one measurement instance. Each radar detection point comprises at least an azimuth angle θ.sub.i and a range rate {dot over (r)}.sub.i, wherein the range rate {dot over (r)}.sub.i represents the rate of change of the distance between the sensor unit and the target at the location of the i-the detection point (cf. FIG. 4). It is understood that the azimuth angle θ.sub.i describes the angular position of the i-th detection point. It is assumed that the plurality of detection points are located on a single target (such target is usually referred to as a distributed target) as shown in FIG. 4. The target is an object.

[0081] Step 2: The compensated range rate {dot over (r)}.sub.i,cmp is determined as:

{dot over (r)}.sub.i,cmp={dot over (r)}.sub.i+u.sub.s cos θ.sub.i+v.sub.s sin θ.sub.i,

wherein u.sub.s represents the first (e.g. longitudinal) velocity component of the sensor unit and wherein v.sub.s represents the second (e.g. lateral) velocity component of the sensor unit. The compensated range rate is the range rate compensated for the velocity of the host vehicle. Therefore, the compensated range rate can be interpreted as the effective velocity of the target at the location of the i-th detection point. The compensated range rate corresponds to an estimated velocity of the target.

[0082] Step 3: From the results of steps 1 and 2, an estimation {tilde over (c)}.sub.t of the first coefficient c.sub.t of the velocity profile equation of the target and an estimation {tilde over (s)}.sub.t of the second coefficient s.sub.t of the velocity profile equation of the target are preferably determined by using an iteratively reweighted least squares (IRLS) methodology (ordinary least squares would also be possible) comprising at least one iteration and applying weights w.sub.i to the radar detection points, wherein the velocity profile equation of the target is represented by:

{dot over (r)}.sub.i,cmp=c.sub.t cos θ.sub.i+s.sub.t sin θ.sub.i.

The IRLS methodology is initialized, e.g., by the ordinary least squares (OLS) solution. This is done by first computing:

[00002]$\left[\begin{array}{c}{\tilde{c}}_t\\ {\tilde{s}}_t\end{array}\right]={\left[X^{T}X\right]}^{-1}{X}^T{\stackrel{.}{r}}_{\mathrm{cmp}},$

wherein {dot over (r)}.sub.cmp represents the vector of compensated range rates {dot over (r)}.sub.i for i=1, 2 . . . n. Using

{dot over ({circumflex over (r)})}.sub.i,cmp={tilde over (c)}.sub.t cos θ.sub.i+{tilde over (s)}.sub.t sin θ.sub.i.

an initial solution for {dot over ({circumflex over (r)})}.sub.i,cmp is computed. Then, the initial residual is

e.sub.{dot over (r)},i={dot over (r)}.sub.i,cmp−{dot over ({circumflex over (r)})}.sub.i,cmp

is computed.

[0083] The variance of the residual is then computed as:

[00003]${\hat{\sigma }}_{\stackrel{.}{r}}^2=\frac{{.Math.}_{i=1}^n{\left(e_{\stackrel{.}{r},i}\right)}^2}{n-2}.$

[0084] Next, an estimation of the variance of the estimations {tilde over (c)}.sub.t and {tilde over (s)}.sub.t is computed:

{circumflex over (σ)}.sub.VP.sup.2={circumflex over (σ)}.sub.{dot over (r)}.sup.2(X.sup.T X).sup.−1,

wherein

[00004]$X=\left[\begin{array}{cc}\cos {\theta }_1& \sin {\theta }_1\\ .Math.& .Math.\\ \cos {\theta }_n& \sin {\theta }_n\end{array}\right].$

[0085] With the initial solution, weights w.sub.i ∈[0; 1] can be computed in dependence of the residuals, wherein predefined thresholds may be used to ensure that the weights are well defined.

[0086] The weights w.sub.i are then arranged in a diagonal matrix W and the estimation of the coefficients of the first iteration is given as:

[00005]$\left[\begin{array}{c}{\tilde{c}}_t\\ {\tilde{s}}_t\end{array}\right]={\left[X^{T}WX\right]}^{-1}{X}^{T}W{\dot{r}}_{cmp}.$

[0087] Step 4: From the solution of the first iteration an estimation {dot over ({circumflex over (r)})}.sub.i,cmp of the velocity profile is determined represented by:

{dot over ({circumflex over (r)})}.sub.i,cmp={tilde over (c)}.sub.t cos θ.sub.i+{tilde over (s)}.sub.t sin θ.sub.i,

wherein the azimuth angle θ.sub.i is determined from step 1 and the estimation of the first and second coefficients {tilde over (c)}.sub.t and {tilde over (s)}.sub.t is determined from step 3 (initial solution). A new residual is computed as:

e.sub.{dot over (r)},i={dot over (r)}.sub.i,cmp−{dot over ({tilde over (r)})}.sub.i,cmp.

[0088] The variance of the new residual is then computed as:

[00006]${\hat{\sigma }}_r^2=\frac{{.Math.}_{i=1}^n{\left(\psi \left(e_{\stackrel{.}{r},i}\right)\right)}^2}{{\left({.Math.}_{i=1}^n{\psi \left(e_{\stackrel{.}{r},i}\right)}^{\prime }\right)}^2\left(n-2\right)}$$\mathrm{with}$$\psi \left(e_{\stackrel{.}{r},i}\right)=w_i{e}_{\stackrel{.}{r},i},$

wherein Ψ(e.sub.{dot over (r)},i)′ represents the first derivative of Ψ(e.sub.{dot over (r)},i) with respect to the residual e.sub.{dot over (r)},i, and wherein n represent the number of detection points.

[0089] Next, an estimation of the variance of the estimations {tilde over (c)}.sub.t and {tilde over (s)}.sub.t is computed as:

{circumflex over (σ)}.sub.VP.sup.2={circumflex over (σ)}.sub.{dot over (r)}.sup.2(X.sup.TX).sup.−1,

wherein the variance may be compared to a stop criterion (e.g., a threshold) in order to decide whether or not a further iteration is carried out for determining the estimated coefficients {tilde over (c)}.sub.t and {tilde over (s)}.sub.t. In this way, a final solution for the coefficients {tilde over (c)}.sub.t and {tilde over (s)}.sub.t can be obtained.

[0090] It can be shown that if the target 2 moves along a straight line (linear movement), the first and second estimated coefficients correspond to a portion of the velocity in the first and second spatial dimensions (i.e., x-direction and y-direction), respectively, this is:

V.sub.t,i.sup.x={tilde over (c)}.sub.t

V.sub.t,i.sup.y={tilde over (s)}.sub.t,

wherein V.sub.t,i.sup.x is the velocity component in the x-direction for the i-th detection point and V.sub.t,i.sup.y is the velocity component in the y-direction for the i-th detection point. In FIG. 5, these velocity components are indicated for one of the detection points 6, namely i=1, wherein V.sub.t,i.sup.x=u.sub.t,i and V.sub.t,i.sup.y=v.sub.t,i.

[0091] In case the target has a non-zero yaw rate, i.e. ω.sub.t is not zero, the velocity components with respect to the first and second spatial dimensions can be expressed as:

[00007]$\left[\begin{array}{c}{V}_{t,i}^x\\ V_{t,i}^y\end{array}\right]=\left[\begin{array}{c}\left(y_{t,\mathrm{COR}}-y_{t,i}\right){\omega }_t\\ \left(x_{t,i}-x_{t,\mathrm{COR}}\right){\omega }_t\end{array}\right],$

wherein x.sub.t,i is a first coordinate of the i-th detection point and y.sub.t,i is a second coordinate of the i-th detection point.

[0092] The range rate equation for each detection point can then be expressed as:

{dot over (r)}.sub.i,cmp=(y.sub.t,COR,−y.sub.t,i)ω.sub.t cos θ.sub.i+(x.sub.t,i −x.sub.t,COR)ω.sub.t sin θ.sub.i,

wherein this equation can be reduced to:

{dot over (r)}.sub.cmp=(y.sub.t,COR)ω.sub.t cos θ.sub.i+(−x.sub.t,COR)ω.sub.t sin θ.sub.i,

because of

y.sub.t,i cos θ.sub.i=r.sub.t,i sin θ.sub.i cos θ.sub.i=x.sub.t,i sin θ.sub.i.

[0093] Recall that the range rate equation is generally defined as:

[00008]${\stackrel{.}{r}}_{i,cmp}=\begin{array}{cc}[\cos {\theta }_i& \sin {\theta }_i]\end{array}\left[\begin{array}{c}{c}_t\\ s_t\end{array}\right].$

[0094] A comparison with the formulation of the range rate equation which includes the yaw rate shows that the estimated first and second coefficients can be expressed as

{tilde over (c)}.sub.t=(y.sub.t,COR)ω.sub.t

{tilde over (s)}.sub.t=(−x.sub.t,COR)ω.sub.t,

respectively. Therefore, the velocity of the i-th detection point can be expressed as:

V.sub.t,i.sup.x={tilde over (c)}.sub.t+(−y.sub.t,i)ω.sub.t

V.sub.t,i.sup.y={tilde over (s)}.sub.t+(x.sub.t,i)ω.sub.t.

[0095] The yaw rate is usually unknown but may be estimated. Taking into account such an estimation, the estimated velocity at the i-th detection point can be expressed as:

[00009]${\hat{V}}_{i,t}=\left[\begin{array}{c}{\hat{V}}_{t,i}^x\\ {\hat{V}}_{t,i}^y\end{array}\right]=\left[\begin{array}{c}{\tilde{c}}_t\\ {\tilde{s}}_t\end{array}\right]+\left[\begin{array}{c}-y_{t,i}\\ x_{t,i}\end{array}\right]\left[{\hat{\omega }}_t\right],$

wherein the velocity portion with respect to the yaw rate can be identified as:

[00010]${\hat{V}}_{i,\omega }=\left[\begin{array}{c}-y_{t,i}\\ x_{t,i}\end{array}\right]\left[{\hat{\omega }}_t\right],$

with a second coordinate −y.sub.t,i in the first spatial dimension x and a first coordinate x.sub.t,i in the second spatial dimension y, i.e. the first and second coordinates define the position of the i-th detection point as (x.sub.t,i, y.sub.t,i) with the second coordinate being inverted.

[0096] In a more compact notation the estimated velocity at the i-th detection point can be expressed as:

{circumflex over (V)}.sub.i,t=+{circumflex over (V)}.sub.i,ω,

wherein this estimated velocity can be set equal to the estimated compensated range rate of the velocity profile equation, as addressed further above.

[0097] The uncertainty estimate of the estimated velocity for the i-th detection point is preferably defined as:

.Math..sub.V.sub.i,t.sup.2=.Math..sub.VP.sup.2+.Math..sub.V.sub.i,ω.sup.2,

wherein .Math..sub.V.sub.i,t.sup.2 is a two-dimensional matrix, .Math..sub.VP.sup.2 is a two-dimensional matrix and a first portion of the uncertainty estimate .Math..sub.V.sub.i,t.sup.2, and .Math..sub.V.sub.i,ω.sup.2 is a two-dimensional matrix and a second portion of the uncertainty estimate .Math..sub.V.sub.i,t.sup.2 . The estimate as such, as well as the both portions are preferably squared figures, which avoid negative values for the estimate. The matrices also preferably represent dispersions with respect to the first and second spatial dimensions. Although the uncertainty estimate is defined here as a sum of the first and second portion other combinations of the first and second portion are possible in order to determine the uncertainty estimate.

[0098] The first portion .Math..sub.VP.sup.2 represents the uncertainty with respect to the first estimated coefficient {tilde over (c)}.sub.t and the second estimated coefficient {tilde over (s)}.sub.t of the velocity profile equation. Thus, the first portion can be interpreted to represent the uncertainty with respect to the velocity profile solution. This can be expressed as:

[00011]${\hat{U}}_{VP}^2=\left[\begin{array}{cc}{\hat{U}}_{c_t}^2& {\hat{U}}_{c_t{s}_t}^2\\ {\hat{U}}_{s_t{c}_t}^2& {\hat{U}}_{s_t}^2\end{array}\right],$

wherein .Math..sub.c.sub.t.sup.2 is the uncertainty estimate of the first estimated coefficient, .Math..sub.s.sub.t.sup.2 is the uncertainty estimate of the second estimated coefficient, and .Math..sub.c.sub.t.sub.s.sub.t.sup.2=.Math..sub.s.sub.t.sub.c.sub.t.sup.2 is the cross-uncertainty estimate between the first and second estimated coefficient. In this way, the first portion corresponds in general to a covariance matrix.

[0099] The first portion can be further defined as:

.Math..sub.VP.sup.2={circumflex over (σ)}.sub.VP.sup.2+,

with a covariance portion {circumflex over (σ)}.sub.VP.sup.2 representing a covariance matrix of the first estimated coefficient and the second estimated coefficient, and a bias portion representing a bias of the first estimated coefficient and the second estimated coefficient.
The covariance portion can be expressed as:

[00012]${\hat{\sigma }}_{VP}^2=\left[\begin{array}{cc}{\hat{\sigma }}_{c_t{c}_t}& {\hat{\sigma }}_{c_t{s}_t}\\ {\hat{\sigma }}_{s_t{c}_t}& {\hat{\sigma }}_{s_t{s}_t}\end{array}\right],$

wherein {circumflex over (σ)}.sub.c.sub.t.sub.c.sub.t is the variance estimate of the first estimated coefficient, {circumflex over (σ)}.sub.s.sub.t.sub.s.sub.t is the variance estimate of the second estimated coefficient, and {circumflex over (σ)}.sub.c.sub.t.sub.s.sub.t={circumflex over (σ)}.sub.s.sub.t.sub.c.sub.t is the covariance estimate of the first and second estimated coefficient.

[0100] As indicated before the covariance portion is preferable determined as:

{circumflex over (σ)}.sub.VP.sup.2={circumflex over (σ)}.sub.{dot over (r)}.sup.2(X.sup.tX).sup.−1.

[0101] Having regard to the bias portion, a general definition can be given as:

=ƒ(X, Y, k),

wherein Y represents the compensated range rate {dot over (r)}.sub.cmp and k represents some constants. Moreover, the matrix X is the same as before, namely:

[00013]$X=\left[\begin{array}{cc}\cos {\theta }_1& \sin {\theta }_1\\ .Math.& .Math.\\ \cos {\theta }_n& \sin {\theta }_n\end{array}\right].$

In particular, the bias portion can be defined as:

[00014]$=k_{\mathrm{ols\_bias}\mathrm{\_scale}}{\hat{\sigma }}_{VP}^2+B,$$\mathrm{with}$$B=\left[\begin{array}{cc}{k}_{\mathrm{c\_var}\mathrm{\_bias}}& 0\\ 0& k_{\mathrm{s\_var}\mathrm{\_bias}}\end{array}\right],$

wherein k.sub.ols_bias_scale is a scaling calibration parameter, k.sub.c_var_bias is an offset calibration parameter for the first estimated coefficient, and k.sub.s_var_bias is an offset calibration parameter for the second estimated coefficient.

[0102] It is noted that the bias portion is preferably a function of the covariance matrix {circumflex over (σ)}.sub.VP.sup.2 of the first and second estimated coefficient and additional calibration parameters.

[0103] Having regard to the second portion .Math..sub.V.sub.i,ω.sup.2 of the uncertainty estimate the second portion can be defined as:

[00015]${\hat{U}}_{V_{i,\omega }}^2=\left[\begin{array}{c}-y_{t,i}\\ x_{t,i}\end{array}\right]\left[{\hat{U}}_{\omega }^2\right][\begin{array}{cc}-y_{t,i}& x_{t,i}]\end{array},$

wherein .Math..sub.ω.sup.2 is the estimated uncertainty of the yaw rate.

[0104] In order to avoid a dynamic estimation of the uncertainty of the yaw rate, it is possible to rely on a predetermined uncertainty. This can be done under the assumption that yaw rate of objects is bounded. For example the yaw rate of typical traffic objects, e.g., vehicles usually may not exceed 30 degrees per second. It is then possible to model the yaw rate as a distribution (probability density function=pdf), for example a uniform distribution with zero mean and a maximum value ω.sub.t_max of the yaw rate ω.sub.t as:

[00016]$\mathrm{pdf}\left({\omega }_t\right)=\{\begin{array}{cc}\frac{1}{2{\omega }_{\mathrm{t\_max}}}& \mathrm{for}{\omega }_t\in \left[-{\omega }_{\mathrm{t\_max}},{\omega }_{\mathrm{t\_max}}\right]\\ 0& \mathrm{otherwise}\end{array}.$

The maximum value of the yaw rate (also called extremum) can be predetermined by a calibration parameter as:

ω.sub.t_max=k.sub.max_yaw_rate.

From standard mathematics the variance of the uniform pdf is:

[00017]${\sigma }_{{\omega }_t}^2=\frac{{\omega }_{\mathrm{t\_max}}^2}{3}.$

The uncertainty of the yaw rate can then be set to the variance, this is:

U.sub.ω.sub.t.sup.2=σ.sub.ω.sub.t.sup.2.

Therefore, the second portion of the uncertainty estimate is predetermined and expressed as:

[00018]$U_{V_{i,\omega }}^2=\left[\begin{array}{c}-y_{t,i}\\ x_{t,i}\end{array}\right]\left[U_{{\omega }_t}^2\right][\begin{array}{cc}-y_{t,i}& x_{t,i}],\end{array}$

wherein it is understood that the second portion represents the uncertainty of the estimated velocity with respect to an angular velocity, the first coordinate x.sub.t,i in the second spatial dimension y and the second coordinate y.sub.t,i in the first spatial dimension x.

[0105] FIG. 6 gives an overview of some aspects of the method described above. Each of the boxes corresponds to an exemplary step of the method, wherein the step is indicated inside the box. Dashed boxes indicate that the corresponding step is merely optional.

[0106] The proposed uncertainty estimate has been evaluated in two different example scenarios. In order to quantify the validity of the uncertainty estimate the Normalized Estimation Error Squared (NEES) is used as a metric. This metric can be generally interpreted as a measure of consistency between the estimated velocity and an estimated variance or uncertainty estimate. A general definition can be given as:

e.sub.i=({dot over (r)}.sub.i,cmp−{dot over ({circumflex over (r)})}.sub.i,cmp).sup.T{circumflex over (P)}.sub.i.sup.−1({dot over (r)}.sub.i,cmp−{dot over ({circumflex over (r)})}.sub.i,cmp),

with {circumflex over (P)}.sub.i.sup.−1 representing the inverse of an estimated covariance matrix and e.sub.i representing the NEES for the i-th detection point. The covariance matrix {circumflex over (P)}.sub.i is either the estimated covariance matrix {circumflex over (σ)}.sub.VP.sup.2 or the proposed uncertainty estimate .Math..sub.V.sub.i,t.sup.2.

[0107] The estimated velocity and the estimated covariance matrix are consistent if the expected value of NEES is equal to the dimension n of the covariance matrix (here n=2):

H.sub.0:E(e.sub.i)=n.

[0108] In both example scenarios a simulation of a moving object has been carried out with 1000 Monte Carlo iterations. In the first scenario, a straight moving object was simulated. When using the estimated covariance matrix {circumflex over (σ)}.sub.VP.sup.2 the expected NEES is E(e.sub.i)=3.01 which is inconsistent at the 95% significance level. When using the proposed uncertainty estimate E(e.sub.i)=2.03 which is consistent at the 95% significance level.

[0109] In the second scenario a yawing object was simulated. When using the estimated covariance matrix {circumflex over (σ)}.sub.VP.sup.2 E(e.sub.i)=1663.4 which is completely inconsistent. Using this as an “uncertainty estimate” would be dangerous for a tracking application. However, when using the proposed uncertainty estimate then E(e.sub.i)=2.04 which is well consistent at the 95% significance level. Therefore, the proposed uncertainty estimate can be used, e.g., for safe tracking applications.

LIST OF REFERENCE SIGNS

[0110] 1 origin of world coordinate system

[0111] 2 target vehicle

[0112] 3 front bumper

[0113] 3′ rear bumper

[0114] 3″ origin of vehicle coordinate system

[0115] 4 host vehicle

[0116] 5 origin of sensor coordinate system

[0117] 5′ radar system

[0118] 6, 6′ detection point

[0119] 7 center of rotation of the target