2018 Latin American Robotic Symposium, 2018 Brazilian Symposium on Robotics (SBR) and 2018 Workshop on

Robotics in Education (WRE)

Visual-Inertial Fusion for Indoor Autonomous Navigation of a Quadrotor

Using ORB-SLAM
Seyed Jamal Haddadi, Eugenio B. Castelan
Department of Automation and Systems, Federal University of Santa Catarina, Florianopolis, Brazil
*
Corresponding Author Email: [email protected]

Abstract— This brief addresses the accuracy of the pose positions and velocities, an Extended Kalman Filter (EKF) has
estimation of a Low-Cost Quadrotor using visual SLAM and been employed for sensor fusion that fuses the measured
sensor data fusion for making an autonomous indoor navigation navigation data with the obtained pose from the monocular
in a known GPS-denied environment. To perform this approach, camera. Also, a fuzzy visual servoing approach for controlling
two kinds of modules are employed. For the former, a a Quadrotor is presented in [7], which allows performing take-
Simultaneous Localization and Mapping (SLAM) system is used.
This SLAM system uses Oriented Fast and rotated BRIEF (ORB)
off, hovering, and landing without a marker. In their method,
features, also known as ORB_SLAM. The second module is the forward-facing camera is used to obtain visual pose
Extended Kalman Filter (EKF) that is applied for combining the estimates using Parallel Tracking and Mapping (PTAM) based
obtained pose from ORB_SLAM and Inertial Measurement Unit on SLAM.
(IMU). Since in this paper the SLAM process is performed by the Besides the pose estimation, estimating map scale factor and
frontal monocular camera of Quadrotor, so developing scale horizontal drifting elimination are other challenges that have
factor estimation in order to calculate the scale of the map with been explained in [8]. Concerned on the same subject, J. Engle
minimum error is necessary. In addition, a Proportional Integral et al in [9] presented an autonomous visual navigation of a
Derivative (PID) is utilized to conduct the maneuver of the robot. commercial quadrotor using PTAM. In this paper, they used
All of these processes are done by sending data via Wi-Fi to a
ground station in order to perform the data processing. To show
absolute scale factor estimation of generated map and EKF for
the performance of the proposed algorithm, two experiments are sensor fusion in order to have accurate visual odometry and
carried out on an AR-Drone 2.0 quadrotor. The first one is elimination of falsely tracked frames.
position holding that shows the robustness of the estimated The main objectives of this study are: firstly, to improve the
position of the system in hovering situation after take-off. The pose estimation accuracy of Quadrotor and to make an
second one is an indoor trajectory tracking that illustrates the autonomous indoor navigation in a known GPS-denied
difference between the downward camera output position and the environment; and secondly, to implement the appropriate map
estimated position. online scale factor estimation of monocular SLAM that leads to
have an accurate map and pose.
Keywords— Quadrotor, ORB-SLAM, Scale Factor Estimation, The two differences between this work and, [8] and [9] are;
Sensor Fusion, Pose Estimation
i: to change the input of EKF from PTAM to ORB-SLAM. ii:
To use the integration of horizontal accelerations in order to
I. INTRODUCTION obtain horizontal velocities, whereas, in [8] and [9], horizontal
An impressive interest in controlling the Unmanned Aerial velocities had been acquired by the downward camera.
Vehicles (UAVs) such as Quadrotors are significantly To this end, the lightweight frontal camera of an AR-Drone
increasing among researchers. Applications of these UAVs that 2.0 quadrotor is used for pose estimation. So that the quadrotor
can autonomously operate in outdoor/indoor environments broadcasts the data of camera as the video stream using Wi-Fi
have been widely defined in tasks such as traffic monitoring, to the computer in the ground station in which the ROS
rescue operations, surveillance, transportation, and hobbyist. In (Robotic Operation System) software framework is already run.
addition, for all these applications an imperative for UAV In this step, the video stream is processed by ORB-SLAM in
autonomy is the ability of self-localization in the environment. order to determine the 3D position of quadrotor and the 3D
Indeed, a robust accurate localization is crucial in order to spare map of the flight environment. To achieve a more
achieve high-performance flight and to interact with the accurate visual SLAM, calculating and applying a scale factor
environment. Also, due to deny the GPS in the indoor in ORB-SLAM measurements has to be done. To this end, the
environment and lack of self-localization, the autonomous raw image of this frontal camera should be sent to ORB
maneuver of Quadrotor is more challenging. Wrapper node and by combining the output of ORB-SLAM and
Up to now, many researchers have reported the solutions for altitude (by Sonar) in scale estimator, the map scale factor can
indoor navigation challenge with quadrotor by using LIDAR be online calculated. Using EKF, the ORB-SLAM
scanner [1-2], Red Green Blue and Depth (RGBD) [3], Motion measurements are fused with IMU as a navigation onboard
Capture (MoCap) System and [4] Monocular cameras [5]. In sensors measurement of Quadrotor due to reach the more
[6], an autonomous indoor flight of quadrotor using a accurate measurements.
Simultaneous Localization and Mapping (SLAM) system was Figure 1 gives an overview of system configuration
provided. In this paper, in order to achieve accurate 3D comprising ORB-SLAM, scale estimation and EKF.

