Abstract
The superior performance of factor graphs compared to Kalman filtering in various fields and the use of factor graph algorithms instead of Kalman filtering algorithms in moving target localization tasks can reduce target localization error by more than 50%. However, the global factor graph algorithm may cause computational delays due to excessive computational effort. A moving target localization algorithm based on a combination of global and incremental optimization with improved factor graphs is proposed to improve localization accuracy and ensure that the computation time can be adapted to the requirements of online location. A reference point is introduced into the incremental calculation process, and it is first determined whether global or incremental calculation is used for this calculation by comparing the distance between the incremental localization results of the calculated reference point. The position of the UAV itself is then corrected by determining the position of the reference point, and this is used to finally locate the target. Simulation results show that the algorithm has good real-time performance compared to the time-consuming global algorithm. The online localization error of moving targets can be reduced by 17% compared to the incremental calculation results of the classical factor graph algorithm.
1. Introduction
Timely and accurate target location tracking is an important task in the field of unmanned aerial vehicle (UAV) applications, and the common method is laser location. That is, the distance between the target and the UAV and the camera’s rotation angle relative to the UAV is obtained by means of a camera mounted on the gimbal and a laser rangefinder. Then, the target position relative to the UAV can be calculated, and the target position in the geodetic coordinate system is obtained by means of coordinate conversion. The position of the target in the geodetic coordinate system is then obtained by means of a coordinate transformation, which in turn gives the longitude, latitude, and altitude of the target. In this method, there are errors in the UAV’s own navigation, optical axis pointing errors of the camera, and laser ranging errors, which lead to low accuracy in the final result of target positioning due to the introduction of many error sources. However, due to cost constraints, it is unrealistic to improve the sensor’s measurement accuracy. Moreover, due to the presence of random errors, an increase in sensor accuracy may not necessarily lead to an increase in the accuracy of measurement results. Therefore, selecting the appropriate algorithm to process the collected information in order to obtain optimal results is an important goal in the UAV positioning task.
When a single UAV is used for target location and tracking tasks, filtering is generally used to suppress the effects of measurement errors, with Kalman filtering and its modifications being the most common method of data fusion. For example, in the literature [1], Song et al. used the extended Kalman filter (EKF) algorithm for target location. In the literature [2], Ullah et al. combine EKF, unscented Kalman filter (UKF), and particle filter (PF) into one framework to calculate target position. In the process of localization a moving target, the motion state of a noncooperative target cannot be accurately known. To avoid this problem, the literature [3] directly uses the target position at the previous moment plus a noise matrix with large a variance as a priori estimate of the target position and then uses Kalman filtering for moving target locating. To solve this problem, the literature [4–7] used an interacting multiple model (IMM) algorithm to estimate a priori the target position at the next moment and then used the EKF[4, 5] and UKF[6, 7] to fuse the information and determine the state of the target’s motion at the next moment. The IMM algorithm fuses several possible target motion models and adaptively transforms them to characterize the target motion state.
It can be seen that the target motion state is necessarily estimated when using the Kalman filter algorithm for target location. However, when tracking noncooperative targets, errors are inevitable in everything we model about the target. Also, the Kalman filter offers limited improvement in target location results and does not reduce the impact of the UAV’s own position and attitude errors on the final target location results. And to avoid model errors and reduce the UAV’s own position and attitude errors, we can use factor graphs to locate the target.
In addition to eliminating the need to model the target’s state of motion in moving target location tasks, the biggest advantage of the factor graph algorithm over Kalman filtering is that the factor graph algorithm not only calculates the target’s position at the current moment but also optimizes the target’s previous trajectory based on the current measurements, correcting the location results of the moving target, and speeding up convergence. Moreover, in contrast to the Kalman filter algorithm, which can only calculate the position of the target from the position of the UAV, the factor graph algorithm can optimize the position of both the UAV and the target and obtain the optimal estimate for both sides at the same time. Finally, the factor graph algorithm makes full use of the global optimization feature of factor graphs to divide the complex problem into a number of simple problems to be calculated separately, guaranteeing the accuracy of the algorithm while effectively reducing computational effort and computation time.
The factor graph [8–10] algorithm is an estimation algorithm based on Bayesian networks, which has shown good performance in many fields. For example, in the navigation problem, there are Wen et al. in literature [11], Wei et al. in literature [12], Gao et al. in literature [13], Liu et al. in literature [14], and Xu et al. in literature [15], all of which use the factor graph algorithm instead of the Kalman filter algorithm. The factor graph algorithm offers superior performance while also ensuring good robustness. Therefore, the factor graph algorithm can certainly play a larger role in the target localization problem as well.
In fact, there are many applications of factor graph algorithms in the field of target localization. For example, in the literature [16], Zhang et al. assume that the target motion is nonlinear, and the extended Kalman filter (EKF) is applied, where the observation process is implemented by a practical position detector based on AOA, forming a unified factor graph (FG) framework. This dynamic estimation method offers higher robustness in the presence of unstable sensing environments than conventional methods. However, this method requires several iterations and is computationally intensive. In the literature [17], Hao et al. aim at the character of plug and play for iGPS. A factor graph model based on a Bayesian network is built, and then a sum product algorithm is used to convert the fixed model into the form of the product of each node, which reduces the independence of measurement information and improves the confidence of the results. However, it is aimed at measurements in large-scale conditions, requiring a large number of nodes and is not applicable to the case of target location tracking by UAVs. In the [18], Liu et al. proposed a recursive algorithm to estimate the location of mobile stations in an offline line-of-sight environment, the focus is on the non-line-of-sight environment, and it is a recursive algorithm requiring iterations. In the literature [19], Tang et al. proposed a colocation algorithm for distributed underwater node weighted factor graphs, i.e., using factor graphs and sum-product algorithms to decompose global optimization into the product of several local optimization functions, but the complexity of algorithm has a certain improvement. In literature the [20], Wan et al. simultaneously estimate the landmark position and attitude of mobile robots online and distributed by combining range data from each robot with the relative robot-robot or robot landmark distances and angles. Its main focus is on non-Gaussian distributions.
In literature [21], Vanli et al. proposed a method for estimating the covariance of inputs in a factor graph formulation under non-line-of-sight conditions. Same as in literature [18], they focus on non-line-of-sight targets. In the literature [22], Kahar Aziz et al. proposed a new geolocation technique based on factor graphs. They consider a factor graph-based geolocation technique where the input is a sample of the direction of arrival measurements sent from the sensor. But it requires a large number of nodes for positioning. In the literature [23], Yuan et al. provide a unified passive location framework based on factor graph for TOA measurement-based wireless sensor networks. Based on the linearization of distance measurements, they build a Forney-style factor graph model and propose a corresponding Gaussian message passing algorithm to obtain the target position. Its main focus is on the TOA problem. In the literature [24], Zhao et al. proposed a new factor graph and Kalman filtering (FGCKF) integration algorithm. This algorithm, which is still combined with the Kalman filter, does not fully serve the shortcomings of the Kalman filter algorithm.
In the literature [25], Fan et al. established a colocalization algorithm based on factor graphs and sum products (FGS), transforming the global function estimation problem into an incremental function and product estimation problem. In the literature [26], Fan et al. proposed a novel CP algorithm based on factor graph and maximum entropy (FGMC) to improve the global positioning accuracy of AUVs. However, both of the above papers have the same flaw: AUV does not need to take depth into account, so the problem addressed in this paper falls into the two-dimensional category. In the literature [27], Zhang et al. used factor graph optimization (FGO) to optimize the interagent localization solution to improve the accuracy and robustness of the collaborative algorithm. This method utilizes measurement data from surrounding available GNSS receivers; thus, a large number of nodes are required. In literature [28], Zhu et al. proposed a distributed cooperative localization technique based on weighted factor graphs. A belief information transfer model is developed by combining factor graphs and sum-product algorithms with full consideration of collaborative node ranging errors and coordinating terminal location ambiguities and weighting information from neighboring nodes after iterative convergence of information. This method also needs many nodes. In the literature [29], Zhang et al. developed a fingerprinting technique based on the received signal strength difference (RSSD) and a method using factor graphs for two-dimensional scenes to achieve accurate detection of unknown radio transmitters. The methodology in this paper addresses RSSD positioning. In the literature [30], Tang et al. proposed a three-dimensional fast colocalization algorithm (FG-3DCP), which combines factor graph and sum-product theory to establish a multinode colocalization model. This method requires multiple nodes.
In the literature [31], Bai et al. used a carrier phase measurement within a window, known as the window carrier phase, to constrain the state within the factor graph, and a left null matrix is used to remove shared unknown ambiguous variables and localize the target. This is a GNSS localization method. In the literature [32], Pschmann et al. represented the results of an off-the-shelf 3D object detector as a Gaussian mixture model and incorporated it into a factor graph framework, thus providing the flexibility to assign all detection results to all objects simultaneously. This approach is applicable to multitarget tracking. When there are more target points forming a point cloud, the localization results between targets can be corrected for each other, but when there are fewer targets, the accuracy of target localization results from this method may be reduced.
There are also some literatures on related research. In the literature [33], Belge et al. optimal path planning and tracking using the Harris Hawk optimization (HHO) and grey wolf optimization (GWO) algorithms to enable UAVs to achieve payload hold-release missions and avoid obstacles. In the literature [34], Aytaç et al. proposed an MPC controller based on the Hammerstein model for real-time target tracking of a three-axis frame system, resulting in the robustness of the UAV under external disturbances. In the literature [35], Aytaç et al. put the flight data through preprocessing, feature extraction, and feature selection to obtain a nonlinear autoregressive heterogeneous (NARX) model of the UAV. The neural network model obtained was embedded in the flight control card to achieve real-time path tracking of the UAV. In the literature [36], the parameters of the proposed control algorithm for the UAV were estimated using a swarm intelligence-based metaheuristic optimization algorithm by Aytaç et al. Attitude and altitude control of the quadrotor using metaheuristic optimization algorithms such as particle swarm optimization (PSO) and Harris Hawhaks optimization (HHO) for paths with different geometries such as rectangular, circular, and lemon shapes. In the literature [37], Aytaç et al. address target tracking modelling of a three-axis gimbal system on a UAV with an RRR joint structure based on experimental input (motor speed) and output (end-effector position) data. The UAV moves in a certain direction, and the camera on the end-effector of the gimbal system on it adheres to the correct target.
To conclude, although motion factor graph has become more common in the process of target localization, there is still scope for research on factor graph localization in terms of TOA-based tracking of moving targets by a small number of UAVs for the environment described in this paper. This paper proposes a moving target online tracking method with lower computational complexity and fewer iterations, which can provide a more accurate estimate of the target’s position while adapting to the time requirements of online computing.
In the second section of this paper, a single UAV model for ground target location is established, and a motion target location model based on the Kalman filter is briefly introduced. In the third section, the principle of the factor graph algorithm is briefly introduced, and a model for moving target location using the factor graph algorithm is established. In order to solve the shortcomings of the time-consuming offline location task, incremental optimization is used to calculate the target position at each moment, and in order to further reduce the error of the optimization results, reference points are introduced. In the fourth section, the error of the incremental optimization remains large at a later stage. And in order to further improve the accuracy of the online optimization, the global and incremental calculations in the factor graph optimization process are combined. The coordinates of the reference points are obtained by alternating between the global and incremental calculations, and then the UAV position error is constrained by fixing the coordinates of the reference points. Subsequently, the target position error is further reduced by constraint. In the fifth section, a simulation is carried out to compare the incremental computation model of factor graph optimization with the algorithm proposed in this paper, which proves that the proposed method can effectively reduce the errors in the moving target localization process.
The main contributions of this paper are (1)Establishing a factor graph model for the UAV location of moving targets and comparing it with the results of the UKF. The global optimization of the factor graph results in an error reduction of more than 50% compared to UKF(2)Introducing reference points into the incremental location model of the factor graph to constrain the calculation results of the UAV’s own motion trajectory with reference points, thereby reducing the error of the moving target location results(3)Combining global and incremental calculation, choosing the coordinates of a suitable reference point, reducing the calculation time, and at the same time reducing the error of the moving target positioning results
2. Moving Target Location Model
2.1. Principle of Moving Target Location
The UAV target location system discussed in this paper is equipped with a satellite receiver, an inertial measurement unit, and an optoelectronic reconnaissance platform. When the UAV finds the target, the optoelectronic reconnaissance platform is able to drive the camera gimbal and lock the target in the center of the camera’s field of view, while data on the camera’s azimuth, pitch angle, laser ranging distance, and UAV position and attitude are available for target position resolution. The positioning process is shown in Figure 1.
The target is calculated under the airborne coordinate system using a simple trigonometric sine cosine relationship. Let the UAV position coordinates under the geodetic right-angle coordinate system be , and the target position coordinates under the geodetic right-angle coordinate system be . Define the distance between the UAV and the target, the pitch angle on plane , rotated counter-clockwise as positive and clockwise as negative. And the azimuth angle is defined on plane , rotated counter-clockwise as positive. Then, the result of its positioning in the onboard coordinate system, , is as follows:
Subsequently, the target can be converted into a geodetic right-angle coordinate system by means of a coordinate transformation.
2.2. Kalman Filter-Based Model for Moving Target UAV Location
Let the UAV find the target point and take measurements of the target point. At the th measurement, the UAV position coordinates in the geodetic right-angle coordinate system are , and the target position coordinates in the geodetic right-angle coordinate system are . The state variable is selected to represent the target position estimation. Using the method of literature [3], the target can be assumed to be stationary, and a larger system noise matrix is added as a prior estimate for the solution of the new target coordinates. Then, the discrete state equation for the system is where is the state transfer matrix, and is the system noise matrix, obeying a Gaussian distribution with mean 0 and variance , which means that .
For noncooperative targets, the target’s motion state is unknown, and there is no available equation to describe the motion, so the state transfer matrix is set to
The distinction between moving and stationary targets is usually made by modifying the system noise variance . When the target being tracked is stationary, can be set to a matrix of 0. But when the target being tracked is in motion, the diagonal matrix is assigned a value as required.
The system measurement equations are set as follows: where is the measurement of random noise with covariance matrix , system noise , and measurement noise and are both set to mutually uncorrelated zero mean white noise, and the measurement value vector is .
The Kalman gain is then calculated and the posterior mean and covariance updated according to the UKF equations for moving target location calculations.
2.3. Moving Target Localization Algorithm Based on Factor Graph
As we all know, UKF is based on the Kalman filter [38]. The difference between the Kalman filter and the UKF is that the UKF uses moment matching to approximate the distribution of certain computational processes. Therefore, the UKF has the same limitations as the Kalman filter. The Kalman filter belongs to the Gaussian Bayesian filters. It solves the limitation that an analytical solution cannot be found for the prior probability density function, normalization constants, optimal estimates, and so on when infinite integrals are involved by assuming that the state vector and measurement vector are linear and by assuming that both process noise and measurement noise obey a normal distribution.
The factor graph, on the other hand, is a Bayesian network. Thus, Kalman filtering can be considered a simpler application of Bayesian networks. There are only two moments, and , and the target state at moment can only be inferred from the estimation and measurement at moment ; but the target state at moment cannot be inferred back from the target state and measurement at moment .
In the target localization problem, the Kalman filter is more similar to a directed graph with a sequential structure, and its graph optimization structure can be expressed as shown in Figure 2.
Taken individually at a given moment, the Kalman filter can then be represented as shown in Figure 3.
2.4. Factor Graph Model for Moving Target Location
Factor graphs are probabilistic graph models based on Bayesian networks or Markov random fields, and as an undirected graph model, they contain both types of nodes, variable nodes, and factor nodes. In general, the variables to be estimated are denoted as “variable nodes,” the constraints between these variables are denoted as “factor nodes,” and the variable nodes are connected to the variable nodes by factor nodes. In the UAV target location problem, the UAV position and attitude variables and target position variables are usually represented as variable nodes, while the factor nodes are represented as sensor data in the variable system.
By constructing a graphical model of the system over a certain time interval, the factor graph establishes the relationship between the measured values and the system state values and decomposes a complex problem into the product of several simple problems. It is a data fusion method based on posterior estimation theory that calculates the maximum posterior estimate of the joint probability distribution function for all states.
In the factor graph algorithm, the position of the moving target at each moment is considered a signpost, and each signpost corresponds to the position of the UAV at each moment. The motion state of the target is tracked by calculating the coordinates of the signpost at each moment.
The function of the factor graph algorithm is to transform the solution of the maximum posteriori estimate of the navigation state into the inference of the factor graph. In a target location system, if the position and attitude of the UAV at the moment of system are , the state of the UAV up to the moment of can be expressed as .
Similarly, denoting the position of the moving target at the moment as , the position of the target can be expressed as another set by moment as . Denoting GPS system measurements at the moment as , measurements collected by the GPS system up to the moment can be represented as . Denote the measured quantities of the inertial guidance system at moment as , then, by moment , the measured quantities collected by the inertial guidance system are denoted as . Denote the measured quantities from the onboard camera and laser system at moment as , then, by moment , the measured quantities collected by the system are denoted as .
According to factor graph theory, the solution to the optimal estimate of moving target localization can be transformed into the problem of solving the maximum a posteriori estimate of the joint probability distribution function . Thus, the moving target position problem can be decomposed as
Denoting all the quantities to be solved and as , we obtain the maximum a posteriori estimate by maximizing the joint probability in the above equation:
The following nonlinear least squares problem was introduced:
Linearizing the above equation and eliminating the covariance matrix effect, we end up with the following least squares problem: where and are the factor matrix. is the Jacobi matrix set, is the residual measurement matrix, and is the error term in the Taylor expansion process. The factor diagram solution can take the form of matrix decomposition followed by Givens transformation.
As can be seen, the global calculation of the factor graph model not only calculates the target position based on the UAV position but it can also estimate both the UAV position and the coordinate position, thus reducing the positioning error in the final result. Moreover, the factor graph not only derives the new target position, as in the case of the Kalman filter but can also correct the target position already obtained, making full use of all the measurements and improving the target positioning accuracy of the final result.
For any problem, after the system receives a new sensor measurement each time and then generates a new factor in the factor graph, the number of nodes in the factor graph increases with the time the system is working. Therefore, if the factor graph is solved globally at every moment to reestimate the optimal estimate of all variables, this leads to an increase in the size of the matrix as the time increases and the data increases, and the time required for a large number of calculations continues to increase, which can reach more than two hours when the number of nodes reaches 1000 and is not suitable for online localization of moving targets. Therefore, in most cases, only the newly added data is computed, and an incremental solution (sliding window) is used to solve the problem.
The solution and update require a QR decomposition or Cholesky decomposition of the Jacobi matrix into upper triangular matrices. For the problems mentioned in this paper, the partial differential of can be obtained and set to zero, giving the following equation.
is then updated to a new estimate and used as the initial value for the next iteration. In the factor diagram, the final solution is obtained by variable elimination.
According to the principles of factor graphs, the elements of the modified matrix are only relevant to the newly added factor nodes, and the addition of new nodes only affects the estimation of neighboring nodes and the estimation of some of the state variables. Therefore, at the current inference time, only the affected state variables and the newly added state variables need to be estimated. When a new factor node is added to the factor graph, the affected part and the state variables involved are determined based on the state variables contained in the new factor node. The sections containing these state variables and the newly added factor node form a new factor graph that is linked to unaffected sections for incremental calculations.
For example, the Jacobi matrix of the factor graph at moment is .
When at the moment , adding new factors to the graph, the Jacobi matrix is . where denotes the newly added node.
The model for optimizing factor graphs for the moving target localization problem discussed in this paper is as Figure 4.
2.5. Reference Point-Based Factor Graph Model for Moving Target Location
It is clear that factor graph optimization has excellent performance over Kalman filtering in global optimization, but its location error has the potential to continue to be reduced. Given the nature of factor graphs, which estimate the position of both the UAV and the target point, and can continue to optimize the previously obtained state of the moment while updating the state of the moment, and the photographs taken by the camera on board the UAV, where the target is naturally in the center, there are also some other fixed invariant points. Finding a fixed reference point to add to the calculation can reduce the error in moving the target location compared to calculating the target position directly.
The camera carried by the UAV is not alone in the photos obtained during the flight of the UAV; only the target, in which a clearer fixed feature point is found as a reference point, and the coordinates of the reference point are calculated at the same time as each iteration, and the UAV’s self-positioning error is constrained by the condition that the reference point remains unchanged, with the graphical model as shown in Figure 5.
We modify the previous formula by denoting the position of the moving target at the moment as and the position of the reference point as . Then, by the moment , the target position can be modified compared to the previous one as .
Denote the measured quantities from the onboard camera and laser system at moment as . By moment , the measured quantities collected by the system are represented as a different set than before.
Then, we modified the previous nonlinear least squares problem to be modified as follows:
The resulting expression for the least squares problem is invariant
However, the elements of both matrices and are different from those of earlier matrices.
2.6. Factor Graph Alternating between Global and Incremental Optimization
Factor graph optimization is superior to Kalman filtering in global optimization, but in the online optimization process, when only incremental optimization is used, performance is not considered superior, and in extreme cases, it is not even superior to Kalman filtering due to the number of parameters to be estimated. However, if global optimization is performed at all times, the computation time at a later stage is too long to meet the requirements of “timely” and “accurate” in online positioning.
The incremental calculation process will also change the reference point value to some extent during each incremental calculation due to the presence of random measurement errors. Global optimization results in better coordinate errors for both the reference point coordinates and the target than the results of incremental calculations. Therefore, in order to make full use of the reference point, it is possible to consider the position of the reference point as a fixed value after obtaining a reference point with a small error, and to use this to constrain the position of the UAV in subsequent calculations and thus the resulting moving target trajectory. If the position is taken as a fixed value in the above method, then the selection of the appropriate result is also an important part of the task process.
As less data is obtained in the early stages of online optimization, the time taken to perform global optimization is correspondingly smaller. Therefore, during the online calculation process, an appropriate time should be chosen for online optimization in the early stages of the positioning process.
2.7. Calculation of the Coordinates of the Reference Point
The reference point is calculated by extracting the position of the reference point in the pixel coordinate system and converting its coordinates under the geodesic coordinate system by means of the coordinate conversion relationship between the pixel coordinate system and the geodesic coordinate system. Let the state vector of the camera carried by the UAV at the moment be , where is the coordinates of the camera under the geodesic coordinate system, is its attitude, the camera focal length is , the optical center coordinates , and represent the physical dimensions of each pixel on the horizontal axis and the vertical axis , and the coordinates of the reference point in the pixel coordinate system are . This means that the camera’s elements of interior and exterior orientation are known, and the transformation matrix is as follows: where where can be calculated from the coordinated values of primary target .
2.8. An Online Target Localization Method Combining Global Optimization and Incremental Optimization of Factor Graphs
In the pixel extraction process, by means of subpixel rendering, the portion of the pixel that is less than 1 can be rendered. When the percentage value is kept at 3 decimal places, the error range does not exceed 0.1 pixels. For the first time, when the difference between the coordinate value of the reference point obtained online and the initial value of the coordinates of the reference point obtained for the first time exceeds the ground distance d corresponding to 0.1 pixels, then all the information already obtained is optimized globally at once.
And the final single-point positioning error for factor graph optimization can be calculated from a prior Monte Carlo simulation.
In the next calculations, the results of each calculation are also compared with the results of the first global optimization, and when the difference between their distances exceeds , a global optimization is performed on all known information about the target. If the difference between the results of the two global optimizations is less than the final single point location error , the reference point coordinates are considered to have been correctly resolved. And the reference point coordinates can be considered a known quantity in subsequent calculations, and no further iterations are performed to solve for them.
Let global optimization be performed at the initial moment, so that the result of the calculation of the reference point at the initial moment is denoted . Thereafter, if the th global computation is performed at some moment , the result of the calculation of the reference point at this moment is denoted as . The result of the incremental computation at each moment is denoted as . And is the number of times the global computation is performed. At any moment , the problem can be denoted if global location is performed, if incremental optimization is performed, and if incremental optimization after fixed feature points is performed, then we have
Introducing the coefficient matrix , the calculation described in this paper can be expressed as where the coefficient matrix satisfies
The online localization process is computed directly after each new observation is obtained, while the time complexity of the global factor graph algorithm is related to the number of factors A, the number of possible values of variables B, and the number of factor variables C. The algorithm designed in this paper uses the global algorithm of the factor graph algorithm with respect to the calculation of reference points, and therefore has a time complexity of O(ABC).
3. Simulations
3.1. Moving Target Location Trajectory Diagram
In order to test the performance of the factor graph algorithm, simulations of the factor graph algorithm and Kalman filter algorithm were carried out, and the results were analyzed, plotting the final error results against each other. First, the trajectory of the moving target location was represented in Figure 6.
In Figure 6, the set of circular points at the top indicate the UAV’s flight path, which is 100 m above the ground and moves in an S-shape in the air. The asterisk points indicate the movement path of the target, which performs a uniform linear motion of 5 m/s and uses the points (50, 0, and 0) as reference points. Let the measured values of azimuth and pitch angle between the target and the UAV obey a Gaussian distribution with a mean of 0 and a variance of 0.01°. Let the distance measurements between the target and the UAV obey a Gaussian distribution with a mean of 0 and a variance of 0.01 m. Finally, we let
3.2. Comparison of Kalman Filtering Results and Global Optimization of Factor Graph
The global optimization results of the Kalman filter are compared with the results of the factor graph algorithm as Figure 7.
In Figure 7, the dotted line shows the moving target location result of Kalman filtering, and the solid line shows the moving target location result of factor graph optimization. It can be seen that the average location error of the Kalman filter is around 8 m, and the average location error of the global optimization of the factor graph without the reference point constraint is around 3 m or less, which is more than 50% less than the computational error of the Kalman filter. It can be seen that the factor graph algorithm can obtain more accurate target locations than the Kalman filter algorithm, and the accuracy of the factor graph algorithm results is difficult to achieve with the Kalman filter algorithm. Therefore, the next main goal is to improve the factor graph algorithm in order to obtain more accurate localization results.
Simulation results after adding reference points while optimizing are shown in Figure 8.
In Figure 8, the dotted line shows the moving target location results without the addition of reference points, which are the same as in Figure 8, and the solid line shows the moving target location results after the addition of reference points. The global optimized location error without the reference point constraint remains unchanged, and it can be seen that the resultant error of factor graph location with the addition of the reference point constraint is reduced compared to the error before the addition of the reference point, with its location error reduced by more than 20%. It can be seen that the addition of reference points greatly improves the accuracy of the results of factor graph localization. However, the global localization algorithm is not suitable for online localization and is prone to delays when the task moves to a later stage when the computational volume is high.
3.3. Comparison of Incremental Optimization Results for Factor Graph
A comparison of the results of incremental and global calculations after the addition of reference points is shown Figure 9.
In Figure 9, the dotted line indicates the error of the global optimization result with the participation of the reference point, and the solid line indicates the error of the incremental optimization result with the participation of the reference point. The simulation shows that factor graph optimization has superiority over Kalman filtering in global optimization. But in the online optimization process, when only incremental optimization is used, performance is not considered superior. And in extreme cases, it does not even compare better than Kalman filtering results due to the large number of parameters to be estimated. Thus, some improvement of the online optimization algorithm is needed. Direct use of factor graphs for on-line localization does not improve the accuracy of target localization results. Therefore, in order to make full use of the performance of factor graph localization, the on-line factor graph localization algorithm needs to be improved.
Due to various errors in the measurement process, the coordinates of the reference point during online positioning also oscillate during the calculation process, which will be compared with the positioning error of the reference point for global positioning. The results of positioning the reference point during online positioning are shown in Figure 10.
In Figure 10, the dotted line shows the calculation error of the reference point coordinates during incremental positioning, while the solid line shows the positioning error of the reference point obtained after global positioning. It can be seen that the error at the reference point during incremental positioning is large and can reach more than twice the result of the global positioning error. Therefore, a fixed reference point position can be considered to constrain the UAV’s path during online positioning, resulting in more accurate target positioning results.
3.4. A Combination of Incremental and Global Positioning Methods
A comparison of the incremental optimization error with reference points and the final optimization algorithm is shown in Figure 11.
In Figure 11, the resulting error of the incremental optimization algorithm with the reference point constraint is shown on the dotted line, and the resulting error calculated by the combined incremental optimization and global optimization method is shown in a solid line. Compared with the two, the combination of incremental and global optimization can improve location accuracy by more than 17%. Therefore, the method proposed in this paper is effective.
Error between global optimization results with reference points and final optimization algorithm is shown in Figure 12.
In Figure 12, the error of the global optimization algorithm results with reference point constraints is shown on the dotted line, and the error of the results calculated by the combined incremental optimization and global optimization location method is shown in red. It can be seen that the error of the combined incremental optimization and global optimization location method has been reduced considerably but is still large compared to the error of the global calculation. In comparison between the two, the error result of the final positioning method is 30% larger than that of the global optimization of reference points.
4. Conclusions
In this paper, we propose an algorithm for UAV target location based on a combination of global and incremental optimization of the reference point factor graph. Through simulation analysis, the algorithm can effectively reduce the localization error of online target location while ensuring that the calculation time meets the “on-time” requirement of online localization, which can significantly reduce the result error of online localization, 17% less than the online localization method using only incremental location, and ensure the accuracy of the result when moving target localization.
The target localization method proposed in this paper can effectively reduce the accuracy of moving target localization. However, it is equally evident that even after some improvements, there is still a gap between the target localization accuracy of the online localization algorithm and that of the global localization algorithm.
Therefore, there is still a need to improve the factor graph-based target localization algorithm, and the future work plan for this paper is (1)Develop a method to further reduce the location error of moving targets to reduce the gap between online and global location error of moving targets(2)To obtain the final single-point location error A for factor graph optimization by numerical calculation, replacing the data obtained by Monte Carlo simulation
Data Availability
Data are available on request.
Conflicts of Interest
The authors declare that there is no conflict of interest regarding the publication of this paper.
Acknowledgments
This research was funded by the National Natural Science Foundation of China (NSFC), grant number (61603297) and Natural Science Foundation of Shaanxi Province, grant number (2020JQ-219).