An Adaptive Filter for Nonlinear Multi-Sensor Systems with Heavy-Tailed Noise

Abstract

Aiming towards state estimation and information fusion for nonlinear systems with heavy-tailed measurement noise, a variational Bayesian Student’s t-based cubature information filter (VBST-CIF) is designed. Furthermore, a multi-sensor variational Bayesian Student’s t-based cubature information feedback fusion (VBST-CIFF) algorithm is also derived. In the proposed VBST-CIF, the spherical-radial cubature (SRC) rule is embedded into the variational Bayes (VB) method for a joint estimation of states and scale matrix, degree-of-freedom (DOF) parameter, as well as an auxiliary parameter in the nonlinear system with heavy-tailed noise. The designed VBST-CIF facilitates multi-sensor fusion, allowing to derive a VBST-CIFF algorithm based on multi-sensor information feedback fusion. The performance of the proposed algorithms is assessed in target tracking scenarios. Simulation results demonstrate that the proposed VBST-CIF/VBST-CIFF outperform the conventional cubature information filter (CIF) and cubature information feedback fusion (CIFF) algorithms.

1. Introduction

The Kalman filter (KF) is an optimal state estimator for linear state-space systems [1,2,3]. It is widely used, owing to its optimality, in many applications like, e.g.,  localization, control, target tracking, and signal processing [4,5,6,7,8,9,10,11,12,13,14,15,16,17]. In reality, however, systems are usually characterized by strong non-linearities which make the conventional KF inappropriate. To this end, nonlinear filtering methods have been developed like, e.g., function approximation, deterministic sampling, and Monte Carlo estimation methods [18,19,20]. The function approximation method adopted by the extended Kalman filter (EKF) approximates the nonlinear system equations through truncated Taylor expansions [21]. However, the Jacobian matrix is not computable for systems with non-smooth non-linearities [21]. As for the deterministic sampling method, its main representatives are the unscented Kalman filter (UKF), cubature Kalman filter (CKF), etc. [22,23]. Unfortunately, the state error covariance matrix (SECM) in UKF may result non-positive definite for high-dimensional systems, possibly leading to filter divergence [23]. To overcome the drawbacks of UKF, Arasaratnam and Haykin proposed CKF in 2009 [23]. CKF approximates the posterior probability density function (PDF) by means of the spherical-radial cubature (SRC) rule, thus resulting in improved filtering accuracy for high-dimensional systems [24]. Moreover, compared to the Monte Carlo estimation approach exploited by particle filters (PFs), CKF is not affected by particle depletion issues.

Recall that the KF (and also EKF, UKF, and CKF) can also be implemented in the alternative information filter (IF) form wherein the inverse covariance (information) matrix is propagated instead of the covariance. In this respect, the IF form greatly simplifies the measurement update compared to the traditional covariance form [25]. In this way, it facilitates multi-sensor fusion, wherein measurements from different sensors are fused via directly adding information contributions to the information matrix and vector [26]. Considering the above-stated superiority in the multi-sensor case, the cubature information filter (CIF) will be adopted as a filtering approach in this paper.

Unfortunately, the existing CIF assumes the known and constant measurement noise covariance matrix (MNCM) [27]. However, due to the complex environment where the sensor is located, it is prone to measurement outliers. This might induce a heavy-tailed measurement noise [28]. Actually, an outlier is characterized by the abnormality of its measurement value; correspondingly, outliers might give rise to a heavy-tailed distribution of the measurement noise. There are many practical applications where there is heavy-tailed noise [29,30,31,32]. For instance, in vision-aided inertial navigation, either computer vision data contaminated by outliers or sonar data corrupted by phase noise may lead to heavy-tailed noise [29]. In the detection of FM signals, heavy-tailed noise can be produced by thunderstorms or an iceberg breakup in under-ice acoustics [30]. In target tracking scenarios, measurement outliers come from unreliable sensors and/or targets [31], while in target detection with synthetic aperture radar, heavy-tailed noise occurs due to clutter and buried or obscured observed targets [32], etc. Compared to Gaussian noise (hypothesized by conventional CIF), heavy-tailed noise is characterized by greater uncertainty in its distribution tails, yielding the heavy-tailed shape of the PDF [33]. Under the above circumstance, the filtering accuracy of CIF may deteriorate and filter divergence may occur, due to the discrepancy between the assumed noise statistics and the true one.

To tackle the filtering problem under heavy-tailed noise, several adaptive methods have been developed, including generalized maximum likelihood [34], multi-model [35], and joint estimation [36] methods. Specifically, a representative of generalized maximum likelihood estimation is the Huber-based filter, which minimizes the combined $l_1$ and $l_2$ norms [37] and, however, leads to limited estimation performance [38]. An alternative is the multi-model approach, which models the unknown noise as a random parameter switching according to a Markov Chain and is able to adaptively estimate the system state [35]. Unfortunately, it is limited by the selected sets of models. The greater is the deviation between the selected models and real noise model, the worse the estimation performance of this method [36]. Furthermore, its computational complexity will increase significantly as the number of unknown parameters and selected models increases. The joint estimation method includes expectation-maximization (EM) and variational Bayes (VB) approaches [36]. The EM approach identifies and estimates system states as well as hidden variables through expectation (E) and maximization (M) steps. However, it is difficult to obtain the analytical solution for E and M estimates in the case of high-dimensional systems.

Conversely, another joint estimation method, the VB approach, can avoid the aforementioned drawbacks of the EM approach for high-dimensional systems. By searching an approximation for the true joint distribution, the VB approach is widely used in the adaptive joint estimation of noise statistics [27,29,38,39,40,41,42]. Agamennoni et al. proposed a robust Kalman filter by exploiting a flexible model [39] and also introduced a structured variational approximation approach for heavy-tailed noise [40]. However, the use of these filters is restricted to linear systems. Huang et al. [38] also presented a linear Student’s t (ST) filter. By modeling heavy-tailed noise with ST distribution, the system state as well as noise statistics are adaptively estimated through VB iterations. Unfortunately, this approach is only applicable to linear systems, too. Although there has been some research work on nonlinear systems in [29,41,42], the proposed filters rely on the covariance filter form, instead of the IF form, in order to jointly estimate system state, SECM, and noise statistics, thus resulting into complex computation. Moreover, the current VB approach cannot be embedded into nonlinear CIF directly, since the computation of noise parameters like the scale matrix involves a nonlinear function and, therefore, requires the use of the SRC rule. Hence, to jointly estimate states and parameters of the noise statistics (especially the scale matrix) via the information filter framework is still an open problem in nonlinear systems with heavy-tailed noise. In addition, the existing research work is only able to deal with single-sensor estimation problems. For nonlinear multi-sensor systems with heavy-tailed noise, the current state-of-art does not allow to estimate and fuse the states as well as noise statistics.

To conclude, there still exist the following issues to be addressed for the state estimation of nonlinear multi-sensor systems. On the one hand, the nonlinear information filter cannot be embedded into the current VB framework, since the scale matrix cannot be directly computed in the same way as for linear or nonlinear systems in the covariance filter form. On the other hand, it is not suited to multi-sensor systems. There are different heavy-tailed noise signals when multiple sensors are working in a multi-sensor system. This makes the conventional filter for the single-sensor case fail to fuse different estimated states. Thus, for nonlinear systems with heavy-tailed noise, especially systems with multiple sensors, it is still an open issue how to jointly estimate system state and noise statistics by combining information from different sensors.

To solve the above mentioned problems, an adaptive variational Bayesian Student’s t-based cubature information filter (VBST-CIF) algorithm is developed. Furthermore a variational Bayesian Student’s t-based cubature information feedback fusion (VBST-CIFF) algorithm is proposed for nonlinear multi-sensor systems with heavy-tailed noise. Key contributions of this paper are the following.

(1)

The information filter form is adopted for simplified computation and facilitation of multi-sensor fusion.

(2)

A novel VBST-CIF algorithm for nonlinear systems with heavy-tailed noise is proposed. The SRC rule is introduced into the VB approach for joint estimation of states and noise statistics, by employing the ST distribution for modeling heavy-tailed noise.

(3)

The proposed VBST-CIF algorithm is further extended to multi-sensor fusion, deriving a novel VBST-CIFF algorithm. The proposed VBST-CIFF algorithm facilitates multi-sensor fusion in nonlinear systems with different heavy-tailed measurement noise statistics for each sensor.

(4)

Simulation and experimental results show that the proposed VBST-CIF/VBST-CIFF algorithms outperform conventional CIF and cubature information feedback fusion (CIFF) algorithms in scenarios concerning nonlinear systems with heavy-tailed noise.

The rest of the paper is organized as follows. First, the problem of state estimation for nonlinear systems with heavy-tailed measurement noise is formulated in Section 2. Then, the SRC rule and ST distribution are introduced in Section 3, wherein the ST distribution is utilized to model heavy-tailed noise and the VBST-CIF algorithm is also derived. Furthermore, a VBST-CIFF algorithm is provided for multi-sensor fusion of nonlinear systems with heavy-tailed noise in Section 4. In Section 5, the proposed VBST-CIF and VBST-CIFF algorithms are tested in nonlinear target tracking scenarios. Conclusions are provided in Section 6.

2. Problem Formulation

Consider the following model:

$$\begin{array}{cc}\hfill x_k& =f\left(x_{k-1}\right)+w_{k-1}\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill y_k& =h\left(x_k\right)+v_k\hfill \end{array}$$

