Unmanned aerial vehicle positioning based on multi-sensor information fusion

Abstract Unmanned aerial vehicle (UAV) positioning is one of the key techniques in the field of UAV navigation. Although the high positioning precision of UAV can be achieved through global positioning system (GPS), the frequency of updating signal in GPS is low and the energy consumption of GPS module is huge, which does not satisfy the real-time demand of UAV positioning. In this paper, a multi-sensor information fusion method based on GPS, inertial navigation system (INS), and the visible light sensors is proposed for UAV positioning. The Kalman filter combining with simulated annealing algorithm is used to estimate the position error between GPS or INS and the visible light sensors, and then the motion trajectory is corrected according to this position error information. Therefore, the positioning accuracy of UAV can be improved in case of only INS being available. Experimental results demonstrate that the proposed method can remarkably improve the positioning accuracy and greatly reduce the energy consumption.


Introduction
In recent years, with the rapid development of the technology, unmanned aerial vehicle (UAV) have been widely used in many aspects of life, such as target recognition, target tracking, city monitoring, and so on. UAV has many advantages that can save a lot of manpower and material costs.
At present, improving UAV positioning accuracy is very popular at home and abroad. The positioning system of UAV is usually composed of global positioning system (GPS) module and inertial navigation system (INS) module. GPS module provides high precision positioning information, including positioning information, and velocity information, but GPS is vulnerable to external interference, so GPS signal is easy to lose. Moreover, the measuring frequency of GPS is low and cannot satisfy the real-time demand of UAV positioning system. INS module has strong anti-external interference capability, and the measuring frequency is also much higher than GPS. It can satisfy the real-time demand. But INS module accumulates measuring error that can lead to drifting for a long-time measurement. GPS and INS are characterized by complementary advantages. The positioning information of GPS can be used to correct the INS errors based on data fusion technology, which can improve the accuracy of INS measurement. However, most of the current research is based on GPS and INS. Error correction is not in real-time because of the large difference between the two measuring frequencies. Most of the methods involved are based on Kalman filter algorithm. Kalman filter algorithm is an algorithm that is based on recursive estimation theory and the mean square error theory. It estimates in the order of "prediction -measurement -correction", the selection of the initial value is difficult. GPS module energy consumption is much larger than INS module, and it seriously affects UAV lifetime.
In recent years, scholars worldwide have done a lot of research for the positioning technology of GPS and INS integrated system. Kalman filter algorithm has excellent ability of information forecasting and processing. The basic theory of linear discrete Kalman filter is applied to the data estimation and flight control for UAV positioning. It helps to realize accurate positioning and trajectory tracking of UAV . By means of the extended Kalman filter method and the least squares method, it can enhance the acceptance of the airborne radio frequency (RF) positioning signal under the interference of colored noise (Yang et al. 2016). Fade in Kalman filter technology effectively prevents the filtering divergence of GPS and INS integrated navigation system, with a wider range of use (Yang et al. 2012). The "fuzzy logic" self-adaption algorithm can solve the problems of INS position error with time, the blocked GPS transmission signal and poor accuracy of GPS positioning (Wang, Jiang, and Yan 2016). The hybrid dynamic belief propagation algorithm that is used in Multi-UAV co-positioning, can give a distributed online estimation of the positioning and velocity of each UAV, according to the output of other GPS, the relative distance between adjacent UAV and the output of accelerometer. (Klein 2004;Jiu, Bu, and Zhong 2016).
Foreign scholars have also made many fruitful works. Strong tracking filter has a strong tracking ability for the mutation state, and can maintain the ability until the filter reaches steady state, so it can effectively improve the positioning accuracy of UAV (Yoo and Ahn 2003;George and Sukkarich 2005;Samadzadegan and Abdi 2012). Extended Kalman filter algorithm has the advantage that can be implemented simply (KhademSohi, Sharifian, and Faez 2017). Machine learning, machine vision processing, and so on have also been widely used in the fields of UAV navigation and posture assessment (Zhang, Wang, and Farooq 2010;Tang et al. 2015).
In this paper, we proposed a UAV positioning method, considering that GPS, INS, and the visible light sensors have different measuring frequencies (Gross, Gu, and Rhudy 2012). We use Kalman filter to estimate the position error based on GPS (or INS) and the visible light sensors, respectively. The trajectory of UAV is corrected according to the position error information, so UAV positioning accuracy is improved in case of only INS being available, and the UAV lifetime is prolonged.

