Kalman Filter Based Sensor Fusion for Altitude Estimation of Aerial Vehicle

Stabilization of Aerial Vehicles requires altitude estimation. In this paper sensor fusion method is developed for aerial vehicles which focuses on estimation of the altitude. Altitude measurement using sensors is too noisy and biased so Kalman filter based sensor fusion is used to estimate altitude. The proposed method is divided into two parts: attitude estimation and altitude estimation. The method is implemented using IMU and barometer sensors. The results show that altitude is estimated accurately and errors are modelled in few seconds after initialization.


Introduction
Sensor fusion is a technique which combines data from different sensor to get an optimized/ best estimates/ results. In air navigation/ aerial vehicles different sensors are used, data from sensors are usually noisy and biased. To achieve better results sensor fusion technique is applied. In this paper a method is proposed to get vertical height of aerial vehicles using MEMS (micro electro mechanical system) sensor I M U (inertial measurement units) accelerometer, gyroscope and barometer. However, the data retrieve from the inertial measurement units and barometer are noisy, due to which vertical height estimate or orientation/ attitude estimation is not easy to achieve. If only relay on inertial measurement unit sensors data to get vertical height/altitude, it may suffer from integration drift which cause positioning error. Changes in atmospheric pressure by unpredictable atmospheric condition and rapid changes in altitude of aerial vehicles cause the barometric altimeter data to be noisy and their accuracy is poor. Therefore, to stabilize the position and orientation of IMU sensors it is customary to use fusion method [1]. Applications of the sensor fusion algorithm include vertical trajectory determination in navigation, control of robotic systems, fall detection and sports. [2] To acquire vertical height position barometric altimeter is used with 3 Axis accelerometer and 3 Axis gyroscope. Hardware part consists of MPU 6050 and BMP 180 barometer, these sensors are employed on a quadcopter to extract real-time data using Arduino DUE. MPU 6050 (6-DOF) uses I2C or (  interface with Arduino to measure the barometric pressure. Barometer based algorithm obtains the height change using the observed barometric pressure change [3] by using Equation (11). The IMU-barometer fusion method consists of two parts: first one is orientation /attitude estimation and the other is altitude /height estimation. Kalman filter (KF) and Extended Kalman filter (EKF) are used to measure attitude and altitude of aerial vehicles by collecting the requisite data from raw accelerometer and gyroscope. Attitude measured by IMU and barometer is used to estimate vehicles altitude/ height. This paper presents the algorithm for real experiment, the proposed methodology, a collection of available data sources and data processing techniques to provide better navigation solution [4].

System Dynamics
A causal, linear and time-invariant system can be described in state-space form using the following format; x is a state vector, t u is input vector and t z is measurement vector.
A , B and C are system matrices: A relates the current states to the next states, B relates inputs to the next states, and C relates the system states to the measured states. t w is additive process and t v is measurement noise, b is the gyro bias with respect to  and  at time t .The system matrices may be designed on following considerations: Varying gyro bias at every instant in time causes drift so this should be eliminated, to get an updated estimate of angular position the bias is subtracted from transformed angular rates of changed measured by the gyros. The general state space form is:

Accelerometer Model
The accelerometer measures gravitational acceleration by resolving forces through basic trigonometry. Estimate of pitch and roll can be calculated as follows:

Gyroscope Model
The gyroscope measures angular rates of change around each axis with respect to the body frame. Assuming noise-free measurements, we could integrate these raw measurements to get the angular position relative to the body frame. If we were to integrate this, we would add up all the small errors due to noise, which then would accumulate over time and become unbounded in magnitude. This is commonly known as gyroscopic drift. We cannot simply integrate the angular rates of change given by the gyroscope to get our roll, pitch, and yaw angles in the inertial frame. The rates of change of Euler angles depend on more than one body frame angular velocity. To get the rates of change in the inertial frame, we need to transform our states using the following matrix.
1 sin tan cos tan 0 cos sin 0 sin sec cos sec To get the inertial attitude, we will use this transformation in a sensor fusion algorithm. , usually referred to as p, q, and r respectively. ,  -Roll angle (rotation about X axis in degrees) and pitch angle (rotation about Y axis in degrees) respectively.

Barometer Model
Barometer pressure sensor is used to find altitude with change in atmospheric pressure. The barometric Altimeter are used in most of aerial vehicles. The pressure is measured in Pascal then converted to change in height in meters by [5]  P is the standard pressure given as 101.325 kPa and P is the measured pressure in Pascal. Errors related to barometric height estimates are corrected through barometric errors [6].

Sensor Fusion
Sensor fusion is a technique of combining data from multiple sensors to minimize the amount of uncertainty in order to provide improved and best estimate of the state. Here the uncertainty minimization means the estimate would be more accurate and more reliable. Both the sensors being used are not of ideal accuracy. Accelerometers provide good results when the system is in rest state and gyroscopes are fine for short intervals only, but over longer periods of time, both sensors will provide lesser reliable estimates of pitch and roll. Therefore, sensor fusion method is used to mitigate the estimation uncertainties.

Kalman Filter
The Kalman Filter is also a Linear Quadratic Estimator. It is a state estimator which is optimal in the sense that it tries to minimize a quadratic cost function. Kalman Filter equations are divided to two group of equations: Prediction and Update. Prediction:

Attitude Estimation
The attitude can be estimated by fusion of integrated angular velocities and acceleration measurements [7]. It is a 3D orientation of the IMU with respect to the Earth coordinates. In the controlling of aerial vehicles, it is compulsory to have correct feedback states, but there are some practical impediments associated with IMU sensors like accelerometer and gyroscope. Gyroscopes measure angular rate of change but not the angular position. Similarly, accelerometers measure gravitational acceleration but not the linear acceleration. Measurements computed by the sensors are noisy and biased. Body-frame states need to be transformed to the inertial reference frame.

Altitude Estimation
Attitude estimation resolves acceleration vector into vertical acceleration which is used for estimating the IMU barometer system's Altitude. The state vector for Altitude estimation consists of altitude, accelerometer biases and acceleration scale factor [8].

Results
The system is implemented on MATLAB based computer program where real data is collected from sensors using Arduino mounted on Ariel vehicle. We acquired different data sets and performed experiments several number of times.
In the first part, data is used to estimate Attitude of vehicle, which results in pitch, and roll angle of the vehicle in degrees. Kalman filter is used to fuse the data from accelerometer and gyroscope to find out best estimate of the pitch and roll angle. The EKF is used for combining IMU-Barometer to estimate the vertical height of aerial vehicle. The measurements from sensors which are affected by noise are used in attitude estimation and with EKF  6 algorithm process these characterize the noise to accurately estimate the Altitude. Accuracy of estimates may be increased with the implementation of EKF. MATLAB simulation results are shown here:

Conclusion
In this paper we proposed an algorithm for sensor fusion based on IMU and barometer for Altitude estimation. The Kalman filter is used as states estimator. The Kalman filter performs a method of Altitude estimation with the help of IMU and barometer together. The barometer is used with IMU