distributed gps-denied navigation - worcester … effects of distributed multimodal sensor fusion...

Collaborative Effects of Distributed Multimodal Sensor Fusion

for First Responder NavigationJim Kaba, Shunguang Wu, Siun-Chuon Mau, Tao Zhao

Sarnoff Corporation

Briefed By: Jim Kaba(609) 734-2246 [email protected]

mailto:[email protected]

©2008 Sarnoff Corporation2/20

Overview•Sensor-independent distributed multimodal sensor fusion framework for improving navigation accuracy–Theoretical & analytical models and simulations of how performance scales

versus • Different algorithms • Different sensor error models • Varying numbers of users

•Practical navigation algorithm and system design –Excellent implementation characteristics–Capable of achieving predicted error reduction effects

•Collaborative Error Reduction Effects -- Exploit the collaborative nature of team-oriented operations–Teamwork Effect

• Enables firefighters operating in groups to achieve better navigation accuracy than when operating individually

–Anchor Effect • Flexible use of minimal numbers of Deployable Anchor Nodes as RF navigation reference beacons

–Reset Effect • Immediate opportunistic accuracy gains under common conditions


Motivation: GPS-denied Navigation

•GPS is not always available–Weak signal, corrupted signal, no signal –Urban Canyons, Indoors, Underground–Jammed and/or spoofed

•Significant challenges or complications–Cluttered RF & magnetic environment–Size, Weight and Power (SWaP) and Cost issues–Mobility–Pragmatic operational use and deployment constraints for certain applications

Hostile operating environment compounded by mission constraints


Locatus System Design Goals

• Provide absolute & relative 3-D position to ground forces for:– Individual warfighter navigation– Team coordination with neighbors (fireteam, squad, platoon)– Position status updates to command / headquarters

• Support Military Operations on Urban Terrain– Outdoor urban areas, building interiors, subterranean

environments…and open areas

• Maintain strict position accuracy– 25m SEP after traveling 20km over 8hrs => 0.125 % “system

error”…while GPS-denied

• Man portable; size, weight and power restrictions– < 400cm3 & < 1kg per unit, battery powered– Associated pragmatic operational constraints

Target Application: Dismounted Warfighter Navigation in a fully GPS-denied Environment

Strong match to Firefighter requirements


Locatus System Overview

…a multimodal position/navigation system for GPS-denied environments*…

Ad Hoc Network Relay Functionality

RF Ranging &Ad Hoc Network Relay Functionality

RF Ranging,MEMS Inertial Navigation &

Ad Hoc NetworkRelay Functionality

*Illustrated as a man-portable position/navigation system for GPS-denied urban operations. System approach and algorithms can be applied to heterogeneous combinations of aircraft, vehicles, UAVs, UGVs, munitions, etc.

Primary Subsystem Components• Inertial Navigation System

3-axis accelerometer, rate sensors,

magnetometers, pressure altimeter

• Inter-node Distance Measurement • Inter-node Data Communications

Network

• INS provides baseline displacement estimates• Inter-node ranges bound the growth of inertial position error over time


Concept of Operation• Each firefighter wears a Mobile Locator Node

– Inertial Navigation System + RF Ranging & Comms

• Deployable Anchor Node(s) positioned around the periphery of the operational area (optional, mission-permitting)– RF Ranging + Comms– Provides a framework of RF “beacons” at known locations

• Firefighters move into the GPS-denied operational area– INS tracks 3-D location (with some error)– RF Ranging Radios track inter-node (Anchors and Firefighters) distances

(with some error)– Locatus system fuses inertial and RF position estimates, appropriately

weighted for operational & error characteristics

• Firefighters move deeper into the GPS-denied operational area– Warfighters deploy additional Anchor Nodes as needed to maintain RF

measurement connectivity with external framework

• Throughout the incident response– 3-D position estimate is maintained with bounded (but growing) error as

the incident progresses (with time, distance, constrained operational areas)– Situational Awareness (navigation capability and accountability) is provided

throughout incident for firefighters and command structure