978-1-5386-7761-2/18/$31.00 ©2018 IEEE 106

DOI 10.1109/LARS/SBR/WRE.2018.00028
 of this scale factor ∈ ℝ of monocular SLAM system before
applying the SLAM measurements, is essential [8].
According to [9], the quadrotor measures the d-
dimensional distance traveled in regular intervals, in which the
first set ∆ ∈ ℝ corresponds to Visual SLAM system
(subtracting start and end position). Also, the second set ∆ ∈
ℝ denotes the available metric sensors information
(subtracting start and end position, or integrating over estimated
speeds), where ∆ is scaled based on visual map and ∆ as
∆ ≈ ∆ . This pair of measurements are defined by the
following formulas:

∆ = ( )− ( )
(1)
∆ = ( )− ( )

where ( ), ( ), ( ) and ( ) are output sensors at

two successive step-times. By assuming that these values are
Fig. 1. System overview
noisy by Gaussian noise, the distributed samples of sensors
output with constant variance are described as:
Furthermore, in order to obtain the control commands
control, four PID controller for three positions and yaw angle ∆ = ( ∆ , × )
(2)
are employed. Finally, by considering that the positions of ∆ = ( ∆ , × )
obstacles are available, an indoor autonomous navigation of a
quadrotor is carried out. where ∆ denotes actual distance covered, and and are
The organization of this paper is as follow. In section II, known variances of the measurement errors. Furthermore, the
odometry based on vision, scale factor estimation and ORB- dimension of identity is three, due to the ability in quadrotors
SLAM are described. Then, sensor data fusion using EKF is to measure distance in all three dimensions. Note that for an
presented in section III. Also, experimental results are analyzed altimeter = 1, as only vertical motion can be measured, and
in section IV to illustrate the system performance. Finally, also = 3, as vertical and horizontal , motions can be
conclusions are established in section V. measured.
In this step, by collecting sensor data at one time and using
Maximum Likelihood (ML) method, the negative log-
II. ODOMETRY BASED ON VISION likelihood of measurements ∆ and ∆ is as below [11]:
A. Monocular ORB-SLAM
1 ‖∆ − ∆ ‖
The ORB-SLAM is a feature-based monocular Visual ℒ(∆ …∆ , )∝ (
SLAM system that, as the main idea behind of it, uses a binary 2
(3)
descriptor called ORB [14]. This ORB binary descriptor is very ‖∆ − ∆ ‖
fast, rotation invariant and resistant to noise based on the + )
estimation of the key-point orientation using the intensity
centroid of the image patch. In the ORB-SLAM algorithm, the According to [11], making derivative of ℒ(∆ … ∆ , ) and
features are directly extracted from the imagery. Such an assuming ∆ → 0, the optimal values of ∆ in terms of are
algorithm is different from other monocular SLAM systems as follows:
such as Large-Scale-Direct SLAM because it does not extract
features from the imagery but directly works on pixel intensity ∆ +∆
∗
gradient of imagery. ORB-SLAM can operate in real time and ∆ = (4)
+
also in various environments such as indoor, outdoor, small and
wide. In addition, robustness against intense motions, re-
localizing and full automatic initialization are other advantages Then, by minimizing above equation, optimal value of scale
of this system. factor estimation ∗ is obtained by [8]:

