International Journal of Instrumentation and Control Systems (IJICS) Vol.2, No.2, April 2012 
DOI : 10.5121/ijics.2012.2204                                                                                                                       45 
 
AN OPTIMAL CONSENSUS TRACKING CONTROL 
ALGORITHM FOR AUTONOMOUS UNDERWATER 
VEHICLES WITH DISTURBANCES 
 
Jian Yuan1 Wen-Xia Zhang2 and Zhou-Hai Zhou1 
 
1Institute of Oceanographic Instrumentation of Shandong Academy of Science, Qingdao, 
China 
 
jyuanjian801209@163.com 
 
2 Mechanical & Electrical Engineering Department, Qingdao College, Qingdao,China 
 
dasinusheng@126.com 
 
 
ABSTRACT 
 
The optimal disturbance rejection control problem is considered for consensus tracking systems affected by 
external persistent disturbances and noise. Optimal estimated values of system states are obtained by 
recursive filtering for the multiple autonomous underwater vehicles modeled to multi-agent systems with 
Kalman filter. Then the feedforward-feedback optimal control law is deduced by solving the Riccati 
equations and matrix equations. The existence and uniqueness condition of feedforward-feedback optimal 
control law is proposed and the optimal control law algorithm is carried out. Lastly, simulations show the 
result is effectiveness with respect to external persistent disturbances and noise. 
 
KEYWORDS 
 
Autonomous Underwater Vehicles; Consensus Tracking; Optimal Disturbance Rejection 
 
1. INTRODUCTION 
 
The AUVs formation control is a typical problem of multi-robot coordination and cooperation. 
The coordinated control of multiple AUVs can significantly improve many applications including 
ocean sampling, imaging, and surveillance abilities. The large-scale multiple AUVs system is 
modelled to multi-agent system to study the consensus problem on their spatial location. Wang 
and Xiao [1] proposed a finite-time formation control framework for large-scale multi -agent 
system. They divide the formation information into two types: global information and local 
information, in which the global information can decide the formation shape and only the leader 
can obtain such the global information; followers can only get local information. This framework 
can reduce the amount of communication between the agents. And then they design a nonlinear 
consensus protocol, and apply it to the time-invariant, time-varying and trajectory-tracking 
control. Wang and Hong [2] propose some types of consensus control algorithms for the first-order 
dynamic system with variable coupling topology. They design a finite-time consensus protocol 
and give its non-smooth controller using time-invariant Lyapunov function and graph theory tool. 
Further, they propose a non-smooth time-invariant consensus algorithm for a second-order 
dynamic systems. They demonstrate the existence of the finite-time control law using Lyapunov 
functions and graph theory in [3]. Wang and Chen [4] study the consensus problem of continuous-
International Journal of Instrumentation and Control Systems (IJICS) Vol.2, No.2, April 2012 
46 
 
time multi-agent with communication time delay. They design a class of continuous but non-
smooth finite-time controller which ensures that the multi-agent systems with time delay reach a 
consistent state in finite time. The underwater environment is extremely complex, of strong noise 
and disturbance, resulting in multi-agent status subjecting to these external disturbances. 
Reducing or overcoming the impact of these disturbances and noise on multi-agent formation has 
important theoretical and practical significance. In this paper, the feedforward-feedback optimal 
tracking control problem for the multi-agent system with external disturbance and noise under a 
given performance index based on the Kalman filter is studied. Optimal estimated values of 
system states are obtained by recursive filtering for the multiple autonomous underwater vehicles 
modelled to multi-agent systems with Kalman filter. Then the feedforward-feedback optimal 
control law is deduced by solving the Riccati equations and matrix equations. The existence and 
uniqueness condition of feedforward-feedback optimal control law is proposed and the optimal 
control law algorithm is carried out. Simulations show the result is effectiveness with respect to 
external persistent disturbances and noise. 
 
2.  PROBLEM DESCRIPTION 
 
The consensus algorithm with external disturbance is described as 
 
0
0
( )
(
( )
( ))
( )
(
( )
( ))
( )
t
j
i
i
ij
j
i
vi
i
i
i
i
v
N
t
a
t
t
a
t
a
t
t
t
Î
=
-
+
+
-
+
å
&x
x
x
v
x
x
m
                                       (1) 
where 
( )
i t
v
 denotes external disturbance and satisfies 
( )
( ( ))
i
i
t
f
t
=
&v
v
, 
0( )t
x
 is the desired 
trajectory and satisfies
0
0
 
