Formation Control and Network Localization via
Distributed Global Orientation Estimation in 3-D

arXiv:1708.03591v1 [] 10 Aug 2017

Byung-Hun Lee† and Hyo-Sung Ahn†

Abstract—In this paper, we propose a novel distributed formation control strategy, which is based on the measurements of
relative position of neighbors, with global orientation estimation
in 3-dimensional space. Since agents do not share a common
reference frame, orientations of the local reference frame are not
aligned with each other. Under the orientation estimation law,
a rotation matrix that identifies orientation of local frame with
respect to a common frame is obtained by auxiliary variables. The
proposed strategy includes a combination of global orientation
estimation and formation control law. Since orientation of each
agent is estimated in the global sense, formation control strategy
ensures that the formation globally exponentially converges to
the desired formation in 3-dimensional space.

I.

I NTRODUCTION

Cooperative control of multiple autonomous agents has
attracted a lot of attention due to its practical potential in
various applications. Formation control of mobile autonomous
agents is one of the most actively studied topics in distributed
multi-agent systems. Depending on the sensing capability
and controlled variables, various problems for the formation
control have been studied in the literature [1].
Consensus-based control law for the desired formation shape
is known as the displacement-based approaches. In this approach, agents measure the relative positions of their neighbors
with respect to a common reference frame. Then, agents
control the relative position for the desired formation. In the
literature, it has been known that this method achieves global
asymptotic convergence of the formation to the desired one
[2]–[5]. Fundamental requirement for the displacement-based
approach is that all agents share a common sense of orientation. Therefore, local reference frames need to be aligned with
each other. Oh and Ahn [5] found the displacement-style formation control strategies based on orientation estimation, under
the distance-based setup (i.e., local reference frames are not
aligned with each other). In this approach, agents are allowed
to align the orientation of their local coordinate systems by
exchanging their relative bearing measurements. By controlling
orientations to be aligned, the proposed formation control
law allows agent to control relative positions simultaneously.
Similar strategy using the consensus protocol is found in [6]. In
the paper, control objective is to reach a desired formation with
specified relative positions and orientations in the absences of
a global reference frame. These strategies ensure asymptotic
convergence of the agents to the desired formation under the
† The authors are with School of Mechatronics, Gwangju Institute
of Science and Technology, Gwangju, Korea. E-mail: bhlee@gist.ac.kr;
hyosung@gist.ac.kr.

assumption that interaction graph is uniformly connected and
initial orientations belong to an open interval [0, π) [5], [6].
In our previous work [7], we proposed a novel formation
control strategy via the global orientation estimation. The
orientation of each agent is estimated based on the relative
angle measurement and auxiliary variables assigned in the
complex plane. Unlike the result of [5], [6], in this new
approach, consensus property, which is based on the Laplacian
matrix with complex adjacency matrix, is applied to allow the
global convergence of the auxiliary variables to the desired
points. In this way, we showed that the global convergence
of formation to desired one for the multi-agent system having
misaligned frames could be achieved.
In this paper, we extend the result of [7] to 3-dimensional
space. Even though we share the philosophy of the algorithm
of [7], there are intrinsic distinctions from [7] as follows. First,
in 2-dimensional space, an unique auxiliary variable for each
agent is required to estimate the orientation of the local frame.
However, for the estimation of orientation in 3-dimensional
system, we require more than two auxiliary variables for each
agent, since at least two auxiliary variables are required for
obtaining the rotation matrix in SO(3). Second, while the
direction of the unique auxiliary variable can be considered
as the orientation in 2-dimensional space, the orientation in
3-dimensional space can not be determined directly from
the two different directions of auxiliary variables. Thus, we
need to have additional processes compared to the case of
2-dimensional space for obtaining the estimated orientation.
The novel method proposed in this paper uses the consensus
protocol with auxiliary variables to estimate a rotation matrix
in SO(3). This consensus protocol achieves global convergence
of auxiliary variable, since configuration space of auxiliary
variable is a vector space instead of nonlinear space like
circle. We show that auxiliary variables construct the rotation
matrix that identifies the estimated orientation. For the case
of general n-dimensional space, the estimation method for
unknown orientation is proposed in our previous work [8]. In
this paper, we modify the estimation method for the case of 3dimensional space, and combine it with the formation control
law for the desired formation shape. The estimated direction
of the frame is used to compensate misaligned frames of each
agent in the formation system. Consequently, the formation
control law with the estimated coordinate frame achieves a
global convergence of multi-agent system to desired shape in
3-dimensional space, under the distance-based setup. Third,
we also show that the proposed strategy can be applied to
the localization problem of sensor networks equipped with
displacement and orientation measurement sensors. We design