B. Scale Factor Calculation

4 + − + −
Determination of scale factor is one of the major challenges ∗ (5)
=
in using monocular SLAM. This factor comes from scale 2
difference between the visual SLAM output and the real world.
Indeed, the challenge occurs when monocular SLAM is used where = ∑ , = ∑ and =
for localization and map generation. So, having the estimation ∑ .

107
 III. SENSOR DATA FUSION AND CONTROL where Ε ∈ SE(3) is 6DoF camera pose estimation by ORB-
SLAM that is scaled with ∗ . Also ∈ SE(3) is the
A. Extended Kalman Filter (EKF) transformation from camera coordinate to quadrotor coordinate
One of the essential tasks in implementing the autonomous that is constant. Furthermore ℱ: SE(3) → ℛ is the conversion
navigation of drones is to estimate the states such as positions from SE(3) to orientation including Roll, Pitch and Yaw.
and velocities that lead up to have the accurate state, faster
controller reaction and, finally, to allow the drone to be 2) IMU Sensor
controlled more precisely. The Extended Kalman Filter (EKF)
is a modified version of the Kalman Filter and is used when The IMU that is embedded in quadrotor, sends measurements
model dynamics (system and/or measurements) are non-linear; including orientation angles, angular velocities and linear
it contains two steps: observation and prediction [15]. This accelerations, to the ground station. To make an IMU sensor
section presents the EKF in order to fuse optical pose estimate observation model, some of these updated values are needed.
obtained from ORB-SLAM and sensor measurement These values are horizontal velocities in and axis, Roll and
information provided by IMU. Pitch angles, altitude, and also Yaw angular velocity. In order
The state space is comprised of ten state variables as follows: to obtain the quadrotor horizontal velocities , from
IMU in quadrotor coordinate, strap-down algorithm is used that
=( ̇ ̇ ̇ ̇ ) ∈ℝ
is described as follows [12]:

where ( , , ) denotes the position of quadrotor in mass

center, in meters [ ], as well as ( ̇ , ̇ , ̇ ) represents linear =
velocity, in meter per second [ ⁄ ], both of them in world- (7)
coordinates. Furthermore, the orientation of the system,
contains Roll, Pitch and Yaw angles, which are respectively = + .
denoted by , and , all three in degree [ ]. The last
state variable is Yaw-angular velocity ̇ that is measured in
where and are direct acceleration provided by IMU,
degrees per second [ ].
and denotes sample time in the strap-down algorithm.
Since, the drone’s state variables have to be measured in world
x The Observation Models coordinate, so the observation function ( ) and
measurement vector are given by:
In observation model, the pose estimate vector depends
on current state at time through observation function ( )=( ̇ , ̇ , ̇ , , , ̇ )
( ). So, for each sensor, an observation function should be
defined as follows:

1) Visual SLAM ⎛ ⎞
⎜ℎ −ℎ ⎟
⎜ ⎟ (8)
ORB-SLAM provides 6-DoF camera pose that is not scaled
=⎜ ⎟
yet. So firstly, it has to be scaled by applying scale estimation ⎜ ⎟
factor ∗ when ORB-SLAM is successfully tracking video ⎜ ⎟
frame. The second problem is to pay attention to an important ⎜ ⎟
−
issue in defining the observation model of visual SLAM
system. This essential issue is to consider the offset (distance) ⎝ ⎠
between the coordinate system of frontal camera and mass
center coordinate system of the quadrotor. To overcome this where ℎ denotes the height measurement obtained by
problem, one transformation has to be done from the coordinate altimeter and represents the time passed between time-step
system of the frontal camera to coordinate system of the k to k+1. Also and are direct observation that are
quadrotor. This transformation leads to the direct observation measured by IMU. Furthermore, to account for yaw-drift and
of respective 6-DoF pose as follows: uneven ground, the height measurements ℎ and are
differentiated and have been treated as observations of the
( )=( , , , , , ) (6) respective velocities.

