Center of Buoyancy Estimation for

Large Autonomous Underwater Vehicles

Mingyu Wang, Yutao Liu

June 2, 2016

As Autonomous Underwater Vehicles(AUVs) grow in size, the limited energy they can carry on
board imposes serious constraints on both workload and range. The problem of maintaining attitude
with minimum control effort is especially important for large AUVs to improve power efficiency. A
good estimation of the vehicles attitude enables better control of propulsion, which saves power for
other operations [1]. This project investigates two state estimation techniques, the Extended Kalman
Filter and the Unscented Kalman Filter, to determine the center of buoyancy(CoB) of the vehicle
from onboard sensor measurements.

1 Introduction
Conventional AUV dynamics can be modeled as a rigid-body balanced with gravity, buoyancy,
thrust, drag and other perturbations from the underwater environment. In more recent applications
with manipulators, the AUV becomes a multi-body system connected through joints, which adds
some disturbance to the equilibrium. Figure 1 gives some examples of current AUVs developed
by different research centers. SAUVIM is a semi-autonomous vehicle with a 7-dof manipulator
which can perform task at a depth of 4000m. Girona 500 AUV is developed by Girona Underwater
Vision and Robotics Team and is reconfigurable for different tasks. With up to 8 thrusters, Girona
can be controlled all 6 degrees of freedom. Maximum operation depth for Girona 500 is 500m.
VORTEX has a 7-link manipulator, the control architecture of which can be found in [11] One of
the most important goals during autonomous tasks is to maintain desired position and attitude while
manipulator performs tasks which may involve interaction with surrounding environment. Reaction
force acts as a disturbance to the system dynamics. In the ideal case, the gravity and buoyancy
forces align, making the vehicle neutrally buoyant. However for large AUVs under diverse working
conditions, variable buoyancy brings more advantages. The problem is how to identify dynamic
equation parameters, such as CoB position. By measuring the position of COB with respect to
CG, ballast operation and the thruster forces could be optimized for stabilization, depth control,
and navigation. In this project, the team investigates algorithms to estimate Center of Buoyancy
position with respect to Center of Mass. Extended Kalman Filter and Unscented Kalman Filter are
applied to the vehicle transition and measurement models to provided a good estimate of system
states. Different algorithm performances are compared and discussed.

2 Problem Formulation
Modeling of Autonomous Underwater Vehicle has been studied in many literature, [2] [3] [4], where
dynamic models of multibody systems are derived. Derived from Newton-Euler Equations, dynamic
equations for a general multibody system has the following form:

A(q)p + B(q, p)p = (1)

1
 (a) SAUVIM, University of Hawaii

(b) Girona, Girona Underwater Vision (c) vortex, Inria and Ifremer
and Robotics team

Figure 1: Autonomous Underwater Vehicles

where A is a n n inertial matrix. q is generalized coordinates of the system. p is system quasive-

locity vector. is generalized force projected onto system joint space. The second term represents
Coriolis and centrifugal forces. Expressed in joint space, this equation allows future generalization
to multibody systems. To implement filter algorithms, q, p and 10 other vehicle states are included
which will be discussed in detail in the following section. In underwater environment, hydrodynamic
effects on a rigid body present in several aspects, including added mass and inertia, damping effects
and current effects. Among these complex fluid theories, we are interested in a phenomenon only if
it effects the properties of the system under consideration. In this case,

= D p + TCM th + r (2)

where D is damping matrix, TCM is thruster control matrix and r is restoring force, including
buoyancy and gravity.
As for measurement, GPS is not available underwater, so Inertial Navigation System(INS) is
equipped. Specifically, PHINS with a self-adaptive Kalman Filter can provide transformation ma-
trix from PHINS frame to inertial frame, linear and angular velocity, and acceleration. In all, 21
measurement variables are used in filter update step.
Different versions of the Kalman filter have been applied to this estimation problem. Besides
conventional designs like the EKF and UKF, novel filter architecuture have been developed that
tailors to attitude sensing. In particular, the Invariant Extend Kalman Filter (IEKF) [5] applied to
the 3D rotation group has been shown to perform well [6].