the position estimation law for the given displacement measurements. Then, the position estimation law is combined with
the orientation estimation law. It thus shows that convergence
property of localization algorithm is identical to the one of
formation control algorithm.
The outline of this paper is summarized as follows. In
Section II, preliminaries are described. In Section III, an idea
for estimation law is described and stability of estimation law
is analyzed. Convergence property of the formation control
strategy with orientation estimation is then analyzed in Section
IV. By using the proposed method, localization algorithm in
networked systems is analyzed in Section V. Simulation results
are provided in Section VI. Concluding remarks are stated in
Section VII.
II. P RELIMINARIES
In this paper, we use the following notation. Given N
column vectors x1 , x2 . . . xN ∈ Rn , x denotes the stacking of
the vectors, i.e. x = (x1 , x2 , . . . , xN ). The matrix In denotes
the n-dimensional identity matrix. Given two matrices A and
B, A ⊗ B denotes the Kronecker product of the matrices. The
exterior product, also known as wedge product, of two vectors
u and v is denoted by u ∧ v.
A. Consensus Property
Given a set of interconnected systems, the interaction topology can be modeled by a directed weighted graph G =
(V, E, A), where V and E denote the set of vertices and
edges, respectively, and A is a weighted adjacency matrix
with nonnegative elements akl which is assigned to the pair
(k, l) ∈ E. Let Ni denote neighbors of i. The Laplacian
matrixPL = [lij ] associated to G = (V, E, A) is defined as
lij = k∈Ni aik if i = j and lij = −aij otherwise. If there is
at least one node having directed paths to any other nodes, G
is said to have a rooted-out branch. In the following, we only
consider the graph G with a rooted-out branch. The following
result is borrowed from [9].
Theorem 2.1: Every eigenvalue of L has strictly positive
real part except for a simple zero eigenvalue with the corresponding eigenvector [1, 1, . . . 1]T , if and only if the associated
digraph has a rooted-out branch.
Let us consider N -agents whose interaction graph is
given by G = (V, E, A). A classical consensus protocol in
continuous-time is as follows [10]:
X
ẋi (t) =
aij (xj (t) − xi (t)), ∀i ∈ V
(1)
j∈Ni

n

where xi ∈ R . Using the Laplacian matrix, (1) can be
equivalently expressed as
ẋ(t) = −L ⊗ In x(t)

(2)

The following result is a typical one.
Theorem 2.2: The equilibrium set Ex = {x ∈ RnN : xi =
xj , ∀i, j ∈ V} of (2) is globally exponentially stable if and
only if G has a rooted-out branch. Moreover, the state x(t)
converges to a finite point in Ex .

Fig. 1: Twenty particles on unit circle with all-to-all sensing
graph (left figure) and the simulation result(right figure) of (4)
are demonstrated.

B. Orientation Alignment
We assume that relative orientation and relative displacement measurements are only available in the formation control
problem. The orientation of local reference frame can be
controlled to be aligned with others. For the alignment of
orientation, we can consider the consensus algorithm of (1)
based on the relative quantities of states i.e. (xj −xi ), j ∈ Ni .
In the case of SO(2), the orientation of i-th agent is denoted
by θi ∈ S1 . Let us denote the displacement between θj and θi
as θji . Note that θji = θji − 2π and in general, θji 6= θj − θi .
The consensus protocol in the unit circle space is as follows
[5], [6]:
X
θ̇i =
aij θji , i ∈ V
(3)
j∈Ni

In Euclidean space, the convex hull of a set of points in
n-dimensions is invariant under the consensus algorithm of
(1). The permanent contraction of this convex hull allows to
conclude that the agents end up at a consensus value [11], [12].
In unit circle space, convergence property of the consensus
algorithm based on the relative quantities of states is analyzed
in similar way, while the convergence of states to a consensus
value is not possible for all initial value of states. In other
words, there is a subset such that convex hull of the set of
points is invariant. In the following example, it will be shown
that there is a domain which does not achieve the contraction
of the convex hull under the consensus algorithm (3).
• Example of anti-synchronization: Let us suppose that
twenty agents on the unit circle have all-to-all interaction
topology shown in Fig. 1. Consensus protocol based on
the displacement is designed as follows :
X
θ̇i =
θji , i ∈ {1, 2, . . . , 20}
(4)
j∈Ni

The initial values of θi are assigned on out of range
of π as the pattern which is shown in Fig.1. The
simulation result of (4) in Fig.1 shows that consensus
is not guaranteed on unit circle space.
In the unit circle space, a sufficient condition for the permanent
contraction of a convex hull is stated in the following theorem.
Theorem 2.3 ( [5], [13]): Consider the coupled oscillator
model (3) with a connected graph G(V, E, A). Suppose that

