Relative Localization with Symmetry Preserving

Observers
Oscar De Silva, George K.I. Mann, and Raymond G. Gosine
Intelligent Systems Lab, Faculty of Engineering and Applied Science,
Memorial University of Newfoundland,
St. Johns, NL A1B 3X5, Canada.
Email: {oscar.desilva,gmann,rgosine}@mun.ca
AbstractSymmetry preserving observer design is a recently
developed approach for deriving estimators which exploits the
invariant properties of nonlinear systems. Multi-robot localization
is inherently a system operating on SE(3), thereby posing an in-
teresting problem for application of the lter. This paper presents
an invariant extended Kalman lter design for the problem
of multi-robot relative localization in 2.5D, for application in
ground and aerial mobile platforms. A detailed derivation of
the invariant lter is presented with numerical results analyzing
its performance against a traditional EKF approach to the
problem. The tracking errors, stability to random initialization
and robustness to changing noise characteristics are evaluated.
The strong geometric basis of the lter results in linear Kalman
like gain convergence behavior which is desirable for numerical
stability and applicability as a low cost scheduled gain observer
to the problem.
I. INTRODUCTION
Relative localization is the process of estimating the for-
mation within a team of robots. This enables the robots
to cooperatively navigate in hostile environments where an
external positioning service such as the GPS is unavailable [1].
A relative localization approach performs pose estimation by
utilizing the inter-robot relative measurements in the team [2],
[3]. Such robot-centric localization approaches are reported as
efcient means to address the problem of localization without
heavy resource demand on individual members of the team
[4], [5].
Estimating the pose of robots using relative measure-
ments is traditionally addressed using Extended Kalman Filters
(EKF) [6]. Much subsequent development is reported in this
domain which includes distributed implementation [6], con-
sistency handling [2], measurement correspondence [5], etc.
The use of the EKF for localization has two main draw backs.
First, the EKF linearizes the system dynamics about the current
state estimate which causes the lter to be unstable and even
diverge in cases with higher order nonlinearities. The linearized
approximation additionally gives an inaccurate representation
of the covariance which eventually leads to inconsistent up-
dates. This is tackled by committing computational resources
to Unscented Kalman Filtering(UKF) [3] or particle ltering
approaches.
A second problem of the EKF is that it assumes a linear
state error and an output error in its formulation. This is an
inaccurate error representation for many applications where the
EKF formulation tends to overlook the underlying geometry
of the system in consideration. This is common to even UKF
solutions if applied directly without changing the linear error
state assumptions. This problem is tackled by nonlinear lters
which results in solutions with similar or reduced computation
demand in comparison to an EKF, but with increased difculty
in systematic design for a general system. Attitude heading
reference systems are prime examples where exploiting the
geometry of the rotation group SO(3) results in commercial
grade solutions such as, the multiplicative extended Kalman
lter [7], and the nonlinear complimentary lter [8], with
similar and reduced computational demand in order.
Correspondingly, multi-robot systems operate in the special
Euclidean group SE(3), i.e., the group of translations and
rotations. Different nonlinear lter derivation on SE(3) are
reported in literature where the solutions are deterministic
formulations [9]. A more general approach is reported in
[10], with a constructive method to derive lters for systems
possessing symmetries. This together with error state Kalman
ltering approaches presented in [11], allows to systematically
design localization lters for robots with stochastic Kalman
like gain tuning procedures.
This paper reports the design of a nonlinear symmetry
preserving lter for robots operating in 2.5D using the method
of symmetry preserving observer design. The main contri-
bution of this work is the design of an Invariant Extended
Kalman Filter (IEKF) for relative localization of robots. The
resulting lter requires almost similar computation power due
to the EKF like observer structure. The lter design method
is compared against an EKF with comparative performance
analysis on estimation errors and gain convergence.
II. RELATIVE LOCALIZATION IN 2.5D
A. System dynamics
The objective of a relative localization lter is to estimate
the relative pose x = (p
i
,
l
R
i
) of a robot i with respect to a
local robot l. Information of the platform velocities and relative
measurements between robots are available for this estimation
problem. In indoor localization scenarios, it is common prac-
tice to simplify the full 3D problem to a 2.5D [12] where a non-
roll non-pitch frame of reference of a target robot i and a local
robot l is considered. Therefore, a relative localization lter in
2.5D requires to estimate the position
l
p
t
= [x, y, z]
T
and
the orientation
l
R
t
:= R

