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

Authors: Wang Guangcai; Xiaosu Xu; Yiqing Yao
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

The 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. The excellent efficiency and accuracy of the proposed calibration scheme are validated by simulation and field tests.


---

## Body

## 1. 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). The APS can provide high-accuracy navigation; however, its position sensing relies on the seafloor-mounted transponder beacons. The 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–3].In the past, the accuracy of DVL navigation was mainly limited by the attitude sensor error and the DVL-related 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. Therefore, 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]. Therefore, the calibration algorithm that adopts the reference information provided by GPS instead of LBL was proposed [7, 8]. These calibration methods are all based on the LS; however, its performance is vulnerable to measurement error and stochastic noise. Therefore, the iterative calibration method based on Kalman filter (KF) was proposed where the measurement error’s effect is suppressed by the redundant predicted state. The 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. Therefore, 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–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 second-order 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.


## 2. Problem Formulation

Figure1 shows an underwater vehicle installed with GPS receiver, INS, and DVL. These 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. These two problems, DVL error calibration in other words, are exactly what we will solve in this paper.Figure 1
Sensors installation diagram.During the calibration, the GPS velocity is regarded as the reference velocity. And the INS body frame is coincident with the vehicle body frameb for simplicity of exposition. The relation between DVL and GPS velocity can be expressed as [16](1)vd=kdCbdCnbvn+ωnbb×lb,where vd is the vehicle velocity in frame d provided by DVL; vn the vehicle velocity in frame n provided by GPS; kd the DVL scale factor; Cbd the attitude misalignment from the frame b to the frame d expressed in direction cosine matrix (DCM) [9]; Cnb the attitude misalignment from the n frame to the b frame; ωnbb the angular velocity of b frame with respect to the n frame; lb the lever-arm vector from the origin of frame d to that of b. The 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), kd, Cbd, and lb are unknown parameters. Cnb, vn, and ωnbb are provided by GPS or figured out with simple calculation; therefore, they all can be regarded as known where the calculation process of ωnbb is given as follows:(2)ωnbb=ωibb−εb−Cnbωien+ωenn,where ωibb 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; ωien Earth rotation rate which is approximately 7.3×10−5rad/s, a very small value, so it can be also neglected. In addition, ωenn is achieved as(3)ωenn=−vNRM+hvERN+hvERN+htanL,where vN and vE are the elements in north and east directions of velocity vector; RM is the radius of Earth meridian circle; RN 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 ωenn are approximately equal to zero. From the above analysis, we can conclude that ωnbb≈ωibb, due to the small value of the other variables except ωibb, and we rewrite (1) as(4)vd=kdCbdCnbvn+ωibb×lb.


## 3. 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.


### 3.1. Calibration of Scale Factorkd

Since the DCMCbd does not change the modulus of the vector, the modulus values are still equal after removing DCMs on both sides of (2); that is,(5)vd=kdCnbvn+ωibb×lb.The scale factorkd can be achieved as(6)kd=Cnbvn+ωibb×lbvd.In (6), all the noises of vn, vd, and ωibb will degrade the precision of kd calibration. In order to eliminate this negative influence, we will integrate (2) on the time period 0,T to gain(7)kd=∫0TCnbvn+ωibb×lbdt∫0Tvddt.In order to obtain the suitable form of (7) for computer processing, the discretization of (7) has been done as(8)kd=∑i=1nCnbivni+ωibbi×lb∑i=1nvdi,where i represents the value on the time iΔT with ΔT=T/n. On the right side of (8), the variables except lb can be calculated or measured directly. The effect of lb on the kd calibration emerges only when the vehicle performs angular movement; that is, ωibbi≠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 lb gets weak. Therefore, 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 kd derived from (8) is immune to the attitude misalignment Cbd; besides, it will get higher as the time nincreases.


### 3.2. Calibration of Attitude Misalignment