2
3 Mathematical Models
When implementing Kalman Filter and its variants, process transition model and measurement
model are necessary. Preliminary development of our models is introduced in the following sections.

3.1 Vehicle Dynamics

In our model, the state space is defined by vector comprising the angular and linear rates information
 T
is included as the quasivelocity vector p = x y z vx vy vz . Combining Equation(1) and
(2), derivative of p is given by:

p = A(q)1 (B(q, p)p + D p + TCM th + r ) (3)

TCM projects thruster forces into joint space. To calculate r , duality between velocity and force
is used. Jacobian matrix is used to describe relationships between general velocity of a certain fixed
point on rigid body and joint velocities(state p) as:
 
p
= J(q, xp ) p
vp
 T
p vp is angular and linear velocities of point p in inertial frame, xp is coordinates of point p
in CoM frame and p is system quasivelocity. With this relationship, r is given by:

r = J 0 (q, xcb ) F~b + J 0 (q, ~0) G

~ (4)
 T
The vehicles rotational and translational degrees of freedom, q = X Y Z , com-
pletely describes the vehicles orientation in Euler Angles and position in Cartesian Coordinates.
Evolution of q can be easily expressed using rigid body kinematics with rotation matrix from center
0
of mass frame to inertial frame Rcm :

1 sintan costan wx
= 0 cos sin wy
(5)
0 sin/cos cos/cos w z

where , and are roll, pitch and yaw angles respectively and

X vx
0
Y = Rcm vy
(6)
Z vz

In order to estimate buoyancy, the state vector is augmented with the coordinates of the CoB
with respect to CoM in the body frame cm pcb , as well as the magnitude of the buoyancy force Fb .
During operation, Fb is considered as a constant,

Fb = 0 (7)
while cm pcb is moving slowly, in simulation section, both a static cm pcb and moving cm pcb will be
demonstrated. Also, assuming the vehicle to be neutrally buoyant, buoyancy should be equal and
opposite to gravity, which is a known parameter in simulation. The state vector is therefore:
 T
x = p q cm pcb Fb (8)

3
Specifically 16 variables are chosen for the state vector to represent Markov Process [7] as in Equa-
tion 9.
A(q)1 (B(q, p)p + T CR th + d + r ) t


031


0


xt+1 = xt +
1 sin() tan() cos() tan()
+ wt
(9)
T
0
cos() sin() t
0 sin()/ cos() cos()/ cos()
0 T
Rcm v t
System quasi-velocity vector in six direction p, coordinates of center of buoyancy expressed in
body frame cm pcb , buoyancy Fb , orientation of center of mass frame expressed in Euler angles
and coordinates of the origin of center of mass frame 0 xcm . To simulate actual process, additive
Gaussian noise is added to simulation with zero mean and proper variace. More detailed dynamic
model development process can be found in [9], [10].

3.2 Measurement
The inertial measurement unit (IMU) typically consisting of gyroscopes and accelerometer provides
angular rates in the body frame, as well as the inclination with respect to the earth frame. The
particular AUV model we are constructing is equipped with a a Photonic Inertial Navigation System
(PHINS) unit, which outputs position,heading, roll, pitch, depth, velocity, and heave. The high
accuracy inertial measurement capability is based on Fiber Optic Gyroscope technology coupled
with an embedded digital signal processor that runs an Extended Kalman Filte. This INS is aided
by a differential GPS, a Doppler Velocity Log (DVL) other than the classical depth sensor,in order
to improve the absolute measurement of the vehicle position Doppler Velocity Log(DVL) provides
linear velocity information. Further, access to controller inputs during thruster and ballast operation
is assumed.

3.3 Measurement model

Leveraging the self-adaptive EKF embedded in PHINS, sensory information from onboard INS
includes:

Tp0 transformation matrix from PHINS frame(CoM frame) to main frame;

0 vph linear velocity of PHINS expressed in main frame;

p ph angular velocity of PHINS expressed in PHINS frame;

p aph linear acceleration of PHINS expressed in PHINS frame;

