SLAM-based Integrity Monitoring for Multi-Sensors and Multi...

SLAM-based Integrity Monitoring for Multi-Sensors and Multi-ReceiversSRIRAMYA “RAMYA” BHAMIDIPATI, UNIVERSITY OF ILLINOIS AT URBANA-CHAMPAIGN

GRACE XINGXIN GAO, STANFORD UNIVERSITY

SCPNT Annual Symposium 2019, Palo Alto, CA

• Redundancy plays a pivotal role in integrity assessment [1,2]

• Challenges of GPS-only integrity [3]

▪ Fewer measurements due to degraded satellite visibility

▪ Received signal faults due to multipath effects

▪ Satellite faults due to broadcast anomalies in satellite ephemeris

Integrity in Urban Areas

1

• Incorporate data redundancy by leveraging urban infrastructure▪ Visual feature-rich surroundings using camera

▪ Cooperative inter-vehicle interactions via UWB ranging[1] Blanch, Walter, et.al., ION ITM 2012[2] Pullen, Walter, et.al., GPS World, 2011[3] Joerger, et.al., Inside GNSS 2017

Develop a GPS-aided Integrity Monitoring (IM) algorithm for urban areas, which

▪ Provides a flexible platform for easy scalability across varied sensors;

▪ Accounts for multiple faults in different sensor modalities, not just GPS;

▪ Computes the protection levels of the estimated navigation solution.

Our Objective

2

Simultaneous Localization and Mapping (SLAM)-based Fault Detection and Isolation (FDI) [4] using GPS-only receiver

▪ Simultaneously localizes both receiver and GPS satellites

▪ Performs graph optimization via GPS pseudoranges, receiver motion model and satellite ephemeris

Our Prior Work: SLAM-based FDI

3

[4] S. Bhamidipati and G. X. Gao, ION GNSS+ 2018

Robot: GPS receiverLandmarks: GPS satellites

Simultaneous Localization and Mapping (SLAM)-based Fault Detection and Isolation (FDI) [4] using GPS-only receiver

▪ Simultaneously localizes both receiver and GPS satellites

▪ Performs graph optimization via GPS pseudoranges, receiver motion model and satellite ephemeris

Our Prior Work: SLAM-based FDI

3

[4] S. Bhamidipati and G. X. Gao, ION GNSS+ 2018

Features of SLAM-based FDI

• No prior assumption about distribution of states

• No requirement for prior knowledge of surrounding 3D maps

• It has a flexible platform to add receivers and landmarks

• Each measurement is analyzed for the presence of fault

Key Contributions

4

SLAM-based IM using worst-case failure slope

SLAM-based FDI using single GPS-only [4]

GPS and fish-eye camera [5,6]

+

Cooperative network of GPS receivers [7]

[4] S. Bhamidipati and G. X. Gao, ION GNSS+ 2018[5] S. Bhamidipati and G. X. Gao, ION GNSS+ 2019[6] S. Bhamidipati and G. X. Gao, ION NAVIGATION (Submitted 2019)[7] S. Bhamidipati and G. X. Gao, IEEE GNSS+ 2019

With SLAM-based FDI as the foundation, we perform IM

via two extensions

Our prior work

Key Contributions

4

SLAM-based IM

SLAM-based Fault Detection and Isolation

(FDI) using GPS-only

Multi-sensor: GPS and fish-eye camera [5,6]

+

Multi-receiver: Cooperative network of GPS receivers [7]

Our prior work: SLAM-based FDI

Key Contributions

4

SLAM-based IM

Multi-sensor: GPS and fish-eye camera [5,6]

+

Multi-receiver: Cooperative network of GPS receivers [7]

Our prior work: SLAM-based FDI

Key Contributions

4

SLAM-based IMOur prior work: SLAM-based FDI

11

Assuming position error and measurement residuals to be chi-square distributed

𝑃𝐹𝐴 and 𝑃𝑀𝐷: prescribed integrity standards

Worst-case failure slope

𝝀

ഥ𝑚𝑤𝑐

Under faults

Pre-defined threshold

PL

𝑃𝐹𝐴

𝑃𝑀𝐷

Measurement residual

Po

siti

on

err

or

Under fault-free

SLAM-based IM: PL estimationOur prior work: SLAM-based FDI

• Multi-Sensor SLAM-based IM | GPS and Fish-eye Camera▪ Extended Graph Optimization | Cost Function

▪ Multiple FDI | Super-pixel based Piecewise RANSAC

