CC BY
3
Improvement the Accuracy of Attitude Estimation of UAV using the Extended Kalman Filter based on
Particle Swarm Optimization
Ammar Assad and Sergey Serikov
Abstract—The issue of Unmanned Aerial Vehicle (UAV) attitude estimation has been extensively studied, yet researchers continue to seek improvements, recognizing that more precise attitude calculations and determination lead to enhance UAV control robustness and accuracy. This research develops and implements an innovative approach combining Particle Swarm Optimization (PSO) with the Extended Kalman Filter (EKF) to improve UAV attitude estimation. The methodology employs raw measurements from gyroscopes, accelerometers, and magnetometers. PSO is utilized to estimate the noise covariance matrix of these measurements, which is then integrated into the EKF process to achieve superior attitude estimation results. Given that PSO is a global optimization tool, its integration with EKF demonstrated superior performance compared to the standard EKF. The implementation was carried out in a Matlab environment, using quaternions to represent UAV attitude. The mean and standard deviation (STD) of estimation errors were calculated, revealing that the PSO-EKF approach significantly enhances estimation accuracy compared to using EKF alone. Comparison with state of art results using Root Mean Square Error of attitude angles, showed that the developed method outperformed existed researches in the field of attitude estimation.
Keywords— Attitude estimation, Extended Kalman Filter, Particle Swarm Optimization, UAV.
I. INTRODUCTION
Recently, there has been growing interest in autonomous navigation vehicles due to their potential for critical missions such as surveillance, monitoring, and inspection in diverse environments, including land, sea, and air. Effective control of these vehicles requires accurate knowledge of their attitude angles, which can be measured using various technologies such as Inertial Navigation Systems (INS). Typically, INS relies on Global Positioning System (GPS) signals, which are fundamental for autonomous navigation. However, GPS signals may experience outages in certain environments due to various factors. With the advancements in Micro-Electro-Mechanical Systems (MEMS) technology, lightweight and cost-effective solutions such as Inertial Measurement Units (IMUs) have emerged, particularly for lightweight Unmanned Aerial Vehicles (UAVs) [1].
MEMS-based systems are often integrated with filters to calculate attitude angles without relying on GPS. The most widely used filter is the Kalman filter, which comes in various forms, such as the extended and unscented Kalman filters. However, the Kalman filter's approach to attitude estimation has limitations due to unknown noise in sensor measurements. These inaccuracies require additional methods to address the resulting errors, which directly impact the calculated angles and, consequently, the control algorithms. One of the methods used to mitigate these issues is artificial intelligence (AI), particularly Machine Learning (ML) techniques. ML approaches have been widely applied in fields related to UAVs and autonomous navigation. Integrating ML with the Kalman filter is a critical area of research, as ML offers diverse approaches, including classification, forecasting, tuning, and optimization, each with different objectives. Since attitude estimation is a realtime problem, it requires ML techniques specifically designed for real-time applications [2-3]. Machine Learning (ML) involves using machines to perform calculations, process data, or make decisions, often replacing human input. One example of a computational ML method is Particle Swarm Optimization (PSO), which is inspired by the swarm behavior observed in nature, such as groups of fish or birds working together to find solutions. In this research, the PSO approach is applied to enhance the Extended Kalman Filter (EKF), as will be explained in later sections.
In attitude estimation, sensor data is integrated with the Kalman filter to estimate attitude angles. The primary challenge lies in the noise present in the measurements, which, when integrated, leads to a cumulative error in the calculated attitude. In the Kalman filter, sensor noise is represented in matrices, which express the covariance of measurement errors. Accurate values for these matrices can significantly improve the precision of attitude estimation. However, in real-world scenarios, noise is not constant and exhibits random characteristics, requiring real-time estimation. Typically, these covariance matrices are treated
as constant, which can limit the accuracy of the filter [4-5]. Estimating, tuning or correction these matrixes will increase the accuracy of attitude estimation. Many methods are used for this correction, PSO is adopted in this research.
II. RELATED WORKS
In [6], authors implemented non-linear Complementary Filter (CF) for attitude estimation in unmanned aerial vehicles. The gain parameters of the CF were determined using particle swarm optimization (PSO), eliminating the need for manual tuning of KP and KI parameters. The CF automatically adjusts these parameters when the error between the accelerometer-measured attitude and the CF exceeds a predefined threshold. The research utilized a measurement unit comprising low-cost tri-axial rate gyros, accelerometers, and magnetometers based on micro-electromechanical systems (MEMS), without relying on global positioning system (GPS) data. The efficiency of the CF was evaluated experimentally using reference attitude and raw sensor data from a commercial inertial measurement unit (IMU). Simulation results demonstrated that the proposed PSO-aided non-linear complementary filter (PNCF) effectively obtained the required gain parameters and exhibited promising performance in attitude estimation.
In [7], authors developed a novel fuzzy-adaptive extended Kalman filter (FAEKF) aimed at real-time attitude estimation for agile mobile platforms equipped with magnetic, angular rate, and gravity (MARG) sensor arrays. The filter architecture integrates a quaternion-based EKF with an adaptive extension, incorporating innovative measurement techniques to assess system vibrations, external accelerations, and magnetic distortions. These external disturbances are treated as inputs to a sophisticated fuzzy inference machine, which employs fuzzy IF-THEN rules-based adaption laws to dynamically adjust the noise covariance matrices of the filter, ensuring precise and resilient attitude estimates. To evaluate the filter's performance, a six-degrees of freedom (6 DOF) test bench was constructed, enabling the execution of diverse dynamic behaviors and the acquisition of ground truth attitude angles alongside raw MARG sensor data. Parameter tuning was conducted via numerical optimization using measurements obtained from the test environment. The comprehensive analysis demonstrates that the proposed adaptive strategy significantly enhances attitude estimation quality, while effectively mitigating the impacts of both slow and rapid external perturbations. The versatility of the FAEKF renders it applicable to any mobile system requiring accurate attitude estimation for localization, particularly in scenarios where external disturbances pose significant challenges to filter accuracy.
In [8], authors devised two innovative approaches for accurately estimating the attitudes of mobile robots by integrating low-cost accelerometers and gyroscopes. Firstly, a specialized test bench was utilized to simulate various dynamic behaviors of wheeled robots while concurrently measuring their actual attitude angles and raw sensor data. These measurements were leveraged for offline optimization of Kalman filter parameters within a simulation environment. Secondly, a novel adaptive Kalman filter structure was introduced, dynamically adjusting the noise covariance values based on the system's instantaneous dynamics. These dynamics were characterized by the magnitudes of both instantaneous vibration and external acceleration. The proposed adaptive solution employed fuzzy logic to adaptively modify filter parameters in real
time. Results demonstrated that the adaptive filter improved overall convergence by a notable 10.9% compared to using the optimized Kalman filter, affirming its efficacy as an accurate and robust attitude filter. Benchmarking against other common methods showcased the adaptability of the developed filter, which not only competed but also surpassed benchmark filters in performance.
In [6], authors applied developed method where the error between attitude calculated using accelerometers measurement and attitude calculated by CF is exceeds predefined threshold but none of them is considered as
referenced orientation. In [7], authors used accelerometers, magnetometers and gyroscopes data as input for correction covariance matrix of measurements errors, the used vibrations from gyroscopes as input of the developed method. In [8] as in [7], they only used accelerometers and gyroscopes. There are many researches in this field to enhance the accuracy of orientation estimation of UAV without using GPS data for autonomous navigation. Even if, existing approaches do not fully meet the requirements. This research enriches this field by suggesting using of PSO to minimize input to be minimum as possible as it can be, in optimal cases, this term should be zero. In this paper, developed method is applied when there is an error between referenced measurements of accelerometers and magnetometer and real measured data without any threshold.
Previous studies [9], [10], [11] and [12] explored a mathematical model to match the error covariance matrix of measurements by utilizing accurate statistical characterizations of system noise. This approach relies on innovation and residual sequences to estimate the covariance matrices of both process and measurement noises. The key difference between these works and the developed approach in this research lies in the use of PSO. In our method, PSO is employed to minimize a specific term related to innovation and residual sequences. As will be explained in the following sections, minimizing this term proves to be more robust than relying solely on mathematical models to estimate the error covariance matrix, as seen in prior research.
III. PROPOSED METHOD
A. Extended Kalman Filter
Used sensors: A: Accelerometers, M: Magnetometers, G: Gyroscopes. The measurement vector is the values measured by these devices. The state vector is of EKF x, it contains attitude representation as Quaternion (4 components) and Gyroscopes drifts (3 drift). Kalman Filter for attitude estimation has two phases (in discrete time) [13]:
• Propagation or prediction phase: In this phase, the state vector is being predicted using the gyroscopes measurements using the state process f (f is nonlinear function)
*- = /(*+-!. Wk) (1)
Where Wk~ N(0, Qk) is state noise that is assumed to be white noise with zero mean and process noise with unknown diagonal covariance matrix Qk. The sign '+' refers to state vector that is outputted from second phase, while sign ('-') refers to state vector that is outputted from Propagation phase and k is the iteration.
• Update of correction phase: when new measurements (A and M) are arrived:
Z~ = h(xI, A, M) + Vk (2)
Where Vk is measurement noise, that is assumed to be white noise with zero mean and process noise with unknown diagonal covariance matrix Rk. h is nonlinear function.
Kalman filter parameters are: Kk: Kalman gain, Pk: State Covariance and Hk\ Measurement Linearization matrix. The matrix R is used directly in the calculations of Kalman parameters and in real conditions and experiments, it is unknown, so wrong value of it will reflects directly in the calculations and make the EKF wrongly estimate UAV
attitude. More details about the model of EKF is in [14].
B. PSO with EKF
Particle Swarm Optimization (PSO) is an ML technique used to find approximate solutions to complex or numerically challenging maximization and minimization problems [15]. As previously described, it involves a set of particles (or "neurons") that search for solutions within a network. These particles are connected through internal relations, and the best solution corresponds to the best positions in this network. PSO has several key parameters [16]:
1. Number of particles: This variable determines the number of particles in the swarm. They represent potential solutions to predict the matrix R. When number of particles is increased, this can lead to more thorough searching and exploration of search space but also will increase cost of computations.
2. Max iterations: Maximum number of iterations that the PSO algorithm will run PSO iteratively updates the positions and velocities of particles in the search space until this maximum number of iterations is reached.
3. Inertia: This parameter controls how particle's current velocity impact its movement. It's applied to previous velocity as a weighting factor.
4. Cognitive parameter: It determines the particle's tendency to move towards better position. It has impact on the exploration of space of searching by emphasizing the particle's own best-known solution.
5. Social parameter: It controls the particle's tendency to move towards the global best position found by any particle in the swarm. It facilitates exploitation of the search space by incorporating information from other particles' experiences.
In PSO, particles are spread randomly in a multidimensional space, each position representing a potential solution. As they move, their positions and velocities are adjusted based on individual experiences and interactions with neighboring particles. The particles evaluate each position based on objective criteria, such as minimizing a term called the Degree of Matching (DoM).
Adjustments are guided by each particle's personal best position and the best position found by the entire swarm. Some particles explore new positions, while others refine previously discovered ones. As iterations progress, the swarm continuously exchanges information, allowing the PSO algorithm to converge on optimal solutions in the most promising areas of the search space [17].
The main idea is to use PSO to search for best value of correction of matrix R to minimize DoM. At each iteration of developed model of EKF, attitude is calculated from EKF and DoM is computed from new measurements vector and EKF calculations. Define Innovation Sequences Innk (Or Innovation Adaptive Estimation) as below:
lnnk = Z~k - h(xk) (3)
It is the difference between the measurement vector and its estimate according to the estimation of state vector, Innk represents an additional useful information that is available to the filter. Occurrence of bad data first shows up in Innk. EKF in optimal cases could predict the real measurements. In this way and in real cases Innk will show when there is a difference between predicted and actual measurement. In extended Kalman filter where Innk can be linearized as
Innk = h(x-) + Vk- h(x+) = h(SXk) + Vk (4)
¡nnk = HkSXk + Vk
SXk is error of state vector. Calculate Sk, the covariance of Innk which is the sum of two independent stochastic processes
Sk = cov(HkSXk) + cov(yk)
Sk = (Hk) cov(SXk) (Hfc)T + Rk (5)
Sfc = (Hk)Pk (Hk)T + Rk
Sk can be calculated statistically in real time processing using next equation which will be denoted by
CInn = (Zk - h(X+)) (Zkk - h(x+))T (6)
The main usage of arithmetic covariance is to detect rapid changes in variance (case of maneuvering). Arithmetic covariance is calculated at every step or within a window. Define the term:
DoM = CInnk - Sk (7)
The term DOM represents Degree of Matching between two quantities (Ctnnk,Sk), and should be zero nominally, so the optimization process by PSO is used to keep this term as close to zero as possible by changing some parameters, Figure 1 below shows the general diagram of optimization process.
Figure 1: General optimization process EKF
The output of PSO is vector of six elements, they are represented the main diagonal of the matrix AR (three for accelerometers noise and three for magnetometers noise). PSO was used to adapt the matrix R as next
At iteration k:
• Start of EKF
• Propagation phase
• Correction phase
• PSO optimization
• Calculate DoM
• Search for matrix AR that minimize DoM
• Return AR
• R = R + AR
End of iterations
The full process is shown in Figure 2
UAV Attitudbey modelling the developed algorithm in script file, the sampling time was 16 milli-seconds, gyroscopes measurements are in radian per seconds, accelerometers measurements are in (m. s-2) and magnetometers measurements are in Micro Tesla. the calculations were represented by 15 digits after the decimal point. Typical extended Kalman filter and PSO-EKF were calculated in the same simulation with different values. The execution time with PSO was recorded each 1000 samples, the averaged execution time was 10.6 seconds for processing 1000 samples (1000 samples equals 16 seconds in real time). To be considered real-time, the processing time must be shorter than or equal to the time it takes to acquire the data. In this case:
• The data acquisition time is 16 seconds.
• The processing time is 10.6 seconds.
Since processing time 10.6 seconds is less than 16 seconds (data acquisition), the algorithm can be applied in real-time. This means that MATLAB can finish processing the data before new data is available for the next batch of 1000 samples.
The settings for PSO are: number of particles: 50, iterations: 50; inertia: 0.05; cognitive parameter: 1.5; social parameter: 5. After setting up all needed parameters, the results show that when using PSO, attitude estimation is improved and accuracy increased. During the first experiment and in accordance with the random setting of the PSO. At the beginning of the estimation, the errors are too large. It takes some time for them to stabilize, as shown in Figure 3 and Figure 4
Corrected UAV Attitude
Figure 2: EKF optimization process with PSO
40 _
30 _
20 _
10 _
0 _
-10 _
-20 _
-
600 800 Time, Sec
-PSO-EKF: Mean=0.66 , ff=5.06
30 _ 25 _
Figure 3: Pitch estimation (Experiment 1)
Figure 4: Roll estimation (Experiment 1)
At the start of algorithm, it needs time to be stabilized, in move. Figure 5, Figure 6 and Figure 7 show results after real application and in this period of time, UAV should not stabilization of PSO.
200
400
1000
1200
1400
Pitch error
0 _
200
400
50C
800
1000
1200
1400
Sec
Roll error
Pitch
Pitch error
Figure 5: Pitch estimation (Experiment 1, after 400 seconds)
Roll
Roll error
Figure 6: Roll estimation (Experiment 1, after 400 seconds)
Figure 7: Yaw estimation
Table 1 shows results of EKF and PSO-EKF and comparisons between them.
Table 1: Results comparisons
Comparing results with state of art results in Table 2, results of [16] and [17] are mentioned in [6].
Table 2: Comparison with state of art
STD (degree) Mean (degree)
Roll Pitch Yaw Roll Pitch Yaw
EKF 3.45 2.21 7.30 1.28 0.74 2.76
PSO-EKF 1.65 1.81 6.37 0.46 0.73 1.09
Improvement 52.17 % 18.09 % 12.73 % 64.06 % 1.35 % 60.05 %
In [4] 50.24 % 54.58 %
Roll Pitch Yaw Average
PSO-EKF 0.0340 0.0298 0.1128 0.0588
[6] dataset 1 0.0595 0.0526 0.6929 0.2683
[6] dataset 2 0.0799 0.0654 0.0804 0.0752
[6] dataset 3 0.1805 0.1631 1.0025 0.4487
[6] dataset 4 0.0254 0.0174 0.0827 0.0418
[8] best results (average)1.4310
[18] 0.0638 0.0460 0.1030 0.0710
[19] 0.1989 0.1611 0.8088 0.3896
RMSE (rad)
The most important thing is to decrease STD, since if there as an error with constant mean, this can be reduced with by compensation method, but STD refers how error changes around the mean. Results of experiment 2 are shown in Figure 8 and Figure 9. In the second experiment, PSO parameters were edited manually to obtain fast convergence of the algorithm. Fast convergence of the algorithm might result in high error but the whole performance remains better that typical extended Kalman filter.
eKF: Mean=2.76 , PSO-EKF: Mean=1.0S
600 800 Time, Sec
Figure 8: Pitch estimation (Experiment 2)
Figure 9: Roll estimation (Experiment 2)
This means that the attitude estimation accuracy has been improved compared to the typical EKF. The simulation showed that the accuracy is increased, the data contains many different phases of motions, this includes static, dynamic and maneuvers. In static as shown in first experiment, the algorithm fails because it tries to make the term DoM zero even if there a noise but less that than the level that make the filter diverges. but after a tuning of the parameters, there was no divergence. By Zooming in the results (Figure 10, Figure 11 and Figure 12), the area of zoom contains at first non-static mode with maneuvers then there is a static flight (from 1100 to 1175 seconds), in both cases the developed algorithm tracks referenced values and better that EKF
Pitch
6C
lime, sec
Pitch error
Roll
20C
40C
50C
30C
I00C
120C
I40C
Sec
Roll error
Figure 10: Zooming in Roll angle
Figure 12: Zooming in Yaw angle
In addition, matrix R is updated each sample for Accelerometers and Magnetometers, it can be said that features of measurement noise are estimated also as shown in Figure 13.
Figure 11: Zooming in Pitch angle
Evaluation of matrix R for ACC
900 e, Sec
Evaluation of matrix R for MAG
900 me, Sec
Figure 13: Features of measurement noise
s 2
S 2
500
500
700
800
1000
1300
1400
From the figure above, where matrix R is clearly has high values, these are considered as features to estimate if there is a maneuver of not. According to that, the data of UAV tells that UAV made more than 7 maneuvers. And where there a maneuver the PSO edited the value of R matrix.
V. Conclusion
In this paper, a novel approach of using PSO to aid EKF for attitude estimation of UAV is implemented, the main findings are that the accuracy when using PSO adapting EKF is higher than the EKF alone, besides that, the PSO parameters can be edited for fast convergence. The features of noise also can be predicted since PSO is working to tune matrix of measurement noise. In future, more tests can be achieved in this field such as correction of matrix Q, tuning it or using only accelerometers and gyroscopes.
Declaration
• Availability of data and materials: Not applicable.
• Acknowledgements: I would like to express my sincere gratitude to my supervisor, Serikov Sergey Anatolievich, for his invaluable guidance and unwavering support throughout the research process.
REFERENCES
1 Bassolillo, S. R., D'Amato, E., Notaro, I., Ariante, G., Del Core, G., & Mattei, M. (2022). Enhanced attitude and altitude estimation for indoor autonomous uavs. Drones, 6(1), 18.
2 Liu, X., Li, X., Shi, Q., Xu, C., & Tang, Y. (2021). UAV attitude estimation based on MARG and optical flow sensors using gated recurrent unit. International Journal of Distributed Sensor Networks, 17(4), 15501477211009814.
3 Xiaoqian, T., Feicheng, Z., Zhengbing, T., & Hongying, W. (2022). Nonlinear extended Kalman filter for attitude estimation of the fixed-wing UAV. International Journal of Optics, 2022.
4 Candan, B., & Soken, H. E. (2021). Robust attitude estimation using IMU-only measurements. IEEE Transactions on Instrumentation and Measurement, 70, 1-9.
5 Ding, W., & Gao, Y. (2021). Attitude estimation using low-cost MARG sensors with disturbances reduction. IEEE Transactions on Instrumentation and Measurement, 70, 1-11.
6 Poddar, S., Narkhede, P., Kumar, V., & Kumar, A. (2017). PSO aided adaptive complementary filter for attitude
estimation. Journal of Intelligent & Robotic Systems, 87, 531543.
7 Odry, A., Kecskes, I., Sarcevic, P., Vizvari, Z., Toth, A., & Odry, P. (2020). A novel fuzzy-adaptive extended kalman filter for real-time attitude estimation of mobile robots. Sensors, 20(3), 803.
8 Odry, A., Fuller, R., Rudas, I. J., & Odry, P. (2018). Kalman filter for mobile-robot attitude estimation: Novel optimized and adaptive solutions. Mechanical systems and signal processing, 110, 569-589.
9 Sage, A. P., & Husa, G. W. (1969, November). Algorithms for sequential adaptive estimation of prior statistics. In 1969 IEEE Symposium on Adaptive Processes (8th) Decision and Control (pp. 61-61). IEEE.
10 Narasimhappa, M., Mahindrakar, A. D., Guizilini, V. C., Terra, M. H., & Sabat, S. L. (2019). MEMS-based IMU drift minimization: Sage Husa adaptive robust Kalman filtering. IEEE Sensors Journal, 20(1), 250-260.
11 Meng, Y., Gao, S., Zhong, Y., Hu, G., & Subic, A. (2016). Covariance matching based adaptive unscented Kalman filter for direct filtering in INS/GNSS integration. Acta Astronautica, 120, 171-181.
12 Chen, Y. W., & Tu, K. M. (2022). Robust self-adaptive Kalman filter with application in target tracking. Measurement and Control, 55(9-10), 935-944.
13 Filipe, N., Kontitsis, M., & Tsiotras, P. (2015). Extended Kalman filter for spacecraft pose estimation using dual quaternions. Journal of Guidance, Control, and Dynamics, 38(9), 1625-1641.
14 Assad, A., Khalaf, W., & Chouaib, I. (2019). Novel adaptive fuzzy extended Kalman filter for attitude estimation in GPS-denied environment. Gyroscopy and Navigation, 10, 131-146.
15 Wang, D., Tan, D., & Liu, L. (2018). Particle swarm optimization algorithm: an overview. Soft computing, 22, 387408.
16 Kassoul, K., Belhaouari, S. B., & Cheikhrouhou, N. (2021, February). Dynamic cognitive-social particle swarm optimization. In 2021 7th International Conference on Automation, Robotics and Applications (ICARA) (pp. 200205). IEEE.
17 Couceiro, M., Ghamisi, P., Couceiro, M., & Ghamisi, P. (2016). Particle swarm optimization (pp. 1-10). Springer International Publishing.
18 Mahony, R., Hamel, T., Pflimlin, J.-M.: Complementary filter design on the special orthogonal group SO (2). 44th IEEE Conference on Decision and Control, 2005 and 2005 European Control Conference. CDC-ECC'05, pp. 1477- 1484 (2005)
19 Madgwick, S.O., Harrison, A.J., Vaidyanathan, R.: Estimation of IMU and MARG orientation using a gradient descent algorithm. 2011 IEEE International Conference on Rehabilitation Robotics (ICORR), pp. 1-7 (2011)
