Distributed Error Correction of EKF Algorithm in Multi-Sensor Fusion Localization Model

,


I. INTRODUCTION
Location Based Services (LBS) is a basic service that obtains the current location and provides information resources through various mobile location technologies [1]- [3]. At present, the most basic localization technology generally uses GPS sensors for real-time localization, but GPS signals are easily blocked, or interfered, thus high-precision localization cannot be realized [4]- [6]. Therefore, based on a single data source, many research institutions, universities, etc. use multi-source sensor data fusion to complement each other's advantages and realize accurate localization [7]- [9].
The common multi-sensor fusion localization method is to collect real-time data of gyroscopes, acceleration The associate editor coordinating the review of this manuscript and approving it for publication was Rongbo Zhu . sensors, magnetic sensors, mileage sensors, inertial measurement units, vision sensors and other sensors, and to use data fusion for high-precision localization. Ghosh et al. [10] used wheel tester, inertial measurement unit and rotating 2D laser scanner to locate and correct the mobile robot in real time. Nada et al. [11] took odometer, magnetic compass and acceleration sensor data as inputs of Unscented Kalman Filter (UKF) to realize data fusion and real-time localization. Belmonte-Hernández et al. [12] proposed a multisensor fusion adaptive fingerprint (MUFAF) algorithm, which uses interpolation to improve the responsiveness of the algorithm to the environment. Shivanand et al. [13] proposed an asynchronous multi-rate multi-sensor state vector fusion algorithm, which optimizes the localization accuracy by eliminating the coupling between covariance terms. Muniandi and Deenadayalan [14] used wheeled sensors, radar and GNSS as data acquisition sensors, and constructed a VOLUME 8, 2020 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ nonlinear real-time localization model by probability weighting method. Al-Sharman et al. [15] used Kalman innovation sequence and covariance matching technology to continuously adjust through fuzzy inference system, and proposed real-time localization based on adaptive fuzzy Kalman fusion algorithm (AFKF). Plangi et al. [16] proposed a real-time localization algorithm based on Kalman filter algorithm to solve the routing problem in real-time localization. Kumar and Hegde [17] established a multi-sensor combined attenuation model and adopted joint error optimization for multi-sensor data to reduce localization error. Gabela et al. [18] used GNSS and LPS as data sources and improved the localization accuracy by combining extended Kalman filter (EKF) and particle filter (PF). Al Hage et al. [19] proposed an optimal thresholding method based on Kullback-Leibury criterion (KLC), which improves Kalman filter and realizes cooperative localization of robots.
Zsedrovits et al. [20] realized a real-time localization system for unmanned aerial vehicles through airborne cameras and avoidance systems, and useed inertial measurement units and GPS. Ruotsalainen et al. [21] introduced the error probability density function in particle filter, and used the model fitting method to verify the measurement error, thus improving the accuracy of multi-sensor fusion localization. Hosseinyalamdary [22] optimized and improved the measurement error of inertial measurement unit through deep Kalman filter. Rodger [23] used Markov fuzzy, statistical, artificial neural network and nearest neighbor prediction methods to analyze multi-sensor indexes and used improved Kalman filter method to reduce noise in the localization system. Li et al. [24] converted the measured values of different sensors into a set of measurement matrices, which are solved by improving PHD filtering. Cappello et al. [25] implemented a new hybrid controller using fuzzy logic and proportionalintegral-derivative (PID) technology and proposed a real-time localization system based on improved unscented Kalman filter. The contributions of this paper are as follows.
(1) Proposed a multi-sensor fusion localization based on adaptive error correction EKF algorithm to improve the real-time localization accuracy.
(2) Through the contrast adjustment of the sensor detection information, the influence of the error on the estimated value is reduced.
(3) In the two steps of prediction and correction, the normal distribution assumption is carried out twice, so that the predicted value of EKF algorithm is closer to the real value.
(4) Using GA algorithm to optimize EKF algorithm parameters.
The rest of this paper is arranged as follows: Section 2 summarizes the related work; Section 3 performs adaptive error correction on the EKF algorithm; Section 4 performs simulation testing on the improved algorithm; Section 5 summarizes the paper.