Sarnoff’s Technical Approach

• Distributed fusion of multiple position / location / navigation modalities– Multiple sensors on each warfighter/platform and across multiple

warfighters/platforms

• Exploit the complementary nature of sensor’s operational characteristics and errors– Absolute vs. relative, localized vs. distributed, range unlimited vs. tethered, long-term

drift vs. short-term random error

• Use dynamic and continuous cross-modal feedback to bound system errors– Detect & minimize errors, constrain measurements

• Achieve graceful (vs. catastrophic) degradation of system performance over time & distance as dictated by the mission

Continuous, real-time fusion of error-prone position estimates from multiple technologies to achieve accurate absolute & relative location, navigation & mapping capabilities

• Use dynamic Bayesian (belief) network to build a graphical model of inferred node location

– Intractable joint probabilistic distribution of all node locations & uncertainties is factored into a combination of simpler local distributions


Multimodal Fusion Algorithm Overview

N-frame jointoptimization

online jointoptimization

centralizedIEKF

centralizedEKF

I distributedEKF

distributedEKF

centralizedIEKS

centralizedEKS

NBP

NBP-PF

online batch

dist

ribut

edce

ntra

lized

Gaussian Gaussian

Single iterationSingle iteration

Single iteration

I online distributedoptimization

Approx.neighbor

info

Gaussian

online BP

batch BP

Gaussian+linearization

non-parametric

non-parametric

Approx.neighbor

infoApprox.

neighborinfo

Markovian

Markovian

Markovian

Arrows connote added assumptions/constraints

I: iterativeE: extendedK: KalmanF: filterS: smootherN: non-parametricBP: belief propagationPF: particle filter

Family of Inference MethodsIssues:•Performance •Computation

Overhead• Communications

Overhead• Scalability• Reliability• Fault-tolerance


iEKF Fusion Algorithm Data Flow

Nodes individually estimate their position & certainty using localized sensors (INS, altimeter)

Nodes exchange their estimates with neighbors

Nodes refine their estimate using neighbor’s estimates and inter-node range constraints

Nodes repeat the exchange/refine cycle multiple times

• Formulated as a rapidly converging, iterative, message-passing algorithm

• Comparable performance to centralized/optimization algorithms


Multimodal Fusion: Implementation Benefits• Provides accurate Absolute and Relative Positioning

– Using real-world sensors under real-world conditions

• Sensor flexibility– Robust to varying inertial sensor noise/performance models– Easily extensible to include additional individual and distributed sensors– Handles asynchronous sensor inputs– Tolerates inaccuracies of inter-node range constraints

•Achieves low overall position error even with low-accuracy ranging•Accurate positioning even as RF range measurement accuracy fluctuates due to obstructions and multipath interference in urban environment

– Tolerates variations in inter-node ranging interval•Minimal effect on the absolute system accuracy, minor (short term) effect on relative accuracy

• Superior implementation characteristics:– Algorithm is naturally fully distributed– Scalable, robust to dynamic changes in # of users– Iterative algorithm rapidly converges in steady state operation– Computation and communication are inherently localized to nearest-neighbors– Communication load is well within the “Kbps” constraints of tactical military radios


The Teamwork Effect

• The Locatus “Teamwork Effect” enables warfighters operating in groups to achieve significantly better navigation accuracy than when operating individually

• Opportunistic Peer-to-Peer Ranging Constrains INS Drift– Range estimate between two warfighters serves as a “Wireless Tether”

between them and bounds their otherwise independent drifts– Using multiple inter-asset range estimates constrain INS drift further

• Teamwork Effect holds as team size varies– Single pair to large groups

• i.e. Position accuracy improves by a factor √n for an n-node group

• General performance prediction guideline for distributed multimodal fusion


Absolute Position Accuracy

0 5000 10000-5

0

5

10

15

20

25

abso

lute

RM

S er

r (m

)

(n=2)

INU olnydis EKFdis OPTcent EKFcent OPT

0 5000 100000

5

10

15

20

