Mobile target localization and tracking techniques in harsh environment utilizing adaptive multi-modal data fusion

Multi-source cooperative positioning systems relying on federated ﬁltering have become attractive development directions of navigation strategy in real-time localization and tracking under challenging urban environment. However, local Kalman ﬁlters of traditional federated ﬁltering may result in divergence when the system model or the measurements is inaccurate, and the ﬁxed information distribution coefﬁcient in federated ﬁlter cannot adaptively reﬂect the performance of each local ﬁlter. To improve the precision and robustness of the integrated navigation system, a novel adaptive federated strong tracking Kalman ﬁlter with dynamic fading factor mechanism for multi-sensor information fusion is proposed. Through iterative computation of the fading factor and updating the adaptive weight coefﬁcients, strong tracking ﬁlter becomes robust to the uncertainty of system model. Meanwhile, an effective adaptive information distribution estimation algorithm based on the predicted residuals is constructed to balance the contributions of the kinematic model information and measurements on the state estimates. To ensure the stability of ﬁltering, a simpliﬁed fusion strategy is established to solve the singular problem of the global covariance matrix of estimation error. Theoretical analysis and simulation results demonstrate the validity of the proposed approach in improving the accuracy and robustness of the integrated navigation and positioning systems. The proposed integrated multi-modal cooperative navigation and positioning algorithm will be of great signiﬁcance to the implementation of real-time mobile target localization and tracking in harsh environment or dead zone.


