Relative Localization with Symmetry PreservingObservers

Oscar De Silva, George K.I. Mann, and Raymond G. GosineIntelligent Systems Lab, Faculty of Engineering and Applied Science,

Memorial University of Newfoundland,St. John’s, NL A1B 3X5, Canada.

Email: {oscar.desilva,gmann,rgosine}@mun.ca

Abstract—Symmetry preserving observer design is a recentlydeveloped approach for deriving estimators which exploits theinvariant properties of nonlinear systems. Multi-robot localizationis inherently a system operating on SE(3), thereby posing an in-teresting problem for application of the filter. This paper presentsan invariant extended Kalman filter design for the problemof multi-robot relative localization in 2.5D, for application inground and aerial mobile platforms. A detailed derivation ofthe invariant filter is presented with numerical results analyzingits performance against a traditional EKF approach to theproblem. The tracking errors, stability to random initializationand robustness to changing noise characteristics are evaluated.The strong geometric basis of the filter results in linear Kalmanlike gain convergence behavior which is desirable for numericalstability and applicability as a low cost scheduled gain observerto the problem.

I. INTRODUCTION

Relative localization is the process of estimating the for-mation within a team of robots. This enables the robotsto cooperatively navigate in hostile environments where anexternal positioning service such as the GPS is unavailable [1].A relative localization approach performs pose estimation byutilizing the inter-robot relative measurements in the team [2],[3]. Such robot-centric localization approaches are reported asefficient means to address the problem of localization withoutheavy 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 thisdomain 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 currentstate estimate which causes the filter to be unstable and evendiverge in cases with higher order nonlinearities. The linearizedapproximation additionally gives an inaccurate representationof the covariance which eventually leads to inconsistent up-dates. This is tackled by committing computational resourcesto Unscented Kalman Filtering(UKF) [3] or particle filteringapproaches.

A second problem of the EKF is that it assumes a linearstate error and an output error in its formulation. This is aninaccurate error representation for many applications where theEKF formulation tends to overlook the underlying geometryof the system in consideration. This is common to even UKF

solutions if applied directly without changing the linear errorstate assumptions. This problem is tackled by nonlinear filterswhich results in solutions with similar or reduced computationdemand in comparison to an EKF, but with increased difficultyin systematic design for a general system. Attitude headingreference systems are prime examples where exploiting thegeometry of the rotation group SO(3) results in commercialgrade solutions such as, the multiplicative extended Kalmanfilter [7], and the nonlinear complimentary filter [8], withsimilar and reduced computational demand in order.

Correspondingly, multi-robot systems operate in the specialEuclidean group SE(3), i.e., the group of translations androtations. Different nonlinear filter derivation on SE(3) arereported in literature where the solutions are deterministicformulations [9]. A more general approach is reported in[10], with a constructive method to derive filters for systemspossessing symmetries. This together with error state Kalmanfiltering approaches presented in [11], allows to systematicallydesign localization filters for robots with stochastic Kalmanlike gain tuning procedures.

This paper reports the design of a nonlinear symmetrypreserving filter for robots operating in 2.5D using the methodof symmetry preserving observer design. The main contri-bution of this work is the design of an Invariant ExtendedKalman Filter (IEKF) for relative localization of robots. Theresulting filter requires almost similar computation power dueto the EKF like observer structure. The filter design methodis compared against an EKF with comparative performanceanalysis on estimation errors and gain convergence.

II. RELATIVE LOCALIZATION IN 2.5D

A. System dynamics

The objective of a relative localization filter is to estimatethe relative pose x = (pi,

lRi) of a robot i with respect to alocal robot l. Information of the platform velocities and relativemeasurements between robots are available for this estimationproblem. 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 localrobot l is considered. Therefore, a relative localization filter in2.5D requires to estimate the position lpt = [x, y, z]T andthe orientation lRt := Rθ parameterized by relative headingθ. The system dynamics can be expressed by equation (1),where the vector u = [ vTl , ωl, vTi , ωi]