25

abso

lute

RM

S er

r (m

)

INS Stds: σdx =0.2(m), σdy =0.2(m), σdz =0.001(m), MSSI std σd= 1m

(n=4)

0 5000 100000

5

10

15

20

25

abso

lute

RM

S er

r (m

)

(n=6)

0 5000 100000

5

10

15

20

25

abso

lute

RM

S er

r (m

)

(n=8)

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)

(n=10)

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)

(n=12)

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)

(n=14)

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)

(n=16)

Tight grouping of performance

results for four fusion algorithms

Systematic error reduction with increasing network size

Substantial (2-3 X) performance increase even with

a small size network

25mSEP

Error vs. Time for varying team sizes

INU-only

Centralized vs. Distributed;

Optimization vs. EKF


The Teamwork Effect: Simulation Validation

• Validation of the “Teamwork Effect” error scaling law• 1/sqrt(n) reduction in system error with a team of “n” collaborating nodes• Robust to variations in multimodal fusion algorithm• Robust to variations of inertial sensor noise model (not shown)

0.2 0.4 0.6123 frame # = 500

RM

S Er

r (m

)

cent ekf: rms errcent ekf: fitted linedist ekf: rms errdist ekf: fitted line

0.2 0.4 0.6

2

4

6 frame # = 1400

0.2 0.4 0.62

4

6

8 frame # = 2300

INU: σdx =0.2m, σdy =0.2m, σdz =0.001m, MSSI: σd=1m

0.2 0.4 0.62

4

6

8 frame # = 3200

0.2 0.4 0.62468

10 frame # = 4100

RM

S Er

r (m

)

0.2 0.4 0.6

468

1012

frame # = 5000

0.2 0.4 0.6

468

10 frame # = 5900

0.2 0.4 0.6

468

1012

frame # = 6800

0.2 0.4 0.6

5

10

15frame # = 7700

RM

S Er

r (m

)

0.2 0.4 0.6

5

10

15frame # = 8600

0.2 0.4 0.6468

101214

frame # = 9500

0.2 0.4 0.6

5

10

15frame # = 10400

0.2 0.4 0.65

10

15 frame # = 11300

RM

S Er

r (m

)

1/sqrt(n)0.2 0.4 0.6

5

10

15 frame # = 12200

1/sqrt(n)0.2 0.4 0.6

5

10

15 frame # = 13100

1/sqrt(n)0.2 0.4 0.6

5

10

15frame # = 14000

1/sqrt(n)

Error vs. Network Size at 15-minute

intervals


Relative Position Accuracy

Multimodal Fusion:•Relative position error bounded to a constant level < 1m SEP after 20 km & 4 hrs GPS-denied•Even with high-error INS exceeding 400m a relative error ~2-3m SEP achieved (not shown)

10m SEP

1m SEP

INU-only (ZUPT sqrt(d) inertial drift):•Relative position error exceeds 30m SEP after 20 km & 4 hrs GPS-denied

30mSEP


The Anchor Effect

• Deployable Anchor Node– Reference beacon deployed at fixed location– Zero INS drift error: position estimate (and error) remains constant– Anchor point for mobile nodes whose position estimates degrade with

time/distance

• Deployed opportunistically (pre- or during mission) as stationary wireless tethers and communication relay nodes– Self-calibration of deployed nodes based on best location estimate available at

the time of deployment

• The use of even a single Deployable Anchor Node can increase system accuracy by a factor of 2 to 3

• The use of two Deployable Anchor Nodes can bound absolute system error to <1m SEP

• Contrast with classical Time Difference of Arrival (multilateration) and Time of Arrival (trilateration) approaches that require at least 4 constraining measurements


The Anchor Effect: Simulation Validation

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

) (n=2)

inu onlydist w/o anchordist w 1 anchordist w 2 anchors

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)

(n=4)

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)

nMCRs=50, INU: σdx =0.2(m), σdy =0.2(m), σdz =0.001(m), MSSI: σD=1(m)

(n=6)

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)

