## 5 Simultaneous Localization of AUV and Mobile Docking Station  
Assuming that M-DS does a uniform linear motion and has good communication with AUV. The SLAM with mobile object (SLAMMO) problem based on sampling mechanism is to calculate the joint posterior probability distribution of AUV and M-DS. \[ \begin{array}{l}P\left(x_{k},\Theta_{k}\mid x_{k-1},\Theta_{k-1},z_{k},u_{x,k },u_{\Theta,k}\right)\\ =P\left(x_{k}\mid x_{k-1},\Theta_{k-1},z_{k},u_{x,k},u_{\Theta,k}\right)\cdot P \left(\Theta_{k}\mid x_{k},x_{k-1},\Theta_{k-1},z_{k},u_{x,k},u_{\Theta,k}\right) \end{array}  \tag{20}\]  
where \(x_{k-1}\) and \(x_{k}\) are the states of AUV at \(1:k-1\) time and \(1:k\) time, respectively. \(\Theta_{k-1}\) and \(\Theta_{k}\) are the states of M-DS at \(1:k-1\) time and \(1:k\) time, respectively. \(z_{k}\) represents the measurement at \(k\)th time. \(u_{x,k}\) represent the control input of AUV at \(1:k\) time. \(u_{\theta,k}\) represent the control input of M-DS at \(1:k\) time. Firstly, the posterior probability distribution of AUV is calculated, and the proposed distribution is expanded as follows by Bayesian principle:  
\[ \begin{array}{l}P\left(x_{k}\mid x_{k-1},\Theta_{k-1},z_{k},u_{x,k},u_{ \Theta,k}\right)\\ \stackrel{{ Bayes}}{{\mathcal{K}}}\eta P\left(z_{k}\mid x_{k},x_{k -1},\Theta_{k-1},z_{k-1},u_{x,k},u_{\Theta,k}\right)\cdot P\left(x_{k}\mid x_{ k-1},\Theta_{k-1},z_{k-1},u_{x,k},u_{\Theta,k}\right)\end{array}  \tag{21}\]  
According to the motion model of AUV, the above formula is implied to:  
\[ \begin{array}{l}P\left(x_{k}\mid x_{k-1},\Theta_{k-1},z_{k},u_{x,k},u_{ \Theta,k}\right)\\ \stackrel{{ Markov}}{{=}}\eta P\left(z_{k}\mid x_{k},x_{k-1}, \Theta_{k-1},z_{k-1},u_{x,k},u_{\Theta,k}\right)\cdot P\left(x_{k}\mid x_{k-1},u_{x,k}\right)\end{array}  \tag{22}\]  
Using the theory of total probability and convolution theorem, the approximate form of the proposed distribution is obtained. \[P\left(x_{k}\mid x_{k-1},\Theta_{k-1},z_{k},u_{x,k},u_{\Theta,k}\right)= \xi exp\left(-y_{k}\right) \tag{23}\]  
\[S_{k}=R+H_{\Theta}P_{2}H_{\Theta}^{T} \tag{24}\]  
\[y_{k}=\frac{1}{2}\left[\left(z_{k}-\hat{z}_{k}-H_{x}\left(x_{k}-\hat{x}_{k} \right)-H_{\Theta}\left(\Theta_{k}-\hat{\Theta}_{k}\right)\right)^{T}S_{k}^{-1 }\left(z_{k}-\hat{z}_{k}-H_{x}\left(x_{k}-\hat{x}_{k}\right)-H_{\Theta}\left( \Theta_{k}-\hat{\Theta}_{k}\right)\right)\right] \tag{25}\] \begin{table}
\begin{tabular}{l c} \hline \hline
**Step** & **Formula** \\ \hline predict & \(s_{k}=f\left(s_{k-1},u_{k}\right)\) \\ update & \(\Sigma_{sk}^{[m]}=\left[G_{s}^{T}Q_{k}^{[m]-1}G_{s}+P_{k}^{-1}\right]^{-1}\) \\ & \(\mu_{sk}^{[m]}=\Sigma_{sk}^{[m]}G_{s}^{T}Q_{k}^{[m]-1}\left(z_{k}-\hat{z}_{k}^{ [m]}\right)+\hat{s}_{k}^{[m]}\) \\ & \(S_{c}\left(k\right)=G_{\Theta}\Sigma_{c\left(k\right)}^{[m]}\left(k-1\right)G_ {\theta}^{T}+R\left(k\right)\) \\ & \(K_{c}\left(k\right)=\Sigma_{c\left(k\right)}^{[m]}\left(k-1\right)G_{\theta}^{ T}Sc\left(k\right)^{-1}\) \\ & \(\mu_{c\left(k\right)}^{[m]}\left(k\right)=\mu_{c\left(k\right)}^{[m]}\left(k-1 \right)+K_{c}\left(z_{k}-z_{k}\right)\) \\ & \(\Sigma_{c\left(k\right)}^{[m]}\left(k\right)=\left(I-K_{c}\left(k\right)H_{f} \right)\Sigma_{c\left(k\right)}^{[m]}\left(k-1\right)\) \\ & \(L^{[m]}\left(k\right)=G_{s}P\left(k\right)G_{s}^{T}+G_{\Theta}\Sigma_{c\left(k \right)}^{[m]}\left(k-1\right)G_{\theta}^{T}+R\left(k\right)\) \\ & \(w^{[m]}\left(k\right)=\left|2\pi L^{[m]}\left(k\right)\right|^{-\frac{1}{2}} exp\left(-\frac{1}{2}\left(z_{k}-z_{k}\right)^{T}L^{[m],-1}\left(z_{k}-z_{k} \right)\right)\right.\) \\ \hline \hline \end{tabular}
\end{table} 
Table 2: FastSLAM2.0. The distribution represented by the formula (23) is Gaussian, and its mean and variance are given by the minimum value and curve of the formula (25). By calculating the first order and second order of differential of \(y_{k}\) to \(x_{k}\), the following formulas are obtained.