Calibration of attitude misalignment, in other words, is solvingCbd. In (4), kd has been solved and lb is also given a coarse value, while Cbd is the only remaining unknown matrix. Therefore, 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. The discrete form of (4) is given directly as(9)∑i=1nvdi=Cbd∑i=1nkdCnbivni+ωibbi×lb.Substitutingβ=∑i=1nkdiCnbivni+ωibbi×lb and α=∑i=1nvdi into (9) gives the observation equation of Cbd as(10)α=Cbdβ.SubstitutingCbd with the equivalent quaternion qbd and reorganizing the terms [18, 19],(11)κ=qbd∗⊗α⊗qbd−β,where qbd∗ is the conjugation of qbd. 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(12)Fqbd=normκ,where normg is the operator of calculating modulus. Our goal is to solve a quaternion qbd to minimize the value of loss function; that is,(13)minqbd∈R4×1Fqbd=minqbd∈R4×1normκ.Substitutingqbd=q1q2q3q4Tα=αxαyαzT and β=βxβyβzT into (11) achieves the expansion of κ as [20](14)κ=1−2q32−2q42αx+2q2q3−2q1q4αy+2q2q4−2q1q3αz−βx2q2q3+2q1q4αx+1−2q22−2q42αy+2q3q4−2q1q2αz−βy2q2q4+2q1q3αx+2q3q4−2q1q2αy+1−2q22−2q32αz−βz.In (14), κ is the quadratic function of elements of quaternion qbd, and so is its modulus normκ. Therefore, if the first-order optimization algorithm, such as the gradient descent method, is used to search the optimal solution of qbd, 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 first-order one. In the following, the calibration process using the Newton method will be introduced in detail.The second-order Taylor series expansion ofFqbd around qbkd is(15)Φqbd=Fqbkd+∇Fqbkdgqbd−qbkd+12qbd−qbkdTg∇2Fqbkdgqbd−qbkd,where qbkd 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, ∇Fqbd and ∇2Fqbd are, respectively, denoted as gqbd and Hqbd; in addition, ∇Fqbkd and ∇2Fqbkd are denoted as gk and Hk.The optimal solution ofqbd is that one which makes the function value of Φqbd reach minimum, which is the stationary point of Φqbd; that is,(16)∇Φqbd=0.The expression of∇Φqbd can be obtained by calculating the gradient of (15):(17)∇Φqbd=gk+Hkgqbd−qbkd.Setting (17) equal to zero gains(18)gk+Hkgqbd−qbkd=0.If the initial valueqb0d is given, the iterative solution of quaternion qbkd satisfying (18) can be constructed as(19)qbk+1d=qbkd−Hk−1⋅gk,k=0,1,....The 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 Hk with the approximate matrix Bk 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]. The so-called Wolfe principle is to search the step λkthat meets the following conditions:(20)Fqbkd+λkdk≤Fqbkd+ρ˜λkdkTgk,dkTgqbkd+λkdk≥ρdkTgk,where ρ˜∈0,0.5; ρ∈ρ˜,1; and dk=−Hk−1⋅gk.Before constructing the approximate matrix B, we need to derive the Newton principle from (18):(21)yk=Hk⋅sk,where sk=qbk+1d−qbkd; yk=gk+1−gk. Hk can be replaced with Bk calculated as in the following iteration formulation:(22)Bk=Bk−1+ΔBk−1,k=1,2,...,where B0 can be taken as the unit matrix. Therefore, the key is constructing the correction matrix ΔBk−1 in each step. We can hold it as(23)ΔBk−1=ηuuT+μvvT.Substituting (23) into (22) and combining formula (21),(24)yk=Bk−1sk+ηuTsku+μvTskv.SupposingηuTsk=1, μvTsk=−1, u=yk, and v=Bksk gains(25)η=1ykTsk,μ=−1skTBksk.Based on (23)–(25), the correction matrix is obtained as follows:(26)ΔBk−1=ykykTykTsk−Bk−1skskTBk−1skTBk−1sk.Substitutingλk and Bk derived, respectively, from (20) and (22) into (19) gains the improved iterative formula as(27)qbk+1d=qbkd−λkBk−1⋅gk,k=0,1,....It must be noted that a thresholdε should be set as the criterion of stopping iteration. If the normgk is smaller than ε, qbk+1d is taken as the final calibration result. The quaternion qbk+1d is only one way to express the attitude misalignment. It is completely equivalent to other expressions, such as Euler angle and DCM. These 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 Table1.Table 1InitializationSetqb0d=1000T, B0=I, ε, and a coarse lbCalibration processStep 1Substitutelb into (8) to calculatekdStep 2Substitutekd and qbkd into (9) to construct the observation equation of CbdStep 3Establish the loss functionFqbd according to (12)Step 4Substituteqbkd into loss function Fqbd, and calculate its gradient value ∇FqbkdStep 5Determine the search directiondk=−Hk−1⋅gkStep 6Calculate the best stepλk and the approximate matrix Bk, respectively, with (20) and (22)Step 7Iterate the latest quaternion of attitude misalignmentqbk+1d with (27)Step 8k=k+1, and loop step 1–5 until gk<ε


