Professional Documents
Culture Documents
BioOne (www.bioone.org) is a nonprofit, online aggregation of core research in the biological, ecological, and
environmental sciences. BioOne provides a sustainable online platform for over 170 journals and books published
by nonprofit societies, associations, museums, institutions, and presses.
Your use of this PDF, the BioOne Web site, and all posted and associated content indicates your acceptance of
BioOne’s Terms of Use, available at www.bioone.org/page/terms_of_use.
Usage of BioOne content is strictly limited to personal, educational, and non-commercial use. Commercial inquiries
or rights and permissions requests should be directed to the individual publisher as copyright holder.
BioOne sees sustainable scholarly publishing as an inherently collaborative enterprise connecting authors, nonprofit publishers, academic institutions, research
libraries, and research funders in the common goal of maximizing access to critical research.
Journal of Coastal Research SI 73 698-704 Coconut Creek, Florida Winter 2015
ABSTRACT
Huang, H.; Chen, X.; Zhou, Z., and Lv, C., 2015. Attitude determination for underwater gliders using unscented
Kalman filter based on smooth variable algorithm. In: Mi, W.; Lee, L.H.; Hirasawa, K., and Li, W. (eds.), Recent
Developments on Port and Ocean Engineering. Journal of Coastal Research, Special Issue, No. 73, pp. 698-704.
Coconut Creek (Florida), ISSN 0749-0208.
www.JCRonline.org For improving the attitude accuracy using the lower cost and lower power underwater navigation system, this paper
proposes the new underwater navigation system which is composed of the inertial sensors aided with the magnetometer,
and the unscented Kalman filter based on smooth variable algorithm (UKF-SV) is proposed to improve the attitude
accuracy for the underwater glider. The UKF-SV makes use of the advantage of UKF (unscented Kalman filter) in the
nonlinear model to estimate attitudes and then smoothes the variables estimated by UKF. Through this process, the
attitude accuracy can be greatly improved. The convergence of UKF-SV is proven in theory. The static and dynamic
experiments are done to assess the performance of UKF-SV and compare with the traditional UKF. The experiment
results show that the performance of UKF-SV is better than the traditional UKF.
ADDITIONAL INDEX WORDS: Underwater glider navigation, inertial navigation system (INS), unscented
Kalman filter (UKF), smooth variable.
_________________________________________________________________________________________
INTRODUCTION known and linear, the system and measurement noises are white,
The underwater glider is a special kind of autonomous and the states have initial conditions that are modeled as random
underwater vehicle that glides for a long distance and a long variables with known means and variances (Xu, Chen, and Li,
time by controlling their buoyancy using tanks, pumps and the 2013). However, these assumptions do not always hold in real
pitching angle using the position movable mass (Hussain, applications. If one of these assumptions is violated, the KF
Arshad, and Mohd-Mokhtar, 2011). For keeping gliding steadily becomes sub-optimal and could potentially become unstable
during a long period of time, the navigation and positioning (Al-Shabi, Gadsden, and Habibi, 2013).
system with minimum power and high precision should be The extended Kalman filter (EKF) method is widely used in
designed (Grenon et al., 2001; Houston and Dean, 2013; Sun, nonlinear systems. However, it needs to calculate the Jacobian
Zhu, and Li, 2012; Sun et al., 2013; Zhu and Zhao, 2011). matrix for the linearization of nonlinear systems, which results
For the Faraday cage effect of water to radio-frequency signal, in low accuracy and output frequency. The unscented Kalman
GPS (Global Positioning System) signal could not be received filter (UKF) method was presented by Juliear and Uhlman in
in deep water (Li et al., 2011; Morton et al., 1993; Watson, 1995 (Crassidis and Markley, 2003; Crassidis, Markley, and
Coleman, and Handsworth, 2008). The inertial measurement Yang, 2007), in which they used special sample points to fit the
unit mainly composed of gyroscopes and accelerometers can be nonlinear state and let the mean and variance of sampling points
used as the navigation system for an underwater glider (Chen et equal to those of the states at the sampling moment. Based on
al., 2013; Sabatini, 2006). such sampling points in the nonlinear system, the corresponding
High accuracy attitude information as the key parameter of transformation sampling points were obtained, during which the
navigation is very important for underwater gliders (Chen, Shen, corresponding mean and variance were calculated. For
and Zhao, 2013). The performance of attitude determination, non-linearized systems, this method can achieve higher
including accuracy and speed, not only depends on the accuracy of filtering than the EKF (Quan et al., 2013).
capability of inboard sensors, but also relies on the performance Although UKF gives better results in attitude estimation than
of attitude determination algorithms (Quan et al., 2013). EKF, it has some flaws because of the theoretical hypothesis
The Kalman filter (KF) assumes that the system model is that the distribution of the state parameter is similar to Gaussian.
Consequently, the performance of UKF worsens in the cases of
____________________
DOI: 10.2112/SI73-120.1 received 5 August 2014; accepted in revision non-Gaussian distribution. Moreover, even if the initial state
5 November 2014. distribution, the process noise, and the measurement noise are
*Corresponding author: chxiyuan@seu.edu.cn Gaussian, the real distribution of state may not be Gaussian
© Coastal Education & Research Foundation, Inc. 2015 because of the nonlinearity of the model (Wu et al., 2012).
Attitude Determination for Underwater Gliders Using Unscented Kalman Filter Based on Smooth Variable Algorithm 699
The smooth variable structure filter (SVSF) (Habibi, 2006; Where VEDR, N ,U are the velocities estimated by DR (Dead
Habibi and Burton, 2002; Habibi and Burton, 2007) is a
predictor-corrector filter that is based on the sliding mode Reckoning); VEINS
, N ,U
are the velocities measured by INS;
control (SMC) concepts, and can be applied to both linear and ϕ
Gyro , Mag
are the heading angles measured by gyroscopes and
nonlinear systems. The SVSF benefits from the stability and
robustness of SMC. Previous attempts were made in combining magnetometers, respectively; θ Gyro , Acce are the pitch angles
the EKF with the SVSF, however the results were poor and the measured by gyroscopes and accelerometers, respectively;
implementation was overly complex (Habibi, 2007). γ Gyro , Acce are the roll angles measured by gyroscopes and
This paper designed a new navigation system applied to the accelerometers, respectively; H is the observation matrix; V
underwater glider, and proposed the unscented Kalman filter represents Gaussian measurement noise with zero mean value.
based on smooth variable algorithm (UKF-SV) to improve the In the practical underwater navigation, the model of data fusion
accuracy of attitude calculated by underwater glider navigation is not linear. Therefore, the considered nonlinear discrete -time
system. system is represented (Xiong, Zhang, and Chan, 2006):
X (t ) = F (t ) X (t ) + W (t ) (1) χ i , k −1 = Xˆ k −1 , i=0
X = [δ L δλ δh δ VE δ VN δ VU φE φN φU
χ i , k −1 = Xˆ k −1 + ( a LPˆk −1 ) i , i = 1, , L (5)
∇ bx ∇ by ∇ bz ε bx ε by ε bz δMx δMy δMz]
T
χ i , k −1 = Xˆ k −1 − ( a LPˆk −1 ) i − L i = L + 1, , 2 L
Where F is the system matrix; W is the system noise,
assumed to be white with zero mean; δ L , δλ , δ h represents
position errors in East-North-Upward (ENU) axes, respectively; Where a ∈ R is a turning parameter denoting the spread of
δ VE , N ,U represents velocity errors; φE , N ,U are the orientation
the sigma points around Xˆ k −1 and is often set to a small
errors; ∇ bx , by , bz represent the biases of accelerometers; ε bx , by , bz positive value. This parameter can affects errors caused by more
represent the gyroscopes drifts; δ M x , y , z are the measured than second-order matrices. ( LPˆk −1 ) i is the ith column of
magnetic field errors. the matrix square root of LPˆk −1 which is real symmetric
The measurement model is described as Equation (2):
positive definite matrix.
The sample points is composed of sigma points set {χ i }
VEINS − VEDR
INS DR i = 0, , 2 L , and the corresponding weight with these sample
VN − VN
1 1
V − VUDR
INS
points are described as: ω =1− , ω = , i = 1, , 2 L . ωi
Z (t ) = UGyro = H (t ) X (t ) + V (t ) (2) 0
a
2 i
2 La
2
ϕ − ϕ
Mag
2n
γ − γ
Acce
Remark 2. Each point is instantiated through the process model
χ i , k | k −1 = f ( χ i , k −1 ) (6) Xˆ k = Γ Xˆ k ,Wk
k +1
( ) (16)
The predicted mean and covariance are computed as The corresponding predicted measurements
2L
ˆ ˆ k +1
Zˆ k = hX
Xˆ k | k −1 = ωi χ i , k | k −1
k +1
(7) k
(17)
i=0
Pˆk | k −1 = ωi ( χ i , k | k −1 − Xˆ k | k −1 )( χ i , k | k −1 − Xˆ k | k −1 ) + Qk (8)
T
= Z k +1 − Zˆ k
i =0 k +1
eZ k +1 (18)
k
Z i , k | k −1 = h ( χ i , k | k −1 ) (9) eZ k = Z k − Zˆ k (19)
k
Zˆ k = ωi Z i , k | k −1
−1
(10) G k + 1 = hˆ k +1
Zk
+λ eZ k
Zk
k +1 (20)
k
i =0
Xˆ k +1 = Xˆ k + Gk +1
k +1
2n (21)
Pˆk = ωi ( Z i , k | k −1 − Zˆ k )( Z i , k | k −1 − Zˆ k ) + Rk (11)
zz T
i =0
2n
Pˆk = ωi ( χ i , k | k −1 − Xˆ k | k −1 )( Z i , k | k −1 − Zˆ k ) (12) Repeat from Equation (3) to Equation (15) at time step k + 1 ,
xz T
i =0
then Pˆk +1| k and Pˆkzz+1 are derived.
K k = Pˆk ( Pˆk )
xz zz −1
(23)
The updated state estimates
Theorem. If eZ k ⋅ ( eZ k − eZ k −1 ) < 0 , the estimation process is
k k k −1
stable.
Xˆ k = Xˆ k | k −1 + K k ( Z k − Zˆ k ) (14)
Proof. We must prove the convergence of UKF-SV algorithm
The covariance matrix of error in the updated state estimates proposed in this paper. According to Theorem, because
eZ k > 0 , then eZ k − eZ k −1 < 0 (24) Then E eX k < E eX k −1 is derived from (30) when
k k k −1
k k −1
Equation (25) is satisfied. So the estimation process is stable
Transform (24), then and the algorithm proposed in this paper is convergent.
eZ ( ⋅ ) = heX ( ⋅ ) + Vk (27)
(⋅) (⋅)
( ) ( )
diag he k ⋅ diag he k + diag he k −1 ⋅ diag he k −1 +
Xk Xk X k −1 X k −1 (
) ( )
diag (V ) ⋅ diag (V ) + diag (V ) ⋅ diag (V ) +
k k k −1 k −1 (28)
<
( Xkk)
diag he ⋅ diag V + diag he
( k ) k −1
X k −1
⋅ diag ( V k −1 ) + (
) Figure 2. The underwater navigation system and the test turn table.
diag (Vk ) ⋅ diag heX k
k
( )
diag (Vk −1 ) ⋅ diag heX k −1
k −1
( ) In addition, the AHRS (Attitude and Heading Reference
System) with higher attitude accuracy is used as the attitude
Assumption 2. If the measurement noise is stationary white reference in the experiments. There are three definitions in this
noise, then: paper as follows:
Definition 1. Absolute standard deviation (ASTD)
For example: calculate the standard deviation (STD) for the
E diag (he(⋅) ) ⋅ diag (V( ⋅) ) and E diag (V( ⋅) ) ⋅ diag (he( ⋅) ) attitude reference; calculate the standard deviation for UKF;
calculate the standard deviation for UKF-SV.
in (28) are eliminated. Take the expectation of both sides in (28) Definition 2. Relative standard deviation (RSTD)
then: For example: calculate the standard deviation for UKF
relative to the attitude reference; calculate the standard
deviation for UKF-SV relative to the attitude reference.
E diag (heX ) ⋅ diag ( heX ) + diag (Vk ) ⋅ diag (Vk )
k
k
k
k
Definition 3. Maximum deviation (MAXD)
(29) For example: calculate the maximum deviation between UKF
< E diag (heX ) ⋅ diag (heX ) + diag (Vk −1 ) ⋅ diag (Vk −1 )
k −1
k −1
k −1
k −1
and attitude reference; calculate the maximum deviation
between UKF-SV and attitude reference.
Equation (29) is transformed as Equation (30) because The Static Accuracy Experiment
Equation (29) is positive, diagonal and time-invariant The experiment equipment mainly includes a three-axis
measurement matrix. non-magnetic turn table and a host computer used as data
sampling, as shown in Figure 2. The precision of heading, pitch
and roll of this turn table is 0.05º, 0.1º, and 0.1º, respectively.
E diag (eX ) ⋅ diag (eX ) < E diag (eX ) ⋅ diag (eX ) (30)
k
k
k
k
k −1
k −1
k −1
k −1
Adjust the inner frame; the middle frame and the external frame
of the turn table to local level with initialization zero point. The
SUNS-1 is fixed stably to the turn table and is sampled under
Table 1. The attitude ASTD for the attitude reference, UKF and UKF-SV (statically).
Table 2. The attitude RSTD and MAXD for UKF and UKF-SV (statically).
UKF UKF-SV
RSTD MAXD RSTD MAXD
Heading (Deg) 0.4409 1.8169 0.2718 0.8721
Pitch (Deg) 0.0987 0.6905 0.0571 0.3179
Roll (Deg) 0.1199 0.6373 0.0753 0.3315
-8
Roll Comparison
-9 2.8
Reference
-10 2.6 UKF-SV
UKF
-11 2.4
0 0.5 1 1.5 2 2.5 3 3.5 4
Time (sec) 4
x 10
2.2
1.8
Pitch Comparison
-1 1.6
Reference
-1.2 UKF-SV
1.4
UKF
-1.4
1.2
-1.6
1
0 0.5 1 1.5 2 2.5 3 3.5 4
Pitch (deg)
-1.8
Time (sec) 4
x 10
-2
Table 3. The attitude ASTD for the attitude reference, UKF and UKF-SV (dynamically).
Table 4. The attitude RSTD and MAXD for UKF and UKF-SV (dynamically).
UKF UKF-SV
RSTD MAXD RSTD MAXD
Heading (Deg) 1.2802 7.1349 0.8434 5.0838
Pitch (Deg) 1.0084 3.3230 0.6377 1.7748
Roll (Deg) 1.0267 3.5618 0.6202 1.5685
Roll Comparison
7
Reference
6 UKF-SV
5 UKF
SUNS-1 4
Roll (deg)
2
-1
AHRS
-2
-3
Figure 6. The vehicle experiment platform.
-4
0 20 40 60 80 100 120 140 160 180
Time (sec)
Heading Comparison
200
Reference
150
UKF-SV Figure 9. The roll angle comparison (dynamically).
UKF
100
UKF-SV, vehicle experiments were done. The experiment was
performed with the non-magnetic trolley on the playground.
Heading (deg)
50
0
AHRS and SUNS-1 are all fixed to the trolley. The attitudes
measured by SUNS-1 and AHRS are sampled at the same time
-50
with a frequency of 1 Hz. The attitude angles measured by
-100 AHRS are as the reference to evaluate the attitude accuracy for
-150
SUNS-1. Figure 6 shows the experiment trolley and the setup of
0 20 40 60 80 100
Time (sec)
120 140 160 180
these navigation systems in detail.
The trolley runs with the velocity of 0.5 m/s along with the
Figure 7. The heading angle comparison (dynamically). presupposition rectangular trajectory (Length: 40 m, Width: 16
m). The attitude angles estimated by the attitude reference, UKF
3
Pitch Comparison and the proposed UKF-SV are shown in Figures 7-9.
Reference It is seen from Figure 7 to Figure 9 that the proposed
2 UKF-SV
UKF UKF-SV performs better than UKF. To further analyze the
1
attitude accuracy for UKF-SV and compare UKF-SV with UKF,
0 more detailed comparison results are given in Tables 3-4.
In Table 3, the ASTD of heading is not calculated because the
Pitch (deg)
-1
34.12%. Moreover, by employing UKF-SV, the MAXD of ASME 2002 International Mechanical Engineering Congress
heading is reduced from 7.1349º to 5.0838º. The similar results and Exposition (New Orleans, Louisiana, USA), pp. 157-165.
are also analyzed for the pitch and the roll. Therefore, it can be Habibi, S.R. and Burton, R., 2007. Parameter identification for a
concluded that the proposed UKF-SV algorithm can perform high performance hydrostatic actuation system using the
better in terms of attitude determination accuracy than variable structure filter concept. ASME Journal of Dynamic
conventional UKF under the dynamic condition. Systems, Measurement, and Control, 129(2), 229-235.
Houston, J.R., and Dean, R.G., 2013. Effects of sea-level decadal
CONCLUSION variability on acceleration and trend difference. Journal of
This paper designed a new underwater navigation system and Coastal Research, 29(5), 1062-1072.
proposed the UKF-SV algorithm to improve the attitude Hussain, N.N.A.; Arshad, M.R., and Mohd-Mokhtar, R., 2011.
determination accuracy for an underwater glider. The UKF-SV Underwater glider modelling and analysis for net buoyancy,
algorithm makes use of the advantage of UKF in the nonlinear depth and pitch angle control. Ocean Engineering, 38(16),
model to estimate attitudes and then smoothes the variables 1782-1791.
estimated by UKF. Through this process, the attitude accuracy Li, Y.; Pang, Y.J.; Jiang, Y.Q., and Chen, P.Y., 2011. Application
can be greatly improved. The static experiment can show that of strong tracking kalman filter in dead reckoning of
the attitude accuracy for UKF-SV is much higher than UKF. In underwater vehicle. Electrical Engineering and Control,
the dynamic experiment, the advantage of UKF-SV is more Lecture Notes in Electrical Engineering, 98, 909-915.
obvious than UKF and the attitude accuracy for UKF-SV is Morton, R.A.; Leach, M.P.; Paine, J.G., and Cardoza, M.A.,
higher than UKF. 1993. Monitoring beach changes using GPS surveying
techniques. Journal of Coastal Research, 9(3), 702-720.
ACKNOWLEDGMENTS Quan, W.; Xu, L.; Zhang, H.J., and Fang, J.C., 2013. Interlaced
optimal-REQUEST and unscented Kalman filtering for
This work is supported by Public Science and Technology
attitude determination. Chinese Journal of Aeronautics, 26(2),
Research Funds Projects of Ocean (No. 201205035),
449-455.
Specialized Research Fund for the Doctoral Program of Higher
Sabatini, A.M., 2006. Quaternion-based extended kalman filter
Education (No. 20110092110039), National Natural Science
for determining orientation by inertial and magnetic sensing.
Foundation of China (No. 51375087), 973 Program (No.
IEEE Transactions on Biomedical Engineering, 53(7),
2009CB724002) and Research Innovation Program for College
1346-1356.
Graduates of Jiangsu Province (No. CXLX12_0082)
Sun, B.; Zhu, D.Q.; Ding, F., and Yang, S.X., 2013. A novel
tracking control approach for unmanned underwater vehicles
REFERENCES
based on bio-inspired neurodynamics. Journal of Marine
Al-Shabi, M.; Gadsden, S.A., and Habibi, S.R., 2013. Kalman
Science and Technology (Japan), 18(1), 63-74.
filtering strategies utilizing the chattering effects of the smooth
Sun, B.; Zhu, D.Q., and Li, W.C., 2012. An integrated
variable structure filter. Signal Processing, 93(2) 420-431.
backstepping and sliding mode tracking control algorithm for
Chen, X.Y.; Shen, C.; Zhang, W.B.; Tomizuka, M.; Xu, Y., and
unmanned underwater vehicles. Proceedings of the 2012
Chiu, K.L., 2013. Novel hybrid of strong tracking Kalman
UKACC International Conference on Control (Cardiff, Wales,
filter and wavelet neural network for GPS/INS during GPS
UK), pp. 644-649.
outages. Measurement, 46(10), 3847-3854.
Watson, C.; Coleman, R., and Handsworth R., 2008. Coastal tide
Chen, X.Y.; Shen, C., and Zhao, Y.F., 2013. Study on GPS/INS
gauge calibration: a case study at macquarie island using gps
system using novel filtering methods for vessel attitude
buoy techniques . Journal of Coastal Research, 24(4),
determination. Mathematical Problems in Engineering,
1071-1079.
Article ID 678943, 8p, DOI: 10.1155/2013/678943.
Wu, J.J.; Liu, K.; Wei, J.B.; Han, D.P., and Xiang, J.H., 2012.
Crassidis, J.L. and Markley, F.L., 2003. Unscented filtering for
Particle filter using a new resampling approach applied to
spacecraft attitude estimation. Journal of Guidance, Control,
LEO satellite autonomous orbit determination with a
and Dynamics, 26(4), 536-542.
magnetometer. Acta Astronautica, 81(2), 512-522.
Crassidis, J.L.; Markley, F.L., and Yang, C., 2007. Survey of
Xiong, K.; Zhang, H.Y., and Chan, C.W., 2006. Performance
nonlinear attitude estimation methods. Journal of Guidance,
evaluation of UKF-based nonlinear filtering. Automatica,
Control, and Dynamics, 30(1), 12-28.
42(2), 261-270.
Grenon, G.; An, P.E.; Smith, S.M., and Healey, J., 2001.
Xu, Y.; Chen, X.Y., and Li, Q.H., 2013. Autonomous integrated
Enhancement of the inertial navigation system for the
navigation for indoor robots utilizing on-line iterated extended
morpheus autonomous underwater vehicles. IEEE Journal of
rauch-tung-striebel smoothing. Sensors, 13(12), 15937-15953.
Oceanic Engineering, 26(4), 548-560.
Zhu, D.Q. and Zhao, Y., 2011. A bio-inspired dynamic
Habibi, S., 2006. The extended variable structure filter. Journal
path-following control of autonomous underwater vehicle.
of Dynamic Systems, Measurement, and Control, 128(2),
Proceedings of the 16th IASTED International Conference on
341-351.
Robotics and Applications (Vancouver, BC, Canada), pp.
Habibi, S., 2007. The smooth variable structure filter.
22-29.
Proceedings of the IEEE, 95(5), 1026-1059.
Habibi, S.R. and Burton, R., 2002. The variable structure filter.