Weighted Measurement Fusion Particle Filter for Nonlinear Systems with Correlated Noises

Abstract

The multi-sensor information fusion particle filter (PF) has been put forward for nonlinear systems with correlated noises. The proposed algorithm uses the Taylor series expansion method, which makes the nonlinear measurement functions have a linear relationship by the intermediary function. A weighted measurement fusion PF (WMF-PF) was put forward for systems with correlated noises by applying the full rank decomposition and the weighted least square theory. Compared with the augmented optimal centralized fusion particle filter (CF-PF), it could greatly reduce the amount of calculation. Moreover, it showed asymptotic optimality as the Taylor series expansion increased. The simulation examples illustrate the effectiveness and correctness of the proposed algorithm.

1. Introduction

In the last few years, filtering algorithms for nonlinear systems have attracted a great deal of attention from many scholars due to their wide applications such as guidance, signal processing, aircraft attitude estimation, trajectory tracking, and so on [1,2,3,4,5,6,7,8]. Due to the low accuracy and the poor fault tolerance of single-sensor systems, multi-sensor information fusion technologies are gaining more and more attention from scholars and have been widely used due to their high accuracy and reliability. This paper proposes a multi-sensor information fusion filter for nonlinear systems with correlated noise.

Nonlinear systems have many filtering methods including the extended Kalman filter (EKF) [9,10], unscented Kalman filter (UKF) [11,12], cubature Kalman filter (CKF) [13], Sequence Monte Carlo (SMC) [14,15], Markov Chain Monte Carlo (MCMC) [16], particle filter (PF) [17,18,19,20,21], and so on. As PF does not demand the system noises to be Gaussian, it can be applied to more situations. In practice, the conditions of noise independence are difficult to meet due to the changes in the internal and external environment. When the measurement noises and the system noises are correlated, the classical filters (EKF, UKF, CKF, PF) will be biased or even divergent. Based on the Bayesian estimation framework, there are two fundamental methods for the estimation problem with correlated noise. One is the de-correlation method [22,23,24,25,26], which can convert the correlated noises into uncorrelated ones. Its advantage is that the algorithm is simple in structure and implementation. The other is to redesign the new filters by improving the existing algorithm framework [27,28,29,30,31]. Its advantage is that the algorithm generally has less computational cost than the de-correlation method, and its disadvantage is that the algorithm does not have a general structure and is difficult to design. Taking into account the universality, the de-correlation method was used in this paper.

So far, there are two main kinds of fusion methods: the centralized fusion (CF) algorithm and distributed information fusion algorithms. The CF algorithm spreads all sensor information and outputs into a fusion state estimate. The CF algorithm is globally optimal under certain performance criteria when all sensors work well, because there is no information lost [32]. However, the disadvantages of the CF algorithm include the expensive computational cost and poor fault tolerance [32,33]. The distributed fusion algorithms send local state estimates to the fusion center and perform weight estimation according to a certain fusion criterion [34,35,36,37]. Distributed fusion algorithms have a lower computational cost and better fault tolerance than the centralized fusion algorithm. However, the estimated accuracy is globally suboptimal and locally optimal. In order to ensure the accuracy of the estimation, a kind of centralized fusion method, the weighted measurement fusion (WMF) algorithms, was adopted in this paper. Unlike the traditional centralized fusion method, the WMF algorithms fuse the measurements according to certain criterion, then the dimensional measurement is used in the filter for estimating. The WMF is a kind of data compression method and widely used in the occasions of mass data such as the sensor network and so on [38,39,40,41,42]. The WMF algorithms require that the measurement noises and the system noises are independent, and the filters [41] can deal with the filtering problem with correlated noise by the Lagrange multiply method. However, both of them can only be used in linear systems.

In this paper, a WMF algorithm was proposed for nonlinear systems with correlated noises by using the weighted least squares (WLS) method and Taylor series expansion. It can compress a high-dimensional measurement into a low-dimensional measurement for nonlinear measurements. Furthermore, a weighted measurement fusion particle filter (WMF-PF) was presented by combining it with PF. The proposed filter could solve the fusion estimation problem for multi-sensor nonlinear systems with correlated measurement noises and the system noises. The proposed WMF-PF had an approximate accuracy and a reduced calculation when compared with the centralized fusion particle filter.

