Estimation of Altitude and Vertical Velocity for ... - Semantic Scholar
Estimation of Altitude and Vertical Velocity for Multirotor Aerial Vehicle using Kalman Filter Przemyslaw G¸asior, Stanislaw Gardecki, Jaroslaw Go´sli´ nski and Wojciech Giernacki Poznan University of Technology, Institute of Control and Information Engineering, pl. M. Sklodowskiej-Curie 5 p.108, 60-965 Poznan, Poland [email protected], [email protected], [email protected] [email protected]
Abstract. Knowledge about precise robot localization is a key ingredient in controlling it, but the task is not trivial without any visual or GPS feedback. In this paper, authors concentrate on estimation of information about the robot’s altitude. One of the ways to acquire it, is a barometer. This type of sensor returns atmospheric pressure from which the height above the sea level can be computed. These readings have some disadvantages e.i.: vulnerability to pressure jumps and temperature drift as well as delay on the output. These problems can be solved by using Kalman filter algorithm for estimating altitude and vertical velocity, based not only on barometer readings, but also on accelerometer data. In the paper, derivation of the Kalman equations for the process to estimated are shown. Also improvements of the algorithm are described. The results of tests of this algorithm on real flying robot proved that estimates calculated with this method are precise and noise resistant. Keywords: Barometer, Accelerometer, IMU, Unmanned Aerial Vehicle UAV, Drone, Multirotor.
1
Introduction
In past few years there has been growing interest in the field of unmanned aerial vehicles. One of the problems is to get precise information about attitude and altitude. There are many ways to acquire attitude, using quaternions [1][2] or Kalman filter [3]. For attitude estimation, accelerometers, gyroscopes and magnetometers are used mostly, but for altitude estimation, only accelerometer and barometer readings are valid. Along with attitude and altitude data availability it is possible to create controllers stabilizing the drone. This part is crucial for correct stabilization, otherwise flying robots cannot be used in applications that they were designed for e.g.: commercial flaying cameras, reconnaissance units in rescue missions or even moving inside buildings with implemented SLAM (Simultaneous Localization and Mapping).
Calculating altitude only from the barometer measurements is not accurate enough, therefore authors proposed Kalman filter based on basic physics equations. This filter estimates altitude and vertical velocity, taking two inputs: relative altitude and acceleration in Z axis. Described algorithm will work only if robot is located parallel to the ground (Z axis is vertical to the ground). Therefore method for tilting of the robot’s frame compensation was proposed. Thanks to that, estimation of the altitude was correct in every orientation. At the end of paper an experiment is presented. Results prove correct implementation of the algorithm. Paper is divided into three main parts. In the first part, the research platform and on-board sensors are presented. Next one, contains description of all main steps of the algorithm with all needed equations. Finally, in the last part, the results of test during flights with the conclusions regarding the algorithm are shown.
2
Research platform
All measurements and experiments in this paper are done on the research platform built in the Institute of Control and Information Engineering at Poznan University of Technology. The platform is shown in Fig. 1. This is a high quality robot constructed mostly with carbon fiber. From perspective of this paper, the most important part is the AHRS (Attitude and Heading Reference System) module. It is a part of the on-board avionics, fully designed by researchers working on this construction. It consists of IMU sensor and microcontroller realizing computational algorithms and communicating with main controller board.
Fig. 1. Flaying robot Falcon
2.1
IMU
The main on-board sensor is IMU (Inertial Measurement Unit) from Analog Devices (type: ADIS16488). It is a high quality tactical grade sensor that integrates 3-axis accelerometer, gyroscope, magnetometer and barometer with filtering capabilities, Fig. 2. Based on its measurements, the attitude is computed and sent to main unit where stabilization of the robot is implemented.
Fig. 2. AHRS module with tactical grade IMU sensor - ADIS16488
3
Description of the algorithm
In the first part of this section, simple way of altitude computation is presented. This method is based only on barometer readings and returns height above the sea level. In the next part, problems with that type of measurement and a way to calculate the relative altitude from starting position of the robot are presented. In the third part, the simple introduction to the Kalman filter and measurement model is described. Finally, improvements to the algorithm and complete scheme of actions are shown. 3.1
Absolute height above the sea level
Taking [5] into consideration, there is possibility to compute atmospheric pressure at a selected height, Eqn. 1. p = p0 (1 −
L · h gM ) RL T0
(1)
From that equation the absolute height above the sea level can be derived (2). h=−
RL T p gM 0 −1 · p0 L
(2)
where the coefficients are as follows: p - atmospheric pressure at defined level, p0 - sea level standard atmospheric pressure, L - temperature lapse rate, T0 sea level standard temperature, g - Earth-surface gravitational acceleration, M - molar mass of dry air, R - universal gas constant. Thanks to that equation, the height of the robot can be computed, though the result in not precise enough and depends mostly on barometer resolution. What is more, the effect of changing of the surrounding temperature is also not taken into consideration. ADIS16488 has self-heating capability, which stabilizes temperature inside the case at constant level, independently of the outside conditions. Unfortunately, heating process takes very long time to end (approx. 1 hour) and has very significant influence on the barometer readings. Example data with temperature drift during constant altitude is shown in Fig. 3. The error grows up to 7 meters. Therefore authors decided to compensate this effect. Unfortunately
Fig. 3. Barometer temperature drift
height drift depends not only on temperature but also on the weather conditions. In this case compensation was correct but the aim was not achieved. Based on measurements when robot is still on the ground, the relative altitude during the flight phase can be calculated. This is done by taking one of the first height measurements and subtract it from next ones (3). hrel = habs − h0
(3)
where h0 is a starting height and habs is the result from the (2) 3.2
Kalman filter
Kalman filter is divided into two main parts: prediction and correction. In first part, the state vector values are computed based on the estimations from last
iteration. Also covariance matrix is updated. x ˆ(k|k − 1) = Aˆ x(k − 1|k − 1) + Bˆ u(k − 1)
(4)
P(k|k − 1) = AP(k − 1|k − 1)AT + Q
(5)
The next step is an update based on predictions and measurements. K(k) = P(k|k − 1)CT (CP(k|k − 1)CT + R)−1
(6)
x ˆ(k|k) = x ˆ(k|k − 1) + K(k) z(k) − Cˆ x(k|k − 1) P(k|k) = I − K(k|k)C P(k|k − 1)
(7) (8)
where: x ˆ - state vector estimate, z - measurement vector, A - process matrix, B - control matrix, C - input matrix, P - covariance matrix, K - Kalman gain matrix, Q - process noise covariance matrix, R - measurement noise covariance matrix [4]. 3.3
Measurement model
Robot equations in axis Z has been created using basic physics knowledge, based on available measurements - relative altitude and acceleration. vk = vk−1 + ak−1 T
(9)
ak−1 T 2 (10) 2 Where T is a sample period. Taking altitude, vertical acceleration and vertical velocity as state variables, equations 9 and 10 can be written in a matrix representation: xk = Axk−1 + Buk (11) hk = hk−1 + vk−1 T +
T2 hk hk−1 1 2 T ak = 0 1 0 ak−1 vk vk−1 0 T 1
(12)
y = Cxk−1 + Duk
(13)
" # hk−1 100 y= ak−1 010 vk−1
(14)
Since there is no control matrix, model is non-controllable and for T 6= 0 it is observable.
3.4
Robot tilt acceleration compensation
When robot is tilting in Roll or Pitch axis, vertical accelerometer readings are changing and Kalman filter will do not work properly. Errors will appear mostly in velocity estimation. Therefore authors developed compensation algorithm to reduce errors caused by tilt it both axes. The algorithm adds correction to accelerometer measurement in the Kalman filter, calculated in following equations: x = sin(α)
(15)
y = sin(β)
(16)
z=
p
acor =
x2 + y 2
(17)
p
(18)
1 − z2
Where α is the Roll angle and β i the Pitch angle. In Fig. 4 example data, gathered in special testbed, allowing only rotary movement is shown. As it can be seen, the compensation method works as required.
Fig. 4. Tilt compensation example. Angles α (gray) and β (black) in top graph. In bottom, accelerometer in z axis measurement (gray) and compensated accelerometer (black)
3.5
The estimation algorithm
The proposed algorithm consists of all above methods and it can be presented in points: 1. 2. 3. 4. 5. 6.
Calculation of the robot attitude Measurement of barometric pressure, vertical acceleration and temperature Temperature drift compensation of the barometer measurement Calculation of relative altitude Robot tilt compensation Estimation of the altitude and the vertical velocity with the Kalman filter
Fig. 5. Flowchart of the algorithm
Thanks to the algorithm, the altitude and the vertical velocity were estimated. Results during the test flight are presented in next the section.
4
Experiments
One experiment was performed in order to check accuracy and dynamics of estimation during the flight. At the beginning robot was on the ground with motors rotating. During the next phase the robot took off and had a few changes in altitude, finally robot landed. Data was sampled at frequency of approximately 200Hz. Results are presented in Fig. 6. As it can been seen, temperature drift has substantial influence on altitude estimation. Difference between start and landing altitude was approximately 1 meter. After compensation, results are very satisfactory and error is lower than 0.1 meters. It is important to mention that temperature drift of barometric readings has minimal influence on velocity estimation. Disturbances in barometer’s altitude during take off and landing were caused by air movement from propellers. Estimated altitude was very smooth and that is important while using it in altitude controller. Though, the accelerometer readings were very noisy 7, the algorithm gives desired results.
Fig. 6. Experiment results. Relative altitude (light gray), estimated altitude (black) and estimated altitude with temperature compensation (dark gray) in top graph. In bottom, vertical velocity from integrating accelerometer in Z axis (light gray), estimated vertical velocity (black) and estimated vertical velocity with temperature compensation (dark gray)
Fig. 7. Readings from accelerometer
5
Conclusions
As the experiment shown, presented algorithm estimates properly and can be reliable. Thanks to that, a regulator to hold desired altitude and the other one to hold velocity can be designed and implemented . These controllers will be capable to control robot more easily. Based on controllers for altitude and velocity, it will be possible to decrease or increase the vertical velocity with high precision.
The algorithm can be implemented in various machines (flaying, driving or stationary) equipped with IMU, containing barometer and accelerometer. To increase precision and reliability it is possible to fuse this algorithm with GPS receiver or visual system. Future works will be focused on controllers for the UAV, which has velocity and altitude estimators on the feedback signals.
References 1. Mahony, R., Hamel, T., Pflimlin, J-M.: Nonlinear Complementary Filters on the Special Orthogonal Group. IEEE Transactions on Automatic Control, Vol. 53, No. 5, June 2008 2. Madgwick, S.O.H., Harrison, A.J.L., Vaidyanathan, R.: Estimation of IMU and MARG orientation using a gradient descent algorithm. Rehabilitation Robotics (ICORR), 2011 3. Wang S., Yang Y.: Quadrotor aircraft attitude estimation and control based on Kalman filter. Control Conference (CCC), 1072 - 1077, 2010 4. Welch, G., Bishop, G.: An Introduction to the Kalman Filter, University of North Carolina at Chapel Hill, July 24, 2006 5. Portland State Aerospace Society, A Quick Derivation relating altitude to air pressure, 2004
Abstract. Knowledge about precise robot localization is a key ingredient in controlling it, but the task is not trivial without any visual or GPS feedback. In this paper, authors concentrate on estimation of information about the robot’s altitude. One of the ways to acquire it, is a barometer. This type of sensor returns atmospheric pressure from which the height above the sea level can be computed. These readings have some disadvantages e.i.: vulnerability to pressure jumps and temperature drift as well as delay on the output. These problems can be solved by using Kalman filter algorithm for estimating altitude and vertical velocity, based not only on barometer readings, but also on accelerometer data. In the paper, derivation of the Kalman equations for the process to estimated are shown. Also improvements of the algorithm are described. The results of tests of this algorithm on real flying robot proved that estimates calculated with this method are precise and noise resistant. Keywords: Barometer, Accelerometer, IMU, Unmanned Aerial Vehicle UAV, Drone, Multirotor.
1
Introduction
In past few years there has been growing interest in the field of unmanned aerial vehicles. One of the problems is to get precise information about attitude and altitude. There are many ways to acquire attitude, using quaternions [1][2] or Kalman filter [3]. For attitude estimation, accelerometers, gyroscopes and magnetometers are used mostly, but for altitude estimation, only accelerometer and barometer readings are valid. Along with attitude and altitude data availability it is possible to create controllers stabilizing the drone. This part is crucial for correct stabilization, otherwise flying robots cannot be used in applications that they were designed for e.g.: commercial flaying cameras, reconnaissance units in rescue missions or even moving inside buildings with implemented SLAM (Simultaneous Localization and Mapping).
Calculating altitude only from the barometer measurements is not accurate enough, therefore authors proposed Kalman filter based on basic physics equations. This filter estimates altitude and vertical velocity, taking two inputs: relative altitude and acceleration in Z axis. Described algorithm will work only if robot is located parallel to the ground (Z axis is vertical to the ground). Therefore method for tilting of the robot’s frame compensation was proposed. Thanks to that, estimation of the altitude was correct in every orientation. At the end of paper an experiment is presented. Results prove correct implementation of the algorithm. Paper is divided into three main parts. In the first part, the research platform and on-board sensors are presented. Next one, contains description of all main steps of the algorithm with all needed equations. Finally, in the last part, the results of test during flights with the conclusions regarding the algorithm are shown.
2
Research platform
All measurements and experiments in this paper are done on the research platform built in the Institute of Control and Information Engineering at Poznan University of Technology. The platform is shown in Fig. 1. This is a high quality robot constructed mostly with carbon fiber. From perspective of this paper, the most important part is the AHRS (Attitude and Heading Reference System) module. It is a part of the on-board avionics, fully designed by researchers working on this construction. It consists of IMU sensor and microcontroller realizing computational algorithms and communicating with main controller board.
Fig. 1. Flaying robot Falcon
2.1
IMU
The main on-board sensor is IMU (Inertial Measurement Unit) from Analog Devices (type: ADIS16488). It is a high quality tactical grade sensor that integrates 3-axis accelerometer, gyroscope, magnetometer and barometer with filtering capabilities, Fig. 2. Based on its measurements, the attitude is computed and sent to main unit where stabilization of the robot is implemented.
Fig. 2. AHRS module with tactical grade IMU sensor - ADIS16488
3
Description of the algorithm
In the first part of this section, simple way of altitude computation is presented. This method is based only on barometer readings and returns height above the sea level. In the next part, problems with that type of measurement and a way to calculate the relative altitude from starting position of the robot are presented. In the third part, the simple introduction to the Kalman filter and measurement model is described. Finally, improvements to the algorithm and complete scheme of actions are shown. 3.1
Absolute height above the sea level
Taking [5] into consideration, there is possibility to compute atmospheric pressure at a selected height, Eqn. 1. p = p0 (1 −
L · h gM ) RL T0
(1)
From that equation the absolute height above the sea level can be derived (2). h=−
RL T p gM 0 −1 · p0 L
(2)
where the coefficients are as follows: p - atmospheric pressure at defined level, p0 - sea level standard atmospheric pressure, L - temperature lapse rate, T0 sea level standard temperature, g - Earth-surface gravitational acceleration, M - molar mass of dry air, R - universal gas constant. Thanks to that equation, the height of the robot can be computed, though the result in not precise enough and depends mostly on barometer resolution. What is more, the effect of changing of the surrounding temperature is also not taken into consideration. ADIS16488 has self-heating capability, which stabilizes temperature inside the case at constant level, independently of the outside conditions. Unfortunately, heating process takes very long time to end (approx. 1 hour) and has very significant influence on the barometer readings. Example data with temperature drift during constant altitude is shown in Fig. 3. The error grows up to 7 meters. Therefore authors decided to compensate this effect. Unfortunately
Fig. 3. Barometer temperature drift
height drift depends not only on temperature but also on the weather conditions. In this case compensation was correct but the aim was not achieved. Based on measurements when robot is still on the ground, the relative altitude during the flight phase can be calculated. This is done by taking one of the first height measurements and subtract it from next ones (3). hrel = habs − h0
(3)
where h0 is a starting height and habs is the result from the (2) 3.2
Kalman filter
Kalman filter is divided into two main parts: prediction and correction. In first part, the state vector values are computed based on the estimations from last
iteration. Also covariance matrix is updated. x ˆ(k|k − 1) = Aˆ x(k − 1|k − 1) + Bˆ u(k − 1)
(4)
P(k|k − 1) = AP(k − 1|k − 1)AT + Q
(5)
The next step is an update based on predictions and measurements. K(k) = P(k|k − 1)CT (CP(k|k − 1)CT + R)−1
(6)
x ˆ(k|k) = x ˆ(k|k − 1) + K(k) z(k) − Cˆ x(k|k − 1) P(k|k) = I − K(k|k)C P(k|k − 1)
(7) (8)
where: x ˆ - state vector estimate, z - measurement vector, A - process matrix, B - control matrix, C - input matrix, P - covariance matrix, K - Kalman gain matrix, Q - process noise covariance matrix, R - measurement noise covariance matrix [4]. 3.3
Measurement model
Robot equations in axis Z has been created using basic physics knowledge, based on available measurements - relative altitude and acceleration. vk = vk−1 + ak−1 T
(9)
ak−1 T 2 (10) 2 Where T is a sample period. Taking altitude, vertical acceleration and vertical velocity as state variables, equations 9 and 10 can be written in a matrix representation: xk = Axk−1 + Buk (11) hk = hk−1 + vk−1 T +
T2 hk hk−1 1 2 T ak = 0 1 0 ak−1 vk vk−1 0 T 1
(12)
y = Cxk−1 + Duk
(13)
" # hk−1 100 y= ak−1 010 vk−1
(14)
Since there is no control matrix, model is non-controllable and for T 6= 0 it is observable.
3.4
Robot tilt acceleration compensation
When robot is tilting in Roll or Pitch axis, vertical accelerometer readings are changing and Kalman filter will do not work properly. Errors will appear mostly in velocity estimation. Therefore authors developed compensation algorithm to reduce errors caused by tilt it both axes. The algorithm adds correction to accelerometer measurement in the Kalman filter, calculated in following equations: x = sin(α)
(15)
y = sin(β)
(16)
z=
p
acor =
x2 + y 2
(17)
p
(18)
1 − z2
Where α is the Roll angle and β i the Pitch angle. In Fig. 4 example data, gathered in special testbed, allowing only rotary movement is shown. As it can be seen, the compensation method works as required.
Fig. 4. Tilt compensation example. Angles α (gray) and β (black) in top graph. In bottom, accelerometer in z axis measurement (gray) and compensated accelerometer (black)
3.5
The estimation algorithm
The proposed algorithm consists of all above methods and it can be presented in points: 1. 2. 3. 4. 5. 6.
Calculation of the robot attitude Measurement of barometric pressure, vertical acceleration and temperature Temperature drift compensation of the barometer measurement Calculation of relative altitude Robot tilt compensation Estimation of the altitude and the vertical velocity with the Kalman filter
Fig. 5. Flowchart of the algorithm
Thanks to the algorithm, the altitude and the vertical velocity were estimated. Results during the test flight are presented in next the section.
4
Experiments
One experiment was performed in order to check accuracy and dynamics of estimation during the flight. At the beginning robot was on the ground with motors rotating. During the next phase the robot took off and had a few changes in altitude, finally robot landed. Data was sampled at frequency of approximately 200Hz. Results are presented in Fig. 6. As it can been seen, temperature drift has substantial influence on altitude estimation. Difference between start and landing altitude was approximately 1 meter. After compensation, results are very satisfactory and error is lower than 0.1 meters. It is important to mention that temperature drift of barometric readings has minimal influence on velocity estimation. Disturbances in barometer’s altitude during take off and landing were caused by air movement from propellers. Estimated altitude was very smooth and that is important while using it in altitude controller. Though, the accelerometer readings were very noisy 7, the algorithm gives desired results.
Fig. 6. Experiment results. Relative altitude (light gray), estimated altitude (black) and estimated altitude with temperature compensation (dark gray) in top graph. In bottom, vertical velocity from integrating accelerometer in Z axis (light gray), estimated vertical velocity (black) and estimated vertical velocity with temperature compensation (dark gray)
Fig. 7. Readings from accelerometer
5
Conclusions
As the experiment shown, presented algorithm estimates properly and can be reliable. Thanks to that, a regulator to hold desired altitude and the other one to hold velocity can be designed and implemented . These controllers will be capable to control robot more easily. Based on controllers for altitude and velocity, it will be possible to decrease or increase the vertical velocity with high precision.
The algorithm can be implemented in various machines (flaying, driving or stationary) equipped with IMU, containing barometer and accelerometer. To increase precision and reliability it is possible to fuse this algorithm with GPS receiver or visual system. Future works will be focused on controllers for the UAV, which has velocity and altitude estimators on the feedback signals.
References 1. Mahony, R., Hamel, T., Pflimlin, J-M.: Nonlinear Complementary Filters on the Special Orthogonal Group. IEEE Transactions on Automatic Control, Vol. 53, No. 5, June 2008 2. Madgwick, S.O.H., Harrison, A.J.L., Vaidyanathan, R.: Estimation of IMU and MARG orientation using a gradient descent algorithm. Rehabilitation Robotics (ICORR), 2011 3. Wang S., Yang Y.: Quadrotor aircraft attitude estimation and control based on Kalman filter. Control Conference (CCC), 1072 - 1077, 2010 4. Welch, G., Bishop, G.: An Introduction to the Kalman Filter, University of North Carolina at Chapel Hill, July 24, 2006 5. Portland State Aerospace Society, A Quick Derivation relating altitude to air pressure, 2004