parameterized by relative heading

. The system dynamics can be expressed by equation (1),
where the vector u = [ v
T
l
,
l
, v
T
i
,
i
]
T
captures the input
velocities v
i
= [v
x
, v
y
, v
z
]
T
, v
l
= [v
lx
, v
ly
, v
lz
]
T
and

l frame
i frame
r
x
i
*
frame
y
z
p
v
l
v
i

i
Fig. 1. Relative localization frames of reference for 2.5D localization
angular speed
i
= [0, 0,
i
]
T
,
l
of the target robot i and
the local robot l. This scenario is illustrated in Fig. 1, where
the non-roll non-pitch frame of reference i is considered in
place of the body xed frame i

, with all vector quantities

expressed in the new frame i. The random variable w
v
and
w

capture the process noise which is assumed to be zero

mean Gaussian with covariance E(w
T
w) = Q.
x = f(x, u, w)

_
p

_
=
_
R

v
i
v
l

l
p +w
v

i

l
+w

_
(1)
The outputs of the system are the relative measurements
between the robots. A position measurement sensor is assumed
which measures the relative position y
p
with a measurement
model given in (2). The measurement is corrupted with random
noise with covariance E(
T
) = R. More realistic relative
sensing models which measure the relative range r and bearing
, between robots are considered later in this paper.
y = h(x, u, )
y
p
= p +
p
(2)
The system and measurement models presented denotes a
practically realizable multi-robot team. Authors previous work
studies hardware realization [13] and observability analysis
[14] of the particular system.
B. Extended Kalman Filter
A traditional EKF based estimator takes the form (3) with
a prediction term f( x, u) and a correction term KE, where a
linear output error E( x, x, ) = (y h( x, u)) is assumed.

x = f( x, u) +K(y h( x, u)) (3)

The EKF performs a rst order Taylor series linearization of
the system dynamics (1) about the current state estimate (x =
x, w = 0, = 0).
x = f( x, u) +
f
x
(x x) +
f
w
(w 0)
y = h( x, u) +
h
x
(x x) +
h

( 0)
(4)
The state error x is dened as x x. The dynamics of the
state error is found by subtracting (4) from (3) to deduce a
linear Kalman structure.

x = (AKC) x Mw+KN
_
A =
f
x
, C =
E
x
, M =
f
w
, N =
E

( x,0,0)
The Kalman gain computation and covariance propagation
takes the usual linear form.

P = AP +PA
T
+MQM
T
PC
T
(NRN
T
)
1
CP
K = PC
T
(NRN
T
)
1
For the system under study the matrices A, C, M, N reads
A =
_
S(
l
) d/d(R

)v
i
0
13
0
_
C = [I
33
0
31
]
M = I
44
N = I
33
The operator S(.) refers to the skew symmetric representation
of a vector in standard notation. It is evident that the matrices
are dependent on the current state estimate

. As a result non
converging gains are expected. This can be partly traced to the
linear state and output errors assumed during the formulation,
which does not consider the geometry of SE(2)
1
,where the
error between the estimator frame and the actual frame is

T
1
T if a homogenous transformation matrix parametrization
T is assumed. The next section presents the design of a lter
such that the geometry and the underlying properties of the
group SE(2) are preserved.
III. INVARIANT EXTENDED KALMAN FILTER
A. IEKF Design Preliminaries
The proposed IEKF design follows the symmetry pre-
serving design methodology in [10], [11], which presents a
systematic approach to derive an observer which preserves the
symmetries of the system. A tutorial of the lter formulation is
given in [15], for readers interested in application. The design
methodology is presented in the coordinate free framework
of differential geometry. The following denitions are used
throughout the derivation of the IEKF.
Denition 1: A set G is a Group if for all g G,
we can dene a Group operation (multiplication) with a
corresponding identity element e, and an inverse element g
1
,
which satises the conditions of closure and associativity.
The set of translations and rotations (p, ) = g SE(2) in
which the robots operate can be identied as a group with the
group operation for g
0
, g G dened as (5), which satises
closure g
0
g G, and associativity (g
0
g) g = g
0
(g g).
An identity element e = (0, 0, 0, 0) G and an inverse
element g
1
= (R
T

p, ) G can be identied such that