(2)

where: k denotes the discrete time index; $x_k$ and $y_k$ are the state and measurement vector, respectively; f and h denote state transition and, respectively, measurement function; $w_k$ is a zero-mean Gaussian process noise with covariance $Q_k$, i.e., $w_k\sim \mathcal{N}(0,Q_k)$, while $v_k$ is heavy-tailed measurement noise; $\mathcal{N}(\mu ,\Sigma )$ denotes a Gaussian random variable with mean $\mu$ and variance $\Sigma$. Moreover, it is assumed that $w_i$ and $v_j$ are uncorrelated for any i and j.

Conventional CIF can deal with state estimation in the Gaussian case. It has been introduced in [43] under the assumption that measurement noise has Gaussian distribution. Unfortunately, for systems (1) and (1) affected by heavy-tailed measurement noise, it may lead to non-accurate estimation because of the deviation between the assumed noise model and the actual one.

To this end, Huang et al. [38] have proposed an adaptive filter for linear systems. Nevertheless, such a filter is limited to linear single-sensor systems and is not suited to multi-sensor nonlinear systems. Although there are some methods for nonlinear state estimation, how to calculate the matrix parameter of heavy-tailed noise is still an open issue. Specifically, the posterior distribution parameters of the heavy-tailed noise contain a matrix related to the measurement matrix in traditional linear systems. Unfortunately, such a matrix cannot be directly obtained for nonlinear systems, thus implying that the analytical solution for joint estimation of system state as well as the noise parameters is not possible. On the other hand, for multi-sensor systems, multiple noise matrices need to be estimated, one for each sensor s. Hence, how to estimate these parameters together with the system state and fuse them still remains another open issue.

To conclude, the following problems still need to be addressed for the state estimation of a nonlinear system with heavy-tailed measurement noise.

(1)

The conventional CIF assumes Gaussian measurement noise distribution. For systems with heavy-tailed noise, CIF is not able to estimate states and the noise statistics simultaneously.

(2)

The current VB approach based on the conventional KF framework is not suited to nonlinear systems. In particular, the unknown noise matrix parameter is difficult to obtain in nonlinear systems;

(3)

For multi-sensor systems with measurement noise signals of the various sensors possibly having different statistics, it is an open issue to estimate and fuse states as well as noise statistics.

Therefore, a novel adaptive filter for a multi-sensor nonlinear system with heavy-tailed measurement noise will be proposed in this paper. Results in this work will help to deal with state estimation and data fusion problems for nonlinear systems with heavy-tailed measurement noise.

3. Variational Bayesian Student’s t-Based Cubature Information Filter

3.1. Spherical-Radial Cubature Rule

The main ingredient of the CIF for nonlinear systems is the SRC rule. Based on the prior mean and covariance, an initial set of sampling points are selected and then propagated through the nonlinear function, thus providing transformed sampling points. Then, by weighting these transformed sampling points, the posterior mean and variance can be obtained. Specifically, the SRC rule operates as follows.

Nonlinear filtering involves integrals of this form:

$$\begin{array}{cc}\hfill Int\left(I\right)& ={\int }_{{\mathbb{R}}^n}I\left(x\right)exp(-x^{\mathrm{T}}x)\mathrm{d}x\hfill \end{array}$$

(3)

where $I(·)$ is a nonlinear function and ${\mathbb{R}}^n$ denotes the integration domain. Let $x=rg$ with $g^{\mathrm{T}}g=1$ and $r\in [0,\infty ]$, so that $x^{\mathrm{T}}x=r^2$, where g is a direction vector and r is the radius. Then, integration in (3) can be rewritten as:

$$\begin{array}{cc}\hfill Int\left(I\right)& ={\int }_0^{\infty }{\int }_{\Xi }I\left(rg\right)r^{n-1}exp(-r^2)\mathrm{d}\sigma \left(g\right)\mathrm{d}r\hfill \end{array}$$

(4)

where $\Xi =\{g\in {\mathbb{R}}^n|g^{\mathrm{T}}g=1\}$ denotes an n-dimensional unit sphere surface and $\sigma (·)$ is an element on $\Xi$. Thus, the integration in (4) can be transformed into a spherical integration:

$$\begin{array}{cc}\hfill Sph\left(r\right)& ={\int }_{\Xi }I\left(rg\right)\mathrm{d}\sigma \left(g\right)\hfill \end{array}$$

(5)

and a radial integration:

$$\begin{array}{cc}\hfill Int& ={\int }_0^{\infty }Sph\left(r\right)r^{n-1}exp(-r^2)\mathrm{d}r.\hfill \end{array}$$

(6)

Hence, for a Gaussian integration, it turns out that:

$$\begin{array}{cc}\hfill {\int }_{{\mathbb{R}}^n}I\left(x\right)\mathcal{N}(x;\mu ,\Sigma )\mathrm{d}x& \cong \frac{1}{2n}\sum _{i=1}^{2n}I(\sqrt{\Sigma }{r}_i+\mu )\hfill \end{array}$$

(7)

where: $\sqrt{\Sigma }$ can be obtained by performing Cholesky decomposition of $\Sigma$; $r_i=\sqrt{n}{\left\{e\right\}}_i$, ${\left\{e\right\}}_i$ being the i-th column of $\left\{e\right\}$, with, for example, $\left\{e\right\}=\left\{\left[\begin{array}{c}1\hfill \\ 0\hfill \end{array}\right],\left[\begin{array}{c}0\hfill \\ 1\hfill \end{array}\right],\left[\begin{array}{c}\hfill -1\\ \hfill 0\end{array}\right],\left[\begin{array}{c}\hfill 0\\ \hfill -1\end{array}\right]\right\}$ if $\left\{e\right\}\in {\mathbb{R}}^2$.

3.2. Student’s t Distribution and Time Update

The ST distribution [38] can be described as:

$$\begin{array}{cc}\hfill St(\alpha ;0,M,\kappa )& ={\int }_0^{+\infty }\mathcal{N}(\alpha ;0,M/\gamma )G\left(\gamma ;\frac{\kappa }{2},\frac{\kappa }{2}\right)\mathrm{d}\gamma \hfill \end{array}$$

(8)

where: $St(·;\mu ,M,\kappa )$ represents the ST PDF with mean $\mu$, scale matrix M, and degree of freedom (DOF) parameter $\kappa$; $\gamma$ denotes an auxiliary parameter; $\mathcal{N}(·;\mu ,M)$ is the Gaussian PDF with mean $\mu$ and variance M; $G(·;a,b)$ denotes the Gamma PDF with parameters a and b.

Since the measurement noise has heavy-tailed characteristic, it can be modeled as an ST distribution with zero mean, scale matrix $R_k$, and DOF ${\kappa }_k$ [38] as parameters, i.e.,

$$\begin{array}{cc}\hfill St(v_k;0,R_k,{\kappa }_k)& ={\int }_0^{+\infty }\mathcal{N}(v_k;0,R_k/{\gamma }_k)G\left({\gamma }_k;\frac{{\kappa }_k}{2},\frac{{\kappa }_k}{2}\right)\mathrm{d}{\gamma }_k\hfill \end{array}$$

(9)

where ${\gamma }_k$ denotes the auxiliary parameter at time k.

Prior distributions of $R_k$, ${\kappa }_k$, as well as ${\gamma }_k$, are selected as inverse Wishart and, respectively, Gamma distribution, i.e.,

$$\begin{array}{cc}\hfill p\left(R_{k-1}\right)& =IW(R_{k-1};{\widehat{\delta }}_{k-1|k-1},{\widehat{\Delta }}_{k-1|k-1})\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill p\left({\gamma }_{k-1}\right)& =G({\gamma }_{k-1};{\widehat{a}}_{k-1|k-1},{\widehat{b}}_{k-1|k-1})\hfill \end{array}$$

(11)

$$\begin{array}{cc}\hfill p\left({\kappa }_{k-1}\right)& =G({\kappa }_{k-1};{\widehat{\varphi }}_{k-1|k-1},{\widehat{\Phi }}_{k-1|k-1})\hfill \end{array}$$

(12)

where $IW(·)$ denotes the inverse Wishart distribution [44] and ${\widehat{\delta }}_{k-1|k-1}$, ${\widehat{\Delta }}_{k-1|k-1}$ are its parameters estimated at time $k-1$. Likewise, ${\widehat{a}}_{k-1|k-1}$, ${\widehat{b}}_{k-1|k-1}$ and ${\widehat{\varphi }}_{k-1|k-1}$, ${\widehat{\Phi }}_{k-1|k-1}$ are the corresponding estimated distribution parameters of ${\gamma }_{k-1}$ and, respectively, ${\kappa }_{k-1}$.

Time updating is performed by applying a forgetting factor $\tau$ to the prior estimates [44], i.e.,

$$\begin{array}{cc}\hfill {\widehat{\delta }}_{k|k-1}& =\tau ({\widehat{\delta }}_{k-1|k-1}-n_z-1)+n_z+1\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill {\widehat{\Delta }}_{k|k-1}& =\tau {\widehat{\Delta }}_{k-1|k-1}\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill {\widehat{\varphi }}_{k|k-1}& =\tau {\widehat{\varphi }}_{k-1|k-1}\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill {\widehat{\Phi }}_{k|k-1}& =\tau {\widehat{\Phi }}_{k-1|k-1}\hfill \end{array}$$

(16)

