ROV State Estimation Using Mixture of Gaussian Based on Expectation-Maximization Cubature Particle Filter

Abstract

The underwater motion of the ROV is affected by various environmental factors, such as wind, waves, and currents. The complex relationship between these disturbance variables results in non-Gaussian noise distribution, which cannot be handled by the classical Kalman filter. For the accurate and real-time observation of ROV climbing, and, at the same time, to reduce the influence of the uncertainty of the noise distribution, the ROV state filter is designed based on the mixture of Gaussian model theory with the expectation-maximization cubature particle filter (EM-MOGCPF). The EM-MOGCPF considers different sensor measurement noises, and the addition of mixture of Gaussian (MOG) improves the fineness and real-time properties, while the expectation-maximization (EM) reduces the complexity of the algorithm. To estimate the ROV xyz-axis and yaw angular states, we establish a four-degree-of-freedom (4-DOF) ROV kinetics model, which uses a simulation platform for multiple sea state degrees. The results show that the EM-MOGCPF effectively improves the estimation accuracy and exhibits strong adaptability to nonlinear and non-Gaussian environments. We believe that this algorithm holds promise in solving the state estimation challenge in these difficult environments.

1. Introduction

The adhesion of marine organisms to the anchor chain can significantly reduce its weight support capability. Furthermore, the secretions from these organisms can speed up the corrosion of the steel structure. To ensure proper weight support and extend the lifespan of the steel structure, frequent underwater cleaning of the anchor chain is necessary. This cleaning must adhere to safety regulations set forth by the classification society and the operation area. Given the complexity of the underwater environment, the cleaning process requires a specialized ROV. This type of ROV is strongly coupled and strongly nonlinear [1,2,3]. Therefore, the ROV’s control accuracy is essential in ensuring that it runs normally. To calculate the current state of the ROV using a series of filtering and data fusion algorithms, state estimation is adopted, which plays a critical role in the control of the ROV. It serves as the intermediary layer between sensor measurement and controller calculation. By using a series of filtering and data fusion algorithms, the current state of the ROV can be accurately calculated. This information can then be used to improve the accuracy of the controller, creating a closed loop that enhances the ROV’s performance.

Many scholars have carried out a lot of work on the state estimation algorithm. The Kalman filter [4] has been widely used in the model-driven state estimation method because of how straightforward and easy it is to use. Standard Kalman filter theory only applies to linear systems; however, in engineering applications, the system is typically nonlinear. Bucy and Sunahara proposed the extended Kalman filter (EKF) [5], Dong et al. [6] proposed an EKF-based adaptive object tracking method for AUV state estimation, Han et al. [7] designed a tracking controller based on feedback linearization by using EKF to estimate the state of UVMS, and Zheng et al. [8] implemented the adaptive control of UVMSs using EKF to estimate system state and forecast external disturbances, but EKF ignores the high-order terms of the nonlinear system, lowering the estimation accuracy [9]. The original filtering method has been enhanced with further research [10]. The other two frequently used filtering techniques are unscented Kalman filtering (UKF) [11] and the cubature Kalman filter (CKF) [12]. UKF uses a series of sigma points to propagate the state and covariance matrix [13]. CKF calculates the integral; unlike UKF, CKF uses a 2n cubature point propagation state and covariance matrix. When using an identical matrix factorization method, such as the Cholesky method, CKF has a lower calculation burden than UKF and has better stability and higher precision [14]. Chen et al. [15] designed an augmented state cubature Kalman filter to improve the stability of the long-range navigation of surface ships.

When the system noise cannot be approximated as a Gaussian noise distribution, the estimation accuracy of the Kalman filter decreases. In this case, a numerical approximation method, the particle filter, is needed [16]. By using a Monte Carlo simulation, a huge number of particles are created, and the distribution of these particles is utilized to estimate the probability distribution of states. Particle filters are used in research [17] to complete attitude estimation during the UAV landing procedure; a particle filter based on multi-model interaction [18] was presented to address the issue of estimating the performance of several rotors, and it minimizes the estimate error when compared to EKF. The correlation filter [19], which combines particle filters and correlation filters, performs well in state estimation; Jing and Chen [20] proposed a particle filter based on color distribution for object tracking, which increased the tracking accuracy by addressing the issue of object tracking illumination changes and occlusion.

Mixture of Gaussian (MOG) can be seen as a combination of k single Gaussian models; MOG is widely utilized to mimic non-Gaussian density because it enhances the real-time state estimation performance while maintaining the estimation performance [21]. Kotecha and Djuric [22] extended MOG and PF to dynamic space models with non-Gaussian noise; Ulmschneider et al. [23] used MOG in multi-path-assisted positioning to improve the positioning accuracy. A recursive estimator for non-Gaussian problems was derived in [24]; any probability density can be approximated by a mixture model of finite Gaussian functions [25]. MOGKF outperforms PF and IMM-KF in non-Gaussian issues, as Bilik showed in [26].

In this paper, we address the challenging problem of ROV state estimation in an underwater environment that is nonlinear and non-Gaussian. Due to the complexity of the underwater environment, the process noise of ROV sensors is non-Gaussian, which poses a significant challenge to state estimation. To overcome this issue, we propose a state estimation algorithm that operates effectively in a non-Gaussian noise environment. Specifically, we apply the CKF to the PF framework to compute the mean and variance of the importance density function, generate a new posterior probability density distribution, and regenerate new particles. The problem of particle degradation is alleviated by using MOG instead of a single Gaussian noise distribution in the EM algorithm, which also reduces the amount of calculation required during resampling. Simulation trials are used to confirm the algorithm’s efficacy and noise resistance.

