## 3 Robust Square-Root Continuous-Discrete Cubature Kalman Filter Algorithms
### VBSRCD-CKF Algorithm Based on Variational Bayes Criterion  
Variational Bayesian approximation is an iterative optimization algorithm that approximates the posterior probability distribution of parameters [26]. The VB-based filter uses VB approximation to estimate the joint posterior distribution of the state and covariance \(p(\mathbf{x}_{k},\mathbf{R}_{k}|\mathbf{z}_{1:k})\), as follows:  
\[p(\mathbf{x}_{k},\mathbf{R}_{k}|\mathbf{z}_{1:k})\approx\mathbf{Q}_{x}(\mathbf{x}_{k})\mathbf{Q}_{\rm R }(\mathbf{R}_{k}), \tag{32}\]  
where \(\mathbf{Q}_{x}(\mathbf{x}_{k})\) and \(\mathbf{Q}_{R}(\mathbf{R}_{k})\) are the yet unknown approximating densities. Unlike the correntropy, the Kullback-Leibler (KL) divergence compares the closeness of two probability distributions. It is now possible to form a VB approximation by minimizing the KL divergence between the separable approximation and the true posterior:  
\[\mathrm{KL}[\mathbf{Q}_{x}(\mathbf{x}_{k})\mathbf{Q}_{\rm R}(\mathbf{R}_{k})\|\mathbf{p}(\mathbf{x}_{ k},\mathbf{R}_{k}|\mathbf{z}_{1:k})]=\int\mathbf{Q}_{x}(\mathbf{x}_{k})\mathbf{Q}_{\rm R}(\mathbf{R}_{k}) \times\log\Big{(}\frac{\mathbf{Q}_{x}(\mathbf{x}_{k})\mathbf{Q}_{\rm R}(\mathbf{R}_{k})}{p( \mathbf{x}_{k},\mathbf{R}_{k}|\mathbf{z}_{1:k})}\Big{)}dx_{k}dR_{k}. \tag{33}\]  
Minimizing the KL divergence associated with the probability densities \(\mathbf{Q}_{x}(\mathbf{x}_{k})\) and \(\mathbf{Q}_{\rm R}(\mathbf{R}_{k})\) in turn, while keeping the other fixed, we can obtain the following equations:  
\[\mathbf{Q}_{x}(\mathbf{x}_{k})\propto\exp\left(\int\log p(\mathbf{z}_{k},\mathbf{x}_{k},\mathbf{R }_{k}|\mathbf{z}_{1:k})\mathbf{Q}_{\rm R}(\mathbf{R}_{k})dR_{k}\right), \tag{34}\]  
\[\mathbf{Q}_{\rm R}(\mathbf{R}_{k})\propto\exp\left(\int\log p(\mathbf{z}_{k},\mathbf{x}_{k}, \mathbf{R}_{k}|\mathbf{z}_{1:k})\mathbf{Q}_{x}(\mathbf{x}_{k})dx_{k}\right). \tag{35}\]  
Computing the above equations, we can obtain the following densities [19]:  
\[\mathbf{Q}_{x}(\mathbf{x}_{k})=N(\mathbf{x}_{k}|\mathbf{\hat{x}}_{k},\mathbf{P}_{k}), \tag{36}\]  
\[\mathbf{Q}_{\rm R}(\mathbf{R}_{k})=\mathrm{IW}(\mathbf{R}_{k}|v_{k},\mathbf{V}_{k}), \tag{37}\]  
where \(\mathrm{IW}(\cdot)\) represents the inverse Wishart (IW) distribution, and the parameters \(\mathbf{\hat{x}}_{k}\), \(\mathbf{P}_{k}\), \(v_{k}\) and \(\mathbf{V}_{k}\) can be calculated by Kalman-type filters. The VBSRCD-CKF Algorithm 5 is as follows:  
**Step 1** At Step 1 of the SRCD-CKF algorithm, the initialization of \(v_{k}\) and \(\mathbf{V}_{k}\) are included. **Steps 2.1-2.5** are equivalent to Steps 2.1-2.5 of the SRCD-CKF algorithm. **Step 2.6** Calculate the parameters of the IW distribution of measurement noise covariance:  
\(v_{k|k-1}=\rho(v_{k-1}-n-1)+n+1\), \(\mathbf{V}_{k|k-1}=\mathbf{CV}_{k-1}\mathbf{C}^{T}\), where \(\rho\) is a scale factor that \(0<\rho\leq 1\) and \(\mathbf{C}\) is a matrix that \(0<|\mathbf{C}|\leq 1\) with a reasonable choice for the matrix \(\mathbf{C}=\sqrt{\rho}I_{d}\). \(I_{d}\) is an identity matrix, and \(d\) is the dimension of the measurement. **Step 3.1** Before calculating the state cubature point, first let \(\mathbf{\hat{x}}_{k}^{(0)}=\mathbf{\hat{x}}_{k|k-1}\), \(\mathbf{V}_{k}^{(0)}=\mathbf{V}_{k|k-1}\), and \(v_{k}=1+v_{k|k-1}\). **Steps 3.2-3.8** are equivalent to Steps 3.1-3.7 of the SRCD-CKF algorithm. **Step 3.9** For \(j=1\):M, iterate the following steps (M is the times of algorithm iteration). **Step 3.9.1** Calculate the measurement noise covariance matrix: \(\mathbf{R}_{k}^{(j)}=(v_{k}-n-1)^{-1}\mathbf{V}_{k}^{(j-1)}\). **Step 3.9.2** Update the innovation covariance matrix: \(\mathbf{P}_{\mathbf{z}=\mathbf{z}_{k|k-1}}^{(j)}=\mathbf{Z}_{k|k-1}\mathbf{Z}_{k|k-1}^{T}+\mathbf{R}_{k} ^{(j)}\). **Step 3.9.3** Calculate the continuous-discrete filter gain: \(\mathbf{K}_{k}^{(j)}=\mathbf{P}_{\mathbf{z}_{k|k-1}}\Big{(}\mathbf{P}_{\mathbf{z}_{k|k-1}}^{(j)} \Big{)}^{-1}\).