▪ Protection Level Computation | Worst-Case Failure Slope

▪ Experimental Results | Using Ground Vehicle

• Multi-Receiver SLAM-based IM | Network of Receivers▪ Algorithm Details | Distributed Graph Optimization

▪ Experimental Verification | Simulated Setup

• Summary

Outline

5

Multi-Sensor Architecture

6

Upwards pointed

fish-eye camera Raw image

Vision module: hybrid sky detection

1. Extended graph optimization

Empirical normal distribution for GPS

Superpixel-based piecewise RANSAC

for vision

3. Protection level computation

Estimated overall state vector

Measurement fault hypothesis

Vehicle position and protection levels

Sky pixels

GPS receiver Pseudoranges,

Doppler and 𝐶/𝑁0

Non-sky pixels

Receiver motion model and satellite

ephemeris

Motion inputs

2. Multiple FDI

Multi-Sensor Architecture

6

Upwards pointed

fish-eye camera Raw image

Vision module: hybrid sky detection

Temporal correlation for GPS

Spatial correlation for vision

PL estimation via worst-case failure slope

Estimated overall state vector

Fault status of measurements

Estimated position and PL

Sky pixels

GPS receiver Pseudoranges,

Doppler and 𝐶/𝑁0

Non-sky pixels

Receiver motion model and satellite

ephemeris

Motion inputs

Multiple FDI based on measurement residuals

Extended graph optimization

Graph Optimization: Cost Function

7

𝒆𝒕 = 𝒆𝒗𝒊𝒔𝒊𝒐𝒏 + 𝒆𝒑𝒔𝒆𝒖𝒅𝒐𝒓𝒂𝒏𝒈𝒆 + 𝒆𝑫𝒐𝒑𝒑𝒍𝒆𝒓 + 𝒆𝒔𝒂𝒕_𝒆𝒑𝒉 + 𝒆𝒗𝒆𝒉𝒎𝒐𝒕𝒊𝒐𝒏

Execute graph optimization over time history to localize image pixels, vehicle and GPS satellites

▪ Non-sky pixel intensities processed via direct image alignment [9, 10]

▪ Detected sky region characterizes GPS measurement covariance

Weighted residuals based on the measurement fault status

estimated at past time instant

Weighted residual based on the protection level estimated

at past time instant

𝑎𝑟𝑔𝑚𝑖𝑛

𝑛=𝑡−𝑇

𝑡

𝑒𝑛 T: past time instants [9] Engel, et.al., ECCV, 2014

[10] Caruso, et.al., IEEE IROS 2015

IM for Graph-SLAM: Challenges and Solutions

8

[11] Joerger, et.al., Navigation 2014

Graph-SLAMWorst-Case Failure

Slope [11]Our Solution


8




Non-linearity in the cost function

Derived for linear measurement model

Linearize the graph optimization framework at

estimated overall state vector


8








It is a sequential localization technique

Falls under snapshot integrity method

Linearize over the past time history of measurements







It is a sequential localization technique

Falls under snapshot integrity method

Linearize over the past time history of measurements

Large number of states and measurements

Evaluates all possible fault hypotheses

Formulate single fault hypothesis using multiple FDI


8


• Detect and isolate received signal and satellite faults

• Evaluate empirical Gaussian distribution▪ Considering errors to be Gaussian during non-faulty conditions

• Deviation of residual 𝛾𝑖 from empirical CDF Φ𝑖 indicates fault

Multiple FDI: GPS Faults

9

GPS fault status = 4 Φ𝑖 𝛾𝑖 − 0.52

Fau

lt S

tatu

s

• Detect and isolate received signal and satellite faults

• Evaluate empirical Gaussian distribution▪ Considering errors to be Gaussian during non-faulty conditions

• Deviation of residual 𝛾𝑖 from empirical CDF Φ𝑖 indicates fault

Multiple FDI: GPS Faults

9

Unlike GPS, vision errors show high

spatial correlation

GPS fault status = 4 Φ𝑖 𝛾𝑖 − 0.52

Fau

lt S

tatu

s

Multiple FDI: Vision Faults

10

1. Segment to 𝐿 superpixels [12]

Steps of our superpixel-based piecewise RANSAC

[12] Li, et.al., IEEE CVVR 2015

[13] Conte and Doherty, EURASIP, 2009


10

2. Estimate the fitted line via RANSAC [13]; compute outlier fraction 𝑂𝐹𝑘

