A Fast Weakly-Coupled Double-Layer ESKF Attitude Estimation Algorithm and Application
Abstract
:1. Introduction
- Considering the small UAV short-range autonomous navigation flight environment, we designed a low-cost attitude IMU error model, which reasonably simplified the impact of the cone error and earth radius on the attitude solution accuracy.
- For the data coupling phenomenon caused by the inconsistency of measurement sensor update frequency during the attitude filtering, a weakly-coupled double-layer ESKF attitude filtering algorithm was presented to improve the attitude solution accuracy and robustness.
- To verify the effectiveness of the proposed algorithm, we analyzed the simulation and flight test compared with other attitude algorithms. It was more comprehensive to study the performance of the proposed algorithm through the mathematical statistics methods [25] and the running time of the algorithm.
2. Attitude Mathematical Model
2.1. Quaternion Attitude Determination Model
2.2. Attitude Sensors Model
3. The Weakly-Coupled Double-Layer ESKF Attitude Estiamtion
3.1. Attitude Filtering System Model
3.2. Attitude Error Dynamic Equation
3.3. Attitude Error Measurement Equation
3.4. Attitude Filter Algorithm
- (1)
- attitude filtering time update and to the attitude error dynamic equation:
- (2)
- The the first-layer attitude filtering measurement update and to the attitude error measurement Equation (22):
- (3)
- The second-layer attitude filtering measurement update and to the attitude error measurement Equation (23):
Algorithm 1 Proposed weakly-coupled double-layer ESKF. |
|
4. The Experimental Simulation and Flight Test
4.1. Simulation and Flight Verification Platform
4.2. Simulation and Analysis
4.2.1. Static Conditions
4.2.2. Dynamic Condition
4.3. Flight Real-Time Test
4.4. Dynamic Test
4.5. Autonomous Flight Test
5. Conclusions and Future Work
Author Contributions
Funding
Conflicts of Interest
Abbreviations
UAV | unmanned aerial vehicle |
DL-ESKF | double-layer error-state Kalman filter |
IMU | inertial measurement unit |
MEMS | microelectromechanical systems |
DCM | direction cosine matrix |
DOF | degrees of freedom |
MCU | microcontroller unit |
Mag | magnetometer |
DSKF | direct state Kalman filter |
ESKF | error state Kalman filter |
GPS | global position system |
kg | kilogram |
EKF | Extended Kalman filter |
RMSE | root mean square errors |
FCS | flight control system |
PWM | pulse width modulation |
References
- Wang, C.; Wang, J.; Shen, Y.; Zhang, X. Autonomous navigation of UAVs in large-scale complex environments: A deep reinforcement learning approach. IEEE Trans. Veh. Technol. 2019, 68, 2124–2136. [Google Scholar] [CrossRef]
- Xiong, L.; Xia, X.; Lu, Y.; Liu, W.; Gao, L.; Song, S.; Han, Y.; Yu, Z. IMU-based automated vehicle slip angle and attitude estimation aided by vehicle dynamics. Sensors 2019, 19, 1930. [Google Scholar] [CrossRef] [Green Version]
- Koksal, N.; Jalalmaab, M.; Fidan, B. Adaptive linear quadratic attitude tracking control of a quadrotor UAV based on IMU sensor data fusion. Sensors 2019, 19, 46. [Google Scholar] [CrossRef] [PubMed] [Green Version]
- Kamata, H.; Kimishima, M.; Sawada, T.; Suga, Y.; Takeda, H.; Yamashita, K.; Mitani, S. MEMS Gyro Array Employing Array Signal Processing for Interference and Outlier Suppression. In Proceedings of the 2020 IEEE International Symposium on Inertial Sensors and Systems (INERTIAL), Hiroshima, Japan, 23–26 March 2020; pp. 1–4. [Google Scholar]
- Zotov, S.; Srivastava, A.; Kwon, K.; Frank, J.; Parco, E.; Williams, M.; Shtigluz, S.; Lyons, K.; Frazee, M.; Hoyh, D.; et al. In-Run Navigation Grade Quartz MEMS-Based IMU. In Proceedings of the 2020 IEEE International Symposium on Inertial Sensors and Systems (INERTIAL), Hiroshima, Japan, 23–26 March 2020; pp. 1–4. [Google Scholar]
- Wu, J.; Zhou, Z.; Gao, B.; Li, R.; Cheng, Y.; Fourati, H. Fast linear quaternion attitude estimator using vector observations. IEEE Trans. Autom. Sci. Eng. 2017, 15, 307–319. [Google Scholar] [CrossRef] [Green Version]
- Javed, M.A.; Tahir, M.; Ali, K. Cascaded Kalman Filtering-Based Attitude and Gyro Bias Estimation With Efficient Compensation of External Accelerations. IEEE Access 2020, 8, 50022–50035. [Google Scholar] [CrossRef]
- Lee, B.; Lee, Y.J.; Sung, S. Attitude Determination Algorithm based on Relative Quaternion Geometry of Velocity Incremental Vectors for Cost Efficient AHRS Design. IEEE Aerosp. Electron. Syst. Mag. 2018, 19, 459–469. [Google Scholar] [CrossRef]
- Islam, M.S.; Shajid-Ul-Mahmud, M.; Islam, T.; Amin, M.S.; Hossam-E-Haider, M. A low cost MEMS and complementary filter based attitude heading reference system (AHRS) for low speed aircraft. In Proceedings of the 2016 3rd International Conference on Electrical Engineering and Information Communication Technology (ICEEICT), Dhaka, Bangladesh, 22–24 September 2016; pp. 1–5. [Google Scholar]
- Mahony, R.; Euston, M.; Kim, J.; Coote, P.; Hamel, T. A non-linear observer for attitude estimation of a fixed-wing unmanned aerial vehicle without GPS measurements. Trans. Inst. Meas. Control 2011, 33, 699–717. [Google Scholar] [CrossRef]
- Lai, Y.C.; Jan, S.S. Attitude estimation based on fusion of gyroscopes and single antenna GPS for small UAVs under the influence of vibration. GPS Solut. 2011, 15, 67–77. [Google Scholar] [CrossRef]
- Marantos, P.; Koveos, Y.; Kyriakopoulos, K.J. UAV state estimation using adaptive complementary filters. IEEE Trans. Control Syst. Technol. 2015, 24, 1214–1226. [Google Scholar] [CrossRef]
- Euston, M.; Coote, P.; Mahony, R.; Kim, J.; Hamel, T. A complementary filter for attitude estimation of a fixed-wing UAV. In Proceedings of the IEEE International Conference on Intelligent Robots and Systems, Nice, France, 22–26 September 2008; pp. 340–345. [Google Scholar]
- Rogne, R.H.; Bryne, T.H.; Fossen, T.I.; Johansen, T.A. On the Usage of Low-Cost MEMS Sensors, Strapdown Inertial Navigation, and Nonlinear Estimation Techniques in Dynamic Positioning. IEEE J. Oceanic Eng. 2020, 1–16. [Google Scholar] [CrossRef]
- Coviello, G.; Avitabile, G.; Florio, A. A Synchronized Multi-Unit Wireless Platform for Long-Term Activity Monitoring. Electronics 2020, 9, 1118. [Google Scholar] [CrossRef]
- Wu, J.; Zhou, Z.; Chen, J.; Fourati, H.; Li, R. Fast complementary filter for attitude estimation using low-cost MARG sensors. IEEE Sensors J. 2016, 16, 6997–7007. [Google Scholar] [CrossRef]
- Welch, G.; Bishop, G. An Introduction to the Kalman Filter. 1995. Available online: http://e.guigon.free.fr/rsc/techrep/WelchBishop95.pdf (accessed on 3 September 2020).
- Meinhold, R.J.; Singpurwalla, N.D. Understanding the Kalman filter. Am. Stat. 1983, 37, 123–127. [Google Scholar]
- Choukroun, D.; Bar-Itzhack, I.Y.; Oshman, Y. Novel quaternion Kalman filter. IEEE Trans. Aerosp. Electron. Syst. 2006, 42, 174–190. [Google Scholar] [CrossRef] [Green Version]
- Kingston, D.; Beard, R. Real-time attitude and position estimation for small UAVs using low-cost sensors. In Proceedings of the AIAA 3rd Unmanned Unlimited Technical Conference, Workshop and Exhibit, Chicago, IL, USA, 20–23 September 2004; p. 6488. [Google Scholar]
- Song, Y.; Weng, X.; Guo, X. Small UAV Attitude Estimation Based on the Algorithm of Quaternion Extended Kalman Filter. J. Jilin Univ. (Sci. Ed.) 2015, 53, 511–518. [Google Scholar]
- Liu, H.; Li, Q.; Li, C.; Zhao, H. Application Research of an Array Distributed IMU Optimization Processing Method in Personal Positioning in Large Span Blind Environment. IEEE Access 2020, 8, 48163–48176. [Google Scholar] [CrossRef]
- Coviello, G.; Avitabile, G. Multiple Synchronized Inertial Measurement Unit Sensor Boards Platform for Activity Monitoring. IEEE Sens. J. 2020, 20, 8771–8777. [Google Scholar] [CrossRef]
- Sabatelli, S.; Galgani, M.; Fanucci, L.; Rocchi, A. A double-stage Kalman filter for orientation tracking with an integrated processor in 9-D IMU. IEEE Trans. Instrum. Meas. 2012, 62, 590–598. [Google Scholar] [CrossRef]
- Yadav, N.; Bleakley, C. Accurate orientation estimation using AHRS under conditions of magnetic distortion. Sensors 2014, 14, 20008–20024. [Google Scholar] [CrossRef] [PubMed] [Green Version]
- Youn, W.; Rhudy, M.B.; Cho, A.; Myung, H. Fuzzy adaptive attitude estimation for a fixed-wing UAV with a virtual SSA sensor during a GPS outage. IEEE Sens. J. 2019, 30, 1456–1472. [Google Scholar] [CrossRef]
- Sabatini, A.M. Kalman-filter-based orientation determination using inertial/magnetic sensors: Observability analysis and performance evaluation. Sensors 2011, 11, 9182–9206. [Google Scholar] [CrossRef] [PubMed]
- Madyastha, V.; Ravindra, V.; Mallikarjunan, S.; Goyal, A. Extended Kalman filter vs. error state Kalman filter for aircraft attitude estimation. In Proceedings of the AIAA Guidance, Navigation, and Control Conference, Portland, OR, USA, 8–11 August 2011; p. 6615. [Google Scholar]
- Markley, F.L. Attitude error representations for Kalman filtering. J. Guid. Control Dyn. 2003, 26, 311–317. [Google Scholar] [CrossRef]
- Yang, Y.; Liu, X.; Zhang, W.; Liu, X.; Guo, Y. A Nonlinear Double Model for Multisensor-Integrated Navigation Using the Federated EKF Algorithm for Small UAVs. Sensors 2020, 20, 2974. [Google Scholar] [CrossRef] [PubMed]
- Wu, Z.; Yao, M.; Ma, H.; Jia, W.; Tian, F. Low-cost antenna attitude estimation by fusing inertial sensing and two-antenna GPS for vehicle-mounted satcom-on-the-move. IEEE Trans. Veh. Technol. 2012, 62, 1084–1096. [Google Scholar] [CrossRef]
- Meier, L.; Honegger, D.; Pollefeys, M. PX4: A node-based multithreaded open source robotics framework for deeply embedded platforms. In Proceedings of the 2015 IEEE International Conference on Robotics and Automation, Seattle, WA, USA, 26–30 May 2015; pp. 6235–6240. [Google Scholar]
UAV | Take-off Weight | Max Playload | Wingspan | Flight Time |
---|---|---|---|---|
Specification | 1 kg | 0.5 kg | 450 mm | 30 min |
Size | Weight 38 g, width 50 mm, height 15.5 mm, and length 81.5 mm |
CPU | 32-bit STM32F427 and STM32F103 |
Sensor | Invensense MPU6000 six-axis accelerometer/gyro, ST Micro L3GD20 16-bit gyroscope, |
ST Micro LSM303D 14-bit accelerometer/magnetometer, MS5611 MEAS barometer, | |
GPS module | |
Interface | UART,I2C, SPI, 2 CAN, USB, 3.3 V, and 6.6 V ADC input |
Sample frequency | IMU (250 Hz), magnetometer (100 Hz), barometer (100 Hz), GPS module (10 Hz) |
Filter output frequency | attitude update output (500 Hz), navigation update output (100 Hz) |
State estimation | attitude, velocity, position, angular rate |
Size | Weight 15 g, width 3 mm, height 4 mm |
CPU | 32-bit STM32F407VGT6 |
Sensor | Invensense MPU6500 six-axis accelerometer/gyro, three-axis HMC5883 magnetometer, |
MS5611 barometer, and NEO-M8N GPS module | |
Interface | UART,I2C, SPI, USB |
Sample frequency | IMU (500 Hz), magnetometer (100 Hz), barometer (100 Hz), GPS module (10 Hz) |
Filter output frequency | attitude update output (500 Hz), navigation update output (100 Hz) |
State estimation | attitude, velocity, position, angular rate |
Flying mode | stability, loiter, waypoint |
Angular Rate | X | Y | Z |
---|---|---|---|
bias instability (rad/s) | |||
angular rate random noise (rad/s/) |
Acceleration | X | Y | Z |
---|---|---|---|
bias instability (m/s) | |||
acceleration random noise (m/s) | 0.026 | 0.026 | 0.029 |
Algorithms | Parameter Settings |
---|---|
Mahony-CF | , , , |
Song-KF | |
, | |
DL-ESKF | |
Algorithms | Mahony-CF | Song-KF | DL-ESKF |
---|---|---|---|
roll angle | 0.9364 | 0.9740 | 0.8248 |
pitch angle | 0.6828 | 0.2869 | 0.1291 |
yaw angle | 4.1121 | 3.2202 | 2.8287 |
Algorithms | Mahony-CF | Song-KF | DL-ESKF |
---|---|---|---|
running time | 0.785 | 3.843 | 0.909 |
Algorithms | Mahony-CF | Song-KF | DL-ESKF |
---|---|---|---|
roll angle | 5.3933 | 4.6066 | 1.5075 |
pitch angle | 4.1439 | 4.9547 | 1.6082 |
yaw angle | 7.6442 | 5.2164 | 2.5671 |
Algorithms | Mahony-CF | Song-KF | DL-ESKF |
---|---|---|---|
running time | 0.648 | 1.984 | 0.839 |
© 2020 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).
Share and Cite
Yang, Y.; Liu, X.; Zhang, W.; Liu, X.; Guo, Y. A Fast Weakly-Coupled Double-Layer ESKF Attitude Estimation Algorithm and Application. Electronics 2020, 9, 1465. https://doi.org/10.3390/electronics9091465
Yang Y, Liu X, Zhang W, Liu X, Guo Y. A Fast Weakly-Coupled Double-Layer ESKF Attitude Estimation Algorithm and Application. Electronics. 2020; 9(9):1465. https://doi.org/10.3390/electronics9091465
Chicago/Turabian StyleYang, Yue, Xiaoxiong Liu, Weiguo Zhang, Xuhang Liu, and Yicong Guo. 2020. "A Fast Weakly-Coupled Double-Layer ESKF Attitude Estimation Algorithm and Application" Electronics 9, no. 9: 1465. https://doi.org/10.3390/electronics9091465