Inertial Navigation for Quadrotor Using Kalman Filter with Drift Compensation

International Journal of Electrical and Computer Engineering (IJECE)
Vol. 7, No. 5, October 2017, pp. 2596~2604
ISSN: 2088-8708, DOI: 10.11591/ijece.v7i5.pp2596-2604  2596
Journal homepage: http://iaesjournal.com/online/index.php/IJECE
Inertial Navigation for Quadrotor Using Kalman Filter with
Drift Compensation
Lasmadi, Adha Cahyadi, Samiadji Herdjunanto, Risanuri Hidayat
Department of Electrical Engineering and Information Technology, Faculty of Engineering
Universitas Gadjah Mada, Yogyakarta, Indonesia
Article Info ABSTRACT
Article history:
Received Oct 25, 2016
Revised Apr 10, 2017
Accepted Jun 26, 2017
The main disadvantage of an Inertial Navigation System is a low accuracy
due to noise, bias, and drift error in the inertial sensor. This research aims to
develop the accelerometer and gyroscope sensor for quadrotor navigation
system, bias compensation, and Zero Velocity Compensation (ZVC). Kalman
Filter is designed to reduce the noise on the sensor while bias compensation
and ZVC are designed to eliminate the bias and drift error in the sensor data.
Test results showed the Kalman Filter design is acceptable to reduce the
noise in the sensor data. Moreover, the bias compensation and ZVC can
reduce the drift error due to integration process as well as improve the
position estimation accuracy of the quadrotor. At the time of testing, the
system provided the accuracy above 90 % when it tested indoor.
Keyword:
Drift compensation
Imu
Inertial navigation
Kalman filter
Quadrotor Copyright © 201x Institute of Advanced Engineering and Science.
All rights reserved.
Corresponding Author:
Adha Cahyadi,
Department of Electrical Engineering and Information Technology,
Faculty of Engineering,
Jln Grafika 2, Universitas Gadjah Mada, Yogyakarta, Indonesia.
Email: adha.imam@ugm.ac.id
1. INTRODUCTION
The quadrotor is one of Unmanned Aircraft System (UAS) that can fly autonomously or remotely
controlled [1]. Recently, quadrotor is a fruitful topic of interest in many research areas, including in civilian
and military. Quadrotor does not need a large place for landing, it can take off vertically and ability to high
maneuver than the other vehicle [2],[3]. This makes UAS or UAVs (Unmanned Aerial Vehicles) has been
widely used in various fields of both military and civilian. In the military field, UAVs are used to combat
operations and decision-making, intelligence, ISR (Intelligent Surveillance and Reconnaissance), as well as
atmospheric research [4]. In addition, UAVs are used to SAR (Search and Rescue), supervision real-time,
reconnaissance, inspection of the dangerous place [1], traffic monitor [5], monitor of natural disasters [6],
pest and disease control [7], inspection of electrical networks [8] as well as in the field of meteorology [9].
Quadrotor can be used efficiently to perform tasks that can be risky if the human pilot [10]. The research
topics of UAVs are broad from modeling and control [11]–[13], navigation, path planning to simultaneous
localization and mapping (SLAM) [14], and much more. It has been separately addressed by researchers in
areas of robotics, mechatronics, computer sciences, etc [15].
Navigation systems take an important role of the UAV system. To be able to move autonomously in
the specific area, a robot must have the ability to determine its own position and reach the destination by
using the best possible path [10]. Before reaching the destination, a quadrotor must determine its position and
angular orientation. This technique is called localization. Usually, the use GPS in the UAV navigation system
can provide accurate position measurement [16]. However, the information from GPS has limitations in the
certain environment that led to the signal noisy or unavailable, such as inside a building [17]–[19], in the
urban environment [20]–[22], and underwater [23]. In the environment that is not covered by GPS,