a convex hull of all initial values is within an open semi
circle. Then, this convex hull is positively invariant, and
each trajectory originating in the convex hull achieves an
exponential synchronization.
The fundamental difference between a consensus protocol
(1) in Euclidean space and another protocol (3) in unit circle
space is the non-convex1 nature of configuration spaces like
circle or sphere. For this reason, a global convergence analysis of the consensus algorithm in nonlinear space is quite
intractable and at least very dependent on the communication
graph [12].
In this paper, instead of controlling the orientation to be
aligned, we estimate the orientation by using auxiliary variables defined on the higher dimensional vector spaces rather
than unit circle space. Although consensus algorithm plays still
important role in the proposed method, unlike the alignment
law (3), the proposed method achieves global convergence of
the estimated variables. This is described in the next section.
C. Problem Statement
Consider N single-integrator modeled mobile agents in the
space: ṗi = ui , i = 1, . . . N, where pi ∈ R3 and ui ∈ R3
denote the position and control input, respectively, of agent i
with P
respect to the global reference frame, which is denoted
by g . Following the notions of [15], we assume that agent
i maintains its own local reference frame with the origin at
pi . An orientation of i-th local reference frame with respect to
the global reference frame is identified by a proper orthogonal
matrix Ci ∈ SO(3) as illustrated in Fig. 2. By adopting a
notation in which superscripts are used to denote reference
frames, the dynamics of agents are described as ṗii = uii , i =
1, . . . N, where pii ∈ R3 and uii ∈ R3 denote the position
P and
control input, respectively, of agent i with respect to i . For
a weighted directed graph G = (V, E, A), agent i measures
the
Prelative positions of its neighboring agents with respect to
i
as:
piji := pij − pii , j ∈ Ni , i ∈ V.

(5)

We note that pj − pi = Ci−1 (pij − pii ), because the local
reference frames are oriented as much as the rotation matrix
Ci from the global reference frame.
In this paper, we attempt to estimate Ci by using the relative
orientation measurement. Then, the estimated orientation is
used to calibrate Ci for the formation control. Let the desired
formation p∗ = (p∗1 , . . . , p∗N ) ∈ R3N be given. The formation
problem is then stated as follows:
Problem 2.1: Consider N -agents modeled by (1). Suppose
G = (V, E, A) is the interaction
graph of the agents. For a
P
common reference frame a , design a control law such that
paj − pai → p∗j − p∗i as t → ∞ for all i, j ∈ V based on the
measurements such as the relative displacement and relative
orientation, which are measured in local coordinate frames of
each agent.
1 Non-convexity is the opposite of convexity. Convexity property implies that
the future, updated value of any agent in the network is a convex combination
of its current value as well as the current values of its neighbors. It is thus
clear that max{x1 , x2 , . . . , xN } is a non-increasing function of time [14].

Fig. 2: The local coordinate frame
P of i-th agent is rotated from
the global reference frame (g ) by Ci . {ei,1 , P
ei,2 , ei,3 } is the
set of orthonormal bases of i-th local frame i .

We next formulate the localization problem. Under the same
assumption of the Problem 2.1, we desire to estimate the
position of each agent. We denote p̂i as the estimated position
of pi with respect to the global reference frame. Then, the
localization problem is stated in follows:
Problem 2.2: Consider N -agents modeled by (1). Suppose
G = (V, E, A) is the interaction graph of the agents. For each
agent i ∈ V, design an estimation law of p̂i such that kp̂j −
p̂i k → kpj − pi k as t → ∞ for all j ∈ V based on the
measurements such as the relative displacement and relative
orientation, which are measured in local coordinate frames of
each agent.
The following section proposes a method for the orientation
estimation by using only relative orientation measurement. We
describe the estimation method in general n-dimensional space
instead of 3-dimensional space.
III. N OVEL M ETHOD FOR O RIENTATION E STIMATION
Orientation or attitude is often represented using three
or four parameters in 3-dimensional space. Three or fourparameter representations include the unit-quaternions, axisangle and Euler angles. While these parameters cannot represent orientation globally nor uniquely, the rotation matrix in
SO(n) is unique and globally defined [16].
The orientation in the n-dimensional space is defined by an
n × n proper orthogonal matrix C ∈ SO(n) containing, as
column vectors, the unit vectors ci (i = 1, 2, . . . n) identifying
the orientation of the local frame with respect to the reference
frame. A matrix C has properties such as C T C = I and
det(C) = +1. When the common reference frame is the Cartesian coordinate system with standard basis ei (i = 1, . . . , n),
the orientation of local frame is represented as ci = Cei which
is the i-column vector of C.
A. Orientation estimation in SO(n)
Consider N agents whose interaction graph is G that has
a spanning tree in n-dimensional space. Each agent does not

share a common reference frame. It follows that the i-th local
reference frame is rotated from the global reference frame
with the amount of Ci ∈ SO(n). Let us suppose that Cji
denotes the orientation of j-th local reference frame with
respect to the i-th local reference frame. It provides i-th agent
view to the j-th local reference frame with respect to the i-th
local reference frame. If the global reference frame has the
standard basis, each basis of i-th local frame with respect to
the global reference frame is the column vector of Ci . It is
well-known that Cji is defined by the multiplication of two
rotation matrices as follows:
Cji = Cj Ci−1 = Cj CiT

(6)

We assume that the i-th agent measures the orientation of j-th
agent with respect to the i-th local reference frame. Therefore,
Cji is obtained by the measurement. Let Ĉi ∈ SO(n), ∀i ∈ V
be an estimated orientation which is the proper orthogonal matrix. Orientation estimation problem can be stated as follows:
Problem 3.1: Consider N -agents whose interaction graph is
G in n-dimensional space. For the common orientation which
is identified by C ∗ ∈ SO(n), design an estimation law such
that Ci (t)T Ĉi → C ∗ as t → ∞ for all i ∈ V based on the
relative orientation (6).
Let us denote Bi ∈ SO(n) which is defined as
Bi := Ci X , X ∈ SO(n), ∀i ∈ V.