g g
1
= e.
g
0
g =
_
p
0

0
_

_
p

_
=
_
R
0
p +p
0
+
0
_
(5)
1
more precisely SE(2)R
1
since we are considering 2.5D
Denition 2: A smooth map
g
is a Group action on a set
M, if (g, m) G M
g
(m) M, s.t.
e
(m) = m and

g1
(
g2
(m)) =
g1g2
(m).
As an example, the group operation
g1
(g
2
) = g
1
g
2
G
can be considered as a trivial smooth map, where a group G
acts on itself. More examples of group actions are given in the
equation set (6).
Denition 3: The system x = f(x, u) is G-invariant if
f(
g
(x), (u)) = D
g
(x) f(x, u), for group actions
g
(x)
on states x X, and
g
(u) on inputs u U.
Denition 4: The output y = h(x, u) is G-equivariant if
h(
g
(x), (u)) =
g
(y), for group action
g
(y) on outputs
y Y .
The system model (1) is found to be G-invariant and the
measurement model (2) is found to be G-equivariant for a set
of smooth maps. This is identied in the derivation process of
the lter (Equations 6,7, and 8).
The process of deriving the invariant extended Kalman
lter is separated to four main steps for clarity. First, the
system symmetries are identied. Second, the complete set of
invariants are found using the moving frame method. Third,
an invariant observer is formed using the general form given
by [10]. The nal step derives an error state Kalman lter for
stabilization of the error state dynamics in order to nd the
gains of the observer.
B. IEKF- Relative localization
1) System symmetries: Identifying symmetries of the sys-
tem is the process of nding the actions on the states x,
inputs u, and outputs y, such that the system is G-invariant.
Consider the system given by (1) with noises removed i.e.
= 0, w = 0. For group G = SE(2) dene group actions
on states
g
: G X X, on input
g
: G U U, and
on measurements
g
: GY Y as follows

g
_
p

_
=
_
R
0
p +p
0
+
0
_

g
_
_
_
v
l

l
v
i

i
_
_
_ =
_
_
_
R
0
v
l

l
p
0

l
v
i

i
_
_
_

g
(y
p
) = (R
0
y
p
+p
0
)
(6)
Consider a group element g
0
= (p
0
,
0
) G. Equation
set (7) verify that the system given by (1) is G-invariant
under the group actions
g
and
g
, i.e., D
g
f(x, u) =
f(
g
(x),
g
(u)).
D
g
f(x, u) =
d
dt

g
_
p

_
=
d
dt
_
R
0
p +p
0
+
0
_

_
R
0
p

_
=
_
R
0
(R

v
i
v
l
) R
0

l
p

i

l
_
f(
g
(x),
g
(u))
(7)
Here the smooth map D
g
can be identied as a group
action on the tangent space of X, i.e., D
g
: GT X T X.
The measurement model given by (2) is found to be G-
equivariant under the group actions
g
,
g
,
g
. i.e.,
g
(y) =
h(
g
(x),
g
(u)).
h(
g
(x),
g
(u)) = R
0
p +p
0
= R
0
y
p
+p
0

g
(y)
(8)
The particular group action
g
on the state space relates
to translation and rotation of the reference coordinate system
to a different location attached to the robot. The mappings of
inputs u and outputs y to this new frame of reference is dened
by the group actions
g
,
g
. Thus, the group actions do not
change the intrinsics dynamics of the system, rather they are
transformed to a new coordinate representation.
2) Moving frame, Invariants, Invariant frame : To identify
the complete set of invariants, the moving frame method is
applied [10]. The moving frame is identied by solving the
equation
g
(x) = c = e for g := (x).