The rest of this paper is as follows. Section 2 analyzes the ROV motion state and establishes a four-degree-of-freedom dynamic model. Section 3 selects state quantities and observations and reviews the CKF algorithm. Section 4 introduces the proposed EM-MOGCPF algorithm. Section 5 presents the simulation results. Then, Section 6 summarizes the main work of this paper.

2. ROV Modeling

2.1. Description

The research object of this paper is an anchor chain cleaning robot; the three-dimensional model is illustrated in Figure 1. The ROV features an octahedral open frame structure, with a maximum operational water depth of 500 m.

The crawling state of the anchor chain is shown in Figure 2. The operation process of the ROV is divided into five steps, namely lifting into the water, underwater movement, cleaning operation, inspection operation, and recovery.

2.2. Coordinate System

According to Figure 3, this paper establishes a dynamic model of the ROV in two coordinate systems, namely the earth frame O-XYZ and the body-fixed frame o-xyz. The earth frame is a fixed system with O as the origin, where OX and OY are two horizontal directions perpendicular to each another, and OZ points vertically to the water surface. Meanwhile, the ROV’s position and direction are described by the body-fixed frame, which is a local coordinate system fixed on the moving rigid body. The body-fixed frame has o as the origin, where ox represents the front of the ROV movement, oz represents the bottom, and oy and the other two axes form a right-handed coordinate system.

The ROV has six-degree-of-freedom (DOF) motions: three translational motions, surge (X), sway (Y), and heave (Z). There are three Euler angles, roll ($\phi$), pitch ($\theta$), and yaw ($\psi$), which represent the position and attitude of the ROV relative to the inertial system. The linear velocity and angular velocity of the ROV are denoted by ($u,v,w$) and ($p,q,r$), respectively.

The motion state of the ROV on the anchor chain is constrained according to the working state specified in Section 2.1, and the self-stabilizing roll and pitch motions make it unnecessary to consider the roll and pitch angles. Therefore, the ROV motion model’s 4-DOF are simplified, and the motion parameters are defined as presented in

Table 1. Definition of motion parameters.

DOF	Motions	Forces and Moments	Linear and Angular Velocities	Position and Euler Angles
1	Surge	X	u	x
2	Sway	Y	v	y
3	Heave	Z	w	z
4	Yaw	N	r	$\psi$

[27].

In the inertial coordinate system, the attitude vector is $\eta ={\left[xyz\psi \right]}^{\mathrm{T}}$, where $xyz$ is the three-axis translation displacement, and $\psi$ is the heading angle. The motion coordinate system’s velocity vector is $V={\left[uvwr\right]}^{\mathrm{T}}$, where $uvw$ is the linear velocity, and r is the angular velocity around the z-axis. A few assumptions are made as follows:

• The ROV density is distributed uniformly;
• There is low-speed movement;
• The center of buoyancy and gravity are congruent.

The space conversion equation is given by

$$\dot{\eta }=J\left(\eta \right)V,$$

(1)

where $J\left(\eta \right)$ is Jacobian matrix

$$J\left(\eta \right)=\left[\begin{array}{cc}{J}_1\left(\eta \right)& 0_{3\times 3}\\ 0_{3\times 3}& J_2\left(\eta \right)\end{array}\right]$$

(2)

$J_1\left(\eta \right)$ and $J_2\left(\eta \right)$ are given by

$$J_1\left(\eta \right)=\left[\begin{array}{ccc}cos\psi & -sin\psi cos\phi +cos\psi sin\theta sin\phi & sin\psi sin\phi +cos\psi sin\theta sin\phi \\ sin\psi cos\theta & cos\psi cos\phi +sin\phi sin\theta sin\psi & -cos\psi sin\phi +sin\theta sin\psi cos\phi \\ -sin\theta & cos\theta sin\phi & cos\theta cos\phi \end{array}\right],$$

(3)

$$J_2\left(\eta \right)=\left[\begin{array}{ccc}1& sin\phi tan\theta & cos\phi tan\theta \\ 0& cos\phi & -sin\phi \\ 0& sin\phi sec\theta & cos\phi sec\theta \end{array}\right],$$

(4)

2.3. ROV Dynamic Model

In Fossen’s theory [27], ROV dynamics consist of a rigid body force, hydrodynamic force, and water surface static force:

$$\begin{array}{l}M\dot{V}+C_{RB}\left(V\right)V+C_A\left(V\right)V+\dots \hfill \\ D_{NL}\left(V\right)V+D_L\left(V\right)V+g\left(\eta \right)+g_r=\tau +{\tau }_{env},\end{array}$$

(5)

The system inertia matrix $M\in R^{4\times 4},M=M_{RB}+M_A$, composed of the rigid-body and added mass matrices,

$$M_{RB}=\left[\begin{array}{cccc}m& 0& 0& 0\\ 0& m& 0& 0\\ 0& 0& m& 0\\ 0& 0& 0& I_z\end{array}\right],M_A=\left[\begin{array}{cccc}-X_{\dot{u}}& 0& 0& 0\\ 0& -Y_{\dot{v}}& 0& 0\\ 0& 0& -Z_{\dot{w}}& 0\\ 0& 0& 0& -N_{\dot{r}}\end{array}\right],$$

(6)

The Coriolis-centripetal matrix $C\left(V\right)\in R^{4\times 4}$, $C\left(V\right)=C_{RB}\left(V\right)+C_A\left(V\right)$ is composed of the rigid-body and added mass effects:

$$C_{RB}\left(V\right)=\left[\begin{array}{cccc}0& 0& 0& -mv\\ 0& 0& 0& mu\\ 0& 0& m& 0\\ mv& -mu& 0& 0\end{array}\right],C_A\left(V\right)=\left[\begin{array}{cccc}0& 0& 0& Y_{\dot{v}}v\\ 0& 0& 0& -X_{\dot{u}}u\\ 0& 0& 0& 0\\ -Y_{\dot{v}}v& X_{\dot{u}}u& 0& -N_{\dot{r}}\end{array}\right],$$

(7)

The damping matrix $D\left(V\right)\in R^{4\times 4},D\left(V\right)=D_L+D_{NL}\left(V\right)$ is composed of the linear and nonlinear hydrodynamic damping:

$$D_L=\left[\begin{array}{cccc}-X_u& 0& 0& 0\\ 0& -Y_v& 0& 0\\ 0& 0& -Z_w& 0\\ 0& 0& 0& -N_r\end{array}\right],$$

(8)

$$D{}_L=\left[\begin{array}{cccc}-X_{\left|u\right|u}\left|u\right|& 0& 0& 0\\ 0& -Y_{\left|v\right|v}\left|v\right|& 0& 0\\ 0& 0& -Z_{\left|w\right|w}\left|w\right|& 0\\ 0& 0& 0& -N_{\left|r\right|r}\left|r\right|\end{array}\right],$$

(9)

$g\left(\eta \right)$ and $g\left(0\right)$ are the restoring forces that try to bring the system to its natural equilibrium; it is implemented as

$$g\left(\eta \right)=\left[\begin{array}{c}0\\ 0\\ -(W-B)\\ 0\end{array}\right],$$

(10)

where W and B are, respectively, buoyancy and gravity.

For the ROV, $\tau$ gives propulsion. There are only four different motions, surge, sway, heave, and yaw, because the ROV is attached to an anchor chain; hence, $\tau \in R^{4\times 1},\tau =\left[{\tau }_x{\tau }_y{\tau }_z{\tau }_{\psi }\right]$

The ROV is exposed to environmental disturbance forces and moments in the water throughout the diving process, which are made up of secondary wave forces, ocean currents, and wind forces. The bias model $\dot{b}$ is

$$\dot{b}=F_b+w_b,$$

(11)

where $w_b$ is a zero mean Gaussian white noise vector, and $F_b$ is a environmental disturbance matrix.

2.4. Complete ROV State Space Model

The complete mathematical model of an ROV is represented using a state space formulation that integrates the coordinate system and dynamic model. By combining Equations (1) and (5), we obtain a state space model.

$$\begin{array}{l}\dot{\eta }=R\left(\psi \right)V,\\ \dot{V}=M^{-1}[-C\left(V\right)v-D\left(V\right)V-g\left(\eta \right)-g_r+\tau +{\tau }_{env}],\end{array}$$

(12)

where, as in Table 1, it has 4-DOF; when moving on the anchor chain, the transverse rolling and pitching movements do not need to be considered. The transformation matrix $J\left(\eta \right)$ is simplified to a pure rotational matrix $R\left(\psi \right)$.

3. State Estimation Based on Nonlinear Gaussian Systems

3.1. Observer Design

The Kalman filter is designed on the ROV of the 4-DOF dynamic model, and the dynamic model of the ROV has been given in Section 2.3. The state variables are defined as follows:

$$\widehat{x}=\left[xyz\phi uvwr{b}_x{b}_y{b}_z{b}_{\phi }\right],$$

(13)

where $\widehat{x}\in R^{12\times 1}$. In a linear system, the state space equation can be expressed as

$$\begin{array}{l}\dot{x_k}=A{x}_{k-1}+B{u}_{k-1}+E{w}_{k-1},\\ z_k=H{x}_k+v_k,\end{array}$$

(14)

Expanding the matrices in Equation (14) yields

$$A=\left[\begin{array}{c}R\left(\psi \right)V\\ M^{-1}(-D\left(v\right)v-C\left(v\right)v+R^{\mathrm{T}}\left(\psi \right)b)\\ F_b\\ 0_{9\times 1}\end{array}\right],B=\left[\begin{array}{c}0\\ M^{-1}\\ 0\end{array}\right],$$

(15)

$$E=\left[\begin{array}{ccc}0& 0& 0\\ 0& M^{-1}& 0\\ 0& 0& 1_{4\times 4}\end{array}\right],H=\left[\begin{array}{cc}{1}_{4\times 4}& 0_{4\times 8}\end{array}\right],$$

(16)

where $A\in R^{12\times 1}$, is the state-transition matrix, $B\in R^{12\times 4}$ is the input matrix, $E\in R^{12\times 12}$ is the noise matrix, $H\in R^{4\times 12}$ is the measurement, w is the process noise, $w\sim (0,Q_k)$, and v is the measurement noise, $v\sim (0,R_k)$.