T captures the inputvelocities vi = [vx, vy, vz]

T , vl = [vlx, vly, vlz]T and

αβ

θ

l frame

i frame

r

x

i*frame

y

zp

vl

vi

ωl

ω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, wherethe non-roll non-pitch frame of reference i is considered inplace of the body fixed frame i∗, with all vector quantitiesexpressed in the new frame i. The random variable wv andwω capture the process noise which is assumed to be zeromean Gaussian with covariance E(wTw) = Q.

x = f(x,u,w)˙[pθ

]=

[Rθvi − vl − ωl × p+wv

ωi − ωl +wω

](1)

The outputs of the system are the relative measurementsbetween the robots. A position measurement sensor is assumedwhich measures the relative position yp with a measurementmodel given in (2). The measurement is corrupted with randomnoise ν with covariance E(νT ν) = R. More realistic relativesensing models which measure the relative range r and bearingα, β between robots are considered later in this paper.

y = h(x,u,ν)yp = p +νp

(2)

The system and measurement models presented denotes apractically realizable multi-robot team. Authors previous workstudies hardware realization [13] and observability analysis[14] of the particular system.

B. Extended Kalman Filter

A traditional EKF based estimator takes the form (3) witha prediction term f(x,u) and a correction term KE, where alinear 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 first order Taylor series linearization ofthe 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 defined as x − x. The dynamics of thestate error is found by subtracting (4) from (3) to deduce alinear Kalman structure.

˙x = (A−KC)x−Mw +KNν[A =

∂f

∂x, C =

∂E

∂x, M =

∂f

∂w, N =

∂E

∂ν

] ∣∣∣∣(x,0,0)

The Kalman gain computation and covariance propagationtakes the usual linear form.

P = AP + PAT +MQMT − PCT (NRNT )−1CPK = PCT (NRNT )−1

For the system under study the matrices A,C,M,N reads

A =

[S(−ωl) d/dθ(Rθ)vi013 0

]C = [I33 031]M = I44N = I33

The operator S(.) refers to the skew symmetric representationof a vector in standard notation. It is evident that the matricesare dependent on the current state estimate θ. As a result nonconverging gains are expected. This can be partly traced to thelinear state and output errors assumed during the formulation,which does not consider the geometry of SE(2)1,where theerror between the estimator frame and the actual frame isT−1T if a homogenous transformation matrix parametrizationT is assumed. The next section presents the design of a filtersuch that the geometry and the underlying properties of thegroup 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 asystematic approach to derive an observer which preserves thesymmetries of the system. A tutorial of the filter formulation isgiven in [15], for readers interested in application. The designmethodology is presented in the coordinate free frameworkof differential geometry. The following definitions are usedthroughout the derivation of the IEKF.

Definition 1: A set G is a Group if for all g ∈ G,we can define a Group operation (multiplication) � with acorresponding identity element e, and an inverse element g−1,which satisfies the conditions of closure and associativity.

The set of translations and rotations (p, θ) = g ∈ SE(2) inwhich the robots operate can be identified as a group with thegroup operation for g0, g ∈ G defined as (5), which satisfiesclosure g0 � g ∈ G, and associativity (g0 � g) � g = g0 � (g � g).An identity element e = (0, 0, 0, 0) ∈ G and an inverseelement g−1 = (−RT

θ p, − θ) ∈ G can be identified such thatg � g−1 = e.

g0 � g =

(p0

θ0

)�(pθ

)=

(Rθ0p+ p0

θ + θ0

)(5)

1more precisely SE(2)×R1 since we are considering 2.5D

Definition 2: A smooth map φg is a Group action on a setM, if (g,m) ∈ G×M⇒ φg(m) ∈M, s.t. φe(m) = m andφg1(φg2(m)) = φg1�g2(m).