INTRODUCTION
With the development of new generation of information technology, the demands for high performance navigation and guidance systems are increasing. Accurate and reliable navigation services are now occupying important position in ubiquitous wireless communications and intelligent transportation in recent years [1][2][3][4][5][6][7]. However, the single navigation system can no longer provide accurate information for universal guidance due to the technique imperfections in non-line-of-sight (NLOS) case. For instance, global navigation satellite system (GNSS), which can provide absolute positioning information covering any part of the world during day and night, is widely This is an open access article under the terms of the Creative Commons Attribution License, which permits use, distribution and reproduction in any medium, provided the original work is properly cited. © 2021 The Authors. IET Communications published by John Wiley & Sons Ltd on behalf of The Institution of Engineering and Technology used in both military and civil fields. However, if the vehicle is moving in complex environments, such as city canyon, underground garage, tunnels or indoor environments, the low power signal received from GNSS satellites, coupled with multipath and the geometry of the satellite positions, may cause errors in locating a device [8][9][10][11][12]. The frequently-used inertial navigation system (INS) has the advantage of giving continuous navigation information independent of the external environment. However, INS is insensitive to electromagnetic interferences and its errors will accumulate over time due to the uncompensated errors of gyros and accelerometers [13][14][15]. Doppler Velocity Log (DVL), which is designed based on the Doppler Effect, is an ideal sensor to provide velocity measurements in high precision [16][17][18]. However, due to acoustic principle and installation factors, there will be interference noise and deviation in speed measurement. Barometers, which compute current height through the relationship between the height and air data such as pressure and temperature, can provide the height measurements continuously, and have been commonly used [19,20]. However, as time elapses and as the air-vehicle moves to another place, the characteristics of the atmosphere around the vehicle are changed to produce large barometer errors. Therefore, the research of navigation technology based on multi-sensor fusion not only helps to remedy the shortcomings of individual navigation technique, but also has great significance for making full use of multi-sensor information carried by moving vehicles. As a result, many researchers have studied the multi-sensor integrated positioning technique [21][22][23][24][25][26]. For instance, the widelyused integrated GNSS/INS that combines the complementary features of the two sensors to provide relative positioning accuracy from decimetre-to-centimetre-level has become a standard approach in modern navigation area. The integrated GNSS/INS system has many advantages including decreased inertial position and velocity errors, the easy calibration of gyros and accelerometers, and has been widely used in land vehicles, military and civilian aircrafts, ships and submarines etc.
To obtain improved performance and accuracy, researchers have paid more attention to the integrated navigation system with additional sensors for relative navigation. In multi-sensor information fusion system, the decentralized filtering technique has attracted more and more attention from researchers, and the decentralized federated Kalman filter (FKF) created by Carlson [27], which is known for its small memory requirement, high computational efficiency, and easy fault detection and isolation to individual sensor, has been greatly applied in the multi-sensor fusion system. The information distribution which directly affects filter performance, such as the accuracy and fault tolerance, is the key part of the federal Kalman filter design. However, the information distribution principle in the traditional FKF was based on a fixed ratio, which means all the sub-filters have the same weights in the process of information distribution. Meanwhile, the effectiveness of the observed information has a direct impact on the estimation accuracy of each local filter, which is easily influenced by the external environment and the limitations of the sensors as well. In this case, the inaccurate estimation results will influence the global estimation of the main filter, which in turn affects the performance of the local filters, thereby degrading the performance of the overall system.
Consequently, considering that each sub-filter's performance and estimation quality change constantly in real-time high dynamic navigation environment, a variety of adaptive navigation algorithms have been proposed to improve the performance of the federal filter. In [28], the authors introduced fault diagnosis and isolation technology for the federated filter system to maintain the convergence of local filters, and the information distribution coefficient is dynamically adjusted according to the quality of the states and covariances of local filters. Kim et al. [29] proposed a two-stage federated Kalman filter (TS-FKF) for an high-speed trains (HSTs) navigation system, and presented a slip and slide detection algorithm and an adaptive information-sharing algorithm to deal with a large tachometer error and performance difference between sensors to provide reliable navigation information for high-speed trains. In [30], Li et al. presented a precise point positioning PPP/INS tightly coupled navigation system based on adaptive federated filter to improve the measurement accuracy of the target, where the formal equivalence of the federated local filter and the adaptive filter was proved, and an information distribution coefficient was constructed based on the adaptive filter factor. Wang et al. [31] proposed an information sharing factor federated filter (AISFF) for the multi-sensor integrated navigation system, where the information distribution coefficient was adaptively adjusted according to the mathematic relationship between the theoretical innovation covariance and the estimated innovation covariance. In [32], the authors introduced the least square principle and adaptively adjusted information sharing factors to obtain the optimal estimation in the hybrid GPS/INS/DVL positioning system. In [33], Xiong et al. presented a vector-form information sharing algorithm with the eigenvalue of error covariance matrix and the singular value of observability matrix to improve the precision of the federated filter. However, the increasing dimension of observability matrix may result in heavy computing demands, and even suffer from the curse of dimensionality or divergence or both.
It is worth noting that effective implementation of the estimating algorithm also plays a crucial role in integrated navigation. Kalman filter (KF) is the most commonly used estimation technique for integrating signals output from sensors to generate the optimal solution, which can only be used in linear system originally. The extended Kalman filter (EKF) [34], which simply linearizes all non-linear models to the first-order, is widely used to tackle the non-linear state estimation problem. However, in many practical engineering systems, there exists uncertainty of modelling dynamic system and the instability of the components [35][36][37], which may result in performance degradation or filtering divergence when using the conventional KF and EKF. For this reason, strong tracking filter (STF) was proposed by Zhou et al. [38], which introduces a fading factor to increase prediction error covariance and adjusts the corresponding gain matrix online. STF is mainly used in the area of control and decision to obtain good performance because of its strong robustness against model uncertainties and good real-time state tracking ability even when a state jump occurs. This paper proposes a novel adaptive federated strong tracking Kalman filtering (AFSTKF) approach combining multi-sensor modules, which can still provide precise localization and tracking information even when the vehicle runs in harsh environment or moves in complex motion senses, to improve the precision and stability of the integrated navigation and positioning system. In local filter, the STF algorithm is developed to eliminate the influence of model inaccuracy and enhance the robustness of the system by utilizing the dynamic fading factor. In additional, an adaptive information distribution estimation method is proposed according to the statistics of the predicted residual vectors. The proposed adaptive factor has the advantages in balancing the contribution of each sub-filter dynamically and remaining the stability of the overall system. Meanwhile, we establish a novel multi-source information fusion method to solve the singular covariance problem in the fusion process, which helps to reduce the computational complexity as well as to ensure the stability of the hybrid cooperative navigation and positioning system.
The main contributions of this paper can be summarized as follows: (1) We propose a novel INS/GNSS/DVL/Barometer integrated navigation and positioning approach based on adaptive federated strong tracking Kalman filter, which is capable of providing accurate navigation and positioning information and maintaining system stability even when the target is moving under poor conditions; (2) We propose a dynamic information distribution estimation technique based on the statistical characteristic of residual vectors, which can adaptively balance the contributions of local filters; (3) we propose a novel adaptive information fusion algorithm in consider of the singular problem of the covariance matrix of the global estimation P g , which helps to reduce computational effort as well; (4) The proposed algorithm and architecture are assessed by simulations conducted through a self-developed platform which is capable of providing whole-procedure simulation from trajectory generation to performance evaluation. Simulation results demonstrate that the proposed INS/GNSS/DVL/Barometer system based on the AFSTKF algorithm can improve the navigation and positioning accuracy and stability significantly.
The remainder of the paper is organized as follows. In Section 2, we present the basic structure of conventional FKF. The system state model and measurement equations are presented in Sections 3 and 4, respectively. Section 5 introduces the multisensor integrated navigation method with adaptive federated strong tracking Kalman filter algorithm. Simulation results are presented and the system performance is discussed in section 6. Finally, Section 7 summarizes the whole paper.