## 3.1. Calibration of Scale Factorkd

Since the DCMCbd does not change the modulus of the vector, the modulus values are still equal after removing DCMs on both sides of (2); that is,(5)vd=kdCnbvn+ωibb×lb.The scale factorkd can be achieved as(6)kd=Cnbvn+ωibb×lbvd.In (6), all the noises of vn, vd, and ωibb will degrade the precision of kd calibration. In order to eliminate this negative influence, we will integrate (2) on the time period 0,T to gain(7)kd=∫0TCnbvn+ωibb×lbdt∫0Tvddt.In order to obtain the suitable form of (7) for computer processing, the discretization of (7) has been done as(8)kd=∑i=1nCnbivni+ωibbi×lb∑i=1nvdi,where i represents the value on the time iΔT with ΔT=T/n. On the right side of (8), the variables except lb can be calculated or measured directly. The effect of lb on the kd calibration emerges only when the vehicle performs angular movement; that is, ωibbi≠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 lb gets weak. Therefore, 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 kd derived from (8) is immune to the attitude misalignment Cbd; besides, it will get higher as the time nincreases.


## 3.2. Calibration of Attitude Misalignment

Calibration of attitude misalignment, in other words, is solvingCbd. In (4), kd has been solved and lb is also given a coarse value, while Cbd is the only remaining unknown matrix. Therefore, 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. The discrete form of (4) is given directly as(9)∑i=1nvdi=Cbd∑i=1nkdCnbivni+ωibbi×lb.Substitutingβ=∑i=1nkdiCnbivni+ωibbi×lb and α=∑i=1nvdi into (9) gives the observation equation of Cbd as(10)α=Cbdβ.SubstitutingCbd with the equivalent quaternion qbd and reorganizing the terms [18, 19],(11)κ=qbd∗⊗α⊗qbd−β,where qbd∗ is the conjugation of qbd. 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(12)Fqbd=normκ,where normg is the operator of calculating modulus. Our goal is to solve a quaternion qbd to minimize the value of loss function; that is,(13)minqbd∈R4×1Fqbd=minqbd∈R4×1normκ.Substitutingqbd=q1q2q3q4Tα=αxαyαzT and β=βxβyβzT into (11) achieves the expansion of κ as [20](14)κ=1−2q32−2q42αx+2q2q3−2q1q4αy+2q2q4−2q1q3αz−βx2q2q3+2q1q4αx+1−2q22−2q42αy+2q3q4−2q1q2αz−βy2q2q4+2q1q3αx+2q3q4−2q1q2αy+1−2q22−2q32αz−βz.In (14), κ is the quadratic function of elements of quaternion qbd, and so is its modulus normκ. Therefore, if the first-order optimization algorithm, such as the gradient descent method, is used to search the optimal solution of qbd, 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 first-order one. In the following, the calibration process using the Newton method will be introduced in detail.The second-order Taylor series expansion ofFqbd around qbkd is(15)Φqbd=Fqbkd+∇Fqbkdgqbd−qbkd+12qbd−qbkdTg∇2Fqbkdgqbd−qbkd,where qbkd 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, ∇Fqbd and ∇2Fqbd are, respectively, denoted as gqbd and Hqbd; in addition, ∇Fqbkd and ∇2Fqbkd are denoted as gk and Hk.The optimal solution ofqbd is that one which makes the function value of Φqbd reach minimum, which is the stationary point of Φqbd; that is,(16)∇Φqbd=0.The expression of∇Φqbd can be obtained by calculating the gradient of (15):(17)∇Φqbd=gk+Hkgqbd−qbkd.Setting (17) equal to zero gains(18)gk+Hkgqbd−qbkd=0.If the initial valueqb0d is given, the iterative solution of quaternion qbkd satisfying (18) can be constructed as(19)qbk+1d=qbkd−Hk−1⋅gk,k=0,1,....The 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 Hk with the approximate matrix Bk 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]. The so-called Wolfe principle is to search the step λkthat meets the following conditions:(20)Fqbkd+λkdk≤Fqbkd+ρ˜λkdkTgk,dkTgqbkd+λkdk≥ρdkTgk,where ρ˜∈0,0.5; ρ∈ρ˜,1; and dk=−Hk−1⋅gk.Before constructing the approximate matrix B, we need to derive the Newton principle from (18):(21)yk=Hk⋅sk,where sk=qbk+1d−qbkd; yk=gk+1−gk. Hk can be replaced with Bk calculated as in the following iteration formulation:(22)Bk=Bk−1+ΔBk−1,k=1,2,...,where B0 can be taken as the unit matrix. Therefore, the key is constructing the correction matrix ΔBk−1 in each step. We can hold it as(23)ΔBk−1=ηuuT+μvvT.Substituting (23) into (22) and combining formula (21),(24)yk=Bk−1sk+ηuTsku+μvTskv.SupposingηuTsk=1, μvTsk=−1, u=yk, and v=Bksk gains(25)η=1ykTsk,μ=−1skTBksk.Based on (23)–(25), the correction matrix is obtained as follows:(26)ΔBk−1=ykykTykTsk−Bk−1skskTBk−1skTBk−1sk.Substitutingλk and Bk derived, respectively, from (20) and (22) into (19) gains the improved iterative formula as(27)qbk+1d=qbkd−λkBk−1⋅gk,k=0,1,....It must be noted that a thresholdε should be set as the criterion of stopping iteration. If the normgk is smaller than ε, qbk+1d is taken as the final calibration result. The quaternion qbk+1d is only one way to express the attitude misalignment. It is completely equivalent to other expressions, such as Euler angle and DCM. These 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 Table1.Table 1InitializationSetqb0d=1000T, B0=I, ε, and a coarse lbCalibration processStep 1Substitutelb into (8) to calculatekdStep 2Substitutekd and qbkd into (9) to construct the observation equation of CbdStep 3Establish the loss functionFqbd according to (12)Step 4Substituteqbkd into loss function Fqbd, and calculate its gradient value ∇FqbkdStep 5Determine the search directiondk=−Hk−1⋅gkStep 6Calculate the best stepλk and the approximate matrix Bk, respectively, with (20) and (22)Step 7Iterate the latest quaternion of attitude misalignmentqbk+1d with (27)Step 8k=k+1, and loop step 1–5 until gk<ε