where: $n_z$ denotes the dimension of the measurement vector $y_k$; $k|k-1$ stands for prediction from time $k-1$ to k, while $k-1|k-1$ stands for the filtered estimation at $k-1$.

Suppose that the prior state estimate and SECM are ${\widehat{x}}_{k-1|k-1}$ and $P_{k-1|k-1}$, respectively. By applying the SRC rule to the prior information, we have:

$$\begin{array}{cc}\hfill P_{k-1|k-1}& =T_{k-1}{T}_{k-1}^{\mathrm{T}}\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill {\chi }_{i,k-1}& =T_{k-1}{r}_i+{\widehat{x}}_{k-1|k-1}(i=1,2,\cdots ,2n_x)\hfill \end{array}$$

(18)

$$\begin{array}{cc}\hfill {\Psi }_{i,k-1}& =f\left({\chi }_{i,k-1}\right)\hfill \end{array}$$

(19)

where: $T_{k-1}$ is a matrix obtained by performing the Cholesky decomposition of $P_{k-1|k-1}$; $r_i$ represents the i-th column of $\left\{e\right\}$ in the SRC rule; ${\chi }_{i,k-1}$ denotes the i-th sampling point; $n_x$ is the dimension of $x_k$; ${\Psi }_{i,k-1}$ denotes the corresponding transformed i-th sampling obtained by propagating ${\chi }_{i,k-1}$ through $f(·)$. To this end, the predicted ${\widehat{x}}_{k|k-1}$ as well as $P_{k|k-1}$ are obtained as:

$$\begin{array}{cc}\hfill {\widehat{x}}_{k|k-1}& =\frac{1}{2n_x}\sum _{i=1}^{2n_x}{\Psi }_{i,k-1}\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill P_{k|k-1}& =\frac{1}{2n_x}\sum _{i=1}^{2n_x}{\Psi }_{i,k-1}{\Psi }_{i,k-1}^{\mathrm{T}}-{\widehat{x}}_{k|k-1}{\widehat{x}}_{k|k-1}^{\mathrm{T}}+Q_{k-1}.\hfill \end{array}$$

(21)

Thus, the predicted information matrix and state vector, $Z_{k|k-1}$ and ${\zeta }_{k|k-1}$, are obtained through

$$\begin{array}{cc}\hfill Z_{k|k-1}& =P_{k|k-1}^{-1}\hfill \end{array}$$

(22)

$$\begin{array}{cc}\hfill {\zeta }_{k|k-1}& =Z_{k|k-1}{\widehat{x}}_{k|k-1}.\hfill \end{array}$$

(23)

3.3. Variational Bayesian Student’s t-Based Cubature Information Filter (VBST-CIF)

The joint posterior PDF of system state $x_k$, scale matrix $R_k$, DOF parameter ${\kappa }_k$, and  auxiliary parameter ${\gamma }_k$, denoted as $p(x_k,R_k,{\kappa }_k,{\gamma }_k|y_{1:k})$, needs to be computed. However, for the nonlinear system (1) and (), an analytical solution for $p(x_k,R_k,{\kappa }_k,{\gamma }_k|y_{1:k})$ is impossible to obtain. For the purpose of obtaining an approximate solution, the VB approach is introduced.

The key idea of the VB approach is to get an approximate posterior distribution $q(·)$ via minimizing the Kullback–Leibler divergence (KLD) between the approximate $q(·)$ and true one $p(·)$ [36], i.e.,

$$\begin{array}{cc}\hfill q^{*}& =\arg \min KLD\left(q\right|\left|p\right)\hfill \end{array}$$

(24)

where the KLD is defined as:

$$\begin{array}{cc}\hfill KLD\left(q\right|\left|p\right)& \equiv \int q(·)\log \frac{q(·)}{p(·)}\mathrm{d}(·).\hfill \end{array}$$

(25)

Hence, the problem can be transformed into searching an approximate $q(·)$ that minimizes the KLD between itself and the true $p(·)$, i.e.,

$$\begin{array}{cc}\hfill q^{*}(·)& =\arg \min \left(q(·)\right|\left|p(x_k,R_k,{\kappa }_k,{\gamma }_k|y_{1:k})\right).\hfill \end{array}$$

(26)

The joint posterior distribution of the parameters to be estimated is approximated via the following factored form [36]:

$$\begin{array}{c}\hfill p(x_k,R_k,{\kappa }_k,{\gamma }_k|y_{1:k})\cong q_x\left(x_k\right)q_R\left(R_k\right)q_{\kappa }\left({\kappa }_k\right)q_{\gamma }\left({\gamma }_k\right)\end{array}$$

(27)

where $q_x(·)$, $q_R(·)$, $q_{\kappa }(·)$, and $q_{\gamma }(·)$ represent approximate posterior PDFs for $x_k$, $R_k$, ${\kappa }_k$, and ${\gamma }_k$, respectively. By applying logarithm to (27), it can be deduced that:

$$\begin{array}{cc}\hfill \log q\left(\xi \right)& =E_{{\Omega }^{(-\xi )}}\left(\log p(\Omega ,y_{1:k})\right)+c_{\xi }\hfill \\ \hfill \Omega & \equiv \left\{x_k,R_k,{\kappa }_k,{\gamma }_k\right\}\hfill \end{array}$$

(28)

where: $\Omega$ is the set of parameters $(x_k,R_k,{\kappa }_k,{\gamma }_k)$ that need to be estimated; $\xi$ is an element of $\Omega$; ${\Omega }^{(-\xi )}$ denotes the complementary set of $\xi$ in $\Omega$; $E(·)$ denotes the expectation operator; $c_{\xi }$ is a quantity that is independent of $\xi$.

By exploiting the hierarchical Gaussian form in [38], it can be obtained that:

$$\begin{array}{cc}\hfill & p(x_k,R_k,{\kappa }_k,{\gamma }_k,y_{1:k})\hfill \\ \hfill & =\mathcal{N}\left(y_k;h\left(x_k\right),\frac{R_k}{{\gamma }_k}\right)\mathcal{N}\left(x_k;{\widehat{x}}_{k|k-1},P_{k|k-1}\right)IW(R_k;{\delta }_k,{\Delta }_k)G\left({\gamma }_k;\frac{{\kappa }_k}{2},\frac{{\kappa }_k}{2}\right)G\left({\kappa }_k;{\varphi }_k,{\Phi }_k\left)p\right(y_{1:k-1}\right)\hfill \end{array}$$

(29)

Setting $\xi =R_k$,

$$\begin{array}{cc}\hfill \log q^{(j+1)}\left(\xi \right)=& \log q^{(j+1)}\left(R_k\right)\hfill \\ \hfill =& -0.5({\widehat{\delta }}_{k|k-1}+n_z+2)\log \left(R_k\right)-0.5\mathrm{tr}\left\{[{\widehat{\Delta }}_{k|k-1}+E^{(j+1)}\left({\gamma }_k\right)B_k^{\left(j\right)}]R_k^{-1}\right\}+c_R\hfill \end{array}$$

(30)

from which we have:

$$\begin{array}{cc}\hfill q^{(j+1)}\left(R_k\right)& =IW\left(R_k;{\widehat{\delta }}_{k|k}^{(j+1)},{\widehat{\Delta }}_{k|k}^{(j+1)}\right)\hfill \end{array}$$

(31)

whose parameters ${\widehat{\delta }}_{k|k}^{(j+1)}$ and ${\widehat{\Delta }}_{k|k}^{(j+1)}$ can be obtained by:

$$\begin{array}{cc}\hfill {\widehat{\delta }}_{k|k}^{(j+1)}& ={\widehat{\delta }}_{k|k-1}+1\hfill \end{array}$$

(32)

$$\begin{array}{cc}\hfill {\widehat{\Delta }}_{k|k}^{(j+1)}& ={\widehat{\Delta }}_{k|k-1}+E^{(j+1)}\left({\gamma }_k\right)B_k^{\left(j\right)}\hfill \end{array}$$

(33)

where $B_k^{\left(j\right)}$ can be obtained via the SRC rule as follows:

$$\begin{array}{cc}\hfill P_{k|k}^{\left(j\right)}& =T_k{T}_k^{\mathrm{T}}\hfill \end{array}$$

(34)

$$\begin{array}{cc}\hfill {\chi }_{i,k}^{\left(j\right)}& =T_k{r}_i+{\widehat{x}}_{k|k}^{\left(j\right)}\hfill \end{array}$$

(35)

$$\begin{array}{cc}\hfill Y_{i,k}^{\left(j\right)}& =h\left({\chi }_{i,k}^{\left(j\right)}\right)\hfill \end{array}$$

(36)

$$\begin{array}{cc}\hfill {\widehat{y}}_{k|k}^{\left(j\right)}& =\frac{1}{2n_x}\sum _{i=1}^{2n_x}{Y}_{i,k}^{\left(j\right)}\hfill \end{array}$$

(37)

$$\begin{array}{cc}\hfill B_k^{\left(j\right)}& =\frac{1}{2n_x}\sum _{i=1}^{2n_x}{Y}_{i,k}^{\left(j\right)}{Y_{i,k}^{\left(j\right)}}^{\mathrm{T}}-{\widehat{y}}_{k|k}^{\left(j\right)}{\left({\widehat{y}}_{k|k}^{\left(j\right)}\right)}^{\mathrm{T}}.\hfill \end{array}$$

(38)

Next, setting $\xi ={\gamma }_k$, we get:

$$\begin{array}{cc}\hfill \log q^{(j+1)}\left(\xi \right)=& \log q^{(j+1)}\left({\gamma }_k\right)\hfill \\ \hfill =& \left(\frac{n_z+E^{\left(j\right)}\left({\kappa }_k\right)}{2}-1\right)\log \left({\gamma }_k\right)-0.5\{\mathrm{tr}\left[B_k^{\left(j\right)}{E}^{\left(j\right)}\left(R_k^{-1}\right)\right]+E^{\left(j\right)}\left({\kappa }_k\right)\}{\gamma }_k+c_{\gamma }\hfill \end{array}$$

(39)

from which:

$$\begin{array}{cc}\hfill q^{(j+1)}\left({\gamma }_k\right)& =G\left({\gamma }_k;{\widehat{a}}_{k|k}^{(j+1)},{\widehat{b}}_{k|k}^{(j+1)}\right)\hfill \end{array}$$

(40)

where:

$$\begin{array}{cc}\hfill {\widehat{a}}_{k|k}^{(j+1)}& =0.5(n_z+E^{\left(j\right)}\left({\kappa }_k\right))\hfill \end{array}$$

(41)

$$\begin{array}{cc}\hfill {\widehat{b}}_{k|k}^{(j+1)}& =-0.5\{\mathrm{tr}\left[B_k^{\left(j\right)}{E}^{\left(j\right)}\left(R_k^{-1}\right)\right]+E^{\left(j\right)}\left({\kappa }_k\right)\}.\hfill \end{array}$$

(42)

Then, setting $\xi ={\kappa }_k$, we get

$$\begin{array}{cc}\hfill \log q^{(j+1)}\left(\xi \right)=& \log q^{(j+1)}\left({\kappa }_k\right)\hfill \\ \hfill =& \frac{{\kappa }_k}{2}\log \left(\frac{{\kappa }_k}{2}\right)-\log \Gamma \left(\frac{{\kappa }_k}{2}\right)\hfill \\ \hfill & +\left(\frac{{\kappa }_k}{2}-1\right)E^{(j+1)}\left(\log \left({\gamma }_k\right)\right)-\frac{{\kappa }_k}{2}{E}^{(j+1)}\left({\gamma }_k\right)+({\widehat{\varphi }}_{k|k-1}-1)\log \left({\kappa }_k\right)-{\widehat{\Phi }}_{k|k-1}{\kappa }_k+c_{\kappa }\hfill \end{array}$$

(43)

from which we have:

$$\begin{array}{cc}\hfill q^{(j+1)}\left({\kappa }_k\right)& =G\left({\kappa }_k;{\widehat{\varphi }}_{k|k}^{(j+1)},{\widehat{\Phi }}_{k|k}^{(j+1)}\right)\hfill \end{array}$$

(44)

where:

$$\begin{array}{cc}\hfill {\widehat{\varphi }}_{k|k}^{(j+1)}& ={\widehat{\varphi }}_{k|k-1}+0.5\hfill \end{array}$$

(45)

$$\begin{array}{cc}\hfill {\widehat{\Phi }}_{k|k}^{(j+1)}& ={\widehat{\Phi }}_{k|k-1}-0.5E^{(j+1)}\left(\log \left({\gamma }_k\right)\right)+0.5E^{(j+1)}\left({\gamma }_k\right)-0.5.\hfill \end{array}$$

(46)

Finally, setting $\xi =x_k$,

$$\begin{array}{cc}\hfill \log q^{(j+1)}\left(\xi \right)=& \log q^{(j+1)}\left(x_k\right)\hfill \\ \hfill =& -0.5{(x_k-{\widehat{x}}_{k|k-1})}^{\mathrm{T}}{E}^{\left(j\right)}\left(P_{k|k-1}^{-1}\right)(x_k-{\widehat{x}}_{k|k-1})\hfill \\ \hfill & -0.5E^{\left(j\right)}\left({\gamma }_k\right){(y_k-h\left(x_k\right))}^{\mathrm{T}}{E}^{\left(j\right)}\left(R_k^{-1}\right)(y_k-h\left(x_k\right))+c_x\hfill \end{array}$$

(47)

according to which we have:

$$\begin{array}{c}\hfill q^{(j+1)}\left(x_k\right)=\mathcal{N}\left(x_k;{\widehat{x}}_{k|k}^{(j+1)},P_{k|k}^{(j+1)}\right)\end{array}$$

(48)

where ${\widehat{x}}_{k|k}^{(j+1)}$ and $P_{k|k}^{(j+1)}$ are the estimated state and SECM in the j-th VB iteration, respectively.

In order to obtain ${\widehat{x}}_{k|k}^{(j+1)}$ and $P_{k|k}^{(j+1)}$, the associated information matrix and information vector contributions $I_k^{(j+1)}$ and, respectively, ${\iota }_k^{(j+1)}$ should be first computed. By utilizing a linearized error propagation method [26], they can be obtained as follows:

$$\begin{array}{cc}\hfill I_k^{(j+1)}& =H_k^{\mathrm{T}}{\left({\tilde{R}}_{k|k}^{(j+1)}\right)}^{-1}{H}_k\hfill \end{array}$$

(49)

$$\begin{array}{cc}\hfill {\iota }_k^{(j+1)}& =H_k^{\mathrm{T}}{\left({\tilde{R}}_{k|k}^{(j+1)}\right)}^{-1}(y_k-{\widehat{y}}_{k|k-1}+H_k{\widehat{x}}_{k|k-1})\hfill \end{array}$$

(50)

where ${\tilde{R}}_{k|k}^{(j+1)}$ denotes the estimated scale matrix and $H_k$ a pseudo-measurement matrix. Then, exploiting (31) and a property of the inverse Wishart distribution, it can be obtained that:

$$\begin{array}{cc}\hfill E^{(j+1)}\left(R_k^{-1}\right)& =({\widehat{\delta }}_{k|k}^{(j+1)}-n_z-1){\left({\hat{\Delta }}_{k|k}^{(j+1)}\right)}^{-1}.\hfill \end{array}$$

(51)

Similarly, taking into account (40) and (44) as well as a property of the Gamma distribution, we have:

$$\begin{array}{cc}\hfill E^{(j+1)}\left({\kappa }_k\right)& ={\widehat{\varphi }}_{k|k}^{(j+1)}/{\widehat{\Phi }}_{k|k}^{(j+1)}\hfill \end{array}$$

(52)

$$\begin{array}{cc}\hfill E^{(j+1)}\left({\gamma }_k\right)& ={\widehat{a}}_{k|k}^{(j+1)}/{\widehat{b}}_{k|k}^{(j+1)}.\hfill \end{array}$$

(53)

Then, ${\tilde{R}}_{k|k}^{(j+1)}$ is computed as:

$$\begin{array}{cc}\hfill {\tilde{R}}_{k|k}^{(j+1)}& ={({\widehat{\delta }}_{k|k}^{(j+1)}-n_z-1)}^{-1}{\widehat{\Delta }}_{k|k}^{(j+1)}.\hfill \end{array}$$

(54)

Moreover, the pseudo-measurement matrix $H_k$ in (49) and (50) can be obtained as:

$$\begin{array}{cc}\hfill H_k& =P_{xz,k|k-1}^{\mathrm{T}}{Z}_{k|k-1}\hfill \end{array}$$

(55)

where $P_{xz,k|k-1}$ is computed by means of the SRC rule as follows:

$$\begin{array}{cc}\hfill P_{k|k-1}& =T_{k|k-1}{T}_{k|k-1}^{\mathrm{T}}\hfill \end{array}$$

(56)

$$\begin{array}{cc}\hfill {\chi }_{i,k|k-1}& =T_{k|k-1}{r}_i+{\widehat{x}}_{k|k-1}\hfill \end{array}$$

(57)

$$\begin{array}{cc}\hfill Y_{i,k|k-1}& =h\left({\chi }_{i,k|k-1}\right)\hfill \end{array}$$

(58)

$$\begin{array}{cc}\hfill {\widehat{y}}_{k|k-1}& =\frac{1}{2n_x}\sum _{i=1}^{2n_x}{Y}_{i,k|k-1}\hfill \end{array}$$

(59)

$$\begin{array}{cc}\hfill P_{xz,k|k-1}& =\frac{1}{2n_x}\sum _{i=1}^{2n_x}{\chi }_{i,k-1}{Y}_{i,k-1}^{\mathrm{T}}-{\widehat{x}}_{k|k-1}{\widehat{y}}_{k|k-1}^{\mathrm{T}}.\hfill \end{array}$$

(60)

Then, the information matrix and vector are updated as follows:

$$\begin{array}{cc}\hfill Z_{k|k}^{(j+1)}& =Z_{k|k-1}+I_k^{(j+1)}\hfill \end{array}$$

(61)

$$\begin{array}{cc}\hfill {\zeta }_{k|k}^{(j+1)}& ={\zeta }_{k|k-1}+{\iota }_k^{(j+1)}.\hfill \end{array}$$

(62)

Finally, we have:

$$\begin{array}{cc}\hfill {\widehat{x}}_{k|k}^{(j+1)}& =Z_{k|k}^{(j+1)}\backslash {\zeta }_{k|k}^{(j+1)}\hfill \end{array}$$

(63)

$$\begin{array}{cc}\hfill P_{k|k}^{(j+1)}& =Z_{k|k}^{(j+1)}\backslash I_{n_x}\hfill \end{array}$$

(64)