Expression of each measurement is given below,

 0 
0 Rp x0p
Tp = = Tpc m Tcm
0
(10)
013 1
0
 
Rcm x0cm
under the assumption that PHINS frame coincides with CoM frame, = Tp0= 0
Tcm which
013 1
can be expressed by Euler Angles and CoM positions. From Jacobian matrix we calculated before,

4
 0 
vph
0 = J 0 (q, xph )p (11)
ph
row 4 to 6 is PHINS linear velocity expressed in inertial frame.
Angular velocity of PHINS in PHINS frame is computed by:

p ph
 T
ph = Rcm x y z (12)
The equations of linear acceleration are more complicated than the other three. Considering the
relationship between derivative of system configuration q and quasivelocity p, we got
6
d dq d dV dp X V dp
( ) = (V p) = p+V =( [V p]i )p + V (13)
dt dt dt dt dt i=1
qi dt
where V is a matrix transform velocity p into derivative of system configuration q, i.e.

  1 sin() tan() cos() tan()
0
V = E 03 03 Rcm andE = 0 cos() sin()
0 sin()/ cos() cos()/ cos()

Combining Equations ( 10,11,12,13), we get 21 equations to describe measurement model in the

Kalman Filter.

3.4 Filtering techniques

The nonlinear nature of the system dynamics in concern naturally motivates the application of the
Extended Kalman Filter, which is a straightforward adaptation of the Kalman Filter to nonlin-
ear systems. The standard EKF is initialized with a belief about the mean and covariance, and
each iteration consists of the prediction and update step. For propagating covariance during the
prediction step, we compute in closed form the Jacobian to the dynamic equation x = f (x, u):

 A1 A2 A3
f 

A(x, u) = = 046 I4 046
x (x,u)
A4 064 A5

with Ai s denoting the block components of the matrix. Similarly for the computation of Kalman
gain and the update step, the Jacobian to the measurement equation z = h(x) calculates to:

 C 1 0124 C 2 03
h  046 034 046 I3
C(x, u) = =
x (x,u) C3 034 C4 03

C5 C6 C7 03

Observation of this block structure is useful in alleviating the computation burden, but it is still
a very cumbersome task, and decision is made to use MATLABs symbolic solver to derive the
exact equations. Ideally, linearization should be made online at the current operation point, but
Simulink does not support the linearization command in the embedded function. In practice, online
linearization would also pose a forbidding computation cost to the filter.

5
 Figure 2: Block diagram of the simulation model in MATLAB Simulink

The UKF [13], on the other hand, samples 2n + 1 = 33 sigma points by the unscented trans-
formation which uses vectors from the Cholesky factorization of the covariance matrix. These
points are propagated directly through the nonlinear system and measurement equations, then to
be transformed back to the posterior distribution by preallocated weights. This procedure reduces
the singluarities involved in inverting covariance matrices and further introduces extra parameters
to tune the sigma points. Some literature propose schemes for tuning the unscented transformation,
but a general methodology does not exist because of the heuristic nature of the UKF design.

3.5 Simulation setup

The team use MATLAB Simulink to implement the process for better visualization and integration.
Since the system of concern is highly nonlinear, the models are first discretized and linearized before
fed into the filter. As proposed in Section 3.1, state vector contains 16 variables which makes
the linearization step more challenging. Symbolic computation was performed in this step using
MATLABs symbolic toolbox. In simulation step, additive Gaussian white noise is introduced into
the system to generated state and measurement output, with the diagonals of the noise covariance
matrices scaled to match the dimensions of the state vector.
Filter algorithms are implemented in the embedded MATLAB function, which is run throughout
the simulation at a fixed update rate (100Hz). By comparing the algorithms some insight is gained
on the drawbacks and advantages of each model. While they have different computation complexity
and robustness, the online performance of each algorithm in practice may also be analyzed.

3.6 Discussion of results