IJECE ISSN: 2088-8708 
Inertial Navigation for Quadrotor Using Kalman Filter with Drift Compensation (Lasmadi)
2597
navigation or localization of a UAV can only depend on the IMU (Inertial Measurement Unit) and the
exteroceptive sensor such as the laser range finder (LRF) and the camera.
The systems use the LRF sensor for UAV navigation system has been carried out in [21],[24], and
[25]. The LRF sensor has a limitation of the range that may lead to less accurate position measurement [19].
This approach only works in the structured environment [26]. Then, navigation systems based on the camera
that has been reported in [2],[19],[22], and [27], have good accuracy when the camera's field of view changes
relatively slowly [20]. The camera can be used to 3D localization, but they more slowly than the IMU [28].
In addition, the system based on visual odometry can work effectively when it in an environment with
sufficient lighting and static scenery with enough texture in order to obtain clear movement to be extracted
[23]. In general, the camera-based system requires computation capability heavier because of the sensor data
to be processed [2],[29], so it will be difficult to be implemented in the embedded systems [30].
In this study, the navigation system based on IMU sensor with Kalman Filter (KF) is proposed. An
IMU consist of the accelerometer and gyroscope which technology based on Micro Electro Mechanical
System (MEMS). The MEMS technology has the advantage of small size, light weight, low power
consumption, and high resistance [31] that is able to provide rapid response [28]. The IMU usually is applied
to estimate the orientation of a robot and spacecraft. However, due to limitation performance at low cost-
MEMS, the measurement accuracy of position and speed will decrease with increasing integration time
[2],[26]. This is caused by the bias and the drift error in the IMU [30].
Kalman Filter has been the subject of extensive application and research, especially in the
autonomous navigation and guided navigation areas. The Kalman Filter performs well in practice and
attractive in theoretical because it can minimize the variance of the estimation error [32]. The small
computational requirement, recursively properties, and status as optimal estimator are the great success of the
Kalman Filter [33]. In this study, Kalman Filter is designed to reduce the noise on the sensor.
Zero Velocity Compensation (ZVC) is a method that is used to elimination the drift effect on the
signal data [34]. In this study, ZVC is designed to reducing the drift error on the sensor signal data and
stationary detection of the quadrotor. With ZVC, the sensor signal data would be considered to zero if the
sensor is stationary even though not equal to zero. Ideally, if the accelerometer and gyroscope are stationary,
the velocity output is equal to zero. However, the fact despite the sensor does not move, the sensor output is
not equal to zero. This can result in a calculation error in determining the position and direction sensor when
the sensor does not move. This error is known as drift.
The contributions of this study are the development of the algorithm for estimation rotational and
translational displacement of the flying robot based on IMU sensor, especially for the quadrotor. The
proposed algorithm also can compensate the drift error in the gyroscope signal data. The benefits of this
research provide a method for handling the noise of the IMU sensor to improve the accuracy of navigation
data and contribute of development Kalman Filter as navigation state estimator and noise filter on the
accelerometer and gyroscope sensor. In general usage, this system can provide an alternative solution to a
low-cost navigation as the navigational under the lack of GPS signal.
2. RESEARCH METHOD
In this study, we use the ROS (Robot Operating System) to get data from the IMU sensor that
running on the Linux Ubuntu operating system. The data at this moment is processed off-line. The illustration
of angular and translation movement test is shown in Figure 1. The schematic of experimental setup and data
processing are shown in Figure 2. The algorithm design includes the phases as follows.
(a) Angular
movement (roll)
(b) Translation
movement
Figure 2. The schematic of experimental setup and
data processing
Figure 1. Illustration of roll and translation
movement test

 ISSN: 2088-8708
IJECE Vol. 7, No. 5, October 2017 : 2596 – 2604
2598
2.1. Measurement Model of Sensor and Rotation Matrix
The Accelerometer is a sensor measures the linear acceleration of vehicles. In reality, the
accelerometer sensor not only measures the linear acceleration but also the gravity acceleration. The gravity
acceleration is measured by the sensor will interfere the measurement results. Therefore, the measurement of
the accelerometer can be modeled as:
𝑎 𝐵𝑖 = 𝑎̃ 𝐵𝑖 − 𝑔 + 𝑏 𝐵𝑖 + 𝜀 𝐵𝑖, (1)
where 𝑎 𝐵𝑖 is the accelerometer reading in the body frame, 𝑎̃ 𝐵𝑖 is the actual acceleration, 𝑔 is the gravity
constant, 𝑏 𝐵𝑖 is the accelerometer bias, and 𝜀 𝐵𝑖 is the noise in accelerometer. On the other hand, the
measurement of a MEMS gyroscope can be modeled as:
Ω 𝐵𝑖 = Ω̃ 𝐵𝑖 + 𝛿 𝐵𝑖 + 𝑛 𝐵𝑖, (2)
where Ω 𝐵𝑖 is the gyroscope reading in the body frame, Ω̃ 𝐵𝑖 is the actual angular velocity, 𝛿 𝐵𝑖 and 𝑛 𝐵𝑖 are the
gyroscope bias and the noise in gyroscope respectively.
The signal data measured by the sensor is the data in the body coordinate (body frame). Therefore,
the rotation matrix needs to be transformed from body frame to navigation frame (global frame). To
transform the measurement data from body frame to navigation frame can be carried out as follows.
𝑎 𝑊 𝑖
= 𝑅 𝐵
𝑊
[𝑎 𝐵𝑖], (3)
Where 𝑎 𝑊 𝑖
= [𝑎 𝑊𝑥 𝑎 𝑊𝑦 𝑎 𝑊𝑧] 𝑇
, 𝑅 𝐵
𝑊
= [
𝑐𝜃𝑐𝜓 𝑠𝜙𝑠𝜃𝑐𝜓 − 𝑐𝜃𝑠𝜓 𝑐𝜙𝑠𝜃𝑐𝜓 + 𝑠𝜙𝑠𝜓
𝑐𝜃𝑠𝜓 𝑠𝜙𝑠𝜃𝑠𝜓 + 𝑐𝜙𝑐𝜓 𝑐𝜙𝑠𝜃𝑠𝜓 − 𝑠𝜙𝑐𝜓
−𝑠𝜃 𝑠𝜙𝑐𝜃 𝑐𝜙𝑐𝜃
] , and
𝑎 𝐵 𝑖
= [𝑎 𝐵𝑥 𝑎 𝐵𝑦 𝑎 𝐵𝑧] 𝑇
.
The transformation matrix 𝑅 𝐵
𝑊
is used to convert from the sensor frame to global frame, 𝑎 𝐵𝑖 is the
acceleration in the body frame, 𝑎 𝑊𝑖 is the acceleration in the global frame, with i is the axis index (i ϵ
{x,y,z}), c represents cosine, s represents sinus, θ is the roll angle, 𝜙 is the pitch angle, and 𝜓 is the yaw
angle.
2.2. Kalman Filter Design for Gyroscope Data
In this study, the Kalman Filter is used to reduce the noise of gyroscope signal. The gyroscope
signal is the angular velocity data of each axis. Hence, the angular velocity data is used to input the Kalman
Filter for obtaining the orientation angle of the quadrotor. The process model and measurement model of the
Kalman Filter for gyroscope sensor can be expressed in equation (4) and (5) as,
𝜒 𝑘+1 = 𝐴𝑖 𝜒 𝑘 + 𝐵𝑖 𝑢 𝑘 + 𝑤 𝑘 , (4)
ɀ 𝑘 = 𝐶𝑖 𝜒 𝑘 + 𝑣 𝑘, (5)
with 𝐴𝑖 = [
1 −𝑇
0 1
] , 𝐵𝑖 = [
𝑇
0
], and 𝐶𝑖 = [1 0] ,
In the above equation, 𝜒 𝑘 is the processed state in time k, 𝐴𝑖 is the state transition matrix to i-axis, 𝐵𝑖
is the control matrix to i-axis, 𝑢 𝑘 is the input vector, and 𝑤 𝑘 is the process noise. Then, ɀ 𝑘 is the measurement
model, 𝐶𝑖 is the measurement matrix to i-axis, and 𝑣 𝑘 is the measurement noise for gyroscope signal. T is a
sampling time data and i is the axis index (i ϵ {x,y,z}). For this study, the input vectors 𝑢 𝑘 is the raw data of
the gyroscope sensor of each axis 𝜔𝑖 after it calibration.
Kalman Filter assumed the process noise and measurement noise uncorrelated each other, with their
average values are zero. The process noise covariance 𝑄 and measurement noise covariance R are expressed
as,
𝑄 = 𝐸(𝑤 𝑘. 𝑤 𝑘
𝑇
) = 𝛾, (6)
𝑅 = 𝐸(𝑣 𝑘. 𝑣 𝑘
𝑇
) = 𝑐𝑜𝑣 (𝜔𝑖). (7)