where $I_{n_x}$ denotes the $n_x\times n_x$ identity matrix. To summarize the above developments, the pseudocode of VBST-CIF is reported in Algorithm 1.

Algorithm 1 Time-recursion of VBST-CIF

Input: ${\widehat{x}}_{k-1|k-1}$, $P_{k-1|k-1}$, ${\widehat{\delta }}_{k-1|k-1}$, ${\widehat{\Delta }}_{k-1|k-1}$, ${\widehat{\varphi }}_{k-1|k-1}$, ${\widehat{\Phi }}_{k-1|k-1}$, $y_k$, $Q_{k-1}$, $\tau$

Step 1: Time update:

(1) Compute ${\widehat{x}}_{k|k-1}$ and $P_{k|k-1}$ based on the SRC rule by (17)–(21).

(2) Compute $Z_{k|k-1}$ and ${\zeta }_{k|k-1}$ by (22) and (23).

(3) Compute ${\widehat{\delta }}_{k|k-1}$ and ${\widehat{\Delta }}_{k|k-1}$ by (13) and (14).

(4) Compute ${\widehat{\varphi }}_{k|k-1}$ and ${\widehat{\Phi }}_{k|k-1}$ by (15) and (16).

Step 2: Variational fixed point iterations:

Initialize:

${\widehat{x}}_{k|k}^{\left(0\right)}={\widehat{x}}_{k|k-1}$, $P_{k|k}^{\left(0\right)}=P_{k|k-1}$, ${\widehat{\delta }}_{k|k}^{\left(0\right)}={\widehat{\delta }}_{k|k-1}$, ${\widehat{\Delta }}_{k|k}^{\left(0\right)}={\widehat{\Delta }}_{k|k-1}$, ${\widehat{\varphi }}_{k|k}^{\left(0\right)}={\widehat{\varphi }}_{k|k-1}$, ${\widehat{\Phi }}_{k|k}^{\left(0\right)}={\widehat{\Phi }}_{k|k-1}$

Measurement update:

(1) Perform Cholesky decomposition of $P_{k|k-1}$ and cubature sampling via (56) and (57).

(2) Compute $P_{xz,k|k-1}$ by (58)–(60).

(3) Variational parameter update

for $j=0,1,\cdots ,N-1$ do

(a) Compute $q^{(j+1)}\left(R_k\right)$ by (31):

Compute $B_k^{\left(j\right)}$ by (34)–(38).

Update ${\widehat{\delta }}_{k|k}^{(j+1)}$ and ${\widehat{\Delta }}_{k|k}^{(j+1)}$ by (33) and (33).

(b) Compute $q^{(j+1)}\left({\gamma }_k\right)$ by (40):

Compute ${\widehat{a}}_{k|k}^{(j+1)}$ and ${\widehat{b}}_{k|k}^{(j+1)}$ by (41) and (42).

(c) Compute $q^{(j+1)}\left({\kappa }_k\right)$ by (44):

Update ${\widehat{\varphi }}_{k|k}^{(j+1)}$ and ${\widehat{\Phi }}_{k|k}^{(j+1)}$ by (45) and (46).

(d) Compute $q^{(j+1)}\left(x_k\right)$ by (48):

Compute ${\tilde{R}}_{k|k}^{(j+1)}$ by (54).

Compute $H_k$ by (55).

Update $Z_{k|k}^{(j+1)}$ and ${\zeta }_{k|k}^{(j+1)}$ by (61) and (61).

Compute ${\widehat{x}}_{k|k}^{(j+1)}$ and $P_{k|k}^{(j+1)}$ by (63) and (64).

end for:

Step 3: State update:

${\widehat{x}}_{k|k}={\widehat{x}}_{k|k}^{\left(N\right)}$, $P_{k|k}=P_{k|k}^{\left(N\right)}$, ${\widehat{\delta }}_{k|k}={\widehat{\delta }}_{k|k}^{\left(N\right)}$, ${\widehat{\Delta }}_{k|k}={\widehat{\Delta }}_{k|k}^{\left(N\right)}$, ${\widehat{\varphi }}_{k|k}={\widehat{\varphi }}_{k|k}^{\left(N\right)}$, ${\widehat{\Phi }}_{k|k}={\widehat{\Phi }}_{k|k}^{\left(N\right)}$

Output: ${\widehat{x}}_{k|k}$, $P_{k|k}$, ${\widehat{\delta }}_{k|k}$, ${\widehat{\Delta }}_{k|k}$, ${\widehat{\varphi }}_{k|k}$, ${\widehat{\Phi }}_{k|k}$

4. Variational Bayesian Student’s t-Based Cubature Information Feedback Fusion (VBST-CIFF)

4.1. Multi-Sensor Cubature Information Feedback Fusion (CIFF)

The information feedback fusion (IFF) algorithm derived in this section is suitable for multi-sensor fusion. The overall structure of the IFF is schematized in Figure 1, where each local filter receives raw measurements from the relative sensor and produces sensor-dependent variables to be used by the information fusion center in order to update the information matrix and state vector, which are then fed back to the local filters.

Based on the structure of IFF, the CIFF algorithm is derived as follows. Consider that there are S sensors. For each sensor s, $s\in \{1,2,\cdots ,S\}$, a measurement $y_{k,s}$ is available at time k, i.e.,

$$\begin{array}{cc}\hfill y_{k,s}& =h_s\left(x_k\right)+v_{k,s}\hfill \end{array}$$

(65)

where $h_s(·)$ denotes the measurement function of sensor s, and  $v_{k,s}$ the corresponding measurement noise. It is assumed that $v_{k,m}$ and $v_{k,n}$ are uncorrelated for any $m\ne n$. Let, at time $k-1$, the state estimate and SECM for all sensors be ${\widehat{x}}_{k-1|k-1}$ and, respectively, $P_{k-1|k-1}$. Correspondingly, the initial information vector and matrix for all sensors are ${\zeta }_{k-1|k-1}$ and $Z_{k-1|k-1}$, respectively. Then, by exploiting the SRC rule, predicted ${\zeta }_{k-1|k}$ and $Z_{k-1|k}$ can be obtained, i.e.,

$$\begin{array}{cc}\hfill {\left(Z_{k-1|k-1}\right)}^{-1}& =T_{k-1}{T}_{k-1}^{\mathrm{T}}\hfill \end{array}$$

(66)

where $Z_{k-1|k-1}$ is the inverse of $P_{k-1|k-1}$. Transforming ${\chi }_{i,k-1}$ in (18) through $f(·)$, we have ${\Psi }_{i,k-1}$ in (19). Then, predicted $Z_{k|k-1}$ and ${\zeta }_{k|k-1}$, are computed via (22) and (23).

At the fusion center, the estimated information vector and matrix can, therefore, be updated as follows:

$$\begin{array}{cc}\hfill {\zeta }_{k|k}& ={\zeta }_{k|k-1}+\sum _{s=1}^S{\iota }_{k,s}\hfill \end{array}$$

(67)

$$\begin{array}{cc}\hfill Z_{k|k}& =Z_{k|k-1}+\sum _{s=1}^S{I}_{k,s}\hfill \end{array}$$

(68)

where: the local sensor contributions $I_{k,s}$ and ${\iota }_{k,s}$ are given by:

$$\begin{array}{cc}\hfill I_{k,s}& =H_{k,s}^{\mathrm{T}}{R}_{k,s}^{-1}{H}_k\hfill \end{array}$$

(69)

$$\begin{array}{cc}\hfill {\iota }_{k,s}& =H_{k,s}^{\mathrm{T}}{R}_{k,s}^{-1}(y_{k,s}-{\widehat{y}}_{k|k-1,s}+H_k{\widehat{x}}_{k|k-1});\hfill \end{array}$$

(70)

the pseudo-measurement matrix $H_{k,s}$ of sensor s can be obtained through the statistical linearized error propagation approach as in Section 3.3; $R_{k,s}$ denotes the MNCM of sensor s; ${\widehat{y}}_{k|k-1,s}$ can be obtained by the SRC rule as in the conventional CIF [25].

4.2. Variational Bayesian Student’s t-Based Cubature Information Feedback Fusion (VBST-CIFF)

Consider now that each sensor s is characterized by heavy-tailed measurement noise $v_{k,s}$. In the time-update, the parameters ${\widehat{\delta }}_{k|k-1,s}$, ${\widehat{\Delta }}_{k|k-1,s}$, ${\widehat{\varphi }}_{k|k-1,s}$, and ${\widehat{\Phi }}_{k|k-1,s}$ for sensor s, can be obtained via (71)–(74), respectively, i.e.,

$$\begin{array}{cc}\hfill {\widehat{\delta }}_{k|k-1,s}& =\tau ({\widehat{\delta }}_{k-1|k-1,s}-n_z-1)+n_z+1\hfill \end{array}$$

(71)

$$\begin{array}{cc}\hfill {\widehat{\Delta }}_{k|k-1,s}& =\tau {\widehat{\Delta }}_{k-1|k-1,s}\hfill \end{array}$$

(72)

$$\begin{array}{cc}\hfill {\widehat{\varphi }}_{k|k-1,s}& =\tau {\widehat{\varphi }}_{k-1|k-1,s}\hfill \end{array}$$

(73)

$$\begin{array}{cc}\hfill {\widehat{\Phi }}_{k|k-1,s}& =\tau {\widehat{\Phi }}_{k-1|k-1,s}.\hfill \end{array}$$

(74)

