A Weighted Measurement Fusion Particle Filter for Nonlinear Multisensory Systems Based on Gauss–Hermite Approximation

Abstract

We addressed the fusion estimation problem for nonlinear multisensory systems. Based on the Gauss–Hermite approximation and weighted least square criterion, an augmented high-dimension measurement from all sensors was compressed into a lower dimension. By combining the low-dimension measurement function with the particle filter (PF), a weighted measurement fusion PF (WMF-PF) is presented. The accuracy of WMF-PF appears good and has a lower computational cost when compared to centralized fusion PF (CF-PF). An example is given to show the effectiveness of the proposed algorithms.

1. Introduction

State estimation algorithms play an important part in automatic control, target tracking, navigation, fault diagnosis, and so on. However, it is difficult for single sensor to obtain accurate estimation and good fault tolerance, so multisensory fusion estimation technology was created to overcome this issue. There are two main kinds of fundamental estimation fusion structures: centralized fusion and distributed fusion. Centralized fusion combines measurements of all sensors into an augmented measurement, and then the data processing center outputs the fused state estimate. The advantage of centralized fusion is that there is no information loss and is ideal when all sensors are fully functional, so it is used as a comparative standard for other fusion algorithms. However, with a large number of sensors, the centralized fusion algorithm performs poorly in real time and is not reliable because of expensive computation due to augmented high-dimension measurements. In the sensor network, a large number of different types of sensors exist. If the data from the sensors are processed in a centralized way, the computational cost will be high. Especially in wireless sensor networks, large amounts of data from nodes make it difficult for decision centers to make timely decisions. In such cases, data compression must be used, and weighted measurement fusion (WMF) is one of these data compression methods [1,2,3].

Distributed fusion algorithms produce a fused estimate by combining local state estimates under a certain criterion in the fusion center [4,5,6,7]. Distributed fusion algorithms, such as the distributed fusion federated Kalman filter [8], the distributed weighted fusion estimation [9], and the distributed covariance intersection (CI) fusion estimation [10], are robust and flexible because of their parallel computing structures [3]. They are optimal for local application but are suboptimal for global use, as their accuracy is lower than the centralized fusion algorithm. WMF algorithms compress a high-dimension measurement to a low-dimension measurement under certain criterion [11,12,13], so the computational cost can be reduced when implementing particle filter (PF) based on the compressed measurement. For linear systems, the WMF algorithms are equivalent to centralized fusion [11,13], so they are also optimal in terms of least mean squares. In this paper, the WMF method will be studied given the abovementioned advantages.

Estimation fusion has formed a complete theory for linear systems over the past years. However, most systems have nonlinear parts. For example, the measurement functions, in sensor models, of most tracking systems are established under spherical coordinates, and are strongly nonlinear when the states are estimated under Cartesian coordinates [14,15]. For nonlinear systems, fusion algorithms, which are achieved by the Taylor series, based on the Extended Kalman filter (EKF) are commonly used [16,17,18,19]. These algorithms are simple since they can be converted into linear systems. However, they result in large estimation bias and even lead to filtering divergence due to the amount of information being omitted. Many nonlinear fusion approaches have been presented, including random set, artificial neural networks, fuzzy logic, rough set, dempster-shafer, and other non-probabilistic approaches [20,21]. These methods fuse information and compress data. Given the amount of information lost, they are usually suboptimal.

Nonlinear filtering algorithms, based on the Bayesian estimation framework and sample approximation, have been widely studied over the past decades, including the Extended Kalman filter (EKF), Unscented Kalman Filter (UKF) [22,23], Cubature Kalman Filter (CKF), and the particle filter (PF) [24]. These algorithms are effective for nonlinear filtering problems with a single sensor [25,26,27,28]. In Hao et al. [29], we presented a weighted measurement fusion UKF (WMF-UKF) via Taylor series and UKF, which can universally handle nonlinear fusion problems. However, this algorithm needs to calculate the coefficients of a Taylor series expansion at every moment, so it is expensive and leads to slow convergence if the wrong expansion location is used.

PF can solve the estimation problem for nonlinear non-Gaussian systems and has high accuracy with an adequate number of sampling points. However, a large number of sampling points results in a huge computational burden, especially for a multisensor centralized fusion estimator. Gauss–Hermite approximation [30,31,32,33] can approximate most nonlinear functions with some sampling points and has an excellent fitting effect. We first proposed a weighted measurement fusion based on Gauss–Hermite approximation and weighted least squares. Next, a nonlinear weighted measurement fusion particle filter (WMF-PF) is presented by combining the fusion algorithm with PF for nonlinear multisensory systems. The proposed algorithm handles the nonlinear fusion problem with any noise and reduces the computational cost compared to centralized fusion PF. Moreover, it overcomes the shortcomings found in Hao et al. [29]. WMF-PF provides an effective compression method for nonlinear multisensory systems, and has potential applications in target tracking [34], communication, and massive data processing.

