A Novel Variational Bayesian Method with Unknown Noise for Underwater INS/DVL/USBL Localization

Abstract

In the complex underwater environment, it is hard to obtain accurate system noise prior information. If uncertainty system noise model is used in state determination, the precision will decrease. To address the problem, this paper proposes a novel inverse-Wishart (IW) based variational Bayesian adaptive cubature Kalman filter (IW-VACKF), and the inverse-Wishart distribution is employed as the conjugate prior distribution of system noise covariance matrices. To improve the modeling accuracy, a mixing probability vector is introduced based on the inverse-Wishart distribution to better characterize the uncertainty and dynamic of state noise in underwater environments. Then, the state transition and the measurement process are derived as hierarchical Gaussian models. Subsequently, the posterior information of the system is jointly calculated by employing the variational Bayesian method. Simulations and real trials illustrate that the proposed IW-VACKF can improve the state estimation precision efficiently in the complex underwater environment.

1. Introduction

With the rapid development of ocean exploration, underwater navigation technology has attracted increasing attention across various fields [1,2]. Underwater environments present unique challenges for signal transmission, as electromagnetic signals cannot propagate effectively over long distances. In contrast, acoustic signals exhibit superior propagation capabilities, making acoustic-based sensors indispensable for underwater positioning [3,4,5]. Figure 1 illustrates the structure of a typical underwater navigation system, which relies on the integration of multiple sensors, including Doppler Velocity Log (DVL), Inertial Measurement Unit (IMU), and Ultra-Short Baseline (USBL). Each sensor plays a critical role in providing essential navigation data. The DVL and USBL utilize acoustic signals to measure the velocity and position of underwater devices [6,7,8], while the IMU calculates position and velocity based on angular velocity and specific force measurements [9]. By fusing data from these sensors, the system can provide accurate and reliable navigation information [10,11]. However, the performance of these sensors is significantly affected by the complex and noisy underwater environment. Acoustic signals are susceptible to interference, leading to inaccuracies in the measurements provided by DVL and USBL. Additionally, the data of IMU can drift over time due to the accumulation of errors. These challenges highlight the need for robust data processing algorithms to mitigate the impact of noise and ensure the reliability of the navigation system [12,13]. Therefore, the accuracy and reliability of underwater navigation systems largely depend on the ability to address the challenges associated with different sensors.

To address these problems, data fusion techniques based on state estimation theory have been widely adopted. The extended Kalman filter (EKF) is a powerful technique for position estimation, which is derived based on the Taylor’s formula and Bayes principle. However, when the system is strongly nonlinear, the EKF usually performs poorly. With the application of the unscented transformation, the unscented Kalman filter (UKF) has a satisfactory accuracy in a system with extreme nonlinearity. Unfortunately, the UKF suffers from the curse of dimensionality. When the system is a high-dimensional state space, the accuracy of UKF will decrease significantly. By introducing the cubature sampling rule, the cubature Kalman filter (CKF) avoids the curse of dimensionality [14,15,16,17].

However, the above algorithms are fundamentally limited by their assumption of precisely known noise statistics, a requirement rarely met in practical underwater environments. The prior information of the true noise covariance matrix (TNCM) plays an essential role in the state estimation of the above algorithms. An incorrect noise covariance matrix (NCM) may cause the filter to produce significant determination errors or even experience filtering divergence [18,19]. In the underwater environment, due to the unknown disturbance, the TNCM is hard to determine and may be time-varying. To specifically address these challenges posed by unknown noise characteristics in the underwater localization, various adaptive Kalman filters (AKF) have been investigated. For instance, by reconstructing the model and introducing the maximum information potential (MIP) criterion with adaptive kernel bandwidth, a novel adaptive filtering algorithm to address compass failure and non-Gaussian measurement noise in multi-AUV cooperative localization is proposed [20]. Using variational Bayesian (VB) to estimate measurement noise covariance and a variable kernel width MCC strategy to suppress outliers, a VB-based Maximum Correntropy Criterion (MCC) adaptive Kalman filter can tackle unknown, time-varying noise and measurement outliers in SINS/USBL integrated navigation systems [21]. Furthermore, using VB inference for state and process and measurement noise covariances (PMNC) estimation, a variational Bayesian adaptive Kalman filter for scenarios with inaccurate PMNC and outliers, modeling state transition and measurement likelihood state probability density function (PDF) as Gaussian–Gamma mixture distributions [22]. These studies represent significant efforts to improve positioning accuracy by more effectively modeling and mitigating the impact of complex underwater noise and system uncertainties.

These AKFs are usually considered an effective approach to address the issue. The AKF obtains the optimal state estimation by adaptively adjusting the filtering parameters based on system measurement and working environment [23]. The VB-based AKF (VBAKF) is a classical adaptive Kalman filtering algorithm. To achieve precise state estimation, the VBAKF models the state transition and measurement process by selecting an appropriate conjugate prior distribution; thereby, the joint PDF is established. Then, the system state can be approximately acquired by minimizing the Kullback–Leibler divergence (KLD) between the prior and posterior PDF [24,25]. But the VBAKF cannot provide accurate state estimation in nonlinear systems. To solve this problem, the VB method is usually combined with linear approximation approaches, such as unscented transformation or the cubature sampling rule, to perform state determination in nonlinear system. The robust CKF based on VB and transformed posterior sigma points error and the inverse-Wishart-based VBCKF (VBCKF-QR) are all designed based on this method [26]. In the underwater environment, due to various unknown disturbances underwater such as ocean current impact and electromagnetic interference, the state noise is often significantly more complex than measurement noise.

By introducing a mixing probability vector to describe the statistical characteristics of state noise, the Gaussian-inverse-Wishart mixture (GIWM) distribution is designed. By introducing the mixing probability vector into the modeling of system noise, the GIWM is suitable for modeling this complex noise with a strongly inaccurate covariance matrix. By utilizing the VB approach to estimate system state, the variational AKF with the GIWM distribution (VAKF-GIWM) is proposed [27]. Nevertheless, the VAKF-GIWM has two disadvantages: Firstly, it is hard to acquire the analytical solution of system state in a non-linear system. Secondly, in the process of modeling state transition, it is necessary to use the previous state, leading to the repeated estimation of the state at the last time, so the application scope of the VAKF-GIWM is limited due these problems.

In order to accurately estimate the system state in underwater nonlinear system, a novel inverse-Wishart based variational Bayesian adaptive cubature Kalman filter (IW-VACKF) is proposed in this paper. By introducing the mixing probability, the state transition process is modeled and the cubature sampling rule is integrated into the approximate calculation of the nonlinear integral part. Meanwhile, Taylor’s formula is utilized to simplify the nonlinear calculate part in the variational iterative process. To avoid repeated calculations of the system state from the previous step, the state estimation results from the last moment are directly used in the iterative calculation at the current moment. The highlights of this work are shown below:

(1)

To accurately describe underwater noise, the IW distribution and mixing probability vectors are introduced, and the state transition and the measurement process are derived as hierarchical Gaussian models.

(2)

To acquire the optimal position estimation, the IW-VACKF is proposed by using the VB method and cubature sampling rule.

(3)

The proposed IW-VACKF has been successfully validated in practical experiments, demonstrating superior performance in enhancing the accuracy and robustness of multi-sensor integrated navigation systems under complex underwater environments.

The main structure of this paper is formed below. Section 2 shows that the formulation of the nonlinear state-space model is displayed and the formulation of the cubature sampling rule is presented. In Section 3, the state transition and system measurement are described by inverse-Wishart distribution and the mixing probability vector. Section 4 introduces the IW-VACKF. In Section 5, the proposed IW-VACKF is compared with the UKF, CKF, VBCKF-QR, and the optimal filter, under simulations and surface vessel trials performed in the Yangtze River, China. Finally, the conclusion is drawn in Section 6.

2. Problems Model and Cubature Kalman Filter

2.1. State-Space Model for INS/DVL/USBL Integrated Navigation

To achieve accurate and reliable navigation information, the problem model considered in this paper is shown:

$$\left\{\begin{array}{l}{x}_k=\mathrm{f}\left(x_{k-1}\right)+w_{k-1}\\ z_k=\mathrm{h}\left(x_k\right)+v_k\end{array}\right.$$

(1)

where $x_k\in {ℝ}^m$ and $n$ represent the state vector and the dimension of the state vector, and $x_k$ include velocity state vector and position state vector; $z_k\in {ℝ}^m$ and m are the measurement vector and the dimension of the measurement vector, and $z_k$ include measurement data at the time k for USBL, DVL, and IMU; $\mathrm{f}\left(·\right)$ and $\mathrm{h}\left(·\right)$ represent the state transition function and the measurement update function, usually non-linear; $w_{k-1}$ and $v_k$ are the system state noise in time k−1 and measurement noise in time k, respectively; and they are fit the Gaussian distribution, i.e., $w_{k-1}~N\left(0,Q_{k-1}\right)$ and $v_k~N\left(0,R_k\right)$ [28]. However, in the underwater environment, the NCM and $R_k$ are time-varying and non-constant; the system of Equation (1) is called as non-Gaussian noise system.

2.2. Cubature Kalman Filter

According to the model (1) and cubature sampling rule principle [29], the prior PDF $p\left(x_k\right)$ is modeled by $x_{k-1}~N\left({\widehat{x}}_{k|k-1},P_{k|k-1}\right)$. ${\widehat{x}}_{k|k-1}$ is the one-step prediction of state vector; $P_{k|k-1}$ the one-step prediction error covariance matrix; $x_0$ is the initial state vector, and it satisfies $N\left({\widehat{x}}_{0|0},P_{0|0}\right)$ which is independent with state noise $w_{k-1}$ and measurement noise $v_k$. Based on the above conditions, the posterior PDF $p\left(x_k\right)$ at time k can be determined with the posterior PDF $p\left(x_{k-1}\right)$ and the measurement $z_k$. Suppose $x_k~N\left(\widehat{x},P_{k|k}\right)$, and ${\xi }_i$ for CKF is selected as:

$${\xi }_i=\left\{\begin{array}{l}\sqrt{n}{e}_i, i=1,2,\dots ,n\\ -\sqrt{n}{e}_{i-n}, i=n+1,n+2,\dots ,2n\end{array}\right.$$

(2)

where $e_i$ is the i-th elementary column vector. Define root mean square $S_{k|k-1}=\mathrm{chol}\left({\mathrm{P}}_{k|k-1}\right)$, $S_{k-1|k-1}=\mathrm{chol}\left({\mathrm{P}}_{k-1|k-1}\right)$ and m = 2n. The mean state vector ${\widehat{x}}_{k|k}$ and error covariance matrix $P_{k|k}$ can be determined by employing the CKF as:

(1)

Time update

By using (3), the cubature points $X_{i,k-1|k-1}^{+}$ are updated:

$$X_{\mathrm{i},k-1|k-1}^{+}=S_{k-1|k-1}{\xi }_i+{\widehat{x}}_{k-1|k-1}, S_{k-1|k-1}=\mathrm{chol}\left(P_{k-1|k-1}\right)$$

(3)

where $i=1,\dots ,m$.

Determine the state predicted vector and covariance predicted matrix as follows:

$$\left\{\begin{array}{l}{\widehat{x}}_{k|k-1}={\displaystyle \sum _{i=1}^m{\omega }_{i}f\left(X_{i,k-1|k-1}^{+}\right)}\\ P_{k|k-1}={\displaystyle \sum _{i=1}^m{\omega }_{i}f\left(X_{i,k-1|k-1}^{+}\right)f{\left(X_{i,k-1|k-1}^{+}\right)}^{\mathrm{{\rm T}}}}-{\widehat{x}}_{k|k-1}{\widehat{x}}_{k|k-1}+Q_{k-1}\end{array}\right.$$

(4)

(2)

Measurement update

Renew cubature points as:

$$X_{\mathrm{i},k|k-1}^{-}=S_{k|k-1}{\xi }_i+{\widehat{x}}_{k|k-1}, S_{k|k-1}=\mathrm{chol}\left(P_{k|k-1}\right)$$

(5)

Calculate measurement predicted vector and covariance predicted matrices as:

$$\left\{\begin{array}{l}{\widehat{z}}_{k|k-1}={\displaystyle \sum _{i=1}^m{\omega }_{i}h\left(X_{i,k|k-1}^{-}\right)}\\ P_{zz,k|k-1}={\displaystyle \sum _{i=1}^m{\omega }_{i}h\left(X_{i,k|k-1}^{-}\right)h{\left(X_{i,k|k-1}^{-}\right)}^{\mathrm{{\rm T}}}}-{\widehat{z}}_{k|k}{\widehat{z}}_{k|k-1}^{\mathrm{{\rm T}}}+R_k\\ P_{xz,k|k-1}={\displaystyle \sum _{i=1}^m{\omega }_i{X}_{i,k|k-1}^{-}h{\left(X_{i,k|k-1}^{-}\right)}^{\mathrm{{\rm T}}}}-{\widehat{x}}_{k|k}{\widehat{z}}_{k|k-1}^{\mathrm{{\rm T}}}\end{array}\right.$$

(6)

Then the system state vector ${\widehat{x}}_{k|k}$ and the predictive error covariance matrix $P_{k|k}$ can be estimated as:

$$\left\{\begin{array}{l}{\widehat{x}}_{k|k}={\widehat{x}}_{k|k-1}+K_k\left(z_k-{\widehat{z}}_{k|k-1}\right)\\ K_k=P_{xz,k|k-1}{\left(P_{zz,k|k-1}\right)}^{-1}\\ P_{k|k}=P_{k|k-1}-K_k{P}_{zz,k|k-1}{K}_k^{\mathrm{{\rm T}}}\end{array}\right.$$

(7)

By the analyses of Equations (2)–(7), it is found that the CKF is established on the basis of a Gaussian noise system, which the SNCM $Q_k$ and MCNM $R_k$ are constant. However, the underwater system is non-Gaussian noise system, so in this case, the CKF has a low estimation accuracy.

3. Inverse Wishart-Based Prior Modeling of System Noise

3.1. Measurement Likelihood PDF

According to the Bayesian principle, the IW distribution can be seen as the conjugate prior of the unknown covariance matrix of Gaussian distribution, and the PDF of IW distribution is shown as [30]:

$$\mathrm{IW}\left(B;\zeta ,\Psi \right)={\displaystyle \frac{\left|\Psi {|}^{\zeta /2}\right|B{|}^{-(\zeta +d+1)/2}{e}^{-tr(\Psi B^{-1})/2}}{2^{d\zeta /2}{\mathrm{\Gamma }}_d\left(\zeta /2\right)}}$$

(8)

where $B$ and $\Psi$ are the positive definite random matrix and the inverse scale matrix, respectively; $\mathrm{tr}(\cdot )$ represents the trace operation of matrix; ${\mathrm{\Gamma }}_d(\cdot )$ is the d-variate Gamma function; $\zeta$ and d are the degrees of freedom (DoF) parameter and the dimension of the matrix $B$.

The expected value of the positive random matrix $B$ can be calculated as follows:

$$B~\mathrm{IW}\left(\zeta ,\Psi \right)\Rightarrow \mathrm{E}\left[B^{-1}\right]=\left(\zeta -d-1\right){\Psi }^{-1}$$

(9)

Because state noise $w_k$ and measurement noises $v_k$ represent non-Gaussian noise, the IW distribution can be seen as the conjugate prior of NCM. Then, the likelihood PDFs of state transition and measurement are established.

As analyses of Equations (8) and (9), the MNCM $R_k$ can be modeled as follows:

$$p\left(R_k\right)=\mathrm{IW}\left(R_k;{\widehat{u}}_{k|k-1},{\widehat{U}}_{k|k-1}\right)$$

(10)

where ${\widehat{u}}_{k|k-1}$ and ${\widehat{U}}_{k|k-1}$ are the DoF parameter and the inverse scale matrix, respectively. And the measurement likelihood PDFs can be described as follows:

$$p\left(z_k|x_k\right)={\displaystyle \int \mathrm{N}\left(z_k;\mathrm{h}\left(x_k\right),R_k\right)\mathrm{IW}}\left(R_k;{\widehat{u}}_{k|k-1},{\widehat{U}}_{k|k-1}\right)\mathrm{d}{R}_k$$

(11)

3.2. State Transition Likelihood PDF

In the complex underwater environment, due to various sudden interferes, the statistical characteristics of state noise are more complex than measurement noise. Therefore, the model shown in Equation (11) cannot accurately describe the system state transition process. To improve the modeling accuracy, the random mixing probability vector ${\tau }_k$ is introduced [24]. Then the conditional PDF of SNCM $Q_k$ is modeled as follows:

$$p\left(Q_k|{\tau }_k\right)={\displaystyle \sum _{j=1}^M{\tau }_j\mathrm{IW}\left(Q_k;{\widehat{t}}_{j,k|k-1},{\widehat{T}}_{j.k|k-1}\right)}, \mathrm{s}.\mathrm{t}., {\displaystyle \sum _{j=1}^M{\tau }_j=1}$$

(12)

The random mixing probability vector ${\tau }_k$ is assumed as Dirichlet distributed, i.e.,

$$p\left({\tau }_k\right)=\mathrm{Dir}\left({\tau }_k;{\widehat{\alpha }}_{k|k-1},M\right)$$

(13)

where ${\widehat{\alpha }}_{k|k-1}=\left[{\alpha }_1,\dots ,{\alpha }_j\right]$ is the prior concentration parameter vector satisfying ${\alpha }_j>0$. By using the categorical distributed random vector ${\lambda }_k$, Equation (12) is modeled as hierarchical Gaussian form, the categorical distributed random vector ${\lambda }_k$ is defined as:

$$\lambda =[{\lambda }_1,\dots ,{\lambda }_M], \mathrm{s}.\mathrm{t}. {\displaystyle \sum _{j=1}^M{\lambda }_j=1}$$

(14)

where the categorical element ${\lambda }_j=0$ or ${\lambda }_j=1$, the categorical vector $\lambda$ is an identity vector. According to above condition, the categorical distributed random vector ${\lambda }_k$ only has $M$ possible values, i.e., $\lambda \in {\left\{e_j\right\}}_{j=1}^M$, where $e_j$ is the j-th column of a unit matrix. To model the Equation (12) as hierarchical Gaussian form, the probability of ${\lambda }_k=e_j$ is defined as ${\tau }_j$, i.e.,

$$\mathrm{Pr}\left({\lambda }_k=e_j\right)={\tau }_j, j=1,2,\dots ,M, \mathrm{s}.\mathrm{t}.{\displaystyle \sum _{j=1}^M{\tau }_j=1}$$

(15)

where the probability value ${\tau }_j$ is the j-th element of the mixing probability vector ${\tau }_k$.

Using Equation (13), the PDF of the random vector ${\lambda }_k$ can be formulated by mixing probability vector ${\tau }_k$:

$$p\left({\lambda }_k|{\tau }_k\right)=\mathrm{Cat}\left({\lambda }_k;{\tau }_k,M\right)$$

(16)

By Equations (12)–(17), the PDF of SNCM $Q_k$ can be modeled as follows hierarchical Gaussian form:

$$p(Q_k)={\displaystyle \prod _{j=1}^M{\left[\mathrm{IW}\left(Q_k;{\widehat{\mathrm{t}}}_{j,k|k-1}{\widehat{T}}_{j,k|k-1}\right)\right]}^{{\lambda }_{j,k}}}\mathrm{Cat}\left({\lambda }_k;{\tau }_k,M\right)\times \mathrm{Dir}\left({\tau }_k;{\widehat{\alpha }}_{k|k-1},M\right)$$

(17)

Then the state transition likelihood PDF is described as follows:

$$\begin{array}{c}p\left(x_k|x_{k-1}\right)={\displaystyle \iiint N\left(x_k;\mathrm{f}\left(x_{k-1}\right),Q_k\right)}{\displaystyle \prod _{j=1}^M}{\left[\mathrm{IW}\left(Q_k;{\widehat{t}}_{j,k|k-1},{\widehat{T}}_{j,k|k-1}\right)\right]}^{{\lambda }_{j,k}}\mathrm{Cat}\left({\lambda }_k;{\tau }_k,M\right)\\ \times \mathrm{Dir}\left({\tau }_k,{\widehat{\alpha }}_{k|k-1},M\right)\mathrm{d}{Q}_k\mathrm{d}{\lambda }_k\mathrm{d}{\tau }_k\end{array}$$

(18)

According to Equations (11) and (18), the hierarchical Gaussian forms of state transition and measurement transition are established. In Section 4, a novel VACKF will be derived based on VB method and cubature sampling rule.

4. The Proposed IW-VACKF

4.1. VB Method

Based on the state transition likelihood PDF Equation (18) and measurement likelihood PDF Equation (11), the optimal estimate of the joint vector ${\Theta }_k\triangleq \left\{x_k,x_{k-1},Q_k,R_k,{\lambda }_k,{\tau }_k\right\}$ can be obtained by using the VB method. To use the VB method, the true posterior PDF $p\left\{{\Theta }_k|z_{1:k}\right\}$ is approximated as follows [31]:

$$p\left\{{\Theta }_k|z_{1:k}\right\}={\displaystyle \prod _{{\theta }_k\in {\Theta }_k}{q}^{*}\left({\theta }_k\right)}$$

(19)

where ${\theta }_k\in {\Theta }_k$. $q^{*}\left({\theta }_k\right)$ represents the approximate posterior PDF for each parameter ${\theta }_k$. The core of the VB method is to find a factorized approximation $q^{*}\left({\theta }_k\right)$ that is as close as possible to the true posterior $p\left\{{\Theta }_k|z_{1:k}\right\}$. This approximation assumes that the posterior distributions of the parameters are independent.

Then, the approximate posterior PDF $q^{*}\left({\theta }_k\right)$ can be acquired by KLD minimization as follows:

$$q^{*}\left({\theta }_k\right)=\arg \underset{q\left({\theta }_k\right)}{\min }\mathrm{KLD}\left({\displaystyle \prod _{{\theta }_k\in {\Theta }_k}q\left({\theta }_k\right)\parallel p\left({\Theta }_k|z_{1:k}\right)}\right)$$

(20)

where the KLD between the prior PDF and posterior PDF is calculated as follows:

$$\mathrm{KLD}\left(q\left({\theta }_k\right)\left|\right|p\left({\theta }_k\right)\right)={\displaystyle \int q\left({\theta }_k\right)\log \left(\frac{q\left({\theta }_k\right)}{p\left({\theta }_k\right)}\right)}d{\theta }_k$$

(21)

Equation (21) defines the KLD, which measures the difference between two probability distributions: the approximate posterior $q\left({\theta }_k\right)$ and the true $p\left({\theta }_k\right)$. Minimizing this divergence means we are seeking an approximate distribution $q^{*}\left({\theta }_k\right)$ that best matches the true posterior $p\left\{{\Theta }_k|z_{1:k}\right\}$ under the assumption of Equation (19).

Then, the optimal approximate PDF solution can be obtained as follows:

$$\log q^{*}({\theta }_k)=E_{{\Theta }_k^{\left(-{\theta }_k\right)}}^{*}\left[\log p\left({\Theta }_k,z_{1:k}\right)\right]+c_{{\theta }_k}$$

(22)

where ${\Theta }_k^{\left(-{\theta }_k\right)}$ satisfies $\left\{{\theta }_k\right\}\cup {\Theta }_k^{\left(-{\theta }_k\right)}$. Equation (22) provides the general form for updating the logarithm of the approximate posterior for a single parameter ${\theta }_k$. It states that this log-posterior is proportional to the expectation of the logarithm of the joint probability of all parameters ${\theta }_k$ and observations $z_{1:k}$, where the expectation $E_{{\Theta }_k^{\left(-{\theta }_k\right)}}^{*}$ is taken with respect to all other parameters ${\Theta }_k^{\left(-{\theta }_k\right)}$ using their current optimal approximate distributions $q^{*}\left(·\right)$. The term $c_{{\theta }_k}$ is a normalization constant.

By using the fixed-point iteration method, the mutual coupling of the variational parameters can be addressed, then Equation (22) is transformed as:

$$\log q^{(i+1)}\left({\theta }_k\right)=E_{{\Theta }_k^{(-{\theta }_k)}}^{(i)}\left[\log p\left({\Theta }_k,z_{1:k}\right)\right]+c_{{\theta }_k}$$

(23)

Equation (23) specifies the iterative update rule. In the i + 1-th iteration, the logarithm of the approximate posterior for ${\theta }_k$, denoted $\log q^{(i+1)}({\theta }_k)$, is updated using the expectations $E_{{\Theta }_k^{\left(-{\theta }_k\right)}}^{*}$ calculated with the approximate posteriors $q^{(i)}\left(·\right)$ from the previous i-th iteration for all parameters other than ${\theta }_k$. This iterative process continues until convergence.

Utilizing the measurement likelihood PDF Equation (11) and state transition likelihood PDF Equation (18), the joint posterior PDF is computed as:

$$\begin{array}{cc}\hfill p\left({\Theta }_k,z_{1:k}\right)& =\mathrm{N}\left(z_k;\mathrm{h}\left(x_k\right),R_k\right)\mathrm{N}\left(x_k;\mathrm{f}\left(x_{k-1}\right),Q_k\right)\hfill \\ & \times {\displaystyle \prod _{j=1}^M{\left[\mathrm{IW}\left(Q_k;{\widehat{t}}_{j,k|k-1},{\widehat{T}}_{j,k|k-1}\right)\right]}^{{\lambda }_{j,k}}}\mathrm{Dir}\left({\tau }_k;{\widehat{\alpha }}_{k|k-1},M\right)\hfill \\ & \times \mathrm{IW}\left(R_k;{\widehat{u}}_{k|k-1},{\widehat{U}}_{k|k-1}\right)\mathrm{Cat}\left({\lambda }_k;{\tau }_k,M\right)p\left(z_{1:k-1}\right)\times \mathrm{N}\left(x_{k-1};{\widehat{x}}_{k-1|k-1},P_{k-1|k-1}\right)\hfill \end{array}$$

(24)

Equation (24) expresses the full joint probability of all parameters ${\theta }_k$ and the history measurement information $z_{1:k}$. It is constructed by multiplying the likelihoods of the current measurement $\mathrm{N}\left(z_k;\mathrm{h}\left(x_k\right),R_k\right)$ and current state $\mathrm{N}\left(x_k;\mathrm{f}\left(x_{k-1}\right),Q_k\right)$, with the prior distributions for the noise covariances $Q_k$ and $R_k$, and the prior for the previous state $\mathrm{N}\left(x_k;\mathrm{f}\left(x_{k-1}\right),Q_k\right)$. The term $p\left(z_{1:k-1}\right)$ represents the evidence from previous measurements.

By introducing forgetting factor $\rho \in (0,1]$ and tuning parameters ${\pi }_k$, the prior statistics parameters of system noise $\left\{{\widehat{u}}_{k|k-1},{\widehat{U}}_{k|k-1}\right\}$,${\left\{{\widehat{t}}_{j,k|k-1},{\widehat{T}}_{j,k|k-1}\right\}}_{j=1}^M$, the prior concentration parameter vector ${\widehat{\alpha }}_{k|k-1}$ can be calculated as follows:

$$\left\{\begin{array}{l}{\widehat{\alpha }}_{k|k-1}=\rho {\widehat{\alpha }}_{k-1|k-1},{\widehat{t}}_{1,k|k-1}=\rho {\widehat{t}}_{k-1|k-1}\\ {\widehat{T}}_{1,k|k-1}=\rho {\widehat{T}}_{k-1|k-1},{\widehat{t}}_{j,k|k-1}={\pi }_k,{\widehat{T}}_{j,k|k-1}={\pi }_k{\widehat{Q}}_{j,k}\\ {\widehat{u}}_{k|k-1}=\rho {\widehat{u}}_{k|k-1},{\widehat{U}}_{k|k-1}=\rho {\widehat{U}}_{k-1|k-1}\end{array}\right.$$

(25)

where $j=2,3,\dots ,M$; ${\widehat{Q}}_{j,k}$ represents the j-th nominal SNCM. Equation (25) give detailed explanations about the prior parameters for the noise distributions at time k are predicted from their posterior estimates at time k−1. The forgetting factor $\rho$ allows for adapting to time-varying noise characteristics by down-weighting past information. The tuning parameters ${\pi }_k$ can provide a lower bound or ensure sufficient degrees of freedom for the Inverse-Wishart distribution of the system noise covariance.

4.2. Joint Inference

According to the joint posterior PDF Equation (24) and the fixed-point iteration Equation (23), the system parameters ${\theta }_k\in {\Theta }_k$ can be calculated.

Setting ${\theta }_k=x_k$, $\log q^{(i+1)}({\theta }_k)$ is updated as:

$$\begin{array}{cc}\log q^{(i+1)}\left({\theta }_k\right)=& -0.5{\left(z_k-\mathrm{h}\left(x_k\right)\right)}^{\mathrm{{\rm T}}}{\mathrm{E}}_k^{(i+1)}\left(R_k^{-1}\right)\left(z_k-\mathrm{h}\left(x_k\right)\right)\hfill \\ & -0.5\left(x_k-\mathrm{f}{\left(x_{k-1}\right)}^T\right){\mathrm{E}}_k^{(i+1)}\left(Q_k^{-1}\right)\times \left(x_k-\mathrm{f}\left(x_{k-1}\right)\right)+c_{x_k}\hfill \end{array}$$

(26)

while

$$q^{(i+1)}\left(x_k\right)=\mathrm{N}\left(x_k;{\widehat{x}}_{k|k}^{(i+1)},P_{k|k}^{(i+1)}\right)$$

(27)

Equation (26) is derived from the general VB update rule specifically for the state $x_k$. It shows that the log-posterior of $x_k$ is a sum of quadratic terms related to the measurement prediction error $\left(z_k-\mathrm{h}\left(x_k\right)\right)$ and the state prediction error $\left(x_k-\mathrm{f}\left(x_{k-1}\right)\right)$, weighted by the expected inverse covariance matrices ${\mathrm{E}}_k^{(i+1)}\left(R_k^{-1}\right)$ and ${\mathrm{E}}_k^{(i+1)}\left(Q_k^{-1}\right)$ from the previous iteration. This form implies that the updated approximate posterior for $x_k$, $q^{(i+1)}\left(x_k\right)$, will be a Gaussian distribution, as shown in Equation (27), with mean ${\widehat{x}}_{k|k}^{(i+1)}$ and covariance $P_{k|k}^{(i+1)}$.

Where the parameters ${\widehat{x}}_{k|k}^{(i+1)}$ and ${\widehat{P}}_{k|k}^{(i+1)}$ can be approximately obtained by:

$$\left\{\begin{array}{l}{K}_k^{(i+1)}\approx {\tilde{Q}}_k^{(i+1)}\dot{\mathrm{h}}{\left(x_k^{(i)}\right)}^{\mathrm{{\rm T}}}{\left(\dot{\mathrm{h}}\left(x_k^{(i)}\right){\tilde{Q}}_k^{(i)}\dot{\mathrm{h}}{\left(x_k^{(i)}\right)}^{\mathrm{{\rm T}}}+{\tilde{R}}_k^{(i)}\right)}^{-1}\\ {\widehat{x}}_{k|k}^{(i+1)}={\widehat{x}}_{k|k-1}+K_k^{(i+1)}\left(z_k-{\widehat{z}}_{k|k-1}\right)\\ P_{k|k}^{(i+1)}\approx \left(I_n-K_k^{(i+1)}\dot{\mathrm{h}}\left(x_k^{(i)}\right)\right){\tilde{Q}}_k^{(i+1)}\end{array}\right.$$

(28)

where ${\widehat{x}}_{k|k-1}$ and ${\widehat{z}}_{k|k-1}$ are the state prediction vector and measurement prediction vector, respectively; $\dot{\mathrm{h}}\left(x_k^{(i)}\right)$ represents the first-order differentiation of a measurement process, and they can be calculated as Equations (4) and (6). The modified SNCM ${\tilde{Q}}_k^{(i)}$ and modified MNCM ${\tilde{R}}_k^{(i)}$ are calculated as follows:

$${\tilde{Q}}_k^{\left(i\right)}={\left\{{\mathrm{E}}^{\left(i\right)}\left[Q_k^{-1}\right]\right\}}^{-1},{\tilde{R}}_k^{\left(i\right)}={\left\{{\mathrm{E}}^{\left(i\right)}\left[R_k^{-1}\right]\right\}}^{-1}$$

(29)

Based on Equations (26)–(29), setting ${\theta }_k=x_{k-1}$, $q^{(i+1)}\left(x_{k-1}\right)$ can be analytically updated as Gaussian PDF, i.e.,

$$q^{(i+1)}(x_{k-1})=\mathrm{N}\left(x_{k-1};{\widehat{x}}_{k-1|k}^{\left(i+1\right)},P_{k-1|k}^{\left(i+1\right)}\right)$$

(30)

Considering that the state vector $x_{k-1}$ and prediction covariance matrix $P_{k-1}$ have been calculated at time k−1. Therefore, to reduce computation burden, the estimation results of the state vector $x_{k-1}$ and prediction covariance matrix $P_{k-1}$ at time k−1 are directly used as the optimal estimation result, i.e.,

$${\widehat{x}}_{k-1|k}^{*}={\widehat{x}}_{k-1|k-1},{\widehat{P}}_{k-1|k}^{*}=P_{k-1|k-1}$$

(31)

Similar to Equations (26)–(29), setting ${\theta }_k=R_k$ and ${\theta }_k=Q_k$, $q^{(i+1)}\left(R_k\right)$ and $q^{(i+1)}\left(Q_k\right)$ are updated as inverse Wishart PDFs:

$$\left\{\begin{array}{l}{q}^{\left(i+1\right)}=\mathrm{IW}\left(Q_k;{\widehat{t}}_{k|k}^{\left(i+1\right)},{\widehat{T}}_{k|k}^{\left(i+1\right)}\right)\\ q^{\left(i+1\right)}=\mathrm{IW}\left(R_k;{\widehat{u}}_{k|k}^{\left(i+1\right)},{\widehat{U}}_{k|k}^{\left(i+1\right)}\right)\end{array}\right.$$

(32)

where the statistics parameters of system noise $\left\{{\widehat{u}}_{k|k}^{\left(i+1\right)},{\widehat{U}}_{k|k}^{\left(i+1\right)}\right\}$, ${\left\{{\widehat{t}}_{j,k|k},{\widehat{T}}_{j,k|k}\right\}}_{j=1}^M$ are updated as follows:

$$\left\{\begin{array}{l}{\widehat{t}}_{k|k}^{(i+1)}={\displaystyle \sum _{j=1}^M{\mathrm{E}}^{(i)}\left[{\kappa }_{j,k}\right]{\widehat{t}}_{j,k|k-1}+1}\hfill \\ {\widehat{T}}_{k|k}^{(i+1)}={\displaystyle \sum _{j=1}^M\left|{\mathrm{E}}^{(i)}{\kappa }_{j,k}\right|}{\widehat{T}}_{j,k|k-1}+A_k^{(i+1)}\hfill \\ {\widehat{u}}_{k|k}^{(i+1)}={\widehat{u}}_{k|k-1}+1, {\widehat{U}}_{k|k}^{(i+1)}={\widehat{U}}_{k|k-1}+B_k^{(i+1)}\hfill \\ A_k^{(i+1)}={\mathrm{E}}^{(i+1)}\left[\left(x_k-\mathrm{f}\left(x_{k-1}\right)\right){\left(x_k-\mathrm{f}\left(x_{k-1}\right)\right)}^{\mathrm{{\rm T}}}\right]\hfill \\ B_k^{(i+1)}={\mathrm{E}}^{(i+1)}\left[\left(z_k-\mathrm{h}\left(x_k\right)\right){\left(z_k-\mathrm{h}\left(x_k\right)\right)}^{\mathrm{{\rm T}}}\right]\hfill \end{array}\right.$$

(33)

Equation (32) states that the posteriors for the noise covariance matrices $Q_k$ and $R_k$ are Inverse-Wishart distributions. Equation (33) shows how their parameters are updated. For $Q_k$, the update incorporates a sum over its M mixture components from the prior, weighted by the expected mixing proportions ${\mathrm{E}}^{(i)}\left[{\kappa }_{j,k}\right]$, and adds the expected squared state prediction error $A_k^{(i+1)}$. For $R_k$, the prior parameters are updated with the expected squared measurement prediction error $B_k^{(i+1)}$.

According to Equations (26)–(29), setting ${\theta }_k={\lambda }_k$, $q^{(i+1)}\left({\lambda }_k\right)$ is renewed as categorical PMF:

$$q^{\left(i+1\right)}\left({\lambda }_k\right)=\mathrm{Cat}\left({\lambda }_k;{\widehat{\beta }}_k^{\left(i+1\right)},M\right)$$

(34)

where ${\widehat{\beta }}_k^{\left(i+1\right)}=\left[{\widehat{\beta }}_{1,k}^{\left(\mathrm{i}+1\right)},\dots ,{\widehat{\beta }}_{M,k}^{\left(\mathrm{i}+1\right)}\right]$ represents the mixing probability vector estimate, and it can be determined:

$$\left\{\begin{array}{l}{\widehat{\beta }}_k^{(i+1)}={\beta }_k^{(i+1)}/{\displaystyle \sum _{j=1}^M{\beta }_{j,k}^{(i+1)}}\hfill \\ {\beta }_{j,k}^{(i+1)}=\exp \left\{\begin{array}{l}{\mathrm{E}}^{(i+1)}\left[\log {\tau }_{j,k}\right]+0.5{\widehat{t}}_{j,k|k-1}\log \left|{\widehat{T}}_{J,k|k-1}\right|-0.5tr\left({\widehat{T}}_{j,k|k-1}{\mathrm{E}}^{(i+1)}\left[Q_k^{-1}\right]\right)\\ -0.5\left({\widehat{t}}_{j,k|k-1}+n+1\right)\times {\mathrm{E}}^{(i+1)}\left[\log \left|Q_k\right|\right]-\log {\mathrm{\Gamma }}_n\left(0.5{\widehat{t}}_{j,k|k-1}\right)-0.5n{\widehat{t}}_{j,k|k-1}\log 2\end{array}\right\}\hfill \end{array}\right.$$

(35)

where ${\mathrm{\Gamma }}_n\left(\cdot \right)$ means the n-variate gamma function.

Based on Equations (26)–(29), setting ${\theta }_k={\tau }_k$, $q^{\left(i+1\right)}\left({\tau }_k\right)$ is modeled as Dirichlet PDF as follows:

$$q^{\left(i+1\right)}\left({\tau }_k\right)=\mathrm{Dir}\left({\tau }_k;{\widehat{\alpha }}_{k|k}^{\left(i+1\right)},M\right)$$

(36)

where ${\widehat{\alpha }}_{k|k}^{\left(i+1\right)}$ is the concentration parameter vector, and it can be updated by:

$${\widehat{\alpha }}_{k|k}^{\left(i+1\right)}={\widehat{\alpha }}_{k|k-1}+{\mathrm{E}}^{\left(i+1\right)}\left[{\lambda }_k\right]$$

(37)

where ${\widehat{\alpha }}_k^{\left(i+1\right)}=\left[{\widehat{\alpha }}_{1,k}^{\left(i+1\right)},\dots ,{\widehat{\alpha }}_{M,k}^{\left(i+1\right)}\right]$ is the concentration parameter vector estimate.

The SNCM $Q_k$ and MNCM $R_k$ are rewritten as inverse Wishart PDFs, so $Q_k^{-1}$ and $R_k^{-1}$ are Wishart distribution as follows:

$$\left\{\begin{array}{l}{q}^{\left(i+1\right)}\left(Q_k^{-1}\right)=\mathrm{W}\left(Q_k^{-1};{\widehat{t}}_{k|k}^{\left(i+1\right)},{\left\{{\widehat{T}}_{k|k}^{\left(i+1\right)}\right\}}^{-1}\right)\\ q^{\left(i+1\right)}\left(R_k^{-1}\right)=\mathrm{W}\left(R_k^{-1};{\widehat{t}}_{k|k}^{\left(i+1\right)},{\left\{{\widehat{R}}_{k|k}^{\left(i+1\right)}\right\}}^{-1}\right)\end{array}\right.$$

(38)

By using (56), ${\mathrm{E}}^{(i+1)}\left[Q_k^{-1}\right]$, ${\mathrm{E}}^{(i+1)}\left[R_k^{-1}\right]$ and ${\mathrm{E}}^{(i+1)}\left[\log \left|Q_k\right|\right]$ can be renewed as:

$$\left\{\begin{array}{l}{\mathrm{E}}^{(i+1)}\left[Q_k^{-1}\right]={\widehat{t}}_{k|k}^{(i+1)}{\left\{{\widehat{T}}_{k|k}^{(i+1)}\right\}}^{-1}\hfill \\ {\mathrm{E}}^{(i+1)}\left[R_k^{-1}\right]={\widehat{t}}_{k|k}^{(i+1)}{\left\{{\widehat{U}}_{k|k}^{(i+1)}\right\}}^{-1}\hfill \\ {\mathrm{E}}^{(i+1)}\left[\log \left|Q_k\right|\right]=\log \left|{\widehat{T}}_{k|k}^{(i+1)}\right|-n\log 2-{\psi }_n\left(0.5{\widehat{t}}_{k|k}^{(i+1)}\right)\hfill \end{array}\right.$$

(39)

where ${\psi }_n(\cdot )$ denotes the $n-\mathrm{variate}$ digamma function.

According to the Gaussian approximations of $q^{\left(i+1\right)}\left(x_k\right)$ and $q^{\left(i+1\right)}\left(x_{k-1}\right)$ in Equations (27)–(31), auxiliary matrices $A_k^{\left(i+1\right)}$ and $B_k^{\left(i+1\right)}$ are calculated as:

$$\left\{\begin{array}{l}{A}_k^{\left(i+1\right)}\approx \dot{\mathrm{f}}\left(x_{k-1}\right){\widehat{P}}_{k-1|k-1}^{*}\dot{\mathrm{f}}{\left(x_{k-1}\right)}^{\mathrm{{\rm T}}}+P_{k|k}^{*}+\left({\widehat{x}}_{k|k}^{\left(i+1\right)}-{\widehat{x}}_{k|k-1}\right)\times {\left({\widehat{x}}_{k|k}^{\left(i+1\right)}-{\widehat{x}}_{k|k-1}\right)}^{\mathrm{{\rm T}}}\\ B_k^{\left(i+1\right)}\approx \dot{\mathrm{h}}(x_k^{(i)}){\widehat{P}}_{k-1|k}^{*}\dot{\mathrm{h}}{(x_k^{(i)})}^{\mathrm{{\rm T}}}+P_{k|k}^{*}+\left(z_k-{\widehat{z}}_{k|k-1}\right)\times {\left(z_k-{\widehat{z}}_{k|k-1}\right)}^{\mathrm{{\rm T}}}\end{array}\right.$$

(40)

where $\psi \left(\cdot \right)$ denotes the digamma function.

According to Equations (32)–(35), the expectation ${\mathrm{E}}^{\left(i+1\right)}\left[{\lambda }_k\right]$,${\mathrm{E}}^{\left(i+1\right)}\left[{\tau }_k\right]$, ${\mathrm{E}}^{\left(i+1\right)}\left[\log {\tau }_{j,k}\right]$ are updated as:

$$\left\{\begin{array}{l}{\mathrm{E}}^{(i+1)}\left[{\lambda }_k\right]={\widehat{\beta }}_k^{(i+1)},{\mathrm{E}}^{(i+1)}\left[{\tau }_k\right]={\widehat{\delta }}_{k\mid k}^{(i+1)}/{\displaystyle \sum _{j=1}^M}{\widehat{\delta }}_{j,k\mid k}^{(i+1)}\hfill \\ {\mathrm{E}}^{(i+1)}\left[\log {\lambda }_{j,k}\right]=\psi \left({\widehat{\delta }}_{j,k\mid k}^{(i+1)}\right)-\psi \left({\displaystyle \sum _{j=1}^M}{\widehat{\delta }}_{j,k\mid k}^{(i+1)}\right)\hfill \end{array}\right.$$

(41)

The pseudo code of the proposed IW-VACKF is given in Algorithm 1, where $\epsilon$ denotes the termination threshold and $N_m$ represents the upper limit of the number of iterations.

Algorithm 1: Time recursion of IW-VACKF

Inputs: ${\widehat{x}}_{k-1|k-1}$, $P_{k-1|k-1}$, $\mathrm{f}\left(\cdot \right)$, $\mathrm{h}\left(\cdot \right)$, $z_k$, ${\left\{{\tilde{Q}}_{j,k}\right\}}_{j=1}^M$, ${\widehat{\alpha }}_{k-1|k-1}$, ${\widehat{u}}_{k-1|k-1}$, ${\widehat{U}}_{k-1|k-1}$, ${\widehat{t}}_{k-1|k-1}$, ${\widehat{T}}_{k-1|k-1}$, ${\pi }_k$, $\rho$, $M$, $\epsilon$, $N_m$ Time update (1) Calculate ${\widehat{x}}_{k|k-1}$ and ${\widehat{z}}_{k|k-1}$ based on cubature sampling rule according to Equations (3)–(6) (2) Update ${\widehat{\alpha }}_{k|k-1}$, ${\left\{{\widehat{t}}_{j,k|k-1},{\widehat{T}}_{j,k|k-1}\right\}}_{j=1}^M$, $\left\{{\widehat{u}}_{k|k-1},{\widehat{U}}_{k|k-1}\right\}$ by using Equation (25) Measurement update: (3) Initialization: ${\mathrm{E}}^{\left(0\right)}\left[\log {\tau }_{j,k}\right]=\psi \left({\widehat{\alpha }}_{j,k|k-1}\right)-\psi \left({\displaystyle {\sum }_{j=1}^M{\widehat{\alpha }}_{j,k|k-1}}\right)$, ${\mathrm{E}}^{\left(0\right)}\left[R_k^{-1}\right]={\widehat{u}}_{k-1|k-1}{\widehat{U}}_{k-1|k-1}^{-1}$, ${\mathrm{E}}^{\left(0\right)}\left[{\lambda }_k\right]={\widehat{\alpha }}_{k|k-1}/{\displaystyle {\sum }_{j=1}^M{\widehat{\alpha }}_{j,k|k-1}}$, ${\widehat{x}}_{k-1|k}^{*}={\widehat{x}}_{k-1|k-1}$, ${\widehat{P}}_{k-1|k}^{*}=P_{k-1|k-1}$, ${\mathrm{E}}^{\left(0\right)}\left[Q_k^{-1}\right]=\left({\displaystyle {\sum }_{j=1}^M{\mathrm{E}}^{\left(0\right)}\left[{\lambda }_{j,k}\right]{\widehat{t}}_{j,k|k-1}}\right){\left({\displaystyle {\sum }_{j=1}^M{\mathrm{E}}^{\left(0\right)}\left[{\lambda }_{j,k}\right]{\widehat{T}}_{j,k|k-1}}\right)}^{-1}$ for $i=0:N_m-1$ (4) Calculate $x_{k|k}^{\left(i+1\right)}$ and $P_{k|k}^{\left(i+1\right)}$ by Equations (27)–(29) (5) Update $A_k^{\left(i+1\right)}$ and $B_k^{\left(i+1\right)}$ according to Equation (40) (6) Calculate ${\left\{{\widehat{t}}_{j,k|k}^{\left(i+1\right)},{\widehat{T}}_{j,k|k}^{\left(i+1\right)}\right\}}_{j=1}^M$ and $\left\{{\widehat{u}}_{k|k}^{\left(i+1\right)},{\widehat{U}}_{k|k}^{\left(i+1\right)}\right\}$ employing Equations (32) and (33) (7) Update ${\mathrm{E}}^{\left(i+1\right)}\left[Q_k^{-1}\right]$, ${\mathrm{E}}^{\left(i+1\right)}\left[R_k^{-1}\right]$, ${\mathrm{E}}^{\left(i+1\right)}\left[\log \left|Q_k\right|\right]$ based on Equation (39) (8) Renew the mixing probability vector ${\widehat{\beta }}_k^{\left(i+1\right)}$ by Equation (35) (9) Calculate ${\mathrm{E}}^{\left(i+1\right)}\left[{\lambda }_k\right]$ by Equation (41) (10) Renew the concentration parameter vector ${\widehat{\alpha }}_{k|k}^{\left(i+1\right)}$ according to Equation (37) (11) Update ${\mathrm{E}}^{\left(i+1\right)}\left[{\tau }_k\right]$ and ${\mathrm{E}}^{\left(i+1\right)}\left[\log {\tau }_{j,k}\right]$ according to Equation (41) (12) If $i\ge N_m$ or $‖{\widehat{x}}_{k|k}^{\left(i+1\right)}-x_{k|k}^{\left(i\right)}‖/‖x_{k|k}^{\left(i\right)}‖\le \epsilon$, iteration is finished end for (13) ${\widehat{x}}_{k|k}={\widehat{x}}_{k|k}^{\left(i\right)}$, $P_{k|k}=P_{k|k}^{\left(i\right)}$, ${\widehat{\alpha }}_{k|k}={\widehat{\alpha }}_{k|k}^{\left(i\right)}$, ${\widehat{u}}_{k|k}={\widehat{u}}_{k|k}^{\left(i\right)}$, ${\widehat{U}}_{k|k}={\widehat{U}}_{k|k}^{\left(i\right)}$, ${\widehat{t}}_{k|k}={\widehat{t}}_{k|k}^{\left(i\right)}$, ${\widehat{T}}_{k|k}={\widehat{T}}_{k|k}^{\left(i\right)}$ Outputs: ${\widehat{x}}_{k|k}$, $P_{k|k}$, ${\widehat{\alpha }}_{k|k}$, ${\widehat{u}}_{k|k}$, ${\widehat{U}}_{k|k}$, ${\widehat{t}}_{k|k}$, ${\widehat{T}}_{k|k}$.

4.3. Discussions

The proposed IW-VACKF algorithm, illustrated in Figure 2, operates two main stages: time update and measurement update. During the time update, the state one-step prediction vector and the measurement one-step prediction vector are updated using the cubature sampling rule. Critically, to account for non-stationary state and measurement noise, prior system noise parameters are updated by introducing a forgetting factor $\rho$ and tuning parameters ${\pi }_k$. The measurement update stage renews the state estimation and prediction covariance matrix, with system noise model parameters updated via iterative calculations derived from the VB method.

(1)

Parameter Selection

Reasonable parameter choice is crucial for the successful implementation of the IW-VACKF. The forgetting factor $\rho$ in Equation (25), typically set as $\rho \in \left[0.9,1\right]$ [32]. Tuning parameters ${\pi }_k$ to adjust the DoF for the SNCM; larger values are suitable for stable state noise, while smaller values are used for unstable noise changes. The parameter M for the state noise model is set to balance the accuracy and computing burden. Initial DoF parameters for the IW distributions have an influence on the initial uncertainty of noise covariance estimates for the IW-VACKF. ${\widehat{\alpha }}_{k|k-1}$ is centration parameter vector. The initial value of ${\widehat{\alpha }}_{k|k-1}$ is a vector of all 1s with a length of 1 × M and update by Equation (37). To ensure the SNCM $Q_k^{\left(i\right)}$ and the MNCM $R_k^{\left(i\right)}$ converge to their true values, the initial MNCM and the initial SNCM must include the expected range of SNCM variations. To reduce computational burden, MNCM and SNCM are often set as diagonal matrices.

(2)

Theoretical Advantages and Computational Complexity

Compared to existing VB adaptive filters like VBCKF-QR, the theoretical advantage of IW-VACKF lies in more accurate estimation of the SNCM and MNCM. Based on the VB structure, the IW-VACKF uses multiple IW distributions to enhance the accuracy of noise and state estimation. Regarding computational complexity, the time complexity of a standard CKF is $O\left(n^3\right)$, where n is the state dimension. The number of iterations is Nm. For each VB iterations, IW-VACKF updates the state estimation, so the time complexity for every step is $O\left(N_m·M·n^3\right)$.

5. Simulation and Experimental Verification

5.1. Simulation Test

Consider a trajectory tracking test with two fixed sensors ($S_1$,$S_2$), where $S_1$ is located at the origin (0,0) and $S_2$ is located in the position (10,10). The state vector of the target of the target is set as $x_k={\left[{\eta }_k,{\varphi }_k,{\dot{\eta }}_k,{\dot{\varphi }}_k\right]}^{\mathrm{{\rm T}}}$, where ${\eta }_k$ and ${\varphi }_k$ are the position coordinates of target. ${\dot{\eta }}_k$ and ${\dot{\varphi }}_k$ are the corresponding velocity components. In real underwater environment, there is a pressure sensor to accurately obtain the depth information, so the simulation focuses on horizontal position information. The simulation is set [18]:

$$\left\{\begin{array}{l}{x}_k=\mathrm{f}\left(x_{k-1}\right)+w_k\\ z_k=\mathrm{g}\left(x_k\right)+v_k\end{array}\right.$$

(42)

where $w_k$ and $v_k$ are the system noises with time-varying SNCM $Q_k$ and MNCM $R_k$, respectively, and the state transition process of the target is:

$$x_k=\left[\begin{array}{cc}{I}_2& \mathrm{\Delta }t{I}_2\\ 0& I_2\end{array}\right]x_{k-1}+w_k$$

(43)

where $\mathrm{\Delta }t=1 \mathrm{s}$ represents the sampling time; $I_2$ is the two-dimensional identity matrix. The SNCM at the time $k$ is shown as follows: $Q_k=\left[q+0.5\cos \left({\displaystyle \frac{\pi k}{T}}\right)\right]Q_0$ and $Q_0=\mathrm{\Delta }\kappa \left[\begin{array}{cccc}\mathrm{\Delta }{t}^3/3& 0& \mathrm{\Delta }{t}^2/2& 0\\ 0& \mathrm{\Delta }{t}^3/3& 0& \mathrm{\Delta }{t}^2/2\\ \mathrm{\Delta }{t}^2/2& 0& \mathrm{\Delta }t& 0\\ 0& \mathrm{\Delta }{t}^2/2& 0& \mathrm{\Delta }t\end{array}\right]$.

• where $\mathrm{\Delta }\kappa$ is the scale factors selected as $\mathrm{\Delta }\kappa =(m^2/m^3)$; q is an adjust factor for the system state noise strength. In this simulation, by adjusting the factor q, the estimation consistency is verified under different state noise value. T = 150 s is the total simulation time. The measurement process is shown as follows:

$$z_k=\left[\begin{array}{l}\sqrt{{\left({\eta }_k-{\eta }_{s_1}\right)}^2+{\left({\varphi }_k-{\varphi }_{s_1}\right)}^2}\\ \mathrm{atan}2\left({\eta }_k-{\eta }_{s_1},{\varphi }_k-{\varphi }_{s_1}\right)\\ \sqrt{{\left({\eta }_k-{\eta }_{s_2}\right)}^2+{\left({\varphi }_k-{\varphi }_{s_2}\right)}^2}\\ \mathrm{atan}2\left({\eta }_k-{\eta }_{s_2},{\varphi }_k-{\varphi }_{s_2}\right)\end{array}\right]+v_k$$

(44)

where $\left({\eta }_{s_1},{\varphi }_{s_1}\right)$ and $\left({\eta }_{s_2},{\varphi }_{s_2}\right)$ are the known position coordinates of sensors S₁ and S₂, respectively. The MNCMs in different sensors are selected as:  $R_0=\mathrm{diag}\left(10 {\mathrm{m}}^2,0.0174 {\mathrm{rad}}^2\right)$, $R_{k,1}=0.5R_0\left[0.1+0.05\cos \left({\displaystyle \frac{\pi k}{\mathrm{\Delta }t}}\right)\right]$, $R_{k,2}=0.3R_0\left[0.1+0.05\cos \left({\displaystyle \frac{\pi k}{\mathrm{\Delta }t}}\right)\right]$. The initial state is set as $x_0={\left[40,50,8,8\right]}^{\mathrm{{\rm T}}}$, and the initial prediction error covariance matrix is chosen as $P_{0|0}=\mathrm{diag}\left(\left[4,2,2,2\right]\right)$.

In the proposed IW-VACKF, the number of mixture terms is set as $M=4$; the initial DoF parameters are chosen as ${\widehat{t}}_{0|0}=5$ and ${\widehat{u}}_{0|0}=10$; the initial concentration parameter vector is selected as ${\widehat{\alpha }}_{0|0}=1_{1\times 4}$; the tuning parameter ${\pi }_k$ and the forgetting factor $\rho$ are respectively chosen as ${\pi }_k=5$, $\rho =0.996$. To control the number of iterations calculation, set $\epsilon$ and $N_m$ as ${10}^{-10}$ and 50, respectively. And the CKF with TNCM (CKF-TNCM) is set as the reference. In the CKF-TNCM, the TNCM are employed to achieve recursive state estimates.

The root-mean-square errors (RMSEs) and the average RMSEs (ARMSEs) are used as performance index to compare the state estimate accuracy in different algorithms, and they are calculated as [33]:

$$\left\{\begin{array}{l}{\mathrm{RMSE}}_{pos}\triangleq \sqrt{{\displaystyle \frac{1}{M}}{\displaystyle {\sum }_{s=1}^M\left({({\eta }_k^s-{\widehat{\eta }}_k^s)}^2+{({\varphi }_k^s-{\widehat{\varphi }}_k^s)}^2\right)}}\\ {\mathrm{ARMSE}}_{pos}\triangleq \sqrt{{\displaystyle \frac{1}{MT}}{\displaystyle {\sum }_{k=1}^T{\displaystyle {\sum }_{s=1}^M\left({({\eta }_k^s-{\widehat{\eta }}_k^s)}^2+{({\varphi }_k^s-{\widehat{\varphi }}_k^s)}^2\right)}}}\end{array}\right.$$

(45)

where $\left({\alpha }_k^s,{\beta }_k^s\right)$ is the true position at the s-th Monte Carlo run, $\left({\widehat{\alpha }}_k^s,{\widehat{\beta }}_k^s\right)$ represents the estimated positions, and the total number of Monte Carlo runs is denoted by M. Equation (45) shows the calculation process of the position errors. Similarly, the velocity errors can be acquired in the same way as Equation (45).

In addition, the normalized estimation errors squared (NEES) and the average normalized estimation errors squared (ANEES) are employed to evaluate the estimation consistency, and then define them as follows [34]:

$$\mathrm{ANEES}={\displaystyle \frac{1}{MT}}{\displaystyle \sum _{k=1}^T{\displaystyle \sum _{s=1}^M{(x_k^s-{\widehat{x}}_{k|k}^s)}^{\mathrm{{\rm T}}}{(P_{k|k}^s)}^{-1}(x_k^s-{\widehat{x}}_{k|k}^s)}}$$

(46)

where $x_k^s$ and ${\widehat{x}}_{k|k}^s$ are respectively the true and estimated system state vector at the time k; $P_{k|k}^s$ is the system estimation error covariance matrix in the s-th Monte Carlo run, and M is the total number of Monte Carlos runs. Based on the principle of state determination, if the values of ANEES are within the expected bounds around the state dimension n. It means that the theoretical estimation error covariance is consistent with the actual estimation error.

To provide detailed statistical analysis and better quantify performance gains as requested, we conduct Monte Carlo simulations for the q = 1 scenario. The RMSE distributions for position and velocity are presented in the boxplot of Figure 3. Figure 3 include the median, mean, interquartile range (IQR), and outliers for each algorithm. These results demonstrate the clear advantage of the proposed IW-VACKF: its mean and median position RMSE and velocity RMSE are substantially lower than those of standard UKF and CKF. Furthermore, IW-VACKF generally exhibits a more compact RMSE distribution, indicating more consistent performance. Compared to VBCKF-QR, IW-VACKF shows comparable or slightly superior RMSE with a similarly tight distribution. This statistical evidence robustly supports the claim that IW-VACKF offers enhanced estimation accuracy.

Figure 4 shows the estimate error in different filters when the adjust factor is selected as q = 2, the initial nominal SNCM set ${\left\{{\tilde{Q}}_{j,0}\right\}}_{j=1}^M$ is chosen as ${\tilde{Q}}_{1,0}=1.8I_4$, ${\tilde{Q}}_{2,0}=2I_4$, ${\tilde{Q}}_{3,0}=2.2I_4$ and ${\tilde{Q}}_{4,0}=2.5I_4$. The proposed IW-VACKF has a better performance than the compared CKF, UKF and VBCKF-QR.

Table 1. ARMSEs of position and velocity in multi-sensor tracking.

Parameters	Filter	${\mathbf{ARMSE}}_{\mathit{p}\mathit{o}\mathit{s}}$ (m)	${\mathbf{ARMSE}}_{\mathit{v}\mathit{e}\mathit{l}}$ (m/s)
$q=1$	UKF	53.3109	2.9186
	CKF	45.2893	2.8961
	VBCKF-QR	10.8589	3.3216
	IW-VACKF	10.2683	2.5733
	CKF-TNCM	8.9651	2.2954
$q=2$	UKF	61.9777	4.3249
	CKF	58.6286	4.3189
	VBCKF-QR	12.3005	3.6468
	IW-VACKF	11.2230	3.3291
	CKF-TNCM	10.6104	3.0313
$q=3$	UKF	73.9291	5.5212
	CKF	96.2329	7.2268
	VBCKF-QR	13.8656	4.2379
	IW-VACKF	11.4812	3.1558
	CKF-TNCM	11.4189	3.4854

shows the average state estimation accuracy under different system state noise value parameters q. This is a quantitative assessment of how each filter’s accuracy responds to increasing levels of uncertainty in the system model. The proposed IW-VACKF has improved position estimation precision by at least 10%.

It is illustrated from Figure 5 that the IW-VACKF has the best determination consistency than the compared UKF, CKF and VBCKF-QR under different adjusting factor q. Because the SNCM and MNCM are time-vary and unknown, the existing algorithms with fixed NCMs perform poor in estimation consistency.

It is illustrated from Figure 5 that the IW-VACKF has the best determination consistency than the compared UKF, CKF and VBCKF-QR under different adjusting factor q. The proposed IW-VACKF consistently maintains its ANEES at a low level across this range, generally performing closest to the optimal CKF-TNCM which utilizes true noise covariance matrices. This indicates robust of IW-VACKF even as the system noise characteristics change. Because the SNCM and MNCM are time-vary and unknown, the existing algorithms with fixed NCMs perform poor in estimation consistency. For example, both UKF and CKF exhibit generally higher and more fluctuating ANEES values compared to IW-VACKF, while VBCKF-QR also shows less stable consistency with ANEES values that tend to be higher than those of IW-VACKF, especially for larger q values.

5.2. Experimental Verification on Underwater Vehicle

To validate the performance of the IW-VACKF, the real experiment is performed in Yangtze River. USBL is used underwater to provide positioning information for underwater vehicle as reference, but the USBL is prone to be interfered by unknown and random noises. To evaluate the performance of the proposed method compared with other traditional methods, we develop the surface vessel experimental platform shown in Figure 6, which acquires high-precision reference data through GPS. DVL and USBL are deployed underwater to collect underwater navigation data and fusion with IMU. Detailed specifications of the IMU and DVL are provided in

Table 2. Parameters of sensors in surface vessel trial.

Items	Gyroscope
In run bias stability $\left(°/\mathrm{h}\right)$	8
Scale factor stability $(\mathrm{p}\mathrm{p}\mathrm{m})$	500
Angular Random Walk $\left(°/\sqrt{\mathrm{h}}\right)$	0.18
One year bias stability $(°/\mathrm{s})$	0.4
Items	Accelerometers
Scale factor stability $(\mathrm{p}\mathrm{p}\mathrm{m})$	1000
Velocity Random walk $(\mathrm{\mu }\mathrm{g}/\sqrt{\mathrm{Hz}})$	57
In run bias instability $(\mathrm{\mu }\mathrm{g})$	14
One year bias stability $(\mathrm{m}\mathrm{g})$	5
Items	GPS
Single positional Accuracy $(\mathrm{m})$	5
Velocity Accuracy $(\mathrm{m}/\mathrm{s})$	3
Items	DVL
Velocity Accuracy $(\mathrm{m}/\mathrm{s})$	0.001

, while the reference trajectory used in the trials is shown in Figure 7.

Considering the characteristics of complex underwater noise, the IW-VACKF introduces the mixed probability vector ${\tau }_k$ on the basis of IW distribution. Then, the system can obtain an accurate model of state noise and based on the method of fixed-point iteration, the proposed IW-VACKF obtains higher precision position estimations than other compared methods.

Figure 8, Figure 9 and Figure 10 show the position determination error in different algorithms, the proposed IW-VACKF has the best performance than exiting algorithms. According to

Table 3. Estimated Error of position target tracking of surface vessel.

Algorithms	North Error (m)	East Error (m)	Down Error (m)
IW-VACKF	2.82	1.99	0.23
VBCKF-QR	2.94	2.07	0.24
EKF	6.73	2.89	0.57
UKF	7.87	3.49	0.65
CKF	5.80	2.41	0.51

, the proposed IW-VACKF has improved accuracy by at least 4.3% than compared algorithms in north direction. In east direction, the proposed IW-VACKF has improved accuracy by at least 4.0%. And in down direction, the proposed IW-VACKF has improved accuracy by at least 4.3%. Figure 11 shows the estimated trajectories of different algorithms. As can be seen from Figure 11, the proposed method is the closest to the true value compared to other compared methods.

Validated through simulation and field tests, the proposed system model and navigation method exhibit superior performance, enabling underwater vehicles to achieve high-precision navigation and positioning.

6. Conclusions

This paper proposes a novel inverse-Wishart-based variational Bayesian adaptive Kalman filter algorithm, to address the complex noise problem of underwater integrated navigation systems, thereby enhancing the accuracy and robustness of multi-sensor data fusion in complex underwater environments. Firstly, based on the characteristics of underwater complex noise, the nonlinear system state transition and measurement process are described as hierarchical Gaussian model. Then, in the basis of the hierarchical Gaussian model, the system state is estimated by VB approach. Finally, by utilizing the cubature rule, the proposed IW-VACKF can be implemented using as fixed-point iteration method. Simulations and real trials demonstrate that the proposed IW-VACKF has better state estimation precision.