Then, in the measurement update, the estimated scale matrix for sensor s is given by:

$$\begin{array}{cc}\hfill {\tilde{R}}_{k|k,s}^{(j+1)}& ={\left({\widehat{\delta }}_{k|k,s}^{(j+1)}-n_z-1\right)}^{-1}{\widehat{\Delta }}_{k|k,s}^{(j+1)}\hfill \end{array}$$

(75)

where ${\widehat{\delta }}_{k|k,s}^{(j+1)}$ and ${\widehat{\Delta }}_{k|k,s}^{(j+1)}$ can be obtained via:

$$\begin{array}{cc}\hfill {\widehat{\delta }}_{k|k,s}^{(j+1)}& ={\widehat{\delta }}_{k|k-1}+1\hfill \end{array}$$

(76)

$$\begin{array}{cc}\hfill {\widehat{\Delta }}_{k|k,s}^{(j+1)}& ={\widehat{\Delta }}_{k|k-1}+E^{(j+1)}\left({\gamma }_{k,s}\right)B_{k,s}^{\left(j\right)}\hfill \end{array}$$

(77)

where $B_{k,s}^{\left(j\right)}$ can be derived in the same way as in (34)–(38) with cubature sampling points transformed through $h_s(·)$ as:

$$\begin{array}{cc}\hfill Y_{i,k|k-1,s}& =h_s\left({\chi }_{i,k|k-1,s}\right)\hfill \end{array}$$

(78)

$$\begin{array}{cc}\hfill {\widehat{y}}_{k|k-1,s}& =\frac{1}{2n_z}\sum _{i=1}^{2n_z}{Y}_{i,k|k-1,s}\hfill \end{array}$$

(79)

in which the original sampling points ${\chi }_{i,k|k-1,s}$ are given by (56) and (57). Furthermore, $E^{(j+1)}\left({\gamma }_{k,s}\right)$ for sensor s can be obtained by:

$$\begin{array}{cc}\hfill E^{(j+1)}\left({\gamma }_{k,s}\right)& ={\widehat{a}}_{k|k,s}^{(j+1)}/{\widehat{b}}_{k|k,s}^{(j+1)}\hfill \end{array}$$

(80)

where,

$$\begin{array}{cc}\hfill {\widehat{a}}_{k|k,s}^{(j+1)}& =0.5(n_z+E^{\left(j\right)}\left({\kappa }_{k,s}\right))\hfill \end{array}$$

(81)

$$\begin{array}{cc}\hfill {\widehat{b}}_{k|k,s}^{(j+1)}& =-0.5\{\mathrm{tr}\left[B_{k,s}^{(j+1)}{E}^{\left(j\right)}\left(R_{k,s}^{-1}\right)\right]+E^{\left(j\right)}\left({\kappa }_{k,s}\right)\}\hfill \end{array}$$

(82)

and $E^{(j+1)}\left({\kappa }_{k,s}\right)$ is given by:

$$\begin{array}{cc}\hfill E^{(j+1)}\left({\kappa }_{k,s}\right)& ={\widehat{\varphi }}_{k|k,s}^{(j+1)}/{\widehat{\Phi }}_{k|k,s}^{(j+1)}\hfill \end{array}$$

(83)

in which:

$$\begin{array}{cc}\hfill {\widehat{\varphi }}_{k|k,s}^{(j+1)}& ={\widehat{\varphi }}_{k|k-1,s}+0.5\hfill \end{array}$$

(84)

$$\begin{array}{cc}\hfill {\widehat{\Phi }}_{k|k,s}^{(j+1)}& ={\widehat{\Phi }}_{k|k-1,s}-0.5E^{(j+1)}\left(\log \left({\gamma }_{k,s}\right)\right)+0.5E^{(j+1)}\left({\gamma }_{k,s}\right)-0.5.\hfill \end{array}$$

(85)

The pseudo-measurement matrix for sensor s turns out to be:

$$\begin{array}{cc}\hfill H_{k,s}& =P_{xz,k|k-1,s}^{\mathrm{T}}{Z}_{k|k-1}\hfill \end{array}$$

(86)

where $P_{xz,k|k-1,s}^{\mathrm{T}}$ can be obtained via

$$\begin{array}{cc}\hfill P_{xz,k|k-1,s}& =\frac{1}{2n_x}\sum _{i=1}^{2n_x}{\chi }_{i,k-1}{Y}_{i,k-1,s}^{\mathrm{T}}-{\widehat{x}}_{k|k-1}{\widehat{y}}_{k|k-1,s}^{\mathrm{T}}.\hfill \end{array}$$

(87)

Hence, the information matrix and vector contributions due to sensor s can be derived as follows:

$$\begin{array}{cc}\hfill I_{k,s}^{(j+1)}& =H_{k,s}^{\mathrm{T}}{\left({\tilde{R}}_{k|k,s}^{(j+1)}\right)}^{-1}{H}_{k,s}\hfill \end{array}$$

(88)

$$\begin{array}{cc}\hfill {\iota }_{k,s}^{(j+1)}& =H_{k,s}^{\mathrm{T}}{\left({\tilde{R}}_{k|k,s}^{(j+1)}\right)}^{-1}(y_{k,s}-{\widehat{y}}_{k|k-1,s}+H_{k,s}{\widehat{x}}_{k|k-1})\hfill \end{array}$$

(89)

from which the information matrix and vector can be updated according to

$$\begin{array}{cc}\hfill Z_{k|k}^{(j+1)}& =Z_{k|k-1}+\sum _{s=1}^S{I}_{k,s}^{(j+1)}\hfill \end{array}$$

(90)

$$\begin{array}{cc}\hfill {\zeta }_{k|k}^{(j+1)}& ={\zeta }_{k|k-1}+\sum _{s=1}^S{\iota }_{k,s}^{(j+1)}.\hfill \end{array}$$

(91)

Finally, the global state estimate and covariance can be obtained by (63) and (64). To summarize the above developments, the flow-chart of the proposed VBST-CIFF is shown in Figure 2. Accordingly, the pseudocode is reported in Algorithm 2.

Please notice that VBST-CIFF is actually tightly-coupled (centralized) in that the raw measurements from all sensors are integrated into a single nonlinear information filter. Thanks to the adoption of the information filter form and the assumption that measurement noise signals of the various sensors are uncorrelated, the filter’s multi-sensor measurement update turns out to be decoupled into single-sensor updates as in (67) and (68). However, the overall filter is coupled through the information feedback of $Z_k,{\zeta }_k$ into the local filters (see Figure 1).

Algorithm 2 Time-recursion of VBST-CIFF

Input: ${\widehat{x}}_{k-1|k-1}$, $P_{k-1|k-1}$, $y_{k,s}$, $\tau$, ${\widehat{\delta }}_{k-1|k-1,s}$, ${\widehat{\Delta }}_{k-1|k-1,s}$, ${\widehat{\varphi }}_{k-1|k-1,s}$, ${\widehat{\Phi }}_{k-1|k-1,s}$, $Q_{k-1}$

Step 1: Time update:

(1) Compute $Z_{k|k-1}$ and ${\zeta }_{k|k-1}$ by (22) and (23).

(2) Compute ${\widehat{\delta }}_{k|k-1,s}$ and ${\widehat{\Delta }}_{k|k-1,s}$ by (71) and (72).

(3) Compute ${\widehat{\varphi }}_{k|k-1,s}$ and ${\widehat{\Phi }}_{k|k-1,s}$ by (73) and (74).

Step 2: Variational fixed-point iterations:

Initialize:

${\widehat{x}}_{k|k}^{\left(0\right)}={\widehat{x}}_{k|k-1}$, $P_{k|k}^{\left(0\right)}=P_{k|k-1}$, ${\widehat{\delta }}_{k|k,s}^{\left(0\right)}={\widehat{\delta }}_{k|k-1,s}$, ${\widehat{\Delta }}_{k|k,s}^{\left(0\right)}={\widehat{\Delta }}_{k|k-1,s}$, ${\widehat{\varphi }}_{k|k,s}^{\left(0\right)}={\widehat{\varphi }}_{k|k-1,s}$, ${\widehat{\Phi }}_{k|k,s}^{\left(0\right)}={\widehat{\Phi }}_{k|k-1,s}$

Measurement update:

(1) Perform Cholesky decomposition of $P_{k|k-1}$ and cubature sampling via (56) and (57).

(2) Compute $P_{xz,k|k-1,s}$ by (87).

(3) Variational parameter update:

for$j=0,1,\cdots ,N-1$do

(a) Update $E^{\left(j\right)}\left({\kappa }_{k,s}\right)$, ${\widehat{\varphi }}_{k|k,s}^{(j+1)}$, and ${\widehat{\Phi }}_{k|k,s}^{(j+1)}$ by (83)–(85).

(b) Update $E^{(j+1)}\left({\gamma }_{k,s}\right)$, ${\widehat{a}}_{k|k,s}^{(j+1)}$, and ${\widehat{b}}_{k|k,s}^{(j+1)}$ by (80)–(82).

(c) Update ${\tilde{R}}_{k|k,s}^{(j+1)}$, ${\widehat{\delta }}_{k|k,s}^{(j+1)}$ and ${\widehat{\Delta }}_{k|k,s}^{(j+1)}$ by (75)–(77).

(d) Update $H_{k,s}$ by (86).

(e) Update $I_{k,s}^{(j+1)}$ and ${\iota }_{k,s}^{(j+1)}$ by (88) and (89).

