## 3 Target Tracking Method Based on EKF  
In this scenario, the AUV tracks MRS with the acoustic sensor, which measures the position of MRS in the AUV coordinate frame. The state of the target motion model is described by a nearly constant velocity model and at time step (k) includes the position in two dimensional (2D) Cartesian coordinates x and y and the velocity towards those coordinates axes \(v_{x}\) and \(v_{y}\). All coordinate frames are shown in Figure 3.  
In Figure 3, \((x_{v},y_{v})\) and \(\psi_{v}\) are the position and heading of AUV, respectively. \(\left(x_{f},y_{f}\right)\) and \(\psi_{f}\) are the position and heading of MRS, respectively. \(O_{1}\) is the global North, East coordinate frame, \(O_{2}\) is the local AUV coordinate frame. The measurement at time index k can be expressed as: \(z_{k}=\left[z_{x}\ z_{y}\right]^{T}\). \(z_{x}\) and \(z_{y}\) are the horizontal and vertical direction of the measurement components in \(O_{2}\) local coordinate system. \(d_{x}\) and \(d_{y}\) are the horizontal and vertical direction of relative distance components between the AUV and MRS in global coordinate frame. The system state Cartesian coordinates is defined as follow:  
Figure 2: The overview of our algorithm. \[\mathbf{x}_{k}=\left[x_{v,k}\;y_{v,k}\;x_{f,k}\;\hat{x}_{f,k}\;y_{f,k}\;\hat{y}_{f,k} \right]^{T} \tag{1}\]  
where \(\mathbf{x}_{k}\) is the system state at time index \(k\), \(x_{v,k}\) and \(y_{v,k}\) are the Cartesian coordinates of AUV, \(x_{f,k}\) and \(y_{f,k}\) are the Cartesian coordinates of MRS, \(\hat{x}_{f,k}\) and \(\hat{y}_{f,k}\) are the horizontal and vertical direction of the forward speed components of MRS in \(O_{1}\) global coordinate frame. The state transition model is:  
\[\mathbf{x}_{k}=\mathbf{F}\cdot\mathbf{x}_{k-1}+\mathbf{B}_{k}+\mathbf{\Gamma}_{k}\cdot\mathbf{w}_{k} \tag{2}\]  
where \(\mathbf{F}= \begin{bmatrix}1&0&0&0&0&0\\ 0&1&0&0&0&0\\ 0&0&1&dt&0&0\\ 0&0&0&1&0&0\\ 0&0&0&0&1&dt\\ 0&0&0&0&0&1\end{bmatrix} \), \(\mathbf{B}_{k}= \begin{bmatrix}u_{v,k}\cdot cos\left(\psi_{v,k}\right)\cdot dt\\ u_{v,k}\cdot sin\left(\psi_{v,k}\right)\cdot dt\\ 0\\ 0\\ 0\end{bmatrix} ,\mathbf{\Gamma}_{k}= \begin{bmatrix}cos\left(\psi_{v,k}\right) \cdot dt&-u_{v,k}\cdot sin\left(\psi_{v,k}\right)\cdot dt&0\\ sin\left(\psi_{v,k}\right)\cdot dt&-u_{v,k}\cdot cos\left(\psi_{v,k}\right) \cdot dt&0\\ 0&0&cos\left(\psi_{f,k}\right)&-u_{f,k}\cdot sin\left(\psi_{f,k}\right)\\ 0&0&sin\left(\psi_{f,k}\right)\cdot dt&u_{f,k}\cdot cos\left(\psi_{f,k}\right) \end{bmatrix} ,dt=t_{k}-t_{k-1}\cdot u_{v,k}\]  
is the forward speed of AUV at time index \(k\), \(\psi_{v,k}\) is the heading of AUV at time index \(k\). \(u_{f,k}\) is the forward speed of MRS at time index \(k\), \(\psi_{f,k}\) is the heading of MRS at time index \(k\). \(w_{k}\) is process noise which is assumed to be zero mean non-Gaussian noise, and its covariance is a fourth-order square matrix. The covariance of process noise is \(\mathbf{Q}= \begin{bmatrix}Q\left(u_{v}\right)&0&0&0\\ 0&Q\left(\psi_{v}\right)&0&0\\ 0&0&Q\left(u_{f}\right)&0\\ 0&0&0&Q\left(\psi_{f}\right)\end{bmatrix} \), where \(Q\left(u_{v}\right)\) is the noise variance of AUV speed, \(Q\left(\psi_{v}\right)\) is the noise variance of AUV heading, \(Q\left(u_{f}\right)\) is the noise variance of MRS speed, \(Q\left(\psi_{f}\right)\) is the noise variance of MRS heading. The MRS is tracked by an acoustic sensor in the underwater and provides measurements of the position of MRS in the AUV coordinate frame. Figure 3: Coordinate frames schematic diagram. The measurement model is given as  
\[\mathbf{z}_{k}=h\left(\mathbf{x}_{k}\right)+\mathbf{v}_{k} \tag{3}\] \[h\left(\mathbf{x}_{k}\right)=