Design of UAV integrated positioning system model
GPS signal is susceptible to interference, and its real-time performance is not excellent. INS information accuracy depends on the inertial measurement devices and position error accumulated over time, so both of them are not suitable as a reliable source of location information alone for a long time. GPS and INS integrated positioning system solves the problems that precision and realtime are not excellent to some degree. The difference between GPS measuring frequency and INS measuring frequency is large: GPS is usually 1−2 Hz and INS is 100 Hz. It inevitably affects the result of UAV positioning. In some fields of high positioning accuracy and realtime demand, fusing GPS and INS data is a common method to meet the demand. In order to improve the accuracy of UAV positioning, the advantages of GPS, INS, and the visible light sensors can be used.
The working frequency of the visible light sensors is usually 20 Hz as needed, and the positioning information and velocity information are obtained based on the image processing technology. In the case that the resolution of the visual information is high enough, the positioning accuracy of the visible light sensors is better than INS. Moreover, the accumulation of device error in the visible light sensors is far less than INS.
The proposed method uses Kalman filter to fuse GPS and the visible light sensors data when GPS is updating and uses the position error between them to update the visible light sensors. In the same way, the method uses Kalman filter to fuse INS and the visible light sensors data when GPS is not working and uses the position error between them to update the flight parameters of UAV. The initial value of Kalman filter is usually a set of random values in the solution domain. The simulated annealing algorithm is a generalized optimization algorithm with global optimization, which can obtain the optimal initial value of Kalman filter and improve the accuracy of the estimation. The overall algorithm flow is shown in Figure 1. Ultimately, the accuracy of UAV positioning is improved in case of only INS being available.

Kalman filter
Kalman filter algorithm is a widely used real-time filtering method (Suchanek and Weikum 2014). It outputs the state or parameters (estimated value) of the system based on the system noise. The observation noise statistics and observation of the system are the filter input. It is an optimal estimation method.
In general, for nonlinear system, the linear approximation of Taylor's expansion is analyzed. Kalman filter state equation and the measurement equation of the stochastic discrete system are given when the control function of the system determine input is not taken into account.
where X t is the state of the system at time t, Φ t, t−1 is the state transition matrix of the system, W t−1 is the estimated process noise of the system, Z t is the observation of the system at time t, H t is the observation matrix of the system at time t, V t is the observation noise of the system at time t. The estimated process noise of the system and the observation noise are assumed to be the Gaussian white noise which is independent and normal distribution.
(1) Kalman filter is used to estimate the current and future state of the system based on time updating equation and state updating equation (Thomas and Balakrishnan 2009). It is a recursive calculation method, and the minimum error variance is the estimation criterion. State step prediction : One step prediction error variance matrix: Equations (5) and (6) are the time updating equations. State Estimation: Filtering gain matrix: Estimated error variance matrix: Equations (7-9) are the state updating equations.

