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

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.


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. 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.

Problem Formulation
Consider the following model: 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 ∼ N (0, Q k ), while v k is heavy-tailed measurement noise; N (µ, Σ) denotes a Gaussian random variable with mean µ and variance Σ. 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 (2) 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.

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: where I(·) is a nonlinear function and R n denotes the integration domain. Let x = rg with g T g = 1 and r ∈ [0, ∞], so that x T x = r 2 , where g is a direction vector and r is the radius. Then, integration in (3) can be rewritten as: where Ξ = {g ∈ R n |g T g = 1} denotes an n-dimensional unit sphere surface and σ(·) is an element on Ξ. Thus, the integration in (4) can be transformed into a spherical integration: and a radial integration: Hence, for a Gaussian integration, it turns out that: where: √ Σ can be obtained by performing Cholesky decomposition of Σ;

Student's t Distribution and Time Update
The ST distribution [38] can be described as: where: St(·; µ, M, κ) represents the ST PDF with mean µ, scale matrix M, and degree of freedom (DOF) parameter κ; γ denotes an auxiliary parameter; N (·; µ, M) is the Gaussian PDF with mean µ 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 κ k [38] as parameters, i.e., where γ k denotes the auxiliary parameter at time k.
Suppose that the prior state estimate and SECM arex k−1|k−1 and P k−1|k−1 , respectively. By applying the SRC rule to the prior information, we have: 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 {e} in the SRC rule; χ i,k−1 denotes the i-th sampling point; n x is the dimension of x k ; Ψ i,k−1 denotes the corresponding transformed i-th sampling obtained by propagating χ i,k−1 through f (·). To this end, the predictedx k|k−1 as well as P k|k−1 are obtained as: Thus, the predicted information matrix and state vector, Z k|k−1 and ζ k|k−1 , are obtained through

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 κ k , and auxiliary parameter γ k , denoted as p(x k , R k , κ k , γ k |y 1:k ), needs to be computed. However, for the nonlinear system (1) and (2), an analytical solution for p(x k , R k , κ k , γ 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., q * = arg min KLD (q||p) (24) where the KLD is defined as: Hence, the problem can be transformed into searching an approximate q(·) that minimizes the KLD between itself and the true p(·), i.e., The joint posterior distribution of the parameters to be estimated is approximated via the following factored form [36]: where q x (·), q R (·), q κ (·), and q γ (·) represent approximate posterior PDFs for x k , R k , κ k , and γ k , respectively. By applying logarithm to (27), it can be deduced that: where: Ω is the set of parameters (x k , R k , κ k , γ k ) that need to be estimated; ξ is an element of Ω; Ω (−ξ) denotes the complementary set of ξ in Ω; E(·) denotes the expectation operator; c ξ is a quantity that is independent of ξ. By exploiting the hierarchical Gaussian form in [38], it can be obtained that: from which we have: can be obtained by: where B (j) k can be obtained via the SRC rule as follows: Next, setting ξ = γ k , we get: from which: where:â Then, setting ξ = κ k , we get from which we have: where:φ according to which we have: and P (j+1) k|k are the estimated state and SECM in the j-th VB iteration, respectively.
In order to obtainx should be first computed. By utilizing a linearized error propagation method [26], they can be obtained as follows: whereR (j+1) k|k 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: Similarly, taking into account (40) and (44) as well as a property of the Gamma distribution, we have: Then,R (j+1) k|k is computed as:R Moreover, the pseudo-measurement matrix H k in (49) and (50) can be obtained as: where P xz,k|k−1 is computed by means of the SRC rule as follows: Then, the information matrix and vector are updated as follows: Finally, we have:x where I n x denotes the n x × n x identity matrix. To summarize the above developments, the pseudocode of VBST-CIF is reported in Algorithm 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 ∈ {1, 2, . . . , S}, a measurement y k,s is available at time k, i.e., 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 = n. Let, at time k − 1, the state estimate and SECM for all sensors bex k−1|k−1 and, respectively, P k−1|k−1 . Correspondingly, the initial information vector and matrix for all sensors are ζ k−1|k−1 and Z k−1|k−1 , respectively. Then, by exploiting the SRC rule, predicted ζ k−1|k and Z k−1|k can be obtained, i.e., where Z k−1|k−1 is the inverse of P k−1|k−1 . Transforming χ i,k−1 in (18) through f (·), we have Ψ i,k−1 in (19). Then, predicted Z k|k−1 and ζ 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: where: the local sensor contributions I k,s and ι k,s are given by: 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;ŷ k|k−1,s can be obtained by the SRC rule as in the conventional CIF [25].
Then, in the measurement update, the estimated scale matrix for sensor s is given by: k|k,s can be obtained via: where B (j) k,s can be derived in the same way as in (34)- (38) with cubature sampling points transformed through h s (·) as: in which the original sampling points χ i,k|k−1,s are given by (56) and (57). Furthermore, E (j+1) (γ k,s ) for sensor s can be obtained by: where,â and E (j+1) (κ k,s ) is given by: in which:φ The pseudo-measurement matrix for sensor s turns out to be: where P T xz,k|k−1,s can be obtained via Hence, the information matrix and vector contributions due to sensor s can be derived as follows: from which the information matrix and vector can be updated according to 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 , ζ k into the local filters (see Figure 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 = [ς,ς, µ,μ, ω] T , where ς and µ are the Cartesian coordinates of position, whileς andμ represent the corresponding velocity, and ω is the turning rate. The car moves according to the coordinated turn (CT) model: with a turning-rate dependent state-transition matrix: fixed at turning rate ω = 10 • ; w k−1 = N (0, Q k−1 ); with noise intensity factors p 1 = 0.1, p 2 = 1.75 × 10 −4 and sampling interval T = 1 [s]. Conversely, the measurement function of the radar is: where: atan2(ς, µ) denotes the 4-quadrant inverse-tangent defined as the argument of the complex number ς + jµ, j denoting the imaginary unit; v k is heavy-tailed measurement noise such that: . Moreover, the initial values for state and SECM of the car are set as follows: A total of 100 independent Monte Carlo trials have been carried out, with each trial lasting for t = 50 [s]. Furthermore, the root-mean-square-error (RMSE) is adopted to assess tracking precision. For variable b at time k, it is defined as: where: N MC denotes the number of Monte Carlo trials; b denotes either the position p = [ς, µ] T or velocity v = [ς,μ] T ; and b i k is the true value in the i-th trial at time k. Conversely,b i k 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: 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 = Λ 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 τ = 0.9; number of VB iterations N = 10; initial noise parameters of the scale matrix δ = n z + 2 = 4 and ∆ = Λ k ; initial noise parameters of the DOF parameter φ = 5 and Φ = 1.
Results are presented in Figures 3-6 and Table 1. Figures 3-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.

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 = [ς,ς, µ,μ] T where ς,ς, µ, andμ have the same same meaning as in Section 5.1. The state transition matrix in (92) is: The process noise covariance is Q k−1 = I 4 , where I 4 denotes the 4 × 4 identity matrix. Furthermore, the measurement model is: where: s = 1, 2 refers to S 1 , S 2 ; y k,s denotes the measurement of radar s at time k; [ς s , µ s ] T refers to the known position of radar s, specifically [ς 1 , µ 1 ] T = [10,10] T and [ς 2 , µ 2 ] T = [0, 0] T ; v k,s is the heavy-tailed noise of radar s at time k generated as follows: v k,1 ∼ N (0, Λ k,1 ) , with probability 0.9 N (0, 100Λ k,1 ) , with probability 0.1 (100) v k,2 ∼ N (0, Λ k,2 ) , with probability 0.9 N (0, 100Λ k,2 ) , with probability 0.1 where 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 = Λ k,1 for sensor 1, and R = Λ 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 τ, number of VB iterations N, as well as initial parameters of the scale matrix (δ, ∆), initial parameters of the DOF parameter (φ, Φ) of the previous single-sensor case-study of Section 5.1 have been adopted.
The results are presented in Figures 7-10 and Table 2. It is clear from Figures 7 and 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.

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) [mm]. 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)  Let us consider the experimental results reported in Figures 12 and 13 and Table 3. 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.

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.