1. HQ U.S. Air Force Academy I n t e g r i t y - S e r v i c e - E x c e l l e n c e Improving the Performance of Out-of-Order Sigma-Point Kalman Filters for Cooperative Target Localization Using Multiple UAVs Pedro DeLima Dimitri Zarzhitsky Daniel J. Pack Presented by: Chad E. Hager

2. I n t e g r i t y - S e r v i c e - E x c e l l e n c e 10/11/2009 Overview Overall Goal Sensor network objectives Problems of out-of-order measurements Out-of-Order Sigma-Point Kalman Filter Proposed method for merging Triangulation and O 3 SPKF Simulation results Summary 2

3. I n t e g r i t y - S e r v i c e - E x c e l l e n c e 10/11/2009 Overall Goal Develop a cooperative, heterogeneous, persistent intelligence, surveillance, reconnaissance (ISR) UAV system 3

4. I n t e g r i t y - S e r v i c e - E x c e l l e n c e Target Localization Examples 10/11/2009 4

5. I n t e g r i t y - S e r v i c e - E x c e l l e n c e 10/11/2009 Multi-Sensor, Multi-UAV Sensor Network Provides target detection and localization capabilities using distributed heterogeneous sensor platforms Capability of multiple sensor types: e.g., RF/VR/IR Capability of multiple sensor modalities: e.g., DOA, TDOA Processed data sent to local and remote UAVs GPS synchronization in real time allows for sensor readings to be time stamped at the sensing source in a common timeframe. Provide quick and accurate estimation of the target’s states, as well as the uncertainty associated with those, based on heterogeneous sensor measurements that become available with different degrees of delays and out-of- order. Sensor Fusion Objectives: 5

6. I n t e g r i t y - S e r v i c e - E x c e l l e n c e 10/11/2009 The Out-Of-Order Sigma Point Kalman Filter Target state (position and velocity) estimates are computed using O 3 SPKF Kalman-filter based method provides near-optimal state estimate with constant computational complexity Nonlinear Kalman filter handles intrinsic nonlinearities in conversion between measurement and state SPKF (a variant of the UKF) has better accuracy than EKF, same computational complexity Handles asynchronous and out of order measurements from local UAV and remote UAVs Fuses measurements from multiple sensor modalities Communication Latency 6

7. I n t e g r i t y - S e r v i c e - E x c e l l e n c e 10/11/2009 The Out-Of-Order Sigma Point Kalman Filter The classical SPKF requires that measurements arrive in- order. How to deal with measurements that arrive out-of- order (O 3 )? “O 3 SPKF Approach”: Modify SPKF algorithm to explicitly account for O 3 measurements  correctly estimate position and velocity despite latency of communication channel No buffer memory requirements; no added latency; all measurements retained O 3 SPKF 7

8. I n t e g r i t y - S e r v i c e - E x c e l l e n c e 10/11/2009 Generic Kalman Filtering General Kalman filtering maintains an estimate of a “state” of a system using a two-step predict/correct process The assumption is that the state and output-error densities are Gaussian, so can be characterized by their means and covariances All Kalman filters employ the following equations: 8

9. I n t e g r i t y - S e r v i c e - E x c e l l e n c e 10/11/2009 Implementation For linear systems, with certain assumptions on noises, KF implements these equations exactly For nonlinear systems, EKF approximates these equations to get analytical algorithm: Assume E[ f ( x )] = f (E[ x ]), which is not generally true Linearize equations to get local linear model Also for nonlinear systems, SPKF approximates in a different way to get empirical algorithm: Propagate “sigma points” X for variable x through f ( x ) to get Y = f (X) and take E[Y] (similar for covariance) No need to linearize equations 9

10. I n t e g r i t y - S e r v i c e - E x c e l l e n c e 10/11/2009 The O 3 SPKF modifies the SPKF steps slightly. If t is the present time, t x is the state-estimate time and t m is the measurement time, ( f(. ) must be invertible) For in-order measurements, O 3 SPKF = SPKF as t  t x  t m For O 3 measurements, t > t x > t m Sigma points representing state at time t m are generated based on inverse dynamic equation Output estimate & covariance at time t m are generated Gain matrix computed and update applied Modifications to SPKF Leading to O 3 SPKF 10

11. I n t e g r i t y - S e r v i c e - E x c e l l e n c e 10/11/2009 O 3 SPKF Performance: Speed of Convergence Localization progress: RMS localization error versus time for six methods, 1s nominal sampling period, two sensors per UAV, 500m orbit (500 simulations averaged) Lower curves are better, O 3 SPKF outperforms standard SPKF and implementations with different buffer sizes (ideal case is not achievable) 11

12. I n t e g r i t y - S e r v i c e - E x c e l l e n c e 10/11/2009 Merging O 3 SPKF and Triangulation Whenever two or more UAVs detect a same target within a second (discretization level), a triangulation point x t (t) is generated. xt(t)xt(t) xt(t)xt(t) 12

13. I n t e g r i t y - S e r v i c e - E x c e l l e n c e 10/11/2009 Merging O 3 SPKF and Triangulation The O3SPKF state is then propagated to the time t The following equation is then used to merge the current target position state of the filter, x(t), with the triangulation point x t (t) to the degree specified by w t 13

14. I n t e g r i t y - S e r v i c e - E x c e l l e n c e Performance Comparison 10/11/2009 O 3 SPKF O 3 SPKF + triangulation Average results of 100 independent trials Team size = 3 UAVs UAV speed = 25m/s Sensor uncertainty = 15 o Sensor rate = 3Hz w t = 0.5 14

15. I n t e g r i t y - S e r v i c e - E x c e l l e n c e 10/11/2009 Summary O 3 SPKF handles asynchronous and O 3 measurements from local UAV and remote UAVs Triangulation technique allows us to take advantage of episodic joint sensor measurements that are more accurate than individual sensor measurements O 3 SPKF state can be improved by merging them with triangulation points based on the estimated uncertainty of the filter O 3 SPKF augmented with triangulation techniques provides faster localization error reduction, with no loss of performance at steady state behavior 15

16. HQ U.S. Air Force Academy I n t e g r i t y - S e r v i c e - E x c e l l e n c e Improving the Performance of Out-of-Order Sigma-Point Kalman Filters for Cooperative Target Localization Using Multiple UAVs Pedro DeLima Dimitri Zarzhitsky Daniel J. Pack

17. I n t e g r i t y - S e r v i c e - E x c e l l e n c e 10/11/2009 For in-order measurements, t  t x  t m ; t 0 = t m  t x The following equations optimized for linear state equation and nonlinear output equation with additive noises O 3 SPKF: In-Order Measurements 17

18. I n t e g r i t y - S e r v i c e - E x c e l l e n c e 10/11/2009 For out-of-order measurements, t > t x > t m ; t 0 = t x  t m O 3 SPKF: Out-of-Order Measurements 18