STRUCTURE OF THE FEDERATED FILTER
Commonly, the federated filter consists of several sensordedicated local filters and a master filter, which is a partitioned and two-stage data processing technique, and has advantages of simplicity and fault-tolerant capability over other decentralized filter techniques. Figure 1 shows the general structure of a federated filter. For ease of reading, we listed some of the variables used and gave the corresponding descriptions in Table 1.
In practice, the local filter outputs that are derived from different navigation sensors based on the same dynamic model prediction are usually independent. Then the main filter fuses the outputs of the local filters to get the optimal fusion result, and then distributes the information among several component filters according to the information distribution principle. As depicted in Figure 1,X g is the global state estimation of the master filter, P g denotes the global covariance matrix of estimation errors,X i is the state variables of each filtering subsystem, P i represents the corresponding error covariance matrix, and N stands for the number of local filters. We consider N = 3. The  Adaptive information distribution factor of ith local filter whole process mainly includes two parts, information distribution and information fusion.
1) Information fusion: The main task is to fuse the outputs of all local filters to get the global optimal estimation by the following algorithm, 2) Information distribution: The main task is to distribute system information among the sub-filters. The amount of state estimation information P −1 g and the process noise information Q −1 g are allocated to all the local filters, as follows, where i (i = 1, 2, … , N ) is the information distribution coefficient belonging to the ith subsystem at time-step k.
The coefficients satisfy the principle of information conser-

SYSTEM STATE MODEL
We consider INS as the basic navigation system. The error states of INS, gyroscope and accelerometer are selected as the state vector of the integrated navigation system. The system state equation is described as follows [39]: where F (t ) denotes state transition matrix, W (t ) denotes the state noise matrix assumed to be white Gaussian noise with zero-mean and the covariance matrix Q(t ). The system state vector is where E , N , U are the attitude errors of platform; v E , v N , v U are velocity errors; L, , h are latitude, longitude and height errors, respectively; rx , ry , rz are gyro drifts, and ∇ E , ∇ N , ∇ U represent accelerometer biases. The subscripts E, N and U denotes the east, north and upward components of the corresponding errors in the navigation frame, respectively. Generally, the system state equation is rewritten in a discrete difference form for the sake of practical implementation: where F i,k|k−1 is the system transition matrix from (k − 1) timestep to k. And the subscript i denotes the ith local filter. The system noise W i,k is the zero-mean white Gaussian noise with the covariance Q i,k . All local filters share the system state equation.

SYSTEM MEASUREMENT EQUATIONS
In the multi-sensors navigation system, INS is taken as the reference system, together with GNSS, DVL, barometer, there will be three integrated system measurement equations. They are described as follows respectively. 1) INS/GNSS measurement equation (local filter 1): The difference between position and velocity output by INS and that by the GNSS are taken as observations of measurements. The measurement equation of local filter 1 can be expressed as follows where V 1,k is the measurement noise of local filter 1 which corresponds to the measurement error of the GNSS, including velocity errors v G 1 , v G 2 , v G 3 and position errors v G 4 , v G 5 , v G 6 . They are independent zero-mean Gaussian white-noise processes with the corresponding covariance R 1 . The measurement matrix H 1 is where V 2,k is the measurement noise of local filter 2 which corresponds to the measurement error of DVL, including the velocity errors v d 1 , v d 2 , v d 3 . They are independent zero-mean Gaussian white-noise processes with the corresponding covariance R 2,k . The measurement matrix H 2 is