Remark: Measurement vector of ORB-SLAM is described as x The Prediction Model

follows [9]:
In this section, the prediction model is used to predict the
= ℱ( , ) state transition according to current state and control
signals at time + . So, the first step is to approximate

108
the horizontal acceleration of quadrotor in x and y axis ( ̈ , ̈ ) B. Drone Control
according to its current state , and making estimation of In this section, the use of PID controller in quadrotor is
altitude acceleration ̈ , angular acceleration around Z-axis ( ̈ ) reviewed. Actually, PID controller calculates input of system
and also angular velocity around X and Y axis ( ̇ , ̇ ). ( ) according to:
The horizontal acceleration is proportional to total horizontal
forces acting upon the quadrotor that is comprised of sum of
two forces including air resistance and accelerating force ( )= . ( )+ ∫ ( ) + ̇( ) (13)
that is given by:
where , and are tunable PID gains that are commonly
̈
∝ − (9) specified by mean of trial and error.
̈
According to position and velocity estimated from EKF and
which ∈ ℛ is approximately proportional to its current assuming = ( ) represents the external
reference values vector, the control signal
horizontal velocity and ∈ ℛ is proportional to the
=( ̇ ̇) is calculated by applying PID
projection of quadrotor in Z-axis to the horizontal side [11]. By
controller for theses four state variables as follows:
considering the assumption that and are constant.
over short time interval, then these explanations lead to obtain = (0.5( − ) + 0.32 ̇ ) cos − (0.5( − ) + 0.32 ̇ ) sin
horizontal acceleration of quadrotor as below:
= −(0.5( − ) + 0.32 ̇ ) sin − (0.4( − ) + 0.32 ̇ ) cos
̈( )= (cos sin cos − sin sin )− ̇
̇ = 0.6( − ) + 0.1 ̇ + 0.01 ∫( − )
̈( )= (− sin sin cos − cos sin )− ̇ (10)
̇ = 0.02( − )

where and are model constant coefficients. Also, the total

Note that the explained PID controller is in continues time.
trust force that is generated by four propellers, is assumed
Thus, in order to practical implementation, it is approximated
constant. Similar to estimation of horizontal acceleration, the
by a discrete-time PID model.
influence of control command = ̇̅ ̇ ∈
[1, −1] in which desired Roll and Pitch angles, desired Yaw IV. EXPERIMENTAL TEST
angular velocity and desired altitude velocity, approximated by To perform the experimental tests, various tools are
the linear model below: required including AR-Drone by Parrot for the experiments,
Robot Operating System (ROS) with ardrone_autonomy [13]
̇( , ) ( − ) driver to make a communication between the ground station
⎛ ̇( , ) ⎞ ⎛ ( − )⎞ and the quadrotor. Furthermore, an ORB_SLAM package is
⎜ ̈ ⎟=⎜ ( (11) needed to obtain the feature of environment and mapping.
( , ) ̇ − ̇ )⎟
In this section, an analysis of the performance of the
⎝ ̈ ( , )⎠ ⎝ ( ̇̅ − ̇ ) ⎠ explained IMU+ORB-SLAM (using EKF) in section 3 is
presented. Indeed, this analysis is done by making the
Actually, the behavior of control command is modeled comparison between EKF estimation (red line) and the
by (11) where to are model constants which are measurement output of the downward camera (blue line) that is
determined experimentally in a series of flights. shown in the sequel as the real measurement (without EKF).
Now the complete state transition function at time period Since, during the test, we did not have the Motion-Capture
system as the ground truth measurement recorder, so we had to
is updated as follows:
use AR-Drone’s downward camera measurements. To this end,
̇ two tests are described.
̇ A. Position holding
⎛ ⎞
⎛ ⎞ ⎛ ⎞ ̇ The simplest flight for a quadrotor is to be in hovering
⎜ ⎟
⎜ ̇ ⎟ ⎜ ̇ ⎟ ⎜ ̈( ) ⎟ mode. But the important issue in hovering mode is to hold the
⎜ ̇ ⎟ ⎜ ̇ ⎟ ̈( ) ⎟ determined position. In order to achieve the robustly estimated
⎜
⎜ ̇ ⎟=⎜ ̇ ⎟+ (12)
⎜ ̈( , ) ⎟ pose, output pose of ORB-SLAM is combined with data of
⎜ ⎟ ⎜ ⎟
⎜ ̇ ( , )⎟ IMU using EKF based on the formulation of section 3. In this
⎜ ⎟ ⎜ ⎟
⎜ ⎟ ⎜ ⎟
⎜ ̇ ( , )⎟ test, holding the quadrotor in a specified position (take-off
⎜ ⎟ point) is performed by EKF and ORB-SLAM. It means the
̇
⎝ ̇ ⎠ ⎝ ̇ ⎠ quadrotor has to hold itself in its position after auto take-off for
⎝ ̈ ( , )⎠ some seconds and then it does auto-landing. Therefore, the
performance of quadrotor is evaluated under conditions which
in the front camera of AR-Drone is sending the video stream to