As an example, the group operation φg1(g2) = g1 �g2 ∈ Gcan be considered as a trivial smooth map, where a group Gacts on itself. More examples of group actions are given in theequation set (6).

Definition 3: The system x = f(x,u) is G-invariant iff(ϕ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 .

Definition 4: The output y = h(x, u) is G-equivariant ifh(ϕg(x), ψ(u)) = ρg(y), for group action ρg(y) on outputsy ∈ Y .

The system model (1) is found to be G-invariant and themeasurement model (2) is found to be G-equivariant for a setof smooth maps. This is identified in the derivation process ofthe filter (Equations 6,7, and 8).

The process of deriving the invariant extended Kalmanfilter is separated to four main steps for clarity. First, thesystem symmetries are identified. Second, the complete set ofinvariants are found using the moving frame method. Third,an invariant observer is formed using the general form givenby [10]. The final step derives an error state Kalman filter forstabilization of the error state dynamics in order to find thegains of the observer.

B. IEKF- Relative localization

1) System symmetries: Identifying symmetries of the sys-tem is the process of finding 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) define group actionson states ϕg : G ×X → X , on input ψg : G × U → U , andon measurements ρg : G× Y → Y as follows

ϕg

(pθ

)=

(Rθ0p+ p0

θ + θ0

)

ψg

vlωlviωi

=

Rθ0vl − ωl × p0

ωlviωi

ρg (yp) = (Rθ0yp + p0)

(6)

Consider a group element g0 = (p0, θ0) ∈ G. Equationset (7) verify that the system given by (1) is G-invariantunder the group actions ϕg and ψg , i.e., Dϕg ◦ f(x,u) =f(ϕg(x), ψg(u)).

Dϕg ◦ f(x,u) = ddtϕg

(pθ

)= d

dt

(Rθ0p+ p0

θ + θ0

)⇒(Rθ0 p

θ

)=

(Rθ0(Rθvi − vl)−Rθ0ωl × p

ωi − ωl

)⇒ f(ϕg(x), ψg(u))

(7)

Here the smooth map Dϕg can be identified as a groupaction on the tangent space of X , i.e., Dϕg : G×T 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θ0p+ p0

= Rθ0yp + p0 ⇒ ρg(y)(8)

The particular group action ϕg on the state space relatesto translation and rotation of the reference coordinate systemto a different location attached to the robot. The mappings ofinputs u and outputs y to this new frame of reference is definedby the group actions ψg , ρg . Thus, the group actions do notchange the intrinsics dynamics of the system, rather they aretransformed to a new coordinate representation.

2) Moving frame, Invariants, Invariant frame : To identifythe complete set of invariants, the moving frame method isapplied [10]. The moving frame is identified by solving theequation φg(x) = c = e for g := γ(x).

ϕg

(pθ

)=

(Rθ0p+ p0

θ + θ0

)=

(03×10

)⇒ γ(x) = (−RT

θ p,−θ)

Then the set of invariants I(x,u), J (x,y) is found byoperating γ(x) on the inputs u and the output y.

I(x,u) = ψγ(x)

vlωlviωi

=

RTθvl + ωl ×RT

θ pωlviωi

=

IvlIωlIviIωi

J (x,u) = ργ(x) (yp) = RT

θyp −RT

θp

The Invariant frame is defined to be the mapping of basisvectors ν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θν

pi τ + p

νθi τ + θ

)|τ=0

=

(Rθei1

)⇒W = diag(RθI3, 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 thegeneral 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 inextended Kalman filters and the term W.L.E is the correctionterm with gain L, output error E , and invariant frame W .

The output error E is not the usual linear innovation termh(x,u) − y in EKFs, rather it is a nonlinear invariant errorterm. This error is defined 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)= RT

θ(p− yp)

Substituting the invariant output error and the invariant frame,we get the invariant observer as(

˙p˙θ

)=