(n=8)

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)

(n=10)

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)

(n=12)

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)

(n=14)

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)

(n=16)

Absolute Position AccuracyShown

Unaided (ins-only) Operation

1 Anchor 2 AnchorsRF-aided Operation

• 1 Anchor 2-3X performance improvement• 2 Anchors Constant, low level error 1-2m SEP


The Reset Effect

• Locatus’ continual refinement of position estimates can result in estimates that improve, rather than degrade, over time– The inaccurate position estimates of Mobile Locator Nodes are “reset” to a lower

level in the middle of a mission even after those errors have grown– Provides immediate, rather than gradual, improvement of the estimate accuracy

• “Reset Effect” conditions:– a) A Mobile Locator Node whose position error has grown establishes contact with a

stationary Deployable Anchor Node. The mobile node’s position estimate, and its confidence in accuracy of that estimate, will improve

– b) A Mobile Locator Node whose position error has grown establishes contact with another Mobile Locator Node with a better location certainty

– c) An individual or a small group of Mobile Locator Nodes merges with another group of Mobile Locator Nodes, forming a larger group allowing for collectively better error performance (e.g. due to the “Teamwork Effect”)

• The “Reset Effect” is demonstrable for Deployable Anchor Nodes and large groups of Mobile Locator Nodes

• The “Reset Effect” enables teams of warfighters that split into subgroups and merge some time later to regain the same level of positioning performance as if they had not split


The Reset Effect: Simulation Validation

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

) (n=2)

inu onlydist w/o anchordist w 1 anchordist w 2 anchors

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)

(n=4)

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)

Anchor(s) deployed at f=3000, INU: σdx =0.2m, σdy =0.2m, σdz =0.001m, MSSI: σd=1m

(n=6)

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)

(n=8)

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)

(n=10)

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)

(n=12)

0 5000 100000

5

10

15

20

25

Frame #

abso

lute

RM

S er

r (m

)(n=14)

0 5000 100000

5

10

15

20

25

Frame #ab

solu

te R

MS

err (

m)

(n=16)

• Simulation results support the Locatus “Reset Effect” by which system errors can actually decrease over time

Opportunistic encounter with 1 or 2 anchor nodes resets error to a lower level

Example:Encounter

with Anchor Node(s)

Unaided Operation

Contact with Anchor

Error “Reset”1 and 2 Anchors

RF-aided Operation


The Reset Effect: Simulation Validation

2000 4000 6000 8000 10000 12000 140000

5

10

15

← ← ←→ → →n=8 n=4 n=8

abso

lute

RM

S er

r (m

)

nMCRs=50, INS Stds: σdx =0.2(m), σdy =0.2(m), σdz =0.001(m), MSSI std: σd=1 (m)

dynamical nodesn=8n=4

2000 4000 6000 8000 10000 12000 140000

1

2000 4000 6000 8000 10000 12000 140000

1

rela

tive

RM

S er

r (m

)

2000 4000 6000 8000 10000 12000 140000

1

Frame #

• Simulation results support the Locatus “Reset Effect” by which system errors can actually decrease over time

Affects not just error rate (curve slope), but absolute level as well!

Example:Team Split/Merge

Error “Reset”

8-node team splits

Two 4-node teams merge

AbsoluteError

RelativeError

teams operate independently


Summary

•System Design & Multimodal Sensor Fusion Framework–Sensor independent–Excellent implementation characteristics

•Exploits the collaborative nature of team-oriented operations–Expected performance trends & behaviors

•Teamwork Effect – Firefighter position accuracy can improve through collaboration with other mobile nodes–…without additional infrastructure requirements

•Anchor Effect – When reference beacons can be used, they can be used –In minimal numbers–With flexibility, deployed at any time

•Reset Effect – Position accuracy need not be constantly degrading–Improvements over time can be expected

End of Brief…

Jim Kaba(609) 734-2246

[email protected]

mailto:[email protected]

distributed gps-denied navigation - worcester … effects of distributed multimodal sensor fusion...

Documents