Sequential Fusion Filter for State Estimation of Nonlinear Multi-Sensor Systems with Cross-Correlated Noise and Packet Dropout Compensation

This paper is concerned with the problem of state estimation for nonlinear multi-sensor systems with cross-correlated noise and packet loss compensation. In this case, the cross-correlated noise is modeled by the synchronous correlation of the observation noise of each sensor, and the observation noise of each sensor is correlated with the process noise at the previous moment. Meanwhile, in the process of state estimation, since the measurement data may be transmitted in an unreliable network, data packet dropout will inevitably occur, leading to a reduction in estimation accuracy. To address this undesirable situation, this paper proposes a state estimation method for nonlinear multi-sensor systems with cross-correlated noise and packet dropout compensation based on a sequential fusion framework. Firstly, a prediction compensation mechanism and a strategy based on observation noise estimation are used to update the measurement data while avoiding the noise decorrelation step. Secondly, a design step for a sequential fusion state estimation filter is derived based on an innovation analysis method. Then, a numerical implementation of the sequential fusion state estimator is given based on the third-degree spherical-radial cubature rule. Finally, the univariate nonstationary growth model (UNGM) is combined with simulation to verify the effectiveness and feasibility of the proposed algorithm.


Introduction
In recent years, the application of multi-sensor systems has gained widespread attention in the fields of signal and processing, robot localization and navigation, multi-target tracking, and navigation and guidance systems [1][2][3]. Due to the problem of network latency or limited data communication capability of sensor systems, the packet dropout and noise coupling of measurement data are inevitable [4]. This situation seriously affects the effectiveness and real-time state estimation, which leads to unreliable system networks. The noise in the system is correlated and cross-correlated, resulting from various practical scenarios. This transforms the system from an ideal, noise-independent, uncorrelated state to a complex, noise-coupled situation. The long-time noise superposition effect and dynamic influence of observed states in a common noisy environment contribute to this transformation. Therefore, the problem of state estimation with correlated noise has been a hot topic in recent years.
Kalman filtering [5] is a classical fusion algorithm for control systems, based on the assumption that process noise and observation noise are independent and uncorrelated, and it is only applicable to linear systems. In fact, common physical systems often exhibit and noise correlation [25]. Therefore, the sequential fusion method is also applicable to the state estimation of multi-sensor nonlinear systems to solve the problem of simultaneous correlation of observation noise, where the observation noise of each sensor is correlated with the process noise of the previous moment. Therefore, in this paper, a sequential fusion approach is used for multi-sensor information fusion.
In networked systems, data packet dropout due to network congestion or sensor degradation occurs from time to time, which can easily lead to an increase in the uncertainty of system state estimation. Therefore, compensating for the lost data when the sensor measurements are lost at the current moment is also a hot issue. Currently, researchers have proposed the zero-input strategy (zero value is used for compensation), the hold-input strategy (the latest measurement received previously is used for compensation), and the measurement prediction strategy (when the measurement packet is lost at the current moment, the predicted value of the current sensor measurement is used for compensation) [26]. Zhang et al. [27] proposed a compensation strategy based on the random packet dropout problem of network control systems (NCSs) using the measurement prediction strategy. In [28], a new model with two Bernoulli random variables is used to describe the case of multiple packet dropout and one-step random delay, and the optimal linear state estimation is given in the linear least variance sense. Zhao et al. [29] use the same approach to compensate for the lost measurement data, and based on the proposed centralized and distributed fusion estimator, the optimal filter with compensating factor and the associated noise posterior covariance, new square root of error covariance factors are derived. In addition, it has been demonstrated in Ma et al. (2017) [30] that the measurement prediction strategy has higher accuracy compared to the compensation mechanisms such as the hold-input strategy.
The proposed algorithm is based on a sequential fusion framework, which ensures efficient data processing and state estimation accuracy, and is similar to the ETCKF-SF proposed by Cheng et al. [24], where the noise of each sensor is synchronously correlated [31], but the difference is that this paper adds measurement compensation and also considers the estimated amount of measurement noise to enhance the fault tolerance and robustness of the system. Based on the above analysis, there is no relevant literature on sequential fusion filters for nonlinear multi-sensor systems with cross-correlated noise and packet dropout compensation. The main contributions of this paper are as follows: (1) an observational noise estimator based on an innovation analysis method was designed to avoid noise de-correlation; (2) based on the noise estimator, a nonlinear multi-sensor sequential fusion filter with cross-correlated noise and packet dropout compensation is designed based on the measurement prediction compensation mechanism; (3) a numerical implementation of the filter fusion algorithm proposed in this paper is carried out based on third-degree spherical-radial cubature rule deterministic sampling method.
The paper is organized as follows. In Section 2, the problem of nonlinear systems with cross-correlated noise and measurement compensation is stated. Section 3 gives the special design of the sequential fusion filter with cross-correlated noise and Section 4 gives the numerical implementation procedure of the proposed algorithm based on the third-degree spherical-radial cubature rule. Section 5 shows the simulation experiments and results analysis. Section 6 gives a summary of the paper.
Notations: R n represents an n-dimensional Euclidean vector space; I n represents an n-dimensional unit matrix; the superscript −1 and T represents the inverse and transpose of matrix; k+1 . E[ ] denotes the mathematical expectation; ( ) is the same as the preceding neighboring term in parentheses; δ k,l denotes the Kronecker Delta function; N(µ, σ 2 ) is the normal distribution. ς i , ξ i , ζ i , τ i represent the ith cubature point.

