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

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 nonlinear properties. With the development of complex control systems, a large number of state estimation methods for nonlinear systems have been proposed. First, the more common solution is based on the nonlinear function linearization of the Extended Kalman Filter (EKF) [6], and similarly the Divided Difference Filter (DDF) [7] and Central Difference Filter (CDF) [8]. Julier et al. proposed the Unscented Kalman Filter (UKF) [9] based on the traceless transformation, which obtains the desired state distribution by transforming the selected sigma points through a nonlinear transformation. Arasaratnam et al. proposed a Cubature Kalman Filter (CKF) [10] based on the third-degree spherical-radial cubature rule, which also is based on deterministic sampling to numerically approximate the integral of the nonlinear function and the Gaussian distribution in a high-dimensional scene. It has been demonstrated in the literature that the CKF has better numerical implementation stability than the UKF [11,12]. In addition, the particle filtering (PF) [13] based on the Monte Carlo sampling method, which is one of the random sampling methods, is computationally expensive and has the problem of resampling due to particle degradation, which is a tedious process.

The fusion algorithms in the field of multi-sensor information fusion [14,15] are mainly divided into three types: centralized fusion, sequential fusion, and distributed fusion [16]. The centralized fusion approach sends all observations to the data estimation center for fusion estimation at the same time step, thus requiring high computational power of the data processing center, which provides the best estimation accuracy when the sensors are in healthy working condition. However, the centralized fusion approach may produce unreliable estimations when the sensors degrade or fault. On the contrary, distributed fusion has the advantage of a parallel structure, which can detect and isolate the faulty sensors and generate local estimates based on the measurements of each sensor [17], and then send the local estimates to the fusion center to generate fusion estimates by using certain fusion rules [16]. Therefore, distributed fusion has good reliability and fault tolerance. Sequential fusion does not need to wait for all sensor data to be transmitted to the fusion center, it can process the observations in real time according to the order of their arrival at the fusion center, avoiding observation augmentation and largely reducing the computational cost.

Correlated noise is widely present in practical engineering systems, where continuous systems are discretized and synchronized with non-uniform sampling, and network systems with random delays and packet dropout can be converted to random parameterized systems with correlated noise [18]. For the study of the filtering problem of nonlinear systems with the correlated process and observation noise, the difficulty is mainly focused on how to deal with the coupling term between the random noise and the system state, which inherently causes the correlation between the process noise and the observation of the system due to the correlation of the noise space, which in turn leads to the computational complexity of the system state prediction. Yan et al. [19] focus on the noise of different sensors’ cross-correlated problems to perform optimal state estimation under linear systems. Sun et al. [20] proposed an optimal distributed fusion filter using the matrix-weighted fusion algorithm, combining the local optimal filter and cross-covariance matrix of filtering error between any two local filters. Based on this, Lin et al. [21] proposed the global optimal distributed fusion filter in the sense of linear minimum variance (LMV) based on distributed fusion as well as sequential fusion approach for linear systems. Hao et al. [22] proposed MW-CKF for nonlinear systems based on the underlying framework of cubature Kalman combined with distributed filter fusion. In order to reduce the energy loss of the system while obtaining higher estimation efficiency, Yan et al. [23] and Cheng et al. [24] proposed a sequential fusion filter based on an event-triggered mechanism. In addition, with the development of science and technology, the performance requirements of the filters are getting higher and higher in order to meet the demand for higher estimation accuracy. The presence of energy loss, data delay, and auto-correlated noise in multi-sensor network systems poses certain challenges for state estimation of complex nonlinear systems. In addition, sequential fusion can handle multi-sensor state fusion with fading measurements 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: ${\mathit{R}}^n$ represents an n-dimensional Euclidean vector space; ${\mathit{I}}_n$ represents an n-dimensional unit matrix; the superscript $-1$ and T represents the inverse and transpose of matrix; ${\Pi }_i(k+1)$ is a linear space spanned by $\left\{Y_{k+1}^{\left(1\right)},Y_{k+1}^{\left(2\right)},\cdots ,Y_{k+1}^{\left(i\right)}\right\}$. $E[◾]$ denotes the mathematical expectation; $(◾)$ is the same as the preceding neighboring term in parentheses; ${\delta }_{k,l}$ denotes the Kronecker Delta function; $N(\mu ,{\sigma }^2)$ is the normal distribution. ${\varsigma }_i$, ${\xi }_i$, ${\zeta }_i$, ${\tau }_i$ represent the ith cubature point.

