An Iterative Doppler Velocity Log Error Calibration Algorithm Based on Newton Optimization

*e performance of the inertial navigation system/Doppler velocity log (INS/DVL) is mainly degraded by DVL error. An optimization-based algorithm is widely used and effective on calibrating DVL error. In this algorithm, the loss function established is a quadratic function of DVL attitude error; however, the existing optimization-based calibration method is just of first-order generally, so that it is not able to reach the best calibration efficiency. In order to improve the efficiency, an iterative calibration algorithm based on the second-order Newton optimization is proposed innovatively to calibrate the DVL error. In addition, the Wolfe principle is introduced to limit its loss function’s oscillation around the extreme point to improve the efficiency and accuracy of the Newton optimization. *e excellent efficiency and accuracy of the proposed calibration scheme are validated by simulation and field tests.


Introduction
High-accuracy navigation is a key technology that ensures the implementation of military and commercial activities for underwater vehicles. According to different application scenarios, there are many different navigation systems, such as acoustic positioning system (APS), Doppler velocity log (DVL), and inertial navigation system (INS). e APS can provide high-accuracy navigation; however, its position sensing relies on the seafloor-mounted transponder beacons. e INS is an autonomous navigation system, but its velocity and position tend to drift with time due to the integrations performed. In contrast, DVL not only can measure velocity without depending on the other equipment but also has good long-term accuracy; therefore, it has been widely used in long-endurance underwater vehicles [1][2][3].
In the past, the accuracy of DVL navigation was mainly limited by the attitude sensor error and the DVLrelated errors such as DVL scale factor error and the attitude misalignment between DVL and attitude sensor. In recent years, a series of new high-accuracy attitude sensors were developed to provide attitude information for DVL with only 0.01°error or even less. erefore, the attitude sensor error has been largely reduced, while the DVL-related errors have become the remaining challenges. In order to suppress their negative effect on navigation accuracy, many calibration algorithms have been proposed. According to the external information used in the calibration process, these algorithms can be divided into self-calibration algorithm without external information dependence [4,5], two-point method utilizing the positions of only two points on the trajectory, and the reference-assisted method which requires the high-accuracy position or velocity information on the whole calibration trajectory. Although the self-calibration method and the two-point method have less dependence on external information, their calibration time is generally longer and accuracy is poorer; therefore, the reference-assisted calibration method is the most preferable scheme for vehicles which performs high-accuracy long voyage tasks.
In the early research, the calibration schemes were usually based on the least square estimation (LS) in which the reference position was provided by the long baseline acoustic positioning system (LBL) [6]. In later research, the calibration accuracy was further improved because the regularity of attitude matrix was utilized to constrain the LS result [7]. Limited by the distance of acoustic transmission, LBL can only provide the position information for the vehicles within 10 kilometers from the transponder. In contrast, the global positioning system (GPS) has the ability to provide superior global navigation and has been employed widely by ocean vehicles [6]. erefore, the calibration algorithm that adopts the reference information provided by GPS instead of LBL was proposed [7,8]. ese calibration methods are all based on the LS; however, its performance is vulnerable to measurement error and stochastic noise. erefore, the iterative calibration method based on Kalman filter (KF) was proposed where the measurement error's effect is suppressed by the redundant predicted state. e attitude misalignment can be effectively calibrated using the LS and KF, but there are still two main disadvantages remaining: (1) ignoring the DVL scale factor error: the scale factor error is one of the main error sources for DVL velocity; it cannot be ignored in the high-accuracy navigation applications; (2) considering the relationship between attitude misalignment and measurement as linear simply: this relationship is nonlinear in real application because the effects on the measurement of attitude misalignment and scale factor are coupling. erefore, the accuracy of these linear estimation methods (KF, LS, etc.) must be suppressed by the nonlinear coupling error. In order to avoid the nonlinear error, calibration scheme comparing the DVL-derived tracks with the reference one is proposed. Because the influences of scale factor error and attitude misalignment on DVL position are different, the geometric relationship between DVL-derived track and the reference can be used to deduce the attitude misalignment and scale factor conversely [9][10][11][12]. In this scheme, the vehicle must navigate on the specified track for a long time [13]. To improve the calibration efficiency, an iterative calibration algorithm based on gradient descent optimization was proposed where the scale factor error and attitude misalignment can both be figured out without dependence on a specified track [14]. However, the fact that the loss function established in this algorithm is of secondorder while the gradient descent is only first-order optimization method causes low calibration efficiency [15].
A new iterative calibration method based on the Newton optimization, a second-order optimization algorithm, is proposed in this paper to improve the calibration efficiency. In addition, considering that the fixed-step principle adopted in the Newton optimization is prone to cause the oscillation of loss function value near the extreme point, the Wolfe principle is introduced to calculate the best step adaptively to solve this problem. Finally, the advantage of the proposed algorithm on accuracy and efficiency is verified, respectively, in simulation and field tests. Figure 1 shows an underwater vehicle installed with GPS receiver, INS, and DVL. ese three sensors provide the navigation information, respectively, in the navigation frame (n), vehicle body frame (b), and DVL body frame (d). In order to fuse the information of INS and DVL and establish the high-accuracy integrated navigation system, two problems should be settled in advance: (1) figuring out the attitude misalignment from the frame b to the frame d and the lever-arm vector caused by the misalignment of installation centers of INS and DVL, so as to eliminate the spatial inconsistency of two subsystems; (2) calculating the DVL scale factor to eliminate its negative effect on the accuracy of DVL velocity. ese two problems, DVL error calibration in other words, are exactly what we will solve in this paper.