109
ORB-SLAM wrapper in the ground station and by extracting B. Trajectory Tracking
the features, ORB-SLAM estimates the pose and then this In this test, one L-shaped trajectory is determined for the
ORB-SLAM pose is sent to EKF and EKF pose estimation will analysis the navigation performance of the proposed algorithm.
be performed. The size of this trajectory is as 4 meters longitude (y-direction)
Figure 2 shows the features taken by ORB_SLAM in this and 1.5 meters latitude (x-direction) that quadrotor has to start
test. its mission from the take-off point and travels to target point
with z=1 m and then lands. Figure 6 shows the implementation
testing steps of trajectory tracking.

Fig. 2. Features taken by ORB_SLAM in this test.

Since the selective position is the take-off point (0, 0) in

height z=1 m, results illustrate that the quadrotor has moderate
drifting (until auto-landing time) in horizontal sides; 40 cm for
x, and 1.7 m for y. But it seems the result is more acceptable in
z direction because it is smoother rather than the measured Fig. 6. Implementation testing steps of trajectory tracking
height by altimeter. The obtained plots in figures 3 to 5
confirmed this drifting. Performance evaluation of the proposed algorithm is
analyzed by plots that compare the measured position by
downward camera, and EKF estimator position output. As
regards that the goal points of x and y were 1.5 and 4
respectively, so the results indicate the EKF is efficient to reach
the specified point. Figures 7-9 showed the obtained results in
3D positions including x, y and z.
Auto-landing time

Fig. 3. Motion x during the position holding test

Auto-landing time

Fig. 7. Motion x during the trajectory test

Fig. 4. Motion y during the position holding test

Auto-landing time

Fig. 8. Motion y during the trajectory test

Fig. 5. Height changes during the position holding test

110
 control is used to control the robot during the test. To verify the
proposed algorithm, two kinds of experimental tests are
accomplished that the first one is position holding and the
second one is trajectory tracking. The tests results showed that
the combining the ORB_SLAM data and IMU data by EKF
could be an appropriate method to improve the position
accuracy of quadrotor, especially in indoor trajectory tracking.
ACKNOWLEDGMENT
Fig. 9. Height changes during the trajectory test
The experimental test in this study was done in
Also, the trajectory travelled by Quadrotor is plotted as Mechatronic Research Laboratory (MRL) of Qazvin Islamic
2Dimensions in x-y axis via Figure 10 that indicates a clear Azad University in Iran. The first author has a scholarship from
difference between using the proposed EKF algorithm and the CAPES and the second author is also supported by CNPq,
downward camera output. Brazil.

