An Integrated Navigation System Design
For Quadrotors
Ilkay Gumusboga
Emre Kiyak
Anadolu University
Department of Avionics
Eskisehir, Turkey
ilkaygumusboga@anadolu.edu.tr
Anadolu University
Department of Avionics
Eskisehir, Turkey
ekiyak@anadolu.edu.tr
Abstract—In this work, a particle filter based integrated
navigation system is designed and tested with real quadrotor
systems. Conventionally, Kalman filter based algorithms is used
for navigation systems which is highly non-linear. Kalman Filter
has significant assumptions such as linearity of system and
measurement models and Gaussian distributions. This situation
causes that Kalman filter does not work effective with navigation
systems. Nowadays, particle filter based algorithms take over with
respect to other estimation algorithms because of progression of
computer’s processors capability. Within the scope of this study, a
particle filter based integrated navigation system is designed. This
designed navigation system is tested on real quadrotor system. As
a result of the real flight tests, the error margin is reduced a
quarter according to the most trusted navigation aid.
Keywords—navigation;
estimation
particle
filter;
quadrotor
system;
I. INTRODUCTION
Kalman filter has significant assumptions such as linearity of
system and measurement models and Gaussian distributions.
Conventionally, Kalman filter based algorithms is used for
navigation systems which is highly non-linear. This situation
causes that Kalman filter not to work effective. Recently,
particle filter based algorithms take over with respect to other
estimation algorithms. The most important reason for this,
unlike Kalman filter, particle filter has no assumption about
system and measurement model linearity or distribution
characteristics. This advantage provides to work on more
realistic model of the real systems. Studies about this subject
show that particle filter based navigation algorithms works more
accurate with regard to Kalman filter based algorithms [1-4].
The principal study about navigation with particle filter is
published by Gustafsson. In this study additionally, the other
estimation techniques based on Baessian approach together with
particle filter are examined and applied to submarine, land and
aerial vehicles in order to estimate positions [1].
Ren and Ke use the particle filter in their study due to
accomplishing inertial sensors’ non-linear nature. MEMS based
inertial sensor data and Global Positioning System (GPS) data
are used for obtaining navigation solution by a method based on
data fusion logic substantially [2].
Hernaez and Giribet compare the particle filter and the
linearized Kalman filter in the point of performance on
integrated navigation systems. In this study, Inertial Navigation
System (INS) and GPS data are used in the simulation scenario
with a limited number of states. As a result of this work, the
authors shows that particle filter has a better performance with
respect to Kalman filter [4].
A terrain based navigation system is developed by Melo and
Matos with using particle filter algorithm as a base of the system.
Furthermore, a technique which reduces the number of particles
is proposed by the authors [5].
The ultimate goal of our study is to design an accurate,
reliable, integrated navigation solution for a system which has
six degrees of freedom. This solution is calculated
independently from motion model of the system. Because the
navigation algorithm works as a fusion algorithm which require
different data sources. Due to the independence of motion model
of the system, the designed navigation system is used for various
aerial platforms such as fix or rotary wings aircrafts or missiles.
Conversely to the most studies in literature, the designed
navigation system is tested with real quadrotor system in our
laboratory.
II. MATHEMATICAL BASICS
A. Coordinate Frames and Transformations
Inertial measurement unit (IMU) which is the main
component of the INS measures specific force, 𝒇𝒃𝒊𝒃 , and angular
rate, 𝝎𝒃𝒊𝒃 , with respect to body frame. The body frame comprises
the origin and orientation of the object for which a navigation
solution is sought. The orgin is coincident with the vehicle’s
center of gravity, and the axes remain fixed with respect to the
body [6-7].
To achieve a complete integrated navigation solution, a
reference coordinate frame should be used. In order to use more
than one solution together we need to state all navigation
expressions with respect to the reference coordinate frame.
Therefore, local navigation coordinate frame (NED frame) is
used for all expressions. In the local navigation frame,
commonly abbreviated to navigation frame, the z-axis always
points along the Earth’s center. The x- and y-axis point along the
north and the east respectively.
𝑽𝑛𝑒𝑏 : Velocity vector in the navigation frame
𝒈
: Gravity vector in the navigation frame
Now, body frame to navigation frame transformation matrix
𝑪𝒏𝒃 can be defined as follows:
𝜏𝑖
: The period of IMU measurements
𝑪𝒏𝒃 =
c(Ѳ)c(𝛹)
[s(𝛷) s(Ѳ) c(𝛹) − c(𝛷)s(𝛹)
𝑐(𝛷)𝑠(Ѳ)𝑐(𝛹) + 𝑠(𝛷)𝑠(𝛹)
𝑻
c(Ѳ)s(𝛹)
−𝑠(Ѳ)
𝑠(𝛷)𝑠(Ѳ)𝑠(𝛹) + 𝑐(𝛷)𝑐(𝛹) 𝑠(𝛷)𝑠(Ѳ)]
𝑐(𝛷)𝑠(Ѳ)𝑠(𝛹) − 𝑠(𝛷)𝑐(𝛹) 𝑐(𝛷)𝑐(Ѳ)
Where s(*) and c(*) denote sin(*) and cos(*), respectively.
In this formulation; Ψ is yaw angle, Ѳ is pitch angle, Φ is roll
angle (the Euler angles).
B. Inertial Navigation Equations
As stated previously, inertial navigation frame is used for all
navigation expressions. With navigation equations, the outputs
of IMU, specific force and angular velocity, are used to update
navigation frame attitude, velocity, and position.
Navigation equations are introduced in four steps: attitude
update, specific-force frame transformation, velocity update,
and position update [6]. In these equations, the Earth rotation is
neglected and the surface of the Earth is assumed flat.
1) Attitude Update:
𝑪𝒏𝒃 (+) = 𝑪𝒏𝒃 (−)(𝑰𝟑 + 𝛀𝒃𝒊𝒃 𝝉𝒊 )
(1)
2) Specific-force Frame Transformation:
𝒇𝒏𝒊𝒃 =
𝟏 𝒏
(𝑪 (−) + 𝑪𝒏𝒃 (+))𝒇𝒃𝒊𝒃
𝟐 𝒃
𝒓𝑛𝑒𝑏 : Position vector in the navigation frame
The calculation of the 𝒈 vector is explained in [6] in depth
according to the WGS 84 (Word Geodetic System) standard.
III. PARTICLE FILTER BASED NAVIGATION
Particle filter (also known as sequential Monte Carlo
method) is known as a non-linear filtering technique in signal
processing community. In this technique, each of the states’
likelihood is represented as particles. Baessian approach is used
like Kalman filter but there is no assumptions on the system such
as motion model and measurement model linearity or Gaussian
distributions. The studies show that because the system is
modeled more realistic, more efficient results are achieved with
the particle filter [1-3]. For having a wide acquaintance with
particle filter theory, [8-10] can be examined.
A. Integrated Navigation
Inertial Navigation Systems are dead-reckoning systems. So,
errors of INS increase with time. For this reason, INS cannot be
used with long-term applications. In order to overcome this
situation, it is reasonable to integrate INS data with a different
navigation aid.
In the literature, several integration algorithm architectures
are exists. These are loosely coupled, tightly coupled, ultratightly coupled, closely coupled, deep integration architectures
[6-7]. In this work, we use loosely coupled integration system.
This system uses the secondary navigation aid’s position and
velocity solution as the measurement inputs to the integration
algorithm. In this work particle filter algorithm is used as
integration algorithm.
(2)
3) Velocity Update:
𝑽𝒏𝒆𝒃 (+) = 𝑽𝒏𝒆𝒃 (−) + (𝒇𝒏𝒊𝒃 + 𝒈) 𝝉𝒊
(3)
4) Position Update:
𝒓𝒏𝒆𝒃 (+) = 𝒓𝒏𝒆𝒃 (−) + 𝑽𝒏𝒆𝒃 (+)𝝉𝒊 − ((𝒇𝒏𝒊𝒃 + 𝒈)
𝝉𝑖 𝟐
)
𝟐
(4)
Fig. 1. The block diagram of the navigation system
In these formulations;
𝛀𝑏𝑖𝑏 : Skew-symmetric matrix of IMU angular rate
measurements.
𝛀𝒃𝒊𝒃
𝟎
𝒃 𝒛
= [ 𝝎𝒊𝒃
𝒚
−𝝎𝒃𝒊𝒃
𝒛
−𝝎𝒃𝒊𝒃
𝟎
𝒙
𝝎𝒃𝒊𝒃
𝝎𝒃𝒊𝒃
𝒚
𝒙
−𝝎𝒃𝒊𝒃 ]
𝟎
(5)
B. Problem Definition
Within the scope of this paper, the algorithm which is
calculates navigation solutions for a quadrotor system is
designed. The solutions contain 7 states, positions and velocities
in three dimensional space and the yaw angle (heading). The
designed navigation system is applied on a real system. For this
purpose, QBall-X4 [11] system is used as a navigation test
platform. Within this context, indoor motion capture system is
used as secondary navigation aid in the navigation system.
Under the normal condition, the motion capture system (camera
system) is produce very precise positioning data according to the
IMU. But in this study, additional noise is added on the
measurement. Hence, additional noise free camera system’s
position output is assumed true positions of the quadrotor.
Figure 1 illustrates the block diagram of the designed system.
with using remote control which connected with ground station.
Figure 3 illustrates the recorded flight path during the flight test.
The data which comes from IMU mounted on the quadrotor
frame and the 3D motion capture system for indoor positioning
is used as inputs of the navigation system. This system includes
six cameras which is deployed in our laboratory. Figure 2
illustrates the test platform established in laboratory.
Fig. 3. The recorded flight path
In the course of flight test, the obtained IMU and GPS-like
camera data is used as an inputs of the designed particle filter
based navigation system. The error between the true states and
the estimated states of the designed system is calculated with the
formula (6).
𝑒𝑟𝑟𝑜𝑟 = |𝑡𝑟𝑢𝑒 𝑠𝑡𝑎𝑟𝑒 − 𝑒𝑠𝑡𝑖𝑚𝑎𝑡𝑒𝑑 𝑠𝑡𝑎𝑡𝑒|
Fig. 2. The test platform established in laboratory
(6)
Figure 4 illustrates the yaw angle error. The error margin is
approximately 0.15 radyan (~8.59 degree). A good tracking is
ensured according to the true yaw angle.
Eventually, the designed navigation system produce good
results under the test scenario for the 7 states. 7 states are
estimated because estimating roll and pitch angles are found
unnecessary. Quadrotors move around its equilibrium point.
Therefore, small changes occur to roll and pitch angles when
quarotors move from a point to another point. Consequently,
there is no need tracking roll and pitch angles for only navigation
purposes.
IV. FLIGHT TEST RESULTS
The designed algorithm is tested with Qball-X4 quadrotor
system in our laboratory. Due to QBall is an indoor system, low
power GPS signals is not available. As a result of this, the system
uses 3D motion capture system for positioning the quadrotor.
The positioning system which uses six fixed position camera in
the laboratory produces GPS-like navigation data (with
additional noise which shows GPS error characteristics).
The Qball quadrotor is controlled with a computer as a
ground control station. The software installed in the control
station works with MATLAB/Simulink. The system has manual
and automatic control mode for flight test.
In this study, the position of the quadrotor is controlled by
automatic mode but the yaw angle is controlled by manual mode
Fig. 4. The yaw angle error of designed navigation system
Figure 5-6 illustrate the position errors and the velocity
errors respectively. Due to the error margin is very small
according to the formula (6), the good tracking is achieved as
shown in the figures.
Fig. 5. The position errors of the designed navigation system
Fig. 6. The velocity errors of the designed navigation system
V. CONCLUSIONS
In this study, a particle filter based integrated navigation system
is designed and tested on a real qaudrotor system. For this
purpose QBall-X4 system is established in our Control and
Avionics Laboratory (CAL) as a navigation test platform. The
main contribution of this study is the designed algorithm’s
validation with a real air vehicle system.
REFERENCES
[1]
[2]
[3]
[4]
Gustafsson, F., “Particle Filter Theory and Practice with Positioning
Applications”, IEEE A&E Systems Magazine Vol. 25, No. 7, 2010.
Ren, Y., Ke, X., “Particle Filter Data Fusion Enhancements for MEMSIMU/GPS”, Intelligent Information Management, 2, 417-421, 2010.
Ma, L., Guan, B., “SINS/GPS Integrated Navigation System using an
Improved Particle Filter based on State Reconstruction”, Control
Engineering and Applied Informatics, Vol. 13, No. 3, pp. 58-64, 2011.
Hernaez, F., Giribet, J. I., “Comparison Between Linearized Kalman
Filters and Particle Filters Applied to Integrated Navigation Systems”, 6th
International ESA Conference on Guidance, Navigation and Control
Systems, Greece, 2006.
[5] Melo, J., Matos, A., “On the Use of Patricle Filters for Terrain Based
Navigation of sensor-limited UAVs”, IEEE, ISBN 978-1-4799-0000-8,
pp. 1-8, 2013.
[6] Groves, P. D., “Principles of GNSS, Inertial, and Multisensor Integrated
Navigation Systems”, Artech House, Boston, 2008.
[7] Weston, J.L., Titterton, D.H., “Strapdown Inertial Navigation
Technology”, 2004.
[8] Gustafsson, F., Gunnarsson, F., Bergman, N., Forssell, U., Jansson, J.,
Kalsson, R., Nordlund, P., “Patricle Filters for Positioning, Navigation
and Tracking”, IEEE Transactions on Signal Processing, Special issue on
Monte Carlo methods for statistical signal processing, 2002.
[9] Nordlund, P., Gustafsson, F., “Sequential Monte Carlo Filtering
Techniques Applied to Integrated Navigation System”, In Proc. of the
American Control Conference, Arlington, Virginia, U.S.A, 2001.
[10] Nordlund, P., Gustafsson, F., “Marginalized Particle Filter for Accurate
and Reliable Terrain-Aided Navigation”, IEEE Transactions on
Aerospace and Electronic Systems, Vol. 45, No. 4, 2009.
[11] Quanser Innovative Educate, "Quanser QBall-X4 User Manual”,
Document Number 888.