3D LiDAR SLAM Integration with GPS/INS for

UAVs in Urban GPS-Degraded Environments

Sebastian Hening1∗, Corey Ippolito2†, Kalmanje Krishnakumar2‡,

Vahram Stepanyan2§

, Mircea Teodorescu1¶

1University of California Santa Cruz, Santa Cruz, CA, 95064

2NASA Ames Research Center, Moffett Field, CA, 94035

This paper presents a data fusion algorithm, using an Adaptive Extended Kalman filter

(AFK) for estimation of velocity and position of a UAV. A LIDAR sensor provides local

position updates using a SLAM technique, a GPS provides corrections when available

and an Inertial Navigation System (INS) is used as an additional input to the Extended

Kalman filter. We adapt the measurement noise covariance (R) of the AKF based on

both the Global Positioning System (GPS) receiver error as well as on the LiDAR point

cloud point-to-point match error. A simulation environment was developed to test the

proposed SLAM as well as navigation (e.g., autopilot) algorithms in a virtual, but accurate

environment. We show that by adapting the measurement noise covariance (R) of the AKF

we improve both the accuracy and reliability of the position estimate, specially in areas

with GPS signal drop outs such as urban canyon environments.

I. Introduction

Modern aircraft systems are becoming increasingly dependent on satellite-based Global Positioning System (GPS) services for precision navigation and timing (PNT) as well as for guidance, navigation and control.

Unfortunately, in urban environments GPS signals are extremely weak, un-precisse and highly susceptible

to interference (both intentional and unintentional, from both natural and man-made sources). Therefore,

GPS-based navigation algorithms for Unmanned Aircraft Systems (UAS) are even more susceptible to GPS

vulnerabilities than manned aircraft. Precise Location and Navigation was identified as a technical challenge

in the JPDO NextGen UAS develop11 ment roadmap, citing that the strong dependence of UAS on GPS

technologies creates a potential single-point of failure, and the lack of an onboard human pilot and lack of

situational awareness places human life and infrastructure at risk. Further, in the growing need to perform

low-altitude UAS operations, a significant number of new environmental hazards and uncertainties are imposed on the unmanned aircraft system that limits or negates the ability for UAS to operate autonomously.

For these emerging scenarios, GPS-based localization is not sufficient. GPS-free localization, mapping and

avoidance are crucial missing technologies that limit the use of autonomous UAS in urban low-altitude environments.

Simultaneous localization and mapping (SLAM) was first introduced in Smith and Cheeseman.6 It is a

technique allowing a autonomous vehicle to simultaneously build a map of a previously unknown environment

while also localizing itself within this map. SLAM based approaches have been very successful for ground

robotics. When multiple sensors such as a GPS/INS and wheel encoders are available concurrently, the

problem is usually solved through an extended Kalman filter Montemerlo et al.9 or a particle filter Thurn et

al.13Aerial platforms however have more challenges to overcome which makes traditional SLAM approaches

harder to implement. These are described in Achtelik et al.,1 Bachrach et al.4 as:

∗Advanced Control and Evolvable Systems Group, sebastian.hening@nasa.gov

†Advanced Control and Evolvable Systems Group, corey.a.ippolito@nasa.gov

‡Advanced Control and Evolvable Systems Group, kalmanje.krishnakumar@nasa.gov

§Advanced Control and Evolvable Systems Group, vahram.stepanyan@nasa.gov

¶Department of Computer Engineering, mteodore@ucsc.edu, AIAA member

1 of 10

American Institute of Aeronautics and Astronautics

Downloaded by NASA AMES RESEARCH CENTER on February 15, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2017-0448 

 AIAA Information Systems-AIAA Infotech @ Aerospace 

 9 - 13 January 2017, Grapevine, Texas 

 AIAA 2017-0448 

 Copyright © 2017 by the American Institute of Aeronautics and Astronautics, Inc. All rights reserved. 

 AIAA SciTech Forum 
Downloaded by NASA AMES RESEARCH CENTER on February 15, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2017-0448 

1. Limited payload reduce the onboard processing and sensing capabilities

2. Indirect odometry compared to direct (wheel encoding) in ground robotics

3. Indirect position estimates without GPS as well as rapid accumulation of error from integration of

inertial sensors

4. Fast dynamics

5. Need to estimate and control velocity

6. Constant motion

The purpose of our research is to explore accurate navigation solutions for UAVs in urban environments

by taking information from multiple different kinds of sensors in order improve the quality of position and