Given appropriate prior knowledge of the states, both EKF and UKF could yield good estimation
results. In all following figures, true states are shown in solid red while estimated states are rep-
resented by dashed blue lines. All values are plotted in SI units againtst time in seconds. The
buoyancy estimations are able to converge well within the simulation period, while the other states
are more accurately estimated because direct sensor information is available.
Under different initial conditions and inputs, though, the estimation performance is not guar-
anteed. When the CoB and buoyancy values are initiated with values that deviate more than 10

6
 4
10
0.2 1.41

0.1
1.4
0

-0.1 1.39

-0.2
0 5 10 15 1.38

0.6
1.37
0.4
1.36
0.2
1.35
0
0 5 10 15
1.34
0.6

0.4 1.33

0.2
1.32
0

-0.2 1.31
0 5 10 15 0 5 10 15
X cb [m] Buoyancy F b [N]

Figure 3: Center of buoyancy - EKF Figure 4: Buoyancy - EKF

10 4
2 1.425

0 1.42

-1
0 5 10 15
1.415
0.4

0.2
1.41
0

-0.2
0 5 10 15 1.405

1
1.4
0

-1

-2 1.395
0 5 10 15 0 5 10 15
X cb [m] Buoyancy F b [N]

Figure 5: Center of buoyancy - UKF Figure 6: Buoyancy - UKF

15 20

10 10

5 0

0 -10

-5 -20
0 5 10 15 0 5 10 15

4 2

1
2
0
0
-1

-2 -2
0 5 10 15 0 5 10 15

2 2

1
0
0
-2
-1

-4 -2
0 5 10 15 0 5 10 15
linear velocities [m/s] angular rates [rad/s]

Figure 7: linear velocities Figure 8: Angular velocities

7
percent from ground truth, estimation over 10 to 15 seconds (1000 to 1500 time steps) are liable
to diverge. In the EKF case, this is attributable to the highly nonlinear dynamics involved with
buoyancy that deteriorates the quality of the linearization. For the UKF however, which does not
suffer from linearization issues, there also occurred cases where it diverges from the truth over ex-
tended period of time. One possible explanation is that the sigma point transformation, despite its
capability to handle nonlinearities, could fail to track the evolution of distribution in non-Gaussian
processes.

4 Conclusion
In summary, the project constructs a nonlinear model of the AUV dynamics with focus on the
buoyancy variation. Vehicle and sensor dynamics are treated as Gaussian processes with additive
Gaussian white noise. A linearized version of the system is used to construct the EKF, while the
UKF makes a sigma point transformation and propagates the resulting samples directly through
the nonlinear equations. Performances of both filters showed similar accuracy in estimating speeds
and orientations, states that we havd direct sensor information on. For buoyancy and the center of
buoyancy, however, neither EKF nor UKF achieved stable performance, and the test is inconclusive.
Future work could be carried forth in two aspects. Firstly, new filtering techniques that has been
developed for the attitude sensing problem could be implemented. One of the concepts the team
looked into is the Multiplicative Extended Kalman Filter (MEKF), of which the aforementioned
IEKF is a generalization. Multiplicative EKF [12] differs from standard EKF in that (a) it makes
use of quaternion representation of the attitude which avoids the singularity issue and (b) it is a
indirect Kalman Filter that estimates the error state, measured by the true state with respect to a
reference state, where the system nominally stabilizes. By employing this error state Kalman Filter
structure, the filter operates in a small range of deviations where the linearization assumptions are
more likely to be valid. And since quaternions are used to represent attitude, the error state could
also be expressed as an error quaternion that exactly describes the deviation. Further, the team
could implement a simple control law and test the filter in closed loop. It is expected that given a
wisely chosen feedback, the states are more likely to remain in the working range of the filter, thus
providing a more accurate state information for control, which is the ultimate goal of the estimation
problem.