3.2. 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, Arasaratnam and Haykin [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{array}{l}\dot{x_k}=f(x_{k-1},u_{k-1})+w_k,\\ z_k=h\left(x_k\right)+v_k,\end{array}$$

(17)

where $f(•)$ and $h(•)$ 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{array}{l}{\widehat{x}}_k=E\left(x_0\right),\\ P_k=E(x_0-{\widehat{x}}_k){(x_0-{\widehat{x}}_k)}^{\mathrm{T}},\end{array}$$

(18)

and substituting Equation (17) into Equation (18) yields

$${\widehat{x}}_k=E[f(x_{k-1},u_{k-1})+w_{k-1}].$$

(19)

Since $w_{k-1}$ is assumed to be zero-mean and independent of past measurements, we obtain

$$\begin{array}{cc}\hfill {\hat{x}}_k& =E[f(x_{k-1},u_{k-1})+w_{k-1}]\hfill \\ & =\int f(x_{k-1},u_{k-1})p\left(x_{k-1}\right|Y_{k-1})d{x}_{k-1}\hfill \\ & =\int f(x_{k-1},u_{k-1})\times \mathcal{N}(x_{k-1};{\widehat{x}}_{k-1|k-1},P_{k-1|k-1})d{x}_{k-1},\hfill \end{array}$$

(20)

where $\mathcal{N}$ is a symbol for the Gaussian density, $Y_{k-1}=[u_i,z_i],i=1\dots k-1$ indicates the input measurement from 1 to $k-1$, and $p\left(x_{k-1}\right|Y_{k-1})$ is the old posterior density at time $k-1$,

Similarly, we can obtain the error covariance $P_{k-1}$:

$$\begin{array}{cc}\hfill P_k& =E(x_0-{\widehat{x}}_k){(x_0-{\widehat{x}}_k)}^{\mathrm{T}}\hfill \\ & =\int f(x_{k-1},u_{k-1})f^{\mathrm{T}}(x_{k-1},u_{k-1})\times \mathcal{N}(x_{k-1};{\widehat{x}}_{k-1|k-1},P_{k-1|k-1})d{x}_{k-1}\hfill \\ & -{\widehat{x}}_{k|k-1}{\widehat{x}}_{k-1|k-1}^{\mathrm{T}}+Q_{k-1},\hfill \end{array}$$

(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{array}{l}{P}_{k-1}=U_{k-1}\left[\begin{array}{cc}{S}_{k-1}& 0\\ 0& 0\end{array}\right]U_{k-1}^{\mathrm{T}},\\ x_k^i=U_{k-1}{\sqrt{S_{k-1}\xi }}_i+{\widehat{x}}_k,i=1,2,\dots ,2n,\\ {\xi }_i=\left\{\begin{array}{cc}\sqrt{n}{\left[1\right]}_i,\hfill & i=1,2,\dots ,2n,\hfill \\ -\sqrt{n}{\left[1\right]}_i,\hfill & i=n+1,n+2,\dots ,2n,\hfill \end{array}\right.\end{array}$$

(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, ${\left[1\right]}_i$ denotes the column i of $\left[1\right]$, and $\left[1\right]$ is a symmetric matrix with a dominant diagonal line of 1.

Propagate the cubature point:

$$x_{k+1|k}^i=f\left(x_k^i\right),$$

(23)

The state variable predicted values and error covariance predicted values are

$$\begin{array}{l}{\widehat{x}}_{k+1|k}=\frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}}{x}_{k+1|k}^i,\\ P_{k+1|k}=\frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}}{x}_{k+1|k}^i{\left(x_{k+1|k}^i\right)}^{\mathrm{T}}-\widehat{x_{k+1|k}}{\left(\widehat{x_{k+1|k}}\right)}^{\mathrm{T}}+Q,\end{array}$$

(24)

Step 3: Measurement Update

Perform the matrix decomposition with SVD and calculate the cubature point:

$$\begin{array}{cc}\hfill & \hfill P_{k+1|k}=U_{k+1|k}\left[\begin{array}{cc}{S}_{k+1|k}& 0\\ 0& 0\end{array}\right]U_{k+1|k}^{\mathrm{T}},\hfill \\ \hfill & \hfill x_{k+1|k}=U_{k+1|k}{\sqrt{S_{k+1|k}\xi }}_i+{\widehat{x}}_{k+1|k},\hfill \end{array}$$

(25)

Propagate the cubature point:

$$z_{k+1|k}^i=h(x_{k+1|k}^i),$$

(26)

Calculate the measured predicted values:

$${\widehat{z}}_{k+1}=\frac{1}{2n}\sum _{i=1}^{2n}{z}_{k+1}^i,$$

(27)

The measurement error covariance and cross-covariance at time k+1 have the following forms:

$$\left\{\begin{array}{l}{P}_{k+1}^z=\frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}}{z}_{k+1}^i{\left(z_{k+1}^i\right)}^{\mathrm{T}}-\widehat{z_{k+1}^i}{\left(z_{k+1}^i\right)}^{\mathrm{T}}+R,\\ P_{k+1}^{xz}=\frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}}{x}_{k+1}^i{\left(z_{k+1}^i\right)}^{\mathrm{T}}-\widehat{x_{k+1}}{\left(\widehat{z_{k+1}}\right)}^{\mathrm{T}},\end{array}\right.$$

(28)

The Kalman gain, states, and error covariance are expressed as follows:

$$\begin{array}{l}{K}_{k+1}=P_{k+1}^{xz}{\left(P_{k+1}^{xy}\right)}^{-1},\\ {\widehat{x}}_{k+1}={\widehat{x}}_{k+1|k}+K_{k+1}(z_{k+1}-{\widehat{z}}_{k+1}),\\ P_{k+1}=P_{k+1|k}-K_{k+1}{P}_{k+1}^Z{K}_{k+1}^T,\end{array}$$

(29)

4. State Estimation Based on Nonlinear/Non-Gaussian Systems

4.1. CPF Algorithm

