Target tracking and guidance using particles

1
Target tracking and guidance using particles
David Salmond QinetiQ Farnborough
UK collaborators Nick Everett, Neil Gordon
(DSTO Australia), Kevin Gilholm, Malcolm Rollason
Target tracking is usually a means to an
end e.g. to generate a guidance demand
Contents 1 The guidance / control problem 2 An
example scenario 3 Illustrative results
2
Structure of estimator / controller
Cost function - a function of current and future
state vectors X k x k , x k1 , x k2 ,
, x N control effort
Sensors
Control law
Estimator
Measurements Zk
Demand uk
Effectors
3
Available information for estimator / controller
design
General problem Guidance problem
System dynamics models - Model of pursuer
dynamics as a function of control
demand - Dynamics models of target and
other scenario objects - Model of scenario
development birth / death of
objects Measurement models - Model relating
pursuers sensor (measurements as a
function measurements to target, other of system
state) scenario objects and clutter Cost
function - Interception requirement
in terms of miss distance
4
For guidance problems usually force problem into
a Linear Quadratic Gaussian (LQG) formulation,
i.e. assume 1 All models (dynamics and
measurement) are linear 2 All disturbances and
errors are Gaussian 3 The cost function is
quadratic In this case the Certainty Equivalence
principle holds.
Control depends only on expected value of x k
Optimal filter/controller
z k H k x k v k
x k
z k
u k
Estimator
Control law
Measurements
Demand
Kalman filter
Linear state regulator
x k x k K k (z k - H k x k )
u k Gk x k
5
In practice, for many (most) guidance problems,
none of the LQG assumptions are valid. For
example, - for a Cartesian state vector, the
measurement model is non-linear (sensors provide
polar measurements) - in stressing scenarios
with multiple objects and clutter, a quadratic
cost function is not appropriate - again, due to
measurement association uncertainty, the
measurement information is far more complex than
a simple Gaussian perturbation. Certainty
Equivalence does not hold for such problems.
(Extended) Kalman filter - linear state regulator
combination is often markedly sub-optimal.
6
A more general structure from a Bayesian point of
view
Information state - current state of system
knowledge
Measurement likelihood p(z k x k)
p(x k Zk)
u k
z k
Estimator
Control law
Measurements
Demand
Bayes rule etc
Select demand u k to minimise expected value of
cost function
7
Particle filter implementation
Information state - current state of system
knowledge
Measurement likelihood p(z k x k)
Bayes rule etc
p(x k Zk)
u k
z k
Estimator
Control law
Measurements
Demand
Sample set Sk x k(i) i1,,NS
Particle filter
u k uk(Sk)
IN GENERAL, CONTROL SHOULD DEPEND ON FULL SAMPLE
SET - NOT JUST THE MEAN - CERTAINTY EQUIVALENCE
IS A POOR USE OF THE AVAILABLE INFORMATION
8
Stochastic control problem minimise current and
future costs At time step k, define Sequence
of future states X k x k , x k1 , x k2 ,
, x N Sequence of future controls U k
u k , u k1 , u k2 , , u N-1 Available
measurements Z k z 1 , z 2 , z 3 , , z k
Previous controls (known) U k-1 u 1 , u 2
, u 3 , , u k-1 Find the sequence of
future controls U k that minimises the
cost J Z k , U k-1 min E g( X k ,
U k ) Z k , U k-1 U k
Available information
Expectation over all uncertainty current state,
future dynamics, and future measurements
Specified future cost
9
Approximations to make the problem tractable
1 Ignore the information that future measurements
will become available - Open Loop Optimal
Feedback (OLOF) principle - - so expectation over
future measurements is ignored (no possibility of
dual effect) 2 For guidance problem, assume
particular forms for cost function (predicted
miss) and future controls to reduce
dimensionality
So,
J Z k , U k-1 min E g( X k , U k )
Z k , U k-1 U k min
E g( x k , u k ) Z k , U k-1 u
k
Expectation over uncertainty in current state only
10
Evaluation of expected cost using particles (for
given u k)
Samples from particle filter, approximately
distributed as p( x k Z k , U k-1 )
Hence optimisation problem reduced to
NS
min ? g( x k(i) , u k ) u k
i1
11
Cost functions for guidance problems
Cost is usually some function of the miss
distance
Pursuers prediction of miss distance is
imperfect principally due to i) Uncertainty
in current target state x k ii) Uncertainty in
future target behaviour
PURSUER
MISS DISTANCE
TARGET
For significant measurement association
uncertainty (i) will dominate so assume
miss m( x k , u k ) gt 0 , - i.e. achieved
miss depends only on current state and future
controls
Cost function is of the form g( X k , U k )
g( x k , u k ) f( m( x k , u k ) )
12
Quadratic cost cost rises as square of miss -
unbounded - always drives system towards mean
of cost function Inverse Gaussian cost cost of
missing essentially constant when miss exceeds 3
normalised units i.e. a large miss is as bad
as a very large miss
QUADRATIC COST (UNBOUNDED)
COST f( m )
INVERSE GAUSSIAN COST (BOUNDED)
MISS DISTANCE m
13
Example scenario single target (T) in dense
random clutter with intermittent spurious object
(D)
D is spawned in the vicinity of T and with a
similar velocity
Sensor resolution is limited T / D pair may
initially be unresolved
The sensor takes measurements of range and
bearing and is carried by the pursuer
Measurements are corrupted by dense random clutter
A (very poor) classification flag may be
associated with each measurement but T and D
cannot be distinguished the following example
14
Particle filter includes
Second order dynamics for T and D (noise driven
constant velocity) Markov model to represent
birth / death of D objects Measurement
association uncertainty via assignment
hypotheses Classification data within
measurement likelihood
A possible assignment for Nk measurements
received at time step k
Type
Meas. number
1
Target D J unresolved Clutter
2
3
4
. . .
N
k
????1,2,..., Nk T,D,J,C
Unknown assignment
15
Pursuer model
Pursuer moves at a constant speed VM Heading is
controlled by a turn rate (guidance) demand uk
updated at every time step (no lag) So heading
f k1 fk uk Dt where uk lt a MAX
/ VM . Assume that choice of future controls
U k is restricted to a constant turn rate, so
uj uk for jgtk Guidance problem is to
select a single number uk from the range ( - a
MAX / VM , a MAX / VM ) to minimise the
expected cost ? f( m( x k(i) , u k ) )
NS
i1
16
Object paths
1.15
T path
D path
1.1
D deployed at this point
1.05
y
1
Constant turn rate
Constant velocity
0.95
T and D indistinguishable at split
0.9
-1
-0.5
0
0.5
1
1.5
2
2.5
3
3.5
x
17
Totality of measurements in vicinity of T and
D (transformed from polar co-ords)
18
ALL OBJECT MEASUREMENTS GREENT, REDD,
BLUEUNRESOLVED ONLY ONE FRAME OF CLUTTER
YELLOW (TRANSFORMED FROM POLAR CO-ORDS)
19
D present and resolved
D present but not resolved
D not present
Prob. from filter
Particle filters assessment of scenario state
20
(No Transcript)
21
EVOLUTION OF CROSS-RANGE pdf FROM PARTICLE
FILTER
pdf
CROSS-RANGE
TIME STEP
22
Guidance via particle filter with inverse
Gaussian cost function
23
TARGET TRACK AND TRUE MEASUREMENTS
Guidance via particle filter with inverse
Gaussian cost function
24
EXPECTED COST
GUIDANCE DEMAND (u)
TIME STEPS TO GO
25
Distribution of predicted miss with uj 0 for
jgtk, i.e. for zero pursuer effort
Guidance via particle filter with inverse
Gaussian cost function
26
Guidance via particle filter with quadratic cost
function
27
TARGET TRACK AND TRUE MEASUREMENTS
Guidance via particle filter with quadratic cost
function
28
Distribution of predicted miss with uj 0 for
jgtk, i.e. for zero pursuer effort
Guidance via particle filter with quadratic cost
function
29
Conclusions
1 Have demonstrated a guidance law for
exploiting output of a particle
filter 2 Guidance law is based on a bounded cost
function of the predicted miss distance 3 A
smooth transition from a hedging / learning
strategy to a firm selection decision has been
demonstrated