IJECE ISSN: 2088-8708 
2599
By calculating all axes, the matrices A, C, 𝑄, and R of the Kalman Filter for gyroscope sensor data
are as follows:
𝐴 = [
𝐴 𝑥 02 02
02 𝐴 𝑦 02
02 02 𝐴 𝑧
] , 𝐶 = [
𝐶 𝑥
01×2
01×2
01×2
𝐶 𝑦
01×2
01×2
01×2
𝐶𝑧
] , 𝑄 = 𝛾 [
1
02
02
02
1
02
02
02
1
] , 𝑅 = 𝑐𝑜𝑣 [
𝜔 𝑥 0 0
0 𝜔 𝑦 0
0 0 𝜔𝑧
],
where 02 is 2 × 2 zero matrix, 01×2 is 1 × 2 zero matrix.
2.3. Kalman Filter Design for Accelerometer Data
As in the gyroscope, the Kalman Filter is also used to reduce the noise of accelerometer signal. The
accelerometer data is the linear acceleration of each axis. The process model and measurement model of the
Kalman Filter for accelerometer sensor can be expressed in equation (7) and (8) as,
𝜁𝑘+1 = 𝐷𝑖 𝜁𝑘 + 𝐹𝑖 𝑢 𝑘 + ϛ 𝑘 , (8)
ʑ 𝑘 = 𝐻𝑖 𝜁𝑘 + 𝑒 𝑘, (9)
where 𝐷𝑖 = [
1
0
0
𝑇
1
0
−0.5𝑇2
−𝑇
1
], 𝐹𝑖 = [
0.5𝑇2
𝑇
0
], and 𝐻𝑖 = [1 0 0] .
In the above equations, 𝜁𝑘 is the process state in time k, 𝐷𝑖 is the state transition matrix to i-axis, Fi
is the control matrix to i-axis, uk is the input vector, and ϛ 𝑘 is the process noise. Then, ʑ 𝑘 is the measurement
model, Hi is the measurement matrix to i-axis, and 𝑒 𝑘 the measurement noise for accelerometer signal. T is
time sampling data and i is an index axis (i ϵ {x,y,z}). As in the gyro sensor, the input vector 𝑢 𝑘 is the raw
data of the accelerometer sensor for each axis ai after it calibrated, then the offset is reduced.
The process noise covariance 𝑄 and the measurement noise covariance R are expressed as:
𝑄 = 𝐸(ϛ 𝑘. ϛ 𝑘
𝑇) = 𝛽, (10)
𝑅 = 𝐸(𝑒 𝑘. 𝑒 𝑘
𝑇
) = 𝑐𝑜𝑣 (𝑎𝑖). (11)
By calculating all axes, the matrices D, H,𝑄, and R on Kalman Filter for accelerometer sensor data
are set as follows:
𝐷 = [
𝐵𝑥 02 02
02 𝐵𝑦 02
02 02 𝐵𝑧
] , 𝐻 = [
𝐻𝑥
01×2
01×2
01×2
𝐻 𝑦
01×2
01×2
01×2
𝐻𝑧
] , 𝑄 = 𝛽 [
1
02
02
02
1
02
02
02
1
] , 𝑅 = 𝑐𝑜𝑣 [
𝑎 𝑥 0 0
0 𝑎 𝑦 0
0 0 𝑎 𝑧
] ,
where 02 is 2 × 2 zero matrix, 01×2 is 1 × 2 zero matrix.
2.3. Zero Velocity Compensation Algorithms
In order to enable the sensor automatically determines the condition while at rest, we compare the
standard deviation of the sensor data (𝜎 𝑠) within an n-sized sample at time k with a threshold value (𝜎𝑡ℎ) as
described in Algorithm 1. If the standard deviation of data is less than 𝜎𝑡ℎ, the sensor is considered in the rest
condition so that the speed is assumed zero (velocity=0) and the position is assumed zero (position≈0). If the
standard deviation of data is not less than 𝜎𝑡ℎ, the sensor is moving. The stationary detection algorithm can be
shown as in Algorithm 1 [34].
In the inertial sensor, drift problem is a problem in an inertial navigation system of which the
computed position moves even if the vehicle does not move. This phenomenon occurs because of the
integration process of the sensor signal that is very sensitive to the noise. The noise will propagate to the
position rapidly via the integration process. To reduce this phenomenon, we used the Algorithm 2. Firstly, the
algorithm will compute the moving mean by creating series of averages of different subsets of the full data
set. In this study, moving mean is used to smooth out short-term fluctuations and highlight longer-term trends
of the raw data. To compensate the bias error, we used the mean of static data. If the moving mean is less

 ISSN: 2088-8708