2. Problem Formulation

In order to facilitate the description of the algorithm, we used the scalar systems as an example. Consider the scalar nonlinear systems with L sensors:

$$x(k+1)=f(x(k),k)+w(k)$$

(1)

$$z_{}^{(j)}(k)=h_{}^{(j)}(x(k),k)+v_{}^{(j)}(k),j=1,2,\cdots ,L$$

(2)

where $f(\cdot ,\cdot )\in \Re$ is the process function, $x(k)\in \Re$ is the scalar state at time $k$, $h^{(j)}(\cdot ,\cdot )\in \Re$ is the measurement function of the jth sensor, $z^{(j)}(k)\in \Re$ is the measurement of the jth sensor, $w(k)\sim p_{w_k^{}}(·)$ ($p_{*}(·)$ is the probability density function) is the process noise, and $v_{}^{(j)}(k)\sim p_{v_k^{(j)}}^{}(·)$ is the measurement noise of the jth sensor. $w(k)$ and $v_{}^{(j)}(k)$ are uncorrelated white noises with zero mean and variances, ${\sigma }_w^2$ and ${\sigma }_{vj}^2$, i.e.,

$${\rm E}\left\{\left[\begin{array}{c}w(t)\\ v^{(j)}(t)\end{array}\right]\left[\begin{array}{cc}{w}^{\mathrm{T}}(k)& v^{(l)}{}_{}^{\mathrm{T}}(k)\end{array}\right]\right\}=\left[\begin{array}{cc}{\sigma }_w^2& 0\\ 0& {\sigma }_{vj}^2{\delta }_{jl}\end{array}\right]{\delta }_{tk}$$

(3)

where E denotes the mathematical expectation, the superscript T denotes the transpose, and ${\delta }_{tk}$ and ${\delta }_{jl}$ are the Kronecker delta functions, i.e., ${\delta }_{tt}=1$ and ${\delta }_{tk}=0(t\ne k)$.

Assumption 1: 

The $f(\cdot ,\cdot )$, $h^{(j)}(\cdot ,\cdot )$, $p_{w_k^{}}(·)$, and $p_{v_k^{(j)}}^{}(·)$ are known.

Assumption 2: 

The state $x(k)$ is bounded.

For the systems in Equations (1) and (2), the augmented measurement equation of the centralized fusion system (CFS) is given a

$${\mathit{z}}^{(0)}(k)={\mathit{h}}_{}^{(0)}(x(k),k)+{\mathit{v}}^{(0)}(k)$$

(4)

where

$${\mathit{z}}^{(0)}(k)={[z_{}^{(1)}(k),z_{}^{(2)}(k),\cdots ,z_{}^{(L)}(k)]}^{\mathrm{T}}$$

(5)

$${\mathit{h}}^{(0)}(x(k),k)={[h_{}^{(1)}(x(k),k),h_{}^{(2)}(x(k),k),\cdots ,h_{}^{(L)}(x(k),k)]}^{\mathrm{T}}$$

(6)

$${\mathit{v}}^{(0)}(k)={[v_{}^{(1)}(k),v_{}^{(2)}(k),\cdots ,v_{}^{(L)}(k)]}^{\mathrm{T}}$$

(7)

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

$${\mathit{R}}^{(0)}=\mathrm{diag}({\sigma }_{v1}^2,{\sigma }_{v2}^2,\cdots ,{\sigma }_{vL}^2)$$

(8)

where $\mathrm{diag}(·)$ denotes a diagonal matrix.

For the systems in Equations (1) and (4), we obtained the centralized fusion PF combined with the particle filter. However, Equation (4), with a high dimension, will result in high computational costs, particularly in massive sensor networks. Therefore, it is important to find the equivalent or approximate fusion methods to reduce the computational cost.

Lemma 1. 

For the systems in Equations (1) and (2), if there are linear relationships between measurement functions $h_{}^{(j)}(x(k),k)$, $j=1,2,\cdots ,L$, that is to say, there is a intermediary function $\mathit{h}(x(k),k)\in {\Re }^{p\times 1}$ that satisfies $h_{}^{(j)}(x(k),k)={\mathit{H}}_{}^{(j)}(k)\mathit{h}(x(k),k)$ with matrix ${\mathit{H}}_{}^{(j)}(k)\in {\Re }^{1\times p}$, the compressed measurement function of the weighted measurement fusion system (WMFS) is given as [29]

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

(9)

$${\mathit{z}}^{(\mathrm{I})}(k)={[{\mathit{M}}^{\mathrm{T}}(k){\mathit{R}}^{(0)-1}\mathit{M}(k)]}^{-1}{\mathit{M}}^{\mathrm{T}}(k){({\mathit{R}}^{(0)})}_{}^{-1}{\mathit{z}}^{(0)}(k)$$