## 4. Simulation Test

The 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. The whole simulation test lasts for 600 s, during which the vehicle accelerates, keeps uniform velocity, and turns left and right as Table2 shows to simulate the real movements of the vehicle.Table 2
Vehicles dynamic state in simulation test.
Time period(s)ActionParameter0–15Acceleration1m/s215–50Uniform velocity15m/s50–70Turning right−4.5°/s70–150Uniform velocity15m/s150–210Turning left3°/s210–410Uniform velocity15m/s410–470Turning right−3°/s470–550Uniform velocity15m/s550–570Turning left4.5°/s570–600Uniform velocity15m/sIn the simulation test, the vehicle moves along the trajectory presented in Figure2. Reference velocity is provided by GPS, so the GPS velocity error is regarded as 0. The 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. The output frequencies of INS, DVL, and GPS are 200 Hz, 1 Hz, and 1 Hz, respectively. The noise of GPS velocity is 0.1m/s and the constant bias of the gyroscopes and accelerometers is 0.02°/s and 100ug, respectively. The initial parameters of simulation are set properly where the scale factor error of DVL is 0.005; attitude misalignment between frame d and frame b0.9°,−0.21°,1.2°; lever-arm vector from DVL to INS 05m0; starting point position 32.05°118.79°; initial velocity 0.Figure 2
Trajectory in simulation test.The EKF with the velocity error measurement, gradient descent optimization, and Newton optimization is used to calibrate DVL scale factor and attitude misalignment. In Figures3–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.Figure 3
Scale factor error in simulation test.Figure 4
Pitch angle error in simulation test.Figure 5
Roll angle error in simulation test.Figure 6
Yaw angle error in simulation test.Table 3
Means of calibration results.
EKFGradient descentNewtonScale factor9.55e − 04−3.15e − 04−3.15e − 04Pitch−0.0135−0.0076−0.006Roll1.56730.20960.2098Yaw−0.00890.00150.0016Table 4
Root mean square errors of calibration results.
EKFGradient descentNewtonScale factor0.00652.93e − 042.93e − 04Pitch0.00670.00146.52e − 04Roll0.42242.81e − 051.24e − 05Yaw0.00810.00177.90e − 04The 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 Figure3, 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.Figures4 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.The estimated roll angle error is shown in Figure5. Obviously, it is not calibrated correctly. This 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.


