# A Novel Hybrid Approach to Deal with DVL Malfunctions for Underwater Integrated Navigation Systems
## 2 Underwater Integrated Navigation System
### System Model  
Set the Right-Front-Up (RFU) frame as the body frame, and the East-North-Up (ENU) geographic coordinates as the navigation frame. The error state vector of the integrated navigation system is set as  
\[X_{k}=[\phi_{e}\quad\phi_{n}\quad\phi_{u}\quad\delta V_{e}\quad\delta V_{n} \quad\delta V_{u}\quad\delta L\quad\delta\lambda\quad\delta h\quad\varepsilon _{x}\quad\varepsilon_{y}\quad\varepsilon_{z}\quad\
abla_{x}\quad\
abla_{y} \quad\
abla_{z}]^{T} \tag{1}\]  
where \(\phi_{e}\)\(\phi_{n}\)\(\phi_{u}\) are attitude errors. \(\delta V_{e}\), \(\delta V_{n}\), \(\delta V_{u}\) are velocity errors. \(\delta L\), \(\delta\lambda\), \(\delta h\) are position errors. \(\varepsilon_{x}\), \(\varepsilon_{y}\), \(\varepsilon_{z}\) are gyroscope drifts. \(\
abla_{x}\), \(\
abla_{y}\), \(\
abla_{z}\) are accelerometer biases. The system equation and the measurement equation for the local filters can be expressed as follows. \[\left\{ \begin{array}{c}X_{k}=\Phi_{k,k-1}X_{k-1}+W_{k}\\ Z_{k}=H_{k}X_{k}+V_{k}\end{array} \right. \tag{2}\]  
where \(X_{k-1}\) is the error states of the integrated navigation system at time \(t  
The integration of the DVL and the SINS is processed in the local filter 1. The measurement model of the local filter 1 can be constructed as  
\[ \begin{array}{ll}Z_{k}^{(1)}&=\left(\begin{array}{cc}V_{eSINS}^{n}-V_{eDVL}^{ n}&V_{nSINS}^{n}-V_{nDVL}^{n}&V_{uSINS}^{n}-V_{uDVL}^{n}\\ &=H_{k}^{(1)}X_{k}^{(1)}+V_{k}^{(1)}\end{array}\right)^{T}\\ \end{array}  \tag{3}\]  
where the superscript \(n\) denotes the navigation frame. As mentioned, the DVL provides the velocity measurements in the body frame and hence could not be used as the measurement for alignment directly. \(V_{eDVL}^{n}\), \(V_{nDVL}^{n}\) and \(V_{uDVL}^{n}\) are the projected velocity of the DVL measurements in the navigation frame, which can be calculated according to the following formula. \[V_{DVL}^{n}=C_{b}^{n}V_{DVL}^{b} \tag{4}\]  
where \(V_{DVL}^{b}\) is the DVL velocity measurements in the body frame. \(C_{b}^{n}\) is the transformation matrix from the body frame to the navigation frame, which can be expressed with the attitude angles. According to the relationship between the measurement vector and the error state vector, the measurement matrix of the local filter 1 can be expressed as follows. \[H_{k}^{(1)}=\left( \begin{array}{cc}0_{3\times 3}&I_{3\times 3}&0_{3\times 9} \end{array} \right) \tag{5}\]  
where \(I_{3\times 3}\) is a 3 \(\times\) 3 identity matrix. \(0_{m\times n}\) is a zero matrix with \(m\) rows and \(n\) columns. The covariance matrix of the DVL measurements caused by instrumental noise is given in [26], namely. \[R_{k}^{(1)}=\left( \begin{array}{ccc}\sigma_{b_{1}}^{2}+\sigma_{b_{2}}^{2}&0& \sigma_{b_{1}}^{2}-\sigma_{b_{2}}^{2}\\ \frac{4\sin^{2}\beta}{4\sin^{2}\beta}&0&\frac{\sigma_{b_{1}}^{2}-\sigma_{b_{2 }}^{2}}{8\sin\beta\cos\beta}\\ 0&\frac{\sigma_{b_{1}}^{2}+\sigma_{b_{2}}^{2}}{4\sin^{2}\beta}&\frac{\sigma_{ b_{2}}^{2}-\sigma_{b_{2}}^{2}}{8\sin\beta\cos\beta}\\ \frac{\sigma_{b_{1}}^{2}-\sigma_{b_{1}}^{2}}{8\sin\beta\cos\beta}&\frac{ \sigma_{b_{1}}^{2}+\sigma_{b_{2}}^{2}+\sigma_{b_{2}}^{2}+\sigma_{b_{2}}^{2}}{ 16\cos^{2}\beta}\end{array} \right) \tag{6}\]  
where \(\sigma_{b_{i}}^{2}\) is the variance of the radial velocity due to instrumental noise along the beam \(i\) (\(i=1,2,3,4\)). Assume that the radial velocity variances are equal, and then the covariance matrix can be simplified as  
\[R_{k}^{(1)}=\left( \begin{array}{ccc}\sigma_{v_{x}}^{2}&0&0\\ 0&\sigma_{y}^{2}&0\\ 0&0&\sigma_{v_{z}}^{2}\end{array} \right) \tag{7}\]  
where the diagonal elements are the variance of the DVL measurement along the body frame. The local filter 2 is responsible for fusing the MCP and the SINS. In parallel, the local filter 3 is in charge of the fusion between the PS and the SINS.