g
_
p

_
=
_
R
0
p +p
0
+
0
_
=
_
0
31
0
_
(x) = (R
T

p, )
Then the set of invariants I( x, u), J( x, y) is found by
operating ( x) on the inputs u and the output y.
I( x, u) =
( x)
_
_
_
v
l

l
v
i

i
_
_
_ =
_
_
_
R
T

v
l
+
l
R
T

l
v
i

i
_
_
_ =
_
_
_
I
v
l
I

l
I
vi
I
i
_
_
_
J( x, u) =
( x)
(y
p
) = R
T

y
p
R
T

p
The Invariant frame is dened to be the mapping of basis
vectors
i
of the tangent space T X, by the smooth map D
g
.
This is found as follows.
d
d
_

( x)
1(
i
)
_
|
=0
=
d
d
_
R

p
i
+ p

i
+

_
|
=0
=
_
R

e
i
1
_
W = diag(R

I
3
, 1)
3) Invariant observer: Invariant observer for a general G-
invariant system x = f(x, u), and a G-equivariant output y =
h(x, u), is given by Theorem 1 in reference [10]. It takes the
general form
F( x, u, y) = f( x, u) +W( x)L(I, E)E( x, u, y)
The term f( x, u) is the usual forward prediction as seen in
extended Kalman lters and the term W.L.E is the correction
term with gain L, output error E, and invariant frame W.
The output error E is not the usual linear innovation term
h( x, u) y in EKFs, rather it is a nonlinear invariant error
term. This error is dened as E = J( x, y) J( x, h( x, u))
and is found as
E( x, u, y) = J( x, h( x, u)) J( x, y)
= R
T

( p y
p
)
Substituting the invariant output error and the invariant frame,
we get the invariant observer as
_

_
=
_
R

v
i
v
l

l
p

i

l
_
+
_
R

L
p
R
T

(y
p
p)
L

R
T

(y
p
p)
_
(9)
This completes the symmetry preserving observer design. The
gains (L
p
, L

) should be selected such that the invariant

observer (9) is stable. For a systematic local gain selection,
the observer is formed to an invariant extended Kalman lter
following the procedure in [11].
4) Error State Kalman Filter: Error state Kalman ltering
allows Kalman like adaptive gain stabilization of nonlinear
observers. The lter derivation differs in its nonlinear esti-
mation error and nonlinear output error E in place of the
usual linear error state x and output error E seen in the
EKF formulation. The EKF formulation given in section 2
rst linearizes the system dynamics before deducing the error
state dynamics of the observer. In error state formulation
considered here, rst, the error dynamics are formulated using
the system equations (1) and observer model (9). This gives
a nonlinear error dynamic system which is then linearized to
deduce a Kalman like structure given in (3). This approach is
common practice in many engineering applications including
multiplicative attitude heading reference systems and nonlinear
inertial navigation systems.
The invariant estimation error of the observer is dened
as =
(x)
( x)
(x)
(x). Notice that this is different from
the usual liner estimation error term ( x x) seen in EKFs.
The invariant estimation error reads as
=
(x)
( x)
(x)
(x)
=
_
R
T

( p p)

_
=:
_

_
The process and measurement noises are introduced to the
system equations to recover the form (1). To preserve system
symmetry, the noise terms are modied to invariant noise
terms. This is so that the G-invariant and G-equivariant
property of the system is preserved with the addition of the
noise terms. The process noise w and measurement noise are
replaced by invariant noises M(x)w and N(x) respectively.
Thus the requirement for G-invariance and G-equivariance
are modied as in [16] to D
g
(f(x, u) + M(x)w) =
f(
g
(x),
g
(u)) +M(
g
(x))w and
g
(h(x, u) +N(x)) =
h(
g
(x),
g
(u)) +N(
g
(x)). The new invariant denitions
are satised when M(x) = R

and N(x) = R