(7)

According to (6), it follows that
(8)

Under the assumption of Problem 3.1, finding the steady state
solution of Ĉi is equivalent to finding Bi satisfying the equality
(8). Then, Problem 3.1 can be restated as follows:
Problem 3.2: Under the assumption of Problem 3.1, design
an evolutionary algorithm for Bi : [0, ∞) → Rn×n such that
Bi (t) ∈ SO(n) , ∀t ∈ [0, ∞)
Bj (t) → Cji Bi (t) as t → ∞ , ∀(i, j) ∈ E

Let us suppose that n − 1 auxiliary variables are generated
at each agent. Each auxiliary variable at i-th agent is denoted
by zi,k ∈ Rn , k ∈ {1, 2, . . . , n − 1}. Let Bi be represented as
Bi = [bi,1 , bi,2 , . . . , bi,n ] where bi,k ∈ Rn , k ∈ {1, . . . n}, is
a k-th column vector of Bi . To generate orthonormal column
vectors of Bi from zi,k , we can use Gram-Schmidt process
with any independent vectors zi,k ∀k ∈ {1, . . . n − 1} as
follows:
vi,1 := zi,1
vi,2 := zi,2 − hzi,2 , bi,1 ibi,1
..
.
Pm−1
vi,m := zi,m − k=1 hzi,m , bi,k ibi,k

bi,1 ∧ bi,2 ∧ · · · ∧ bi,n−1 .

(10)

Since every vector can be written as a linear combination
of basis vectors, ∧(n−1) (Rn ) can be expanded to a linear
combination of exterior products of those basis vectors. The
pseudovector can be obtained from coefficients of (10). Since
we obtain bi,n from the formalization of pseudovector, the
matrix Bi is a proper rotation.
This is one way to calculate the quantity bi,n in higher
dimensional space, while there is no natural identification of
∧(n−1) (Rn ) with Rn . Another convention for obtaining bi,n
with proper rotation Bi is stated in [8].
Now, we consider a second goal of the Problem 3.2. We
propose a single integrator model for dynamics of auxiliary
variables as follows :
żi,k = ui,k

, ∀i ∈ V, ∀k ∈ {1, . . . , n − 1}.

(11)

Under the assumption of the Problem 3.1, a control law for (11)
is designed as follows for all i ∈ V and k ∈ {1, . . . , n − 1}:
X
−1
ui,k =
aij (Cji
zj,k − zi,k )
(12)
j∈Ni

Bj = Cji Bi , ∀(i, j) ∈ E.

•
•

such that the determinant of Bi is equivalent to 1. We consider bi,n as a pseudovector2 for making det(Bi ) to be one.
Pseudovector is a quantity that transforms like a vector under a
proper rotation. In n-dimensional vector space, a pseudovector
can be derived from the element of the (n − 1)-th exterior
powers, denoted ∧(n−1) (Rn ). It follows that

v

, bi,1 := kvi,1
i,1 k
v
, bi,2 := kvi,2
i,2 k
..
.
v
, bi,m := kvi,m
i,m k

(9)

where m = n − 1 and h·, ·i denotes the operator of inner
product. The rest of the procedure include determining bi,n

−1
where aij > 0. From the definition of Cji , we have Cji
=
T
Cji = Cij . Let zk be a stacked vector form defined by zk :=
(z1,k , z2,k , . . . , zN,k ). Here, (11)-(12) can be written in terms
of zk as follows:

żk = Hzk
where H ∈ RnN ×nN is a block

H11 H12
 H21 H22
H := 
..
 ...
.
HN 1 HN 2

(13)

matrix which is defined as

· · · H1N
· · · H2N 

(14)
..
..

.
.
···

HN N

Each partition Hij ∈ Rn×n is written as follows:

, j ∈ Ni
 Paij Cij
− j∈Ni aij In , i = j
Hij =

0n
,j ∈
/ Ni

(15)

where 0n ∈ Rn×n is a matrix having only zero entries.
2 Pseudovector is a vector-like object which is invariant under inversion. In
physics a number of quantities behave as pseudovector including magnetic
field and angular velocity. In 3-dimensional vector space, the pseudovector p
is associated with the cross product of two vectors a and b which is equivalent
to 3-dimensional bivectors : p = a × b.

B. Analysis of the Stability
Let us consider the eigenvalue of H. By using a nonsingular
matrix D, similarity transformation can be achieved as follows
: H̄ := D−1 HD. Suppose that D is a block diagonal matrix
defined by D := diag(C1 , C2 , . . . , CN ) where Ci ∈ Rn×n ,
i = {1, . . . N }. Suppose that H̄ij ∈ Rn×n denotes a partition
of i-th row and j-th column in the block matrix H̄. H̄ij is
written as follows :