3) INS/barometer measurement equation (local filter 3):
Barometer is sensitive to height, and can provide accurate height information in the localization process. As a result, the difference between the altitude measured by the INS and that given by the barometer, is taken as the observation information of the INS/Barometer integrated subsystem. Thus the corresponding system measurement equation is where V 3,k is zero-mean Gaussian measurement noise caused by the low-cost barometer with the covariance matrix R 3,k . And the measurement matrix is

THE MULTI-SENSOR INTEGRATED NAVIGATION METHOD USING ADAPTIVE ROBUST FEDERATED STRONG TRACKING KALMAN FILTER ALGORITHM
For the convenience of discussion, without loss of generality, INS is considered as the common reference system, integrating with other sensors, including GNSS, DVL, and barometer. We establish three local filters based on STF, and calculate the adaptive information distribution coefficients based on the statistics of the predicted residuals. Figure 2 shows the proposed INS/GNSS/DVL/Barometer integrated navigation scheme based on AFSTKF algorithm. Figure 3 shows the flow chart of the proposed AFSTKF algorithm.

5.1
The strong tracking filter The basic idea of strong tracking filter is that we let the error caused by the uncertainty of system model or noise parameter be equivalent to the estimate error of filtering, by utilizing a suboptimal fading factor k to modify the covariance matrix of one step prediction of state error P k,k−1 , to increase the gain matrix and eventually increase the weights of the newly measured data [40][41][42]. According to the orthogonality principle of Wiener filter, the predicted residual has the following statistical property [43] E where Equation (13) describes the orthogonality principle (OP) and indicates that the output residuals at different time remain orthogonal all the way. In other words, all useful information has been extracted. In practical application, the uncertainty of the system model will cause the output residuals of the filter non-orthogonal. STF utilizes a suboptimal fading factor k to force the output residual sequences to remain orthogonal, and to extract as much as possible valid information from the output residual sequences. Hence, the STF method can still provide reliable real-time tracking ability even when the system model is uncertain.
If the system model is accurate and the measurement z k at epoch k is reliable, the statistical distribution of the residual satisfies the normal distribution [44], An alternative expression to Equation (15) is On the left side of the equation, V ok represents the estimated covariance of the predicted residual vector; the right side is its theoretical result. The equation is valid in theory. However, V ok is not always accurate since the residual of the filter is easily affected by the uncertainty of the system model or unaccounted errors. Thus we introduce the fading factor to resist the uncertainty of the system. Here we derive the fading factor k in the following.
After introducing the fading factor k , the covariance matrix of one step prediction of state error is modified as To ensure Equation (16) holds, we substitute Equation (17) into Equation (16) and obtain Then we define It is easy to derive the fading factor following [45] In order to estimate k appropriately, weight coefficients are considered to balance the old data and the new observation. We have increased the weight of the latest measurement data. Accordingly, V ok can be given by where is an empirical constant, which is usually equal to 0.95 − 0.995. We set = 0.95. The fading factor k is of great importance in the STF method. An appropriate value will increase the accuracy of the system and measurement models by minimizing the impact of obsolete data. Rapid fading occurs when data does not quite fit for the model, and slow fading for a good fit.