## 5. Field Test

The land vehicle field test is conducted to further evaluate the proposed algorithm to estimate the possibility of applying it to the underwater vehicles. The inertial measurement unit of INS is composed of three fiber optic gyroscopes with0.01°/h constant bias and three quartz accelerometers with 50ug constant bias. The reference velocity is provided by a reference navigation system whose sensors are PHINS developed by French firm IXBLUE and FlexPark6 GNSS receiver developed by NovAtel. The DVL data is simulated by reference system, where the velocity under frame n is transformed to frame d using the true attitude information. And then the scale factor error, attitude misalignment, and the lever-arm vector are artificially added to the simulated DVL date, respectively, as 0.005, [0.8°, −0.2°,0.6°], and [0 5 m 0]. The INS and PHINS are fixed on the same mounting plate shown in Figure 7.Figure 7
IMU and PHINS installation diagram.The 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 Figure8. 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.Figure 8
Trajectory in field test.Figure 9
Scale factor error in field test.Figure 10
Pitch angle error in field test.Figure 11
Roll angle error in field test.Figure 12
Yaw angle error in field test.Table 5
Root mean square errors of calibration results.
EKFGradient descentNewtonScale factor1.0e − 033.3e − 043.3e − 04Pitch0.090.010.01Roll0.897.2e − 045.6e − 04Yaw8.1e − 034.2e − 033.8e − 04The 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 Figure9 are identical. In Figures 10–12, the calibrated attitude misalignment errors are extremely approaching after 200 s. Thus, their mean value in Table 6 is almost equal. However, the Newton algorithm reaches this accuracy earlier than the gradient descent algorithm. This proves that the second-order 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.Table 6
Means of calibration results.
EKFGradient descentNewtonScale factor2.0e − 037.4e−047.4e − 04Pitch0.150.010.02Roll−4.350.220.03Yaw0.040.010.01


## 6. 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) The loss function established in optimization algorithms is a quadratic function of the attitude misalignment angles. Therefore, the higher efficiency of convergence can be gained by the second-order Newton optimization algorithm compared with the first-order gradient descent. (2) The introduction of the Wolfe principle enables us to search the optimal step adaptively making the calibration accuracy steadier. (3) The 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.



---

*Source: 3194034-2020-09-18.xml*