Professional Documents
Culture Documents
Abstract—Having an accurate mathematical model is essential of an AUV and its parameters were identified by open-loop ex-
in design, control, and navigation process of an autonomous un- perimental data. They used the ordinary least squares and the
derwater vehicle (AUV). Due to the modeling simplifications, the total least squares (TLS) methods as the estimation algorithms
available mathematical models suffer from the uncertainty of their
parameters and they usually need an identification phase for im- and showed that the TLS has a better performance. Xu et al. [6]
proving the modeling accuracy. In case of AUVs, the hydrodynamic developed an identification method for modeling the nonlinear
coefficients of the model play an important role and they should dynamics of a typical torpedo-shape AUV. They used a hybrid
be identified using the available experimental data. In this paper, algorithm of least square (LS) and support vector machines
a 6-DoF dynamic model of an AUV is presented and then some (SVM) in identification of Coriolis, centrifugal, and damping
of its parameters including viscous damping, the body lift, and
control input coefficients that have the highest effects on the mod- coefficients by experimental data. Petrich et al. [7] presented
eling error are identified by augmented state model method. The a fourth-order dynamic model for pitch motion. The dynamic
extended Kalman filter (EKF), cubature Kalman filter (CKF), and model was used to control the pitch and heave motions. The hy-
transformed unscented Kalman filter (TUKF) are used as the es- drodynamic coefficients were identified for designing a robust
timation filters. To verify and compare the three estimation filters and stable control, based on experimental data. Taubert et al. [8]
with consideration of the predetermined hydrodynamic coefficients
and spiral maneuver AUV results, these three methods are eval- derived an accurate model of Cwolf AUV for designing a robust
uated. The results indicate that the TUKF identifies the best hy- and stable autopilot system. The damping parameters, Coriolis
drodynamic model due to solving both the CKF nonlocal sampling and centrifugal forces, added mass, gravity, and buoyancy were
problem and the EKF linearization problem. estimated by open-loop experimental tests and the identified
Index Terms—Autonomous underwater vehicle (AUV), cuba- model was used in PID controller designing. Geng and Zhao [9]
ture Kalman filter (CKF), extended Kalman filter (EKF), system assessed a dynamic model of an AUV in the horizontal plane and
identification, transformed unscented Kalman filter (TUKF).
an ARMAX model in the vertical plane. The identified model
I. INTRODUCTION was used in designing a model predictive controller and the con-
YSTEM identification and parameter estimation are strong troller performance was evaluated by experimental tests of the
S techniques in developing an accurate mathematical dy-
namic model of a system using the experimental data. In marine
depth control. Rentschler et al. [3] studied the adaptive control
of Odyssey III AUV using the model identification method. Xu
industry, establishing an identified dynamic model of a vehicle et al. [10] derived a nonlinear and coupled dynamic model of
is very important since the model can be applied in a variety an AUV in the vertical plane. The hydrodynamic parameters of
of applications such as control, navigation system, and optimal mathematical model were estimated by the LS-SVM method
design of a vehicle [1]–[3]. and obtained data from the numerical simulation. Sadeghzadeh
Many researchers have studied the model identification and Mehdigholi [11] calculated the hydrodynamic coefficients
and parameter estimation of autonomous underwater vehicles of the SUT-2 AUV by using experimental tests. In these tests,
(AUVs). Valeriano et al. [4] derived a dynamic model of the they used a scaled size of the SUT-2 AUV. The calculated hy-
Hydrographic Research Center (HRC)-AUV, based on low-cost drodynamic coefficients were applied in dynamic model and
sensors and a set of low-cost experimental tests. The model controller design. Hegrenaes et al. [12] studied the model iden-
was evaluated by experimental data and a simplified version of tification of HUGIN4500 AUV. The models were identified by
it was used for pitch, depth, and heading controls. Martin and experimental data and LS estimation method and dynamic mod-
Whitcomb [5] presented a 6-DoF second-order nonlinear model els were derived by estimation of hydrodynamic coefficients.
In online identification methods, the model parameters are
updated recursively. The KF and recursive least square (RLS)
Manuscript received July 23, 2016; revised February 18, 2017 and February methods are the most famous filters for online system identifi-
24, 2017; accepted April 9, 2017. Date of publication May 16, 2017; date of
current version April 12, 2018. (Corresponding author: Alireza Fathi.) cation. In online identification methodologies, many simplifica-
Associate Editor: B. Bingham. tions such as linearity, assuming a two-dimensional (2-D) model
The authors are with the Department of Mechanical Engineering, Nooshirvani and gray box are included into the model [12], [14], [18]. Pin-
University of Technology, Babol 4714871167, Iran (e-mail: mohammadtaghi.
sabet@gmail.com; mohammadi@nit.ac.ir; fathi@nit.ac.ir; Alizadeh@nit.ac.ir). heiro and Barros [13] used EKF in estimation of hydrodynamic
Digital Object Identifier 10.1109/JOE.2017.2694470 coefficients of an AUV and the identified model was evaluated
0364-9059 © 2017 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.
See http://www.ieee.org/publications standards/publications/rights/index.html for more information.
458 IEEE JOURNAL OF OCEANIC ENGINEERING, VOL. 43, NO. 2, APRIL 2018
TABLE I TABLE II
INTRODUCTION OF THE FORCE MATRICES AND VECTORS IN (8) INTRODUCTION OF THE COEFFICIENTS
M = MR B + MA Added mass and rigid body inertial matrix X u̇ , Y v̇ , Z ẇ kg Added mass coefficients
C (v) = C R B (v) + C A (v) Added mass and rigid body Y ṙ , Z q˙ kg·m/rad
Coriolis-centripetal matrix K p˙ , M q˙ , N ṙ kg·m2 /rad
D (v) Damping matrix M ẇ , N v̇ kg m
g(η ) Restoring forces and moments
X u |u | , Y v |v | , Z w |w | kg/m Damping matrix coefficients
τ = τlift + τc o ntro l + τp ro p u lsio n Control inputs, lift, and propulsion force
Y r |r | , Z q |q | kg·m/rad2
vector
K p |p | , M q |q | , N r |r | kg·m2 /rad2
M w |w | , N v |v | kg
as [19] Yu r l , Zu q l kg/rad Lift vector coefficients
Yu v l , Zu w l kg/m
m v̇b/n
b b
+ ω̇b/n ×rbg +ωb/n
b
×vb/n
b b
+ωb/n × ωb/n
b
×rbg = fbb Mu q l , Nu r l kg·m/rad
Mu w l , Nu v l kg
(5)
Yu u δ r , Zu u δ e kg/(m·rad) Control inputs vector coefficients
Mu u δ e , Nu u δ r kg/rad
Ib ω̇b/n
b b
+ωb/n × Ib ωb/n
b
+mrbg × v̇b/n
b b
+ωb/n ×vb/n
b
= mbb
Xp ro p u lsio n N Propulsion vector force and moment
(6) Kp ro p u lsio n N·m
namic and measurement model of the estimation algorithms can My = −(zG W−zB B) sin θ − (xG W − xB B) cos θ cos ϕ
be represented as (11) and (14).
The dynamic model for AUV identification is represented as +Mw |w | |w| w+Mq |q | |q| q+(Mu w l +Xu̇ −Zẇ ) uw
⎡ ⎤ ⎡ ⎤ +(Mu q l − Zq̇ − mxG ) uq + (mxG − Nv̇ ) vp
u̇ F
⎢ ⎥ x
⎢ v̇⎥ ⎢ F ⎥ +mzG (vr − wq) + [(Iz z − Ixx ) + Kṗ − Nṙ ] rp
⎢ ⎥ ⎢ y ⎥
⎢ ẇ⎥ ⎢ Fz ⎥
⎢ ⎥ ⎢ ⎥ +Mu u δ e u2 δe
⎢ ṗ⎥ ⎢ ⎥
⎢ ⎥ ⎢
Mx ⎥
⎢ q̇⎥ ⎢ ⎥ Mz = (xG W − xB B) cos θ sin ϕ + (yG W − yB B) sin θ
⎢ ⎥ = M−1 ⎢
M y ⎥
⎢ ⎥ ⎢ ⎥ +ξ.
⎢ ṙ⎥
a
⎢ M z ⎥
⎢ ⎥ ⎢ ⎥ + Nv |v | |v| v + Nr |r | |r| r + (Nu v l + Yv̇ − Xu̇ ) uv
⎢ ϕ̇⎥ ⎢p+tan (θ) sin (ϕ) q+tan (θ) cos (ϕ) r⎥
⎢ ⎥ ⎢ ⎥
⎢ θ̇⎥ ⎢ cos (ϕ) q − sin (ϕ) r ⎥ + (Yṙ + Nu r l − mxG ) ur + (Mẇ + mxG ) wp
⎢ ⎥ ⎢ ⎥
⎣ ψ̇⎦ ⎣ sin (ϕ)/cos (θ) q+cos (ϕ)/cos (θ) r ⎦ + myG (wq − vr) + [Mq̇ − Kṗ + (Ixx − Iy y )] pq
Ω̇22×1 022×1
(11) + Nu u δ r u 2 δ r . (13)
In (11), parameters F x , F y , F z , M x , M ,
y and
Also, the measurement model (DVL, gyroscope and AHRS
Mz are total forces and moments and Ma is the rigid body
models) is expressed as
and added mass inertial matrix. The detailed expressions of
T
these forces, moments and inertial matrix can be shown as (12), ya = I9×9 09×22 × u v w p q r ϕ θ ψ ΩT22×1 + ζ.
shown at the bottom of the page, and (14)
Three state estimation algorithms, the EKF, CKF, and TUKF
Fx = −(W − B) sin θ + Xu |u | |u| u + (m − Yv̇ ) vr are used for estimation of hydrodynamic coefficients. In this
section, these three algorithms are described.
+ (Zẇ −m) wq−myG pq−mzG pr+(Zq̇ +mxG ) q 2
+ (mxG − Yṙ ) r2 + XPropulsion A. Extended Kalman Filter
Fy = (W − B) cos θ sin ϕ + Yv |v | |v| v + Yr |r | |r| r KF is the most famous filter among the estimation filters. This
filter is an optimal estimator for linear systems with Gaussian
+ Yu v l uv + (Yu r l + Xu̇ − m) ur + (m − Zẇ ) wp distribution and white noise [23]. Based on time priority and
− (Zq̇ + mxG ) pq − mzG qr + myG p2 + r2 posteriority, the KF has two time steps. At the first time step,
called the prediction (time update) step, the state variables x− ak ,
+ Yu u δ r u2 δr
and the error covariance matrix P− ak are predicted by using es-
Fz = (W − B) cos θ cos ϕ + Zw |w | |w| w + Zq |q | |q| q timated state variables x+a k −1 and error covariance matrix P +
a k −1
on the previous step and based on the dynamic model. At the
+ Zu w l uw + (m + Zu q l − Xu̇ ) uq second time step, called the measurement update (time update)
+ (Yv̇ − m) vp + (Yṙ − mxG ) rp − myG rq step, the state variables x+ a k and the error covariance matrix
P+ are updated by updated measurements yk . If the dynamic
+ mzG q 2 + p2 + Zu u δ e u2 δe ak
or measurement model or both of them are nonlinear, the KF
Mx = (yG W − yB B) cos θ cos ϕ + (zG W − zB B) cannot be used for state estimation. In these cases, the EKF
is used for state estimation. In the EKF, the dynamic and the
× cos θ sin ϕ + Kp|p| |p| p + myG (uq − vp) measurement models need not be linear functions of the state
+ mzG (ur − wp) + (Iy y − Iz z ) qr + Kpropulsion but at each time step the Jacobian matrix of the nonlinear func-
⎡ .. ⎤
m − Xu̇ 0 0 0 mzg −myg .
⎢ ⎥
⎢ .. ⎥
⎢ 0 m − Yv̇ 0 −mzg 0 mxg − Yṙ . ⎥
⎢ ⎥
⎢ .. ⎥
⎢ 0 0 m − Zẇ myg − (mxg + Zq̇ ) 0 . 06×22 ⎥
⎢ ⎥
⎢ .. ⎥
⎢ −mzg Ix − Kṗ ⎥
Ma = ⎢ 0 myg 0 0 . ⎥ (12)
⎢ .. ⎥
⎢ mzg 0 − (mxg + Mẇ ) 0 Iy − Mq̇ 0 . ⎥
⎢ ⎥
⎢ .. ⎥
⎢ −my mx − N Iz − Nṙ ⎥
⎢ g g v̇ 0 0 0 . ⎥
⎢ ··· ··· ··· ··· ··· ··· ··· ··· ⎥
⎣ ⎦
..
022×6 . I22×22
SABET et al.: IDENTIFICATION OF AN AUV HYDRODYNAMIC MODEL USING THE EXTENDED, CUBATURE, AND TUKF 461
P−
ak ≈ Φa k P+
a k −1 Φa k
T
+ Qa k dt (15)
where dt is the time step, and Φa k is the state transition matrix
of the discrete time augmented state-space system. The state
transition matrix is calculated as
Φa k = eA a dt ≈ I + Aa dt (16)
where Aa is the state matrix of the linearized continuous system Fig. 2. AUV path to identify the hydrodynamic coefficients.
as the following:
⎡ ⎤
∂f ∂f
∂fa TABLE III
Aa = = ⎣ ∂x ∂Ω ⎦ . (17) NAVIGATION SENSORS SPECIFICATIONS
∂xa x a =x +a 0 0 x =x +
k −1
a a k −1
Sensor type Specifications Accuracy Measuring rate
According to (14), the measurement model is linear and there-
fore, the measurement update step is similar to that in KF and 3DM-GX3-25 (Accelerometer) Bias stability error ±0.003 g 100 Hz
3DM-GX3-25 (Gyroscope) Bias stability error 0.2 °/s 100 Hz
can be presented as Teledyne WHN300 (Speed DVL) Velocity accuracy 0.1 cm/s 7 Hz
−1 HMC1001 (Magnetometer) Angle accuracy –1.5° ∼ +1.5° 10 Hz
Ka k = P− −
a k Ha Ha Pa k Ha + Ra
T T
−
−
a k = xa k + Ka k ya k − Ha xa k
x+
In the Bayes filter, the posterior probability density function
−
P+
a k = (I − Ka k Ha ) Pa k (18) (PDF) of states is a description of the states in the time tk −1 .
In this filter, the posterior PDF is updated in two steps by re-
where Ha = [ I9×9 09×22 ] is the measurement matrix, Ra is ceiving new measurements in the time tk . Two time steps of the
the measurement covariance matrix, and Ka k is the Kalman Bayes filter with Kushner approximations and Gaussian density
gain matrix. Higher order terms of the Taylor series expansion assumptions are presented as follows [33]:
of (16) are ignored in the EKF for simplification, however, they Prediction: In the prediction step, the predicted mean x− k and
are effective in filter application [25]. In EKF method, the fil- the predicted covariance P− of the states are calculated as the
k
ter tuning and the state estimation can be hard for a process following:
with a high degree of nonlinearity and the state estimation algo-
rithm can diverge due to the linearization errors [26]. The EKF x−
k = E [ xk | Dk −1 ]
pseudocode is presented in Table A1 of Appendix A.
= f (xk −1 , uk −1 ) × N xk −1 ; x+
k −1 , Pk −1 dxk −1
+
B. CKF and TUKF Based on Bayes Filter Rn x
(20)
The UKF [27] decreases the linearization errors of the EKF
but it suffers from computational complexity [28] and therefore
it cannot be applicable for high dimensional problems [29]. The where N(xk −1 ; x+k −1 , Pk −1 ) is a Gaussian PDF with the mean
+
CKF [30] is a nonlinear estimation filter that has been developed x+k −1 and the covariance Pk −1 , E is the expected value and
+
k −1
for state estimation of high dimensional nonlinear problems Dk −1 = {ui , yi }i=1 is the input and the measurement pair vec-
[31]. The nonlocal sampling problem occurs in CKF algorithm tor from time t1 to tk −1 .
when the state dimension is very high [32]. In those cases, TUKF
− T
[32] is used, which eliminates the nonlocal sampling problem P−
k =E xk − x −
k x k − x k y 1:k −1
of CKF.
The dynamic and measurement model of a discrete time sys- = f (xk −1 , uk −1 ) f T (xk −1 , uk −1 )
tem can be represented as Rn x
− −T
xk = f (xk −1 , uk −1 ) + ξk −1 × N xk −1 ; x+
k −1 , Pk −1 dxk −1 − xk xk + Qk −1
+
(21)
yk = h (xk , uk ) + ζk (19)
where x is the state variable, u is the control input, and ξ and ζ where nx is the number of states and Qk −1 is the covariance
are the model and measurement noises, respectively. matrix of the dynamic model noise.
462 IEEE JOURNAL OF OCEANIC ENGINEERING, VOL. 43, NO. 2, APRIL 2018
Fig. 3. Estimation of viscous damping coefficients (dash-dot, dash, solid-thin, and solid-thick diagrams are TUKF, CKF, and EKF, and predetermined values
(True), respectively).
TABLE IV
PERCENTAGE OF THE STEADY-STATE ERROR AND THE ROOT MEAN SQUARE
ERROR OF ALL OF THE COEFFICIENTS
Fig. 5. Estimation of body lift coefficients (dash-dot, dash, solid-thin, and solid-thick diagrams are TUKF, CKF, and EKF, and predetermined values (True),
respectively).
Fig. 6. Comparison of the performance of the TUKF, CKF, and EKF identified model and the measurement velocities.
viscous damping coefficients, eight body lift coefficients and In Figs. 4 and 5, estimation of control input and body lift coef-
four control input coefficients are estimated. All coefficients are ficients are shown, respectively. Similar to the viscous damping
initialized zero. coefficients, EKF has the lowest accuracy in comparison to CKF
The identified results of viscous damping coefficients are and TUKF. The identification error is because of the lineariza-
presented in Fig. 3 and Table IV. According to this figure, for tion errors.
most of the coefficients, TUKF and CKF present superior results Table IV illustrates the identification results for all of the
in comparison with those for EKF. The observed weakness of model coefficients. The results show that the average percent of
EKF in these results is due to the linearization error. Since the steady state error in the model coefficient estimates from EKF,
EKF filter utilizes the first term of the Taylor series expansion CKF, and TUKF are about 6.064, 1.815, and 0.767, respectively.
(linearization), it cannot identify the model parameters of a This means that the TUKF has the best accuracy.
system with a high degree of nonlinearity. The estimator filter From the computational complexity and CPU time point of
must be appropriate for at least three orders of the degree of view, the EKF has lowest time, which is 3.543 s while it is 75.8
nonlinearity. and 77.3 s for CKF and TUKF, respectively. The magnitude
466 IEEE JOURNAL OF OCEANIC ENGINEERING, VOL. 43, NO. 2, APRIL 2018
order of CPU time in EKF is much lower than the others, which V. CONCLUSION
means this algorithm can be implemented more easily than the
In this paper, the identification of a 6-DoF dynamic model
others. But the CPU time in CKF and TUKF methods does not of an AUV using EKF, CKF, and TUKF has been studied. The
have a significant difference.
identified parameters are 22 unknown coefficients including the
Fig. 6 illustrates the numerical simulations of the identified
hydrodynamic of viscous damping, body lift, and control input
models. The simulation results are presented as the translational coefficients. Simulation results clearly show that these nonlinear
and rotational velocities in body coordinate representations. The
filters could accurately estimate the hydrodynamic coefficients.
mean absolute errors of these simulations are summarized in
Among these three methods, the TUKF and CKF present supe-
Table V. As this table shows, TUKF has smaller error compared rior results in comparison with the EKF. Moreover it was shown
to the two other methods and therefore the identified model by
that TUKF algorithm improves the nonlocal sampling problem
the TUKF filter has better accuracy. Moreover it can be con- of CKF method and has better prediction accuracy while their
cluded the identified model with the CKF has a lower bounded CPU times are almost the same.
error, while according to Fig. 6 and Table V, the steady-state
error of velocities (especially translational velocities) and root
mean square error are larger in the EKF compared with the two
other methods. APPENDIX A
In Fig. 6 and Table V, the output velocities of the simulation Pseudocodes of EKF, CKF, and TUKF are represented in
are corrupted by sensor errors that are presented in Table III. Tables A1 and A2.
TABLE A1
PSEUDOCODE OF EKF
x− − − − T
The initialization of the state variables and covariance matrix a 0 = E[x a 0 ], P a 0 = E[[x a 0 − x a 0 ][x a 0 − x a 0 ] ]
1) Prediction (Time Update)
tk
Predicted estimation of the augmented state variables x− +
a k = x a k −1 + t k −1 f a (x +
a k −1 , u k −1 ) dt
Predicted estimation of the error covariance matrix P− +
a k ≈ Φ a k P a k −1 Φ a k + Q a k dt
T
∂f ∂f
∂ fa
State transition matrix of continuous time system Aa = ∂ x a |x a = x + = [ ∂ x ∂ Ω ]x = x +
a k −1 0 0 a a k −1
State transition matrix of discrete time system Φ a k = e A a d t ≈ I + A a dt
2) Update (Measurement Update)
Innovation or measurement residual ya k − H a x−ak
Innovation or residual covariance Ha P− a k Ha + Ra
T
Filter gain Ka k = P− −
a k H a (H a P a k H a + R a )
T T −1
− −
Updated estimation of augmented state variables x+a k = x a k + K a k (y a k − H a x a k )
−
Updated estimation of error covariance matrix P+
a k = (I − K a k H a )P a k
TABLE A2
PSEUDOCODE OF CKF AND TUKF
x− − − − T
The initialization of the state variables and covariance matrix a 0 = E[x a 0 ], P a 0 = E[[x a 0 − x a 0 ][x a 0 − x a 0 ] ]