( )
(
( ))
t
f
t
=
&x
x
,
( )
i t
m
denotes Gaussian white noise where 
0
0, 
0
i
vi
a
a
>
>
. without loss of generality, the individual AUV dynamics of multi-AUV systems 
with disturbances described as follows: 
1
2
0
( )
( )
( )
( )
( )
( )
( )
( )
( )
(0)
i
ii
i
ij
j
i
i
i
i
i
j
i
i
i
i
i
i
i
t
t
t
t
t
t
t
t
t
¹
=
+
+
+
+
=
+
=
å
&x
A x
A x
B u
B w
m
y
C x
n
x
x
,i
N
Î
                                            (2) 
where
( )
i t
x
 denotes the states of AUV i  and 
( )
p
i t
R
Î
x
.
( )
j t
x
denotes the states of AUV j  
surrounding AUV i . 
( )
s
i t
R
Î
y
 denotes the system output.
( )
q
i t
R
Î
u
 denotes the input of 
AUV i . 
( )
r
i t
R
Î
w
 denotes the disturbances, 
( )
p
i t
R
Î
m
denotes the process noise, 
( )
p
i t
R
Î
n
 denotes 
the measurement noise.
p p
ii
R ´
Î
A
,
p p
ij
R ´
Î
A
,
1
p q
i
R ´
Î
B
, 
2
p r
i
R ´
Î
B
and 
s p
i
R ´
Î
C
 are 
constant matrice of appropriate dimensions.
ij
A  denotes the corresponding matrix between 
AUV i and AUV j . 
The output trajectory-tracking 
i%y  of virtual leader which the system output
iy tracks is described 
as 
International Journal of Instrumentation and Control Systems (IJICS) Vol.2, No.2, April 2012 
47 
 
( )
( )
( )
( )
( )
( )
i
i
i
zi
i
i
i
zi
t
t
t
t
t
t
=
+
=
+
&
%
z
F z
m
y
H z
n
                                                                                                         (3) 
where 
l
i
R
Î
z
,
s
i
R
Î
%y
,
l l
i
R ´
Î
F
and 
s l
i
R ´
Î
H
are constant matrice of appropriate dimensions. 
And (
,
)
i
i
F H
 is observable. 
( )
l
zi t Î 
m
denotes the process noise of the external system, 
( )
s
zi t
R
Î
n
 is the measurement noise of the external system. So the equation (2) is rewritten as 
1
2
0
( )
( )
( )
( )
( )
( )
( )
( )
(0)
t
t
t
t
t
t
t
t
=
+
+
+
=
+
=
&x
Ax
B u
B w
m
y
Cx
n
x
x
                                                                                      (4) 
 where 
T
T
T
T
1
2
( )
Np
N
t
R
é
ù
=
Î
ë
û
L
x
x
x
x
, 
T
T
T
T
1
2
( )
Np
N
t
R
é
ù
=
Î
ë
û
L
m
m
m
m
 
T
T
T
T
1
2
( )
Nq
N
t
R
é
ù
=
Î
ë
û
L
u
u
u
u
,
T
T
T
T
1
2
( )
Nr
N
t
R
é
ù
=
Î
ë
û
L
w
w
w
w
 
T
T
T
T
1
2
( )
Ns
N
t
R
é
ù
=
Î
ë
û
L
y
y
y
y
,
T
T
T
T
1
2
( )
Ns
N
t
R
é
ù
=
Î
ë
û
L
n
n
n
n
 
11
1
1
N
Np Np
N
NN
R
´
é
ù
ê
ú
ê
ú
=
Î
ê
ú
ê
ú
ë
û
L
M
O
M
L
A
A
A
A
A
,
11
1
1
Np Nq
N
R
´
é
ù
ê
ú
ê
ú
=
Î
ê
ú
ê
ú
ë
û
O
B
B
B
 
21
2
2
Np Nr
N
R
´
é
ù
ê
ú
ê
ú
=
Î
ê
ú
ê
ú
ë
û
O
B
B
B
,
1
Ns Np
N
R
´
é
ù
ê
ú
ê
ú
=
Î
ê
ú
ê
ú
ë
û
O
C
C
C
 
We define the disturbance 
( )t
w
as 
( )
( )
( )
w
t
K
t
t
=
+
&w
w
m
                                                                                                                (5) 
The output trajectory-tracking %y  of virtual leader which the system output y  tracks is described 
as 
( )
( )
( )
( )
( )
( )
z
z
t
t
t
t
t
t
=
+
=
+
&
%
z
Fz
m
y
Hz
n
                                                                                                                  (6) 