System state model
The positioning information error between the sensors is taken as the state value of the system, and the value is estimated by a linear discrete Kalman filter (Attoh-Okine and Mensah 2009). Then, the positioning parameters of the visible light sensors and INS are corrected according to the value. The flight path of UAV is also corrected.
In the geocentric coordinate system, the value of GPS pseudo range is calculated: The transformation between geocentric coordinate system and geodetic coordinate system: where N is the radius of curvature of the point, e is the first eccentricity of the ellipsoid in the geodetic coordinate system.
In the geodetic coordinate system, the value of GPS position is calculated: The inertial navigation measures the acceleration of the moving object and calculates the velocity and position by acceleration. In the body frame coordinate, u, ̇v, and ẇ are the true accelerations of UAV.
where a bx , a by , and a bz are the components of centroid acceleration in the body frame coordinate system when UAV is flying in the curve. g bx , g by , and g bz are the components of gravitational acceleration in the body frame coordinate system. p, q, and r are the instantaneous tri-axial angle vectors of UAV. δe is the measurement error of INS.
After two integral operations on u, ̇v, and ẇ, the position of UAV in the body frame coordinate system can be obtained.
The transformation between body frame coordinate system and navigation coordinate system: where φ, θ, and ψ are, respectively, the roll angle, pitch angle, and yaw angle of UAV.
The transformation between navigation coordinate system and geocentric coordinate system: cos cos − sin cos + cos sin sin sin sin + cos sin cos sin cos cos cos + cos sin sin − cos sin + sin sin cos − sin cos sin cos cos where X si , Y si , and Z si are the position coordinates of the i-th satellite in the geocentric coordinate system; X, Y, and Z are the position coordinates of GPS in the

Estimation and correction
The error between GPS and the visible light sensors can be reduced using the data of GPS to update the visible light sensors when GPS is working. GPS and the visible light sensors compose the positioning system. According to Equation (2), the measurement equation of the system is shown as Equation (21). The position error between GPS and the visible light sensors is the state value of the system, X t = [ L GC , B GC , H GC ] is the measurement of GPS positioning information.
According to Equation (5), X t−1 is the state of the system at time t-1 and is used to estimate the state of the system at time t, X t,t−1 . According to Equation (6), the error variance matrix of the system at time t-1, P t-1 is used to update the error variance matrix of the system at time t, P t, t-1 .
After transformation, Equation (8) can be written in the following form: Equation (9) can be further written in the following form: And: According to the linear minimum variance estimation criterion, the estimated value of mean square error is minimal and the value of filter gain matrix K t can be calculated. Finally, the value K t is substituted into the Equation (7), and the estimated state value X t of the system can be calculated. The value is the estimated positioning information error value between GPS and the visible light sensors at time t. Similarly, according to the recursive relationship, the estimated positioning information error value between GPS and the visible light sensors at time t + 1, t + 2, … can be calculated too.
According to the trend of the future estimated positioning information error value of the visible light sensors and GPS, the positioning parameters of the visible light sensors and the mapping information of positioning are corrected to improve the accuracy of the positioning information of the visible light sensors.
The error based on INS and the visible light sensors can be reduced using the data of the visible light sensors to update INS when GPS is not working. INS and the visible light sensors compose the positioning where ϕ and λ are the latitude and longitude of the origin of the local horizontal coordinate system, respectively.
Based on different coordinate transformation, INS positioning measurement information can be expressed as the sum of the true value of the position and the corresponding error of INS: According to the projection positioning algorithm, the UAV positioning information in the navigation coordinate system can be obtained in real time. The visible light sensors positioning information in the geodetic coordinate system can be got based on coordinate transformation.
The visible light sensors positioning measurement information can be expressed as the sum of the true value of the position and the corresponding error of the visible light sensors: The error of positioning information between GPS and INS is: The error of positioning information between GPS and the visible light sensors is: The error of positioning information between INS and the visible light sensor is: so the accuracy of UAV positioning can be improved in real time.
In the process of Kalman filter, GPS error, INS error and the visible light sensors error, the state transition matrix of the system Φ t,t-1 and the estimated process noise W t-1 can be obtained by prior knowledge. In order to simplify the analysis process, it can be assumed that the observation matrix has a time invariant property, and the observation matrix H and the measurement noise V t of the system can be obtained according to Equations (22) and (25). The entire filter data processing flow chart is shown in Figure 2.
When GPS is working, the error of positioning between GPS and the visible light sensors is the value to be estimated. The Kalman filter is used to fuse the data of GPS and the visible light sensors, then the relevant parameters of the visible light sensors are corrected to reduce the measurement error of positioning between the visible light sensors and GPS based on the forecasting trends. Similarly, when GPS is not working, the error of positioning between INS and the visible light sensors is the value to be estimated. The Kalman filter is used to fuse the data of INS and the visible light sensors, then the relevant parameters of INS are corrected to reduce the measurement error of positioning between the visible light sensors and INS based on the forecasting trends. The operating frequency of the visible light sensors is higher than that of GPS. So the accuracy and the realtime performance of UAV can be improved based on this way in case of only INS being available.

