Appendix I

Figure 1. Block diagramof 6-DOF system.

A. 2-DOF barycenter Stabilization Model
The dynamic model of 2-DOF barycenter system can be
developed from Lagranges equation, which considers the total
energy of all masses in the equivalent model with reference to
fishs body fixed frame. Hence, the dynamic model of system
can be written in 2-nd matrix form as
11 12
21 22
B
B
J J C
J J C
u u u
| | |
t t u u
t t | |
( ( ( ( (
=
( ( ( ( (
(


(A1)
where:
2 2 2 2 2
11
. . . . .
x x x x y y p p b
J m l m h m h m h Bh = + + + +
2
12
. .sin( )
y y
J m h u =
2
21
. .sin( )
y y
J m h u =
2 2 2 2 2 2
22
1
{ . . . . . .
2
y y x x x x y y p p b
J m l m l m h m h m h Bh = + + + + +

2 2 2
. .cos(2 ) . .cos(2 ) . .cos(2 )
x x y y x x
m h m h m l u u u + +

2 2
. .cos(2 ) . .cos(2 ) . . .sin(2 )}
p p b x x x
m h Bh m h l u u | + +

2 2 2
( . . ).sin ( ).sin(2 )
p p b
C B m h Bh
u u
u | = + +
2
1
.cos( ).{4. . . .cos( ) 2.[ 2. .
2
x x x x x
C B m h l m l
| |
u | = + +

2 2 2 2
3. . 3. . ( . . ).cos(2 )].sin( )}
p p b p p b
m h Bh m h Bh | u + + + +

.{ . .cos( ).cos( ) . .cos( ).sin( )
x x
g m l myly
u
t u | | u = +
( . . ).cos( ).sin( )}
x x p p
m h m h | u +
.{ . .cos( ) . .cos( ).sin( )
y y x x
g m l m l
|
t | u | = +
( . . ).cos( ).sin( ) . .sin( ).sin( )}
x x p p x x
m h m l m l | u | u + +
. . .cos( ).cos( )
B b
gBh
u
t | u =
. . .cos( ).cos( )
B b
gBh
|
t u | =
B. 2-DOF barycenter Control
In this paper we utilize Integral State Feedback (ISF)
controller to achieve zero steady state error of tracking reference
angles. The ISF controller design is based on linearized state
space model, which can be written as
x

= Bu Ax + (A2)
y =Cx (A3)
We insert two integrators state z
1
and z
2
in the forward path
between the error comparator and the plant. Therefore, the
system equations can be augmented. Thus new matrix
expression is given by
2 2 2 2 2 2 2 2
2 2 2 2 2 2 2 2
0 0 1
0 0 2
ref
x x x x
x x x x ref
A B x x u
C I z z u
u

(
( ( ( ( ( (
= + + (
( ( ( ( ( (

(A4)

2 2 2 2
[ 0 ]
x x c
y C x = (A5)
The block diagram of the closed-loop sub control system is
shown in Fig. 2.
} }
ref ref
,| u
out out
,| u

z z u

x x

Figure 2. Block diagramof ISF controller for control 2-DOF barycenter system.
The system is controllable, in order to place dominant poles
to the closed-loop system, we consider characteristic equation
c c c
K B A sI + =0. Consequently, the K
c
is state feedback gain
and the control law is given as

c c
u K x = (A6)
where:
[ ]
c I
K K K =
[ ]
c
x x z =

C. Mathematical Model of 6-DOF Motion
The kinetic relation between the fishs body fixed frame and
the fish motion in earth-fixed frame is expressed by
| |
T
x y z q | u = (A7)
| |
T
v u w p q r u = (A8)
( ) J v q q = (A9)
( ) 0
( )
0 ( )
R
J
T
q
O
O (
=
(
O

(A10)
where:
( )
c c s c c s s s s c c s
R s c c c s s s c s s s c
s c s c c
u | u | | | u
u | | u | u |
u u | u |
+ + (
(
O = + +
(
(

.
1
( ) 0
0
s t c t
T c s
s c c c
| u | u
| |
| u | u
O
(
(
O =
(
(

.
While, c(.) =cos(.), s(.) =sin(.), and t(.) =tan(.). Note that
( ) T
O
O is undefined for 2 u t = . However, when the
perturbations are small, such that (|, u, ) may be treated as
small angles
*
, the ( ) T
O
O can be approximated by p | =

, q u =

,
and r = . As a result, the equation of motion of the rigid body is
given by
( ) ( )
RB RB H
v v v gq + = M C (A11)
where:
0 0 0 . 0
0 0 . 0 .
0 0 0 . 0
0 . 0 0
. 0 . 0 0
0 . 0 0
G
G G
G
RB
G x zx
G G y
G zx z
m mz
m mz mx
m mx
M
mz I I
mz mx I
mx I I
(
(

(
(
= (

(
(

(
(



41 51 61
42 52 62
43 53 63
41 42 43 54 64
51 52 53 54 65
61 62 63 64 65
0 0
0 0 0
0 0 0
u uu
RB
p
q
r rr
C C u c c c
c c c
c c c
C
c c c C c c
c c c c C c
c c c c c C C r
( +
(

(
(

(
=

(
(

(
(
+




41
. .
G
c mz r = ,
54
. .
zx zr
c I p I r =

42
. c mw = ,
61
.( . )
G
c m x r v = +

43
.( . )
G
c m z p v = ,
62
. c mu =

51
.( . )
G
c m x q w = ,
63
. .
G
c mx p =

52
.( . . )
G G
c m z r x p = + ,
63
. .
G
c mx p =

53
.( . )
G
c m z q u = + ,
65
. .
zx x
c I r I p =

| |
1 2
| 0 0 |
H H H F
F
| u
t t t ( = =




( ).sin( )
( ).cos( ).sin( )
( ).cos( ).cos( )
( )
. .cos( ).sin( )
. .sin( )
0
G
G
B W
B W
B W
g
z B
z B
u
u |
u |
q
u |
|
(
(

(
(
=
(
(
(
(
(


While, | |
G G G G
r x y z = is the vector from the body origin
to the center of gravity. For underwater vehicles, set
G
x =0 at
the origin and
G
y =0 provided port-starboard symmetry and. In
conclusion, the block diagram of 6-DOF motion in North-East-
Down frame (NED frame) and its control system is depicted in
Fig. 1.

*
Michael V. Cook, Flight Dynamics Principle, ELSEVIER, USA, 2007.