velocity estimation. In order to do so, the use of a KF is explored in a loosely coupled scheme for fusing the

information obtained from GPS, INS and LIDAR sensor.

II. Related Work

One of the earliest papers describing localization for aerial platforms in known mapped environments

is He et al8 in which a quadrotor helicopter with the use of a monocular camera is able to autonomously

navigate a previously mapped indoor environment. Angeletti et al.2 used a similar quadrotor helicopter

to match scans from a 2D lidar to a known map and Gorzonka et al7 used a particle filtering method

to localize a similar vehicle in a map built by a ground robot. Soloviev et al12 presented a method for

integrating a laser scanner with INS for navigation in GPS-denied urban environments. This used a 2-D

laser scanner detect features from reflecting objects in range of the scanner. Line features are extracted from

the scan images and exploited for navigation, as lines are computationally efficient to extract, are common

in man-made environments, and are repeatable. Bachrach et al34 implemented 2D lidar based SLAM on

a quadrotor micro air vehicle (MAV). This approach separates the SLAM process, which provides lower

frequency position updates, from the real-time navigation and control system. Two key differences from our

implementation are that the SLAM algorithm is run on the ground station computer and it is only done in

2D.

Typical methods for 3D map building from onboard LiDAR use off-line batch methods, often using

loop closure to correct for drift over time. A method for real-time odometry and mapping using range

measurements from a LiDAR is described in Zhan et al.14 A similar approach of splitting the simultaneous

localization and mapping in two algorithms is used. One algorithm performs lidar odometry at a high

frequency but low fidelity to estimate velocity of the lidar. Another algorithm runs at a frequency of an

order of magnitude lower for fine matching and registration of the point cloud. Because the method in

Zhan et al.14 achieves both low-drift and low-computational complexity without the need for high accuracy

ranging or inertial measurements our implementation uses a modified version of it for performing SLAM on

a aerial vehicle in GPS degraded urban environments.

III. Problem Statement

The problem addressed in this paper is to use data fusion between a LiDAR sensor, GPS and IMU to

improve the velocity and position estimates of a UAV. LiDAR based odometry and scan-to-scan matching

SLAM techniques are inherently prone to drift and have an unbounded error. Additionally, the LiDAR

has to be within detection range of physical features in order to perform ego-motion estimation on the

point cloud. Unlike LiDAR SLAM techniques, GPS sensors do not drift over time, however they suffer

from signal obstruction and reflections caused by nearby objects. Although prone to these errors in certain

circumstances, even low-cost GPS systems are able to correct the drift and limit the effects of LiDAR SLAM

error during periods of accurate GPS updates. The GPS and LiDAR SLAM techniques are complementary

in terms of their respective weaknesses and together can provide a much more accurate navigation solution.

2 of 10

American Institute of Aeronautics and Astronautics
Downloaded by NASA AMES RESEARCH CENTER on February 15, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2017-0448 

IV. System Overview

The algorithm in this paper is validated using LiDAR, GPS and Inertial Navigation System (INS) data

from a simulation environment as well as by conducting a series of outdoor test flights using our custom

UAV platform.

A. Simulation

The simulation environment is being developed at NASA Ames Research Center for research in guidance,

navigation, and control systems. It provides a fully documented hierarchical control system architecture,

inspired by commercial transport aircraft autopilots. The simulated aircraft shown in Figure 1 uses the

same simulated avionics and payload sensors as the real aircraft. The simulated payload sensors consist of

a Velodyne VLP-16 3D lidar, an array of cameras and a downward facing sonar sensor. The simulated 3D

lidar has a 360 degree horizontal field of view and a 30◦ vertical field of view with 16 channels and a 0.25◦

resolution.

(a) (b)

Figure 1. (a) S1000 octocopter (b) Sensor configuration of the Velodyne VLP-16 lidar and Athena 111m

GPS/INS/ADHRS

B. Hardware Platform

The aircraft uses a DJI S1000 octocopter airframe shown in Figure 2 (a) with a open source Pixhawk

autopilot. The vehicle is equipped with a 360◦ Velodyne VLP-16 3D lidar, a high accuracy Xsens Mi30 IMU

and GPS system. The onboard computer is a Core i7 quad core computer with 8 Gib of memory. It will

at first be used for sensor data collection to validate our models with the final goal of on-board real-time

SLAM in GPS degraded environments.

3 of 10