Problem Formulation
During the calibration, the GPS velocity is regarded as the reference velocity. And the INS body frame is coincident with the vehicle body frame b for simplicity of exposition. e relation between DVL and GPS velocity can be expressed as [16] v where v d is the vehicle velocity in frame d provided by DVL; v n the vehicle velocity in frame n provided by GPS; k d the DVL scale factor; C d b the attitude misalignment from the frame b to the frame d expressed in direction cosine matrix (DCM) [9]; C b n the attitude misalignment from the n frame to the b frame; ω b nb the angular velocity of b frame with respect to the n frame; l b the lever-arm vector from the origin of frame d to that of b. e symbol × is the operator of the cross product, and it satisfies a × b � (a×)b for arbitrary two vectors, where (a×) is the skew-symmetric matrix [17]. On the right side of (1), k d , C d b , and l b are unknown parameters. C b n , v n , and ω b nb are provided by GPS or figured out with simple calculation; therefore, they all can be regarded as known where the calculation process of ω b nb is given as follows: where ω b ib is body angular rate measured by gyroscopes in the body frame; ε b gyroscopes bias which can be neglected due to its small enough quantitative magnitude; ω n ie Earth rotation rate which is approximately 7.3 × 10 − 5 rad/s, a very small value, so it can be also neglected. In addition, ω n en is achieved as where v N and v E are the elements in north and east directions of velocity vector; R M is the radius of Earth meridian circle; R N is the radius of Earth prime vertical circle; h and L, respectively, are the height and latitude at the position of the vehicle staying. In (3), vehicle velocity is far smaller than Earth radius; therefore, the elements of ω n en are 2 Mathematical Problems in Engineering approximately equal to zero. From the above analysis, we can conclude that ω b nb ≈ ω b ib , due to the small value of the other variables except ω b ib , and we rewrite (1) as

Calibration Scheme
All the error terms in (4) can be calibrated by the iterative calibration method based on Newton optimization proposed in this paper. Next, we will introduce the calibration scheme in detail.

Calibration of Scale Factor k d .
Since the DCM C d b does not change the modulus of the vector, the modulus values are still equal after removing DCMs on both sides of (2); that is, e scale factor k d can be achieved as In (6), all the noises of v n , v d , and ω b ib will degrade the precision of k d calibration. In order to eliminate this negative influence, we will integrate (2) on the time period [0, T] to gain In order to obtain the suitable form of (7) for computer processing, the discretization of (7) has been done as where (i) represents the value on the time iΔT with ΔT � T/n. On the right side of (8), the variables except l b can be calculated or measured directly. e effect of l b on the k d calibration emerges only when the vehicle performs angular movement; that is, ω b ib (i) ≠ 0. In addition, its effect will be offset if the vehicle performs an angular movement in the opposite direction. For the reason that the underwater vehicle performs mainly linear movement and few angular movements in practice, the physical meaning of (8) will keep approaching the "scale factor error" while the effect of l b gets weak. erefore, it is not necessary to calibrate the accurate lever-arm vector. A coarse lever-arm vector given artificially can meet the requirement of navigation accuracy. Based on the above analysis, it can be seen that the calibration accuracy of k d derived from (8) is immune to the attitude misalignment C d b ; besides, it will get higher as the time nincreases.