(f) Set variational parameters:

${\widehat{\delta }}_{k|k,s}={\widehat{\delta }}_{k|k,s}^{\left(N\right)}$, ${\widehat{\Delta }}_{k|k,s}={\widehat{\Delta }}_{k|k,s}^{\left(N\right)}$, ${\widehat{\varphi }}_{k|k,s}={\widehat{\varphi }}_{k|k,s}^{\left(N\right)}$, ${\widehat{\Phi }}_{k|k,s}={\widehat{\Phi }}_{k|k,s}^{\left(N\right)}$

end for

Step 3: Fusion (at the global center):

(1) Update $Z_{k|k}^{\left(N\right)}$ and ${\zeta }_{k|k}^{\left(N\right)}$ by (90) and (91);

(2) Update ${\widehat{x}}_{k|k}^{\left(N\right)}$ and $P_{k|k}^{\left(N\right)}$ via (63) and (64).

(3) Set:

${\widehat{x}}_{k|k}={\widehat{x}}_{k|k}^{\left(N\right)}$, $P_{k|k}=P_{k|k}^{\left(N\right)}$, ${\widehat{\delta }}_{k|k,s}={\widehat{\delta }}_{k|k,s}^{\left(N\right)}$, ${\widehat{\Delta }}_{k|k,s}={\widehat{\Delta }}_{k|k,s}^{\left(N\right)}$, ${\widehat{\varphi }}_{k|k}={\widehat{\varphi }}_{k|k,s}^{\left(N\right)}$, ${\widehat{\Phi }}_{k|k}={\widehat{\Phi }}_{k|k,s}^{\left(N\right)}$

Output:${\widehat{x}}_{k|k}$, $P_{k|k}$, ${\widehat{\delta }}_{k|k,s}$, ${\widehat{\Delta }}_{k|k,s}$, ${\widehat{\varphi }}_{k|k,s}$, ${\widehat{\Phi }}_{k|k,s}$

5. Simulation Results

5.1. VBST-CIF Single-Sensor Target Tracking

Consider a target car tracking scenario with just a single sensor (a 2D radar). The state of the car is taken as $x={[\varsigma ,\dot{\varsigma },\mu ,\dot{\mu },\omega ]}^{\mathrm{T}}$, where $\varsigma$ and $\mu$ are the Cartesian coordinates of position, while $\dot{\varsigma }$ and $\dot{\mu }$ represent the corresponding velocity, and $\omega$ is the turning rate. The car moves according to the coordinated turn (CT) model:

$$\begin{array}{cc}\hfill x_k& =F\left({\omega }_{k-1}\right)x_{k-1}+w_{k-1}\hfill \end{array}$$

(92)

with a turning-rate dependent state-transition matrix:

$$F\left(\omega \right)=\left[\begin{array}{ccccc}1& \frac{\sin \left(\omega \right)}{\omega }& 0& -\frac{1-\cos \left(\omega \right)}{\omega }& 0\\ 0& \cos \left(\omega \right)& 0& -\sin \left(\omega \right)& 0\\ 0& \frac{1-\cos \left(\omega \right)}{\omega }& 1& \frac{\sin \left(\omega \right)}{\omega }& 0\\ 0& \sin \left(\omega \right)& 0& \cos \left(\omega \right)& 0\\ 0& 0& 0& 0& 1\end{array}\right]$$

fixed at turning rate $\omega ={10}^{\circ }$; $w_{k-1}=\mathcal{N}\left(0,Q_{k-1}\right)$;

$$Q_{k-1}=\left[\begin{array}{ccccc}\frac{p_1{T}^3}{3}& \frac{p_1{T}^2}{2}& 0& 0& 0\\ \frac{p_1{T}^2}{2}& T& 0& 0& 0\\ 0& 0& \frac{p_1{T}^3}{3}& \frac{p_1{T}^2}{2}& 0\\ 0& 0& \frac{p_1{T}^2}{2}& T& 0\\ 0& 0& 0& 0& p_{2}T\end{array}\right],$$

with noise intensity factors $p_1=0.1$, $p_2=1.75\times {10}^{-4}$ and sampling interval $T=1\left[s\right]$. Conversely, the measurement function of the radar is:

$$\begin{array}{cc}\hfill y_k& =\left[\begin{array}{c}\sqrt{{\varsigma }_k^2+{\mu }_k^2}\hfill \\ \mathrm{atan}2\left({\varsigma }_k,{\mu }_k\right)\hfill \end{array}\right]+v_k\hfill \end{array}$$

(93)

where: $\mathrm{atan}2(\varsigma ,\mu )$ denotes the 4-quadrant inverse-tangent defined as the argument of the complex number $\varsigma +j\mu$, j denoting the imaginary unit; $v_k$ is heavy-tailed measurement noise such that:

$$\begin{array}{c}\hfill v_k\sim \left\{\begin{array}{cc}\mathcal{N}\left(0,{\Lambda }_k\right),\hfill & \hfill \mathrm{with}\mathrm{probability}0.9\\ \mathcal{N}\left(0,100{\Lambda }_k\right),\hfill & \hfill \mathrm{with}\mathrm{probability}0.1\end{array}\right.\end{array}$$

(94)

with ${\Lambda }_k=\left[\begin{array}{cc}4\left[{\mathrm{m}}^2\right]& 0\\ 0& {10}^{-4}\left[{\mathrm{rad}}^2\right]\end{array}\right].$ Moreover, the initial values for state and SECM of the car are set as follows:

$$\begin{array}{cc}\hfill x_0& ={[100\left[\mathrm{m}\right],3\left[\mathrm{m}/\mathrm{s}\right],100\left[\mathrm{m}\right],2\left[\mathrm{m}/\mathrm{s}\right],\frac{-10\pi }{180}\left[\mathrm{rad}\right]]}^{\mathrm{T}}\hfill \end{array}$$

(95)

$$\begin{array}{cc}\hfill P_0& =\left[\begin{array}{ccccc}10\left[{\mathrm{m}}^2\right]& 0& 0& 0& 0\\ 0& 1[{\mathrm{m}}^2/s^2]& 0& 0& 0\\ 0& 0& 10\left[{\mathrm{m}}^2\right]& 0& 0\\ 0& 0& 0& 1[{\mathrm{m}}^2/s^2]& 0\\ 0& 0& 0& 0& {10}^{-4}\left[{\mathrm{rad}}^2\right]\end{array}\right].\hfill \end{array}$$

(96)

A total of 100 independent Monte Carlo trials have been carried out, with each trial lasting for $t=50\left[\mathrm{s}\right]$. Furthermore, the root-mean-square-error (RMSE) is adopted to assess tracking precision. For variable b at time k, it is defined as:

$$\begin{array}{cc}\hfill {RMSE}_b\left(k\right)& =\sqrt{\frac{1}{N_{MC}}\sum _{i=1}^{N_{MC}}{\left(b_k^i-{\widehat{b}}_k^i\right)}^{\mathrm{T}}\left(b_k^i-{\widehat{b}}_k^i\right)}\hfill \end{array}$$

(97)

where: $N_{MC}$ denotes the number of Monte Carlo trials; b denotes either the position $p={[\varsigma ,\mu ]}^{\mathrm{T}}$ or velocity $v={[\dot{\varsigma },\dot{\mu }]}^{\mathrm{T}}$; and $b_k^i$ is the true value in the i-th trial at time k. Conversely, ${\widehat{b}}_k^i$ represents the estimated value in the i-th trial at time k. Furthermore, the mean-root-mean-square-error (MRMSE) is used to measure the average RMSE during the whole simulation time. Specifically, MRMSE for variable b is defined as:

$$\begin{array}{cc}\hfill {RMSE}_b& =\frac{1}{t/T}\sum _{k=1}^{t/T}{RMSE}_b\left(k\right).\hfill \end{array}$$

(98)

Simulation results are presented to compare the conventional CIF [43] with the proposed VBST-CIF. Simulation parameters have been chosen in the same way as in [33]. Specifically, in CIF, the process noise covariance Q and MNCM R are fixed to $Q=Q_{k-1}$ and $R={\Lambda }_k$. Conversely, in the proposed VBST-CIF, $Q=Q_{k-1}$, while the scale matrix as well as other noise statistical parameters are adaptively estimated. Other parameters are set in the same way as in [33]: forgetting the factor $\tau =0.9$; number of VB iterations $N=10$; initial noise parameters of the scale matrix $\delta =n_z+2=4$ and $\Delta ={\Lambda }_k$; initial noise parameters of the DOF parameter $\varphi =5$ and $\Phi =1$.

Results are presented in Figure 3, Figure 4, Figure 5 and Figure 6 and

Table 1. Mean-root-mean-square-error (MRMSE) in single-sensor target tracking.

Filter	${RMSE}_{\mathit{p}}$	${RMSE}_{\mathit{v}}$	${RMSE}_{\mathit{\omega }}$
CIF	5.2364	1.8646	0.0518
VBST-CIF	2.3052	0.9740	0.0412

. Figure 3, Figure 4, Figure 5 and Figure 6 demonstrate that the proposed VBST-CIF outperforms conventional CIF in position, velocity, and turning-rate estimation. Precisely, Table 1 shows that VBST-CIF yields an MRMSE improvement with respect to conventional CIF of respectively 56% in position, 48% in velocity, and 20% in turning-rate.

5.2. VBST-CIFF Multi-Sensor Target Tracking

Let us now consider a multi-sensor target (car) tracking scenario with two (radar) sensors $S_1$ and $S_2$, located at $(10,10)$ and respectively at the origin $(0,0)$. The car’s state is taken as $x={[\varsigma ,\dot{\varsigma },\mu ,\dot{\mu }]}^{\mathrm{T}}$ where $\varsigma$, $\dot{\varsigma }$, $\mu$, and $\dot{\mu }$ have the same same meaning as in Section 5.1. The state transition matrix in (92) is:

$$F=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].$$