.
_
M(x)w
v
M(x)w

N(x)
p
_
=
_
R

w
v
w

p
_
Now the error state dynamics of the system is derived as a
function of and scalar invariants I. The invariant output
error E in terms of reads
E( x, u, y) = R
T

( p y
p
)
= R
T

( p p R

p
)
E(, ) = R
T

p
R
T

p
Where R

is the rotation error i.e. R

=
R
T

(R

) =
R
T

. By differentiating the estimation error state, we have

_

p

_
=
_

R
T

( p p) +R
T

p p)

_
Identifying

R
T

= S(
l

i
)R

and substituting from equa-

tions (1) and (9), we have the error state dynamics of the
observer.
_

p

_
=
_
S(
i
)
p
+ (R

I
3
)v
i
w
v
+L
p
E
L

E w

_
By linearizing the error dynamics about the = (0, 0, 0, 0),
we recover a Kalman like structure.
_

_
= (AKC)
_

_
M
_
w
p
w

_
+KN (
p
)
where the A,C,K,M,N matrices are identied as
A =
_
S(I
i
) S(e
3
)I
vi
0
13
0
_
C = [I
33
0
31
]
M = I
44
N = I
33
K = [L
p
L

]
T
IV. RESULTS
The derived IEKF for relative localization is compared
against the traditional EKF for its performance in different
scenarios. The performance on tracking, gain convergence,
changing sensor models, non-permanent trajectories and ran-
dom initialization is presented in this section.
1 0 1 2 3 4 5
3
2
1
0
1
2
3
x(m)

y
(
m
)
Actual trajectory
Estimated trajectory
Fig. 2. The simulated trajectory of a local and a target robot.
1) Robot tracking: Fig. 2 illustrates the simulated trajec-
tory of a local robot and target robot. The input velocities
(v
lx
,
l
, v
ix
,
i
) were set to (0.5, 0.5, 0.5, 0.5).
The system and sensor models given by (1) and (2) are
stochastically simulated with process and measurement noises
set to Q = diag(5e
3
, 1e
5
, 1e
5
, 2e
2
) and R = 0.05I
33
.
Both the EKF and the IEKF were capable of successfully
estimating the trajectory with comparably similar performance.
0 5 10 15 20 25 30
10
2
10
0
10
2
L
o
g

p
o
s
i
t
i
o
n

e
r
r
o
r

(
m
)


-EKF
3-EKF
-IEKF
3-IEKF
0 5 10 15 20 25 30
10
0
L
o
g

h
e
a
d
i
n
g

e
r
r
o
r

(
d
e
g
)
Time (s)
Fig. 3. Relative position and heading estimation errors of EKF and the IEKF
Fig. 3 presents the relative position and relative heading
estimation errors along with the estimation of their covariance.
The localization performance of both lters were found to be
robust in multiple experiments with random initializations.
2) Gain convergence: Comparing with the corresponding
lter matrices of an EKF, it can be seen that the matrices of the
IEKF do not depend on the current system state, rather they
depend only on the system invariants I. Thus if invariants
remain constant, behavior similar to a linear Kalman lter
with constant gains can be expected. The trajectories where the
invariants I remain constant are termed permanent trajectories.
Fig. 4(a) illustrates the time evolution of the non-zero Kalman
gains K
x
, K
y
, K
z
, and K

of the EKF. In comparison, Fig.

4(b) reports the capability of the IEKF to converge to steady
state gains similar to a linear Kalman lter. This was possible
since the trajectory considered was a permanent trajectory with
constant velocities for the target robot.
3) Non-permanent trajectories: Changing velocities were
introduced to the target robot to analyze the lters response
for non-permanent trajectories. Fig. 5 illustrates the path and
the gain stabilization of the EKF for this case. The EKF
does not exhibit convergence of its gains. The IEKF exhibits
convergence to a new set of gains corresponding to each
permanent trajectory. Fig. 6 illustrates the gain stabilization for
this case along with the velocities of the target robot during
each time window.
4) Range bearing sensing: Although the positioning sensor
model used allows convenient derivation of the lter, in
practical scenarios the nonlinear model (10), provides a more
0 5 10 15 20 25 30
0.6
0.4
0.2
0
0.2
0.4
0.6
0.8
1
E
K
F
-
G
a
in
s