Selection of the initial value of Kalman filter
Simulated annealing algorithm is a comparative random combination optimization algorithm which is developed in the early 1980s. It is a process to simulate the cooling of metal from high temperature. The algorithm has been widely used in solving combinatorial optimization problem. Using the advantage of local search ability of simulated annealing algorithm, it is possible to avoid the case of local minima in the search process. In the process of optimization calculation, simulated annealing algorithm sets the initial temperature firstly, selects a random initial state, and examines the state value of the objective function according to requirements: it accepts a good point with probability 1, accepts a poor point with probability P a . The point is set as the current point until the system reaches the preset cooling state.
Simulated annealing algorithm is a general optimization algorithm, the algorithm has global optimization performance in theoretically. Kalman filter is sensitive to the initial value, inappropriate initial value may result in a larger Kalman filter error and a slower convergence rate, so it is necessary to determine a high quality initial value. The optimal solution obtained by the simulated system, According to Equation (2), the measurement equation of the system is given in Equation (25). The positioning information error between INS and the visible light sensors is the state value of the system, X t = L IC , B IC , H IC is the measurement of INS positioning information.
Based on our proposed method, the estimated positioning information error value between INS and the visible light sensors in the future time t + 1, t + 2, … can be obtained. The positioning information error value of INS is: The estimated positioning information error value between INS and the visible light sensors is a function of t = tN , tE , tU . In case of only INS being available, the velocity of UAV in three directions is corrected in real time, UAV flight path can be controlled in real time,  Figure 2. flow chart of filter data processing.
Step 5: The temperature updating function is: t k+1 = t k , = 0.8. Convergence criteria is: according to experience, if five consecutive solutions are not accepted as a new solution with probability 1, or the number of iterations is T max , the search is stopped and the current solution is the global optimal solution. Otherwise, return to Step 3. The entire simulated annealing algorithm data processing flow chart is shown in Figure 3.
According to Equation (24) and E(X 0 ), the equation about the integration of GPS and the visible light sensors can be introduced: where 2 E , 2 N , 2 U are the attitude misalignment errors of UAV body platform, L 2 V , B 2 V , H 2 V are the priori errors of the visible light sensors, and L 2 G , B 2 G , H 2 G are the priori errors of GPS.
Similarly, the equation on the integration of INS and the visible light sensors can be introduced: where L 2 I , B 2 I , and H 2 I are the priori errors of INS.