The underwater motion of the ROV is affected by various environmental factors, such as wind, waves, and currents. The complex relationship between these disturbance variables results in a non-Gaussian noise distribution, which cannot be handled by the classical Kalman filter. Therefore, it is difficult to meet the actual system’s requirements, and the Kalman filter fails to achieve optimal estimation performance.

In nonlinear and non-Gaussian systems, the particle filtering algorithm can approximate the posterior probability density of random variables satisfying any distribution (Gaussian or non-Gaussian) and state (linear or nonlinear) as the number of particles increases. However, the importance density function of traditional particle filtering is the state transfer probability function, which does not consider the latest quantitative information. As a result, the particles are prone to degeneracy. To alleviate this problem, the cubature Kalman filter is applied to the particle filtering framework.

At time step k, the CKF algorithm is employed to compute the mean and variance of the importance density function based on the observed data. A new posterior probability density distribution is then generated, which can generate new particles. The weights of the particles are calculated, normalized, and resampled to complete the state estimation process.

The importance density distribution used in the PF is designed by the CKF algorithm, which is obtained by the calculations described in Section 3.2.

$$q({\widehat{x}}_{k|k}^i|{\widehat{x}}_{k|k-1}^i,z_k)=N({\widehat{x}}_{k|k}^i,P_{k|k}^i),$$

(30)

where N is the total number of particles, which is sampled from the importance function at moment k:

$$x_k^i\sim N({\widehat{x}}_{k|k}^i,P_{k|k}^i),$$

(31)

When $i=1,2,\dots N$, calculate the importance weights of the particles again and normalize:

$$\begin{array}{l}{w}_k^i=\frac{p(x_k|X_k^i)p(x_k^i|x_{k-1}^i)}{q({\widehat{x}}_{k|k}^i|{\widehat{x}}_{k|k-1^i},z_k)},\\ \overline{w_k^i}=w_k^i[{\displaystyle \sum _{i=1}^N}{w}_k^i{]}^{-1},\end{array}$$

(32)

When the weights of the particles fall below the designated threshold, the particles are resampled, resulting in N particles with equal weights $(X_k^i,\frac{1}{N})$. Calculate state estimates as follows:

$$\begin{array}{l}{\widehat{x}}_{k|k}=\frac{1}{N}{\displaystyle \sum _{i=1}^N}{x}_k^i,\\ {\widehat{P}}_{k|k}=\frac{1}{N}{\displaystyle \sum _{i=1}^N}(x_k^i-{\widehat{x}}_{k|k}){(x_k^i-\widehat{x_{k|k}})}^{\mathrm{T}},\end{array}$$

(33)

4.2. MOGCPF

In this paper, we propose a modified algorithm, MOGCPF, based on the CPF algorithm. CPF leverages the CKF to obtain a Gaussian distribution, which is used as the importance density function. However, in practice, the importance density function cannot always be accurately approximated by a Gaussian distribution, which limits the accuracy of CPF. To address this issue, we leverage MOG, which can approximate any posterior probability density. By selecting appropriate mixture components, we obtain an improved approximation for the importance density function, which enhances the accuracy of the MOGCPF algorithm.

In the framework of CPF, using the MOG approximation, the posterior density of states and the densities of process and measurement noise at moment k are assumed to be approximated as follows:

$$\begin{array}{l}{p}_G\left(x_k\right|z_k)={\displaystyle \sum _{i=1}^G}{w}_k^{i}N(x_k;u_k^i,P_k^i),\\ p_G\left(v_k\right)={\displaystyle \sum _{a=1}^T}{r}_k^{a}N(v_k;u_{v,k}^a,Q_k^a),\\ p_G\left(q_{k+1}\right)={\displaystyle \sum _{m=1}^M}{\gamma }_{k+1}^{m}N(q_{k+1};u_{r,k+1}^m,R_{k+1}^m),\end{array}$$

(34)

where G is the number of mixed components; ${\omega }_i$ is mixed weights. The MOG-approximated predicted density $p_G\left(x_k\right|z_k)$ of states at time $k+1$ is

$$\begin{array}{cc}\hfill p_G\left(x_{k+1}\right|z_k)& =\int p\left(x_{k+1}\right|x_k)p\left(x_k\right|z_k)d{x}_k,\hfill \\ & \approx {\displaystyle \sum _{r=1}^K}{\displaystyle \sum _{i=1}^G}{w}_k^i{r}_k^i\times \int N(x_{k+1};f\left(x_k\right)+u_{v,k}^b,Q_k^b)\times N(x_{k+1};f\left(x_k\right)+u_k^i,P_k^i)d{x}_k,\hfill \end{array}$$

(35)

where $G^{\prime }=GK,i=1,2,\dots ,G,r=1,2,\dots ,K,i^{\prime }=1,\dots ,G^{\prime }$, with mixed weights $w_{k+1}^{i^{\prime }}$

$$w_{k+1}^{i^{\prime }}=\frac{w_k^i{r}_k^i}{{\displaystyle \sum _{i=1}^G\sum _{r=1}^K}{w}_k^i{r}_k^i},$$

(36)

During the CKF time update,

$$\begin{array}{l}{x}_k^{i^{\prime }}=u_k^i,\\ S_k^{i^{\prime }}=\sqrt{P_k^i},\end{array}$$

(37)

Calculate the cubature point (l is the number of dimensions):