Simplified fusion method
All local filters work in parallel, and their local state estimations enter the main filter, resulting in a global state estimation. Suppose that the state estimations of local filter 1, 2 and 3 areX 1,k , X 2,k andX 3,k , respectively. The target function on the basis of local estimation result is whereX k is the estimation result of the main filter. By taking the partial derivatives of Ω with respect tô X k , we can get the constrained relationship ofX k as follows By transposing and simplifying the upper formula, we get the global state estimation, X k = P g,k (P −1 1,kX 1,k + P −1 2,kX 2,k + P −1 3,kX 3,k ) (26) and information matrix where P g,k is the covariance matrix of global state estimation. Thus the globally optimal fusion state estimation in the sense of linear minimum variance is obtained through our above discussion. And the results are equivalent to Equation (1). Observing the structures of Equations (26) and (27), we can find out that the inverse matrix is needed in the calculation process, which may result in heavy computational burden, especially when n (n equals the dimension of the state vector) is large. In addition, the federated filter cannot obtain the algorithm's results when P g,k is a singular covariance [23]. Therefore, we consider a new fusion algorithm to solve the problem. Select the diagonal elements of P i,k as the diagonal elements of where p i j j represents the estimation accuracy of the j th state variable of the ith subfilter.
Due to the simplification of the covariance matrix of each subfilter, the global estimation covariance P g,k obtained from upper formula is a diagonal matrix, which is simple for calculation. Similarly, we can get the global state estimation based on Equation (1), as follows: From the observation we can see that the diagonal matrix is also obtained here. Thus we only consider the calculation of elements on the diagonal line. (2) Calculating the global stateX g,k according to Equation (1).
Denote asX ⋮ X n = p 1 nn p 2 nn X 3 n + p 2 nn p 3 nn X 1 n + p 1 nn p 3 nn X 2 n p 1 nn p 2 nn + p 1 nn p 3 nn + p 2 nn p 3 nn .
The results obtained from this simplified fusion method are sub-optimal fusion estimation results. The computational load involved in this method based on diagonal matrices is significantly reduced, and the stability of the system can be ensured, even though its accuracy is slightly lower due to the disregard of the coupled relationship between the components of the local state estimations.

Improved federated filter based on adaptive information distribution coefficient
As mentioned above, the information distribution coefficient i is an important parameter to be determined for the reliable performance of the FKF, since the error covariance of each local filter is weighted by 1∕ i (i = 1, 2, … , N in Figure 1 ). In the traditional FKF, the fixed information distribution coefficients cannot reflect the states of the sensors during filtering. For example, if one of the sensors breaks down, the performance of the corresponding local filter will deteriorate, even resulting in the deterioration of the overall system. Thus, we pursue an adaptive information distribution coefficient to adjust the performance of each local filter adaptively.
Since the predicted residual vector e k reflects the error magnitude of the kinematic model information in most cases, we consider to calculate the adaptive factor in the perspective of the predicted residual. According to the statistical distribution of the residual in Equation (15), we construct a new kind of statistic as Consequently, the adaptive information distribution factor is valued using the following formula, where c is a constant with experienced values as c = 0.85 ∼ 1.
Since the predicted residuals reflect the kinematic model disturbances at the present epoch, k can be used to determine the state of the current subfilter. When | k | < c, it means the system state model and observation model are both accurate, and the local filter has a good performance. Thus we set ′ k = 1 to demonstrate that the current subfilter is stable and accurate. When | k | > c, it indicates that the filter suffers from measurement outliers or disturbances of the dynamical model or both. Hence, we set ′ k = c | k | to reflect the error magnitude of the current filter.
Meanwhile, in order to ensure the conservation of information, the adaptive factor is normalized where i,k represents the adaptive factor of the ith local filter at time-step k after normalization. In this case, the value changes between 0 and 1, which balances the contribution of each filtering subsystem.

SIMULATION RESULTS AND PERFORMANCE ANALYSIS
In this section, the performance of the proposed integrated navigation system is evaluated with the proposed AFSTKF algorithm. The adaptive federated Kalman filter (AFKF) algorithm proposed in [30] and the conventional FKF algorithm are also tested for comparison.

Simulation description
In order to better illustrate the effectiveness of the proposed approach, we designed one trajectory to simulate the where initial (1 : 3) represent the initial attitudes, initial (4 : 6) represent the initial velocities, and initial (7 : 9) represent the initial positions (36.36 • in north, 120.69 • in east, and 0 m in height).
In Figure 4, we simulate a flight trajectory of the aircraft to simulate complex curve motion. The trajectory consists of different kinds of conditions, including moving forward with constant speed, climbing, level flight, left turning, diving, varying speed and so on. The whole flight lasts for 28 min. We increase the complexity of the motion model by combining different flight conditions.
As discussed in part II, we set the INS error as state variables whose initial value is where X 1,0 , X 2,0 and X 3,0 represent the initial values of STF 1 , STF 2 and STF 3 , respectively. Correspondingly, we set the covariance matrix of estimation as follows where P 1,0 , P 2,0 and P 3,0 represent estimation covariance matrices of STF 1 , STF 2 and STF 3 , respectively.
R 1,k is the covariance matrix of system measurement noise in position and velocity with respect to local filter 1, R 2,k is the covariance matrix of system velocity measurement noise of local filter 2, and R 3,k is the covariance matrix of system measurement noise in height about local filter 3. All of them are white Gaussian noise and the initial values are set as