The rest of the paper is organized as follows. The de-correlation and weighted measurement fusion algorithms are introduced in Section 2. Combined with PF, a weighted measurement fusion particle filter (WMF-PF) algorithm is introduced in Section 3. The simulation analysis is given in Section 4. The conclusions are summarized in Section 5.

2. Problem Formulation

Consider the following nonlinear multiple sensor systems with correlated noises:

$$\mathit{x}(k+1)=\mathit{f}(\mathit{x}(k),k)+\mathit{\Gamma }\mathit{w}(k)$$

(1)

$${\mathit{y}}_j(k)={\mathit{h}}^{(j)}(\mathit{x}(k),k)+{\mathit{v}}^{(j)}(k),j=1,2,\cdots ,L$$

(2)

where $k$ is the discrete time; $\mathit{x}(k)\in {\mathit{R}}^n$ is the system state vector at time $k$; ${\mathit{y}}_j(t)\in {\mathit{R}}^{m_j}$ is the measurement vector of the jth sensor; $\mathit{w}(k)\sim p_{\omega }(·)$ is the system noise and ${\mathit{v}}_j(k)\sim p_{v_j}(·)$ is the measurement noise of the jth sensor; $\mathit{f}(\cdot ,\cdot )\in {\mathit{R}}^n$ and ${\mathit{h}}_j(\cdot ,\cdot )\in {\mathit{R}}^{m_j}$ are the nonlinear state and measurement functions, respectively; and $\mathit{\Gamma }\in {\mathit{R}}^{n\times p}$ is the noise input matrix. $L$ is the number of sensors. $\mathit{w}(k)$ and ${\mathit{v}}_j(k)$ are correlated white noises with zero mean and covariance ${\mathit{Q}}_w$ and ${\mathit{R}}_{ji}$, i.e.,

$$\mathrm{E}\left\{\begin{array}{cc}\left[\begin{array}{c}\mathit{w}(t)\\ {\mathit{v}}_j(t)\end{array}\right]& \left[\begin{array}{cc}{\mathit{w}}^{\mathrm{T}}(k)& {{\mathit{v}}_i}^{\mathrm{T}}(k)\end{array}\right]\end{array}\right\}=\left[\begin{array}{cc}{\mathit{Q}}_w& {\mathit{S}}_i\\ {\mathit{S}}_j^{\mathrm{T}}& {\mathit{R}}_{ji}\end{array}\right]{\delta }_{tk}$$

(3)

where $\mathrm{E}$ is defined as a mathematical expectation; the superscript $\mathrm{T}$ is defined as the transpose; and ${\delta }_{tk}$ is the Kronecker-ä function, i.e., ${\delta }_{tt}=1$, ${\delta }_{tk}=0(t\ne k)$.

Next, we converted the systems in Equations (1) and (2) into the systems with independent noise.

Theorem 1. 

For systems in Equations (1) and (2), the decorrelated centralized measurement fusion systems (CMFS) can be rewritten as:

$$\mathit{x}(k+1)=\overline{\mathit{f}}(\mathit{x}(k),k)+\overline{\mathit{w}}(k)$$

(4)

$${\mathit{y}}^{(\mathrm{C})}(k)={\mathit{h}}^{(\mathrm{C})}(\mathit{x}(k),k)+{\mathit{v}}^{(\mathrm{C})}(k)$$

(5)

where:

$$\overline{\mathit{f}}(\mathit{x}(k),k)=\mathit{f}(\mathit{x}(k),k)-\mathit{M}{\mathit{h}}^{(\mathrm{C})}(\mathit{x}(k),k)+\mathit{M}{\mathit{y}}^{(\mathrm{C})}(k)$$

(6)

$$\mathit{M}=\mathit{\Gamma }{\mathit{S}}^{(\mathrm{C})}{\left({\mathit{R}}^{(\mathrm{C})}\right)}^{-1}$$

(7)

$$\overline{\mathit{w}}(k)=\mathit{\Gamma }\mathit{w}(k)-\mathit{M}{\mathit{v}}^{(\mathrm{C})}(k)$$

(8)