2600
than zero, the velocity is added with the mean of the static condition data. Then, if the moving mean is higher
than zero, the velocity is subtracted with the mean of the static condition data. Moreover, to identify the zero
velocity, we compare the velocity with the threshold. If the velocity is less than the threshold, the velocity is
considered zero. We also used the linear fitting to compensate the linear trend of the signal data due to many
factors such as temperature.
Algorithm 1. Stationary detection
1. 𝜔 𝑘 = 𝜔 𝑘−1 ;
2. 𝛼 𝑘 = 𝛼 𝑘−1 + ω 𝑘 𝑇 ;
3. if 𝜎 𝑠 (k)<𝜎 𝑡 ℎ then
4. 𝜔 𝑘 = 0;
5. 𝛼 𝑘 = 𝛼 𝑘−1;
6. else
7. 𝜔 𝑘 = 𝜔 𝑘−1;
8. 𝛼 𝑘 = 𝛼 𝑘−1 + ω 𝑘 𝑇;
9. end if
Algorithm 2. Zero Velocity Compensation (ZVC)
1. 𝑀 𝑚 =
1
𝑛
∑ 𝜔 𝑘−1
𝑛−1
𝑘=0 ;
2. if 𝑀 𝑚 < 0 then
3. 𝜔 𝑘 = 𝜔 𝑘 + |𝜇0| ;
4. elseif 𝑀 𝑚 > 0 then
5. 𝜔 𝑘 = 𝜔 𝑘 − |𝜇0| ;
6. end
7. if 𝜔 𝑘 < 𝑡ℎ then
8. 𝜔 𝑘 = 0;
9. end if
In the above Algorithm, Mm is the moving mean, n is data sample (in this study, n=30), 𝜔 𝑘 is the
velocity data, 𝜇0 is the mean of the static condition data, th is the threshold, k is the time index, and T is the
sampling time of the data sensor. Furthermore, the Kalman Filter input is updated as in Algorithm 2.
3. RESULTS AND DISCUSSION
3.1. Static Test
The tests are performed at the sensor on the quadrotor at the static condition as (x,y,z)=(0,0,0) m
with rotation angle is fixed as (roll,pitch,yaw)=(0,0,0) rad. In the stationary or a static condition, ideally, the
signal data from the accelerometer and gyroscope will show zero. The tests are conducted by comparing the
sensor signal before and after filtering and compensation is added.
Figure 3 (a) shows the gyroscope signal (raw data) of the X, Y, and Z axes as the quadrotor at the
static condition. The spikes show that the signal is noisy due to force, sensor dynamics, and others. The
minimum and maximum amplitude of the signal in the X axis is -0.0020 and 0.0026; in the Y axis is -0.0067
and 0.0061; and in the Z axis is -0.0060 and 0.0069, in units of rad/s for all. Figure 3 (b) shows the gyroscope
data after filtering and compensation. Ideally, at the static condition, the signal amplitude is zero. The
amplitude of the signals in the X, Y, and Z axes are zero. Moreover, the spike noise has been removed from
the signals. This shows that the algorithm is able to remove the signal noise in the stationary condition.
(a) The gyroscope signal before filtering (b) The gyroscope signal after filtering
Figure 3. The gyroscope signal of static condition
Figure 4 (a) shows the accelerometer signal (raw data) of the X, Y, and Z axes as the quadrotor at
static condition. The signals of the three axes are not equal zero. Moreover, there is DC value (offset). The
0 200 400 600 800 1000 1200 1400 1600 1800 2000
-5
0
5
x 10
-3
Angular rate signal of the axis-X
Sampling
Output(rad/s)
0 200 400 600 800 1000 1200 1400 1600 1800 2000
-0.01
0
0.01
Angular rate signal of the axis-Y
Sampling
Output(rad/s)
0 200 400 600 800 1000 1200 1400 1600 1800 2000
-0.01
0
0.01
Angular rate signal of the axis-Z
Sampling
Output(rad/s)
v-roll
v-pitch
v-yaw
0 200 400 600 800 1000 1200 1400 1600 1800 2000
-1
0
1
Angular rate signal of the axis-X
Sampling
Output(rad/s)
0 200 400 600 800 1000 1200 1400 1600 1800 2000
-1
0
1
Angular rate signal of the axis-Y
Sampling
Output(rad/s)
0 200 400 600 800 1000 1200 1400 1600 1800 2000
-1
0
1
Angular rate signal of the axis-Z
Sampling
Output(rad/s)
v-roll
v-pitch
v-yaw