where 
Nl
R
Î
z
, 
( )
Nl
z t
R
Î
m
,
Ns
R
Î
%y
,
( )
Ns
z t
R
Î
n
,
Nl Nl
R
´
Î
F
and 
Ns Nl
R
´
Î
H
are constant 
matrices of appropriate dimensions. And (
,
)
F H is observable. 
The filtered system by optimal state filtering is described as 
International Journal of Instrumentation and Control Systems (IJICS) Vol.2, No.2, April 2012 
48 
 
1
2
0
ˆ
ˆ
ˆ
ˆ
( )
( )
( )
( )
ˆ
ˆ
( )
( )
ˆ(0)
t
t
t
t
t
t
=
+
+
=
=
&x
Ax
B u
B w
y
Cx
x
x
                                                                                               (7) 
where 
T
T
T
T
1
2
ˆ
ˆ
ˆ
ˆ
( )
Np
N
t
R
é
ù
=
Î
ë
û
L
x
x
x
x
,
T
T
T
T
1
2
ˆ
ˆ
ˆ
ˆ
( )
Nq
N
t
R
é
ù
=
Î
ë
û
L
u
u
u
u
 
T
T
T
T
1
2
ˆ
ˆ
ˆ
ˆ
( )
Nr
N
t
R
é
ù
=
Î
ë
û
L
w
w
w
w
,
T
T
T
T
1
2
ˆ
ˆ
ˆ
ˆ
( )
Ns
N
t
R
é
ù
=
Î
ë
û
L
y
y
y
y
 
The external disturbance system is described as 
ˆ
ˆ
( )
( )
t
K
t
=
&w
w
                                                                                                                            (8) 
The external trajectory-tracking system is described as 
 ˆ
ˆ
( )
( )
ˆ
ˆ
( )
( )
t
t
t
t
=
=
&
%
z
Fz
y
Hz
                                                                                                                             (9) 
where ˆ
Nl
R
Î
z
, ˆ
Ns
R
Î
%y
. Choose Infinite horizon quadratic performance index as 
(
)
(
)
T
T
0
T
T
0
( )
( )
( )
( ) d
ˆ
ˆ
ˆ
ˆ
  
( )
( )
( )
( ) d
J
E
t
t
t
t
t
t
t
t
t
t
¥
¥
é
ù
=
+
ê
ú
ë
û
=
+
ò
ò
e
Qe
u
Ru
e
Qe
u
Ru
                                                                              (10) 
where Q and R  are positive definite matrix of appropriate dimensions. ( )t
e
and ˆ( )t
e
are output 
error and estimated value of the output error, described as 
( )
( )
( )
ˆ
ˆ
ˆ
( )
[ ( )]
( )
( )
t
t
t
t
E
t
t
t
=
-
=
=
-
%
%
e
y
y
e
e
y
y
                                                                                                   (11) 
The optimal formation control is to solve the optimal tracking control law
*ˆ ( )t
u
 to make J  
obtain the minimal value. 
 
3. DESIGN ON OPTIMAL FORMATION CONTROL 
 
In this section we focus on the designing on the optimal formation control law of AUVs with 
disturbance and noise effects. First, we give the following theorem: 
 
Theorem 1 Considered the optimal tracking control problem of disturbed multiple AUVs system 
with equation (4) and (6) under the performance indicators (10), the optimal formation control 
law only exists and is represented by the following formula: 
(
)
*
1
1
1
2
ˆ
ˆ
ˆ
ˆ
( )
( )
( )
( )
T
t
t
t
t
-
= -
+
+
u
R B
Px
P z
P w
                                                                           (12) 
International Journal of Instrumentation and Control Systems (IJICS) Vol.2, No.2, April 2012 
49 
 
where P  is  the unique semi-positive definite solution of the following matrix algebraic 
equation: 
T
T
+
-
+
= 0
A P
PA
PSP
C QC
                                                                                           (13) 
where 
1P is the unique solution of the following matrix algebraic equation: 
T
1
1
1
-
-
+
= 0
T
PF
PSP
C QH
A P
                                                                                         (14) 
where 
2P  is the unique solution of the following matrix algebraic equation: 
T
2
2
2
2
0
-
+
=
A P + P K
PSP
PB
                                                                                                (15) 