Problem Statement
Consider the following discrete-time nonlinear multi-sensor system with cross-correlated noise and packet dropout compensation: where X k ∈ R n is the state vector, Z (i) k ∈ R p i , i = 1, 2, · · · , L is the observation data of the ith sensor, where p i is the dimension of the ith sensor measurement data; F( ) and H( ) are known nonlinear function, W k ∈ R m and V (i) k+1 ∈ R p i , i = 1, 2, · · · , L are correlated process noise and observation noise of the ith sensor; Γ k is known matrix; γ i k is an independent and identically distributed Bernoulli random variable, and it satisfies the probabilities We define Π(k + 1) as the data received by the estimator up to time k + 1.
Assumption 1. The process noise W k and observation noise V (i) k , i = 1, 2, · · · , L are correlated white noise satisfying the follow statistical properties: k, l denote the time step, and i, j represent the sensor serial number. Where j = 1, 2, · · · , L. Assumption 2. The initial value X 0 is uncorrelated with W k and V (i) k , i = 1, 2, · · · , L, and E[X 0 ] = µ 0 and E (X 0 − µ 0 )(X 0 − µ 0 ) T = P 0 . Assumption 3. State X k obeys Gaussian distribution under the condition of measurement data In sequential fusion, the global state estimate at the current moment becomes the initial state at the next moment after iteration so that the state expectation under the condition of measure Π(k − 1) obeys a Gaussian distribution of N(X k ;X 0 k|k−1 , P X 0 k|k−1 ). The superscript 0 indicates the initial or a priori state.

Design of Sequential Filter For Nonlinear Multi-Sensor with Correlated Noise and Dropout Packet Compensation
Since the dropout of measurement packets and correlated noise problems in multisensor systems are considered, we incorporate measurement compensation to ensure the completeness of communication data information and use a sequential fusion framework, combined with noise synchronization correlation, to estimate the state of the multi-sensor system.
The sequential filterX i k+1 and its filtering error covariance matrix P X i k+1|k+1 for the state are calculated as follows: where the expressions of innovation ε i (k + 1), innovation covariance matrix Q i ε (k + 1), and filter gain matrix M X i k+1 of the ith sensor at time k + 1 are where the covariance matrix between state estimation error and innovation is: The predicted estimate of the sensor observation is: Substituting the above Equations (12)- (16) into Equations (10) and (11) gives Q i ε (k + 1) and M X i k+1 . The estimated value of observation noise is: The calculation of gain matrix M V i|l k+1 l = 1, 2, · · · , i − 1, cross covariance matrix P X i−1 V i|i−1 k+1|k+1 between state estimation error and observation noise is as follows: where the covariance matrix between the state estimation error and the observation noise is: The cross-covariance matrix between the observation noise and its estimation error is Then, considering the recursive framework of the sequential fusion filter, the state estimation and estimation error covariance matrix at instant k + 1 are Then, the predictor of sequential fusion iŝ Then,X 0 k+1 and P X 0 k+1|k+1 are calculated as follows The detailed derivation process of the above formula can be found in Appendix A.
In the table below, the implementation steps of Algorithm 1 are summarized.
Algorithm 1 Iterative steps of the sequential fusion algorithm k for the kth time and sensor packet dropout rate p i k . Output: The posterior estimateX k|k of the state and the variance P X k|k of the state error. Initialization: Setting the initial stateX − 0 = µ 0 , the variance P − 0 = P 0 , the process noise variance Q k = σ 2 W , the observation noise variance R i (k) and the covariance S i,j (k) between them, and the sensor packet dropout rate (26) and (27).