(Rθvi − vl − ωl × p

ωi − ωl

)+

(RθLpR

Tθ(yp − p)

LθRTθ(yp − p)

)(9)

This completes the symmetry preserving observer design. Thegains (Lp, Lθ) should be selected such that the invariantobserver (9) is stable. For a systematic local gain selection,the observer is formed to an invariant extended Kalman filterfollowing the procedure in [11].

4) Error State Kalman Filter: Error state Kalman filteringallows Kalman like adaptive gain stabilization of nonlinearobservers. The filter derivation differs in its nonlinear esti-mation error η and nonlinear output error E in place of theusual linear error state x and output error E seen in theEKF formulation. The EKF formulation given in section 2first linearizes the system dynamics before deducing the errorstate dynamics of the observer. In error state formulationconsidered here, first, the error dynamics are formulated usingthe system equations (1) and observer model (9). This givesa nonlinear error dynamic system which is then linearized todeduce a Kalman like structure given in (3). This approach iscommon practice in many engineering applications includingmultiplicative attitude heading reference systems and nonlinearinertial navigation systems.

The invariant estimation error η of the observer is definedas η = ϕγ(x)(x)−ϕγ(x)(x). Notice that this is different fromthe usual liner estimation error term (x − x) seen in EKFs.The invariant estimation error reads as

η = ϕγ(x)(x)− ϕγ(x)(x)

=

(RTθ (p− p)

θ − θ

)=:

(ηpηθ

)

The process and measurement noises are introduced to thesystem equations to recover the form (1). To preserve systemsymmetry, the noise terms are modified to invariant noiseterms. This is so that the G-invariant and G-equivariantproperty of the system is preserved with the addition of thenoise terms. The process noise w and measurement noise ν arereplaced by invariant noises M(x)w and N(x)ν respectively.Thus the requirement for G-invariance and G-equivarianceare modified 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 definitionsare satisfied when M(x) = Rθ and N(x) = Rθ.

(M(x)wv

M(x)wω

N(x)νp

)=

(Rθwv

wω

Rθνp

)

Now the error state dynamics of the system is derived as afunction of η and scalar invariants I. The invariant output

error E in terms of η reads

E(x,u,y) = RTθ(p− yp)

= RTθ(p− p−Rθνp)

⇒ E(η, ν) = RTηθηp −RT

ηθνp

Where Rηθ is the rotation error i.e. Rηθ = ϕRTθ(Rθ) =

RTθRθ. By differentiating the estimation error state, we have[

ηpηθ

]=

[RTθ (p− p) +RT

θ (˙p− p)

˙θ − θ

]

Identifying RTθ = S(ωl − ωi)Rθ and substituting from equa-

tions (1) and (9), we have the error state dynamics of theobserver.[

ηpηθ

]=

[S(−ωi)ηp + (Rηθ − I3)vi − wv + LpE

LθE − wω

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

˙δηp˙δηθ

)= (A−KC)

(δηpδηθ

)−M

(wpwω

)+KN (νp)

where the A,C,K,M,N matrices are identified as

A =

[S(−Iωi) S(e3)Ivi

013 0

]C = [I33 031]M = I44N = I33K = [Lp Lθ]

T

IV. RESULTS

The derived IEKF for relative localization is comparedagainst the traditional EKF for its performance in differentscenarios. 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(vlx , ωl, vix , ωi) were set to (0.5, − 0.5, 0.5, 0.5).The system and sensor models given by (1) and (2) arestochastically simulated with process and measurement noisesset to Q = diag(5e−3, 1e−5, 1e−5, 2e−2) and R = 0.05I33.Both the EKF and the IEKF were capable of successfullyestimating the trajectory with comparably similar performance.

0 5 10 15 20 25 3010

−2

100

102

Log

posi

tion

erro

r (m

)

µ-EKF3σ-EKFµ-IEKF3σ-IEKF

0 5 10 15 20 25 30