IJECE ISSN: 2088-8708 
2601
amplitude of the signal in the Z axis is higher than the amplitude of the signal in the X and Y axes because of
the Z axis measure the acceleration force that is affected by gravity. Ideally, the amplitude of the signal data
in the Z axis is read 9.812 m/s2
. The accelerometer reading in the X and Y axes are not zero could be due to
the improper perpendicular of the sensor to the earth frame. At the stationary, ideally, the amplitude of the
signal in the X, Y, and Z axes are zero. Figure 4 (b) shows the accelerometer data after filtering and
compensation. We can see the signal amplitude of the X and Y axes are zero and the Z axis is 9.812 m/s2
.
Moreover, the bias (offset) can be compensated and the noise can be removed. This shows the filter and
compensation can work quite well so the noise and bias error due to gravity can be eliminated.
(a) The accelerometer signal before filtering (b) The accelerometer signal after filtering
Figure 4. The accelerometer signal of static condition
3.2. Angular Movement Test
Angular movement test aims to test the algorithm on the gyroscope sensor. The algorithm estimates
the angular movement (roll, pitch, and yaw) in the global frame. The angular movement test is started when
the sensor in a stationary (not moving). In this condition, the gyroscope signal is recorded as the stationary
data. While at stationary, the quadrotor is rotated along one certain axis as is illustrated in Figure 1 (a). Then,
the gyroscope signal is recorded as data of the angular movement.
Figure 5 (a) shows the gyroscope signal when it is rotated about the X axis. The signals are the
angular velocity of the X, Y, and Z axes. The signal of the X axis shows higher than others because the
quadrotor is rotated about the X axis while the angular velocity of the Y and Z axes ideally close to zero.
Figure 5 (b) shows the roll angle estimation results. The dotted line curve shows the test results by KF
(without compensation) while the solid line curve shows the test results by KF-ZVC (with compensation). In
Figure 5 (b), the roll angle estimation results by KF can be seen to have drift error while by KF-ZVC
relatively has no drift error. It shows that the algorithm is able compensate drift error in the gyroscope data.
(a) The gyroscope signal of roll movement (b) The roll estimation signal
0 200 400 600 800 1000 1200 1400 1600 1800 2000
0
0.2
0.4
Linear acceleration on the axis-X
Sampling
Accel.(m/s2)
measurement-ax
0 200 400 600 800 1000 1200 1400 1600 1800 2000
0
0.5
1
Linear acceleration on the axis-Y
Sampling
Accel.(m/s2)
measurement-ay
0 200 400 600 800 1000 1200 1400 1600 1800 2000
0
5
10
Linear acceleration on the axis-Z
Sampling
Accel.(m/s2)
measurement-az
0 200 400 600 800 1000 1200 1400 1600 1800 2000
-1
0
1
Linear acceleration on the axis-X
Sampling
Accel.(m/s2)
0 200 400 600 800 1000 1200 1400 1600 1800 2000
-1
0
1
Linear acceleration on the axis-Y
Sampling
Accel.(m/s2)
0 200 400 600 800 1000 1200 1400 1600 1800 2000
8
10
12
Linear acceleration on the axis-Z
Sampling
Accel.(m/s2)
Estimation-Ax
Estimation-Ay
Estimation-Az
0 500 1000 1500 2000 2500
-5
0
5
10
Angular rate signal
Sampling
Output(rad/s)
0 500 1000 1500 2000 2500
-2
-1
0
1
2
Estimation roll angle
Sampling
Output(rad)
v-roll
v-pitch
v-yaw
with compensation
without compensation
0 500 1000 1500 2000 2500
-5
0
5
10
Angular rate signal
Sampling
Output(rad/s)
v-roll
v-pitch
v-yaw
0 500 1000 1500 2000 2500
-2
-1
0
1
2
Sampling
Output(rad)
with compensation

 ISSN: 2088-8708