, j ∈ Ni
 P aij In
− j∈Ni aij In , i = j
H̄ij = CiT Hij Cj =
(16)

0
,j ∈
/N.
n

1
From (20), the steady state solution of qk is as follows:
lim qk (t)

t→∞

= lim (G ⊗ In )eJ⊗In (t−t0 ) (G−1 ⊗ In )qk (t0 )

i

From (16), H̄ is rewritten as H̄ = −LH ⊗ In where LH ∈
RN ×N has zero row sum with dominant diagonal entries.
From the Theorem 2.1, all eigenvalues of LH have strictly
positive real part except for a simple zero eigenvalue with
corresponding eigenvector ξ = (1, 1, . . . , 1) ∈ RN .
Let us consider a coordinate transformation as zk = Dqk .
Then (13) is rewritten as follows:
q̇k (t) = −LH ⊗ In−1 qk (t), ∀k ∈ {1, . . . , n − 1}

(17)

From the Theorem 2.2, it is clear that qk converges to
the equilibrium set which is indicated as Eq = {x =
(x1 , x2 , . . . , xN ) ∈ RnN : x1 = x2 = · · · = xN }. For the
estimation of orientation, we have to avoid the convergence
of variables qk to zero. Therefore, the desired equilibrium
set is defined by Sq := Eq \ {0}. For a square matrix A,
define the column space of matrix A as C(A). Then, we
have the relationship C(A) = null(AT )⊥ , where null(AT )⊥
denotes the orthogonal space of null space of AT . Based on
the consensus property mentioned in the previous section, the
following lemma provides conditions for the convergence of
qk to Sq .
Lemma 3.1: Suppose that G has a rooted-out branch. For
the dynamics (17), there exists a finite point q∞
k ∈ Sq for
each k ∈ {1, . . . n − 1} such that qk exponentially converges
if and only if an initial value qk (t0 ), ∀k ∈ {1, 2, . . . , n − 1}
is not in C(LH ⊗ In ).
Proof: From the Theorem 2.2, qk , ∀k ∈ {1, 2, . . . n − 1}
globally exponentially converges to Eq . Then, for each k ∈
{1, 2, . . . n − 1}, there exists a finite point q∞
k ∈ Eq and
constants λq , ηq > 0 such that
−λq (t−t0 )
kqk (t) − q∞
kqk (t0 ) − q∞
k k ≤ ηq e
k k.

Now, we consider a steady state solution
qk is written as follows:

negative real part, the state transition matrix eJ⊗In (t−t0 ) has
the following form as t → ∞.


0
0


 ⊗ In
eJ⊗In (t−t0 ) → 
(20)
..


.

q∞
k .

qk (t) = e−LH ⊗In (t−t0 ) q(t0 ).

(18)

A solution of
(19)

The Jordan form of LH is obtained by the similarity transformation as follows: G−1 LH G = J. Let G and G−1 be
T T
represented as G = [g1 · · · gN ] and G−1 = [w1T · · · wN
]
respectively. We supposed that gN is the right eigenvector of
the zero eigenvalue. Then, wN is the left eigenvector of the
zero eigenvalue. Since every nonzero eigenvalue of LH has

t→∞

= (gN ⊗ In )(wN ⊗ In )qk (t0 )
= (gN wN ⊗ In )qk (t0 )

(21)

This implies that qk (t) converges to zero if and only if qk (t0 )
is perpendicular to the wN ⊗In which is the left eigenvector of
the zero eigenvalue. Then, it follows that (wN ⊗In )qk (t0 ) = 0
if and only if qk (t0 ) is in null(LTH ⊗ In )⊥ . Since null(LTH ⊗
In )⊥ = C(LH ⊗ In ), it completes the proof.
The result of Lemma 3.1 implies limt→∞ zk (t) = Dq∞
k
for each k. Let Bi ∈ SO(n) be derived from the procedure
including Gram-Schmidt process of zi,k , ∀k ∈ {1, . . . n − 1}
and the calculation of bi,n . Consequently, there exists Pi∞ ∈
SO(n) to which Bi converges. We can show that Bi (t) →
Cij Bj (t), (i, j) ∈ E as t → ∞ from the following result.
Theorem 3.1: Let Ci ∈ SO(n) denote a rotation matrix
which identifies the orientation of the i-th agent. Suppose
that n × n matrix Bi for the i-th agent is derived from the
procedures (9), (10) and (13). Then, there exists a common
matrix X ∈ SO(n) such that Bi converges to Ci X as t → ∞
for all i ∈ {1, . . . N }.
Proof: By using the coordinate transformation, we define
qi,k ∈ Rn for all i ∈ {1, . . . N } and k ∈ {1, . . . , n − 1} such
that zi,k = Ci qi,k . Let us replace the value of qi,k with zi,k in
the procedure (9). From the definition of bi,k for all k in (9),
vi,k is rewritten as follows :
vi,1
vi,2

= Ci qi,1 := Ci xi,1
vi,1
vi,1
= Ci qi,2 − hCi qi,2 , kvi,1
k i kvi,1 k
:= Ci xi,2
(22)