$${\mathit{y}}^{(\mathrm{C})}(k)={[{\mathit{y}}_1^{\mathrm{T}}(k),{\mathit{y}}_2^{\mathrm{T}}(k),\cdots ,{\mathit{y}}_L^{\mathrm{T}}(k)]}^{\mathrm{T}}$$

(9)

$${\mathit{h}}^{(\mathrm{C})}(\mathit{x}(k),k)={[{\mathit{h}}_1^{\mathrm{T}}(\mathit{x}(k),k),{\mathit{h}}_2^{\mathrm{T}}(\mathit{x}(k),k),\cdots ,{\mathit{h}}_L^{\mathrm{T}}(\mathit{x}(k),k)]}^{\mathrm{T}}$$

(10)

$${\mathit{v}}^{(\mathrm{C})}(k)={[{\mathit{v}}_1^{\mathrm{T}}(k),{\mathit{v}}_2^{\mathrm{T}}(k),\cdots ,{\mathit{v}}_L^{\mathrm{T}}(k)]}^{\mathrm{T}}$$

(11)

and the covariance matrix of${\mathit{v}}^{(\mathrm{C})}(k)$is given as:

$${\mathit{R}}^{(\mathrm{C})}={\left({\mathit{R}}_{ij}\right)}_{m\times m},m={\displaystyle \sum _{i=1}^L{m}_i}$$

(12)

The cross-covariance matrix of$\mathit{w}(k)$and${\mathit{v}}^{(\mathrm{C})}(k)$is given as:

$${\mathit{S}}^{(\mathrm{C})}=[{\mathit{S}}_1,{\mathit{S}}_2,\cdots ,{\mathit{S}}_L]$$

(13)

and the system noise statistical property after the transformation is as follows:

$$\mathrm{E}[\overline{\mathit{w}}(k)]=0$$

(14)

$$\mathrm{E}[\overline{\mathit{w}}(k){\overline{\mathit{w}}}^{\mathrm{T}}(k)]=\mathit{\Gamma }[{\mathit{Q}}_w-{\mathit{S}}^{(\mathrm{C})}{\left({\mathit{R}}^{(\mathrm{C})}\right)}^{-1}{\left({\mathit{S}}^{(\mathrm{C})}\right)}^{\mathrm{T}}]{\mathit{\Gamma }}^{\mathrm{T}}$$

(15)

Proof. 

From Equation (2), merging the $L$ measurement equations, Equations (5) and (9)–(13) are obtained. Equation (1) can be transformed via Equations (1) and (2):

$$\mathit{x}(k+1)=\mathit{f}(\mathit{x}(k),k)+\mathit{\Gamma }\mathit{w}(k)+\mathit{M}({\mathit{y}}^{(\mathrm{C})}(k)-{\mathit{h}}^{(\mathrm{C})}(\mathit{x}(k),k)-{\mathit{v}}^{(\mathrm{C})}(k))$$

(16)

Then, we obtain Equations (4), (6) and (8), and the measurement noises and the system noises are uncorrelated in Equations (4) and (5), i.e.,

$$\mathrm{E}[\overline{\mathit{w}}(k){\mathit{v}}^{(\mathrm{C})\mathrm{T}}(k)]=0$$

(17)

We can obtain the $\mathit{M}$ as Equation (7). □

For multi-sensor systems, the centralized fusion method above-mentioned will have a high computational cost due to the high-dimensional augmented measurements. Therefore, many scholars have put forward a weighted measurement fusion method that can compress an augmented high dimensional measurement into a low dimensional measurement. However, many existing results refer to linear systems. In the following, we propose a weighted measurement fusion algorithm to solve the nonlinear systems with correlated noises.

Theorem 2. 

For the systems in Equations (4) and (5), if the measurement functions${\mathit{h}}^{(j)}(\mathit{x}(k),k)$,$j=1,2,\cdots ,L$have linear relationships, i.e.,$\exists \mathit{h}(\mathit{x}(k),k)\in {\Re }^p$satisfies${\mathit{h}}_j(\mathit{x}(k),k)={\mathit{H}}_j(k)\mathit{h}(\mathit{x}(k),k)$, with matrix${\mathit{H}}_j(k)\in {\Re }^{m_j\times p}$. The reduced dimensional measurement function of the weighted measurement fusion system (WMFS) is as follows:

$${\mathit{y}}^{(\mathrm{I})}(k)={\mathit{H}}^{(\mathrm{I})}(k)\mathit{h}(\mathit{x}(k),k)+{\mathit{v}}^{(\mathrm{I})}(k)$$

(18)

where:

$${\mathit{y}}^{(\mathrm{I})}(k)={[{\mathit{K}}^{\mathrm{T}}(k){({\mathit{R}}^{(\mathrm{C})})}^{-1}\mathit{K}(k)]}^{-1}{\mathit{K}}^{\mathrm{T}}(k){({\mathit{R}}^{(\mathrm{C})})}^{-1}{\mathit{y}}^{(\mathrm{C})}(k)$$

(19)

$${\mathit{v}}^{(\mathrm{I})}(k)={[{\mathit{K}}^{\mathrm{T}}(k){({\mathit{R}}^{(\mathrm{C})})}^{-1}\mathit{K}(k)]}^{-1}{\mathit{K}}^{\mathrm{T}}(k){({\mathit{R}}^{(\mathrm{C})})}^{-1}{\mathit{v}}^{(\mathrm{C})}(k)$$

(20)

$\mathit{K}(k)$is a full-column rank matrix and${\mathit{H}}^{(\mathrm{I})}(k)$is a full-row rank matrix, which are the full rank decomposition matrices of matrix${\mathit{H}}^{(\mathrm{C})}(k)\in {\Re }^{m\times p}$, i.e.,

$${\mathit{H}}^{(\mathrm{C})}(k)={[{\mathit{H}}_1^{\mathrm{T}}(k),{\mathit{H}}_2^{\mathrm{T}}(k),\cdots ,{\mathit{H}}_L^{\mathrm{T}}(k)]}^{\mathrm{T}}=\mathit{K}(k){\mathit{H}}^{(\mathrm{I})}(k)$$

(21)

The covariance matrix of${\mathit{v}}^{(\mathrm{I})}(k)$is computed as:

$${\mathit{R}}^{(\mathrm{I})}(k)={[{\mathit{K}}^{\mathrm{T}}(k){\mathit{R}}^{(\mathrm{C})-1}\mathit{K}(k)]}^{-1}$$

(22)

Proof. 

Using ${\mathit{h}}_j(\mathit{x}(k),k)={\mathit{H}}_j(k)\mathit{h}(\mathit{x}(k),k)$ and substituting Equation (21) into Equation (5), we have:

$${\mathit{y}}^{(\mathrm{C})}(k)=\mathit{K}(k){\mathit{H}}^{(\mathrm{I})}(k)\mathit{h}(\mathit{x}(k),k)+{\mathit{v}}^{(\mathrm{C})}(k)$$

(23)

As ${\mathit{K}}^{\mathrm{T}}(k){\mathit{R}}^{(\mathrm{C})-1}\mathit{K}(k)$ is nonsingular, the optimal Gauss-Markov estimate of ${\mathit{H}}^{(\mathrm{I})}(k)\mathit{h}(\mathit{x}(k),k)$ can be obtained as Equations (18)–(22) by using the WLS method. □

For some nonlinear systems, the intermediary function $\mathit{h}(\mathit{x}(k),k)$ is difficult to obtain. In the following, we present a polynomial intermediary function obtained by the Taylor series expansion method that can overcome the constraints of Theorem 2, so that the WMFS can be obtained via Theorem 2.

Theorem 3. 

The approximate measurement equation of WMFS for the system in Equations (4) and (5) is given as:

$${\tilde{\mathit{y}}}^{(\mathrm{I})}(k)={\tilde{\mathit{H}}}^{(\mathrm{I})}(k)\tilde{\mathit{h}}(\mathit{x}(k),k)+{\tilde{\mathit{v}}}^{(\mathrm{I})}(k)$$

(24)

where:

$${\tilde{\mathit{y}}}^{(\mathrm{I})}(k)={[{\tilde{\mathit{K}}}^{\mathrm{T}}(k){({\mathit{R}}^{(\mathrm{C})})}^{-1}\tilde{\mathit{K}}(k)]}^{-1}{\tilde{\mathit{K}}}^{\mathrm{T}}(k){\mathit{R}}^{(\mathrm{C})-1}{\mathit{y}}^{(\mathrm{C})}(k)$$

(25)

