## 5 Description of Data Fusion Using the Extended Kalman Filter
\right]=\left[\begin{array}{ c \( \begin{bmatrix}\frac{\partial f_{x}}{\partial\zeta\partial\zeta}&\frac{\partial f_{x} }{\partial\zeta\partial\zeta}&\frac{\partial f_{x}}{\partial\zeta\partial\zeta} \\ \frac{\partial f_{y}}{\partial\zeta\partial\zeta}&\frac{\partial f_{y}}{\partial \zeta\partial\zeta}&\frac{\partial f_{y}}{\partial\zeta\partial\zeta}\\ \frac{\partial f_{z}}{\partial\zeta\partial\zeta}&\frac{\partial f_{z}}{ \partial\zeta\partial\zeta}&\frac{\partial f_{z}}{\partial\zeta\partial\zeta} \\ \frac{\partial f_{z}}{\partial\zeta\partial\zeta}&\frac{\partial f_{z}}{ \partial\zeta\partial\zeta}&\frac{\partial f_{z}}{\partial\zeta\partial\zeta} \end{bmatrix} \)\( \begin{bmatrix}\sigma_{\text{COG}}{}^{2}&0&0\\ 0&\sigma_{\text{SOG}}{}^{2}&0\\ 0&0&\sigma_{\text{$\Delta\Delta^{2}$}}\end{bmatrix} \)\( \begin{bmatrix}\frac{\partial f_{x}}{\partial\zeta\partial\zeta}&\frac{\partial f_{x}}{ \partial\zeta\partial\zeta}&\frac{\partial f_{y}}{\partial\zeta\partial\zeta} \\ \frac{\partial f_{y}}{\partial\zeta\partial\zeta}&\frac{\partial f_{y}}{\partial \zeta\partial\zeta}&\frac{\partial f_{y}}{\partial\zeta\partial\zeta} \\ \frac{\partial f_{z}}{\partial\zeta\partial\zeta}&\frac{\partial f_{z}}{\partial \zeta\partial\zeta}&\frac{\partial f_{z}}{\partial\zeta\partial\zeta} \end{bmatrix} ^{\text{T}}\)--matrix of noises of the state vector at the time of \(k\) (with adopted mean errors \(\sigma_{\text{COG}}\), \(\sigma_{\text{SOG}}\), \(\sigma_{\text{$\Delta\Delta$}}\) of COG and SOG measurements and changes in submergence),  
\[\sigma_{xz}=\sigma_{yz}=\sigma_{zx}=\sigma_{zy}=0\]  
\(\sigma_{z}{}^{2}=\sigma_{\text{$\Delta\Delta^{2}$}}\)  
\(\mathbf{P}\left(k\right)\) --updated covariance matrix of the state vector used in subsequent time \(k+1\), \(\mathbf{H}\left(k+1\right)=\left[ \begin{array}{ccc}\frac{\partial f_{x}}{ \partial x}&\frac{\partial f_{x}}{\partial y}&\frac{\partial f_{x}}{\partial z }\\ \frac{\partial f_{x}}{\partial x}&\frac{\partial f_{y}}{\partial x}&\frac{ \partial f_{z}}{\partial x}\\ \frac{\partial f_{x}}{\partial x}&\frac{\partial f_{y}}{\partial y}&\frac{ \partial f_{z}}{\partial z}\\ \frac{\partial f_{x}}{\partial x}&\frac{\partial f_{y}}{\partial y}&\frac{ \partial f_{y}}{\partial z}\\ \end{array} \right]\)\( \begin{array}{ccc}\frac{x_{1}\left(k+1\right)-x\left(k+1\right)}{\sqrt{\left|k \left(k+1\right)}}&\frac{y_{1}\left(k+1\right)-y\left(k+1\right)}{\sqrt{ \left|k\left(k+1\right)}}&\frac{z_{1}\left(k+1\right)-z\left(k+1\right)}{ \sqrt{\left|k\left(k+1\right)}}&0\\ \frac{y_{1}\left(k+1\right)-y\left(k+1\right)}{2\left(k+1\right)}&\frac{y_{1} \left(k+1\right)-y\left(k+1\right)\right)\left(z_{1}\left(k+1\right)-z\left(k +1\right)\right)}{\sqrt{\left|z\left(k+1\right)\right|}\cdot 1\left(k+1\right)}&-\frac{ \sqrt{\left|k\left(k+1\right)\right|}}{\left|k\left(k+1\right)\right|}\end{array}  \right]\)  
--Jacobian matrix with function \(\mathbf{h}\left(\mathbf{x}\left(k\right),0\right)\),  
\[l_{3}\left(k+1\right)=\left(x_{1}\left(k+1\right)-x\left(k+1\right)\right)^{2}+ \left(y_{1}\left(k+1\right)-y\left(k+1\right)\right)^{2}+\left(z_{1}\left(k+1 \right)-z\left(k+1\right)\right)^{2}\]  
\[l_{2}\left(k+1\right)=\left(x_{1}\left(k+1\right)-x\left(k+1\right)\right)^{2}+ \left(y_{1}\left(k+1\right)-y\left(k+1\right)\right)^{2}\]  
\[\mathbf{R}\left(k+1\right)=\left[ \begin{array}{ccc}\sigma_{r}{}^{2}&0&0\\ 0&\sigma_{\text{$\Delta\Delta^{2}$}}&0\\ 0&0&\sigma_{\text{$\beta^{2}$}}\end{array} \right]\rightarrow\text{matrix of noises of the observation vector in time of $k+1$}\]  
(adopted mean measurement errors: \(\sigma_{r}\)--range, \(\sigma_{\text{$\alpha$}}\)--yaw, \(\sigma_{\text{$\beta$}}\)--pitch); \(\mathbf{I}\)--identity matrix.