Method for robust estimation of the velocity of a target using a host vehicle
A method for estimating a velocity of a target using a host vehicle equipped with a radar system includes determining a plurality of radar detection points, determining a compensated range rate, and determining an estimation of a first component of a velocity profile equation of the target and an estimation of a second component of the velocity profile equation of the target by using an iterative methodology comprising at least one iteration. The estimations and of the first and second components and of the velocity profile equation are not determined from a further iteration if at least one statistical measure representing the deviation of an estimated dispersion of the estimations and of the first and second components, and of a current iteration from a previous iteration and/or the deviation of an estimated dispersion of the residual from a predefined dispersion of the range rate meets a threshold condition.
1. A method comprising:
estimating a velocity of a target in a horizontal plane using a host vehicle equipped with a radar system, the radar system including a radar sensor unit adapted to receive signals emitted from the host vehicle and reflected by the target in one measurement time instance, the estimating of the velocity of the target in a horizontal plane comprising:
a) emitting a radar signal and determining, from a plurality of radar detection measurements captured by the radar sensor unit over a plurality of ranges and a plurality of azimuth angles, a plurality of radar detection points, each radar detection point comprising an azimuth angle θ i and a range rate {dot over (r)} i , wherein the range rate {dot over (r)} i represents the rate of change of the distance between the radar sensor unit and the target;
b) determining a compensated range rate {dot over (r)} i,cmp represented by:
{dot over (r)} i,cmp ={dot over (r)} i +u s cos θ i +v s sin θ i ,
wherein u s represents a longitudinal velocity component of the radar sensor unit and wherein v s represents a lateral velocity component of the radar sensor unit;
c) determining, from the results of step a) and b), an estimation {tilde over (c)} t of a first component c t of a velocity profile equation of the target and an estimation {tilde over (s)} t of a second component s t of the velocity profile equation by using an iteratively reweighted least squares methodology comprising multiple iterations and applying weights w i to the radar detection points, wherein the velocity profile equation is represented by:
{dot over (r)} i,cmp =c t cos θ i +s t sin θ i ;
d) determining an estimation {dot over ({circumflex over (r)})} i,cmp of the velocity profile equation represented by:
{dot over ( {circumflex over (r)} )} i,cmp ={tilde over (c)} t cos θ i +{tilde over (s)} t sin θ i ,
wherein the azimuth angle θ i is determined from step a) and the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of the velocity profile equation are determined from step c);
e) determining a residual e {dot over (r)},i of the estimation {dot over ({circumflex over (r)})} i,cmp of the velocity profile equation determined from step d) and the compensated range rate {dot over (r)} i,cmp determined from step b), wherein the residual e {dot over (r)},i is represented by a difference of the compensated range rate {dot over (r)} i,cmp and the estimation {dot over ({circumflex over (r)})} i,cmp of the velocity profile equation, and further determining the weights w i with respect to the residual e {dot over (r)},i ;
f) determining an estimation of the velocity of the target on the basis of the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of the velocity profile equation determined from step c); and
wherein, in step c), the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of the velocity profile equation are not determined from at least one iteration of the multiple iterations of the iteratively reweighted least squares methodology if at least one statistical measure representing:
a deviation of an estimated dispersion of the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of a current iteration from a previous iteration; or
a deviation of an estimated dispersion of the residual e {dot over (r)},i from a predefined dispersion of the range rate {dot over (r)} i ;
meets a first threshold condition;
receiving, by a control unit of the host vehicle, the estimation of the velocity of the target; and:
in response to the estimation of the velocity of the target:
controlling, by the control unit and based on the estimation of the velocity of the target, the host vehicle; or
outputting, by the control unit and based on the estimation of the velocity of the target meeting a second threshold condition, a warning signal.
2. The method according to claim 1 , wherein, in step c), the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of the velocity profile equation are also not determined from at least one iteration of the multiple iterations of the iteratively reweighted least squares methodology if at least one statistical measure representing a linear dependency of the radar detection points meets a third threshold condition.
3. The method according to claim 2 , wherein the statistical measure representing the linear dependency of the radar detection points is based on a determinant of
X
T
WX
with
X
=
[
cos
θ
1
sin
θ
1
⋮
⋮
cos
θ
n
sin
θ
n
]
and W representing the weights w i arranged in a diagonal matrix.
4. The method according to claim 1 , wherein, in step c), the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of the velocity profile equation are also not determined from at least one iteration of the multiple iterations of the iteratively reweighted least squares methodology if at least one statistical measure representing the weights w i applied to the radar detection points meets a third threshold condition.
5. The method according to claim 4 ,
wherein the statistical measure representing the weights w i is based on a mean of the weights w i .
6. The method according to claim 1 , wherein the estimated dispersion of the residual e {dot over (r)},i is represented by a square root of:
σ
^
r
.
2
=
∑
i
=
1
n
(
ψ
(
e
r
.
,
i
)
)
2
(
∑
i
=
1
n
ψ
(
e
r
.
,
i
)
′
)
2
(
n
-
2
)
with
ψ
(
e
r
.
,
i
)
=
w
i
e
r
.
,
i
,
wherein ψ(e {dot over (r)},i )′ represents a first derivative of ψ(e {dot over (r)},i ) with respect to the residual e {dot over (r)},i , and wherein n represent a number of radar detection points.
7. The method according to claim 1 , wherein the estimated dispersion of the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of the velocity profile equation is represented by a sum of the estimated dispersions of the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of the velocity profile equation.
8. The method according to claim 1 , wherein the estimated dispersion of the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of the velocity profile equation is represented by a trace evaluated on the basis of:
σ
^
VP
2
=
σ
^
r
.
2
(
X
T
WX
)
-
1
=
[
σ
^
cc
2
σ
^
cs
2
σ
^
sc
2
σ
^
ss
2
]
with
X
=
[
cos
θ
1
sin
θ
1
⋮
⋮
cos
θ
n
sin
θ
n
]
W representing the weights w i arranged in a diagonal matrix,
and {circumflex over (σ)} cc 2 , {circumflex over (σ)} ss 2 , {circumflex over (σ)} cs 2 , {circumflex over (σ)} sc 2 representing estimated dispersion coefficients,
wherein {circumflex over (σ)} {dot over (r)} 2 represents a square of the estimated dispersion of the residual e {dot over (r)},i .
9. The method according to claim 1 , wherein the predefined dispersion of the range rate {dot over (r)} i is given by a specification of the radar sensor unit.
10. A system comprising:
a radar sensor unit configured to estimate a velocity of a target in a horizontal plane using a host vehicle equipped with the system, the radar sensor unit adapted to receive signals emitted from the host vehicle and reflected by the target in one measurement time instance, in estimating the velocity of the target in the horizontal plane, the radar sensor unit further configured to:
emit a radar signal and determine, from a plurality of radar detection measurements captured by the radar sensor unit over a plurality of ranges and a plurality of azimuth angles, a plurality of radar detection points, each radar detection point comprising an azimuth angle θ i and a range rate {dot over (r)} i , wherein the range rate {dot over (r)} i represents the rate of change of the distance between the radar sensor unit and the target;
determine a compensated range rate {dot over (r)} i,cmp represented by:
{dot over (r)} i,cmp ={dot over (r)} i +u s cos θ i +v s sin θ i
wherein u s represents a longitudinal velocity component of the radar sensor unit and wherein v s represents a lateral velocity component of the radar sensor unit;
determine, an estimation {tilde over (c)} t of a first component c t of a velocity profile equation of the target and an estimation {tilde over (s)} t of a second component s t of the velocity profile equation by using an iteratively reweighted least squares methodology comprising multiple iterations and apply weights w i to the radar detection points, wherein the velocity profile equation is represented by:
{dot over (r)} i,cmp =c t cos θ i +s t sin θ i ;
determine an estimation {dot over (r)} i,cmp of the velocity profile equation represented by:
{dot over ( {circumflex over (r)} )} i,cmp ={tilde over (c)} t cos θ i +{tilde over (s)} t sin θ i ,
determine a residual e {dot over (r)},i of the estimation {dot over (r)} i,cmp of the velocity profile equation and the compensated range rate {dot over (r)} i,cmp , wherein the residual e {dot over (r)},i is represented by a difference of the compensated range rate {dot over (r)} i,cmp and the estimation {dot over ({circumflex over (r)})} i,cmp of the velocity profile equation, and further determining the weights w i with respect to the residual e {dot over (r)},i ;
determine an estimation of the velocity of the target on the basis of the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of the velocity profile equation; and wherein the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of the velocity profile equation are not determined from at least one iteration of the multiple iterations of the iteratively reweighted least squares methodology if at least one statistical measure representing:
a deviation of an estimated dispersion of the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of a current iteration from a previous iteration; or
a deviation of an estimated dispersion of the residual e {dot over (r)},i from a predefined dispersion of the range rate {dot over (r)} i ;
meets a threshold condition; and
a control unit configured to receive the estimation of the velocity of the target and, in response to the estimation of the velocity of the target:
control, based on the estimation of the velocity of the target, the host vehicle; or
output, based on the estimation of the velocity of the target meeting a second threshold condition, a warning signal.
11. The system according to claim 10 , wherein the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of the velocity profile equation are also not determined from at least one iteration of the multiple iterations of the iteratively reweighted least squares methodology if at least one statistical measure representing a linear dependency of the radar detection points meets a third threshold condition.
12. The system according to claim 11 , wherein the statistical measure representing the linear dependency of the radar detection points is based on a determinant of
X
T
WX
with
X
=
[
cos
θ
1
sin
θ
1
⋮
⋮
cos
θ
n
sin
θ
n
]
and W representing the weights w i arranged in a diagonal matrix.
13. The system according to claim 10 , wherein the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of the velocity profile equation are also not determined from at least one iteration of the multiple iterations of the iteratively reweighted least squares methodology if at least one statistical measure representing the weights w i applied to the radar detection points meets a third threshold condition.
14. The system according to claim 13 , wherein the statistical measure representing the weights w i is based on a mean of the weights w i .
15. The system according to claim 10 , wherein the estimated dispersion of the residual e {dot over (r)},i is represented by a square root of:
σ
^
r
.
2
=
∑
i
=
1
n
(
ψ
(
e
r
.
,
i
)
)
2
(
∑
i
=
1
n
ψ
(
e
r
.
,
i
)
′
)
2
(
n
-
2
)
with
ψ
(
e
r
.
,
i
)
=
w
i
e
r
.
,
i
wherein ψ(e {dot over (r)},i )′ represents the first derivative of ψ(e {dot over (r)},i ) with respect to the residual e {dot over (r)},i , and wherein n represent a number of radar detection points.
16. The system according to claim 10 , wherein the estimated dispersion of the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of the velocity profile equation is represented by a sum of the estimated dispersions of the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of the velocity profile equation.
17. The system according to claim 10 , wherein the estimated dispersion of the estimations {tilde over (c)} t and {tilde over (s)} t of the first and second components c t and s t of the velocity profile equation is represented by a trace evaluated on the basis of:
σ
^
VP
2
=
σ
^
r
.
2
(
X
T
WX
)
-
1
=
[
σ
^
cc
2
σ
^
cs
2
σ
^
sc
2
σ
^
ss
2
]
with
X
=
[
cos
θ
1
sin
θ
1
⋮
⋮
cos
θ
n
sin
θ
n
]
W representing the weights w i arranged in a diagonal matrix, and {circumflex over (σ)} cc 2 , {circumflex over (σ)} ss 2 , {circumflex over (σ)} cs 2 , {circumflex over (σ)} sc 2 representing estimated dispersion coefficients, wherein {circumflex over (σ)} {dot over (r)} 2 represents a square of the estimated dispersion of the residual e {dot over (r)},i .
18. The system according to claim 10 , wherein the predefined dispersion of the range rate {dot over (r)} i is given by a specification of the radar sensor unit.