Calibration of Attitude Misalignment. Calibration of attitude misalignment, in other words, is solving
In (4), k d has been solved and l b is also given a coarse value, while C d b is the only remaining unknown matrix. erefore, we will construct the observation equation according to (4). Similarly, the integral of velocity will be used to calibrate the attitude misalignment instead of the noise-contained velocity. e discrete form of (4) is given directly as Substituting C d b with the equivalent quaternion q d b and reorganizing the terms [18,19], In theory, if the attitude misalignment is correctly calibrated, the modulus of κ should be extremely small. For this purpose, we establish the loss function as where norm(g) is the operator of calculating modulus. Our goal is to solve a quaternion q d b to minimize the value of loss function; that is,   (11) achieves the expansion of κ as [20] κ � In (14), κ is the quadratic function of elements of quaternion q d b , and so is its modulus norm(κ). erefore, if the first-order optimization algorithm, such as the gradient descent method, is used to search the optimal solution of q d b , the optimization efficiency will not be very high. In order to improve the optimization efficiency, we propose using the Newton method which is a second-order optimization method to search the optimal solution instead of the firstorder one. In the following, the calibration process using the Newton method will be introduced in detail. e second-order Taylor series expansion of where q d b(k) is the quaternion of kth iteration; ∇ the operator of calculating the gradient; ∇ 2 the operator of calculating the Hessian matrix [21]. For simplicity of exposition, ∇F(q d b ) and ∇ 2 F(q d b ) are, respectively, denoted as g(q d b ) and H(q d b ); in addition, ∇F(q d b(k) ) and ∇ 2 F(q d b(k) ) are denoted as g k and H k . e optimal solution of q d b is that one which makes the function value of Φ(q d b ) reach minimum, which is the e expression of ∇Φ(q d b ) can be obtained by calculating the gradient of (15): Setting (17) equal to zero gains If the initial value q d b(0) is given, the iterative solution of quaternion q d b(k) satisfying (18) can be constructed as e method of searching the optimal result according to the iterative format of equation (19) is called the Newton method. Although the method can be used to iterate the optimal results, it has two disadvantages: (1) huge computation burden caused by the recalculation of the Hessian matrix at each iteration, which is composed of the second-order partial derivative of the loss function; (2) low efficiency or oscillation near the stationary point due to the fixed-step iteration of the Newton method [22]. To overcome these problems, two targeted measures are carried out in our research: (1) replacing the Hessian matrix H k with the approximate matrix B k to avoid solving the second partial derivative, that is, adopting the Newton method; (2) searching the best step adaptively using the Wolfe principle instead of the fixed-step strategy [23]. e so-called Wolfe principle is to search the step λ k that meets the following conditions: where ρ ∈ (0, 0.5); ρ ∈ (ρ, 1); and d k � −H −1 k · g k . Before constructing the approximate matrix B, we need to derive the Newton principle from (18): where s k � q d b(k+1) − q d b(k) ; y k � g k+1 − g k . H k can be replaced with B k calculated as in the following iteration formulation: where B 0 can be taken as the unit matrix. erefore, the key is constructing the correction matrix ΔB k−1 in each step. We can hold it as Substituting (23) into (22) and combining formula (21), Based on (23)-(25), the correction matrix is obtained as follows: Substituting λ k and B k derived, respectively, from (20) and (22) into (19) gains the improved iterative formula as It must be noted that a threshold ε should be set as the criterion of stopping iteration. If the norm(g k ) is smaller than ε, q d b(k+1) is taken as the final calibration result. e quaternion q d b(k+1) is only one way to express the attitude misalignment. It is completely equivalent to other expressions, such as Euler angle and DCM. ese expressions can transform with each other, and the conversion relationship between them is shown in [24].
According to the above derivation, the DVL error iterative calibration method based on Newton optimization is summarized in Table 1.