100

Log

head

ing

erro

r (de

g)

Time (s)

Fig. 3. Relative position and heading estimation errors of EKF and the IEKF

Fig. 3 presents the relative position and relative headingestimation errors along with the estimation of their covariance.The localization performance of both filters were found to berobust in multiple experiments with random initializations.

2) Gain convergence: Comparing with the correspondingfilter matrices of an EKF, it can be seen that the matrices of theIEKF do not depend on the current system state, rather theydepend only on the system invariants I. Thus if invariantsremain constant, behavior similar to a linear Kalman filterwith constant gains can be expected. The trajectories where theinvariants I remain constant are termed permanent trajectories.Fig. 4(a) illustrates the time evolution of the non-zero Kalmangains Kx,Ky,Kz , and Kθ of the EKF. In comparison, Fig.4(b) reports the capability of the IEKF to converge to steadystate gains similar to a linear Kalman filter. This was possiblesince the trajectory considered was a permanent trajectory withconstant velocities for the target robot.

3) Non-permanent trajectories: Changing velocities wereintroduced to the target robot to analyze the filter’s responsefor non-permanent trajectories. Fig. 5 illustrates the path andthe gain stabilization of the EKF for this case. The EKFdoes not exhibit convergence of its gains. The IEKF exhibitsconvergence to a new set of gains corresponding to eachpermanent trajectory. Fig. 6 illustrates the gain stabilization forthis case along with the velocities of the target robot duringeach time window.

4) Range bearing sensing: Although the positioning sensormodel used allows convenient derivation of the filter, inpractical 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

EKF-

Gai

ns

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)

IEKF

-Gai

ns

(a) (b)

KxKy

KzK

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

EKF

Gai

ns

Time(s)

KxKy

KzK

Actual trajectory

Estimated trajectory

Fig. 5. The simulated non-permanent trajectories (left) and the correspondingEKF 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)

IEKF

-Gai

ns

vix, ωi= 0.8, 0.8 vix, ωi= 0.8, -0.8 vix, ωi= -0.8, -0.8 vix, ωi= -0.8, 0.8

KxKy

KzK

Fig. 6. IEKF gain evolution for non-permanent trajectories

accurate error representation which measures the range andbearing of a target.

y = h(x)[rαβ

]=

√x2 + y2 + z2

Tan−1(y/x)

Tan−1(z/√x2 + y2)

+

[νrνανβ

](10)

By manipulating this to an equivalent position sensing sensormodel with first order noise approximation N ′(x), the samefilter can be applied to the problem with a change to the

mappings on how the noises enter the measurements.

yp

[yxyyyz

]=

[rCos(β)Cos(α)rCos(β)Sin(α)

rSin(β)

]+N ′(x)

[νrνανβ

]

If we repeat the derivation with the equivalent sensor modelwe arrive at the same result since N ′(x) will be altered duringthe process of identifying an invariant noise resulting in thesame observer error dynamics. Fig. 8 and Fig. 7 presents thegain stabilization and estimation capability of an EKF and anIEKF with changed sensor models. The IEKF exhibits gainconvergence behavior with similar estimation performance asthe EKF.

0 5 10 15 20 25 30

10−2

100

102

Log

posi

tion

erro

r (m

)

µ-EKF3σ-EKFµ-IEKF3σ-IEKF

0 5 10 15 20 25 30

100

102

Log

head

ing

erro

r (de

g)

Time (s)

Fig. 7. Relative position and heading estimation errors of EKF and the IEKFfor 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)

IEKF

-Gai

ns

Time(s)

EKF-

Gai

ns

(a) (b)

KxKy

KzK

Fig. 8. Gain stabilization of the EKF (left) and the IEKF (right) for changedsensor models

V. CONCLUSION

