tracking a maneuvering object in a noisy environment using immpdaf by: igor tolchinsky alexander...
Post on 21-Dec-2015
216 views
TRANSCRIPT
Tracking a Tracking a maneuvering object in maneuvering object in a noisy environment a noisy environment
using IMMPDAFusing IMMPDAF By:By:
Igor TolchinskyIgor TolchinskyAlexander LevinAlexander Levin
Supervisor:Supervisor:Daniel SigalovDaniel Sigalov
Spring 2006Spring 2006
Project goalsProject goals Learn about ATC problems and the Learn about ATC problems and the
algorithms involved.algorithms involved. Implement the simulation track and the Implement the simulation track and the
tracking algorithms IMM and PDAF in tracking algorithms IMM and PDAF in MATLAB.MATLAB.
Test algorithms performance.Test algorithms performance.
Problem DefinitionProblem Definition Estimate target state (position, Estimate target state (position,
velocity and acceleration) in 2-velocity and acceleration) in 2-dimensional Cartesian coordinates.dimensional Cartesian coordinates.
The estimation overcomes the next The estimation overcomes the next problems:problems:
-Discrete sampling.-Discrete sampling. -Noisy sampling, noisy environment. -Noisy sampling, noisy environment. -Detection probability lower then 1.-Detection probability lower then 1.
Solution approachSolution approach KF - Kalman Filter: a linear filter that KF - Kalman Filter: a linear filter that
estimates the state vector.estimates the state vector. IMM - Interacting Multiple Model: uses a KF IMM - Interacting Multiple Model: uses a KF
for each one of the models.for each one of the models. PDAF - Probabilistic Data Association Filter: PDAF - Probabilistic Data Association Filter:
statistical approach that deals with false statistical approach that deals with false alarms. alarms.
IMMPDAF - Combining both algorithms in IMMPDAF - Combining both algorithms in order to estimate the state vector of order to estimate the state vector of maneuvering object in noisy environment.maneuvering object in noisy environment.
Kalman FilterKalman Filter KF estimates the state of a discrete KF estimates the state of a discrete
time controlled process that is time controlled process that is governed by the linear difference governed by the linear difference equation:equation:
KF uses recursive solution:KF uses recursive solution:
1 1 1n
k k k k
mk k k
x Ax Bu w x R state vector
z Hx v z R measurement state
1 1
1
ˆ ˆk k k
Tk k
x Ax BuTime update equations
P AP A Q
1( )
ˆ ˆ ˆ( )
( )
T Tk k k
k k k k k
k k k
K P H HP H R
x x K z Hx Measurement update equations
P I K H P
IMM - Interacting Multiple IMM - Interacting Multiple ModelModel
Defining the number and the kind of Defining the number and the kind of models for the algorithm (each models for the algorithm (each model is Kalman-Filter based).model is Kalman-Filter based).
Calculating the probability for each Calculating the probability for each model, by using a-priori probabilities model, by using a-priori probabilities and transition matrix.and transition matrix.
Combining the estimations of each Combining the estimations of each model in order to estimate the state model in order to estimate the state vector.vector.
IMM treats maneuvering problems by:IMM treats maneuvering problems by:
( 1| 1), ( 1| 1) 1,2...ˆi ix k k P k k i r
Interaction/Mixing ( 1| 1)k k
0 0( 1| 1), ( 1| 1) 1,2...ˆ i ix k k P k k i r
Filter M(i)( )z k ( ) 1,2...i k i r
ˆ ( | ), ( | ) 1,2...i ix k k P k k i r
Mode probability update and mixing
probability calulation
State estimate and covariance
combination( )i k
( | )k k( )k ( )k
ˆ ( | ), ( | )i ix k k P k k ˆ( | )x k k
( | )P k k
IMM – one step of the IMM – one step of the algorithmalgorithm
PDAF – Probabilistic Data PDAF – Probabilistic Data Association FilterAssociation Filter
Determining an ellipsoidal validation Determining an ellipsoidal validation region around the estimated value.region around the estimated value.
Throwing out all the measurements that Throwing out all the measurements that are outside of this region.are outside of this region.
Weighting the inside measurements Weighting the inside measurements according to their distance from the center according to their distance from the center of the region.of the region.
Estimating the state by mixing all the Estimating the state by mixing all the different weights.different weights.
PDAF treats noisy environment problems by:PDAF treats noisy environment problems by:
State estimateˆ( 1| 1)x k k
State covariance( 1| 1)P k k
Predicted stateˆ( | 1)x k k
Covariance of predicted state( | 1)P k k
Predicted measurementˆ( | 1)z k k
Innovation covariance
( )S k
Calculation of innovations and measurement
validation( ) 1,2... ( )iz k i m k
Filter gain calculation
( )W k
Effect of measurement origin on state
covariance( )P k%
Evolution of association
probabilities( )i k
Measurements
Combined innovation
( )v k
Updated state covariance( | )P k k
Updated state estimateˆ( | )x k k
PDAF – one step PDAF – one step of the algorithmof the algorithm
IMMPDAFIMMPDAF For each model the IMM uses a KF For each model the IMM uses a KF
and upon it’s estimation builds the and upon it’s estimation builds the validation region for the PDAF.validation region for the PDAF.
IMMPDAF treats the false alarms for IMMPDAF treats the false alarms for each model separately, and later on each model separately, and later on combines them in order to get the combines them in order to get the final state estimation.final state estimation.
Maneuvering IndexManeuvering Index The definition of the maneuvering index is The definition of the maneuvering index is
T is the sampling period and are T is the sampling period and are sampling noise and process noise sampling noise and process noise respectfully.respectfully.
As we’ll see in the next slides the value of As we’ll see in the next slides the value of the maneuvering index has a great influence the maneuvering index has a great influence on algorithm’s performance. Moreover, on algorithm’s performance. Moreover, smart definition of the model covariance can smart definition of the model covariance can optimize drastically the performance.optimize drastically the performance.
2v
w
T
,v w
Track initialization Track initialization 200 seconds of constant speed200 seconds of constant speed 20 seconds of constant acceleration20 seconds of constant acceleration 50 seconds of circular motion left with 50 seconds of circular motion left with
constant acceleration of 9g. constant acceleration of 9g.
,500 100x ym m
s sV V
2 2,10 10x y
m m
s sA A
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
x 105
-2.5
-2
-1.5
-1
-0.5
0x 10
4
X(meters)
Y(m
eter
s)
combine movement(2D)
Typical graphs presentation for Typical graphs presentation for one runone run
We used 3 types of models: Constant acceleration, constant We used 3 types of models: Constant acceleration, constant
turn left and constant turn rightturn left and constant turn right
0 50 100 150 200 250 300 350 4000
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
time(sec)
Pro
babi
lity
the pdf for the models for the whole motion
PD=1PG=1V=0 process=30m/sec2
R=100m
(MI)=7.5
CA
right turn unknown turn rate
left turn unknown turn rate
0 50 100 150 200 250 300 350 4000
100
200
300
400
500
600
700
800
900
1000
time(sec)
Err
or(S
qare
err
or)
in m
eter
s
the error vs the time
Error estimator
noise
Typical graph presentation for Typical graph presentation for a 50 runs Monte-Carloa 50 runs Monte-Carlo
0 50 100 150 200 250 300 350 4000
100
200
300
400
500
600
700
time(sec)
RM
S-E
rror(
Sqare
err
or)
in m
ete
rs
Monte-Carlo 50runs percentage of successful runs=100%
PD=1PG=1V=50Max Error=673.3233mManeuvering RMS=182.5281mNon-Maneuvering RMS=252.5624m process=10m/sec2
R=100m
(MI)=2.5
Error estimator
average Error is 196.1459[m]
KF vs IMM comparing to Bar-KF vs IMM comparing to Bar-Shalom and Kirubarajan articleShalom and Kirubarajan article
Goals:Goals: Comparing the performances of a single KF for Comparing the performances of a single KF for
constant velocity vs 2 models IMM. Both models constant velocity vs 2 models IMM. Both models are modeling constant velocity. are modeling constant velocity. The maneuvering index of the first The maneuvering index of the first model is constant and the maneuvering index of model is constant and the maneuvering index of the second one is changing accordingly to the the second one is changing accordingly to the maneuver.maneuver.
Reconstructing the results of the article and Reconstructing the results of the article and extend the results for noisy environment (FA). extend the results for noisy environment (FA).
KF vs IMM Simulation ResultsKF vs IMM Simulation Results We used the same track as in the article (2 minutes of We used the same track as in the article (2 minutes of
const velocity with a minute of const acceleration between).const velocity with a minute of const acceleration between). We got the same results as in the articleWe got the same results as in the article
for a noiseless 50 run Monte-Carlo.for a noiseless 50 run Monte-Carlo.
0.5 1 1.5 2 2.5
x 104
1
1.05
1.1
1.15
1.2
1.25x 10
4
X(meters)
Y(m
eter
s)
combine movement(2D)
0 0.5 1 1.5 2 2.5100
200
300
400
Pea
k po
sitio
n er
ror(
m)
comparison to the article:Kalman Filter Versus IMM EstimatorMonte-Carlo :50 runs V=0
IMM
KF
0 0.5 1 1.5 2 2.550
100
150
200
Ove
rall
RM
S p
ositi
on e
rror
(m)
comparison to the article:Kalman Filter Versus IMM EstimatorMonte-Carlo :50 runs V=0
IMM
KF
0 20 40 60 80 100 120 140 160 180
0.35
0.4
0.45
0.5
0.55
0.6
0.65
0.7
time(sec)
Pro
babi
lity
the pdf for the models for the whole motion
=0.2
adaptive var
ConclusionsConclusions As expected, the performance of the As expected, the performance of the
IMM increases with the increase of IMM increases with the increase of the maneuvering index.the maneuvering index.
The intersection point is about The intersection point is about as in the article. as in the article.
0.5
Article extension – noisy Article extension – noisy environmentenvironment The number of false alarms is an independent The number of false alarms is an independent
poisson process with a known .poisson process with a known . The number of errors in this example, was 10 per The number of errors in this example, was 10 per
10 square kilometers.10 square kilometers.
0 0.5 1 1.5 2 2.5150
200
250
300
Pea
k po
sitio
n er
ror(
m)
comparison to the article:Kalman Filter Versus IMM EstimatorMonte-Carlo :50 runs V=10
IMM
KF
0 0.5 1 1.5 2 2.5100
120
140
160
180
Ove
rall
RM
S p
ositi
on e
rror
(m)
comparison to the article:Kalman Filter Versus IMM EstimatorMonte-Carlo :50 runs V=10
IMM
KF
0 0.5 1 1.5 2 2.50
20
40
60
80
Pea
k sp
eed
erro
r(m
/s)
comparison to the article:Kalman Filter Versus IMM EstimatorMonte-Carlo :50 runs V=10
IMM
KF
0 0.5 1 1.5 2 2.50
10
20
30
Ove
rall
RM
S s
peed
err
or(m
/s)
comparison to the article:Kalman Filter Versus IMM EstimatorMonte-Carlo :50 runs V=10
IMM
KF
ConclusionsConclusions In a noisy environment the difference between the In a noisy environment the difference between the
performances lowers due to the fact that the increase of performances lowers due to the fact that the increase of the maneuvering index causes wrong estimation towards the maneuvering index causes wrong estimation towards false alarms. That’s why the intersection point shifted false alarms. That’s why the intersection point shifted rightwards, .rightwards, .
The percentage of lost tracks of the IMM is always lower The percentage of lost tracks of the IMM is always lower then the one of KF, because of the maneuvering part of the then the one of KF, because of the maneuvering part of the track:track:
0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 2.2 2.470
75
80
85
90
95
100
105
110
Per
cent
age
of s
uces
sful
run
s
percentage of successful tracking comparison to the article:Kalman Filter Versus IMM EstimatorMonte-Carlo :50 runs V=20
IMM
KF
0.8
Maneuvering index influence in Maneuvering index influence in noiseless environment noiseless environment
We used our values and got an index of about 20 in the We used our values and got an index of about 20 in the most maneuvering part (turning with constant acceleration most maneuvering part (turning with constant acceleration of 9g) of 9g)
We kept constant of 0.5 for the turning model and We kept constant of 0.5 for the turning model and changed it only for the acceleration model.changed it only for the acceleration model.
0 5 10 15 20 25200
300
400
500
Pea
k po
sitio
n er
ror(
m)
Peak Error vs lamda Monte-Carlo :5 runs best is=25PD=1 PG=1 V=0
R=100m
0 5 10 15 20 25100
150
200
250
300
Pea
k sp
eed
erro
r(m
/s)
0 5 10 15 20 2555
60
65
70
75
Ove
rall
RM
S p
ositi
on e
rror
(m)
checking for the best variance Monte-Carlo :5 runs best is=21PD=1 PG=1 V=0
R=100m
0 5 10 15 20 2522
24
26
28
30
32
Ove
rall
RM
S s
peed
err
or(m
/s)
v
500 , 100wT s m
ConclusionsConclusions At least one of the models should be with a At least one of the models should be with a
low maneuvering index and one with a high low maneuvering index and one with a high one.one.
The increase of the maneuvering index The increase of the maneuvering index improves the performance of the algorithm improves the performance of the algorithm both in maximum errors and average errors.both in maximum errors and average errors.
The algorithm performs optimally when the The algorithm performs optimally when the maneuvering index reaches it’s maximum maneuvering index reaches it’s maximum value. Furthermore increase doesn’t improve value. Furthermore increase doesn’t improve it’s performance. it’s performance.
Noisy environment influenceNoisy environment influence We compared the results of Monte-Carlo 50 run We compared the results of Monte-Carlo 50 run
simulation between a 5 false alarms per 10 simulation between a 5 false alarms per 10 squared kilometer (in average), 50 false alarms squared kilometer (in average), 50 false alarms and 200 false alarms:and 200 false alarms:
0 50 100 150 200 250 300 350 4000
50
100
150
200
250
300
350
400
time(sec)
RM
S-E
rror(
Sqare
err
or)
in m
ete
rs
Monte-Carlo 50runs percentage of successful runs=100%
PD=1PG=1V=5Max Error=361.0659mManeuvering RMS=68.7315mNon-Maneuvering RMS=153.608m process=10m/sec2
R=100m
(MI)=2.5
Error estimator
average Error is 85.2353[m]
0 50 100 150 200 250 300 350 4000
100
200
300
400
500
600
700
time(sec)
RM
S-E
rror(
Sqare
err
or)
in m
ete
rs
Monte-Carlo 50runs percentage of successful runs=100%
PD=1PG=1V=50Max Error=673.3233mManeuvering RMS=182.5281mNon-Maneuvering RMS=252.5624m process=10m/sec2
R=100m
(MI)=2.5
Error estimator
average Error is 196.1459[m]
0 50 100 150 200 250 300 350 4000
200
400
600
800
1000
1200
time(sec)
RM
S-E
rror
(Sqa
re e
rror
) in
met
ers
Monte-Carlo 50runs percentage of successful runs=52%
PD=1PG=1V=200Max Error=1150.1139mManeuvering RMS=323.0527mNon-Maneuvering RMS=731.3376m process=10m/sec2
R=100m
(MI)=2.5
Error estimator
average Error is 402.4414[m]
ConclusionsConclusions The increase in false alarms lowers the The increase in false alarms lowers the
performance of the algorithm in all the performance of the algorithm in all the parameters as expected:parameters as expected:
Optimizations for noisy Optimizations for noisy environmentsenvironments
Our goal in this part was finding the Our goal in this part was finding the optimal maneuvering index for noisy optimal maneuvering index for noisy environment.environment.
We used .We used . We checked algorithm performance for the We checked algorithm performance for the
optimal value of the maneuvering optimal value of the maneuvering index,20, and for lower indexes, 7.5 and index,20, and for lower indexes, 7.5 and 2.5 .2.5 .
50v
0 50 100 150 200 250 300 350 4000
100
200
300
400
500
600
700
800
time(sec)
RM
S-E
rror
(Sqa
re e
rror
) in
met
ers
Monte-Carlo 50runs percentage of successful runs=80%
PD=1PG=1V=50Max Error=745.9781mManeuvering RMS=175.9084mNon-Maneuvering RMS=292.5042m process=80m/sec2
R
=100m
(MI)=20
Error estimator
average Error is 198.5798[m]
0 50 100 150 200 250 300 350 4000
100
200
300
400
500
600
time(sec)
RM
S-E
rror
(Sqa
re e
rror
) in
met
ers
Monte-Carlo 50runs percentage of successful runs=88%
PD=1PG=1V=50Max Error=592.6101mManeuvering RMS=165.7729mNon-Maneuvering RMS=285.3251m process=30m/sec2
R=100m
(MI)=7.5
Error estimator
average Error is 189.0192[m]
0 50 100 150 200 250 300 350 4000
100
200
300
400
500
600
700
800
time(sec)
RM
S-E
rror
(Sqa
re e
rror
) in
met
ers
Monte-Carlo 50runs percentage of successful runs=80%
PD=1PG=1V=50Max Error=730.9782mManeuvering RMS=175.3241mNon-Maneuvering RMS=292.0596m process=10m/sec2
R=100m
(MI)=2.5
Error estimator
average Error is 198.0227[m]
ConclusionsConclusions The optimal value of the The optimal value of the
maneuvering index in noisy maneuvering index in noisy environment is much lower then in environment is much lower then in noiseless one, due to the fact that noiseless one, due to the fact that high index causes estimation high index causes estimation towards false alarms.towards false alarms.
The last conclusions leads us to The last conclusions leads us to choose the maneuvering index of the choose the maneuvering index of the different models not only according different models not only according to the equation but also in response to the equation but also in response to the pdf of the false alarms.to the pdf of the false alarms.
Pd<1 influencePd<1 influence We changed Pd to 0.9.We changed Pd to 0.9. We worked with a noisy environment (50 We worked with a noisy environment (50
errors for 10 squared kilometers).errors for 10 squared kilometers). We choose our maneuvering index as the We choose our maneuvering index as the
optimal for this noise rate.optimal for this noise rate.
0 50 100 150 200 250 300 350 4000
200
400
600
800
1000
1200
time(sec)
RM
S-E
rror
(Sqa
re e
rror
) in
met
ers
Monte-Carlo 50runs percentage of successful runs=34%
PD=0.9PG=1V=50Max Error=1165.44mManeuvering RMS=640.2421mNon-Maneuvering RMS=530.2117m process=30m/sec2
R=100m
(MI)=7.5
Error estimator
average Error is 551.6065[m]
0 50 100 150 200 250 300 350 4000
100
200
300
400
500
600
time(sec)
RM
S-E
rror
(Sqa
re e
rror
) in
met
ers
Monte-Carlo 50runs percentage of successful runs=88%
PD=1PG=1V=50Max Error=592.6101mManeuvering RMS=165.7729mNon-Maneuvering RMS=285.3251m process=30m/sec2
R=100m
(MI)=7.5
Error estimator
average Error is 189.0192[m]
ConclusionsConclusions Pd has a great influence on algorithm performance. Even Pd has a great influence on algorithm performance. Even
for a small decrease in Pd the performance decreases for a small decrease in Pd the performance decreases vastly.vastly.
The maneuvering index value has no major influence for The maneuvering index value has no major influence for Pd=0.9.Pd=0.9.
The validation gate size is very important, especially for a The validation gate size is very important, especially for a noiseless environment. Our motivation for noiseless noiseless environment. Our motivation for noiseless environment is to decrease the validation gate for the environment is to decrease the validation gate for the potential minima (depends on object’s velocities).potential minima (depends on object’s velocities).
-3 -2 -1 0 1 2 3 4 5 6
x 104
-5
-4
-3
-2
-1
0
1
2
3
4
x 104
X(meters)
Y(m
ete
rs)
the false measurments and the validation gate V=50
False alarmsEstimation
SummarySummary We implemented the IMMPDAF in We implemented the IMMPDAF in
Matlab.Matlab. We compared our algorithm to a We compared our algorithm to a
given article and got the same given article and got the same results. Later on we extended the results. Later on we extended the conclusions for noisy environment.conclusions for noisy environment.
We analyzed algorithm’s We analyzed algorithm’s performance for different performance for different parameters.parameters.
Further workFurther work In the next part of the project we In the next part of the project we
intend to implement in Matlab an intend to implement in Matlab an algorithm which uses MCMC method algorithm which uses MCMC method in order to track unknown and in order to track unknown and uninitialized number of objects in uninitialized number of objects in noisy environment, and compare it’s noisy environment, and compare it’s performance to other recently performance to other recently proposed heuristical algorithm.proposed heuristical algorithm.