2602
(c) The gyroscope signal of 46° rolling (d) The roll estimation of 46° rolling
Figure 5. The gyroscope signal of roll movement test
When the sensor is rotated 46° about the X axis, the gyroscope signal is shown in Figure 5 (c). It is
seen that the signal of the X axis shows higher than others because the quadrotor is rotated about the X axis
while the signal along the Y and Z axes are very close to zero. Figure 5 (d) shows the roll angle estimation
result. The dotted line curve shows the estimation result with KF (without compensation) while the solid line
curve shows the estimation result with KF-ZVC (with compensation). The estimation results of roll angle by
KF shows significant drift error while the estimation by KF-ZVC is able to reject drift error in the signal.
Please be noted that the drift error is started at iteration 160. In this iteration, the roll estimation by KF-ZVC
is 0.00024 rad (0.0014°) while the roll estimation by KF is 0.0110 rad (0.6303°). At iterations 645, the roll
estimation by KF-ZVC is 0.0100 rad (0.0573°) while the roll estimation by KF is 0.0459 rad (2.6299°).
Furthermore, the final roll estimation by KF-ZVC is 0.8361 rad (47.9050°) while the final roll estimation by
KF is 0.8877 rad (50.8615°) of angle reference 46°. In this case, the relative error by KF-ZVC (with
compensation) is 1.9050° while by KF (without compensation) is 4.8615°. It shows the algorithm is able to
improve the accuracy of angle estimation.
3.3. Translation Movement Test
Translation movement test is started as the quadrotor’s at static condition (not moving). Then, from
the static condition (as initial position (x,y)=(0,0)) the quadrotor is moved to the goal position in coordinate
(x,y)=(2,1) m as is illustrated in Figure 1 (b). The test aims to test of the algorithm to estimate the translation
movement in the X-Y direction. Furthermore, the estimation results are validated with the true distance to
determine the accuracy of the algorithm.
Figure 6 (a) and (b) shows the accelerometer signal in the X and Y axes. The dotted line curve
shows the accelerometer data before filtering while the solid line curve shows the accelerometer data after
filtering and compensation. We can see the noise and bias error in the signal before filtering. After the signal
is done the bias compensation and filtering, the bias error and noise can be reduced. This shows the algorithm
able to compensate the bias (offset) error.
(a) Acceleration signal of X axis
(b) Acceleration signal of Y axis
(c) Positon estimation of quadrotor in translation test (d) Quadrotor trajectory on 2D plane
Figure 6. Acceleration signal, position estimation, and quadrotor trajectory
0 500 1000 1500
-0.2
0
0.2
0.4
0.6
Angular rate signal
Sampling
Output(rad/s)
0 500 1000 1500
-0.5
0
0.5
1
Sampling
Output(rad)
v-roll
v-pitch
v-yaw
with compensation
0 500 1000 1500
-0.2
0
0.2
0.4
0.6
Sampling
Output(rad/s)
0 500 1000 1500
-0.5
0
0.5
1
Sampling
Output(rad)
v-roll
v-pitch
v-yaw
with compensation
0 100 200 300 400 500 600 700 800
-2
-1
0
1
2
Time-varying Kalman Filter response
Sampling
Acceleration(m/s2)
0 100 200 300 400 500 600 700 800
-0.5
0
0.5
1
1.5
Sampling
Output
Measurement and Estimation Error
Measurement-ax
Estimation KF-Ax
Meas.error-X
Est.Error-X
0 100 200 300 400 500 600 700 800
-2
-1
0
1
2
Time-varying Kalman Filter response
Sampling
Acceleration(m/s2)
0 100 200 300 400 500 600 700 800
-3
-2
-1
0
1
Sampling
Output
Measurement and Estimation Error
Measurement-ay
Estimation KF-Ay
Meas.error-Y
Est.Error-Y
0 100 200 300 400 500 600 700 800
-2
0
2
4
Position estimation
Sampling
Displacement(m)
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
-0.5
0
0.5
1
Quadrotor trajectory
X-plane (m)
Y-plane(m)
KF-X (x1)
KF-Y (x1)
LPF-X (x10)
LPF-Y (x10)
Reference
Trajectory
0 100 200 300 400 500 600 700 800
-2
0
2
4
Position estimation
Sampling
Displacement(m)
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
-0.5
0
0.5
1
Quadrotor trajectory
X-plane (m)
Y-plane(m)
KF-X (x1)
KF-Y (x1)
LPF-X (x10)
LPF-Y (x10)
Reference
Trajectory
x=160
y=0.0110
x=645
y=0.0459
x=1400
y=0.8877