..
.
vi,n−1

= Ci qi,n−1 −
:= Ci xi,n−1

vi,k
vi,k
k=1 hCi qi,n−1 , kvi,k k i kvi,k k

Pn−2

Using this to replace vi,m by xi,m , ∀m ∈ {1, . . . n}, xi,m can
be written as follows:
xi,1 = qi,1
x
x
xi,2 = qi,2 − hqi,2 , kxi,1
i i,1
i,1 k kxi,1 k
(23)
..
.
Pn−2
xi,k
xi,k
xi,n−1 = qi,n−1 − k=1 hqi,n−1 , kxi,k k i kxi,k k
This is a Gram-Schmidt procedure with respect to the
qi,k , ∀k ∈ {1, . . . n − 1}. From the result of Lemma 3.1, for
each k, qi,k converges to a finite point qk∞ ∈ Rn as t → ∞ for
n
all i ∈ V. This implies that there exists a finite vector x∞
k ∈R

∞
∞
such that {xi,1 , xi,2 · · · xi,n−1 } → {x∞
1 , x2 , · · · xn−1 } as
t → ∞ for all i. It is clear that bi,k and xi,k have the following
x
relationship: bi,k = Ci kxi,k
, ∀k ∈ {1, 2 . . . n − 1}. We also
i,k k
derive bi,n from (10). Therefore, in the steady state of xi,k ,
we have the following solution of Bi (t):
 ∞

x1
x∞
x∞
n
2
lim Bi (t) = Ci
·
·
·
, ∀i ∈ V (24)
∞
t→∞
kx∞
kx∞
n k
1 k kx2 k

It completes the proof.
Since Bi (t) → Ci X as t → ∞, it is clear that Bi (t) →
Cij Bj (t) as t → ∞. From the result of Theorem 3.1, we now
see that the proposed algorithm provides the solution of the
Problem 3.2. Since we transformed the Problem 3.1 to Problem
3.2, Bi is the estimated orientation.
In the following section, we combine this algorithm with
the formation control. For a practical reason, we consider
the formation control in 3-dimensional spaces instead of the
general n-dimensional spaces.
IV.

F ORMATION C ONTROL I N 3-D S PACE

In 3-dimensional space, we have to obtain the matrix Bi ∈
SO(3) satisfying conditions stated in Problem 3.2. We have
two auxiliary variables denoted by zi,1 , zi,2 for the i-th agent.
By using the proposed estimation law (13) and Gram-schmidt
process, we can obtain bi,1 and bi,2 . Since the pseudovector is
directly calculated by using a cross product of two vectors in
3-dimensional space, the quantity bi,3 is as follows :
bi,3 = bi,1 × bi,2 ,

∀i ∈ V.

j∈Ni

where ku > 0 and lij > 0. Since ui = Ci−1 uii , the position
dynamics of i-th agent with respect to the global reference
frame is written as follows:
=
=

ui = Ci−1 uii
(27)


X
−1
i
i
∗
∗
ku
lij Ci
(pj − pi ) − Ĉi (pj − pi ) (28)
j∈Ni

=

ku

X
j∈Ni



lij (pj − pi ) − Ci−1 Ĉi (p∗j − p∗i ) (29)

j∈Ni

X
j∈Ni

|

(31)

where e = (e1 , . . . , eN ) and w = (w1 , . . . , wN ). From (19)
and the result of the Theorem 3.1, it is clear that Ci−1 Ĉi
converges to C ∗ with an exponential rate. It follows that there
exist kw > 0 and λw > 0 such that
kw(t)k ≤ kw e−λw (t−t0 ) kw(t0 )k
Now, we consider the solution of e(t) given by
Z t
e(t) = e−L⊗I3 (t−t0 ) e(t0 ) +
e−L⊗I3 (t−τ ) w(τ )dτ.

(32)

(33)

t0

By using the result in Theorem 3.1 of [5], the following result
is obtained.
Theorem 4.1: Under the estimation law (13) and the formation control law (26), there exists a finite point p∞ such that
pi (t), ∀i ∈ V globally exponentially converges to C ∗ p∗i + p∞ ,
if G has a rooted-out branch, and q̂k (t0 ), ∀k ∈ {1, 2}is not in
C(LH ⊗ I3 ).
Proof: Let us define Ee as
Ee := {(x1 , . . . , xN ) ∈ R3N : xi = xj , ∀i, j ∈ V}

(34)

There exists e∗ ∈ Ee , kl > 0 and λl > 0 such that
ke(t) − e∗ k

≤

e−L⊗I3 (t−t0 ) e(t0 ) − e∗
Z t
e−L⊗I3 (t−τ ) w(τ )dτ
+
t0
−λl (t−t0 )

≤ kl e
Z
+

t

t0

ke(t0 ) − e∗ k

(35)
(36)
(37)

e−L⊗I3 (t−τ ) kw(τ )k dτ (38)

According to the definition of induced norm, e−L⊗I3 (t−τ )
is written as
e−L⊗I3 (t−τ ) =

max

