Inertial Navigation for Quadrotor Using Kalman Filter with Drift Compensation

ABSTRACT


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  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.

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.

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: 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: 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. 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.

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, with and 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, where 0 2 is 2 × 2 zero matrix, 0 1×2 is 1 × 2 zero matrix.

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, where ], and In the above equations,   is the process state in time k,   is the state transition matrix to i-axis, F i is the control matrix to i-axis, u k is the input vector, and ϛ  is the process noise.Then, ʑ  is the measurement model, H i 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 a i after it calibrated, then the offset is reduced.
The process noise covariance  and the measurement noise covariance R are expressed as: By calculating all axes, the matrices D, H,, and R on Kalman Filter for accelerometer sensor data are set as follows: where 0 2 is 2 × 2 zero matrix, 0 1×2 is 1 × 2 zero matrix.

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 IJECE Vol. 7, No. 5, October 2017 : 2596 -2604 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 = 0; 5.
= 0; 9. end if In the above Algorithm, M m 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.

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.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/s 2 .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/s 2 .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.

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.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.

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.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.8510m, 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 fit 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 (a i ) represents the acceleration data covariance of 200 data sample (a x for the X axis and a y 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 (a i ) 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.

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.

Figure 2 .Figure 1 .
Figure 2. The schematic of experimental setup and data processing

Figure 3 .
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

( a )Figure 4 .
Figure 4.The accelerometer signal of static condition

Figure 5 .
Figure 5.The gyroscope signal of roll movement test

Figure 6 .Figure 6
Figure 6.Acceleration signal, position estimation, and quadrotor trajectory , and R of the Kalman Filter for gyroscope sensor data are as follows: IJECE ISSN: 2088-8708  Inertial Navigation for Quadrotor Using Kalman Filter with Drift Compensation (Lasmadi) 2599 By calculating all axes, the matrices A, C,

Table 1 .
Position estimation with KF for X-Y movement test