(10)

$${\mathit{v}}^{(\mathrm{I})}(k)={[{\mathit{M}}^{\mathrm{T}}(k){\mathit{R}}^{(0)-1}\mathit{M}(k)]}^{-1}{\mathit{M}}^{\mathrm{T}}(k){({\mathit{R}}^{(0)})}_{}^{-1}{\mathit{v}}^{(0)}(k)$$

(11)

$${\mathit{R}}^{(0)}{}_{}^{-1}={({\mathit{R}}^{(0)})}^{-1}$$

(12)

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

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

(13)

where $\mathit{M}(k)$, with full-column rank, and ${\mathit{H}}_{}^{(\mathrm{{\rm I}})}(k)$, with full-row rank, are the full rank decomposition matrices of matrix ${\mathit{H}}^{(0)}(k)={[{\mathit{H}}_{}^{(1)\mathrm{T}}(k),\cdots ,{\mathit{H}}_{}^{(L)\mathrm{T}}(k)]}^{\mathrm{T}}$, that is,

$${\mathit{H}}^{(0)}(k)=\mathit{M}(k){\mathit{H}}_{}^{(\mathrm{{\rm I}})}(k)$$

(14)

which can be computed by Hermite canonical form [29].

We assumed that the statistics of $w(k)$ and $v_{}^{(j)}(k)$, $j=1,2,\cdots ,L$ are known. In fact, we obtained the noise statistics through identification [35,36,37]. For time-invariant systems, we identified them offline and obtained the optimal weighted measurement fusion algorithms. For time-varying systems, we identified them online and obtained the asymptotic optimal adaptive weighted measurement fusion algorithm.

In order to determine the weighted measurement fusion based on the probabilistic method, we needed to know two things: the relationships among the noise statistics of all the sensors and the relationships among the measurement functions of all sensors. For systems with additive noises, if we knew the relationships between the measurement functions, the relationships among the measurement noise statistics could be determined. If a linear relationship exists among the measurement functions, whether the measurement functions themselves are linear or not, we could use the least square method to find the optimal compressed measurement. If a nonlinear relationship exists among the measurement functions, we could use intermediary functions to achieve the optimal compressed measurement. Unfortunately, finding the intermediary function is difficult because of the complexity and diversity of nonlinear functions.

For example, there are four sensors and their measurement functions are as follows:

$$\begin{gathered} z_{}^{(1)}(k)=1+x(k)+x_{}^2(k)+v_{}^{(1)}(k), z_{}^{(2)}(k)=2+x(k)+2x_{}^2(k)+x_{}^3(k)+v_{}^{(2)}(k), \\ {z}_{}^{(3)}(k)=3+2x(k)+3x_{}^2(k)+x_{}^3(k)+v_{}^{(3)}(k), z_{}^{(4)}(k)=4+x(k)+4x_{}^2(k)+3x_{}^3(k)+v_{}^{(4)}(k) \end{gathered}$$

(15)

where the variances of $v_{}^{(j)}(k),j=1\cdots 4$ are $R_{}^{(1)}=1$, $R_{}^{(2)}=2$, $R_{}^{(3)}=3$, and $R_{}^{(4)}=4$.

Set $\mathit{h}(x(k),k)={[\begin{array}{cccc}1& x(k)& x_{}^2(k)& x_{}^3(k)\end{array}]}_{}^{\mathrm{T}}$, then Equation (15) can be written as

$$\begin{gathered} z_{}^{(1)}(k)=[\begin{array}{cccc}1& 1& 1& 0\end{array}]h(x(k),k)+v_{}^{(1)}(k), z_{}^{(2)}(k)=[\begin{array}{cccc}2& 1& 2& 1\end{array}]h(x(k),k)+v_{}^{(2)}(k), \\ {z}_{}^{(3)}(k)=[\begin{array}{cccc}3& 2& 3& 1\end{array}]h(x(k),k)+v_{}^{(3)}(k), z_{}^{(4)}(k)=[\begin{array}{cccc}4& 1& 4& 3\end{array}]h(x(k),k)+v_{}^{(4)}(k) \end{gathered}$$

(16)

Let

$${\mathit{H}}^{(0)}=\left[\begin{array}{cccc}1& 1& 1& 0\\ 2& 1& 2& 1\\ 3& 2& 3& 1\\ 4& 1& 4& 3\end{array}\right]=\mathit{M}{\mathit{H}}_{}^{(\mathrm{{\rm I}})}=\left[\begin{array}{cc}1& 1\\ 2& 1\\ 3& 2\\ 4& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 1& 1\\ 0& 1& 0& -1\end{array}\right]$$

(17)

where $\mathit{M}$ is full-column rank, and ${\mathit{H}}_{}^{(\mathrm{{\rm I}})}$ is full-row rank. According to the weighted least squares criterion, the fused measurement function is

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

