## 2 Problem Formulation
where:  
\(\widehat{\mathbf{x}}(k+1)\) and \(\mathbf{P}(k+1)\)--the estimated state vector and its covariance matrix, fixed a posteriori at time moment \((k+1)\),  
\(\widehat{\mathbf{x}}(k+1)^{-}\) and \(\mathbf{P}(k+1)^{-}\)--the estimated state vector and its covariance matrix, fixed a priori at time moment \((k+1)\),  
\(\mathbf{F}(k)=\left[ \begin{array}{cccc}\frac{\partial f_{x}}{\partial x}& \frac{\partial f_{x}}{\partial x}&\frac{\partial f_{x}}{\partial xCO}&\frac{ \partial f_{x}}{\partial SCO}\\ \frac{\partial f_{y}}{\partial x}&\frac{\partial f_{y}}{\partial y}&\frac{ \partial f_{y}}{\partial xCO}&\frac{\partial f_{y}}{\partial SCO}\\ \frac{\partial f_{CO}}{\partial x}&\frac{\partial f_{CO}}{\partial y}&\frac{ \partial f_{CO}}{\partial xCO}&\frac{\partial f_{CO}}{\partial SCO}\\ \frac{\partial f_{CO}}{\partial x}&\frac{\partial f_{CO}}{\partial x}&\frac{ \partial f_{CO}}{\partial xCO}&\frac{\partial f_{CO}}{\partial SCO}\\ \end{array} \right]=\)  
\(\left[ \begin{array}{cccc}1&0&\Delta t(k+1)\cdot SOG(k)\cdot cosCOG(k)&\Delta t (k+1)\cdot\text{sinCO}G(k)\\ 0&1&-\Delta t(k+1)\cdot SOG(k)\cdot\text{sinCO}G(k)&\Delta t(k+1)\cdot\text{ cosCO}G(k)\\ 0&1&0\\ 0&0&1\\ 0&0&1\\ \end{array} \right]\)--the so-called system matrix calculated as a Jacobi matrix of functions \(\mathbf{f}(\mathbf{x}(k),\mathbf{u}(k),0)\),  
\(\mathbf{Q}(k)=\left[ \begin{array}{cccc}\sigma_{x}^{2}&\sigma_{xy}&0&0\\ \sigma_{yx}&\sigma_{y}^{2}&0&0\\ 0&0&\sigma_{COG}^{2}&0\\ 0&0&0&\sigma_{SOG}^{2}\\ \end{array} \right]\)--the noise matrix of state vector at the time of moment \(k\) for assumed mean error values of the following measurements: course over ground \(\sigma_{COG}\) and speed over ground \(\sigma_{SOG}\), where:  
\(\sigma_{x}^{2}=\Delta t^{2}(k+1)\cdot(SOG(k)\cdot\sigma_{COG}\cdot\text{cosCOG }(k))^{2}+\Delta t^{2}(k+1)\cdot(\sigma_{SOG}\cdot\text{sinCO}G(k))^{2}\),  
\(\sigma_{y}^{2}=\Delta t^{2}(k+1)\cdot(SOG(k)\cdot\sigma_{COG}\cdot\text{sinCO }G(k+1))^{2}+\Delta t^{2}(k+1)\cdot(\sigma_{SOG}\cdot\text{cosCOG}(k))^{2}\),  
\(\sigma_{xy}=\sigma_{yx}=\Delta t^{2}(k+1)\cdot\sin(2\cdot COG(k))\cdot(\sigma _{SOG}^{2}-(SOG(k)\cdot\sigma_{COG})^{2})/2\),  
Figure 1: Algorithm of the estimation of the vector state \(\widehat{\mathbf{x}}(\mathbf{k}+1)\) using Extended Kalman Filter. \(\mathbf{P}(k)\)--of the state vector updated in the next time moment \(k\),  
\[\mathbf{J}_{\mathbf{h}}(k+1)=\left[ \begin{array}{cc}\frac{\vartheta f_{D_{i}}}{ \vartheta x}&\frac{\vartheta f_{D_{i}}}{\vartheta y}&\frac{\vartheta f_{D_{i}}}{ \vartheta y}\\ \frac{\vartheta f_{R_{i}}}{\vartheta x}&\frac{\vartheta f_{R_{i}}}{\vartheta y }&\frac{\vartheta f_{R_{i}}}{\vartheta x\text{COG}}&\frac{\vartheta f_{R_{i} }}{\vartheta\text{COG}}\end{array} \right]=\]  
\[\left[ \begin{array}{cc}\frac{x_{l_{i}}-x(k+1)}{D_{l_{i}}(k+1)}&\frac{y_{l_{i }}-y(k+1)}{D_{l_{i}}(k+1)}&0\\ \frac{y(k+1)-y_{l_{i}}}{D_{l_{i}}(k+1)D_{l_{j}}(k+1)}&\frac{x(k+1)-x_{l_{i}}}{ D_{l_{i}}(k+1)D_{l_{j}}(k+1)}&1&0\end{array} \right]\text{--the Jacobi matrix of the function }\mathbf{h}(\widehat{\mathbf{x}}(k+1),0)\text{,}\]  
generated for each pair of measurements taken up to the \(i\)-th navigation mark, with \(\text{D}_{l_{i}}(k+1)=\sqrt{\left(x_{l_{i}}-x(k+1)\right)^{2}+\left(y_{l_{i}}- y(k+1)\right)^{2}}\),  
\(\widehat{\mathbf{z}}(k+1)=\mathbf{h}(\widehat{\mathbf{x}}(k+1),0)\),  
\(\mathbf{z}(k+1)=[ \begin{array}{cc}D_{l}(k+1)\\ RB_{i}(k+1)\end{array} ]\),  
\(\mathbf{R}(k+1)=\left[ \begin{array}{cc}\sigma_{D}^{2}&0\\ 0&\sigma_{R}g^{2}\end{array} \right]\)--the noise matrix of a vector \(z(k+1)\) of instantaneous observations \((k+1)\) with the assumed values of the mean errors of the distance and relative bearing measurements \(\sigma_{D}\) and bearing \(\sigma_{RB}\) to the beacons. The SV position coordinates can also be estimated without measurement \(SOG(k)\), i.e., based on measurement results \(COG(k)\), \(RB_{1}(k)\), \(RB_{2}(k)\), \(\ldots\), \(RB_{n}(k)\) and \(D_{1}(k),D_{2}(k),\ldots,D_{n}(k)\).