Simulation Test
e simulation test based on INS/DVL/GPS is conducted first to validate the advance of the proposed iterative calibration method based on the Newton optimization. e whole simulation test lasts for 600 s, during which the vehicle accelerates, keeps uniform velocity, and turns left and right as Table 2 shows to simulate the real movements of the vehicle.
In the simulation test, the vehicle moves along the trajectory presented in Figure 2. Reference velocity is provided by GPS, so the GPS velocity error is regarded as 0. e initial alignment between INS and GPS has been completed by default, so the velocities provided by INS and GPS can be compared after a simple conversion between frame b and frame n. e output frequencies of INS, DVL, and GPS are 200 Hz, 1 Hz, and 1 Hz, respectively. e noise of GPS velocity is 0.1 m/s and the constant bias of the gyroscopes and accelerometers is 0.02°/s and 100 ug, respectively. e initial parameters of simulation are set properly where the scale factor error of DVL is 0.005; attitude misalignment between frame d and frame b 0.9°, −0.21°, 1.2°; lever-arm vector from DVL to INS 0 5 m 0 ; starting point position 32.05°118.79°; initial velocity 0. e EKF with the velocity error measurement, gradient descent optimization, and Newton optimization is used to calibrate DVL scale factor and attitude misalignment. In Figures 3-6, red, black, and blue solid lines, respectively, represent the calibration errors of these three methods. For the convenience of analysis, we calculate the means and root mean square errors of scale factor and attitude misalignment after convergence between 300 s and 600 s and then list them in Tables 3 and 4. e scale factor and attitude misalignment are estimated simultaneously in EKF, while they are estimated separately in the gradient descent optimization method and the Newton optimization method. Because the same method is adopted to solve the scale factor for these two optimization methods, the estimated scale factors have equal values. It is the reason why the black line and blue line coincide completely.
In Figure 3, it can be seen that the estimated scale factor using EKF method fluctuates greatly because it is affected by the noise of measurement, that is, GPS velocity noise. In addition, its mean is relatively larger than that of the scale factors estimated by the other methods due to the coupling of attitude misalignment. Table 1 Initialization Set q d b0 � 1 0 0 0 T , B 0 � I, ε, and a coarse l b Calibration process Step 1 Substitute l b into (8) to calculatek d Step 2 Substitute k d and q d bk into (9) to construct the observation equation of C d b Step 3 Establish the loss function F(q d b ) according to (12) Step 4 Substitute q d bk into loss function F(q d b ), and calculate its gradient value ∇F(q d b(k) ) Step 5 Determine the search direction d k � −H −1 k · g k Step 6 Calculate the best step λ k and the approximate matrix B k , respectively, with (20) and (22) Step 7 Iterate the latest quaternion of attitude misalignment q d b(k+1) with (27) Step 8 k � k + 1, and loop step 1-5 until |g k | < ε   Engineering Figures 4 and 6, respectively, show the estimation errors of pitch angle and yaw angle. It can be seen that the EKF estimation still fluctuates greatly due to the GPS velocity noise, while the estimation accuracy of the gradient descent method and the Newton method is smooth. In addition, the error convergence of the Newton method is significantly faster than that of the gradient descent method. And also, the estimation of the Newton method is steadier after the convergence because of the introduction of the Wolfe principle; the optimal step size is adaptively determined to avoid the oscillation of loss function near the extreme point. e estimated roll angle error is shown in Figure 5. Obviously, it is not calibrated correctly. is is because the vehicle always travels along with the y-axis of frame b; the velocities in x-axis and z-axis are extremely small; therefore, the attitude misalignment in these two directions is unobservable. Fortunately, the attitude misalignment in y-axis, that is, roll angle error, has no effects on the positioning accuracy of INS/DVL, so it is acceptable not to estimate its value.    Figure 7. e whole filed test lasts for 600s where the various actions, such as parking, acceleration, uniform velocity, deceleration, and turning, are carried out by the test vehicle. And the trajectory is shown in Figure 8. EKF and the algorithms based on gradient descent and Newton optimization are all applied to DVL error calibration for comparing their accuracies and efficiencies. In Figures 9-12, the blue lines, red lines, and yellow lines represent, respectively, the calibration errors of these three algorithms. During this field test, the vehicle carried out totally four large maneuvers where the EKF calibration errors in Figures 9-12 all show violent fluctuation, while the optimization-based algorithms are almost not influenced. In Table 5, the RMSEs show this result quantitatively. It reconfirms the conclusion that the optimization algorithms are insensitive to noise.  e same method is adopted by the gradient descent algorithm and the Newton algorithm to calculate the scale factor; therefore, the red and yellow lines in Figure 9 are identical. In Figures 10-12, the calibrated attitude misalignment errors are extremely approaching after 200 s. us, their mean value in Table 6 is almost equal. However, the Newton algorithm reaches this accuracy earlier than the gradient descent algorithm.

Mathematical Problems in
is proves that the secondorder Newton optimization algorithm has higher efficiency than the first-order gradient descent optimization algorithm. In this field test, the roll angle error is still not solved correctly as it is unobservable. Fortunately, it is not necessary to be calibrated because it has no effect on navigation accuracy.

Conclusion
To enhance the performance of the DVL calibration, the iterative calibration algorithm based on Newton optimization is proposed innovatively. According to the analysis in theory and the test validation, three important conclusions can be achieved. (1) e loss function established in optimization algorithms is a quadratic function of the attitude misalignment angles. erefore, the higher efficiency of convergence can be gained by the second-order Newton optimization algorithm compared with the first-order gradient descent. (2) e introduction of the Wolfe principle enables us to search the optimal step adaptively making the calibration accuracy steadier. (3) e iterative calibration algorithm which solves the scale factor and attitude misalignment separately can get a more accurate estimation result because it avoids the coupling between attitude misalignment and scale factor.
Data Availability e raw/processed data cannot be shared at this time as the data also form part of an ongoing study.