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 k+1 , x k+2 , … , x N } + control effort Sensors Control law Estimator Measurements Zk Demand uk “Effectors”

3. Available information for estimator / controller design General problemGuidance 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 pursuer’s sensor (measurements as a functionmeasurements 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 kx k + vk x k z k u k Control law Estimator Measurements Demand Kalman filter: Linear state regulator: x k = x k + K k (z k - H kxk ) 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 | xk) p(x k | Zk) u k z k Control law Estimator 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 | xk) Bayes rule etc p(x k | Zk) u k z k Control law Estimator Measurements Demand Sample set Sk = { x k*(i) : i=1,…,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 statesX k+ = { x k , x k+1 , x k+2 , … , x N } Sequence of future controlsU k+ = { u k , u k+1 , u k+2 , … , u N-1 } Available measurementsZ 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. g( X k+ , U k+ ) g†( x k , u k ) 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. E[ g†( x k , u k )| Z k , U k-1] =g†( x k , u k ) p( x k| Z k , U k-1 ) dx k   g†( x k*(i), u k ) NS i=1 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 i=1

11. . . . . . . . . . . Cost functions for guidance problems Cost is usually some function of the miss distance: Pursuer’s 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) >= 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. 4.5 4 3.5 3 2.5 2 1.5 1 0.5 0 0 0.5 1 1.5 2 2.5 3 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. o o o o o * o D * * * * o INITIALLY UNRESOLVED T o o 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 k+1 = fk + ukDt where | uk | < a MAX / VM . Assume that choice of future controls U k+ is restricted to a constant turn rate, so uj = uk for j>k 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 i=1

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: GREEN=T, RED=D, BLUE=UNRESOLVED 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 filter’s assessment of scenario state

20. BIFURCATION OF PARTICLE SET ON DEPLOYMENT OF D 1.2 Some particles from time steps 40 50 60 70 80 90 1.15 1.1 T PATH 1.05 Y 1 0.95 D PATH 0.9 0.85 1.8 2 2.2 2.4 2.6 2.8 3 3.2 3.4 3.6 X

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 j>=k, 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 j>=k, 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