Some final thoughts After appreciating the work of other team projects, we gained additional
perspectives into the estimation problem.
1. To circumvent the singularity issue of the Extended Kalman Filter, the inverse formulation of
Information Filter may be employed, which avoids inverting the bulky covariance terms resulting
from the high dimension of state and measurement vectors in the prediction step.
2. Instead of initializing the filter with a single Gaussian distribution, multiple Gaussians could be
chosen over the range of possible state values to better capture the uncertainty in the initialization.
It is expected in a small number of iterations that the correct hypotheses would converge, so that
the Gaussian mixture could be pruned to preserve the most confident estimate.
3. During different modes of operation that the AUV is working in, different models could be
used to describe the dynamics of buoyancy. We could also have seperate filters that estimate the
vehicle motion as states and the buoyancy vector as parameters. By transforming the problem
into a dual estimation formulation, we could make use of the computational power more wisely, in

8
that buoyancy parameters could be estimated at a variable time interval, with focus on transition
periods when the vehicle is changing configuration and thus the center of buoyancy moves in the
body frame. In periods of regular motion the buoyancy estimator could be switched off, thus saving
power consumption, which is one of the original motivations.

References
[1] Kinsey, James C., Ryan M. Eustice, and Louis L. Whitcomb. A survey of underwater vehi-
cle navigation: Recent advances and new challenges IFAC Conference of Manoeuvering and
Control of Marine Craft. Vol. 88. 2006.

[2] Antonelli, Gianluca, and Alexander Leonessa, Underwater robots: motion and force control of
vehicle-manipulator systems, IEEE Control Systems Magazine 2008.

[3] Hussain, Nur Afande Ali, Mohd Rizal Arshad, and Rosmiwati Mohd-Mokhtar, Underwater
glider modelling and analysis for net buoyancy, depth and pitch angle control Ocean Engineering
38.16 (2011): 1782-1791.

[4] Ridao, P and Tiano, A and El-Fakdi, A and Carreras, M and Zirilli, A, On the identification of
non-linear models of unmanned underwater vehicles Control engineering practice 12.12 (2004):
1483-1499.

[5] Silvere Bonnabel, Philippe Martin, Erwan Salaun. Invariant Extended Kalman Filter: the-
ory and application to a velocity-aided attitude estimation problem 48th IEEE Conference on
Decision and Control, Dec 2009, Shanghai, China. pp.1297-1304, 2009

[6] Persson, S. Mikael, and Inna Sharf. Steady-state invariant Kalman filter for attitude and imbal-
ance estimation of a neutrally-buoyant airship. Proceedings of the AIAA Guidance, Navigation,
and Control Conference,(Boston, Massachusetts). 2013.

[7] Giacomo Marani, Song K. Choi, Junku Yuh Real-time center of buoyancy identification for
optimal hovering in autonomous underwater intervention Intelligent Service Robotics, 2010,
Volume 3, Number 3, Page 175

[8] Wan, Eric A., and Ronell Van Der Merwe. The unscented Kalman filter for nonlinear estimation
Adaptive Systems for Signal Processing, Communications, and Control Symposium 2000. AS-
SPCC. The IEEE 2000

[9] Fauske, Kjell Magne, Fredrik Gustafsson, and Oyvind Hegrenaes. Estimation of AUV dynamics
for sensor fusion Information Fusion, 2007 10th International Conference on. IEEE, 2007.

[10] McMillan, Scott, David E. Orin, and Robert B. McGhee. Efficient dynamic simulation of an
underwater vehicle with a robotic manipulator Systems, Man and Cybernetics, IEEE Transac-
tions on 25.8 (1995): 1194-1206.

[11] Coste-Maniere, Eve and Wang, Howard H and Rock, Stephen M and Peuch, Alexis and Perrier,
Michel and Rigaud, Vincent and Lee, Michael J. Joint evaluation of mission programming for
underwater robots Robotics and Automation, 1996. Proceedings., 1996 IEEE International
Conference on 3 (1996): 24922497.

9
[12] Burton, R., S. M. Rock, J. Springmann, and J. Cutler Online Attitude Determination of a
Passively Magnetically Stabilized Spacecraft Proceedings of the 23rd AAS/AIAA Space Flight
Mechanics Meeting, Kauai, HI, AAS/AIAA, Feb, 2013

[13] SJ Julier, JK Uhlmann New extension of the Kalman filter to nonlinear systems AeroSense97,
1997

10