0 5 10 15 20 25 30
0.6
0.4
0.2
0
0.2
0.4
0.6
0.8
1
Time(s)


Time(s)
IE
K
F
-
G
a
in
s
(a) (b)
K
x
K
y
K
z
K
Fig. 4. Gains of the EKF (left) and the IEKF (right) during simulation
1 0 1 2 3 4 5
3
2
1
0
1
2
3
x(m)

y
(
m
)
0 5 10 15 20 25 30
0.6
0.4
0.2
0
0.2
0.4
0.6
0.8
1
E
K
F
G
a
in
s


Time(s)
K
x
K
y
K
z
K
Actual trajectory
Estimated trajectory
Fig. 5. The simulated non-permanent trajectories (left) and the corresponding
EKF gain evolution (right)
0 5 10 15 20 25 30
0.6
0.4
0.2
0
0.2
0.4
0.6
0.8
1
Time(s)


I
E
K
F
-
G
a
i
n
s
vix, i= 0.8, 0.8 vix, i= 0.8, -0.8 vix, i= -0.8, -0.8 vix, i= -0.8, 0.8
K
x
K
y
K
z
K
Fig. 6. IEKF gain evolution for non-permanent trajectories
accurate error representation which measures the range and
bearing of a target.
y = h(x)
_
r

_
=
_
_
_
x
2
+y
2
+z
2
Tan
1
(y/x)
Tan
1
(z/
_
x
2
+y
2
)
_
_
+
_

_
(10)
By manipulating this to an equivalent position sensing sensor
model with rst order noise approximation N

(x), the same

lter can be applied to the problem with a change to the
mappings on how the noises enter the measurements.
y
p
_
y
x
y
y
y
z
_
=
_
rCos()Cos()
rCos()Sin()
rSin()
_
+N

(x)
_

_
If we repeat the derivation with the equivalent sensor model
we arrive at the same result since N

(x) will be altered during

the process of identifying an invariant noise resulting in the
same observer error dynamics. Fig. 8 and Fig. 7 presents the
gain stabilization and estimation capability of an EKF and an
IEKF with changed sensor models. The IEKF exhibits gain
convergence behavior with similar estimation performance as
the EKF.
0 5 10 15 20 25 30
10
2
10
0
10
2
L
o
g

p
o
s
i
t
i
o
n

e
r
r
o
r

(
m
)


-EKF
3-EKF
-IEKF
3-IEKF
0 5 10 15 20 25 30
10
0
10
2
L
o
g

h
e
a
d
i
n
g

e
r
r
o
r

(
d
e
g
)
Time (s)
Fig. 7. Relative position and heading estimation errors of EKF and the IEKF
for changed sensor models
0 5 10 15 20 25 30
2
1.5
1
0.5
0
0.5
1
1.5
2
2.5
3


0 5 10 15 20 25 30
0.6
0.4
0.2
0
0.2
0.4
0.6
0.8
1
Time(s)