Numerical Implementation Based on the Third-Degree Spherical-Radial Cubature Rule
Based on the third-degree spherical-radial cubature rule, the sequential fusion algorithm for nonlinear filtering with cross-correlated noise and measurement compensation in this section can be implemented recursively by the following numerical calculation. Assuming all the quantities before the k+1 time are known and the states of the previous local filters, i.e.,X l−1 k+1 , P X l−1 k+1|k+1 , and Q l−1 ε (k + 1), are known, the corresponding numerical implementation is detailed in the following flow.

Estimation Calculation of Observation Noise
We define α as follows: Cholesky factorization: Calculation of cubature points: Propagate the cubature points: Then, the noise gain has the following result: is a known volume, the cross-covariance P X l−1 V i|l−1 k+1|k+1 can be derived from the (33) and (34). In the recursive process, the cross-covariance P V i|i−1 k+1|k+1 between the observation noise and its estimation error can also be calculated by using the above results to obtain the Equation (24).

Estimation Calculation of System State
Cholesky factorization: Calculation of cubature points: , t = 1, 2, · · · , 2(n + p i ) Propagate the cubature points: Therefore, the innovation and the cross-covariance matrix of the state error and the measurement error are: The measurement error covariance is: as a result, M X i k+1 can be obtained using the above calculation, while Equations (7) and (8) can be calculated from Equations (44), (45), and (49) as the final result, where p i k+1 is the probability of dropout of measurement data.
The above steps are repeated until all sensor data are calculated at time k + 1. The state data obtained from the fusion center is used for one-step state prediction.

One-Step Prediction Estimate and One-Step Prediction Covariance Matrix of State
Cholesky factorization: Calculation of cubature points: Propagate the cubature points: Then, the value of the one-step state prediction is: The above is performed after all sensor data processing is completed, and the posterior estimate of the current moment is used as the a priori state data for the next moment.

Simulation
This paper conducts CKF numerical simulation experiments based on the filter algorithm proposed in Section 3 and uses the univariate nonstationary growth model (UNGM) to test the effectiveness of the proposed algorithm.We assume that the entire system includes two sensors and the state and measurement equations of the system can be described as follows: In the above equation, (56) and (57) are the measurement models for each of the two sensors, and the superscripts 1,2 are the ith sensor. W k is a Gaussian white noise obeying a mean of zero and a variance of Q k = σ 2 W . In addition, the process noise is related to the measurement noise of each sensor, satisfying the following relationship: The W k and Gaussian white noise η i (k) ∼ N(0, σ 2 η i ) are independent of each other. The variance and covariance of the observation noise for different sensors are In the experiments, the role of the measurement prediction compensation strategy needs to be taken into account, and the measurement prediction compensation model is defined as follows: where γ 1 k and γ 2 k are the independent and identically distributed Bernoulli random variable for the two sensors at the kth time step. We define the 70 as the estimated length of state X and a single time sampling step T = 1. The error of 100 Monte Carlo runs and the root mean square error (RMSE) can be defined as follows: k|k are the ground truth of the state and the estimated state of the kth epoch at the nth Monte Carlo run, respectively. N = 100 is the number of Monte Carlo runs.
Set σ 2 W = 5, σ 2 η 1 = 2.46, σ 2 η 2 = 8.75, β 1 = 0.8, β 2 = 0.5 and covariance R 1,2 = 0.19. The initial value of the state, as well as the initial variance, are µ 0 = 0.3, P 0 = 5. In order to investigate the effect of state estimation under packet dropout compensation, we set up multiple comparison experiments under different communication rates based on different communication rates, and also compare the local estimation results with the sequential estimation results.
The local estimation is the fusion estimate of half of the total number of sensors, and sequential estimation is the fusion estimate of all sensors.
As shown in Figure 1, the measurement prediction compensation mechanism can reduce measurement errors to a certain level and prevent observation scattering due to packet loss. Figure 2 shows the innovation variance of the multiple local filters. The result indicates that the measurements of the multi-sensor system can still maintain stable estimates when the measurement data is lost. In Figures 3-5, the state estimation, state error, and RMSE of the state are shown for the communication rate p 1 k = 0.4, p 2 k = 0.7 for the proposed algorithm and the EKF algorithm in the framework of this paper, respectively. The results show that the state estimation results of the proposed algorithm are more accurate and stable compared to the EKF. Figures 6 and 7 record the state estimation results for local estimation and sequential estimation. The figures show that the multi-sensor estimation for the sequential fusion is more accurate than the partial estimation, which implies that the proposed algorithm is more reliable when the system generates data packet dropout.        Table 1 shows the results of the partial and full fusion of the proposed algorithm and the RMSE of the EKF at different communication rates when the packet dropout is slight. The simulation results show that the estimation accuracy of the proposed algorithm is significantly better than that of the EKF under normal packet dropout, with a 58.95% improvement in relative accuracy. Again, it is demonstrated that all fusion has better accuracy compared to partial sensor fusion. Table 2 shows a comparison of the results of the root mean square error of the two algorithms when all sensors suffer from severe packet dropout (all sensors have a communication rate below 50%), resulting in a very low communication rate. We can analyze the state estimation of the EKF scatters in this case, especially when the sensor communication rate is p 1 k = 0.35, p 2 k = 0.45. The proposed algorithm is able to maintain a stable accuracy under such extreme conditions, demonstrating that the proposed algorithm can maintain strong robustness in multi-sensor nonlinear systems.