$$\begin{array}{l}{X}_{l,k}^{i^{\prime }}=S_k^{i^{\prime }}\sqrt{n{\left[I\right]}_l}+x_{l,k}^{i^{\prime }},\\ X_{l,k+1}^{i^{\prime }}=f(X_{l,k}^{i^{\prime }})+u_{v,k}^b,\\ {\widehat{x}}_{k+1|k}^{i^{\prime }}=\frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}}{X}_{l,k+1}^{i^{\prime }},\\ P_{k+1|k}^i=\frac{1}{2n}{\displaystyle \sum _{i=1}^{2n}}{X}_{l,k+1}^{i^{\prime }}{(X_{l,k+1}^{i^{\prime }})}^{\mathrm{T}}-{\widehat{x}}_{k+1|k}^{i^{\prime }}{({\widehat{x}}_{k+1|k}^{i^{\prime }})}^{\mathrm{T}}+Q_k^b,\end{array}$$

(38)

and let $u_{k+1}^{i^{\prime }}=x_{k|k+1}^{i^{\prime }},P_{k+1}^{i^{\prime }}=P_{k+1|k}^{i^{\prime }}$; thus, the MOG of the prior density function is

$$p_G\left(x_{k+1}\right|z_k)=\sum _{i=1}^G{w}_{k+1}^{i}N(x_k;u_{k+1}^{i^{\prime }},P_{k+1}^{i^{\prime }}),$$

(39)

Similarly, in the CKF measurement update phase, the posterior density function

$$p_G\left(x_{k+1}\right|z_k)=\sum _{i^{‴}=1}^{G^{″}}{w}_{k+1}^{i^{‴}}N(x_k;u_{k+1}^{i^{″}},P_{k+1}^{i^{″}}),$$

(40)

It can be seen that in the time update, the Gaussian component increases from G to $G^{\prime }$, and the Gaussian component increases from $G^{\prime }$ to $G^{″}$ in the measurement update; as the estimation process proceeds, it leads to an increasing amount of computation, so it is necessary to reduce $G^{\prime }$ to G during resampling.

4.3. EM-MOGCPF

The EM algorithm [28] is used on the basis of MOGCPF to calculate MOG parameters in the set of importance-sampled particles, which reduces $G^n$ to G, improves the problem of particle sample degradation, and reduces the computational effort. The algorithm flow is shown in Figure 4.

The mixture weights for each model are set to $w_g=\frac{1}{G}$, the mean $u_g$ is set to a random number, the covariance matrix $P_g$ is set to a unit matrix, and we calculate $P_g$, $w_g$ and take the proportion of each type of sample to the total number of samples:

$$\rho =[(w_1,u_1,P_1),(w_2,u_2,P_2),\dots ,(w_i,u_i,P_i),\dots ](i=1,\dots ,G),$$

(41)

Calculate $r_1\left(x_k\right),r_2\left(x_k\right),\dots ,r_i\left(x_k\right),k=1,2,\dots ,N$ by $\rho$.

$$r_i\left(x_k\right)=\frac{{\alpha }_{i}N(x_k;u_i,P_i)}{{\displaystyle \sum _{i=1}^G}{\alpha }_{i}N(x_k;u_i,P_i)},$$

(42)

Obtain the new mean ${\widehat{u}}_i$, variance $\widehat{P_i}$, and mixture weights $\widehat{w_i}$:

$$\begin{array}{l}{\widehat{u}}_i=\frac{{\displaystyle \sum _{k=1}^N}{r}_i\left(x^k\right)x^k}{{\displaystyle \sum _{k=1}^N}{r}_i\left(x^k\right)},\\ \widehat{P_i}=\frac{{\displaystyle \sum _{k=1}^N}{r}_i\left(x^k\right)(x^k-{\widehat{u}}_i){(x^k-{\widehat{u}}_i)}^{\mathrm{T}}}{{\displaystyle \sum _{k=1}^N}{r}_i\left(x^k\right)},\\ \widehat{w_i}=\frac{1}{N}{\displaystyle \sum _{k=1}^N}{r}_i\left(x^k\right),\end{array}$$

(43)

Let $\widehat{\rho }=[({\widehat{w}}_1,{\widehat{u}}_1,{\widehat{P}}_1),({\widehat{w}}_2,{\widehat{u}}_2,{\widehat{P}}_2),\dots ,({\widehat{w}}_i,{\widehat{u}}_i,{\widehat{P}}_i),\dots ]$; the iteration stops when $|\rho -\widehat{\rho }|<v$ (v is a minimum value); otherwise, obtain $\rho =\widehat{\rho }$ and return to Equation (42) to calculate $r_i\left(x_k\right)$.

When $w_{k+1}^i={\widehat{w}}_i,u_{k+1}^i={\widehat{u}}_i,P_{k+1}^i={\widehat{P}}_i$, obtain the new posterior density:

$$p_G\left(x_{k+1}\right|z_{k+1})=\sum _{i=1}^G{w}_{k+1}^{i}N(x_{k+1};u_{k+1}^i,P_{k+1}^i),$$

(44)

from the EM algorithm; we can observe a reduction in the number of mixture components, $G^n$ to G, which has the effect of reducing the posterior probability density at moment $k+1$.

State estimate at moment $k+1$:

$$\begin{array}{l}{\widehat{x}}_{k+1}={\displaystyle \sum _{i=1}^G}{w}_{k+1}^i{u}_{k+1}^i,\\ {\widehat{P}}_{k+1}={\displaystyle \sum _{i=1}^G}{w}_{k+1}^i[P_k^i+(u_{k+1}^i-{\widehat{x}}_{k+1}){(u_{k+1}^i-{\widehat{x}}_{k+1})}^{\mathrm{T}}],\end{array}$$

(45)

5. Experimental and Analysis

5.1. Experimental Settings