IE
K
F
-
G
a
in
s
Time(s)
E
K
F
-
G
a
in
s
(a)
(b)
K
x
K
y
K
z
K
Fig. 8. Gain stabilization of the EKF (left) and the IEKF (right) for changed
sensor models
V. CONCLUSION
This paper presented a derivation of an IEKF for purposes
of relative localization between robots. The procedure followed
a symmetry preserving formulation. Its comparison with a
standard EKF in derivation and in performance was presented.
The results indicate an interesting characteristic of the lter in
gain stabilization as pointed out in studies related to attitude
heading reference system design using the IEKF [11]. This
nonlinear formulation of relative localization is geometrically
sound and enables the lter to be implemented with same
computational demand as required by an EKF. It is expected to
apply the solution experimentally to asses its performance in
real world spatial localization tasks. Incorporating additional
asynchronous measurements under the symmetry preserving
formulation is being investigated to develop practical and
efcient multi-robot localization frameworks with an emphasis
on respecting the geometry of the problem.
REFERENCES
[1] N. Michael, S. Shen, K. Mohta, Y. Mulgaonkar, V. Kumar, K. Nagatani,
Y. Okada, S. Kiribayashi, K. Otake, K. Yoshida, K. Ohno, E. Takeuchi,
and S. Tadokoro, Collaborative Mapping of an Earthquake-Damaged
Building via Ground and Aerial Robots, Journal of Field Robotics,
vol. 29, no. 5, pp. 832841, 2012.
[2] A. Bahr, M. R. Walter, and J. J. Leonard, Consistent Cooperative
Localization, in IEEE International Conference on Robotics and Au-
tomation, pp. 34153422, 2009.
[3] G. L. Mariottini, F. Morbidi, D. Prattichizzo, G. J. Pappas, and
K. Daniilidis, Leader-Follower Formations : Uncalibrated Vision-
Based Localization and Control, in IEEE International Conference on
Robotics and Automation, no. April, pp. 1014, 2007.
[4] A. Howard, M. J. Matari c, and G. S. Sukhatme, Putting the I in
Team : an Ego-Centric Approach to Cooperative Localization, in IEEE
International Conference on Robotics and Automation, pp. 868874,
2003.
[5] M. Cognetti, P. Stegagno, A. Franchi, G. Oriolo, and H. H. B ulthoff,
3-D Mutual Localization with Anonymous Bearing Measurements,
in IEEE International Conference on Robotics and Automation, no. 3,
pp. 791 798, 2012.
[6] S. Roumeliotis and G. Bekey, Distributed multirobot localization,
IEEE Transactions on Robotics and Automation, vol. 18, pp. 781795,
Oct. 2002.
[7] F. L. Markley, Attitude error representations for Kalman ltering,
Journal of guidance, control, and dynamics, vol. 26, no. 2, pp. 311
317, 2003.
[8] R. Mahony, T. Hamel, and J.-M. Pimlin, Nonlinear Complementary
Filters on the Special Orthogonal Group, IEEE Transactions on Auto-
matic Control, vol. 53, pp. 12031218, June 2008.
[9] G. Baldwin, R. Mahony, J. Trumpf, T. Hamel, and T. Cheviron,
Complementary lter design on the Special Euclidean group SE(3),
in European Control Conference, pp. 37633770, 2007.
[10] S. Bonnabel, P. Martin, and P. Rouchon, Symmetry-Preserving Ob-
servers, IEEE Transactions on Automatic Control, vol. 53, no. 11,
pp. 25142526, 2008.
[11] P. Martin and E. Sala, Invariant Extended Kalman Filter : theory and
application to a velocity-aided attitude estimation problem, in IEEE
Conference on Decision and Control, pp. 12971304, 2009.
[12] S. Shen, N. Michael, and V. Kumar, Autonomous indoor 3D explo-
ration with a micro-aerial vehicle, 2012 IEEE International Conference
on Robotics and Automation, pp. 915, May 2012.
[13] O. De Silva, G. K. I. Mann, and R. G. Gosine, Development of a
relative localization scheme for ground-aerial multi-robot systems, in
IEEE/RSJ International Conference on Intelligent Robots and Systems,
pp. 870875, 2012.
[14] O. De Silva, G. K. I. Mann, and R. G. Gosine, Pairwise Observable
Relative Localization in Ground Aerial Multi-Robot Networks, in
European Control Conference, 2014. Accepted.
[15] M. Barczyk, S. Member, and A. F. Lynch, Invariant Observer Design
for a Helicopter UAV Aided Inertial Navigation System, IEEE Trans-
actions on Control Systems Technology, vol. 21, no. 3, pp. 791806,
2013.
[16] S. Bonnabel, P. Martin, and P. Rouchon, Non-Linear Symmetry-
Preserving Observers on Lie Groups, IEEE Transactions on Automatic
Control, vol. 54, no. 7, pp. 17091713, 2009.