The process noise covariance is $Q_{k-1}=I_4$, where $I_4$ denotes the $4\times 4$ identity matrix. Furthermore, the measurement model is:

$$\begin{array}{cc}\hfill y_{k,s}& =\left[\begin{array}{c}\sqrt{{({\varsigma }_k-{\varsigma }^s)}^2+{({\mu }_k-{\mu }^s)}^2}\hfill \\ atan2\left({\varsigma }_k,{\mu }_k\right)\hfill \end{array}\right]+v_{k,s}\hfill \end{array}$$

(99)

where: $s=1,2$ refers to $S_1$, $S_2$; $y_{k,s}$ denotes the measurement of radar s at time k; ${[{\varsigma }^s,{\mu }^s]}^T$ refers to the known position of radar s, specifically ${[{\varsigma }^1,{\mu }^1]}^T={[10,10]}^T$ and ${[{\varsigma }^2,{\mu }^2]}^T={[0,0]}^T$; $v_{k,s}$ is the heavy-tailed noise of radar s at time k generated as follows:

$$\begin{array}{c}\hfill v_{k,1}\sim \left\{\begin{array}{cc}\mathcal{N}\left(0,{\Lambda }_{k,1}\right),\hfill & \hfill \mathrm{with}\mathrm{probability}0.9\\ \mathcal{N}\left(0,100{\Lambda }_{k,1}\right),\hfill & \hfill \mathrm{with}\mathrm{probability}0.1\end{array}\right.\end{array}$$

(100)

$$\begin{array}{c}\hfill v_{k,2}\sim \left\{\begin{array}{cc}\mathcal{N}\left(0,{\Lambda }_{k,2}\right),\hfill & \hfill \mathrm{with}\mathrm{probability}0.9\\ \mathcal{N}\left(0,100{\Lambda }_{k,2}\right),\hfill & \hfill \mathrm{with}\mathrm{probability}0.1\end{array}\right.\end{array}$$

(101)

where ${\Lambda }_{k,1}=\left[\begin{array}{cc}{15}^2\left[{\mathrm{m}}^2\right]& 0\\ 0& 0.{015}^2\left[{\mathrm{rad}}^2\right]\end{array}\right]$ and ${\Lambda }_{k,2}=\left[\begin{array}{cc}{10}^2\left[{\mathrm{m}}^2\right]& 0\\ 0& 0.{01}^2\left[{\mathrm{rad}}^2\right]\end{array}\right].$ Initial values for $x_0$ and $P_0$ are set as follows:

$$\begin{array}{cc}\hfill x_0& ={[50\left[\mathrm{m}\right],10\left[\mathrm{m}/\mathrm{s}\right],80\left[\mathrm{m}\right],20\left[\mathrm{m}/\mathrm{s}\right]]}^{\mathrm{T}}\hfill \end{array}$$

(102)

$$\begin{array}{cc}\hfill P_0& =\left[\begin{array}{cccc}1\left[{\mathrm{m}}^2\right]& 0& 0& 0\\ 0& 1[{\mathrm{m}}^2/{\mathrm{s}}^2]& 0& 0\\ 0& 0& 1\left[{\mathrm{m}}^2\right]& 0\\ 0& 0& 0& 1[{\mathrm{m}}^2/{\mathrm{s}}^2]\end{array}\right].\hfill \end{array}$$

(103)

Monte Carlo simulations have been carried out with 100 independent trials. The CIFF of Section 4.1 and the proposed VBST-CIFF of Section 4.2 are compared in the above described multi-sensor target tracking scenario. Specifically, in CIFF, Q and R are fixed to $Q=Q_{k-1}$ for both sensors, $R={\Lambda }_{k,1}$ for sensor 1, and $R={\Lambda }_{k,2}$ for sensor 2. Conversely, in the proposed VBST-CIFF, $Q=Q_{k-1}$, while the scale matrix of each sensor as well as other parameters are adaptively estimated. The same values for the simulation time t, sampling interval T, forgetting factor $\tau$, number of VB iterations N, as well as initial parameters of the scale matrix ($\delta$, $\Delta$), initial parameters of the DOF parameter ($\varphi$, $\Phi$) of the previous single-sensor case-study of Section 5.1 have been adopted.

The results are presented in Figure 7, Figure 8, Figure 9 and Figure 10 and

Table 2. MRMSEs in multi-sensor target tracking.

Filter	Sensor	${RMSE}_{\mathit{p}}$	${RMSE}_{\mathit{v}}$
CIF	1	40.1874	7.5373
CIF	2	27.1914	5.4180
CIFF	Fusion	24.1697	5.4225
VBST-CIF	1	15.5891	3.5502
VBST-CIF	2	11.4030	3.2353
VBST-CIFF	Fusion	10.1193	3.1117

. It is clear from Figure 7 and Figure 8 that the proposed VBST-CIF provides, compared to conventional CIF, smaller RMSEs in both position and velocity for each local sensor during the whole simulation time. Figure 10 shows that the proposed VBST-CIFF outperforms the conventional CIFF, at the fusion center during the whole simulation time. Furthermore, it can be seen that in Table 2 the proposed VBST-CIFF provides an MRMSE improvement of 74% in position and 58% in velocity compared to conventional CIF with sensor 1, as well as an improvement of 62% in position and 42% in velocity compared to CIF with sensor 2. Moreover, the proposed VBST-CIFF provides an MRMSE improvement of 58% in position and 42% in velocity compared to CIFF.

5.3. Experimental Case-Study

Let us consider the experimental setup of Figure 11. A model-car moves along a straight line with a nearly constant velocity starting from initial position $(1018,2619)\left[\mathrm{mm}\right]$. A depth camera (Camera 1, Intel RealSense Depth Camera D435i depth sensor) located at $(0,0)$ measures its distance from the moving car, while another depth camera (Camera 2, Intel RealSense Tracking Camera T265), located onboard the car can get the location and azimuth angle of the car with respect to Camera 1. Please notice that the performance of Camera 1 is highly affected by the unstable Tiny YOLOv3 detector. Thus, the measurement function is the same as in (93) with a measurement noise of unknown heavy-tailed distribution. The total sampling samples are 363, with a sampling interval of $T=1/60\left[\mathrm{s}\right]$. Moreover, initial values of the model-car are selected as:

$$\begin{array}{cc}\hfill x_0& ={[1018\left[\mathrm{mm}\right],2619\left[\mathrm{mm}\right],228.76\left[\mathrm{mm}/\mathrm{s}\right],311.90\left[\mathrm{mm}/\mathrm{s}\right]]}^{\mathrm{T}}\hfill \end{array}$$

(104)

$$\begin{array}{cc}\hfill P_0& =\left[\begin{array}{cccc}1\left[{\mathrm{mm}}^2\right]& 0& 0& 0\\ 0& 1\left[{\mathrm{mm}}^2\right]& 0& 0\\ 0& 0& 1[{\mathrm{mm}}^2/s^2]& 0\\ 0& 0& 0& 1[{\mathrm{mm}}^2/{\mathrm{s}}^2]\end{array}\right].\hfill \end{array}$$

(105)

Let us consider the experimental results reported in Figure 12 and Figure 13 and

Table 3. MRMSEs in the experimental case study.

Filter	${RMSE}_{\mathit{p}}$ [mm]	${RMSE}_{\mathit{v}}$ [mm/s]
CIF	11.7397	1.1985
VBST-CIF	6.3111	1.0991

. Figure 12 shows the true and estimated car trajectories, from which it can be seen that the proposed VBST-CIF can track the car more accurately. Position and velocity RMSEs of both conventional CIF and proposed VBST-CIF are reported in Figure 13. Specifically, the proposed VBST-CIF provides an improvement of 46.24% in position MRMSE and of 0.08% in velocity MRMSE, with respect to conventional CIF, thus demonstratiung the effectiveness and superiority of the proposed VBST-CIF.

6. Conclusions

This paper focused on state estimation and information fusion in multi-sensor nonlinear systems with heavy-tailed measurement noise. An adaptive variational Bayes Student’s t cubature information filter (VBST-CIF) algorithm was first designed for a nonlinear single-sensor system based on the cubature information filter (CIF) framework, wherein the Student’s t (ST) distribution was utilized to model the heavy-tailed measurement noise, and the variational Bayes (VB) method along with the spherical-radial cubature (SCR) rule were exploited in order to jointly estimate the system state as well as noise statistics. The VBST-CIF facilitated multi-sensor fusion, allowing to derive a VBST cubature information feedback fusion (VBST-CIFF) algorithm for nonlinear multi-sensor systems with heavy-tailed measurement noise. The filtering accuracy of the proposed algorithms was assessed in single-sensor and multi-sensor target tracking scenarios. Simulation and experimental results demonstrated that the proposed VBST-CIF/VBST-CIFF outperformed conventional CIF/CIFF algorithms. Future work will focus on consensus filtering for multi-agent systems with heavy-tailed process and measurement noise.