kxk=1,t0 ≤τ ≤t

e−L⊗I3 (t−τ ) x ≤ η

(39)

where η > 0 is a finite value which bounds the matrix norm.
It follows from (32) and (39) that
Z t
e−L⊗I3 (t−τ ) kw(τ )k dτ
(40)
t0
Z t
≤
ηkw e−λw (τ −t0 ) kw(t0 )k dτ
(41)
t0

From the Theorem 3.1, there exists C ∗ ∈ R3×3 such that
Ci−1 Ĉi (t) converges to C ∗ as t → ∞. By defining ei :=
pi − C ∗ p∗i , we obtain the error dynamics as follows:
X
ėi = ku
lij (ej − ei )
+ ku

ė = −ku L ⊗ I3 e + w

(25)

Note that Ĉi (t) = Bi (t) ∈ SO(3). We now propose a control
law for the formation system under the assumption of Problem
2.1. Consider the following control law:
X
uii = ku
lij ((pij − pii ) − Ĉi (p∗j − p∗i ))
(26)

ṗi

Then, (30) can be rewritten in the vector form as follows:

∗

lij (C −

Ci−1 Ĉi )(p∗j

{z

wi :=

−

p∗i )
}

(30)

−ηkw h −λw (τ −t0 ) it
e
kw(t0 )k
=
λw
t0
−ηkw
=
kw(t0 )k (e−λw (t−t0 ) − 1)
λw

(42)
(43)

From (43), (38) can be written as follows:
ke(t) − e∗ k

≤ kl e−λl (t−t0 ) ke(t0 ) − e∗ k
(44)
ηkw
−
kw(t0 )k (e−λw (t−t0 ) − 1). (45)
λw

By replacing t0 with t00 = (t + t0 )/2 in (45), we obtain
ke(t) − e∗ k

) e( t + t0 ) − e∗
2
t−t0
ηkw
t + t0
−
w(
) (e−λw ( 2 ) − 1)
λw
2
≤ kl2 e−λl (t−t0 ) ke(t0 ) − e∗ k
t−t0
t−t0
ηk 2
− w e−λw ( 2 ) kw(t0 )k (e−λw 2 − 1)
λw