Exp

ecte

d in

ten

sity

Received intensity


At each non-sky superpixel


Outliers




10


Exp

ecte

d in

ten

sity

Received intensity

3. Compute Outlier Fraction (OF) at other superpixels

𝑂𝐹1𝑘 𝑂𝐹𝑗

𝑘 𝑂𝐹𝐿−1𝑘



Estimated fitted line


⋯

Outliers




10

Exp

ecte

d in

ten

sity

Received intensity

3. Compute Outlier Fraction (OF) at other superpixels

𝑂𝐹1𝑘 𝑂𝐹𝑗

𝑘




Fault status of each superpixel is product of

all outlier fractions⋯

Estimated fitted line

Outliers

𝑂𝐹𝐿−1𝑘


𝝀

ഥ𝑚𝑤𝑐

Under faults


PL

𝑃𝐹𝐴

𝑃𝑀𝐷

Measurement residualP

osi

tio

n e

rro

r

Protection Levels: Concept

11

[14] Salos, et.al., IEEE ITS 2014

[14]

Assuming position error and measurement residuals to be

chi-square distributed𝑷𝑳 = ഥ𝒎𝒘𝒄

𝟐 𝝀

ഥ𝒎𝒘𝒄𝟐 : Worst-case

squared failure slope𝜆: pre-defined

threshold

[14]

Failure slope =Position error



11

Assuming position error and measurement residuals to be chi-square distributed

𝑃𝐹𝐴 and 𝑃𝑀𝐷: prescribed integrity standards


𝝀

ഥ𝑚𝑤𝑐

Under faults


PL

𝑃𝐹𝐴

𝑃𝑀𝐷


Po

siti

on

err

or

Under fault-free


𝝀

ഥ𝑚𝑤𝑐

Under faults


PL

𝑃𝐹𝐴

𝑃𝑀𝐷

Measurement residualP

osi

tio

n e

rro

r

Protection Levels: Concept

11

[14] Salos, et.al., IEEE ITS 2014

[14]

Assuming position error and measurement residuals to be

chi-square distributed

Failure slope =Position error


𝑷𝑳 = ഥ𝒎𝒘𝒄𝟐 𝝀

ഥ𝒎𝒘𝒄𝟐 : Worst-case

squared failure slope𝜆: pre-defined

threshold

[14]

Worst-case squared failure slope [14]

maximizes the failure slope of Graph-SLAM framework

Protection Levels: Failure Slope

• According to [11], worst-case squared failure slope equals maximum eigenvalue of the failure slope formulation


12

Fault hypothesis Fault vectorKnown Unknown

Protection Levels: Failure Slope

28

ഥ𝒎𝒘𝒄𝟐 = 𝑨𝒇

𝑻𝑺𝑻𝑨𝒇𝑻 𝐈 − 𝐆𝐒 𝑻 𝐈 − 𝐆𝐒 𝑨𝒇

−𝟏𝑨𝒇𝑻𝑺

𝑨𝒇: Diagonal matrix

of fault status from multiple FDI

S: graph optimization matrix from estimated

overall state vector

• According to [11], worst-case squared failure slope equals maximum eigenvalue of the failure slope formulation


G: linearized measurement

model

12

Fault hypothesis Fault vectorKnown Unknown

• Incorporating our design solutions, the worst-case squared failure slope for Graph-SLAM is given by

Experiment Setup

13

Semi-urban area of Champaign, IL experiencing GPS and vision faults

• Real-world experiments conducted in semi-urban area of Champaign, IL

• Utilized a moving ground vehicle, equipped with ▪ Commercial off-the-shelf GPS

receiver

▪ Camera and 180° fish-eye lens

• Duration of experiment 𝑡 = 100 𝑠

Multiple FDI: GPS Fault Status (1/2)

14

Multiple FDI: GPS Fault Status (2/2)

14

Successfully detected and isolated satellites experiencing multipath

Multiple FDI: Vision Fault Status

15

High value of fault status of superpixels in open-sky

and low faults during presence of tall buildings

Markers denote superpixels with top four

highest fault status

Illumination variations

Comparison with GPS-only

16

Mean Error (m)

SLAM-based IM Least SquaresGPS-only Multi-Sensor

Position Error 16.2 8.8 52.44

Size of PL 10.5 6.5 --

Achieved tighter protection levels via

GPS and fish-eye camera

Multi-Sensor SLAM-based IM

SLAM-based IM using GPS-only