IJECE ISSN: 2088-8708 
2603
Figure 6 (c) shows the position estimation results of the quadrotor. The test is performed by
comparing the estimation results by LPF (low pass filter) and KF (Kalman Filter) with compensation. The
experiments were performed indoor with 713 data sampling of 0.01 seconds rate. The curve of LPF-X and
LPF-Y show the test results by LPF in the X axis direction and in the Y axis direction respectively while the
curve of KF-X and KF-Y show the test results by KF in the X axis direction and in the Y axis direction
respectively. The final position along the X direction by KF is 1.9625 m while by LPF is 35.8510 m, with the
true position, is 2 m. The final position along the Y direction by KF is 0.9624 m while by LPF is 17.9774 m,
with the true position, is 1 m. Based on experiments indoor in this case, the KF gave significant improvement
in accuracy than the LPF. This shows the algorithm design able to improve the accuracy of the position
estimation results.
Figure 6 (d) shows the trajectory plot of the quadrotor on the X-Y plane by KF. As can be seen,
there is a difference between the results trajectory and the reference. This is caused by the sensor is not
exactly moved ﬁt with the true testing because there is no accurate test equipment. Frequency sampling may
be less fast, so the data cannot represent the actual acceleration accurately. In addition, the data
characteristics of accelerometer sensor may be not linear or not consistent and the data of each sensor axis is
not really independent. Table 1 shows the position estimation test of the X-Y direction movement by
variations R and Q. The cov (ai) represents the acceleration data covariance of 200 data sample (ax for the X
axis and ay for the Y axis) while the Q value is a number is determined intuitively. As shown in Table 1, by
the R value is 10 times cov (ai) while the Q value is 0.00617 is obtained the KF-X accuracy (the X direction)
and KF-Y accuracy (the Y direction) are 98.12 % and 96.24 % respectively with the root means square error
(RMSE) are 0.0014 m and 0.0014 m respectively.
Table 1. Position estimation with KF for X-Y movement test
Q=β R
True
X(m)
True
Y(m)
Estimation
KF-X (m)
Estimation
KF-Y (m)
RMSE
KF-X
(m)
RMSE
KF-Y
(m)
Accur.
KF-X
(%)
Accur.
KF-Y
(%)
(0.00617)1/4
cov (ai) 2 1 3.5436 2.0508 0.0626 0.0394 22.82 -5.08
(0.00617)1/2
cov (ai) 2 1 2.6569 0.4743 0.0246 0.0197 67.15 47.43
0.00617 cov (ai) 2 1 2.0102 0.7610 3.8355e-04 0.0090 99.49 76.1
0.00617 10×cov (ai) 2 1 1.9625 0.9624 0.0014 0.0014 98.12 96.24
0.00617 100×cov (ai) 2 1 1.9491 0.9311 0.0019 0.0026 97.45 93.11
0.00617 1000×cov (ai) 2 1 1.9451 0.8964 0.0021 0.0039 94.51 89.64
4. CONCLUSION
Based on the test results, the Kalman Filter design is able to reduce noise on the IMU sensor data.
The zero velocity compensation and bias compensation are able to reduce the effects of drift due to
integration and able to improve the position estimation of the quadrotor. The algorithms can provide the
translation estimation and angular rotation estimation of the quadrotor with reasonable accuracy than without
Kalman Filter and compensation. Based on the experiments, the systems can provide the angular accuracy of
about 95 % and the translation accuracy of about 90 %. Moreover, based on the test result it is showed that
the accelerometer and the gyroscope with Kalman Filter perform significant improvement at least for short
period of time. The algorithm has been tested off-line. For the real-time application, this algorithm may have
some limitations.
REFERENCES
[1] S. G. Gupta, M. M. Ghonge, and P. M. Jawandhiya, “Review of Unmanned Aircraft System ( UAS ),” Int. J. Adv.
Res. Comput. Eng. Technol., vol. 2, no. 4, pp. 1646–1658, 2013.
[2] K. Schauwecker and A. Zell, “On-Board Dual-Stereo-Vision for Autonomous Quadrotor Navigation,” in 2013
International Conference on Unmanned Aircraft Systems (ICUAS), 2013, pp. 333 – 342.
[3] K.-H. Oh and H.-S. Ahn, “Extended Kalman Filter with Multi-frequency Reference Data for Quadrotor
Navigation,” in 15th International Conference on Control, Automation and Systems (ICCAS 2015), 2015, no. Iccas,
pp. 201–206.
[4] J. D. Barton, “Fundamentals of Small Unmanned Aircraft Flight,” John Hopkins APL Tech. Dig., vol. 31, no. 2, pp.
132–149, 2012.
[5] A. Puri, K. P. Valavanis, and M. Kontitsis, “Statistical Profile Generation for Traffic Monitoring Using Real-time
UAV based Video Data,” in 2007 IEEE Mediterranean Conference on Control and Automation, Athens. Greece,
2007, no. 27–29 July.
[6] J. M. M. Netot, R. A. Paixao, and L. R. L. Rodrigues, “A Surveillance Task for a UAV in a Natural Disaster
Scenario,” in 2012 IEEE International Symposium on Industrial Electronics (ISIE), 2012, pp. 1516–1522.
[7] J. Amenyo, D. Phelps, O. Oladipo, F. Sewovoe-ekuoe, S. Jadoonanan, T. Tabassum, S. Gnabode, T. D. Sherpa, M.
Falzone, A. Hossain, and A. Kublal, “MedizDroids Project : Ultra-Low Cost , Low-Altitude , Affordable and

 ISSN: 2088-8708