Consider the following discrete-time nonlinear multi-sensor system with cross-correlated noise and packet dropout compensation: where ${\mathit{X}}_k\in {\mathit{R}}^n$ is the state vector, ${\mathit{Z}}_k^{\left(i\right)}\in {\mathit{R}}^{p_i},i=1,2,\cdots ,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, ${\mathit{W}}_k\in {\mathit{R}}^m$ and ${\mathit{V}}_{k+1}^{\left(i\right)}\in {\mathit{R}}^{p_i},i=1,2,\cdots ,L$ are correlated process noise and observation noise of the ith sensor; ${\Gamma }_k$ is known matrix; ${\gamma }_k^i$ is an independent and identically distributed Bernoulli random variable, and it satisfies the probabilities $p\left({\gamma }_k^i=1\right)=E\left[{\gamma }_k^i\right]=p_k^i$ and $p\left({\gamma }_k^i=0\right)=1-E\left[{\gamma }_k^i\right]=1-p_k^i$, $0\le p_k^i\le 1$. We define $\Pi (k+1)$ as the data received by the estimator up to time $k+1$.

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 $\Pi (k-1)$ obeys a Gaussian distribution of $N({\mathit{X}}_k;{\widehat{\mathit{X}}}_{k|k-1}^0,{\mathit{P}}_{k|k-1}^{X^0})$. The superscript 0 indicates the initial or a priori state.

Since the dropout of measurement packets and correlated noise problems in multi-sensor 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.

Let ${\widehat{\mathit{X}}}_{k+1}^i=E\left[{\mathit{X}}_{k+1}\left|\Pi \left(k\right),{\Pi }_i(k+1)\right.\right]$, ${\widehat{\mathit{X}}}_{k+1}^{-}=E\left[{\mathit{X}}_{k+1}\left|\Pi \left(k\right)\right.\right]$, ${\mathit{V}}_{k+1\left|k+1\right.}^{\left(i\left|0\right.\right)}=E\left[{\mathit{V}}_{k+1}^{\left(i\right)}\left|\Pi \left(k\right)\right.\right]$$=0$ and ${\mathit{V}}_{k+1\left|k+1\right.}^{\left(i\left|i-1\right.\right)}=E\left[{\mathit{V}}_{k+1}^{\left(i\right)}\left|\Pi \left(k\right),{\Pi }_{i-1}(k+1)\right.\right]$. Based on measurements $\Pi (k+1)$, we derive the sequential fusion filter ${\widehat{\mathit{X}}}_{k+1}^s$ and its covariance matrix ${\mathbf{P}}_{k+1\left|k+1\right.}^{X^s}$.

The sequential filter ${\widehat{\mathit{X}}}_{k+1}^i$ and its filtering error covariance matrix ${\mathit{P}}_{k+1\left|k+1\right.}^{X^i}$ for the state are calculated as follows: where the expressions of innovation ${\epsilon }_i(k+1)$, innovation covariance matrix ${\mathit{Q}}_{\epsilon }^i(k+1)$, and filter gain matrix ${\mathit{M}}_{k+1}^{X^i}$ 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 ${\mathit{Q}}_{\epsilon }^i(k+1)$ and ${\mathit{M}}_{k+1}^{X^i}$.

The estimated value of observation noise is:

The calculation of gain matrix ${\mathit{M}}_{k+1}^{V^{i\left|l\right.}}l=1,2,\cdots ,i-1$, cross covariance matrix ${\mathit{P}}_{k+1\left|k+1\right.}^{X^{i-1}{V}^{i\left|i-1\right.}}$ between state estimation error and observation noise is as follows: where 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 is

Then, ${\widehat{\mathit{X}}}_{k+1}^0$ and ${\mathit{P}}_{k+1\left|k+1\right.}^{X^0}$ 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

