System and method of target tracking using sensor fusion
View Patent ↗A target tracking and sensory fusion system is adapted for use with a vehicle, and configured to observe a condition of at least one object during a cycle. The system includes a plurality of sensors, and a novel controller communicatively coupled to the sensors and configured to more accurately estimate the condition based on sensory fusion. In a preferred embodiment, Kalman filtering is utilized to produce a fused estimate of the object location. The preferred controller is further configured to match each new sensory observation with a track in a track list, and remove the track from the track list, when a matching observation is not determined, during a subsequent cycle.
1. A computer program for execution by at least one electronic device associated with a plurality of sensors, wherein each of said sensors are configured to estimate at least one condition of at least one object, said program is configured to receive initial estimate data of said at least one condition from the sensors and apply a sensory fusion algorithm to the initial estimate data, so as to determine a state estimate for said at least one condition, said state estimate presents a higher probability and smaller standard of deviation than the initial estimate data, initial and state estimates are determined for a plurality of conditions, said state estimates are stored in a track (y k (t)), the plurality of conditions include at least one rate condition, each of said tracks is dynamically modeled at a time increment (t+1) by applying to y k (t) a vector multiplier (F) that assumes a constant rate condition, and adding a white Guassian noise vector (v k ), initial and state estimates are determined for a plurality of conditions including object range (r), range rate ({dot over (r)}), azimuth angle (θ) and azimuth angle rate ({dot over (θ)}), and the modeled track (y k (t+1)) is determined according to the formula:
y k ( t+ 1)= Fy k ( t )+ v k , where
F
=
[
1
Δ
t
1
1
Δ
t
1
]
.
2. A computer program for execution by at least one electronic device associated with a plurality of sensors, wherein each of said sensors are configured to estimate at least one condition of at least one object, said program is configured to receive initial estimate data of said at least one condition from the sensors and apply a sensory fusion algorithm to the initial estimate data, so as to determine a state estimate for said at least one condition, said state estimate presents a higher probability and smaller standard of deviation than the initial estimate data, initial and state estimates are determined for a plurality of conditions, said state estimates are stored in a track (y k (t)), state estimates are determined for at least one new object, and compared to y k (t) to determine a difference parameter for each of said conditions, each of said difference parameters being passed through a function based on the characteristics of the sensor, further multiplied by a constant coefficient based on the robustness of individual sensor measurements, and then combined to determine a merit value (L k,i ), and said state estimates of said at least one new object is assigned to y k (t) where L k,i is not less than a pre-determined threshold.
3. The program as claimed in claim 2 ,
said function being a fuzzy logic membership function.
4. The program as claimed in claim 2 ,
said function being a symmetric kernel function.
5. The program as claimed in claim 4 ,
said function being
f
R
(
Δ
R
)
=
K
(
Δ
R
h
R
)
,
where h R denotes a scaling factor based on the characteristics of the sensor.
6. The program as claimed in claim 5 ,
said function being a Gaussian Kernel function.
7. The program as claimed in claim 6 ,
said function being:
K ( u )=exp(− u 2 ).
8. The program as claimed in claim 5 ,
said function being a quadratic Kernel function.
9. The program as claimed in claim 8 ,
said function being
K
(
u
)
=
{
(
1
-
d
2
)
if
d
<
1
,
0
otherwise
.
10. The program as claimed in claim 2 , wherein a plurality of observations (O i (t)) are assigned to y k (t), and each of said observations are defined by the formula:
O i ( t )= Gy ( t )+ w i ,
where
G
=
[
1
1
1
0
]
,
w i is a 3-by-1 white Gaussian noise vector derived in part from the accuracy of the sensor system, and the observations are combined to determine a true track status.
11. The program as claimed in claim 10 , wherein the observations are combined using Kalman filtering to determine the true track status.
12. The program as claimed in claim 11 , wherein the true track status is determined in accordance with the formula(s):
y
(
t
+
1
)
=
y
^
(
t
+
1
|
t
)
+
∑
i
=
1
s
K
i
(
O
i
-
G
y
^
(
t
+
1
|
t
)
)
where
y
^
(
t
+
1
|
t
)
=
Fy
(
t
)
.
13. The program as claimed in claim 12 , wherein the true track status is further determined in accordance with the covariance formula(s):
P
(
t
+
1
)
=
(
I
-
∑
i
=
1
s
K
i
G
)
P
^
(
t
+
1
|
t
)
,
where
P
^
(
t
+
1
|
t
)
=
FP
(
t
)
F
T
+
Q
.
14. The program as claimed in claim 12 , wherein the KF gain for the i-th observer (K i ) is determined in accordance with the following formula:
K i ={circumflex over (P)} ( t+ 1 |t ) G T ( R i +G{circumflex over (P)} ( t+ 1 |t ) G T ) −1 .
15. The program as claimed in claim 2 ,
each of said tracks y k (t) being assigned a track strength (q k (t)), wherein q k (t)=1 upon the assignment of an observation to the track, and decays at a rate (λ) over a subsequent period, if no observations are assigned to the track over the period, and the track y k (t) is removed from Y(t), if q k (t) falls below a pre-determined minimum threshold.
16. The program as claimed in claim 15 , wherein a first portion of the tracks decays at a different rate than a second portion of the tracks.