Simulation results analysis
The performance of the proposed INS/GNSS/DVL/ Barometer integrated navigation and positioning mechanism is analyzed in the following part. To prove the effectiveness of the proposed algorithm, we make a comparison among the conventional FKF, the AFKF in [30] and the proposed AFSTKF method in terms of trace and errors. In our simulation, we set sampling interval T s = 0.1. The simulation results in Figures 5 and 6 indicate the effectiveness of the proposed integrated scheme. As shown in Figure 5, the proposed AFSTKF has the strongest tracking ability than the other methods when there is no big measurement outliers during the simulation. At the beginning, the three meth- ods play almost the same role in state estimation. As time going on, the three algorithms show different performance in tracking. Especially, the estimated trajectory of FKF quickly deviates from the real trace which illustrates that the conventional FKF cannot be applied to complex environment. In addition, when there exists big measurement outliers during the simulation, the proposed method still has strong tracking ability in Figure 6. Since the flight trajectory has more motion conditions, thus there are more uncertainties with the system model and measurement noises during the filtering, which may degrade the performance of Kalman filter. However, the strong tracking filter is robust to model uncertainty and measurement biases, thus the proposed AFSTKF can provide reliable realtime tracking ability. Considering the fact that the adaptive information distribution coefficient also has an important influence on the system structure. As shown in the picture, the simulation result demonstrates the outstanding tracking ability of the proposed approach.
The position error and velocity error are shown from Figures 7 to 12, respectively. As depicted in Figures 7-9, the  Comparison of upward velocity error position estimation errors obtained by FKF vary greatly during the filtering, and the results obtained by AFSTKF remain convergence all the way. According to Figure 10 and Figure 12, comparing with the proposed method, the velocity estimation errors of the KF-based methods exist big jitters, but the proposed one remains relatively stable. By analyzing the results, we find that these mutations often occur around the corners where the abrupt changes of motion will lead to model uncertainty and measurement inaccuracy. However, the proposed algorithm has very limited improvement in adjusting the northward velocity. Figure 13 shows the cumulative distribution function of localization errors. It is easy to find that the proposed AFSTKF shows high position accuracy during the filtering.
To further demonstrate the performance of the adaptive algorithm, Table 2 shows the MSE values for the integrated navigation system among FKF, AFKF and the proposed AFSTKF method. Due to the fact that the barometer is more sensitive to height, and GNSS is able to measure position information.  As shown in the table, the results of AFSTKF is much smaller than those of AFKF and FKF. Consequently, the proposed integrated scheme possesses a better navigation performance than FKF and AFKF.

CONCLUSION
We propose a novel adaptive federated strong tracking Kalman filtering (AFSTKF) approach using INS/GNSS/DVL/Barometer modules, which can still provide precise localization and tracking information even when the vehicle moves in complex motion senses. In local filter, the STF algorithm is employed to eliminate the influence of model inaccuracy and enhance the robustness of the system by introducing the fading factor. In addition, an adaptive information distribution method is proposed according to the statistics of the predicted residual vectors. The proposed adaptive factors have the advantages in balancing the contribution of each sub-filter dynamically and remaining the stability of the overall system. Meanwhile, we present a novel multi-source information fusion method to solve the singular covariance problem in the fusion process, which helps to reduce the computational complexity as well as to ensure the stability of the hybrid cooperative navigation and positioning system. The simulation results demonstrate that the proposed algorithm has significant tracking ability and estimation accuracy in complex motion senses compared with FKF and AFKF in the INS/GNSS/DVL/Barometer hybrid navigation system. In addition, the computational efficiency of the system has been greatly improved with respect to the simplification of the covariance matrix.