(18)

where ${\mathit{z}}^{(\mathrm{I})}(k)={[{\mathit{M}}^{\mathrm{T}}(k){({\mathit{R}}^{(0)})}_{}^{-1}\mathit{M}(k)]}^{-1}{\mathit{M}}^{\mathrm{T}}(k){({\mathit{R}}^{(0)})}_{}^{-1}{\mathit{z}}^{(0)}(k)=$$\left[\begin{array}{cccc}-0.3286& 0.1000& -0.0429& 0.3143\\ 0.8571& 0& 0.2857& -0.4286\end{array}\right]{\mathit{z}}^{(0)}(k)$, and the covariance of ${\mathit{v}}^{(\mathrm{I})}(k)$ is ${\mathit{R}}^{(\mathrm{I})}(k)={[{\mathit{M}}^{\mathrm{T}}(k){\mathit{R}}^{(0)-1}\mathit{M}(k)]}^{-1}=$$\left[\begin{array}{cc}0.5286& -0.8571\\ -0.8571& 1.7143\end{array}\right]$. The measurement function of WMF is two-dimensional, and that of the centralized fusion system is four-dimensional, so the WMF can compress measurement dimensions effectively and reduce the computational cost.

In the next section, we provide an approximation method to solve this problem.

3. Gauss–Hermite Approximation

Lemma 2. 

Let $y(x)$ be a determined function [30]. Assuming that there is an ensemble of $S$ points $\{x_i^{},i=1,\cdots ,S\}$ distributed uniformly in the interval $[a,b]$, there exists a point $y_i^{}$ such that $y_i^{}=y(x_i^{})$ for each point $x_i^{}$. Then, the approximation function $\overline{y}(x)$ of $y(x)$, by Gauss–Hermite folding reads

$$\overline{y}(x)=\frac{1}{\gamma \sqrt{\pi }}{\displaystyle \sum _{i=1}^S{y}_i^{}\Delta x_i^{}\exp \left\{-{\left(\frac{x-x_i^{}}{\gamma }\right)}_{}^2\right\}}{f}_n^{}\left(\frac{x-x_i^{}}{\gamma }\right)$$

(19)

where $\gamma$ is a coefficient, $\Delta x_i^{}=\frac{1}{2}(x_{i+1}^{}-x_{i-1}^{})$, and $f_n^{}(u)$ is the correction polynomials which can be decomposed into a series of Hermite polynomials:

$$f_n^{}(u)={\displaystyle \sum _{i=1}^n{C}_i^{}{H}_i^{}(u)}$$

(20)

$$C_i^{}=\frac{1}{2_{}^{i}i!}{H}_i^{}(0)$$

(21)

where $H_i^{}(u)={(-1)}_{}^i{e}_{}^{u_{}^2}{(e_{}^{-u_{}^2})}_{}^{(i)}$ represents Hermite polynomials [31], and $H_i^{}(0)$ is defined as