Size of PL Size of PL

• Multi-Sensor SLAM-based IM | GPS and Fish-eye Camera▪ Extended Graph Optimization | Cost Function

▪ Multiple FDI | Super-pixel based Piecewise RANSAC

▪ Protection Level Computation | Worst-Case Failure Slope

▪ Experimental Results | Using Ground Vehicle

• Multi-Receiver SLAM-based IM | Network of Receivers▪ Algorithm Details | Distributed Graph Optimization

▪ Experimental Verification

• Summary

Outline

17

• Distributed approach to cooperative SLAM

▪ Low bandwidth requirements, i.e., data exchange only during interactions

▪ No requirement for clock synchronization across receivers

▪ Easy scalability to any number of receivers

• At each vehicle, during interaction with neighboring vehicles,

▪ Obtain ranging measurement from UWB sensor

▪ Receives the system data from each neighboring vehicle

o Consists of its UTC time, UWB ranging, GPS satellites’ position, position and protection levels

▪ Perform graph optimization to simultaneously localize the GPS satellites, itself and the neighboring vehicles

Distributed Graph Framework

18

Multi-Receiver Architecture

19

System

Experiments: Simulated Setup

20

Simulated semi-urban area prone to multipath in GPS and UWB▪ A network of 6 receivers

▪ Incorporated clock bias differences of the order ±10𝜇𝑠

▪ Multipath errors are modeled as rise and fall of tanh profile

[Google maps]

Parameters Value

GPS multipath bias 80-140 m

Visible GPS satellites 6

# multipath satellites 2-3

UWB multipath bias 3-5 m

GPS white noise 10 m

UWB white noise 1-2 m

20

Simulated semi-urban area prone to multipath in GPS and UWB▪ A network of 6 receivers

▪ Incorporated clock bias differences of the order ±10𝜇𝑠

▪ Multipath errors are modeled as rise and fall of tanh profile

Parameters Value

GPS multipath bias 80-140 m

Visible GPS satellites 6

# multipath satellites 2-3

UWB multipath bias 3-5 m

GPS white noise 10 m

UWB white noise 1-2 m

[Google maps]

Experiments: Simulated Setup

Comparison with Single GPS

21

Accuracy (m)

SLAM-based IM Least SquaresSingle GPS Multi-Receiver

Position 12.5 9.1 34.2

Size of PL 8.4 5.4 -

Achieved tighter protection levels using Multi-Receiver SLAM-

based IM

Size of PL Size of PL

Receiver-C: Multi-Receiver SLAM-based IM

Multipath Multipath

Receiver-C: SLAM-based IM

(single receiver)

ReceiversLocalization Error (m) RMSE Size of PL (m)

Multiple Single Multiple Single

A (satellite blockage)

7.0 8.4 5.2 6.0

B (Open-sky) 5.9 7.1 4.3 5.8

C (Multipath) 9.1 12.5 5.4 8.4

D (Multipath) 7.1 11.6 4.6 9.7

E (Open-sky) 2.4 3.7 1.8 2.1

F (Static) 4.3 6.2 2.7 4.4

Quantitative Statistics: 100 Runs

22

Achieved lower localization errors and tighter protection levels via Multi-Receiver SLAM-based IM

C: Under multipath, multi-receiver show

significantly tighter PL

E: Under open-sky and moving, single and multi-receiver

show similar PL

F: Under open-sky and static, multi-receiver

show tighter PL

• Proposed SLAM-based Integrity Monitoring (IM) using ▪ Multiple sensors, via GPS and fish-eye camera

▪ Network of receivers and inter-vehicle interactions

• Developed superpixel-based piecewise RANSAC to detect and isolate vision faults

• Estimated protection levels by applying worst-case failure slope analysis to Graph-SLAM framework

• Using real-world and simulated experiments, validated the performance of SLAM-based IM▪ Successful detection and isolation of multiple faults

▪ Validated higher localization accuracy of the vehicle

▪ Achieved tight protection levels associated with the vehicle position

Summary

23

Special thanks to Siddharth Tanwar, Matt Peretic and Shubhendra Chauhan for helping with the ground vehicle experiments

Acknowledgements

24

Sriramya Bhamidipati

Ph.D. student

Department of Aerospace Engineering

25

Thank you!

SLAM-based Integrity Monitoring for Multi-Sensors and Multi...

Documents

Transcript of SLAM-based Integrity Monitoring for Multi-Sensors and Multi...