$${\tilde{\mathit{v}}}^{(\mathrm{I})}(k)={[{\tilde{\mathit{K}}}^{\mathrm{T}}(k){({\mathit{R}}^{(\mathrm{C})})}^{-1}\tilde{\mathit{K}}(k)]}^{-1}{\tilde{\mathit{K}}}^{\mathrm{T}}(k){({\mathit{R}}^{(\mathrm{C})})}^{-1}{\mathit{v}}^{(\mathrm{C})}(k)$$

(26)

The covariance matrix of${\tilde{\mathit{v}}}^{(\mathrm{I})}(k)$is${\tilde{\mathit{R}}}^{(\mathrm{I})}(k)={[{\tilde{\mathit{K}}}^{\mathrm{T}}(k){\mathit{R}}^{(\mathrm{C})-1}\tilde{\mathit{K}}(k)]}^{-1}$, and${\tilde{\mathit{H}}}^{(\mathrm{I})}(k)$and${\tilde{\mathit{K}}}^{\mathrm{T}}(k)$are the full rank decomposition matrices of${\tilde{\mathit{H}}}^{(\mathrm{C})}(k)$as Equation (21), where:

$${\tilde{\mathit{H}}}^{(\mathrm{C})}(k)={\left[\begin{array}{ccccc}{\mathit{D}}^{(\mathrm{C})0}& {\mathit{D}}^{(\mathrm{C})1}& {\mathit{D}}^{(\mathrm{C})2}& \cdots & {\mathit{D}}^{(\mathrm{C})\gamma }\end{array}\right]|}_{\mathit{x}=\widehat{\mathit{x}}}$$

(27)