REFERENCES
[1] Fei Wang, J. Cui, S. K. Phang, Ben M. Chen, Tong H Lee, “A Mono-
Camera and Scanning Laser Range Based UAV Indoor Navigation
System,” Conference: Unmanned Aircraft Systems (ICUAS), 2013,
ISBN: 978-1-4799-0817-2
[2] Fei Wang, J. Cui, S. K. Phang, Ben M. Chen, Tong H Lee, “A
comparison UAV Indoor Navigation System Based on Vision Optical
Flow and Laser Fast SLAM,” Acta Automatica Sinica, Volume 39, Issue
11, November 2013, Pages 1889-1899
[3] Amedeo Rodi Vetrella, Al Savvaris, G. Fasano, D. Accardo, “RGB-D
Camera-Based Quadrotor Navigation in GPS-Denied and Low Light
environment Usibg Known 3D Markers,” Unmanned Aircraft Systems
(ICUAS), 2015 International Conference on. ISBN: 978-1-4799-6010-1
Denver, CO, USA
[4] Fengmin Yu, Guojin Chen, Neng Fan, Yaoxian Song, Lingjun Zhu,
“Autonomous Flight Control Law For An UAV Indoor Quadrotor,”
Fig. 10. Motions x and y in trajectory tracking test Control And Decision Conference (CCDC), 2017 29th Chinese,
ISBN: 978-1-5090-4657-7, May 2017, Chongqing, China
As a result of our implementation, we obtained the scaled [5] R. Huang, P. Tan, and Ben M. Chen. "Monocular vision-based
autonomous navigation system on a toy quadcopter in unknown
map estimated by applying the scale factor into ORB_SLAM environments." Unmanned Aircraft Systems (ICUAS), 2015 International
wrapper that leads to the updated map. Figure 11 demonstrated Conference on. IEEE, 2015
the recorded map of the trajectory tracking test. [6] Elena López, S. García, R. Barea, M. Luis, J. Eduard, R. Arroyo, E
Romera and S. Pardo, “A Multi-Sensorial Simultanous Localization and
Mapping (SLAM) for Low-Cost Micro Aerial Vehicles in GPS-Denied
Environment,” Sensors (Basel), 17(4): 802, Licensee MDPI, Basel,
Switzerland, 2017 Apr
[7] A. Wendel, M. Maurer, M. Katusic, H. Bischof: Fuzzy visual servoing for
micro aerial vehicles. In: Proceedings of the Austrian Robotics Workshop,
2012
[8] J. J. Engel, J. Sturm, and D. Cremers, “Scale-aware navigation of low-
cost quadcopter with a monocular camera,” Robotic and Autonomous
Systems, Volume 62, Issue 11, Pages 1646-1656, November 2014
[9] J. J. Engel, J. Sturm, and D. Cremers: Camera-based navigation of a low-
cost quadrocopter. In Proc. of the International Conference on Intelligent
Robot Systems (IROS), Oct. 2012.
[10] R. Mur-Artal, J. M. M. Montiel: ORB-SLAM: a Versatile and Accurate
Monocular SLAM System” IEEE Transactions on Robotics, vol. 31, pp.
Fig 11. Recorded map of trajectory tracking test 1147 – 1163, October 2015
[11] J. J. Engel: Autonomous Camera-Based Navigation of a Quadrocopter,
V. CONCLUSIONS Master’s Thesis in Informatik, University of Munich, December 15, 2011
This paper addressed an improvement of pose estimation [12] K. K. Lekkala, V. K. Mittal: Accurate and Augmented Navigation for
quadcopter based on Multi-Sensor Fusion, India Conference (INDICON),
accuracy and visual-inertial navigation of a Low-Cost IEEE Annual, Bangalore, India, 2016
Quadrotor to make an autonomous indoor navigation in a [13] https://ardrone-autonomy.readthedocs.io/en/latest/#
known GPS-denied environment. This algorithm is performed [14] E. Rublee, V. Rabaud, K. Konolige, and G. Bradski. ORB: an efficient
by using a SLAM-based approach that also is known alternative to SIFT or SURF. In IEEE International Conference on
Computer Vision (ICCV), pages 2564–2571, Barcelona, Spain, 2011.
ORB_SLAM, and also EKF. In addition, we calculated the [15] L. Ascorti, “An Application of Extended Kalman Filter to the Attitude
scale factor that is obtained as online and it is one of the major Control of Quadrotor,” Master thesis, Polytechnique University of Milan,
challenges in using monocular SLAM. Meanwhile, a PID 2013

111