$\mathbf{Input}$: Sensor measurement data ${\mathit{Z}}_k^{\left(i\right)}$ for the kth time and sensor packet dropout rate $p_k^i$. $\mathbf{Output}$: The posterior estimate ${\widehat{\mathit{X}}}_{k|k}$ of the state and the variance ${\mathit{P}}_{k|k}^X$ of the state error. $\mathbf{Initialization}$: Setting the initial state ${\widehat{\mathit{X}}}_0^{-}={\mu }_0$, the variance ${\mathit{P}}_0^{-}={\mathit{P}}_0$, the process noise variance ${\mathit{Q}}_k={\sigma }_W^2$, the observation noise variance ${\mathit{R}}_i\left(k\right)$ and the covariance ${\mathit{S}}_{i,j}\left(k\right)$ between them, and the sensor packet dropout rate $E\left[{\gamma }_k^i=1\right]=p_k^i$. $\mathbf{State}\mathbf{one}-\mathbf{step}\mathbf{prediction}$: Define ${\mathit{V}}_k^{\left(1\right|0)}=0$ and calculate ${\widehat{\mathit{X}}}_{k+1}^0$, $P_{k+1\left|k+1\right.}^{X^0}$ according to Equations (26) and (27). $\mathbf{Measurement}\mathbf{update}$: Obtain the measurement information ${\mathit{Z}}_{k+1}^{\left(i\right)}$ for each sensor and calculate the innovation ${\epsilon }_i(k+1)$ according to Equation (9), which is then calculated by the following process:      For $i:=1$ to L do         Set ${\mathit{P}}_{k+1|k+1}^{X^0{V}^{i|0}}={\mathit{S}}_i(k+1)$, ${\mathit{P}}_{k+1|k+1}^{V^{i|0}{V}^{1|0}}={\mathit{R}}_{i,1}(k+1)$.         If $i>1$             For $l:=1$ to $i-1$ do                If $l>1$                Calculate ${\mathit{P}}_{k+1\left|k+1\right.}^{X^{l-1}{V}^{i\left|l-1\right.}}$ and ${\mathit{P}}_{k+1\left|k+1\right.}^{V^{i\left|l-1\right.}}$ according to (23) and (24), respectively.                End                Calculate ${\mathit{M}}_{k+1}^{V^{i\left|l\right.}}$ according to (18).             End             Calculate ${\mathit{V}}_{k+1\left|k+1\right.}^{\left(i\left|i-1\right.\right)}$ according to (17).         End         Calculate ${\mathit{Q}}_{\epsilon }^i(k+1)$ according to (10).         Calculate ${\mathit{P}}_{k+1|k+1}^{X^{i-1}{Y}^{i|i-1}}$ according to (12).         Calculate ${\widehat{\mathit{X}}}_{k+1}^i$, ${\mathit{P}}_{k+1\left|k+1\right.}^{X^i}$ according to (7) and (8).      End $\mathbf{State}\mathbf{iteration}$: Calculate ${\widehat{\mathit{X}}}_{k+1}^s$ and ${\mathit{P}}_{k+1\left|k+1\right.}^{X^s}$ by (25); Calculate ${\widehat{\mathit{X}}}_{k+1}^{-}$ and ${\mathit{P}}_{k+1\left|k+1\right.}^{X^{-}}$ according to (26) and (27). Finally, let $k=k+1$. Return to $\mathbf{State}\mathbf{one}-\mathbf{step}\mathbf{prediction}$.

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., ${\widehat{\mathit{X}}}_{k+1}^{l-1}$, ${\mathit{P}}_{k+1|k+1}^{X^{l-1}}$, and ${\mathit{Q}}_{\epsilon }^{l-1}(k+1)$, are known, the corresponding numerical implementation is detailed in the following flow.

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. ${\mathit{W}}_k$ is a Gaussian white noise obeying a mean of zero and a variance of ${\mathit{Q}}_k={\sigma }_W^2$. In addition, the process noise is related to the measurement noise of each sensor, satisfying the following relationship:

The ${\mathit{W}}_k$ and Gaussian white noise ${\eta }_i\left(k\right)\sim N(0,{\sigma }_{{\eta }_i}^2)$ are independent of each other. The variance and covariance of the observation noise for different sensors are ${\mathit{R}}_i\left(k\right)={\sigma }_{{\eta }_i}^2+{\beta }_i{\sigma }_W^2$, ${\mathit{R}}_{i,j}\left(k\right)={\beta }_i{\beta }_j{\sigma }_W^2,i\ne j$ and ${\mathit{S}}_i\left(k\right)={\beta }_i{\sigma }_W^2$, respectively. 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 ${\gamma }_k^1$ and ${\gamma }_k^2$ 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 $\mathit{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: where ${\mathit{X}}_k^{\left(n\right)}$ and ${\widehat{\mathit{X}}}_{k|k}^{\left(n\right)}$ 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 ${\sigma }_W^2=5$, ${\sigma }_{{\eta }_1}^2=2.46$, ${\sigma }_{{\eta }_2}^2=8.75$, ${\beta }_1=0.8$, ${\beta }_2=0.5$ and covariance ${\mathit{R}}_{1,2}=0.19$. The initial value of the state, as well as the initial variance, are ${\mu }_0=0.3$, ${\mathit{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 Figure 3, Figure 4 and Figure 5, the state estimation, state error, and RMSE of the state are shown for the communication rate $p_k^1=0.4$, $p_k^2=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. Figure 6 and Figure 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_k^1=0.35,p_k^2=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.

In this paper, a sequential fusion filtering algorithm is proposed for a nonlinear multi-sensor 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.