$$\tilde{\mathit{h}}(\mathit{x}(k),k)={\left[\begin{array}{ccccc}1& {\left(\mathit{\Delta }\mathit{x}\right)}^{\mathrm{T}}& {\left({\left(\mathit{\Delta }\mathit{x}\right)}^2\right)}^{\mathrm{T}}& \cdots & {\left({\left(\mathit{\Delta }\mathit{x}\right)}^{\gamma }\right)}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$$

(28)

$${\mathit{D}}^{(\mathrm{C})l}=\frac{1}{l!}\frac{{\partial }^l{\mathit{h}}^{(\mathrm{C})}(\mathit{x},k)}{\partial {\mathit{x}}^{\mathrm{T}}},l=1,\cdots ,\gamma$$

(29)

$\gamma$is the order of Taylor series expansion, and defining:

$$\mathit{\Delta }\mathit{x}=\mathit{x}-\widehat{\mathit{x}}$$

(30)

$${(\mathit{\Delta }\mathit{x})}^l={\left[\begin{array}{ccc}{\displaystyle {\prod }_{i=1}^n\mathsf{\Delta }{x}_i^{l_i},}& \cdots ,& {\displaystyle {\prod }_{j=1}^n\mathsf{\Delta }{x}_j^{l_j}}\end{array}\right]}^{\mathrm{T}},{\displaystyle \mathsf{\sum }_{k=1}^n{l}_k}=l$$

(31)

$${\mathit{D}}^{(\mathrm{C})l}{|}_{x=\hat{x}}{(\mathit{\Delta }\mathit{x})}^l=\left[\begin{array}{c}\sum \frac{{\partial }^l{h}_1^{(\mathrm{C})}(\mathit{x},k)}{\partial x_1^{l_1}{x}_2^{l_2}\cdots x_n^{l_n}}(\Delta x_1^{l_1}·\Delta x_2^{l_2}·\cdots ·\Delta x_n^{l_n})\\ \sum \frac{{\partial }^l{h}_2^{(\mathrm{C})}(\mathit{x},k)}{\partial x_1^{l_1}{x}_2^{l_2}\cdots x_n^{l_n}}(\Delta x_1^{l_1}·\Delta x_2^{l_2}·\cdots ·\Delta x_n^{l_n})\\ ⋮\\ \sum \frac{{\partial }^l{h}_m^{(\mathrm{C})}(\mathit{x},k)}{\partial x_1^{l_1}{x}_2^{l_2}...x_n^{l_n}}(\Delta x_1^{l_1}·\Delta x_2^{l_2}·\cdots ·\Delta x_n^{l_n})\end{array}\right],{\displaystyle \sum _{i=1}^n{l}_i}=l$$

(32)

where$h_i^{(\mathrm{C})}(\mathit{x},k),i=1,\cdots ,m$is ith the component of${\mathit{h}}^{(\mathrm{C})}(\mathit{x}(k),k)$.

Proof. 

Using the Taylor series expansion method, the approximation of ${\mathit{y}}^{(\mathrm{C})}(k)$ in Equation (23) can be written as:

$$\begin{gathered} {\tilde{\mathit{y}}}^{(\mathrm{C})}(k)={{\mathit{h}}^{(\mathrm{C})}(\mathit{x},k)|}_{\mathit{x}=\widehat{\mathit{x}}}+{\frac{\partial {\mathit{h}}^{(\mathrm{C})}(\mathit{x},k)}{\partial {\mathit{x}}^{\mathrm{T}}}|}_{\mathit{x}=\widehat{\mathit{x}}}\mathit{\Delta }\mathit{x}+\frac{1}{2!}{\frac{{\partial }^2{\mathit{h}}^{(\mathrm{C})}(\mathit{x},k)}{\partial {\mathit{x}}^{\mathrm{T}}}|}_{\mathit{x}=\widehat{\mathit{x}}}{\left(\mathit{\Delta }\mathit{x}\right)}^2+\cdots \\ +\frac{1}{\gamma !}{\frac{{\partial }^{\gamma }{\mathit{h}}^{(\mathrm{C})}(\mathit{x},k)}{\partial {\mathit{x}}^{\mathrm{T}}}|}_{\mathit{x}=\widehat{\mathit{x}}}{\left(\mathit{\Delta }\mathit{x}\right)}^{\gamma }+{\mathit{v}}^{(\mathrm{C})}(k) \\ =[\begin{array}{cccc}{\mathit{h}}^{(\mathrm{C})}(\mathit{x},k),& \frac{\partial {\mathit{h}}^{(\mathrm{C})}(\mathit{x},k)}{\partial {\mathit{x}}^{\mathrm{T}}},& \frac{1}{2!}\frac{{\partial }^2{\mathit{h}}^{(\mathrm{C})}(\mathit{x},k)}{\partial {\mathit{x}}^{\mathrm{T}}},& \cdots ,\end{array}{\frac{1}{\gamma !}\frac{{\partial }^{\gamma }{\mathit{h}}^{(\mathrm{C})}(\mathit{x},k)}{\partial {\mathit{x}}^{\mathrm{T}}}]|}_{\mathit{x}=\widehat{\mathit{x}}}{\left[\begin{array}{cccc}1& \mathit{\Delta }\mathit{x}& \cdots & {\left(\mathit{\Delta }\mathit{x}\right)}^{\gamma }\end{array}\right]}^{\mathrm{T}}+{\mathit{v}}^{(\mathrm{C})}(k) \end{gathered}$$

(33)

Let $\tilde{\mathit{h}}(\mathit{x}(k),k)$ be Equation (28) and ${\tilde{\mathit{H}}}^{(\mathrm{C})}(k)$ be Equation (27), then Equations (24)–(26) can be obtained by Theorem 2.

By increasing the number of sensors, the dimension of the measurement equation of CMFS will be large and it will cause an expensive computational cost. However, the dimension of the measurement equation of WMFS will be fixed when the order of the Taylor series expansion $\gamma$ is fixed. For example, if the dimensions of the system and measurement functions are both 1 and there are 10 sensors, the dimension of the measurement equation of CMFS is 10, but the dimension of the measurement equation of WMFS is 3 when we choose $\gamma =2$. □

3. Weighted Measurement Fusion Particle Filter (WMF-PF) Algorithm

(1) Initialization:

$${\widehat{\mathit{x}}}^{(\mathrm{I})(i)}(0|0)\sim p_{x_0}({\mathit{x}}_0),i=1,\cdots ,N_s$$

(34)

(2) State prediction particles:

$${\widehat{\mathit{x}}}^{(\mathrm{I})(i)}(k|k-1)=\mathit{f}({\widehat{\mathit{x}}}^{(\mathrm{I})(i)}(k-1|k-1),k-1)+{\mathit{\xi }}^{(\mathrm{I})(i)}(k-1)$$

(35)

where ${\mathit{\xi }}^{(\mathrm{I})(i)}(k-1)$ is the random number with the same distribution of the process noise $\mathit{w}(k)$, and the measurement prediction particles:

$${\widehat{\mathit{y}}}^{(\mathrm{I})(i)}(k|k-1)={\tilde{\mathit{H}}}^{(\mathrm{I})}(k)\mathit{h}({\widehat{\mathit{x}}}^{(\mathrm{I})(i)}(k|k-1),k)$$

(36)

(3) The importance weights:

$${\omega }_k^{(\mathrm{I})(i)}=\frac{1}{N_s}p({\widehat{\mathit{y}}}^{(\mathrm{I})(i)}(k|k-1)|{\widehat{\mathit{x}}}^{(\mathrm{I})(i)}(k|k-1))$$

(37)

that is:

$${\omega }_k^{(\mathrm{I})(i)}=\frac{1}{N_s}{p}_{v_k^{(\mathrm{I})}}({\tilde{\mathit{y}}}^{(\mathrm{I})}(k)-{\widehat{\mathit{y}}}^{(\mathrm{I})(i)}(k|k-1))$$

(38)

where ${\tilde{\mathit{y}}}^{(\mathrm{I})}(k)$ is computed by Equation (25) and ${\overline{\omega }}_k^{(\mathrm{I})(i)}$ is given as:

$${\overline{\omega }}_k^{(\mathrm{I})(i)}=\frac{{\omega }_k^{(\mathrm{I})(i)}}{{\displaystyle \sum _{i=1}^N{\omega }_k^{(\mathrm{I})(i)}}}$$

(39)

(4) Filter and filter error variance matrix:

$${\widehat{\mathit{x}}}^{(\mathrm{I})}(k|k)={\displaystyle \sum _{i=1}^{N_s}{\overline{\omega }}_k^{(\mathrm{I})(i)}{\widehat{\mathit{x}}}^{(\mathrm{I})(i)}(k|k-1)}$$

(40)

$${\mathit{P}}^{(\mathrm{I})}(k|k)\approx {\displaystyle \sum _{i=1}^{N_s}{\overline{\omega }}_k^{(\mathrm{I})(i)}{({\widehat{\mathit{x}}}^{(\mathrm{I})(i)}(k|k-1)-{\widehat{\mathit{x}}}^{(\mathrm{I})(i)}(k|k))}^2}$$

(41)

(5) Resampling:

$$u_i=\frac{(i-1)+r}{N},r\sim U[0,1],i=1,\cdots ,N_s$$

(42)

If ${\displaystyle \sum _{j=1}^{m-1}{\overline{\omega }}_k^{(\mathrm{I})(j)}}<u_i\le {\displaystyle \sum _{j=1}^m{\overline{\omega }}_k^{(\mathrm{I})(j)}}$, we directly copied $m$ particles as the resampling particles ${\widehat{\mathit{x}}}^{(\mathrm{I})(i)}(k|k)$ directly.

We then turn to step (2) and iterated.

The flow chart of the noise-related WMF-PF algorithm is shown in Figure 1.

4. Simulation Example

Let us consider a 4-sensor nonlinear system [43]:

$$x(k)=0.5x(k-1)+25x(k-1)/(1+x{(k-1)}^2)+8\cos (1.2(k-1))+w(k)$$

(43)

$$z^{(j)}(k)=h^{(j)}\left(x(k),k\right)+v^{(j)}(k),j=1,2,3,4$$

(44)

$w(k)=\alpha \xi (k)+w_e(k)$, $v^{(j)}(k)={\beta }^{(j)}\xi (k)+v_e^{(j)}(k)$, where $\xi (k)$ is the background noise and $\xi (k)~N(0,{\sigma }_{\xi }^2)$, $w_e(k)~N(0,{\sigma }_{we}^2)$, $w_v^{(j)}(k)~N(0,{\sigma }_{vj}^2)$, so

$$S_j=\alpha {\sigma }_{\xi }^2{\left({\beta }^{(j)}\right)}^{\mathrm{T}},R_{ji}={\beta }^{(j)}{\sigma }_{\xi }^2{\left({\beta }^{(i)}\right)}^{\mathrm{T}}.$$

(45)

In the simulation, we set ${\sigma }_{\xi }^2={0.4}^2$, ${\sigma }_{we}^2={0.5}^2$, ${\sigma }_{v1}^2={0.6}^2$, ${\sigma }_{v2}^2={0.7}^2$, ${\sigma }_{v3}^2={0.8}^2$, and ${\sigma }_{v4}^2={0.9}^2$ as well as $\alpha =0.8$, ${\beta }^{(1)}=1.5$, ${\beta }^{(2)}=1.2$, ${\beta }^{(3)}=1.4$, and ${\beta }^{(4)}=1.3$.

We selected single-valued functions within the range of $x(k)$, taking into account observability:

$$\begin{gathered} h^{(1)}\left(x(k),k\right)=x(k);h^{(2)}\left(x(k),k\right)=x{(k)}^3/30; \\ {h}^{(3)}\left(x(k),k\right)=10\exp (x(k)/10);h^{(4)}\left(x(k),k\right)=10\sin (\raisebox{1ex}{$\pi $}\!\left/ \!\raisebox{-1ex}{${40}^{x(k)}$}\right.), \end{gathered}$$

(46)

and the initial value is $x(0)=0$.

The first-order Taylor series expansion (WMF-PF1) tracking curve is shown in Figure 2. It can be seen in the figure that the tracking effect was good. The second-order Taylor series expansion (WMF-PF2) tracking curve is shown in Figure 3 where it can be seen that the tracking effect was good.

The estimation performance criterion is the accumulated mean square error (AMSE) [43]:

$$\mathrm{AMSE}(k)={\displaystyle \sum _{t=0}^k\frac{1}{N}{\displaystyle \sum _{j=1}^N{[x_j(t)-{\widehat{x}}_j(t|t)]}^2}},N=50$$

(47)

where ${\widehat{x}}_j(t|t)$ is the jth-time Monte Carlo experiment at time t. The AMSE curves of local PFs (LF1–LF4), the curve of WMF-PF1, the curve of WMF-PF2, and the curve of CF-PF are shown in Figure 4 with 50 Monte Carlo experiments. From Figure 2 and Figure 3 it is evident that all of the fusion filters had better accuracy than the local PFs. Moreover, WMF-PF2 had better accuracy than WMF-PF1, but was worse than CF-PF.

Using WMF-PF2 as an example to analyze the computational burden, the augmented measurement function of CF-PF will be four dimensions. However, for WMF-PF2, the measurement function is three dimensional, so the computational burden of CF-PF will be more expensive than WMF-PF. Moreover, as the number of sensors increases, the dimension of the measurement function of CF-PF will consistently rise, but the measurement function’s dimension of WMF-PF2 will be fixed at three when the order is $\gamma =2$.

It can be seen from Figure 3 that the estimation accuracy from high to low was the centralized fusion PF (CF-PF), weighted measurement fusion based on the second-order Taylor series expansion PF algorithm (WMF-PF2), weighted measurement fusion based on the first-order Taylor series expansion PF algorithm (WMF-PF1), and the four local PF estimation algorithms (LF1–LF4).

From the simulation results in Figure 2, Figure 3 and Figure 4, the effectiveness of the de-correlated weighted measurement fusion PF filtering algorithm proposed in this paper can be fully explained.

5. Conclusions

For a nonlinear multi-sensor system with correlated noise, the system noise and the measurement noise are correlated at the same time stamp. First, by using a de-correlation method, a nonlinear multi-sensor system with correlated noise was transformed into a nonlinear system with independent Gaussian noise. Then, based on the Taylor series approximation method combined with PF algorithms, the weighted measurement fusion PF algorithm was designed. In fact, the proposed fusion algorithm could be combined with many nonlinear filter algorithms (UKF, PF, CKF, etc.). The proposed filter can be used for multi-sensor discrete-time nonlinear systems with correlated measurement noises and system noises. This algorithm asymptotically approximated the CF-PF with the increase of the Taylor series, so it was approximately globally optimal. The algorithm solved the fusion estimation problem of nonlinear systems, and had an asymptotical accuracy. The fusion algorithm was much less computationally intensive than the centralized fusion algorithm. From the simulation, we could see that the weighted measurement fusion PF had a very low computational complexity and good real-time performance. The algorithm can be widely applied in sensor networks.