Experiment and simulation analysis
In order to verify the proposed positioning method, a test platform is developed. The platform is equipped with GPS, INS, and the digital camera. The real flight test has been carried out, and the simulation analysis by Matlab has also been carried out based on the collected data. In order to simplify the analysis, the positioning information of UAV is analyzed only on the E(X)N(Y)-plane in the navigation coordinate system. High-precision positioning device to acquire the positioning information of UAV in real time is used. The dynamic and static positioning accuracy can reach meter level. The information can be used as a real positioning information reference. The data-set used in this paper is a series of measured data in Yichang city. In this data-set, a 2-km path was driven by an equipped vehicle in about 100 s.
annealing algorithm is taken as the initial value of the Kalman filter. The combination of the two algorithms can avoid the larger error of data acquisition because of the influence of the Gaussian noise, improve the convergence rate of the estimate, and further improve the accuracy and the real-time performance. The procedure for finding the optimal initial value of Kalman filter is as follows: Step 1: As defined by the system state, the estimated initial value X 0 is random. In the solution domain, it supposes that there are n random solutions, the alternative value Ṕ g of the global optimal value P g , the temperature parameter t, the number of iterations k, and the maximum number of iterations T max .
Step 2: Calculate the initial temperature: t 0 = − f max − f min ∕ ln( ). ρ τ represents the initial acceptance probability, the general value is 0.1. f max is the target value of the best individual in the initial state and f min is the target value of the worst individual in the initial state.
Step 3: A new feasible solution x can be produced in the solution domain based on x =Ṕ g + × X max + X min × N(0, 1) is the search step, X max is the upper bound of the solution and X min is the lower bound of the solution.
Step 4: Calculate x based on f (x) and calculate Ṕ g based the three-sensor combination positioning system is less than the GPS/INS combined positioning system. The estimation error is the difference between the position information estimated by the positioning system and the true position information.
Compared with the traditional GPS and INS integrated positioning system, the GPS, INS, and the visible light sensors integrated positioning system, with the passage of time, effectively reduces the position error and significantly improves positioning accuracy.
positioning system. In Figures 4 and 5, the flight trajectory estimated by the three-sensor combined positioning system is closer to the true position based on the comparison of the three data.
The position error between real path and estimated path of GPS/INS, and the position error between real path and estimated path of three sensors integration are shown in Figures 6 and 7. The UAV has been controlled to fly for 100 s along X = 200 m and Y = 200 m, respectively. As time goes on, the estimation error of The energy consumption of the UAV integrated positioning system is monitored, and the energy consumption curves are shown in Figure 9. Energy consumption ratio per unit time means the percentage of GPS module in the total energy consumption in 1 s. The total energy consumption was measured with a constant 1-Hz working frequency of GPS module. The energy consumption of the UAV integrated positioning system increases significantly with the increasing frequency of GPS.
The performances of the non-simulated annealing Kalman filter and the simulated annealing Kalman filter are shown in Figure 8. With the increasing calculation times, the filter error increases. However, the growth error curve of simulated annealing Kalman filter is obviously better than the linear discrete Kalman filter. Therefore, the performance of the filter is improved after introducing the simulated annealing algorithm to optimize the initial value of Kalman filter.

Conclusions
Using Kalman filter algorithm to fuse GPS, INS and the visible light sensors data, not only the positioning information of UAV is estimated accurately, but also the error correction efficiency is improved. The accuracy and the real-time performance of positioning are further improved compared with GPS and INS integrated positioning system. At the same time, reducing GPS working time as much as possible can reduce the energy consumption of UAV battery and extend UAV lifetime. We propose a UAV positioning method, integrating the GPS, INS and the visible light sensors. For the case that GPS is not available, the visible light sensors positioning information is used to assist INS to estimate position and correct error. Simulated annealing algorithm is also introduced to optimize the initial value of Kalman filter to improve the accuracy of Kalman filter and the accuracy of the entire positioning system. According to the experimental results, compared with Kalman filter with the random initial value, it can be seen that simulated annealing Kalman filter can improve the accuracy of the estimated value under the ideal condition. Compared with GPS and INS integrated positioning system, the method can effectively improve the accuracy of positioning information and UAV lifetime, so it has a strong research and practice significance. However, the proposed method requires the high resolution of visual information, for which the image processing involves a huge computing work (Garcia et al. 2008). Cloud computing, large throughput, and reliable wireless communication technology may be acted as the solutions in the future.

Notes on contributors
Wenjun Li is an engineer of the 710th Research Institute of China Shipbuilding Industry Corporation. His research focuses on signal and information processing and the control of UAV flight.
Zhaoyu Fu is an engineer of the 710th Research Institute of China Shipbuilding Industry Corporation. His research focuses on integrated circuit design of UAV flight.