≤ kl e−λl (

t−t0
2

Fig. 3: Interaction graph of six agents for simulation

This completes the proof.

j∈Ni

where
is a relative displacement defined in (5) and ku , lij >
0. We know that piji = pij − pii = Ci (pj − pi ). By defining p̃i
as p̃i := p̂i − C ∗ pi , we obtain the estimation error dynamics
as follows :
X
p̃˙i = ku
lij (p̂j − p̂i − ĈiT Ci (pj − pi ))
j∈Ni

= ku

j∈Ni

+ ku

lij (p̃j − p̃i )

X
j∈Ni

|

16

14

12

10

piji

X

18

k(pj − pi ) − C ∗ (p∗i − p∗j )k

V. N ETWORK L OCALIZATION IN 3-D S PACE
Consider the agents under the assumption of Problem 2.2.
We can obtain Ĉi from the proposed estimation method for the
unknown orientation as stated in the previous sections. Then,
we design an estimation law of p̂i as follows:
X
p̂˙i = ku
lij (p̂j − p̂i − ĈiT piji )
(46)

ψi :=

6

4

2

0

0

2

4

6

8

10

12

t(sec)

lij (C ∗ − ĈiT Ci )(pj − pi )
{z

8

(47)

}

Fig. 4: Difference of the measured displacement and the
desired displacement of neighboring agents. C ∗ is a common
rotation matrix.

Thus the overall error dynamics for position estimation can be
written as
p̃˙ = −ku L ⊗ I3 p̃ + Ψ

(48)

where p̃ = (p̃1 , p̃2 , . . . , p̃N ) and Ψ = (ψ1 , ψ2 , . . . ψN ). We
notice that (48) corresponds to (31). We obtain the following
corollary based on the relationship.
Corollary 5.1: Under the orientation estimation law (13)
and the position estimation law (46), there exists a finite point
p̂∞
i , ∀i ∈ V to which p̂i globally exponentially converges, such
∞
that kp̂∞
j − p̂i k = kpj − pi k, if G has a rooted-out branch,
and q̂k (t0 ), ∀k ∈ {1, 2}is not in C(LH ⊗ I3 ).
VI. S IMULATION R ESULT
In this section, we provide simulation results to verify the
proposed methods. We consider six agents whose interaction
graph is illustrated in Fig. 3. Suppose that each agent has its
own reference frame which is rotated from the global reference
frame by the proper orthogonal matrix Ci ∈ R3×3 , i =
{1, 2, 3, 4, 5, 6}. For the simulation, the matrix Ci is arbitrary
determined. Estimated orientation of i-th agent identified by Ĉi
can be obtained by using two auxiliary variables. Under the

estimation law (13), Ĉi (t) converges to a steady state solution
as t goes to infinity. The formation control law (26) of ith agent is simultaneously conducted with the estimation of
orientation Ĉi . According to the result of the Theorem 4.1, the
position of i-th agent denoted by pi converges to C ∗ p∗i + p∞ ,
where C ∗ ∈ R3×3 is a common rotation matrix obtained by
C T Ĉi (t) as t → ∞, p∗i is desired position of i-th agent, and
p∞ is a common position vector. Fig. 4 shows the result that
the measured displacement of neighboring agents converges to
the desired displacement with rotational motion. Fig. 5 depicts
that the formation of six agents using the proposed strategy,
which includes a combination of orientation estimation and
formation control law, converges to the rotated desired shape.
VII.

C ONCLUSION

In this paper, we proposed a novel estimation method for
unknown orientation of agent with respect to the common reference frame in general n-dimensional space. We added virtual
auxiliary variables for each agent. These auxiliary variables

[6]

[7]
8
Initial point
Final point

6

[8]

4
2

[9]
0
−2

[10]
−4

[11]

−6

2
0

2

[12]

0

−2
−2

−4

−4
−6

[13]

−6

Fig. 5: Formation control via orientation estimation in 3
dimensional space

[14]

[15]

are corresponding to column vectors of estimated orientation
Ĉi ∈ SO(n). An estimation law of auxiliary variables is based
on the principle of consensus protocol. Since configuration
space for the proposed method is n-dimensional vector space
instead of nonlinear space like unit circle, global convergence
of auxiliary variables is guaranteed. Consequently, each desired
point to which auxiliary variables converge, under the proposed
estimation law, is transformed to the estimated orientation.
This estimated frame is used to compensate the rotated frame
of each agent in the formation control system. Under the
formation control law with proposed estimation method, we
guarantee a global convergence of multi-agent system to the
desired shape in 3-dimensional space, under the distancebased setup. The proposed strategy can also be applied to
the localization problem in networked systems. The result
remedies the weak point of previous works [5], [6] where the
range of initial orientations is constrained to 0 ≤ θi < π.
R EFERENCES
[1]

K.-K. Oh, M.-C. Park, and H.-S. Ahn, “A survey of multi-agent
formation control,” Automatica, vol. 53, pp. 424–440, 2015.
[2] Z. Lin, B. Francis, and M. Maggiore, “Necessary and sufficient graphical conditions for formation control of unicycles,” IEEE Transactions
on Automatic Control, vol. 50, pp. 121–127, 2005.
[3] W. Ren and E. Atkins, “Distributed multi-vehicle coordinated control
via local information exchange,” Int. Journal of Robust and Nonlinear
Control, vol. 17, pp. 1002–1033, 2007.
[4] D. V. Dimarogonas and K. J. Kyriakopoulos, “A connection between
formation inachievability and velocity alignment in kinematic multiagent systems,” Automatica, vol. 44, no. 10, pp. 2648–2654, 2008.
[5] K.-K. Oh and H.-S. Ahn, “Formation control and network localization
via orientation alignment,” IEEE Trans. on Automatic Control, vol. 59,
no. 2, pp. 540–545, Feb. 2014.

[16]

E. Montijano, D. Zhou, M. Schwger, and C. Sagues, “Distributed
formation control without a global reference frame,” in Proceedings
of Americal Control Control(ACC),2014, 2014, pp. 3862–3867.
B.-H. Lee and H.-S. Ahn, “Distributed formation control via global
orientation estimation,” Automatica, vol. 73, pp. 125–129, 2016.
——, “Distributed estimation for the unknown orientation of the local
reference frames in n-dimensional space,” in Proc. Control Automation
Robotics and Vision(ICARCV), 2016 14th International Conference on,
Nov. 2016.
L. Moreau, “Stability of continuous-time distributed consensus algorithms,” in Decision and Control 2004. CDC. 43rd IEEE Conference
on, Dec. 2004, pp. 3998 – 4003.
L. Scardovi and R. Sepulchre, “Synchronization in networks of identical
linear systems,” Automatica, vol. 45, pp. 2557–2562, 2009.
A. Jadbabaie, J. Lin, and A. S. Morse, “Coordination of groups
of mobile autonomous agents using nearest neighbor rules,” IEEE
Transactions on Automatic Control, vol. 48, no. 6, pp. 988–1001, 2013.
R. Sepulchre, “Consensus on nonlinear spaces,” Annual Reviews in
Control, vol. 35, pp. 56–64, 2011.
F. Dörfler and F. Bullo, “Synchronization in complex networks of phase
oscillator:a survey,” Automatica, vol. 50, no. 6, pp. 1539–1564, 2014.
L. Moreau, “Stability of multiagent systems with time-dependent communication links,” IEEE Trans. on Automatic Control, vol. 50, no. 2,
pp. 169–182, Feb. 2005.
K.-K. Oh and H.-S. Ahn, “Formation control of mobile agents based
on distributed position estimation,” IEEE Trans. on Automatic Control,
vol. 58, no. 3, pp. 737–742, Mar. 2013.
N. Chaturvedi, A. Sanyal, and N. McClamroch, “Rigid-body attitude
control,” IEEE Control Systems, vol. 31, no. 3, pp. 30–51, 2011.