American Institute of Aeronautics and Astronautics
Downloaded by NASA AMES RESEARCH CENTER on February 15, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2017-0448 

(a) (b)

Figure 2. (a) S1000 octocopter at NASA Ames (b) Sensor configuration of the Velodyne VLP-16 lidar and

Athena 111m GPS/INS/ADHRS

Airframe Make/Model DJI S1000 + DJI S1000 +

Configuration Octocopter

Weight 4.2 kg

Payload Weight 2.12 kg

Motor Power 500 W

Motor RPM 9600 RPM

Max Ground Speed 13.4 m/s (30 mph)

Battery 6S 16000 mAh LiPo

Flight Time 22 minutes (est)

Table 1. Flight vehicle specifications

V. Approach

The conventional Extended Kalman filter is widely used for state estimation however it requires an

accurate priori knowledge of the process (Q) and measurement noise (R) covariances. As the noise of both

the GPS receiver and the LiDAR SLAM varies drastically throughout a flight based on the environment, our

implementation uses an Adaptive Kalman Filter (AKF). Figure 3 shows the schematic of the implementation.

The LiDAR and GPS data is logged at 5 Hz while the IMU data is recorded at 1000 Hz. The laser points  ̃ received from one full scan is denoted as S and the underscript k is used to denote each individual scan

cloud. The fusion of the GPS, LiDAR SLAM and INS information is combined by an Adaptive Kalman

Filter (AKF) to provide accurate state estimates of the position and velocity.

When LiDAR and GPS data is not available only IMU measurements are processed and when a new

LiDAR frame is available, the measurement equations are changed to incorporate the new measurement into

the kalman filter. This approach provides two main benefits. Firstly, it increases the accuracy of the position

and velocity estimates. Secondly, it enables the system to provide updates at a much higher frequency than

the LiDAR’s 5 hz which enables the autopilot to control velocity and position of the aircraft accurately in

crowded environments.

4 of 10

American Institute of Aeronautics and Astronautics
Downloaded by NASA AMES RESEARCH CENTER on February 15, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2017-0448 

Figure 3. Schematic of LiDAR SLAM, GPS and IMU integration

Feature points are extracted from the registered LiDAR point cloud Sk and matched using Iterative

Closest Point (ICP)10 to the previous point cloud Sk−1. The mapping block algorithm generates the map

as well as a position estimate. The position output of the EKF is used as initial position guess for the ICP

algorithm in order to converge to a solution faster.

Figure 4. Ground station interface (left), path traveled by aircraft (center), aircraft navigating narrow urban

street in Indianapolis, IN (right)

A. Adaptive Extended Kalman Filter

The Kalman filter is a optimal state estimator of a discrete time linear dynamic system perturbed by white

noise Brown et al.5 where x is the state vector, z the observation vector and the index k denotes the time

index. We use a slightly modified version of the extended Kalman filter Brown et al.5 for data fusion

xk = Φk−1 + ωk−1

zk = Hkxk + vk

The observation vector zk of the extended Kalman filter is the position difference between the INS derived

position PINS and GPS position PGP S as well as INS derived position and LIDAR derived position.

" #

PINSk − PGP Sk zk = PINSk − PLIDARk

The state transition and measurement model matrices are represented by Φ and H while the measurement

noise v is assumed Gaussian with measurement noise covariance matrix R.

5 of 10

American Institute of Aeronautics and Astronautics
Downloaded by NASA AMES RESEARCH CENTER on February 15, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2017-0448 

ω ∼ N(0, Q)

v ∼ N(0, R)

Q = process noise covariance matrix

R = measurement noise covariance

Algorithm for Kalman Filter

1: xˆ− = Φk−1xˆ+

k k−1

2: P − = Φk−1P + ΦT

k k−1 k−1 + Qk−1

3: Compute Rk based on GPS noise and LIDAR point-to-point match error

4: Kk = P −HT (HkP −HT + Rk)−1

k k k k

5: Formulate zk

+ − − 6: xˆ = xˆ + Kk(zk − Hkxˆ ) k k k

7: P + = (I − KkHk)P −(I − KkHk)T + KkRkKT

k k k

xˆk

− = a priori state vector

+ xˆk = a posteriori state vector

P − = a priori state error covariance matrix k

P + = a posteriori state error covariance matrix

Φ =process model matrix, state transition

H =measurement model matrix

zk =measurement vector

Kk = Kalman gain

B. Adapting measurement noise matrix R

The measurement noise covariance matrix R is computed using both the LIDAR point-to-point scan match

