Carrier Phase GPS Augmentation Using Laser Scanners and Using Low Earth Orbiting Satellites Colloquium on Satellite Navigation at TU München Mathieu Joerger December 15 th 2009 1
Navigation using Carrier Phase GPS Carrier Phase GPS has the potential to enable a variety of applications demanding high-accuracy and high-integrity. Limitations will be addressed in this work using ranging augmentation systems Precision Approach of Aircraft Shipboard Relative GPS Autonomous Airborne Refueling 2
GPS Ranging Measurements Code phase measurements provide an instantaneous, unambiguous measure of range with a receiver tracking error of approximately 1m. s e x s L, k k k k, k e x N s s s L, k k k k, k Carrier phase measurements Code (ρ) Carrier ( ) have a tracking error of about 1cm can only be tracked by receivers mod(2π) Local Frame user - s e k x k, k 3
Cycle Ambiguity Resolution Measurement redundancy (instantaneous) five or more ranging sources are needed to provide observability on cycle ambiguities SV motion (over time) provides additional observability over time e x t t t t N t L s s s 1 1 1 1 1 e x t t t t N t L s s s 2 2 2 2 2 t t 1 2 1. Slow GPS L.O.S. Changes reliable cycle ambiguity estimation over short time periods is challenging 4
Land Navigation in GPS-Obstructed Areas GPS signals are easily attenuated or blocked by buildings, trees or rugged terrain. 2. Weak GPS signal power Fewer measurements The process of estimating cycle ambiguity biases N is reset if the line-of-sight is obstructed. Code (ρ) Carrier ( ) 5
Measurement Error Sources Measurement error sources (RMS error: 3-10m) Satellite clock and orbit ephemeris Ionosphere and troposphere Multipath and receiver noise e x E I T s s u s s s L, k k k k k k k, k e x N E I T s s s s s s L, k k k k k k k, k 6
Differential GPS Measurement error sources (RMS error: 3-10m) Satellite clock and orbit ephemeris Ionosphere and troposphere Multipath and receiver noise ~ 1 m () / ~ 1cm () s s ek xk k, k e x N s s s s k k k, k 3. Local reference station Robust carrier phase GPS requires corrections from a local reference station. Reference station 7
Outline Limitations of Carrier Phase GPS weak GPS signal power challenging cycle ambiguity estimation need for corrections from a local reference station GPS Augmentation Using Laser Scanners for navigation in GPS-obstructed environments GPS Augmentation Using Low Earth Orbit (LEO) Satellites in a system named igps for fast floating cycle ambiguity estimation and global high-integrity positioning (igps slides not included in this document) 8
Laser-Augmented CPDGPS 9
CPDGPS Testing Platform Prior Work on AGVs Can we use obstacles (static) as landmarks? Laser for Obstacle Detection Simultaneous Localization And Mapping: SLAM (1990 s) vehicle positioning without prior knowledge of obstacle location originally for indoor applications, typically used with deadreckoning sensors MIT: Leonard, J., and H. Durrant-Whyte. Directed Sonar Sensing for Mobile Robot Navigation. Cambridge, MA: Kluwer Academic Publishers (1992). University of Sydney: Dissanayake, G., P. Newman, S. Clark, H. Durrant-Whyte, and M. Csorba. A Solution to the Simultaneous Localization and Map Building (SLAM) Problem. IEEE Transactions on Robotics Automation. 17.3 (2001) 10
Motivation and Objective 50 30 C/No for PRN 9 (db-hz) Time CPDGPS LASER SLAM UNIFIED NAVIGATION SOLUTION Range-Domain Integration of GPS and Laser Measurements 11
Outline for Laser-Augmented CPDGPS LASER-based Simultaneous Localization And Mapping measurement-level integration algorithm and analysis Experimental testing in urban canyons 12
LASER-Based SLAM 13
Raw Laser Data Processing Laser emitter/receiver Rotating mirror Angle Laser range-limit Obstacle Range RAW Select and Assign Measurements To Extended Kalman Filter (EKF) 14
15 Feature Extraction A B a d a b d b 1 2 1 1 Y X a a p p d 2 2 Y X b b p p d 1 A B 2 To the EKF Data Association a d a b d b A B Feature Extraction Vehicle Tree trunk Noise RAW DATA RAW To EKF Select Data Association Feature Extraction Assign Vehicle (2 back-to-back scanners) Tree trunk Noise
16 Feature Extraction A B a d a b d b 1 2 1 1 Y X a a p p d 2 2 Y X b b p p d 1 A B 2 To the EKF Data Association a d a b d b A B Feature Extraction RAW To EKF Here feature = center of tree trunk Select Data Association Feature Extraction Assign
Data Association Data Association (over consecutive epochs) 1 A B 2 Meas. d A 1 B 2 a a p p b d p b p Feat. 1 X 1 Y 2 X 2 Y Meas. are associated with landmarks (with the EKF s predicted position estimate) using a nearest neighbor approach Select Feature Extraction Assign Data Association State prediction RAW To EKF Matched observations 17
North i p N x N 2-D Model and Estimation Process d i i i i p x N N arctan i p x E E i i 2 i 2 E E N N d d p x p x x E i p E East From literature review and analysis : SLAM provides positioning relative to an initial estimate. The SLAM position error drifts with distance. (compared to CPDGPS which provides absolute position). 18
Measurement-Level Integration 19
GPS/Laser Integration Derived a measurement differencing EKF, which is a unified and compact filter capable of processing: GPS LASER time-correlated carrier phase GPS measurements non-linear laser measurements Select Data Feature Extraction SLAM i, i d Assign Data Data Association ESTIMATION Measurement- Differencing Extended KF j State Prediction p, E j p N POSITION and orientation 20
GPS/Laser Integration Linearized measurement equation: x n S ρ G 0 1 0 0 ε ρ n S ns φ G 0 1 I 0 ε = + φ ν d νθ d F d, x 0 0 0 F d, p L N n θ F k θ, x -1 0 0 Fθ, p p k Process equation: Block matrices k xv ΦV 0 0 xv uv wv N = 0 In 0 S N + 0 + 0 p 0 0 I p 0 0 L k+1 n k k k u x = x T V V N 0 P 0 V T 0 0 0 T T Illustrate the performance in forest scenario 21
Example Forest Scenario Direct simulation: trees modeled as vertical cylinders tree canopy modeled as a horizontal plane no GPS signals from low elevation satellites inside the forest (they would be altered by multipath) Model for the Forest 22
Time: 11 s 60 GPS Only Laser-based SLAM 50 GPS and LASER GPS 40 Simulation North (m) -30 Forest LASER Only - Street 2 Testing 20 10 4 Forest Scenario: Direct Simulation Vehicle and Landmarks Position Estimation 6 Vehicle Laser Scanner Range 0-30 Limit-20-10 0 10 20 30 East (m) 5 3 8 7 1 Trees Covariance Ellipses (x70) North (m) W 25 20 15 10 0-5 300 330 210 N GPS Satellite Blockage due to the Forest 30 60 60 30 0 E 240 120 S 150 Simulated Laser Data, Extraction and 5 Association -15-10 -5 0 5 10 15 East (m) 23
60 Forest Scenario: Direct Simulation Time: 11 s GPS Satellite Blockage due to the Forest N 330 30 300 60 50 W 60 30 0 E North (m) 40 30 20 10 4 Vehicle and Landmarks 6 Position Estimation 2 5 0-30 -20-10 0 10 20 30 East (m) 3 8 7 1 North (m) 25 20 15 10 0-5 240 120 210 S 150 clear blocked Simulated Laser Data, Extraction and 5 Association -15-10 -5 0 5 10 15 East (m) 24
Forest Scenario: Direct Simulation N 60 Time: 11 s 300 330 30 60 50 W GPS Satellite Blockage due to the Forest 60 30 0 E North (m) 40 30 20 4 3 Vehicle and Landmarks 6 Position Estimation 2 5 8 7 1 240 120 210 150 S 25 20 15 10 North (m) 10 5 Simulated Laser Data, Extraction and Association 0-30 -20-10 0 10 20 30 East (m) 0-5 raw (noise) filtered extracted associated -15-10 -5 0 5 10 15 East (m) 25
Forest Scenario: Direct Simulation 26
Forest Scenario: Direct Simulation In summary, the measurement domain integration achieves seamless navigation through GPS-denied areas: extends availability of absolute precise position fixes uses laser data for fast cycle ambiguity re-estimation at the exit (avoids restarting the process from scratch) Overall the system enables the storage and transmission of absolute position information. The transitional (green) area is further investigated in urban canyons. 27
Experimental Testing in Urban Canyons 28
Range-Domain Integration Position-domain Integration Position CPDGPS Algorithm Laser Position Range-domain Integration Output Position CPDGPS Laser Measurement Algorithm Measurement Output Position < 3 SV (2D position) > 1 SV (Rx clock) 29
Testing in a Structured Environment Landmarks are clearly identifiable and sparsely distributed, meaning that they can robustly be extracted and associated Fault-Free case 30
Testing in a Structured Environment Simulated satellite blockage 10m wide street, 50m high buildings (truth trajectory computed using CPDGPS) Time-tagged GPS and laser measurements for synchronization 31
East (m) 0.5 0 0.5 Fault-Free Positioning Result Range-domain Integration -0.5 Actual data 550 560 570 580 590 600 (estimated-truth) 610 Time (s) Position-domain Integration < 3 satellites Covariance envelopes East (m) 0-0.5 550 560 570 580 590 600 610 Time (s) 32
Experimental Setup 33
Natural Environment Noisy laser scans cause: poor SLAM performance. Sky blockage causes: poor GPS performance. The goal is to quantify the combined performance. 34
True Trajectory and Landmark Location 160 140 120 100 80 60 40 20 0 Missed association estimated landmark location range domain position domain -20-60 -40-20 0 20 40 60 80 100 35
Experimental Results East (m) 1 0-1 Range-domain Integration < 3 satellites 50 100 150 200 250 300 Time (s) Position-domain Integration 1 < 3 sat East (m) 0-1 Missed 50 100 150 200 250 300 association Time (s) 36
for CPDGPS/Laser LASER extends the availability of precise CPDGPS position fixes (forest scenario). With the range-domain integration, additional GPS ranging measurements: - contribute to the estimation process (Fault-Free) - Increase the robustness of SLAM (by reducing occurrences of Missed Associations) Future work includes the implementation of more elaborate feature extraction and data association procedures. 37
Low Earth Orbiting (LEO) Satellite- Augmented GPS (slides not included) 38
39
Summary We have addressed limitations of carrier phase GPS by exploiting complementary properties of: GPS Open-sky areas Absolute range Laser Scanner Presence of obstacles Relative range Extended availability of precise positioning Increased robustness of SLAM (data association) GPS Worldwide coverage of 4 SVs continuous tracking of >4 signals over time Iridium Worldwide coverage of 1 SV Fast satellite motion Wide area corrections and measurement error models Redundancy for cycle estimation & fault detection Fast cycle ambiguity estimation At continental scale 40
Current and Future Work Investigate integrity of data association in SLAM overall probability of missed associations algorithm for detection of missed associations Refine measurement error and fault modeling Ionospheric delay (low elevation), ionospheric anomalies Iridium satellite orbit ephemeris errors Derive new fault-detection algorithms theoretical worst-case faults extended-window algorithm to avoid poor Iridium geometries Explore high-integrity multi-constellation navigation systems detection of multiple simultaneous faults fault identification and exclusion 41
Acknowledgements Sponsors The Boeing Company and the Naval Research Laboratory Advisor Dr. Boris Pervan NavLab Members Fang C. Chan, Livio Gratton, Moon B. Heo, Jing Jing, Bartosz Kempny, Samer Khanafseh, Steven Langel, Jason Neale, 42
END 43