where
1
T
1
1
-
=
S
B R B
.  
Proof: A necessary condition of the optimal formation control under performance indicators (10)
for system (4) and (6)for leads to solving the following two point boundary value: 
T
T
T
2
0
0
ˆ
ˆ
( )
( )
( )
( )
ˆ
ˆ
ˆ
( )
( )
( )
( ) ,   
(
)
ˆ(0)
t
t
t
t
t
t
t
t
t
t
l
-
=
-
+
=
-
+
< < ¥
¥
=
=
0
&
&
C QCx
C QHz
A
x
Ax
S
B w
x
x
λλλλ
λλλλ
λλλλ
                                                                       (16) 
Its optimal formation control law is described as 
1
T
1
ˆ( )
( )
t
t
-
= -
u
R B λλλλ
                                                                                                                (17) 
For solving the two point boundary value(16), we define 
1
2
ˆ
ˆ
ˆ
( )
( )
( )
( )
t
t
t
t
=
+
+
Px
P z
P w
λλλλ
                                                                                                (18) 
Take the derivative of both sides of(17), and take the second equation of (16) and (5) into 
account, we obtain 
1
1
2
2
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
( )
( )
( )
( )
( )
( )
( )
t
t
t
t
t
t
t
=
+
+
+
+
+
&
&
&
&
&
&
&
Px
Px
P z
P z
P w
P w
λλλλ
                                                      (19) 
Compare (19) with the first equation of(16), we obtain: 
(
)
(
)
(
)
T
T
T
1
1
T
2
2
2
2
ˆ
ˆ
( )
( )
ˆ ( )
t
t
t
+
-
+
+
-
-
+
+
-
+
= 0
T
A P
PA
PSP
C QC x
PF
PSP
C QH
A P z
A P + P K
PSP
PB
w
                             (20) 
For all ˆ( )t
x
, ˆ( )t
z
and ˆ ( )t
w
, (19) establishes all the time. So we obtain the Ricatti  matrix 
algebraic equation (13),  (14) and (15)For equation (13) is the matrix algebraic equation about P  
International Journal of Instrumentation and Control Systems (IJICS) Vol.2, No.2, April 2012 
50 
 
with unique semi-positive definite solution, we take it into matrix algebraic equation(14)and(15) 
to solve the unique 
1P and 
2P . 
When the unique P ,
1P and
2P are obtained, ( )t
λλλλ
is solved. Furthermore, by (18) we determine 
the optimal formation control law(12). 
For the optimal tracking control problem described by (4) and(6), the designing procedure of the 
above algorithm is as follows: 
Algorithm: (the optimal consensus tracking control algorithm) 

Solving the expected output ˆ( )t
%y
through(9); 

Solving 
( )t
P
,
1( )t
P
and
2( )t
P
 through equations(13), (14) and (15), respectively; 

Calculating ˆ( )t
x
 through(16); 

Calculating ˆ( )t
u
 through(12); 

Calculating ˆ( )t
e
 through(11); 

Calculating J  through(10). 
According to the above system description and optimal consensus tracking control algorithm, we 
obtain the control block of the optimal consensus tracking control illustrated in Fig.1. 
⊗
1
S
⊗
1
T
−
−R B
1
B
A
2
B
P
2P
1
S
F
ˆ&x
ˆx
ˆu
ˆ&z
ˆz
ˆ%y
H
C
ˆy
1P
1
S
K
ˆ&w
ˆw
 
Figure 1. System block of optimal consensus tracking control system with disturbance and noise 
 
4. SIMULATIONS 
 
Considering the multiple AUVs system with noise and disturbance described by equations(4), 
(5) and (6) as follows: 
International Journal of Instrumentation and Control Systems (IJICS) Vol.2, No.2, April 2012 
51 
 
[
]
T
1
0
3
2
0.1
0
( )
( )
( )
( )
( ), (0)
0
0
0
1
4
5
0
1
1
2
( )
( )
( )
3
4
t
t
t
t
t
t
t
t
ì
é
ù
é
ù
é
ù
-
-
ï
ê
ú
ê
ú
ê
ú
+
=
ï
ê
ú
ê
ú
ê
ú
-
ï
ë
û
ë
û
ë
û
í
é
ù
ï
ê
ú
=
+
ï
ê
ú
ï
ë
û
î
&x
=
x
+
u
+
w
m
x
y
x
n
 
