## 2 Problem Formulation  
Let SV be in motion (swims along a given trajectory) in the time interval \(\tau\). During this time, at any moment \(k(k\in\tau)\), the onboard navigation equipment determines course over ground \(COG(k)\) and speed over ground \(SOG(k)\). The measuring system is based on an omnidirectional camera, which determines relative bearings \(RB_{1}(k)\), \(RB_{2}(k)\), \(\ldots\), \(RB_{n}(k)\) and distances \(D_{1}(k),D_{2}(k),\ldots,D_{n}(k)\) to \(n\) beacons, located in the navigation area. At each subsequent moment \((k+1)\), based on the results of the measurements \(COG(k)\) and \(SOG(k)\) taken at the previous moment instant \((k)\), let the so-called state vector be estimated: \(\hat{\mathbf{x}}(k+1)=[ \begin{array}{cc}\mathbf{x}(k+1)&y(k+1)\quad COG(k+ 1)\quad SOG(k+1)\end{array} ]^{\mathrm{T}}\). This is done using a function for a non-linear SV motion model (DR method) (1). \[\mathbf{f}(\widehat{\mathbf{x}}(k),\mathbf{u}(k),\mathbf{w}(k))= \begin{bmatrix}f_{ x}\\ f_{y}\\ f_{COG}\\ f_{SOG}\end{bmatrix}  \tag{1}\] \[= \begin{bmatrix}\mathrm{x}(k)\\ \mathrm{y}(k)\\ COG(k)\\ SOG(k)\end{bmatrix} + \begin{bmatrix}\Delta t(k+1)\cdot\mathrm{SOG}(k)\cdot \mathrm{cosCOG}(k)\\ \Delta t(k+1)\cdot\mathrm{SOG}(k)\cdot\mathrm{sinCOG}(k)\\ COG(k)+\Delta COG(k)\\ SOG(k)+\Delta SOG(k)\end{bmatrix} + \begin{bmatrix}w_{x}(k)\\ w_{y}(k)\\ w_{COG}(k)\\ w_{SOG}(k)\end{bmatrix} ,\]  
where:  
\(x(k),y(k)\)--coordinates of the SV position calculated at the moment \(k\),  
\(\Delta t(k+1)\)--time period from \(k\) to \((k+1)\) time moments,  
\(\Delta COG(k)\), \(\Delta SOG(k)\)--are known control inputs at \(k\) time moment,  
\(w_{x}(k),w_{y}(k),w_{COG}(k),w_{SOG}(k)\)--so-called random disturbances in the determination of coordinates, course over ground and speed over ground in the form of errors with zero expectation value and normal distribution \(\mathrm{N}[0,1]\) at \(k\) time moment. In addition, based on the estimated state vector \(\widehat{\mathbf{x}}(k+1)\) and the known coordinates of \(x_{l_{i}},\ y_{l_{i}}\) of the position of the \(i\)-th beacon \(l_{i}\), let the observation vector be estimated \(\widehat{\mathbf{z}}(k+1)\) (distance and relative bearing to the beacon \(l_{i}\)) using the function (2):  
\[\mathbf{h}(\widehat{\mathbf{x}}(k+1),\mathbf{\
u}(k+1))= \begin{bmatrix}f_{D_{i }}\\ f_{RB_{i}}\end{bmatrix}  \tag{2}\] \[= \begin{bmatrix}\sqrt{\left(x_{l_{i}}-x(k+1)\right)^{2}+\left(y_{ l_{i}}-y(k+1)\right)^{2}}\\ COG^{\prime}(k+1)-\mathrm{atan}\frac{x_{l_{i}}-x(k+1)}{y_{l_{i}}-y(k+1)} \end{bmatrix} + \begin{bmatrix}\
u_{D_{i}}(k+1)\\ \
u_{RB_{i}}(k+1)\end{bmatrix} ,\]  
where:  
\(i\)-th navigational mark against which measurements have been taken \(RB_{i}(k+1)\) and \(D_{i}(k+1)\) is measured using the system based on the omnidirectional camera at the moment \((k+1)\);  
\(COG^{\prime}(k+1)=\left\{ \begin{array}{c}COG(k+1)-360^{\circ},\ COG(k+1)\geq 1 80^{\circ}\\ COG(k+1),\ COG(k+1)<180^{\circ}\end{array} \right.\)--\(COG\) of the SV expressed in semicircular measure, as determined by onboard navigational equipment at the moment \((k+1)\);  
\(\
u_{D_{i}}(k+1),\
u_{RB_{i}}(k+1)\)--so-called random distortions in the determination of the distance and relative bearing to the \(i\)-th navigational mark, expressed in terms of errors with zero expectation and normal distribution \(\mathrm{N}[0,1]\) at the moment \((k+1)\). The functions, constructed as \(\mathbf{f}(\widehat{\mathbf{x}}(k),\mathbf{u}(k),0)\) and \(\mathbf{h}(\widehat{\mathbf{x}}(k+1),0)\), can be used to estimate SV position coordinates according to the EKF algorithm from both measurements \(COG(k)\) and \(SOG(k)\), as well as \(RB_{1}(k),RB_{2}(k),\ldots,RB_{n}(k)\) and \(D_{1}(k),D_{2}(k),\ldots,D_{n}(k)\) (Figure 1).