error as well as the dilution of precision (HDOP and VDOP) reported by the GPS.

⎡ ⎤

R =

⎢

⎢

⎢

⎢

⎢

⎢

⎢

⎢

⎣

σ2

xyk 0 0 0 0 0

0 σ2

xyk 0 0 0 0

0 0 σ2

zk 0 0 0

0 0 0 LRk 0 0

0 0 0 0 LRk 0

0 0 0 0 0 LRk

⎥

⎥

⎥

⎥

⎥

⎥

⎥

⎥

⎦

σxyk = HDOPk x σr

σzk = V DOPk x σr

LRk = ICPerror + (Nmax − Nk)

σxyk = Horizontal position accuracy for time k

σzk = Vertial position accuracy for time k

Nk = Number of features detected in the LIDAR scan k

Sk = LIDAR scan k

Our LiDAR SLAM algorithm uses the best Nmax features detected to match two sequential scans Sk and

Sk−1 and determine the egomotion from them. The noise covariance of the LIDAR depends on the rootmean-square match error ICPerror between scans Sk and Sk−1 as well as based on the number of features

detected in the scans. The more precise the scan matching is and the more feature the LIDAR sees, the

more accurate the position estimate is.

VI. Results

This section presents our experimental evaluation of the proposed framework. Due to safety requirements,

all flight tests are conducted with a tether attached to the aircraft and the state estimation from our filter

is not used as input into the flight controller yet.

6 of 10

American Institute of Aeronautics and Astronautics
Downloaded by NASA AMES RESEARCH CENTER on February 15, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2017-0448 

A. Flight Tests

In order to analyse the performance of the LiDAR SLAM in different conditions, we use two test sites with

different characteristics. The first test site is in close proximity of a large wind tunnel and high voltage

power lines Figure 5(b), similar to an urban environment with high-rise buildings and the GPS problems

associated with them. The second test site is a imitation of a lunar surface Figure 5(a). This site offers very

few features for the LiDAR to reflect off and thus will test our SLAM system.

-

(a) (b)

Figure 5. (a) Roverscape test flight site at NASA Ames (b) Wind tunnel test flight site at NASA Ames

The vehicle position estimate from GPS (Red), LiDAR SLAM (Yellow) and adaptive EKF (Green) is

shown in Figure 6(a). The true start and end location were also recorded in order to calculate the position

drift. The GPS and LiDAR SLAM position outputs are very similar for the first half of the test after which

the LiDAR experiences some drift due to a drop in features being detected inside the empty grass field in

close proximity. This can also be seen in Figure 6(b) by looking at the LiDAR noise covariance R. The

GPS provides reliable position updates until the vehicle is within 40 meters of the wind tunnel building after

which, due to multipath and signal obstruction, the GPS becomes very unreliable. The R measurement

noise covariance over time is shown in Figure 6(b) with the X,Y and Z components of the GPS. After a

total distance of 405 m traveled, the final position error of the GPS, LiDAR and AKF was calculated and is

shown in Table 2 . By adjusting R online our filter is able to take advantage of the complementary nature

of the GPS and LiDAR sensors and reduce the 24.3 m GPS and 7.5 m LiDAR SLAM position error to 3.42

meters.

(a) (b)

Figure 6. (a) LIDAR (Blue), GPS (Red) and adaptive EKF (Green) position estimates and (b) R Measurement

noise covariance matrix values over time for LIDAR and GPS in Obstructed GPS case

7 of 10

American Institute of Aeronautics and Astronautics
Downloaded by NASA AMES RESEARCH CENTER on February 15, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2017-0448 

Drift Error %

GPS 24.3m 6%

LiDAR 7.5m 1.8%

AEKF 3.42m 0.84%

Table 2. Final position drift after 405 meters

The second test is done with the UAV taking off and flying a box pattern over a flat moon like surface.

The GPS position is show in Figure 7(a) and the LiDAR positon and point cloud is shown on the right.

Both them are shown together in Figure 8(a) and their corresponding R measurement noise covariance is

shown in Figure 8(b). In this open area the position reported by the GPS is much more accurate than the

LiDAR SLAM algorithm. The small number of features seen by the LiDAR causes the ICP scan matching

to be imprecise leading to a large increase in the R measurement noise covariance seen in Figure 8(b).

(a) (b)

Figure 7. (a) GPS position estimate and (b) LiDAR SLAM position estimate with corresponding point cloud

map