II. PROBLEM DESCRIPTION
The multi-sensor navigation system [26]- [33] of unmanned aerial vehicle (UAV) is taken as the research object in this paper. Its main sensors are gyroscopes, acceleration sensors, magnetic sensors, mileage sensors, etc. The above sensor data is corrected and fused through the Extended Kalman Filter (EKF) algorithm [34]- [40] to obtain the real-time location and attitude information of UAV, as shown in FIGURE 1.
The UAV is a dynamic motion process of six degrees of freedom, and its motion stateX can be expressed by four elements of location vectorP t et , space motion speed vectorV t et , attitude representationq and gyroscope rotation vectorb t ω of the space coordinate system.
Considering that ambient noise of the four elements in the space motion velocity vector and the attitude representation when the sensor collected data, it is necessary to perform noise reduction processing first.
where,δ b a is the environmental noise when the acceleration sensor is detected,f b is the specific force measurement value, δ b ω is the environmental noise when the gyroscope is measured,ω b ib is the measured value of the gyroscope, andb b ω is the measurement deviation correction value of the gyroscope. The UAV state representation equation of Equation (1) can be converted intoX bt aδ bt b as system noise, Equation (4) can be simplified toX Due to the nonlinear characteristics of UAV multi-sensor fusion localization system, Equation (5) must be linearized. Firstly, Taylor series expansion is used and expressed by Jacobian matrix.
where, F is the external force vector, G is the acceleration vector, and H is the horizontal direction vector. The motion state can be taken as the state quantity by the location vectorP t et of the spatial coordinate system, the spatial motion speed vectorV t et , the four elements of the attitude representationq and the gyro rotation vectorb t ω , and the EKF algorithm is used for state estimation to obtain the covariance matrix, which is used to correct the state parameters. The process is shown in FIGURE 2.
Although EKF algorithm can better fuse and locate the data collected by multi-sensors, there are still certain localization errors. Therefore, it is necessary to further optimize it.

III. ADAPTIVE ERROR CORRECTION EKF ALGORITHM
A. CONTRAST AND ADJUSTMENT OF PARAMETER ERROR Due to the system noise in the process of multi-sensor fusion localization, there is a certain error between the state estimation value and the actual value of EKF algorithm. Therefore, this paper compares and adjusts the information detected by sensors to reduce the influence of the error on the estimation value.
If the error of the state estimation value is W t , it can be expressed as: where, Z is the state transition matrix of the system, the error of the t time state estimation system is obtained to be where, g t is the observation value of the t time and G t is the observation matrix of the state estimation system. Adding an adaptive adjustment factor σ t to dynamically adjust the weight of the state observation parameters of the system   where,P t is the covariance matrix of the system and η t is Lagrange multiplier vector. The adaptive adjustment factor σ t is where, β 0 and β 1 are the experience values.

B. DISTRIBUTED ERROR SECONDARY CORRECTION
In order to make the predicted value obtained by EKF algorithm closer to the real value, this paper also assumes that it is normal distribution twice in the two steps of prediction and correction respectively. Set the filtering value of the system is (x t−1 , 0), the observation value is g(x t , v t ), and the predicted value is (x t,t−1 , 0) at t time, then the Taylor expansion is approximately The sum of M t−1 , N t−1 , O t , and Q t are Jacobian matrices, and their values are obtained by Then, in the prediction phase of the EKF algorithm, the error covariance matrix P t,t−1 predicted by the EKF algorithm is Then the gain matrix of the state estimation system can be expressed as where, R t is the probability matrix of the system. In the correction phase of the EKF, error correction is performed on the system observation values The system state prediction equation can be expressed aŝ VOLUME 8, 2020

C. PARAMETER OPTIMIZATION OF EKF ALGORITHM BASED ON GA
In order to quickly and accurately find the optimal value of EKF algorithm parameters P t , G t , and W t , this paper introduces Genetic Algorithm (GA) to find the optimal value of EKF algorithm. Taking EKF algorithm parameters P t , G t , and W t as individuals of GA algorithm, and setting the absolute value ρ of the difference between EKF estimation value and real value as the standard of measurement performance, the smaller the ρ, the more accurate the prediction value of EKF algorithm and the smaller the localization error of the system.
The parameter optimization process of EKF algorithm based on GA is shown in FIGURE 3.
Firstly, the individual is initialized, i.e. the parameters P t , G t , and W t are assigned values. Then, according to the absolute value of the difference between EKF estimation value and real value, the adaptability is obtained to realize individual evaluation. Through the replication, crossover and mutation processes of GA algorithm, parameter iteration is carried out, and evaluation function ρ values are compared to obtain the parameter P t , G t and W t When ρ is the smallest.

