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 DeLimaDimitri Zarzhitsky

Daniel J. PackPresented by: Chad E. Hager

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 O3SPKF Simulation results Summary

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

Overall Goal

Develop a cooperative, heterogeneous, persistent intelligence, surveillance, reconnaissance (ISR) UAV system

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

Target Localization Examples

-300 -200 -100 0 100 200 300 400

-200

-100

0

100

200

300

400

X (meters)

Y (

met

ers)

Target Location Estimates

121416182085112114270282285473474476477478480485487575641720724121416182085112114270282285473474476477478480485487575641720724

True Target Location

UAV1 Location

UAV2 LocationTarget Location Estimate by UAV1

Target Location Estimateby UAV2

0 50 100 150 200 250 300 350 400-80

-60

-40

-20

0

20

40

60

80

time (sec)

Y lo

catio

n (m

eter

s)

Estimate on Y

True Target Location

Estimated Target Location

0 50 100 150 200 250 300 350 4000

10

20

30

40

50

60

70

80

90

100

time (sec)

X lo

catio

n (m

eter

s)

Estimate on X

True Target Location

Estimated Target Location

10/11/2009 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

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 FusionObjectives:

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

The Out-Of-Order Sigma Point Kalman Filter

Target state (position and velocity) estimates are computed using O3SPKF 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

CommunicationLatency

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

The classical SPKF requires that measurements arrive in-order. How to deal with measurements that arrive out-of-order (O3)? “O3SPKF Approach”: Modify SPKF algorithm to

explicitly account for O3 measurementscorrectly estimate position and velocity despite latency of communication channel

No buffer memory requirements; no added latency; all measurements retained

O3SPKF

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

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

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

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 O3SPKF modifies the SPKF steps slightly. If t is the present time, tx is the state-estimate time and tm is the measurement time, (f(.) must be invertible)

For in-order measurements, O3SPKF = SPKF as t tx tm

For O3 measurements, t > tx > tm

Sigma points representing state at time tm are generated based on inverse dynamic equation

Output estimate & covariance at time tm are generated

Gain matrix computed and update applied

Modifications to SPKF Leading to O3SPKF

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

O3SPKF 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, O3SPKF outperforms standard SPKF and implementations with different buffer sizes (ideal case is not achievable)

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

Merging O3SPKF and Triangulation

Whenever two or more UAVs detect a same target within a second (discretization level), a triangulation point xt(t) is generated.

xt(t)

xt(t)

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 O3SPKF 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 xt(t) to the degree specified by wt

)(N

))((trace)(

N

))((trace1)( )2:1()2:1( tx

tPwtx

tPwtx t

xt

xt

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

Performance Comparison

10/11/2009

O3SPKF

O3SPKF + triangulation

Average results of 100 independent trials

Team size = 3 UAVs UAV speed = 25m/s Sensor uncertainty

= 15o

Sensor rate = 3Hz wt = 0.5

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

10/11/2009

Summary

O3SPKF handles asynchronous and O3 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

O3SPKF state can be improved by merging them with triangulation points based on the estimated uncertainty of the filter

O3SPKF augmented with triangulation techniques provides faster localization error reduction, with no loss of performance at steady state behavior

15

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 DeLimaDimitri Zarzhitsky

Daniel J. Pack

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 tx tm; t0 = tm tx

The following equations optimized for linear state equation and nonlinear output equation with additive noises

O3SPKF: In-Order Measurements

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 out-of-order measurements, t > tx > tm; t0 = tx tm

O3SPKF:Out-of-Order Measurements

18