Conclusions
In this paper, a sequential fusion filtering algorithm is proposed for a nonlinear multisensor system with cross-correlated noise and packet dropout compensation, based on innovation analysis and the measurement prediction compensation mechanism. In the case of synchronous correlation of observation noise of different sensors at the same time, the observation noise of sensors at the current moment is correlated with the process noise of the previous moment and packet dropout of measurement data, the measurement data is updated in real time by combining the measurement prediction compensation mechanism and the observation noise estimation to avoid the noise de-correlation process. A sequential fusion filter is designed according to the innovation analysis method, and finally, a numerical implementation step of the sequential fusion filter is given based on the third-degree spherical-radial cubature rule. Simulation results verify the effectiveness of the sequential fusion filter algorithm proposed in this paper.

Conflicts of Interest:
The authors declare no conflict of interest.

Appendix A
The nonlinear multi-sensor sequential fusion state estimation algorithm with crosscorrelated noise and packet dropout compensation is proved as follows.

Appendix A.1. System Sequential Fusion State Estimation with Correlated Noise and Packet Dropout Compensation
The proofs of the state estimate of the ith sensorX i k+1 , the state estimation error covariance matrix P X i k+1|k+1 , the filter gain M X i k+1 , the innovation ε i (k + 1), and the innovation covariance matrix Q i ε (k + 1) are first given below. According to the projection theorem, at the time step k + 1, the state estimate of the ith sensorX i k+1 as: Then, (7) is obtained. The filter gain expression M X i k+1 is derived as follows: The system state estimation errorX i k+1 is obtained by subtractingX i k+1 from X k+1 : The system state estimation error covariance of the ith sensor P X i k+1|k+1 is: Then, substituting (A3) into (A4) leads to (8). Moreover, (7) and (8) in this paper are proven.
From the definition of the innovation, at the time step k + 1 the innovation ε i (k + 1) of the ith sensor is: Then, substituting (3) into (A5) leads to (9). The proof of the innovation covariance matrix Q i ε (k + 1) are given as follows: substituting (3) into (A6) leads to (10), where The covariance matrix between the state estimation error and the innovation is: Then, substituting (3) and (9) into (A10) leads to (12).

Appendix A.2. Observed Noise Estimation Based on Sequential Fusion with Cross-Correlated Noise and Packet Dropout Compensation
We give the estimates of the observed noise V (i|i−1 ) k+1|k+1 , the gain matrix M V i|i−1 k+1 and the respective covariance matrices of the system in this paper, and its iterative framework.
According to the projection theorem, the estimation of the observation noise V (i|i−1 ) k+1|k+1 as: The (A11) can be iterated to obtain (17), where the observation noise gain matrix M V i|l k+1 is: The covariance matrix P V i|l−1 k+1|k+1 of the estimation error of the observation noise is: (A14) can be iterated to obtain (24). The covariance matrix P X l−1 V i|l−1 k+1|k+1 between the state estimation error and the observation noise can be calculated as follows: = F(X k )N X k ;X L k|k , P X L k|k dX k (A17) (A16) can be iterated to obtain (23). Thus, Equation (21) completes the proof by the above description.