animprovedadaptivekalmanfilterforunderwatersins/ …jul 07, 2020 · researcharticle...
TRANSCRIPT
-
Research ArticleAn Improved Adaptive Kalman Filter for Underwater SINS/DVL System
Di Wang , Xiaosu Xu , and Lanhua Hou
Key Laboratory of Micro-Inertial Instrument and Advanced Navigation Technology, Ministry of Education,School of Instrument Science and Engineering, Southeast University, Nanjing, China
Correspondence should be addressed to Xiaosu Xu; [email protected]
Received 7 July 2020; Accepted 21 July 2020; Published 18 August 2020
Guest Editor: Gaoge Hu
Copyright © 2020 Di Wang et al. 'is is an open access article distributed under the Creative Commons Attribution License,which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.
'e main challenge of Strap-down Inertial Navigation System (SINS)/Doppler velocity log (DVL) navigation system is theexternal measurement noise. Although the Sage–Husa adaptive Kalman filter (SHAKF) has been introduced in the integratednavigation field, the precision and stability of the SHAKF are still the tricky problems to be overcome. 'e primary aim of thispaper is to improve the precision and stability of underwater SINS/DVL system. To attain this, a SINS/DVL tightly integratedmodel is established, where beam measurements are used without transforming them to 3D velocity. 'e proposed improvedSHAKF algorithm is based on variable sliding window estimation and fading filter. 'e simulations and vehicle test resultsdemonstrate the effectiveness of the proposed underwater SINS/DVL tightly integrated navigationmethod based on the improvedSHAKF. In addition, the position accuracy of the designed method outperforms that of the SHAKF method.
1. Introduction
As humans explore the ocean, the demand for resourcedevelopment, marine environmental surveys, and opera-tions has also increased. Underwater vehicles can replacehumans in deep ocean to complete the detection and de-velopment of marine resources, saving a lot of cost forproduction. Underwater vehicles have become one of theimportant tools for human ocean development [1]. With thegradual deepening of human beings in the field of oceanexploration, the tasks to be accomplished by underwatervehicles are becoming more and more complex and theworking time is getting longer and longer. Now, the de-velopment of underwater vehicles faces many problems,such as the realization of high-precision autonomous nav-igation and positioning functions. 'e autonomous navi-gation system can provide navigation information for theunderwater vehicle to ensure the correct travel.
At present, there are many kinds of navigation systemsused on underwater vehicles such as Strap-down InertialNavigation System (SINS), Long Baseline (LBL), Dopplervelocity log (DVL), and Ultrashort Baseline (USBL) [2]. 'e
premise of acoustic navigation system applications such asLong Baselines and Ultrashort Baselines is that the referencematrix is placed in the working sea area beforehand.However, the debugging and arranging process would wastemanpower and material resources. So they are not suitablefor a wide range of navigation and position. An autonomousnavigation system based on SINS requires navigation bymeans of inertial devices (gyroscopes and accelerometers) inthe navigation system [3]. However, due to inertial mea-surement unit (IMU) installation errors and self-propelledproblems, the navigation errors of the system will accu-mulate over time and will not be corrected in time.'erefore, the navigation accuracy will decrease after aperiod of time. DVL can provide relatively accurate speedinformation, which is self-contained, and the error does notaccumulate over time. 'erefore, SINS/DVL-based inte-grated navigation is a commonly used integrated navigationmethod for underwater vehicles [4].
In integrated navigation systems, Kalman filter is awidely used information fusion algorithm. 'e wide ap-plication of Kalman filter began in the 1960s. 'e essence ofthis filter is a recursive process, which is the optimal
HindawiMathematical Problems in EngineeringVolume 2020, Article ID 5456961, 14 pageshttps://doi.org/10.1155/2020/5456961
mailto:[email protected]://orcid.org/0000-0002-7732-1035https://orcid.org/0000-0002-5165-0981https://creativecommons.org/licenses/by/4.0/https://doi.org/10.1155/2020/5456961
-
minimum variance estimation of data. 'e filter can achievean optimal estimate by fusing information from differentsensors.'e traditional Kalman filter can be applied to linearsystems, but in fact many systems are nonlinear systems. So,extended Kalman filter (EKF) is proposed. 'is filter nolonger requires the system to be linear, which compensatesfor the deficiency of traditional Kalman filter to some extent.'en, the unscented Kalman filter (UKF) based on thenonlinear system is proposed [5, 6]. 'e algorithm realizesthe recursion of the state quantity of the system by setting thesampling point, and U transforms the sampling point toreplace the original value. In order to obtain better filterestimation, the above methods all require the system modelto be accurate and the external interference signal is un-correlated white noise with statistical characteristics. Oth-erwise, the filtering accuracy cannot be guaranteed, and eventhe filter divergence may be caused. However, in the actualSINS/DVL system, due to the error drift of the navigationdevice, the dynamic error caused by the vehicle manipu-lation, and the influence of the complex environment in thewater, the systemmodel is difficult to be completely accurateand the noise statistical characteristics are also uncertain. Inorder to further study the above issues, some researchershave introduced the Sage–Husa adaptive Kalman filter al-gorithm [7] to estimate the statistical characteristics of noisein real time.
Reference [8] pointed out the insufficiency of Sage–Husaadaptive filtering; that is, the measurement noise variancematrix R and the system noise variance matrix Q cannot beestimated online at the same time. 'erefore, scholars haveproposed a simplified Sage–Husa filter [9], which can es-timate R when the Q is known, and the method has goodadaptability. According to [10], a modified SHAKF is in-troduced to enhance the performance and increase thestability of the algorithm for the low cost EMES-INS/GPStightly integrated system. 'e proposed Sage–Husa adaptiveKalman algorithm in Reference [11] is based on the adaptiveweight that makes a better performance than KF for fiberoptic gyroscope (FOG) drift error signal denoising. In orderto solve the problem of reduced accuracy or the divergenceof the filter, an adaptive Kalman filter (AKF) algorithm basedon one-step smoothing filter is designed in [12] for inte-grated SINS/DVL systems. 'e variational Bayesian ap-proach is introduced in Kalman filter in [13–15]. Parameterestimation and inference problems in Bayesian models areoften very difficult. 'e adaptive Kalman model based onSage–Husa is relatively simple and meets the real-time re-quirements of integrated navigation.
To further improve the antijamming capability of thenavigation system, some methods are proposed. On the one
hand, a novel SINS/DVL tightly integrated model is built,which can work well when the number of DVL beamchannels is less than 3. On the other hand, an improvedSage–Husa adaptive Kalman filter (ISHAKF) is introduced,which introduces the forgetting factor and variable slidingwindow.
'e structure of this paper is as follows: a SINS/DVLtightly integrated model is designed in Section 2. In Section3, the ISHAKF method is presented, which is based on thefading factor and the variable sliding window method. 'esimulation and vehicle test are designed to illustrate thesuperiority of the method in Section 4. and 5 is devoted tothe conclusions.
2. SINS/DVL Tightly Integrated System
'e underwater SINS/DVL navigation system based on theloose combination takes the difference between the three-dimensional velocity of the DVL output and the three-di-mensional velocity of the SINS. When the number of DVLbeam channels is less than three, DVL will not be able tooutput 3D speed information, so the SINS/DVL integratednavigation system will not work well [16]. 'e tightly in-tegrated navigation method proposed in this paper makesfull use of DVL beam data to extend the measurement in-formation to four dimensions. When the DVL beam in-formation is less than three, the integrated navigation systemcan still work normally, further improving the fault toler-ance of the system. A novel structure of the SINS/DVLtightly integrated navigation system is shown in Figure 1,where it can be seen that the differences among the originalinformation of the four channels output by DVL and theSINS speed are used as the measurement information. At thesame time, the three-dimensional SINS velocity informationis extended to four dimensions through the transfer matrix.'e system can estimate the DVL scale factor error, the gyrozero bias, and the add-on zero bias by Kalman filtering andfinally feed back to the sensor output information to furtherimprove the navigation accuracy of the underwater system.
2.1. State Equation. 'e state equation of the SINS/DVLtightly system can be shown as follows:
_X � FX + GW, (1)
where F denotes the state transition matrix, G is the systemnoise matrix, W is the process noise vector, and X denotesstate vector, which can be obtained as follows:
X � ϕx ϕy ϕz δVnE δVnN δVnU δλ δL δh εx εy εz ∇x ∇y ∇z KD bPS T, (2)
where ϕ represents attitude error, δV represents velocityerror, δλ, δL, δh denote position error, ε represents gyro bias,∇ represents accelerometer bias, KD represents DVL scale
factor, and bPS represents pressure sensor bias. 'e systemstate transition matrix F, the matrix G, and W can beexpressed as follows [17]:
2 Mathematical Problems in Engineering
-
F �
Maa Mav Map −Cnb 03×3Mva Mvv Mvp 03×3 Cnb 03×203×3 Mpv Mpp Cnb 03×3
08×15
⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣
⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦
,
G �
−Cnb 03×303×3 Cnb011×1 011×1
⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣
⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦,
W �wbgwba
⎡⎢⎣ ⎤⎥⎦,
(3)
where Maa,Mav,Map,Mva,Mvv,Mpv,Mpp,Mvp can beknown in [17].
2.2. Measurement Equation. 'e measurement equation ofthe SINS/DVL tightly integrated system model is given as
Z � HX + V, (4)
where H is the measurement transfer matrix, V is themeasurement information noise, and Z is the measuredmatrix, which can be expressed as
Z �
Vd
DVL 1 −V
d
SINS 1
Vd
DVL 2 −V
d
SINS 2
Vd
DVL 3 −V
d
SINS 3
Vd
DVL 4 −V
d
SINS 4
HPS − HPS
⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣
⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦
, (5)
where VdSINS � Vd
SINS 1V
d
SINS 2V
d
SINS_3V
d
SINS 4 T
de-
notes the velocity of SINS under the d frame and VdDVL �
Vd
DVL 1V
d
DVL 2V
d
DVL 3V
d
DVL 4 Tdenotes the velocity of
DVL measurement. 'e noise V is
V � wD wPS T. (6)
We give definitions as follows: n represents the navi-gation frame, b represents body frame, and d represents theDVL frame. 'e relationship between SINS velocity andDVL velocity can be expressed as
VdDVL � VdSINS � C
dbC
bnV
nSINS, (7)
where Cbn is the direction cosine matrix and represents atransition from frame n to frame b and Cdb is the directioncosine matrix and represents a transition from frame b toframe d, which can be given as
Cdb �
0 cos α −sin α
cos α 0 −sin α
0 −cos α −sin α
−cos α 0 −sin α
⎡⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎣
⎤⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎦
, α � 70°, (8)
where α represents the horizontal angle between the beamsand the underwater vehicle. Commonly, there is α � 70°.Taking DVL beam 1 as an example, its conversion rela-tionship is shown in Figure 2.
Figure 2 is reproduced from [17]. (under the creativecommons attribution license/public domain). We define thePS error model as
HPS � HPS + δbPS + wPS, (9)
where HPS represents the true value,δbPS represents the PSbiases, and wPS represents the white noise. 'e DVL mea-surement error model can be expressed as [18]
VdDVL � 1 + KD( VdDVL + wD, (10)
where VdDVL represents the true value, KD represents thescale factor, and wD represents the white noise. Considering
+
+
DVL
IMU
Pressuresensor
Kalmanfilter
Navigationequation
Specificforce
Angularvelocity
VdDVL VdSINS
VnSINS VbSINS
HSINS
HPS
Cdb
Cbn
–
–
Figure 1: Structure of the proposed underwater SINS/DVL tightly integrated system.
Mathematical Problems in Engineering 3
-
the installation angles error, the conversion relationshipbetween frame b and frame d is
Cdb � Cdb I3×3 + φ×( , (11)
where ϕ � φx φy φz T
denotes the installation angleerror between IMU and DVL. According to the aboveanalysis, the velocity of SINS under frame d can be expressedas
VdSINS � Cd
bCbn V
n
SINS �Cdb C
b
n I3×3 + φ×( VnSINS + δV
nSINS(
≈ Cdb Cb
nVnSINS +
Cdb Cb
nδVnSINS +
Cdb Cb
nφ × VnSINS.
(12)
Substituting (7) into (12), we can get
VdSINS � VdDVL +
CdbCbnδV
nSINS −
CdbCbnV
nSINS × φ. (13)
'e measurement information is
VdDVL − Vd
SINS � 1 + KD( VdDVL + wD − V
dDVL −
CdbCbnδV
nSINS
+ CdbCbnV
nSINS × φ
� VdDVLKD + wD − Cd
bCbnδV
nSINS +
CdbCbnV
nSINS × φ
� VdDVLKD + wD − Cdb I3×3 + φ×( C
bnδV
nSINS
+ Cdb I3×3 + φ×( CbnV
nSINS × φ.
(14)
So, the measurement transfer matrixH can be expressedas
H �Mhb Mha 04×9 VdDVL 04×101×8 −1 01×7 1
⎡⎣ ⎤⎦,
·Mha � −Cdb I3×3 + φ×( C
bn
Mhb � Cdb I3×3 + φ×( CbnV
nSINS × .
⎧⎨
⎩
(15)
From (5), it can be seen that the DVL beam measure-ments are directly used without being transformed to frameb.
3. Improved Adaptive Filter Method
3.1. Sage–Husa Adaptive Kalman Filter. In 1969, scholarsA. P. Sage and G. W. Husa designed an adaptive filter al-gorithm, which can estimate the noise characteristics of thesystem in real time through the measurement output whileperforming state estimation [19]. Sage–Husa adaptive al-gorithm is based on the Kalman filter to add time-varyingnoise estimator to estimate and correct the statisticalcharacteristics of system noise in real time, thereby reducingmodel error, suppressing filter divergence, and enhancingfilter accuracy. However, it is impossible to estimate all noiseparameters such as system noise mean and variance andmeasurement noise mean and variance. Considering thatsystem noise generally has less impact, the effects of systemnoise can be ignored. 'e currently used Sage–Husa filter isan adaptive filter algorithm, which is based on the estimatedmeasurement variance matrix R. 'e system model can beestablished as
Xk � Φk,k−1Xk−1 + Γk−1Wk−1,
Zk � HkXk + Vk, (16)
where Xk denotes state vector, Φk,k−1 denotes one-steptransfer matrix from k–1 to k, Γk−1 denotes the system noisematrix, Zk denotes measurement vector,Hk is measurementmatrix, Wk−1 denotes system noise, and Vk is measurementnoise. 'e noise matrices Wk−1 and Vk satisfy the followingstatistical characteristics:
E Wk � qk, Cov Wk,Wj � Qkδkj,
E Vk � rk, Cov Vk,Vj � Rkδkj,
Cov Wk,Vj � 0,
⎧⎪⎪⎪⎪⎨
⎪⎪⎪⎪⎩
(17)
where Qk denotes covariance of the system noise and Rkdenotes covariance of the measurement noise. 'e standardKalman process is as follows [20,21]:
Xk,k−1 � Φk−1 Xk−1 + Γk−1qk−1, (18)
Pk,k−1 � Φk−1Pk−1ΦTk−1 + Γk−1Qk−1Γ
Tk−1, (19)
Kk � Pk,k−1ΗTk ΗkPk,k− 1Η
Tk + Rk
− 1, (20)
Xk � Xk,k−1 + Kk Zk − Hk Xk,k−1 − rk , (21)
Pk � I − KkHk( Pk,k−1. (22)
Vbz
Vby
Vbx
VbDVL_1Beam 1
α
α
Figure 2: 'e relationship between frame b and frame d.
4 Mathematical Problems in Engineering
-
'e simplified SHAKF is based on (18)–(22) to increasethe measurement noise estimator.
εk� Zk − Hk Xk,k−1 − rk, (23)
Rk � 1 −1k
Rk−1 +1k
εkεTk − HkPk,k−1H
Tk . (24)
'e SHAKF algorithm can measure the noise R in realtime and achieve the adaptive effect while estimating thestate.
3.2. Improved SHAKFMethod. In view of the deficiencies ofSHAKF algorithm, this paper makes improvements fromtwo aspects. Firstly, the variable fading factor is designed toimprove the adaptive ability of the filtering. Secondly, thevariable sliding window method is used to measure theestimation of noise, and the estimation accuracy of themeasurement noise is further improved.
(1) Based on the SHAKF algorithm, the fading factor λ isintroduced. 'erefore, considering the idea of combining afading factor, (21) can be given as
Xk � Xk,k−1 + λ · Kk Zk − Hk Xk,k−1 − rk . (25)
'is paper proposes an adaptive adjustment function forfading factors. It can be known from formula (25) that whenthe error is large, λ should be reduced, thereby reducing theweight of the current measurement information. When theerror is small, λ should increase, thereby increasing theweight of the current measurement information. Because thesize of the fading factor can be determined by the error [22],we use the error mean E[εk] as the error point. When it isless than the error mean E[εk], λ is adjusted closer to 1.When it is larger than the error mean E[εk], λ is adjustedcloser to λmin. According to the above analysis, the proposedadaptive adjustment function is as follows:
λ �1 − λmin
πarccot εk − E εk ( + λmin. (26)
From (26), we know that the fading factor λ is controlledby the measurement error, which can ensure that the errorswings around the error mean E[εk], which can improve thestability of the SHAKF. Normally, the value of λmin rangesfrom 0.7 to 0.8, and the value of λ ranges from λmin to 1. Itcan be seen from (26) that the forgetting factor λ is con-trolled by the error, which can ensure that the error swingsaround E[e(n)], and the change is slow, which can buffer thepoor tracking effect caused by the λ change too fast.
It can be known from (23) that if the measurement noiseof the actual system is smaller than the theoretical modelvalue, εkεTk will be smaller. If the state noise setting is toolarge,HkPk,k−1HTk will be larger. Both of the above cases maycause εkεTk − HkPk,k−1H
Tk < 0, which makes it easy for Rk to
lose positive definiteness and cause filtering abnormality. Tosolve this problem, this paper adopts the sequential filteringmethod and limits the size of each element of the Rk diagonal[23–25]. Assume Rk is a diagonal matrix. Sequential filtering
is used to perform the ith scalar sequential measurementupdate. 'e scalar measurement equation is
Z(i)k � H
(i)k Xk + V
(i)k ,
ρ(i)k � Z(i)
k,k− 1 2
− H(i)k P(i)k,k−1 H
(i)k
T.
(27)
Set the Rk value range to meet the following conditions:
R(i)
k �
1 − βk( R(i)
k−1 + βkR(i)min, ρ
(i)k R
(i)max,
1 − βk( R(i)
k−1 + βkR(i)k , others,
⎧⎪⎪⎪⎪⎨
⎪⎪⎪⎪⎩
(28)
where i denotes the ith scalar. R(i)k represents the ith scalarelement of the diagonal matrix Rk at time k.'rough the aboveprocessing, the measurement noise Rk can be limited betweenR
(i)min and R(i)max, thereby having better adaptive ability and
reliability.(2) In order to enhance the estimation accuracy of
measurement noise, a variable sliding window estimationmethod is designed in this paper. 'rough the measurementnoise mean processing in the SHAKF, the estimation ac-curacy of the measurement noise is further improved. 'efollowing improvement is made to (24) in the filteringalgorithm:
Rk � Rk−1 +1m
m
j�0εk−jε
Tk−j − Hk−jPk−j,k−j−1H
Tk−j , (29)
where m is the variable sliding window value. 'e range ofvalues of m satisfies the following conditions:
m �m, mN, (30)
where N is the preset window maximum. When a largechange in the R value is detected, m� 0, and the slidingwindow is incremented from 0 to N. When the value ofm isgreater than N, the window m�N. 'is method is welladapted to the variation of the R value. 'e two processes ofwindow adjustment are shown in Figures 3 and 4. Figure 3 isa window adjustment process when mN. It can be seen from the figure that as the observationdata increases, the moving window size remains N.
3.3. ISHAKF for SINS/DVL Tight System Scheme.Figure 5 is the structure of the improved Sage–Husa adaptiveKalman filter algorithm.'e algorithm is based on the SINS/DVL tight combination navigation system designed in thispaper, which can realize the noise estimation of four-di-mensional velocity information. 'e ISHAKF algorithm isbased on the SHAKF algorithm. Based on the original al-gorithm, the forgetting factor and variable sliding windowmethod are introduced. 'e steps of implementing theISHAKF algorithm are as follows.
Mathematical Problems in Engineering 5
-
(1) Perform ISHAKF algorithm status update.(2) Calculate the forgetting factor according to the in-
troduced forgetting factor adjustment function.(3) Calculate the filter gain according to the forgetting
factor, and perform the ISHAKF filter measurementupdate at the same time.
(4) Calculate the sliding window value, and perform thesliding window averaging processing on the mea-surement noise.
(5) Determine if the R value is outside the set range andfurther process the R value.
4. Simulation and Vehicle Test
To illustrate the navigation performance of the proposedISHAKF method, the simulation and vehicle experiment aredesigned. First, the trajectory is simulated, which simulatesthe underwater vehicle swinging motion. Second, the out-performance of the ISHAKF method is verified comparedwith Sage–Husa adaptive Kalman filter and Kalman filter incomplex sea conditions. Finally, the vehicle test is designed,which simulates underwater SINS/DVL tightly integratednavigation system.
4.1. Simulation. 'e accelerometer biases and the randomwalk noise are set as 100μ g and100μ g/
���Hz
√, respectively.
'e gyroscope biases and the random walk noise are set as0.02°/h and 0.02°/
��h
√, respectively. 'e DVL scale factor is
set as 0.002. 'e initial angle errors are set as 0.02°, −0.02°,and 0.5°, respectively. 'e DVL provides velocity
information for four channels. 'e out frequencies of theIMU and DVL device are set as 200Hz and 1Hz, respec-tively. 'e vehicle movement start position is set to latitude32.056° N and longitude 118.794° E. 'e curve of movingtrajectory is shown in Figure 6. 'e altitude value of movingtrajectory for underwater vehicle is 50m, and the altitudevalue does not change during the whole movement.
Since the underwater vehicle movement is affected by thewater flow, its motion should be a rocking motion. In orderto simulate the swaying motion of the underwater vehicle,the attitude angle of the vehicle is given as
θ � θm sin2π5
t + θ0 ,
c � cm sin2π7
t + c0 ,
ψ � ψm sin2π6
t + ψ0 ,
⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨
⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩
(31)
where the amplitudes of the swaying motion are set asθm � 3°, cm � 4°, and ψm � 3°, respectively, while the cyclesof the swaying are 5 s, 7 s, and 6 s, respectively. 'e initialphases are set as θ0 � 0°, c0 � 0°, and ψ0 � 0°, respectively.'e curve of vehicle velocity and attitude is shown inFigure 7.
In order to illustrate the anti-interference ability of theproposed ISHAKF method to external noise, different noiselevels are set. 'e measurement noise covariance is set as R �
N + 1
N + 1
N + 2
N + 2
N + 2
N + 3
N + 3
N + 3
N + 3
N + 4
N + 4
N + 4
N + 4
N + 4
N + 5
N + 5
N + 5
N + 5
N + 5
N + 6
N + 6
N + 6
N + 6
N + 7
N + 7
N + 7
N + 8
N + 8
N + 9
m = N + 5
m = N + 6
m = N + 7
m = N + 8
Figure 4: Window sliding adjustment process when m>N.
1 2 3 4 5 6 7 8 N
1
1 2
1 2 3
1 2 3 4
1 2 3 4 5
1 2 3 4 5 6
1 2 3 4 5 6 7 N
m = 1
m = 2
m = 3
m = 4
m = 5
m = 6
Figure 3: Window sliding adjustment process when m
-
32.06
32.04
32.02
32
31.98
31.96
Latit
ude (
°)
118.77 118.775 118.78 118.785 118.79 118.795 118.8 118.805
Longitude (°)
Start
End
Figure 6: Curve of moving trajectory.
++
+IMU
DVL
PS
ISHAKFSINS
–
VdDVL~
VdSINS~
HPS~
m = Nm
Rk = Rk–1 + –1
mm
j=1(εk–j εTk–j – Hk–j Pk–j,k–j–1HTk–j)ˆ ˆ
Xk = Xk,k–1 + λ · Kk (Zk – Hk Xk,k–1 – rk)ˆ ˆ ˆ
ˆεk = Zk – Hk Xk,k–1 – rk
ˆXk,k–1 = Фk–1Xk–1 + Γk–1qk–1
Pk,k–1 = Фk–1Pk–1ФTk–1 + Γk–1Qk–1ΓTk–1
Kk = Pk,k–1HTk (HkPk,k–1HTk + Rk)–1
Pk = (I – KkHk)Pk,k–1
λ = (1 – λmin)
π arccot(εk – E[εk]) + λmin
~
othersRk
(i) =
10.90.80.7
0.2 0.4 0.6 0.8 10
λ λmin
E[e(n)]
(1 – βk) R(i) ~
~
k–1+ βkRk(i)
R(i)max ρk(i)
> R(i)maxminρk
(i) < R(i)(1 – βk) R(i)k–1 min+ βk R
(i)
–
Figure 5: Process of the improved Sage–Husa adaptive filter for SINS/DVL.
5
0
–5
–10
Velo
city
(m/s
)
0 200 400 600 800 1000 1200 1400
Time (s)
VEVNVU
(a)
5
0
–5
–100
–150
–200
θ, γ
(°)
0 200 400 600 800 1000 1200 1400
0 200 400 600 800 1000 1200 1400
Time (s)
ψ (°
)
θγ
(b)
Figure 7: Curve of velocity and attitude.
Mathematical Problems in Engineering 7
-
diag (0.1m/s)2, (0.1m/s)2, (0.1m/s)2, (0.1m/s)2 during400 s∼700 s. 'ree methods are listed for SINS/DVL tightlyintegrated system. Figure 8 shows the curve of velocity errorfor KF, SHAKF, and ISHAKF.
'e navigation accuracy and anti-interference ability ofthree methods (KF, SHAKF, and ISHAKF) are compared bycalculating the root-mean-square error (RMSE) of the velocity:
RMSE �
�������������
1n
n
i�1
Xi − Xi( 2
, (32)
where Xi denotes the real velocity obtained by the simu-lation, n is the number of signals, and Xi is the measurementinformation. 'e RMSE has a good reflection of the mea-surement precision. So, the error data in Figure 8 is pro-cessed by the RMSE, and the results are shown in Table 1.
From Table 1 we know that the RMSE of north, east, andup velocity for ISHAKF are 0.138m/s, 0.135m/s, and0.056m/s, respectively, which has better performance thanKF and SHAKF method. Figure 9 shows the curve of po-sition error for KF, SHAKF, and ISHAKF. Compared withKF and SHAKF, it can be known that ISHAKF proposed inthis paper has the smallest position error.
According to navigation system, the positioningperformance of the three methods is compared, the aboveposition error is analyzed, and the max error is calcu-lated, whose results are listed in Table 2. We can see thatthe north and east position errors for ISHAKF are14.303 m and 14.559 m, respectively, which reduced by44.59% and 45.96% than SHAKF method.
In order to further compare the positioning accuracy ofthe three methods, the horizontal position error is calculatedfor the KF, SHAKF, and ISHAKF as shown in Figure 10.'emax horizontal position errors for three methods are126.30m, 32.27m, and 20.39m, respectively, which showsthat the ISHAKFmethod has the better accuracy than the KFand SHAKF method.
4.2. Vehicle Test. 'e vehicle experiment is designed tosimulate underwater vehicle due to the limitations of theexperimental conditions. 'e vehicle experiment device
0.20.1
0
0
0.30.20.1
0.01
0
–0.01
0 200 400 600 800 1000 1200 1400
0 200 400 600 800 1000 1200 1400
0 200 400 600 800 1000 1200 1400
Time (s)
∆VN
(m/s
)∆V
E (m
/s)
∆VU
(m/s
)
KFSHAKFISHAKF
Figure 8: Curve of velocity error for three methods.
Table 1: RMSE for three methods.
KF SHAKF ISHAKFΔVN (m/s) 0.246 0.139 0.138ΔVE (m/s) 0.271 0.145 0.135ΔVU (m/s) 0.061 0.058 0.056
100
50
0
0
4020
0
–0.2
0.2
0 200 400 600 800 1000 1200 1400
0 200 400 600 800 1000 1200 1400
0 200 400 600 800 1000 1200 1400
Time (s)
∆PN
(m/s
)∆P
E (m
/s)
∆PU
(m/s
)KFSHAKFISHAKF
Figure 9: Curve of position error for the three methods.
Table 2: Max error for three methods.
KF SHAKF ISHAKFΔPN (m) 113.137 25.816 14.303ΔPE (m) 57.966 26.946 14.559ΔPU (m) 0.604 1.014 1.014
0
60
40
20
80
140
120
100
0 200 400 600 800 1000 1200 1400
Time (s)
Hor
izon
tal p
ositi
on er
ror (
m)
KFSHAKFISHAKF
Figure 10: 'e horizontal position error for the three methods.
8 Mathematical Problems in Engineering
-
includes a PHINS of IXSEA Corporation, a navigationcomputer, a Flex Park 6 GPS receiver, and an inertialmeasurement unit. Meanwhile, PHINS is used as a referencenavigation system to provide the accurate navigation in-formation in real time. 'e vehicle equipment is shown inFigure 11. In vehicle experiment, the PHINS and GPS
receiver are integrated, which works on the PHINS/GPSmode. So, PHINS provides relatively accurate navigationinformation including velocity position and attitude. 'espeed information output by the PHINS can be used tosimulate the speed information provided by the DVL device.'e instrument specifications are listed in Table 3.
(a) (b)
Figure 11: (a) Experimental vehicle and (b) equipment.
Table 3: Instrument specifications.
Instruments Parameters Accuracy Frequency (Hz)
IMU
Gyroscope bias ≤0.02°/h 200Gyroscope random walk ≤0.005°/
��h
√200
Accelerometer bias variation ± 50ug 200Accelerometer output noise ≤ 50 ug/
���Hz
√200
GPS Velocity (RMS) 0.03m/s 1Position (L1) 1.5m 1PHINS Attitude (GPS aided mode) ≤ 0.01° 200
(a)
31.894
31.892
31.89
31.888
31.886
31.884
Latit
ude (
°)
118.81 118.814 118.818 118.822
Longitude (°)
End
Start
(b)
Figure 12: Curve of moving trajectory.
Mathematical Problems in Engineering 9
-
'e entire test route is on the campus of Southeast Uni-versity and lasts for 1000 s. Figure 12 shows the motion tra-jectory.'e curve of attitude and velocity is shown in Figure 13.
In order to verify the fault-tolerant performance of thedesigned SINS/DVL tightly integrated navigation method, theloosely integrated navigation method is introduced in thissection. 'e DVL scale factor is 0.002. 'e initial attitude erroris[0.1°, 0.1°, 0.5°]. TwoDVL beammissing regions are set: DVLbeam channel 2 signalmissing between 150 s and 200 s andDVLbeam channel 4 signal missing between 600 s and 650 s.
Figure 14(a) shows the curve of velocity error, whosemean and standard deviation (STD) for two methods areshown in Figure 14(b). 'e position error for two methods isshown in Figure 15, where it can be known that the tightlyintegrated method proposed in this paper has better accuracythan the loosely integrated method. When the DVL beamchannel information is less than 4, the loose combinationcannot complete the combined navigation solution, so itsspeed error will continue to diverge. However, the tightcombination method can still complete the combined
0 200 400 600 800 1000
200
0
–200
Attit
ude (
°)
θγψ
Time (s)
10
0
–100 200 400 600 800 1000
Velo
city
(m/s
)
Time (s)
VEVNVU
Figure 13: Curve of attitude and velocity.
0.51
0–0.5
10.5
0
0.2
0
–0.20 200 400 600 800 1000
0 200 400 600 800 1000
0 200 400 600 800 1000
Time (s)
∆VN
(m/s
)∆V
E (m
/s)
∆VU
(m/s
)
Lossely integratedTightly integrated
(a)
0.2
0.1
0
0.3
–0.2
–0.1
∆VN ∆VE ∆VU
Mea
n an
d ST
D o
f the
velo
city
erro
rs (m
/s)
Lossely integratedTightly integrated
(b)
Figure 14: Curve of velocity error (a) and mean and STD of the velocity errors (b) for two methods.
10 Mathematical Problems in Engineering
-
navigation solution in the case of DVL beam loss and suppressthe navigation error divergence. It can be know from theresults that the tight combination method has better faulttolerance. 'e curve of the horizontal position error is shownin Figure 16, where it can be known that the max horizontalposition error for tightly integrated method is 54.51m, whichis reduced by 63.9% compared to loosely integrated method.
'e curve of velocity error for two methods is shown inFigure 17(a), in which the SHAKF is marked by blue line andthe ISHAKF is marked by red line. Mean and standarddeviation (STD) of velocity errors for two methods areshown in Figure 17(b), which displays that the mean andSTD of ISHAKF are better than SHAKF method.
'e RMSE of velocity for SHAKF and ISHAKF methodis listed in Table 4. It can be seen that the north and east
velocity errors for ISHAKF are 0.171m/s and 0.204m/s,respectively, which reduced by 12.7% and 7.2% compared toSHAKF method.
Figure 18 displays the curves of the north, east, and upposition errors for SHAKF and ISHAKF. Compared withSHAKF, we know that the ISHAKF method has smallerposition error. To further compare the positional accuracyof the two methods, the horizontal position error for theSHAKF and ISHAKF is displayed in Figure 19, whichshows that the position accuracy based on ISHAKF isbetter than the SHAKF. 'e max horizontal positionerrors for SHAKF and ISHAKF are 49.76m and 31.49m.Compared with the SHAKF method, the accuracy of theISHAKF method is improved by 36.71% which is listed inTable 5.
10050
0
50
0
–50
10
–1–2–3
0 200 400 600 800 1000
0 200 400 600 800 1000
0 200 400 600 800 1000
Time (s)
∆PN
(m)
∆PE
(m)
∆PU
(m)
Lossely integratedTightly integrated
Figure 15: Curve of position error for two methods.
100
80
60
120
140
160
20
0
40
0 200 400 600 800 1000
Time (s)
Hor
izon
tal p
ositi
on er
ror (
m)
Lossely integratedTightly integrated
Figure 16: Curve of horizontal position error.
Mathematical Problems in Engineering 11
-
0.10
–0.1
–0.2–0.1
0
0.2
0
–0.20 200 400 600 800 1000
0 200 400 600 800 1000
0 200 400 600 800 1000
Time (s)
∆VN
(m/s
)∆V
E (m
/s)
∆VU
(m/s
)
SHAKFISHAKF
(a)
0
–0.02
–0.04
0.02
0.04
0.06
–0.08
–0.1
–0.06
∆VN ∆VE ∆VU
Mea
n an
d ST
D o
f the
velo
city
erro
rs (m
/s)
SHAKFISHAKF
(b)
Figure 17: Curve of velocity error (a) and mean and STD of the velocity errors (b) for two methods.
Table 4: RMSE for two methods.
SHAKF ISHAKFΔVN (m/s) 0.196 0.171ΔVE (m/s) 0.220 0.204ΔVU (m/s) 0.142 0.130
0
–20
–40
–10
10
0
0
–5
5
0 200 400 600 800 1000
0 200 400 600 800 1000
0 200 400 600 800 1000
Time (s)
∆PN
(m/s
)∆P
E (m
/s)
∆PU
(m/s
)
SHAKFISHAKF
Figure 18: Curve of position error for two methods.
50
40
30
10
0
20
0 200 400 600 800 1000
Time (s)
Hor
izon
tal p
ositi
on er
ror (
m)
SHAKFISHAKF
Figure 19: Curve of horizontal position error for two methods.
12 Mathematical Problems in Engineering
-
5. Conclusions
In order to improve the fault tolerance of SINS/DVL in-tegrated navigation in complex environment, this paperproposes an improved Sage–Husa adaptive filtering SINS/DVL tight combination navigation method, which is im-proved from two aspects. Firstly, a new compact combinednavigation model is derived based on the traditional SINS/DVL loose combination and the state equation and mea-surement equation are established. Secondly, based on theSage–Husa adaptive Kalman filter algorithm, the forgettingfactor and variable sliding window are introduced, whichfurther improves the robustness and adaptive ability of theadaptive filter. Finally, simulation and in-vehicle testsverify that the proposed method can further enhance therobustness and navigation accuracy of the SINS/DVLsystem.
Data Availability
'e raw/processed data required to reproduce these findingscannot be shared at this time as the data also form part of anongoing study.
Conflicts of Interest
'e authors declare that they have no conflicts of interest.
Acknowledgments
'is work was supported in part by the National NaturalScience Foundation of China under Grants 51775110,51375088, 51979041, and 61473085; in part by the NaturalScience Foundation of Jiangsu Province, China under GrantBK20190344; in part by the Postgraduate Research &Practice Innovation Program of Jiangsu Province, underGrant KYCX20_0109; in part by the Inertial Technology KeyLab Fund under Grant 614250607011709; in part by theFundamental Research Funds for the Central Universitiesunder Grants 2242019K40041 and 2242020k1G009; by KeyLaboratory Fund for Underwater Information and Controlunder Grant 614221805051809; and in part by the JiangsuKey Laboratory Fund for Green Ship Technology underGrant 2019Z01. Remaining funds were provided by culti-vation project of National Natural Science Foundation ofSoutheast University under Grant 9S20172204.
References
[1] R. Cui, X. Zhang, and D. Cui, “Adaptive sliding-mode attitudecontrol for autonomous underwater vehicles with inputnonlinearities,” Ocean Engineering, vol. 123, pp. 45–54, 2016.
[2] J. J. Leonard and A. Bahr, “Autonomous underwater vehiclenavigation,” in Handbook of Ocean Engineering, Springer,Berlin, Germany, 2016.
[3] Q. Zhang, X. Meng, S. Zhang, and Y. Wang, “Singular valuedecomposition-based robust cubature kalman filtering for anintegrated GPS/SINS navigation system,” Journal of Navi-gation, vol. 68, no. 3, pp. 549–562, 2015.
[4] L. Luo, Y. Zhang, T. Fang, and N. Li, “A new robust kalmanfilter for SINS/DVL integrated navigation system,” IEEEAccess, vol. 7, pp. 51386–51395, 2019.
[5] G. Hu, S. Gao, and Y. Zhong, “A derivative UKF for tightlycoupled INS/GPS integrated navigation,” ISA Transactions,vol. 56, pp. 135–144, 2015.
[6] C. Yang, W. Shi, and W. Chen, “Comparison of unscentedand extended kalman filters with application in vehiclenavigation,” Journal of Navigation, vol. 70, no. 2, pp. 411–431,2017.
[7] N. I. U. Zhen-Zhong, L. I. Sui-Lao, W. Qing-Qing et al.,“Improved sage-husa filter for precision airdrop integratednavigation system,” Science Technology and Engineering,vol. 25, 2012.
[8] A. P. Sage and G. W. Husa, “Algorithms for sequentialadaptive estimation of prior statistics,”in Proceedings ofthe1969 IEEE symposium on adaptive processes (8th) deci-sion andcontrol, IEEE, University Park, PA, USA, November1969.
[9] P. Lu, L. Zhao, and Z. Chen, “Improved Sage-Husa adaptivefiltering and its application,” Journal of System Simulation,vol. 15, pp. 3503–3505, 2007.
[10] K. Badshah and Q. Yongyuan, “Tightly coupled integration ofa low cost MEMS-INS/GPS system using adaptive kalmanfiltering,” International Journal of Control and Automation,vol. 9, no. 2, pp. 179–190, 2016.
[11] J. Sun, X. Xu, Y. Liu, T. Zhang, and Y. Li, “FOG random driftsignal denoising based on the improved AR model andmodified sage-husa adaptive kalman filter,” Sensors, vol. 16,no. 7, p. 1073, 2016.
[12] W. Gao, J. Li, G. Zhou, and Q. Li, “Adaptive kalman filteringwith recursive noise estimator for integrated SINS/DVLsystems,” Journal of Navigation, vol. 68, no. 1, pp. 142–161,2015.
[13] Y. Huang, Y. Zhang, Y. Zhao, P. Shi, and J. Chambers, “Anovel outlier-robust kalman filtering framework based onstatistical similarity measure,” IEEE Transactions on Auto-matic Control, no. 99, p. 1, 2020.
[14] Y. Huang, Y. Zhang, B. Xu et al., “A new adaptive extendedkalman filter for cooperative localization,” IEEE Transactionson Aerospace & Electronic Systems, vol. 54, no. 1, pp. 353–368,2017.
[15] Y. Huang, Y. Zhang, N. Li, Z. Wu, J. A. Chambers et al., “Anovel robust student’s t-based kalman filter,” IEEE Transac-tions on Aerospace Electronic Systems, vol. 53, no. 3,pp. 1545–1554, 2017.
[16] Y. Zhu, X. Cheng, J. Hu, L. Zhou, and J. Fu, “A novel hybridapproach to deal with DVL malfunctions for Underwaterintegrated navigation systems,” Applied Sciences, vol. 7, no. 8,p. 759, 2017.
[17] D.Wang, X. Xu, Y. Yao, T. Zhang, and Y. Zhu, “A novel SINS/DVL tightly integrated navigation method for complex en-vironment,” IEEE Transactions on Instrumentation andMeasurement, vol. 69, no. 7, pp. 5183–5196, 2020.
[18] D. Wang, X. Xu, Y. Yao, Y. Zhu, and J. Tong, “A hybridapproach based on improved AR model and MAA for INS/
Table 5: Maximum absolute error (MAE) for two methods.
SHAKF ISHAKFΔPN (m) 49.68 30.85ΔPE (m) 8.38 8.35ΔPU (m) 4.37 3.05
Mathematical Problems in Engineering 13
-
DVL integrated navigation systems,” IEEE Access, vol. 7,pp. 82794–82808, 2019.
[19] G. Hu, W. Wang, Y. Zhong, B. Gao, and C. Gu, “A new directfiltering approach to INS/GNSS integration,” Aerospace Sci-ence and Technology, vol. 77, pp. 755–764, 2018.
[20] C.-H. Tseng, S.-F. Lin, and D.-J. Jwo, “Robust huber-basedcubature kalman filter for GPS navigation processing,”Journal of Navigation, vol. 70, no. 3, pp. 527–546, 2017.
[21] Y.-L. Hsu, J.-S. Wang, and C.-W. Chang, “A wearable inertialpedestrian navigation systemwith quaternion-based extendedkalman filter for pedestrian localization,” IEEE SensorsJournal, vol. 17, no. 10, pp. 3193–3206, 2017.
[22] S. Song, J. S. Lim, S. J. Baek et al., “Variable forgetting factorlinear least squares algorithm for frequency selective fadingchannel estimation,” IEEE Transactions on Vehicular Tech-nology, vol. 51, no. 3, pp. 613–616, 2002.
[23] G. Hu, B. Gao, Y. Zhong, L. Ni, and C. Gu, “Robust unscentedkalman filtering with measurement error detection for tightlycoupled INS/GNSS integration in hypersonic vehicle navi-gation,” IEEE Access, vol. 7, pp. 151409–151421, 2019.
[24] G. Zhai, C. Wu, and Y. Wang, “Millimeter wave radar targettracking based on adaptive kalman filter,” in Proceeeings of the2018 IEEE intelligent vehicles symposium (IV), pp. 453–458,IEEE, Changshu, China, June 2018.
[25] G. Hu, L. Ni, B. Gao, X. Zhu,W.Wang, and Y. Zhong, “Modelpredictive based unscented kalman filter for hypersonic ve-hicle navigation with INS/GNSS integration,” IEEE Access,vol. 8, pp. 4814–4823, 2020.
14 Mathematical Problems in Engineering