(a) (b)

Figure 8. (a) LIDAR (Yellow), GPS (Red) and AKF (Green) position estimates and (b) R Measurement noise

covariance matrix values over time for LIDAR and GPS for Minimal LIDAR returns case

VII. Conclusion

The results show that by fusing GPS data with a LIDAR in addition to an IMU, the state estimation of

a UAV can be made more reliable in low altitude urban environment flights where GPS signals are degraded.

Moreover, we propose a method to adapt the noise covariance matrix R online using both the dilution of

precision (DOP) of the GPS and the LiDAR detected features count together with the scan match error.

The results presented in this paper are our preliminary work, in the future we would like to explore adapting

online both the process noise matrix Q and the noise covariance matrix as well as add a DGPS.

8 of 10

American Institute of Aeronautics and Astronautics
Downloaded by NASA AMES RESEARCH CENTER on February 15, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2017-0448 

Acknowledgments

This work was supported by the NASA Ames SAFE50 Center Innovation Fund (CIF) project and the

UAS Traffic Management (UTM) Sub-project, under the NASAs Safe Autonomous Systems Operations

(SASO) Project. This research is also partly funded by a NASA-DHS inter-agency agreement. The authors

gratefully acknowledge all members of the SAFE50 team for engaging in many hours of discussions on various

topics discussed in this paper.

9 of 10

American Institute of Aeronautics and Astronautics
Downloaded by NASA AMES RESEARCH CENTER on February 15, 2017 | http://arc.aiaa.org | DOI: 10.2514/6.2017-0448 

References

1Achtelik, A. B., He, R., Prentice, S., and Roy, N. Autonomous navigation and exploration of a quadrotor helicopter

in gps-denied indoor environments. 2009. In IARC First Symposium on Indoor Flight Issues. 2Angeletti, G., Valente, J., Iocchi, L., and Nardi, D. Autonomous indoor hovering with a quadrotor. In Workshop

Proc. SIMPAR (2008), pp. 472–481. 3Bachrach, A., He, R., and Roy, N. Autonomous flight in unknown indoor environments. International Journal of

Micro Air Vehicles 1, 4 (2009), 217–228. 4Bachrach, A., Prentice, S., He, R., and Roy, N. Range–robust autonomous navigation in gps-denied environments.

Journal of Field Robotics 28, 5 (2011), 644–666. 5Brown, R. G., and Hwang, P. Y. Introduction to random signals and applied kalman filtering: with matlab exercises

and solutions. Introduction to random signals and applied Kalman filtering: with MATLAB exercises and solutions, by Brown,

Robert Grover.; Hwang, Patrick YC New York: Wiley, c1997. 1 (1997). 6Cheeseman, P., Smith, R., and Self, M. A stochastic map for uncertain spatial relationships. In 4th International

Symposium on Robotic Research (1987), pp. 467–474. 7Grzonka, S., Grisetti, G., and Burgard, W. Towards a navigation system for autonomous indoor flying. In Robotics

and Automation, 2009. ICRA’09. IEEE International Conference on (2009), IEEE, pp. 2878–2883. 8He, R., Prentice, S., and Roy, N. Planning in information space for a quadrotor helicopter in a gps-denied environment.

In Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on (2008), IEEE, pp. 1814–1820. 9Montemerlo, M., Thrun, S., Koller, D., Wegbreit, B., et al. Fastslam: A factored solution to the simultaneous

localization and mapping problem. In AAAI/IAAI (2002), pp. 593–598. 10Rusinkiewicz, S., and Levoy, M. Efficient variants of the icp algorithm. In 3-D Digital Imaging and Modeling, 2001.

Proceedings. Third International Conference on (2001), IEEE, pp. 145–152. 11Shah, Y. Joint planning and development office (jpdo) unmanned aircraft systems (uas). In Integrated Communications,

Navigation and Surveillance Conference (ICNS), 2013 (2013), pp. 1–8. 12Soloviev, A. Tight coupling of gps, laser scanner, and inertial measurements for navigation in urban environments. In

Position, Location and Navigation Symposium, 2008 IEEE/ION (2008), IEEE, pp. 511–525. 13Thrun, S., Burgard, W., and Fox, D. Probabilistic robotics. MIT press, 2005. 14Zhang, J., and Singh, S. Loam: Lidar odometry and mapping in real-time. In Robotics: Science and Systems Conference

(RSS) (2014).

10 of 10

American Institute of Aeronautics and Astronautics