IV. PERFORMANCE SIMULATION
In order to verify the algorithm proposed in this paper, the performance simulation of the improved EKF algorithm is carried out in the environment of experiment 1. Set the sampling period is T = 0.2, the total number of simulations is N = 50, the random number [0, 1] of environmental noiseδ b a when the acceleration sensor detects and the random number [0, 2] of environmental noiseδ b ω when the gyroscope detects, and randomly set two localization targets D1 and D2.

A. EKF ALGORITHM SIMULATION TEST
The comparison results of EKF-based state parameter estimation values are shown in FIGURE 4 and FIGURE 5.
From the results in Figure 4, the parameter estimation tends to the actual value and its error is reduced.
In the above experiment, the multi-sensor fusion localization error is counted, and the results are shown in FIGURE 5.
From the results of simulation experiments, although EKF algorithm can better fuse and locate the data collected by   multi-sensors, better fit between actual and estimated values, there are still certain localization errors.

B. RESULTS WITH PARAMETER ERROR COMPARISON AND ADJUSTMENT
After the standard EKF is optimized by parameter error comparison adjustment proposed in this paper, the comparison results of state parameter estimation values are shown in FIGURE 6. 93214 VOLUME 8, 2020  From the results in Figure 6, the parameter estimation tends to the actual value and its error is reduced.
The error result of multi-sensor fusion localization is shown in FIGURE 7.
As the information detected by the sensor is compared and adjusted, the influence of the error on the estimated value is reduced, so the localization error is reduced by comparing the results of FIGURE 7 and FIGURE 5.

C. RESULTS WITH DISTRIBUTED ERROR SECONDARY CORRECTION
Based on parameter error comparison and adjustment, the optimization is carried out through the distributed error secondary correction strategy proposed in this paper, and the comparison results of state parameter estimation values are shown in FIGURE 8.
From the results in Figure 8, the parameter estimation tends to the actual value and its error is reduced.The error result of multi-sensor fusion localization is shown in FIGURE 9.
Since the normal distribution assumption is carried out twice in the prediction and correction steps, the predicted value obtained by the improved EKF algorithm is closer to the real value. Compared with the results of FIGURE 9 and FIGURE 7, the localization accuracy is further enhanced.

D. RESULTS WITH GA PARAMETER OPTIMIZATION
Based on distributed error secondary correction, the optimization is carried out through the GA-based parameter optimization processing strategy proposed in this paper, and the comparison results of state parameter estimation values are shown in FIGURE 10.
From the results in Figure 10, the parameter estimation tends to the actual value and its error is reduced.
The error result of multi-sensor fusion localization is shown in FIGURE 11.
As GA algorithm is used to optimize the parameters, the multi-sensor fusion localization error is further reduced. Comparing the results of FIGURE 11 and FIGURE 9, the improved algorithm has better localization accuracy.

V. UAV REAL-TIME LOCALIZATION TEST
In order to verify the application effect of the improved algorithm in the actual system, this paper constructs a multi-sensor UAV real-time localization system for simulation tests. The UAV sensor MCU uses STM32F405 chip, the inertial measurement unit uses MPU6050 chip (integrating accelerometer and gyroscope at the same time), and the VOLUME 8, 2020     In the experimental environment 1, the actual localization error statistical results of the improved EKF algorithm proposed in this paper are as follows.  In the experimental environment 2, the actual localization error statistical results of the improved EKF algorithm proposed in this paper are as follows.
In the experimental environment 3, the actual localization error statistical results of the improved EKF algorithm proposed in this paper are as follows.
From the results of Figure 13 and table 1, the average positioning error is 0.1936 for three positioning points, 0.2324 for six positioning points and 0.2421 for nine positioning points. Therefore, the following conclusions can be drawn, with the continuous increase of localization points, the improved algorithm proposed in this paper still maintains a certain localization accuracy and has strong robustness.

VI. CONCLUSION
The traditional single sensor localization method cannot meet the requirements of high precision and high reliability for moving objects. However, the fusion localization method based on multi-sensor information avoids the deficiency of single sensor and has been studied and applied more and more. In this paper, a multi-sensor fusion localization method based on adaptive error correction EKF algorithm is proposed to solve the problem that the standard extended Kalman filter algorithm has large errors in UAV multi-sensor fusion localization. Experimental simulation results show that the improved algorithm proposed in this paper has higher realtime localization accuracy and higher robustness than the standard EKF algorithm.