## 4 Velocity Tracing Method for DVL Based on Motion Constraint
### Velocity Tracing Algorithm for DVL Based on AUV Motion Constraints  
When the AUV sails, it is driven by an engine, steering, and currents. It is difficult to construct an accurate model describing the motion relationship between the input and output. However, for AUVs conducting long time and distance sailing tasks, they prefer to sail at an economic velocity. Therefore, the driving force from the engine and steering is constant. If the circle wave is ignored and only linear current is considered, the velocity and (or) acceleration of the AUV along the body frame can be addressed as a constant or slowly changing value [17]. To simplify the analysis, the current is assumed to be zero. In this condition, the velocity along the forward direction of the AUV is a fixed value. When the \"right-forward-up\" frame is adopted as the body frame, the velocity along the \(y\)-axis can be considered as a constant value and acceleration as zero. In this condition, the velocity \(V_{by}\) and acceleration \(a_{by}\) can be selected as the state vector, and the measured velocity from the DVL can be used as the measurement vector. Then, the system and measurement equations can be constructed as:  
\[\left\{ \begin{array}{c}\dot{\textbf{X}}_{by}\left(t\right)=\left[\begin{array} []{c}V_{by}\\ a_{by}\end{array}\right]=\left[\begin{array}{cc}0&1\\ 0&0\end{array}\right]\textbf{X}_{by}\left(t\right)+\left[\begin{array}{cc}1 &0\\ 0&1\end{array}\right]\textbf{w}_{by}\left(t\right)\\ \textbf{Z}_{by}\left(t\right)=\left[\begin{array}{cc}1&0\end{array}\right] \textbf{X}_{by}\left(t\right)+\textbf{v}_{by}\left(t\right)\end{array} \right., \tag{22}\]  
Figure 3: Sudden changes in the measurement of the DVL velocity. where \(\mathbf{Z}_{by}=\sqrt{\left(v_{\text{DVL}}^{bx}\right)^{2}+\left(v_{\text{DVL}}^{by} \right)^{2}+\left(v_{\text{DVL}}^{bz}\right)^{2}},\mathbf{w}_{by}=\left[ \begin{array}[ ]{cc}w_{v_{by}}&w_{ay}\end{array} \right]^{\text{T}}\) denotes the disturbance caused by the excitation of the wave, which can be considered as system noise, and \(\mathbf{v}_{by}\) is considered measurement noise. After discretization of Equation (22), the time and measurement can be updated with Equation (13), and the instant DVL velocity \(\hat{V}_{by}\) is estimated. When the line current is considered, the velocity along both the \(x\)-axis and \(z\)-axis should be managed with the same model of Equation (22). When the estimation \(\hat{V}_{by}\) along the \(y\)-axis of the AUV is obtained, the velocity of the AUV in the body frame can be constructed as \(\hat{\mathbf{V}}_{\text{DVL}}^{b}=\left[ \begin{array}{cc}0&\hat{V}_{by}&0\end{array}  \right]^{\text{T}}\). After this velocity vector is projected into the navigation frame with the SINS attitude matrix, \(\mathbf{C}_{b}^{n}\hat{\mathbf{V}}_{\text{DVL}}^{b}\) can be used for data fusion in the SINS/DVL. Based on the above model, the random noise is effectively restrained, which will be proved in the next section. When there is a short, sudden change in DVL velocity, the time update velocity can be used as the substitute velocity to perform data fusion. Then, how to judge this sudden change becomes a problem. Fault diagnosis with \(\chi^{2}\) rules is a common method to detect sudden changes in engineering [16]. In this method, the measurement at time \(k-\ 1\) is assumed without a sudden change, and the prediction \(\hat{\mathbf{X}}_{k,k-1}\) with state vector at \(k-1\) is without error. If the measurement at \(k\) has only random noise, the innovation (also defined as the residual error) is a white noise sequence. However, when the measurement \(\mathbf{Z}_{k}\) at \(k\) has a sudden change, the innovation undergoes a large change. When the change is above a particular threshold, it can be deduced that a sudden change occurs.