2604
Sustainable UAV Multicopter Drones For Mosquito Vector Control in Malaria Disease Management,” in 2014
IEEE Global Humanitarian Technology Conference, 2014.
[8] J. Zhang, L. Liu, B. Wang, X. Chen, and Q. Wang, “High speed Automatic Power Line Detection and Tracking for
a UAV-Based Inspection,” in 2012 IEEE International Conference on Industrial Control and Electronic
Engineering, 2012, pp. 266–269.
[9] K. J. Rogers and A. Finn, “Frequency Estimation for 3D Atmospheric Tomography using Unmanned Aerial
Vehicles,” in 2013 IEEE ISSNIP, 2013, pp. 390–395.
[10] S. Gupte and J. M. Conrad, “A survey of quadrotor Unmanned Aerial Vehicles,” in 2012 Proceedings of IEEE
Southeastcon, 2012, pp. 1–6.
[11] H. Tnunay, M. Q. Abdurrohman, Y. Nugroho, R. Inovan, A. Cahyadi, and Y. Yamamoto, “Auto-Tuning
Quadcopter Using Loop Shaping,” in 2013 International Conference on Computer, Control, Informatics and Its
Applications (IC3INA), 2013, pp. 111–115.
[12] G. Yanhui, X. Qiangui, H. Shousong, and J. Xiao, “Flight Control System Simulation Platform for UAV based on
Integrating Simulink with Stateflow,” TELKOMNIKA, vol. 10, no. 5, pp. 985–991, 2012.
[13] S. Amini and A. A. Akbari, “Adaptive Sliding Mode Controller Design For Attitude Small UAV,” Int. J. Robot.
Autom., vol. 4, no. 3, pp. 219–229, 2015.
[14] A. Budiyanto, A. Cahyadi, T. B. Adji, and O. Wahyunggoro, “UAV Obstacle Avoidance Using Potential Field
under Dynamic Environment,” in 2015 International Conference on Control, Electronics, Renewable Energy and
Communications (ICCEREC), 2015, pp. 187–192.
[15] W. Fei, C. Ben-mei, and L. E. E. T. H, “A Comprehensive UAV Indoor Navigation System Based on Vision
Optical Flow and Laser FastSLAM,” Acta Autom. Sin., vol. 39, no. 11, pp. 1889–1899, 2013.
[16] S. T. Goh, O. Abdelkhalik, and S. A. R. Zekavat, “A Weighted Measurement Fusion Kalman Filter implementation
for UAV navigation,” Aerosp. Sci. Technol., vol. 28, no. 1, pp. 315–323, 2013.
[17] C. Troiani, A. Martinelli, C. Laugier, and D. Scaramuzza, “Low computational-complexity algorithms for vision-
aided inertial navigation of micro aerial vehicles,” Rob. Auton. Syst., vol. 69, pp. 80–97, 2015.
[18] Y. Luol, C. C. Tsang, G. Zhang, Z. Dong, G. Shil, S. Y. Kwok, W. J. Lil, P. H. W. Leong, and M. Yiuwong, “An
Attitude Compensation Technique for a MEMS Motion Sensor Based Digital Writing Instrument,” in Proceedings
of the 1st IEEE Conference on Nano/Micro Engineered and Molecular Systems, 2006, pp. 909–914.
[19] Y. M. Mustafah, A. W. Azman, and F. Akbar, “Indoor UAV Positioning Using Stereo Vision Sensor,” Procedia
Eng., vol. 41, no. Iris, pp. 575–579, 2012.
[20] L. Rodolfo, G. Carrillo, A. Enrique, D. López, R. Lozano, and C. Pégard, “Combining Stereo Vision and Inertial
Navigation System for a Quad-Rotor UAV,” Springer, J. Intell. Robot. Syst., pp. 373–387, 2012.
[21] Y. Song, B. Xian, Y. Zhang, X. Jiang, and X. Zhang, “Towards autonomous control of quadrotor unmanned aerial
vehicles in a GPS-denied urban area via laser ranger finder,” Opt. - Int. J. Light Electron Opt., vol. 126, no. 23, pp.
3877–3882, 2015.
[22] W. Zheng, F. Zhou, and Z. Wang, “Robust and Accurate Monocular Visual Navigation Combining IMU for a
Quadrotor,” IEEE/CAA J. Autom. Sin., vol. 2, no. 1, pp. 33–44, 2015.
[23] B. D. Scaramuzza and F. Fraundorfer, “Visual Odometry,” IEEE Robotic & Automation Magazine, no. December,
pp. 80–92, Dec-2011.
[24] C. Kownacki, “Design of an adaptive Kalman filter to eliminate measurement faults of a laser rangefinder used in
the UAV system,” Aerosp. Sci. Technol., vol. 41, pp. 81–89, 2015.
[25] Y. Hou and C. Yu, “Autonomous Target Localization using Quadrotor,” in The 26th Chinese Control and Decision
Conference (2014 CCDC), 2014, pp. 864–869.
[26] X. Bin, Y. Sen, and Z. Xu, “Control of a quadrotor helicopter using the COMPASS ( BeiDou ) system and on-
board vision system,” Opt. - Int. J. Light Electron Opt., vol. 127, no. 17, pp. 6829–6838, 2016.
[27] S. Agarwal, S. B. Lazarus, and A. Savvaris, “Monocular Vision Based Navigation and Localisation in Indoor
Environments,” in IFAC Proceedings Volumes, 2012, vol. 45, no. 1, pp. 97–102.
[28] D. Abeywardena, S. Kodagoda, G. Dissanayake, and R. Munasinghe, “Improved State Estimation in Quadrotor
MAVs : A Novel Drift-Free Velocity Estimator,” IEEE Robot. Autom. Mag., vol. 20, no. 4, pp. 32–39, 2013.
[29] A. R. Vetrella, A. Savvaris, G. Fasano, and D. Accardo, “RGB-D Camera-Based Quadrotor Navigation In GPS-
Denied and Low Light Environments Using Known 3D Markers,” in International Conference on Unmanned
Aircraft Systems (ICUAS), 2015, pp. 185–192.
[30] M. Achtelik, A. Bachrach, R. He, S. Prentice, and N. Roy, “Autonomous Navigation and Exploration of a
Quadrotor Helicopter in GPS-denied Indoor Environments,” in Robotics: Science and Systems Conference., 2008,
pp. 1–12.
[31] H. Du, “Design of Adaptive Kalman Filter Algorithm in Integrated Navigation System for Land Vehicles,” in
Proceeding of 2013 IEEE International Conference on Mechatronic and Automations August 4 - 7, Takamatsu,
Japan, 2013, pp. 1492–1496.
[32] D. Simon, “Kalman Filtering,” Embedded Systems Programming, vol. 14, no. June, pp. 72–79, 2001.
[33] R. Faragher, “Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation,” IEEE Signal
Process. Mag., vol. 29, no. September, pp. 128–132, 2012.
[34] C. Chiu, “Error Reduction Techniques for a MEMS Accelerometer-based Digital Input Device,” The Chinese
University of Hong Kong, 2008.

Inertial Navigation for Quadrotor Using Kalman Filter with Drift Compensation

More Related Content

What's hot (19)

Similar to Inertial Navigation for Quadrotor Using Kalman Filter with Drift Compensation (20)

More from IJECEIAES (20)

Recently uploaded (20)

Inertial Navigation for Quadrotor Using Kalman Filter with Drift Compensation