$$H_i^{}(0)=\{\begin{array}{cc}\begin{array}{l}1\\ 2_{}^n{(-1)}_{}^n(2n-1)!!\\ 0\end{array}& \begin{array}{l}i=0\\ i=2n\\ i=2n+1\end{array}\end{array}$$

(22)

From Equations (21) and (22), we obtain

$$C_i^{}=\{\begin{array}{ll}1\hfill & i=0\hfill \\ {(-1)}_{}^n\frac{(2n-1)!!}{2_{}^n(2n)!}\hfill & i=2n\hfill \\ 0\hfill & i=2n+1\hfill \end{array}$$

(23)

See the detailed proof in the literature [30,31,32,33].

Remark 1. 

Although $\gamma$ is an arbitrary coefficient related to $\Delta x_i^{}$, it was found that $\overline{y}(x)$ approximates better when $\gamma =1$ and $n=2$ or 4, for most elementary functions [30]. Thus, we used $\gamma =1$ and $n=2$ in this paper, which yielded

$$\overline{y}(x)=\frac{1}{\sqrt{\pi }}{\displaystyle \sum _{i=1}^S{y}_i^{}\Delta x_i^{}}{e}_{}^{-{(x-x_i^{})}_{}^2}(1.5-{(x-x_i^{})}_{}^2)$$

(24)

4. Universal WMF Based on Gauss–Hermite Approximation

If the function ${\displaystyle \sum _{i=1}^S{e}_{}^{-{(x-x_i^{})}_{}^2}(1.5-{(x-x_i^{})}_{}^2)}$ in Equation (24) is seen as the intermediary function $\mathit{h}(x(k),k)$, and $\frac{1}{\sqrt{\pi }}{\displaystyle \sum _{i=1}^S{y}_i^{}\Delta x_i^{}}$ is seen as ${\mathit{H}}_{}^{(j)}(k)$ in Lemma 2, then we can obtain the linear relationships among the measurement functions $h_{}^{(j)}(x(k),k)$ and obtain the following theorem using Lemma 2.

Based on Lemmas 1 and 2, we can obtain the following Theorem 1.

Theorem 1. 

For the systems in Equations (1) and (2), and from Equation (24), the approximate measurement equation of the weighted measurement fusion system is

$${\overline{\mathit{z}}}_{}^{(\mathrm{I})}(k)={\overline{\mathit{H}}}^{(\mathrm{I})}\left(\begin{array}{c}{e}_{}^{-{(x-x_1^{})}_{}^2}(1.5-{(x-x_1^{})}_{}^2)\\ e_{}^{-{(x-x_2^{})}_{}^2}(1.5-{(x-x_2^{})}_{}^2)\\ ⋮\\ e_{}^{-{(x-x_S^{})}_{}^2}(1.5-{(x-x_S^{})}_{}^2)\end{array}\right)+{\overline{\mathit{v}}}_{}^{(\mathrm{I})}(k)$$

(25)

We denoted

$${\overline{\mathit{H}}}^{(0)}=\frac{1}{\sqrt{\pi }}\left(\begin{array}{cccc}{y}_1^{(1)}\Delta x_1^{(1)}& y_2^{(1)}\Delta x_2^{(1)}& \cdots & y_S^{(1)}\Delta x_S^{(1)}\\ y_1^{(2)}\Delta x_1^{(2)}& y_2^{(2)}\Delta x_2^{(2)}& \cdots & y_S^{(2)}\Delta x_S^{(2)}\\ ⋮& ⋮& \cdots & ⋮\\ y_1^{(L)}\Delta x_1^{(L)}& y_2^{(L)}\Delta x_2^{(L)}& \cdots & y_S^{(L)}\Delta x_S^{(L)}\end{array}\right)$$

(26)

$${\overline{\mathit{z}}}^{(\mathrm{I})}(k)={[{\overline{\mathit{M}}}^{\mathrm{T}}{\mathit{R}}^{(0)-1}\overline{\mathit{M}}]}^{-1}{\overline{\mathit{M}}}^{\mathrm{T}}{\mathit{R}}^{(0)-1}{\mathit{z}}^{(0)}(k)$$

(27)

$${\overline{\mathit{v}}}^{(\mathrm{I})}(k)={[{\overline{\mathit{M}}}^{\mathrm{T}}{\mathit{R}}^{(0)-1}\overline{\mathit{M}}]}^{-1}{\overline{\mathit{M}}}^{\mathrm{T}}{\mathit{R}}^{(0)-1}{\mathit{v}}^{(0)}(k)$$

(28)

The covariance matrix of ${\mathit{v}}^{(\mathrm{I})}(k)$ is given by

$${\overline{\mathit{R}}}^{(\mathrm{I})}={[{\overline{\mathit{M}}}^{\mathrm{T}}{\mathit{R}}^{(0)-1}\overline{\mathit{M}}]}^{-1}$$

(29)

where $y_k^{(m)}$$(k=1,\cdots ,S)$ is the kth sample point of the mth sensor, $S$ is the sample point number, $\overline{\mathit{M}}$ and ${\overline{\mathit{H}}}^{(\mathrm{I})}$ are the full rank decomposition matrices of ${\overline{\mathit{H}}}^{(0)}$, $\overline{\mathit{M}}\in {\mathit{R}}_{}^{L\times r}$ is full-column rank, ${\overline{\mathit{H}}}_{}^{(\mathrm{{\rm I}})}\in {\mathit{R}}_{}^{r\times S}$ is full-row rank, $r=\mathrm{rank}({\overline{\mathit{H}}}^{(0)})$, and $r\le \min \{L,S\}$).

An approximate intermediary function,

$$\mathit{h}(x(k),k)=\left(\begin{array}{c}{e}_{}^{-{(x-x_1^{})}_{}^2}(1.5-{(x-x_1^{})}_{}^2)\\ e_{}^{-{(x-x_2^{})}_{}^2}(1.5-{(x-x_2^{})}_{}^2)\\ ⋮\\ e_{}^{-{(x-x_S^{})}_{}^2}(1.5-{(x-x_S^{})}_{}^2)\end{array}\right)$$

(30)

has been constructed using the Gauss–Hermite approximation method in Lemma 2. It establishes some linear relationships between local measurement functions. Therefore, it solves the restriction that the measurement functions must be linear relationships in Lemma 1.

From the above theorem, we can see that, if there is a nonlinear system with L sensors, the centralized fusion system in Equations (1) and (4) has a measurement function with L dimension and the WMF system in Equations (1) and (25) has a measurement function with r dimension. Because $r=\mathrm{rank}({\overline{\mathit{H}}}^{(0)})$ and $r\le \min \{L,S\}$, the computational cost of the WMF filter is less than that of the centralized fusion filter. After the sample point number $S$ is determined, the compression efficiency of the algorithm is significantly improved if there are a large number of sensors ($L>>S$ and S is a constant).

5. WMF-PF Based on Gauss–Hermite Approximation

In this section, we propose a WMF-PF algorithm based on the WMF system in Equations (1) and (25) with a low-dimension measurement.

5.1. WMF-PF Algorithm

The computational procedure of WMF-PF is given as follows:

1. Initialization: ${\widehat{x}}_{}^{(\mathrm{{\rm I}})(i)}(0|0)\sim p_{x_0^{}}^{}(x_0^{}),i=1,\cdots ,N_s^{}$;

2. State prediction particles:

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

(31)

where ${\zeta }_{}^{(\mathrm{{\rm I}})(i)}(k-1)$ is random number with the same distribution of the process noise $w(k)$;

3. Measurement prediction particles:

$${\widehat{\mathit{z}}}_{}^{(\mathrm{{\rm I}})(i)}(k|k-1)={\overline{\mathit{H}}}^{(\mathrm{I})}\mathit{h}({\widehat{x}}_{}^{(\mathrm{{\rm I}})(i)}(k|k-1),k)$$

(32)

4. The importance weight:

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

(33)

that is,

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

(34)

where ${\overline{\mathit{z}}}_{}^{(\mathrm{{\rm I}})}(k)$ is computed by Equation (27), and ${\overline{\omega }}_k^{(\mathrm{{\rm I}})(i)}$ is given by

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

(35)

5. Filtering:

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

(36)

and the filtering variance matrix is computed by

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

(37)

6. Resampling: In this paper, systematic sampling is used as the resampling method, i.e.,

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

(38)

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

We then returned to Step 2 and reiterated.

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

5.2. Time Complexity Analysis

From Equations (31)–(38), we found that the time complexity is determined by Equation (32). From Equation (32), the time complexity of centralized fusion PF (CF-PF) is $O(L_{}^2)$ and that of WMF-PF is $O(r_{}^2)$. Because $r\le \min \{L,S\}$, the time complexity of WMF-PF is less than that of CMF-PF. Particularly, when there are a large number of sensors ($L>>S$), the computational cost can be substantially reduced.

6. Simulation Examples

6.1. Model Description

Let us consider a classical 10-sensor nonlinear system [38]:

$$x(k)=\frac{x(k-1)}{4}+x(k-1)/(1+x{(k-1)}_{}^2)+2\cos (0.5(k-1))+w(k)$$

(39)

$$z_{}^{(j)}(k)=h_{}^{(j)}\left(x(k),k\right)+v_{}^{(j)}(k), j=1,\cdots ,10$$

(40)

Taking observability into account, the sensors selected were single-valued functions within the range of $x(k)$:

$$\begin{gathered} h_{}^{(1)}\left(x(k),k\right)=0.8x(k)+v_{}^{(1)}(k) \\ {h}_{}^{(2)}\left(x(k),k\right)=1.2x(k)+v_{}^{(2)}(k) \\ {h}_{}^{(3)}\left(x(k),k\right)=\exp (x(k)/3)+v_{}^{(3)}(k) \\ {h}_{}^{(4)}\left(x(k),k\right)=1.2\exp (x(k)/3)+v_{}^{(4)}(k) \\ {h}_{}^{(5)}\left(x(k),k\right)=0.05x{(k)}_{}^3+v_{}^{(5)}(k) \\ {h}_{}^{(6)}\left(x(k),k\right)=0.06x{(k)}_{}^3+v_{}^{(6)}(k) \\ {h}_{}^{(7)}\left(x(k),k\right)=5\sin (0.1\pi x(k))+v_{}^{(7)}(k) \\ {h}_{}^{(8)}\left(x(k),k\right)=6\sin (0.1\pi x(k))+v_{}^{(8)}(k) \\ {h}_{}^{(9)}\left(x(k),k\right)=5\arctan (0.1\pi x(k))+v_{}^{(9)}(k) \\ {h}_{}^{(10)}\left(x(k),k\right)=6\arctan (0.1\pi x(k))+v_{}^{(10)}(k) \end{gathered}$$

(41)

The $w(k)~\mathrm{U}(0,1)$ is uniformly distributed, and $v_{}^{(j)}(k)$, $j=1,\cdots ,10$ are uncorrelated Gaussian noises with variance ${\sigma }_{vj}^2={(0.5+0.01j)}_{}^2$. The initial state is $x(0)=0$.

6.2. Gauss–Hermite Approximation

Because the state $x(k)$ ranges from –3 to 4, we chose $S=10$ sampling points ($x_i^{}=-4,-3,\cdots ,5$) to approximate, and the corresponding coefficients were $\gamma =1$, $\Delta x_i^{}=1$, and $n=2$. Their mean square errors (MSEs) are shown in

Table 1. Mean square errors.

Sensor Functions	$h_{}^{(1)}(\cdot )$	$h_{}^{(2)}(\cdot )$	$h_{}^{(3)}(\cdot )$	$h_{}^{(4)}(\cdot )$	$h_{}^{(5)}(\cdot )$	$h_{}^{(6)}(\cdot )$	$h_{}^{(7)}(\cdot )$	$h_{}^{(8)}(\cdot )$	$h_{}^{(9)}(\cdot )$	$h_{}^{(10)}(\cdot )$
MSEs using Gauss–Hermite	0.0032	0.0017	0.0010	0.0014	0.0029	0.0042	0.0009	0.0013	0.0010	0.0015
MSEs using McLaughlin series	0	0	0.0258	0.0371	0.9621	1.3854	0.2286	0.3292	0.3963	0.5707

, and the approximation curves are shown in Figure 2.

To compare with other approximate methods, we introduced the approximation method using a McLaughlin series. We used a third-order McLaughlin series to approximate the nonlinear functions $h_{}^{(j)}\left(x(k),k\right)$. The MSEs of the approximation algorithm, using a McLaughlin series, are also shown in Table 1. From Table 1, we can see that the MSEs of the approximation algorithm using a McLaughlin series are larger than that when using Gauss–Hermite approximation.

6.3. Estimation Using WMF-PF

From the above experiments, we see that the approximation effect is good when using the above coefficients. Next, we established the measurement equation and fusion matrix of WMF. As the intermediary function $\mathit{h}(x(k),k)$ is ${\left(\begin{array}{ccc}{e}_{}^{-{(x-x_1^{})}_{}^2}(1.5-{(x-x_1^{})}_{}^2),& \cdots ,& e_{}^{-{(x-x_{10}^{})}_{}^2}(1.5-{(x-x_{10}^{})}_{}^2)\end{array}\right)}_{}^{\mathrm{T}}$, from Equations (24) and (25), the coefficient matrices ${\overline{\mathit{H}}}_{}^{(0)}$, and its full rank decomposition matrices $\overline{\mathit{M}}$ and ${\overline{\mathit{H}}}_{}^{(\mathrm{{\rm I}})}$, can be computed as follows:

$$\begin{gathered} {\overline{\mathit{H}}}_{}^{(0)}=\left[\begin{array}{cccccccccc}-1.8054& -1.3541& -0.9027& -0.4514& 0& 0.4514& 0.9027& 1.3541& 1.8054& 2.2568\\ -2.7081& -2.0311& -1.3541& -0.6770& 0& 0.6770& 1.3541& 2.0311& 2.7081& 3.3851\\ 0.1487& 0.2076& 0.2897& 0.4043& 0.5642& 0.7874& 1.0989& 1.5336& 2.1403& 2.9871\\ 0.1785& 0.2491& 0.3476& 0.4851& 0.6770& 0.9449& 1.3187& 1.8404& 2.5684& 3.5845\\ -1.8054& -0.7617& -0.2257& -0.0282& 0& 0.0282& 0.2257& 0.7617& 1.8054& 3.5262\\ -2.1665& -0.9140& -0.2708& -0.0339& 0& 0.0339& 0.2708& 0.9140& 2.1665& 4.2314\\ -2.6829& -2.2822& -1.6581& -0.8717& 0& 0.8717& 1.6581& 2.2822& 2.6829& 2.8209\\ -3.2195& -2.7386& -1.9897& -1.0461& 0& 1.0461& 1.9897& 2.7386& 3.2195& 3.3851\\ -2.5350& -2.1321& -1.5825& -0.8587& 0& 0.8587& 1.5825& 2.1321& 2.5350& 2.8319\\ -3.0420& -2.5585& -1.8990& -1.0304& 0& 1.0304& 1.8990& 2.5585& 3.0420& 3.3983\end{array}\right] \\ \overline{\mathit{M}}=\left[\begin{array}{ccccc}-1.8054& -1.3541& -0.9027& -0.4514& 0\\ -2.7081& -2.0311& -1.3541& -0.6770& 0\\ 0.1487& 0.2076& 0.2897& 0.4043& 0.5642\\ 0.1785& 0.2491& 0.3476& 0.4851& 0.6770\\ -1.8054& -0.7617& -0.2257& -0.0282& 0\\ -2.1665& -0.9140& -0.2708& -0.0339& 0\\ -2.6829& -2.2822& -1.6581& -0.8717& 0\\ -3.2195& -2.7386& -1.9897& -1.0461& 0\\ -2.5350& -2.1321& -1.5825& -0.8587& 0\\ -3.0420& -2.5585& -1.8990& -1.0304& 0\end{array}\right] \\ {\overline{\mathit{H}}}_{}^{(\mathrm{{\rm I}})}=\left(\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& -1& -6.0696\\ 0& 1& 0& 0& 0& 0& 0& -1& 0& 15.5971\\ 0& 0& 1& 0& 0& 0& -1& 0& 0& -21.6922\\ 0& 0& 0& 1& 0& -1& 0& 0& 0& 15.8716\\ 0& 0& 0& 0& 1& 2.1121& 2.4612& 3.0862& 4.0573& 0.9212\end{array}\right). \end{gathered}$$

(42)

Using the WMF-PF algorithm, we obtained the state estimate for the system in Equations (39) and (40). The curves of the true values and their estimates, using WMF-PF based on Gauss–Hermite approximation, are shown in Figure 3. The tracking performance is optimal.

6.4. Analysis

In order to compare our findings with other fusion algorithms, we introduced a kind of distributed fusion method, called a fast covariance intersection (CI) fusion algorithm [39]. Its calculation process is

$${\widehat{x}}_{k|k}^{(C\mathrm{I})}={\displaystyle \sum _{i=1}^L{\omega }_i^{}{\mathit{P}}_{C\mathrm{I}}^{}{\mathit{P}}_i^{-1}{\widehat{x}}_{k|k}^{(j)}}$$

(43)

where ${\widehat{\mathit{x}}}_{k|k}^{(C\mathrm{I})}$ is the fusion estimate by CI, ${\widehat{\mathit{x}}}_{k|k}^{(j)},i=1,\cdots ,L$ are the estimates of subsystems, ${\mathit{P}}_i^{},i=1,\cdots ,L$ are the filter error variance matrices of subsystems, and

$${\mathit{P}}_{CI}^{-1}={\displaystyle \sum _{i=1}^L{\omega }_i^{}{\mathit{P}}_i^{-1}}$$

(44)

$${\omega }_j^{}=\frac{\mathrm{trace}({\mathit{P}}_j^{-1})}{{\displaystyle \sum _{i=1}^L\mathrm{trace}({\mathit{P}}_i^{-1})}}$$

(45)

Using the CI fusion algorithm and the PF algorithm, we obtained the covariance intersection PF (CI-PF). This fusion algorithm is simple, robust, and flexible. However, its accuracy is lower than WMF. We introduced an evaluation indicator called the accumulated mean square error (AMSE) [29]:

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

(46)

where ${\widehat{x}}_j^{}(t|t)$ is the jth-time Monte Carlo experiment at time t. The AMSE curves of local PFs (LF1–LF7), the WMF-PF, and the CI-PF are shown in Figure 4 with 20 Monte Carlo experiments. From Figure 4, the WMF-PF based on Gauss–Hermite approximation is shown to have better accuracy than local PF and CI-PF.

In addition, we simulated the AMSE curves of WMF-PF under $S=8$,$x_i^{}=-3.6,-2.4,\cdots ,4.8$, $\Delta x_i^{}=1.2$, $n=2$, and $\gamma =1$ and WMF-PF under $S=10$, $\Delta x_i^{}=1$, $n=2$, and $\gamma =0.9$. To distinguish them with the above WMF-PF, we named them WMF-PF-1 and WMF-PF-2. The AMSE curves of WMF-PF, WMF-PF-1, and WMF-PF-2 are shown in Figure 5. We can see that, as the interval $\Delta x_i^{}$ becomes larger, the computational cost is reduced, but the estimation accuracy declines, and a reasonable parameter $\gamma$ can improve the estimation accuracy. The AMSE curves of WMF-PF and CF-PF are shown in Figure 6. We can see that the accuracy of WMF-PF approximates that of CF-PF.

Next, we compare the computational cost of WMP-PF and CF-PF. Because ${\mathit{H}}_{}^{(\mathrm{{\rm I}})}$ is a $5\times 10$ matrix, the time complexity of WMP-PF is $O(5_{}^2)$. However, ${\mathit{H}}_{}^{(0)}$ is a $10\times 10$ matrix and its time complexity is $O({10}_{}^2)$. Therefore, the computational cost of WMF-PF is obviously lower than that of CF-PF.

In summary, the proposed WMP-PF, based on Gauss–Hermite approximation, is more accurate compared with local PFs, and has a lower computation cost when compared with CF-PF.

7. Conclusions

A general WMF algorithm is presented here for nonlinear multisensory systems based on the Gauss–Hermite approximation of nonlinear functions and the weighted least square method. An augmented high-dimension measurement from all sensors is compressed to a low-dimension one. Combined with the particle filter, the nonlinear weighted measurement fusion PF (WMF-PF) is presented. It can handle the nonlinear fusion estimation problem, and has reasonable accuracy and a reduced computational cost compared to centralized fusion PF. The proposed WMF algorithm can be used in systems of large-scale sensors, which can significantly reduce the computational cost and improve the real-time feature.