The ROV is measured to have a mass of 26 kg; moment of inertia $I_z=1.04$ kg · m². The buoyancy and gravity counterweights are balanced. Meanwhile, the overall structure of the ROV designed in this paper is symmetrical front-to-back, up-and-down, and left-to-right, with the center of gravity coordinates as the origin of the coordinate axis.

By the above analysis and calculation, we can obtain the rigid-body matrix:

$$M_{RB}=\left[\begin{array}{cccc}26& 0& 0& 0\\ 0& 26& 0& 0\\ 0& 0& 26& 0\\ 0& 0& 0& 1.04\end{array}\right],$$

(46)

The added mass matrix:

$$M_A=\left[\begin{array}{cccc}-24.56& 0& 0& 0\\ 0& -34.27& 0& 0\\ 0& 0& -39.19& 0\\ 0& 0& 0& -2.01\end{array}\right],$$

(47)

The linear hydrodynamic damping matrix:

$$D_L=\left[\begin{array}{cccc}-30.57& 0& 0& 0\\ 0& -42.21& 0& 0\\ 0& 0& -89.10& 0\\ 0& 0& 0& -0.028\end{array}\right],$$

(48)

The nonlinear hydrodynamic damping matrix:

$$D_{NL}\left(V\right)=\left[\begin{array}{cccc}-97.43& 0& 0& 0\\ 0& -79.39& 0& 0\\ 0& 0& -117.40& 0\\ 0& 0& 0& -1.25\end{array}\right],$$

(49)

The evaluation index adopted in this study is the root mean square error (RMSE), which represents the square root of the ratio of the squared difference between the predicted and true values to the number of observations N. The RMSE value serves as a measure of the model’s performance, with smaller values indicating more accurate prediction results. The calculation formula for RMSE is presented below:

$$RMSE=\sqrt[2]{\frac{1}{N}\sum _{i=1}^N{(\widehat{y}-y)}^2},$$

(50)

where N is the total number of samples, $\widehat{y}$ is the predicted attitude of the ROV, and y is the ROV’s actual attitude.

Due to the complexity of simulating the real state of the anchor chain underwater, it is necessary to approximate the anchor chain as a suspension line. The suspension line can be described by the general equation

$$y=acosh(\frac{x}{a}-1),$$

(51)

where a is the distance from the vertex of the curve to the horizontal coordinate.

The motion state of the ROV is influenced by varying sea states, which are presented in

Table 2. State of the sea.

Degree	Wind	Wind Speed (m/s)	Wave	Wave Height (m)
0	Calm	0−0.2	Calm glassy	/
1	Light air	0.3−1.5	Calm rippled	0−0.1
2	Light breeze	1.6−3.3	Smooth wavelet	0.1−0.5
3	Gentle breeze	3.4−5.4	Slight	0.5−1.25

[29] below. The strength of ROV interference increases as the sea state degree increases. When the sea state degree is 0, the impact of waves and sea wind can be mostly disregarded, and only the influence of ocean currents needs to be considered. Therefore, a Gaussian noise model can be assumed during this time. Conversely, when the sea state degrees are 1–3, the impact of waves, sea wind, and ocean currents must be taken into account, and a non-Gaussian noise model should be used.

State vector $\widehat{x}=0$, covariance matrix $p_0=diag[0.3,0.3,0.3,0.3,0.3,0.3,0.3,0.3,0.3,0.3,0.3,0.3]$, $T=200$, particles $N=300$, anchor chain length $a=60.845$. Our proposed algorithm is subjected to comparison with CKF, CPF, and PF.

5.2. Results and Discussion

5.2.1. Sea State Degree: 0

At a sea state degree of 0, the surface of the sea is calm with no wind or waves. The only external noise present is caused by underwater currents, and it is assumed that this noise follows a Gaussian distribution. Process noise $w_k\sim N(0,Q)$, measurement noise $v_k\sim N(0,R)$, $R=diag[0.3,0.2,0.3,0.05]$, $Q=diag[0,0,0,0,3,4,5,0.02,2,2,2,0.5]$.

Figure 5 and Figure 6 present the errors in the estimated displacement and velocity of the ROV using the proposed algorithm, as well as three other algorithms: CPF, CKF, and PF. It is evident that the PF algorithm suffers from particle degradation over time, which leads to a decrease in estimation accuracy and filter divergence. On the other hand, the estimation accuracy of CKF in the presence of Gaussian noise is consistent with expectations, and CPF effectively addresses the particle degradation issue by using CKF for importance density function calculation. Consequently, the proposed algorithm outperforms the other algorithms at a sea state of 0.

5.2.2. Sea State Degree: 1−2

There is a light breeze and smooth wavelet on the sea surface at a sea state of 1–2, and the measurement noise is non-Gaussian distributed, $v_k\sim 1.875N(0,R)+0.04N(0.357\ast R),R=diag[0.4,0.3,0.3,0.4]$. Figure 7 and Figure 8 demonstrate that the CKF algorithm diverges in both the XYZ axis and yaw angle estimation when the noise distribution is non-Gaussian. This indicates that CKF is only suitable for Gaussian noise environments. Furthermore, in the estimation of Y and Z axis displacement, particle degradation gradually occurs in PF after a certain period of time. The importance density function of CPF cannot always be approximated by a Gaussian distribution, as evidenced by the small divergence observed at sea state levels 1–2. Hence, the proposed algorithm shows improved noise immunity by incorporating a mixed Gaussian model. Overall, the results suggest that the algorithm presented in this paper outperforms CKF, PF, and CPF under mixed noise conditions.

