SLAM-based Integrity Monitoring for Multi-Sensors and Multi-Receivers SRIRAMYA “RAMYA” BHAMIDIPATI, UNIVERSITY OF ILLINOIS AT URBANA-CHAMPAIGN GRACE XINGXIN GAO, STANFORD UNIVERSITY SCPNT Annual Symposium 2019, Palo Alto, CA
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
IM for Graph-SLAM: Challenges and Solutions
8
[11] Joerger, et.al., Navigation 2014
Graph-SLAMWorst-Case Failure
Slope [11]Our Solution
Non-linearity in the cost function
Derived for linear measurement model
Linearize the graph optimization framework at
estimated overall state vector
IM for Graph-SLAM: Challenges and Solutions
8
[11] Joerger, et.al., Navigation 2014
Graph-SLAMWorst-Case Failure
Slope [11]Our Solution
Non-linearity in the cost function
Derived for linear measurement model
Linearize the graph optimization framework at
estimated overall state vector
It is a sequential localization technique
Falls under snapshot integrity method
Linearize over the past time history of measurements
Graph-SLAMWorst-Case Failure
Slope [11]Our Solution
Non-linearity in the cost function
Derived for linear measurement model
Linearize the graph optimization framework at
estimated overall state vector
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
IM for Graph-SLAM: Challenges and Solutions
8
[11] Joerger, et.al., Navigation 2014
• 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
Multiple FDI: Vision Faults
10
2. Estimate the fitted line via RANSAC [13]; compute outlier fraction 𝑂𝐹𝑘
Exp
ecte
d in
ten
sity
Received intensity
1. Segment to 𝐿 superpixels [12]
At each non-sky superpixel
Steps of our superpixel-based piecewise RANSAC
Outliers
[12] Li, et.al., IEEE CVVR 2015
[13] Conte and Doherty, EURASIP, 2009
Multiple FDI: Vision Faults
10
2. Estimate the fitted line via RANSAC [13]; compute outlier fraction 𝑂𝐹𝑘
Exp
ecte
d in
ten
sity
Received intensity
3. Compute Outlier Fraction (OF) at other superpixels
𝑂𝐹1𝑘 𝑂𝐹𝑗
𝑘 𝑂𝐹𝐿−1𝑘
1. Segment to 𝐿 superpixels [12]
At each non-sky superpixel
Estimated fitted line
Steps of our superpixel-based piecewise RANSAC
⋯
Outliers
[12] Li, et.al., IEEE CVVR 2015
[13] Conte and Doherty, EURASIP, 2009
Multiple FDI: Vision Faults
10
Exp
ecte
d in
ten
sity
Received intensity
3. Compute Outlier Fraction (OF) at other superpixels
𝑂𝐹1𝑘 𝑂𝐹𝑗
𝑘
1. Segment to 𝐿 superpixels [12]
At each non-sky superpixel
Steps of our superpixel-based piecewise RANSAC
Fault status of each superpixel is product of
all outlier fractions⋯
Estimated fitted line
Outliers
𝑂𝐹𝐿−1𝑘
2. Estimate the fitted line via RANSAC [13]; compute outlier fraction 𝑂𝐹𝑘
𝝀
ഥ𝑚𝑤𝑐
Under faults
Pre-defined threshold
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
Measurement residual
Worst-case failure slope
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
Worst-case failure slope
𝝀
ഥ𝑚𝑤𝑐
Under faults
Pre-defined threshold
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
Measurement residual
𝑷𝑳 = ഥ𝒎𝒘𝒄𝟐 𝝀
ഥ𝒎𝒘𝒄𝟐 : 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
[11] Joerger, et.al., Navigation 2014
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
[11] Joerger, et.al., Navigation 2014
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!