## 3 State Estimation Based on Nonlinear Gaussian Systems
### CKF Algorithm  
In underwater scenarios, the kinematic characteristics of ROVs are highly nonlinear due to the interconnections between each degree of freedom. Therefore, linear Kalman filtering is not suitable for modeling such scenarios. To address this issue, [PERSON] and [PERSON] [12] proposed the CKF, which can overcome the divergence problem of the EKF. Unlike the EKF, the CKF does not rely on linearizing the nonlinear system, but instead transforms it to calculate the first- and second-order moments of the filter distribution. As a result, the CKF is applicable to a wide range of nonlinear systems, as long as the error remains within acceptable limits. The following Gaussian noise nonlinear system model is formed:  
\[ \begin{split}\dot{x_{k}}&=f(x_{k-1},u_{k-1})+w_{k}, \\ z_{k}&=h(x_{k})+v_{k},\end{split}  \tag{17}\]  
where \(f(\bullet)\) and \(h(\bullet)\) represent the nonlinear process and measurement equation. Step 1: Initialize  
To produce 2n cubature points, CKF applies the third-order spherical radial volume rule as shown:  
\[ \begin{split}\dot{x}_{k}&=E(x_{0}),\\ P_{k}&=E(x_{0}-\hat{x}_{k})(x_{0}-\hat{x}_{k})^{ \mathrm{T}},\end{split}  \tag{18}\]  
and substituting Equation (17) into Equation (18) yields  
\[\hat{x}_{k}=E[f(x_{k-1},u_{k-1})+w_{k-1}]. \tag{19}\]  
Since \(w_{k-1}\) is assumed to be zero-mean and independent of past measurements, we obtain  
\[ \begin{split}\hat{x}_{k}&=E[f(x_{k-1},u_{k-1})+w_{k- 1}]\\ &=\int f(x_{k-1},u_{k-1})p(x_{k-1}|Y_{k-1})\,dx_{k-1}\\ &=\int f(x_{k-1},u_{k-1})\times\mathcal{N}(x_{k-1};\hat{x}_{k-1 |k-1},P_{k-1|k-1})\,dx_{k-1},\end{split}  \tag{20}\]  
where \(\mathcal{N}\) is a symbol for the Gaussian density, \(Y_{k-1}=[u_{i},z_{i}],i=1...k-1\) indicates the input measurement from \(1\) to \(k-1\), and \(p(x_{k-1}|Y_{k-1})\) is the old posterior density at time \(k-1\),  
Similarly, we can obtain the error covariance \(P_{k-1}\):  
\[ \begin{split} P_{k}&=E(x_{0}-\hat{x}_{k})(x_{0}- \hat{x}_{k})^{\mathrm{T}}\\ &=\int f(x_{k-1},u_{k-1})f^{\mathrm{T}}(x_{k-1},u_{k-1})\times \mathcal{N}(x_{k-1};\hat{x}_{k-1|k-1},P_{k-1|k-1})\,dx_{k-1}\\ &-\hat{x}_{k|k-1}\hat{x}_{k-1|k-1}^{\mathrm{T}}+Q_{k-1},\end{split}  \tag{21}\]  
Step 2: Time Update  
In order to avoid the non-positive definite phenomenon of the system state covariance array caused by using Cholesky decomposition in traditional CKF, singular value decomposition (SVD) is adopted instead of Cholesky decomposition in this paper. Calculate the cubature point:  
\[ \begin{split}& P_{k-1}=U_{k-1}\begin{bmatrix}S_{k-1}&0\\ 0&0\end{bmatrix}U_{k-1}^{\mathrm{T}},\\ & x_{k}^{i}=U_{k-1}\sqrt{S_{k-1}\hat{x}_{i}}+\hat{x}_{k},i=1,2,...,2n, \\ &\xi_{i}=\begin{cases}\sqrt{n}[1]_{i},&i=1,2,...,2n,\\ -\sqrt{n}[1]_{i},&i=n+1,n+2,...,2n,\end{cases}\end{split}  \tag{22}\]  
where \(S_{k-1}\) is a diagonal matrix, cubature point \(\xi_{i}\), \(n\) represents the state variable dimension, \(n=12\) according to Section 3.1, \([1]_{i}\) denotes the column \(i\) of \([1]\), and \([1]\) is a symmetric matrix with a dominant diagonal line of \(1\). Propagate the cubature point:  
\[x_{k+1|k}^{i}=f(x_{k}^{i}), \tag{23}\]  
The state variable predicted values and error covariance predicted values are  
\[ \begin{split}\hat{x}_{k+1|k}&=\frac{1}{2n}\sum_{i=1} ^{2n}x_{k+1|k}^{i}\enspace,\\ P_{k+1|k}&=\frac{1}{2n}\sum_{i=1}^{2n}x_{k+1|k}^{i} (x_{k+1|k}^{i})^{\text{T}}-x_{k+1|k}(x_{k+1|k}^{\text{\tiny{$\sim$}}})^{\text{T }}+Q,\end{split}  \tag{24}\]  
Step 3: Measurement Update  
Perform the matrix decomposition with SVD and calculate the cubature point:  
\[ \begin{split} P_{k+1|k}&=U_{k+1|k}\begin{bmatrix}S_{ k+1|k}&0\\ 0&0\end{bmatrix}U_{k+1|k}^{\text{T}}\enspace,\\ x_{k+1|k}&=U_{k+1|k}\sqrt{S_{k+1|k}\hat{\xi}_{i}}+\hat{x}_{k+1|k}\enspace, \end{split}