5.2.3. Sea State Degree: 3

A sea state of 3 represents the maximum sea state in which an ROV can operate effectively. Beyond this level, the use of an ROV is not recommended. In such challenging environments, the benefits of the improved algorithm become apparent. Moreover, when the sea surface is affected by slight waves and a gentle breeze, the measurement noise follows a non-Gaussian distribution, $v_K\sim 0.1N(0,R_1)+0.9N(0,150\ast R_2)$, $R_1=diag[0.4,0.3,0.3,0.4],R_2=diag[0.2,0.2,0.1,0.003]$. In Figure 9 and Figure 10, it is evident that the estimation error of the CKF algorithm increases as the sea state level rises and the external noise becomes more complex. The filter exhibits multiple divergences in these scenarios. Specifically, when estimating the Z-axis displacement and bow angle, the CKF algorithm’s error is even larger than that of the PF algorithm, which suffers from particle degradation and multiple outliers in the estimation due to its inability to consider the latest measurement information. Moreover, the complexity of the noise model makes it difficult to approximate the mixed Gaussian noise using only a single Gaussian distribution.

The CPF algorithm’s error starts to increase when estimating the X-axis displacement after T > 100 s, and when estimating the Y-axis displacement after T > 40 s, indicating that CPF cannot guarantee estimation accuracy over a long time. On the other hand, the algorithm proposed in this paper not only has no outliers, but also avoids divergence in long-term estimates, while significantly improving the accuracy compared to other algorithms.

Table 3. RMSE of different algorithms in three sea states.

Simulation Case	Attitude	PF	CKF	CPF	EM-MOGCPF
Sea state degree: 0	X	1.8577	1.0618	0.8291	0.5633
	Y	1.2232	0.9602	0.7297	0.5290
	Z	3.8294	0.8827	0.6871	0.5140
	$\psi$	2.5201	1.5164	1.5018	0.9196
Sea state degree: 1–2	X	0.8289	1.1010	0.6611	0.4392
	Y	2.6940	1.4307	0.8125	0.3738
	Z	6.9908	3.9050	1.0363	0.4771
	$\psi$	1.6541	1.7259	0.8849	0.4083
Sea state degree: 3	X	2.1690	3.5037	1.2855	0.5956
	Y	7.5129	5.0190	1.6607	0.3094
	Z	5.9652	10.9400	3.3708	0.6318
	$\psi$	8.7070	14.6010	6.7736	2.1357

and Figure 11 and Figure 12 demonstrate that the RMSE values of $x,y,z,\psi$ attitudes can be found.

(1)

In sea state degree 0, the particle filtering has a maximum RMSE in all four attitudes due to the appearance of particle degeneracy. The CKF algorithm performs better than the particle filtering algorithm in a Gaussian noise environment. CPF reduces particle degradation after importance density sampling with CKF, and the estimation accuracy is improved by $55.37\%,49.35\%,82.06\%,40.41\%$ in $x,y,z,\psi$ compared to particle filtering, respectively. This indicates that CPF was able to solve the particle degradation problem. The algorithm EM-MOGCPF proposed in this paper has the best estimation accuracy, compared with the best phenotypic performance of CPF, and the four attitudes were improved by $32.06\%,27.5\%,24.76\%,38.77\%$, respectively.

(2)

In sea state degree 1–2, underwater noise is non-Gaussian distributed under the influence of wind and waves. The RMSE of the CKF algorithm is slightly greater than that of particle filtering for the x-axis and yaw angular $\psi$ estimates. CKF’s estimation accuracy decreases and it is not suitable for state estimation under non-Gaussian noise. Although CPF combines the applicability of particle filtering for non-Gaussian noise, it is applicable to a single noise model; the EM-MOGCPF algorithm incorporates the MOG model, which effectively improves the estimation accuracy under mixed noise.

(3)

As the sea state degree rises to 3, the RMSE of CKF in three attitudes is greater than that of particle filtering; it shows that as the noise environment becomes more complex, the accuracy of CKF estimation is worsened. Similarly, the performance of CPF at this sea degree is also poor. By comparison, the estimation accuracy of EM-MOGCPF is also the highest among the four algorithms. RMSE does not show large values as the sea degree increases, and it has good noise immunity in complex environments. Compared to CPF, which has the best relative stability, the accuracy is improved by $53.67\%,81.37\%,81.26\%,68.47\%$. This shows that EM-MOGCPF has good estimation accuracy even at the maximum operating conditions.

6. Conclusions

In this paper, we propose the EM-MOGCPF algorithm, an improved state estimation approach based on the CPF framework, for underwater robots operating in environments with nonlinear and non-Gaussian noise distributions. Our algorithm leverages a hybrid Gaussian model to approximate the significant type density function, and employs the EM algorithm instead of traditional resampling to mitigate the particle degradation problem. We also use a modified algorithm for underwater anchor chain cleaning robot observation, and validate the performance of the algorithm under three different sea state conditions. Our experimental results demonstrate that the proposed algorithm can accurately estimate the robot state and achieve strong convergence even in the presence of different noise distributions, while effectively resisting the influence of noise. The algorithm exhibits good environmental adaptability and can address the problem of robot state estimation in underwater nonlinear and non-Gaussian environments. Moreover, the proposed algorithm can be applied to other state estimation fields.

Future research includes developing a simulation platform for underwater robot state observation, exploring underwater robot models with more degrees of freedom and parameter estimation, and conducting additional underwater experiments to further evaluate the real-time performance and accuracy of the algorithm.