This paper presented a derivation of an IEKF for purposesof relative localization between robots. The procedure followeda symmetry preserving formulation. Its comparison with astandard EKF in derivation and in performance was presented.The results indicate an interesting characteristic of the filter ingain stabilization as pointed out in studies related to attitudeheading reference system design using the IEKF [11]. This

nonlinear formulation of relative localization is geometricallysound and enables the filter to be implemented with samecomputational demand as required by an EKF. It is expected toapply the solution experimentally to asses its performance inreal world spatial localization tasks. Incorporating additionalasynchronous measurements under the symmetry preservingformulation is being investigated to develop practical andefficient multi-robot localization frameworks with an emphasison 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-DamagedBuilding via Ground and Aerial Robots,” Journal of Field Robotics,vol. 29, no. 5, pp. 832–841, 2012.

[2] A. Bahr, M. R. Walter, and J. J. Leonard, “Consistent CooperativeLocalization,” in IEEE International Conference on Robotics and Au-tomation, pp. 3415–3422, 2009.

[3] G. L. Mariottini, F. Morbidi, D. Prattichizzo, G. J. Pappas, andK. Daniilidis, “Leader-Follower Formations : Uncalibrated Vision-Based Localization and Control,” in IEEE International Conference onRobotics and Automation, no. April, pp. 10–14, 2007.

[4] A. Howard, M. J. Mataric, and G. S. Sukhatme, “Putting the I inTeam : an Ego-Centric Approach to Cooperative Localization,” in IEEEInternational Conference on Robotics and Automation, pp. 868–874,2003.

[5] M. Cognetti, P. Stegagno, A. Franchi, G. Oriolo, and H. H. Bulthoff,“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. 781–795,Oct. 2002.

[7] F. L. Markley, “Attitude error representations for Kalman filtering,”Journal of guidance, control, and dynamics, vol. 26, no. 2, pp. 311–317, 2003.

[8] R. Mahony, T. Hamel, and J.-M. Pflimlin, “Nonlinear ComplementaryFilters on the Special Orthogonal Group,” IEEE Transactions on Auto-matic Control, vol. 53, pp. 1203–1218, June 2008.

[9] G. Baldwin, R. Mahony, J. Trumpf, T. Hamel, and T. Cheviron,“Complementary filter design on the Special Euclidean group SE(3),”in European Control Conference, pp. 3763–3770, 2007.

[10] S. Bonnabel, P. Martin, and P. Rouchon, “Symmetry-Preserving Ob-servers,” IEEE Transactions on Automatic Control, vol. 53, no. 11,pp. 2514–2526, 2008.

[11] P. Martin and E. Sala, “Invariant Extended Kalman Filter : theory andapplication to a velocity-aided attitude estimation problem,” in IEEEConference on Decision and Control, pp. 1297–1304, 2009.

[12] S. Shen, N. Michael, and V. Kumar, “Autonomous indoor 3D explo-ration with a micro-aerial vehicle,” 2012 IEEE International Conferenceon Robotics and Automation, pp. 9–15, May 2012.

[13] O. De Silva, G. K. I. Mann, and R. G. Gosine, “Development of arelative localization scheme for ground-aerial multi-robot systems,” inIEEE/RSJ International Conference on Intelligent Robots and Systems,pp. 870–875, 2012.

[14] O. De Silva, G. K. I. Mann, and R. G. Gosine, “Pairwise ObservableRelative Localization in Ground Aerial Multi-Robot Networks,” inEuropean Control Conference, 2014. Accepted.

[15] M. Barczyk, S. Member, and A. F. Lynch, “Invariant Observer Designfor a Helicopter UAV Aided Inertial Navigation System,” IEEE Trans-actions on Control Systems Technology, vol. 21, no. 3, pp. 791–806,2013.

[16] S. Bonnabel, P. Martin, and P. Rouchon, “Non-Linear Symmetry-Preserving Observers on Lie Groups,” IEEE Transactions on AutomaticControl, vol. 54, no. 7, pp. 1709–1713, 2009.