[
]
T
0.1
1
( )
( )
( ), (0)
0.4
0.5
1
0.2
0.2
4
( )
( )
( )
2
5
z
z
t
t
t
t
t
t
ì
é
ù
-
ï
ê
ú
+
=
ï
ê
ú
-
-
ï
ë
û
í
é
ù
ï
ê
ú
=
+
ï
ê
ú
ï
ë
û
î
&
%
z
=
z
m
z
y
z
n
 
[
]
T
5
1
( )
( )
( ), (0)
0.3
0.4
1
9
w
t
t
t
é
ù
-ê
ú
+
=
ê
ú
-
-
ë
û
&w
=
w
m
w
 
The covariance 
m
Q and 
n
Q of 
( )t
m
and ( )t
n
are
2
n
=
=
m
Q
Q
, respectively. The covariance 
m
Q
and 
n
Q of 
( )
z t
m
and 
( )
z t
n
are
2
z
zn
=
=
m
Q
Q
, respectively. And the covariance 
w
Q  of 
( )
w t
m
is
2
w =
Q
. The total simulation time is 
300(s)
T =
.we compare the Kalman filter 
based feedforward-feedback tracking control law to the classical feedforward-feedback 
tracking control law.  The result comparison is showed in Fig.2 to Fig.7. 
0
50
100
150
200
250
300
-100
-80
-60
-40
-20
0
20
40
60
80
100
Error1
Time (Sec)
 
 
without filter
with kalman filter
0
50
100
150
200
250
300
-150
-100
-50
0
50
100
150
Error2
Time (Sec)
 
 
without filter
with kalman filter
 
Figure 2. Comparison on error-1                              Figure 3. Comparison on error-2 
0
50
100
150
200
250
300
-25
-20
-15
-10
-5
0
5
10
15
20
x1
Time (Sec)
 
 
without filter
with kalman filter
0
50
100
150
200
250
300
-40
-30
-20
-10
0
10
20
30
40
x2
Time (Sec)
 
 
without filter
with kalman filter
 
Figure 4. Comparison on state-1                          Figure 5. Comparison on stater-2 
International Journal of Instrumentation and Control Systems (IJICS) Vol.2, No.2, April 2012 
52 
 
0
50
100
150
200
250
300
-80
-60
-40
-20
0
20
40
60
80
u1
Time (Sec)
 
 
without filter
with kalman filter
0
50
100
150
200
250
300
-40
-30
-20
-10
0
10
20
30
40
u2
Time (Sec)
 
 
without filter
with kalman filter
 
Figure 6. Comparison on control-1                       Figure 7. Comparison on control-2 
From the simulation result comparison, it is ensured that the Kalman filter based optimal 
consensus tracking control algorithm is effectiveness. The system tracks the expected the external 
system(3) in higher precision under the noise and disturbance. The control law is of better noise 
rejection and the tracking error is smaller than the classic feedforward-feedback tracking control 
law. 
 
5.  CONCLUSIONS 
 
Because the system states of consensus protocol is polluted by the noise, we use Kalman filter to 
filter the noised controlled system to obtain the optimal estimated values of each AUV states, in 
order to achieve coordination control of multi-AUVs in noisy environment. Considering the 
controlled system affected by environmental noise and external disturbances, we design a Kalman 
filter-based feedforward and feedback optimal consensus tracking protocol.  
  
REFERENCES 
 
[1]  F. Xiao, L. Wang, J. Chen, Y. Gao, (2009) “Finite-time formation control for multi-agent systems”, 
Automatica, Vol. 45, No. 11, pp 2605-2611. 
[2] 
 X. Wang, Y. Hong, (2010) “Distributed finite-time χ-consensus algorithms for multi-agent systems 
with variable coupling topology”, Journal of Systems Science and Complexity, Vol.23, No.3, pp 209-
218. 
[3]  X. Wang, Y. Hong, (2008) “Finite-Time Consensus for Multi-Agent Networks with Second-Order 
Agent Dynamics”, Proceedings of the 17th World Congress of the International Federation of 
Automatic Control, Seoul, Korea, July 6-11. 
[4]  L. Wang, Z. Chen, Z. Liu, and Z. Yuan, (2009) “Finite-time agreement protocol design of multi-agent 
systems with communication delays”, Asian Journal of Control, Vol.11, No.3, pp 281-286. 
 
Authors 
Dr. Yuan is Associate Professor, Institute of Oceanographic Instrumentation of 
Shandong Academy of Science, Qingdao, China. His current research areas are:  
Multiple Agents System based control, Networked Control System and Nonlinear 
Filtering. He has published over 20 papers in international journals and international 
conferences.