IWSCFF2019_DPE__1

IWSCFF 19-76

DISTRIBUTED MULTI-TARGET RELATIVE POSE ESTIMATION

FOR COOPERATIVE SPACECRAFT SWARM

Kai Matsuka

∗

, Elena Sorina Lupu

†

, Yashwanth Kumar Nakka

‡

Rebecca Foust

, Soon-Jo Chung

, and Fred Hadaegh

‖

Multi-agent relative state estimation is critical in enabling full swarm autonomy.

However, relative pose estimation of hundreds to thousands of cooperative agents

is challenging due to limited sensing, limited communication, and scalability. We

present a distributed algorithm for cooperative multi-agent localization with both

limited relative sensing and communication. Each agent locally exchanges the

relative measurements and jointly estimates the relative poses of its local neigh-

bors. Because the algorithm only estimates the local neighbors, the number of

states does not grow with the total number of agents given the same local sensing

and communication graphs, making the algorithm suitable for swarm application.

The proposed algorithm is applied to spacecraft swarm localization and verified

in simulation and experiments. Experiments are conducted on Caltech’s robotic

spacecraft simulators, the Multi-Spacecraft Testbed for Autonomy Research (M-

STAR), where each spacecraft uses vision-based relative measurements.

INTRODUCTION

Spacecraft swarms have the potential to revolutionize space technology by enabling missions like

distributed aperture telescopes, space structure assembly, and cooperative deep space exploration.

1, 2

These multi-spacecraft missions have several advantages over monolithic satellite missions, such

as robustness to agent loss and improved science return.

Relative state estimation is a critical

component of formation keeping, path planning, collision avoidance, and rendezvous and docking.

However, in order to enable full swarm autonomy in space, relative state estimation algorithms

must overcome several challenges that get increasingly difficult as the number of agents increases.

Even when the total number of agents is small, multi-agent systems face challenges such as limited

relative sensing and communication ranges. In addition, a swarm with a much larger number of

agents must use an algorithm architecture that does not grow with the total number of agents in

swarm to keep the computation tractable. Existing solutions for multi-agent localization carry the

∗

Graduate Research Assistant, Graduate Aerospace Laboratories of the California Institute of Technology, 1200 E. Cali-

fornia Blvd, Pasadena, CA 91125.

†

Graduate Research Assistant, Graduate Aerospace Laboratories of the California Institute of Technology, 1200 E. Cali-

fornia Blvd, Pasadena, CA 91125.

‡

Graduate Research Assistant, Graduate Aerospace Laboratories of the California Institute of Technology, 1200 E. Cali-

fornia Blvd, Pasadena, CA 91125.

Special Student, Graduate Aerospace Laboratories of the California Institute of Technology, 1200 E. California Blvd,

Pasadena, CA 91125.

Bren Professor of Aerospace, Graduate Aerospace Laboratories of the California Institute of Technology, 1200 E. Cali-

fornia Blvd, Pasadena, CA 91125.

‖

Senior Research Scientist and Chief Technologist, Jet Propulsion Laboratory, 4800 Oak Grove Dr, Pasadena, CA 91109.

states for all the agents in the swarm, they are not scalable for swarm applications. Instead, a local

estimation algorithm that does not grow with swarm size is desired.

Figure 1. Concept mission with a formation flying swarm of Cubesats inspecting a bigger spacecraft

In this paper, we present the Distributed Pose Estimation (DPE) algorithm for collaborative multi-

agent localization of an individual agent with respect to its local neighbors. In the proposed archi-

tecture, each agent senses the relative poses of its neighbors with respect to its local frame us-

ing vision-based techniques and communicates the relative pose measurements with an associated

confidence-level that acts as a prior to the DPE algorithm. The DPE algorithm localizes each agent

by fusing relative estimates received from its neighbors. Figure 2 shows an example of the local

relative sensing graph given the sensing and communication graph topologies of the whole swarm.

The local relative sensing graph can be viewed as a pose graph, over which each agent jointly es-

timates relative pose states. Hence, the algorithm can utilize the additional cyclic structure in the

relative sensing graph, thereby improving the observability.

The DPE algorithm shares measurements with local neighbors, but not their estimated states.

Estimated states from different agents do not depend on each other, thereby allowing for more

straightforward stability analysis. The DPE is formulated as a nonlinear estimation problem with

the measurement equation augmented by relative pose measurements received from the agents in

the local graph. In this paper, we use the Extended Kalman Filter (EKF) approach to solve the DPE

problem for real-time implementation. The DPE algorithm can also be implemented using other

nonlinear estimation algorithms such as an unscented Kalman filter, a particle filter, and an optimal

nonlinear observer.

The algorithm is verified using simulations and experiments. The two simulation scenarios con-

sider a planar relative orbital dynamics in a planetary orbit described by the Hill-Clohessy-Wiltshire

(HCW) equations, one with four spacecraft and another with sixty spacecraft. The DPE algorithm

is compared against an independent EKF run for each neighbor for which direct measurement are

observable. Robotic experiments are conducted on the Caltech’s robotic spacecraft dynamics simu-

lators, the Multi-Spacecraft Testbed for Autonomy Research (M-STAR).

Each spacecraft simulator

is equipped with a monocular camera and unique visual markers with known geometry, whose full

poses are extracted using a computer vision algorithm run on-board each spacecraft. The M-STAR

utilizes a motion capture system, which provides ground truth for algorithmic verification. In sum-

Figure 2. An example of a local relative sensing graph of agent 1 given the communi-

cation and sensing topologies.

mary, we present a distributed algorithm for vision-based pose estimation of multiple targets and its

experimental validation.

Related Work

Extensive work has been done in the general field of distributed estimation. Several algorithms

have been developed to effectively fuse measurements from distributed systems like the Kalman

Consensus Filter,

the Information-Weighted Consensus Filter,

or the Logarithmic Consensus.

These algorithms can be applied to the multi-agent localization problem. Several algorithms have

also been developed specifically for multi-agent localization. For spacecraft and robotic applica-

tions, the

-estimator estimates the relative translational states of a linear system switching sensing

topology,

and another algorithm estimates the relative pose of stationary agents without a common

reference frame.

In wireless sensor network applications, some algorithms

11, 12

estimate the sen-

sor locations with the knowledge of some agents in the network. One paper discusses the conditions

of observability for attitude states for multi-agent system with a relative sensing graph.

Another

discusses observability of multi-agent localization in the sense of graph connectivity.

The algorithms discussed above have been successfully tested with some small-scale multi-agent

applications, but they assume that the states to be estimated by each agent are the same. A challenge

arises when these algorithms are applied to large-scale swarm localization problems where the size

of the states to be estimated grows with the number of agents. Large-scale estimation problems can

be made tractable by limiting each agent to estimate only locally. As long as the size and complexity

of a local graph pertaining to each agent remains approximately the same, the computational load

for each agent does not change as more agents are added to the swarm. Estimation of local states in

a large-scale problem is studied in the context of static estimation for networked power systems.

15, 16

Similar to the existing literature, the algorithm proposed in this paper also considers local relative

states to achieve a tractable algorithm complexity for swarm applications. In contrast, our problem

involves dynamics and the states to be estimated are the local relative poses of neighbor agents.

Another important aspect of multi-agent localization is dealing with sensing and communica-

tion networks simultaneously. This is particularly relevant for spacecraft formation flying missions

where difficult sensing and power constraints exist. In general, the limitations for communication

and relative sensing graph may be different, and may occur simultaneously for swarm systems.

Much of the existing work focuses on either sensing or communication issues and not on the inter-

section of the two.

9, 10, 13

Some prior work

discusses the observability of the states when a com-

bination of relative sensing and communication networks are considered. While this addresses the

fundamental observability of the agent states given combined sensing and communication networks,

it is limited to a case where the communication has an infinite number of information exchange at

each time step. In this paper, we modify this notion of observability

to a case where the number

of information exchanges at each time step is also limited. Based on this observability, we create a

rule to select the size of the set of agents that each agent estimates.

The main contribution of this paper are as follows. First, we present a distributed, multi-agent

3-DOF localization algorithm for both limited sensing and communication networks. Estimated

states are local so the computation does not grow with the swarm size as long as the size of the

local sensing neighbor graph is fixed. Second, we provide a sufficient condition for when each

agent can observe its neighbors’ states. Based on this condition, we provide a rule to select which

agents are included in the local sensing neighbor graph. Finally we validate the proposed DPE

algorithm via simulation and experimentation using robotic spacecraft simulators with vision-based

pose extraction.

The outline of the rest of the paper is as follows. We first review preliminaries pertaining to the

graph theory notations, and the relative orbital dynamics. Next, we propose the Distributed Pose

Estimation (DPE) Algorithm. Then we describe the simulations and the robotic experiments results,

finally followed by the conclusion section.

PRELIMINARIES

Let

= (

)

denote a directed graph that describes the

relative sensing graph

, where

{

,...,N

}

denotes the set of agents and

denotes a set of edges. An edge

(

i,j

)

∈ E

when

-th agent measures the relative pose of

-th agent. Similarly, let

= (

)

denote the

communication graph

, a directed graph for the communication topology. We say

(

i,j

)

∈E

if there

is an information flow from

-th agent to

-th agent. Note that the measurement graph and the com-

munication graph may be different.

An agent

is a neighbor of

in the graph

(

i,j

)

∈E

(

)

The set of neighbors of

in graph

is defined as

{

∈ V |

(

i,j

)

∈ E

(

)

}

. An extended

set of neighbors is defined as

∪{

}

. A

subgraph

of a graph

is a graph

such that

(

)

⊆ V

(

)

(

)

⊆ E

(

)

. Let

→ V

be a mapping from an index of a set of vertices to

the element at that index.

The relative orbital dynamics of formation-flying spacecraft are described using a local-vertical,

local-horizontal (LVLH) attached to each spacecraft. The LVLH frame defined at the

-th spacecraft,

denoted

, is defined as follows in this paper: the

direction,

, is along the position vector to

the spacecraft; the

direction,

, is along the angular momentum vector of the spacecraft’s orbit;

and the

direction, T, completes the right-handed rule. The origin of the LVLH frame, defined for

each spacecraft, coincides with the center of gravity of the spacecraft. The body-fixed frame of

-th

spacecraft is denoted

, and its origin coincides with the center of gravity of the

-th spacecraft.

For two formation flying spacecraft with a separation distance much smaller than its orbit radius,

the attached LVLH frames have negligibly small attitude difference. This is because the radial and

angular momentum vectors for the two spacecraft are much bigger than their differences. For the

rest of the paper, we assume that each pair of agents has a small intersatellite distance compared to

the orbital radius so that the relative attitude between the LVLH frames attached to different agents

are assumed to be aligned.

Relative Orbital Dynamics: 3DOF System

This section reviews the equation of motion for the relative orbital dynamics. This paper con-

siders that spacecraft are flying in formation in a circular orbit around a planet. For spacecraft in a

circular orbit, the linearized relative orbital motion simplifies to the HCW equations assuming no

perturbations and a short distance from the origin of the relative frame. The relative state vector of

the

-th agent with respect to the

-th agent is denoted

j/i

and is defined as

j/i

[

j/i

j/L

]

(1)

where

j/i

∈

are the position and velocity vectors of agent

with respect to agent

and

j/L

∈

)

j/L

∈

are the attitude and angular rate of agent

with respect to LVLH frame

of agent

. Since all spacecraft are in the PRO with respect to all the other spacecraft, the relative

orbit dynamics can be conveniently defined with respect to

. The absolute state vector of the

-th

agent

∈

)

is defined as

[

i/L

]

(2)

where

i/L

∈

)

and

i/L

∈

are the attitude and angular rate of

with respect to

. Recall

that

denotes these vectors are expressed in

frame. Since we assumed that the orientation of

the LVLH frame for each spacecraft

are approximately equal to each other, we will denote this

superscript

with

from here on. While there exist nonlinear, higher fidelity dynamics models

that include

and air drag,

we focus on the hcw dynamics to illustrate the distributed aspects of

the algorithm.

The relative states

j/i

can be propagated via a linear time-invariant state transition matrix

j/i

(

) =

rel

j/i

(

)

, where

rel

[

]

(3)

where

= Φ

HCW

(∆

)

is the state transition matrix given by the solution to the HCW equations

and

is the state transition matrix for the double integrator system.

DISTRIBUTED POSE ESTIMATION ALGORITHM

In this section, we present the Distributed Pose Estimation Algorithm (DPE). The DPE algo-

rithm estimates the relative poses of a subset of agents in a swarm, given the relative sensing and

communication network topologies. First, each spacecraft makes relative pose measurements of

the neighbor agents. Then each spacecraft communicates its relative measurements and associated

measurement covariances with its communication neighbors. The agent

receives this information

from its communicating neighbor agent

∈ N

. Finally, using the relative measurements received

by communication, the DPE estimates the relative pose of the spacecraft. This algorithm is summa-

rized in Algorithm 1. The same algorithm is implemented on each agent.

Algorithm 1:

DPE Algorithm

Result:

Estimate

and

= ˆ

(0)

while

true

−

= ˆ

= Measure()

for

∈N

Broadcast(

)

∪

Receive(

)

end

= PruneUnobservable(

)

= EKF(

−

)

end

We begin by defining a local relative sensing graph

with respect to the agent

. The local

relative sensing graph is a subgraph of the relative sensing graph (i.e.

⊆ G

∫

). We suppose that

a list of vertices for the local relative sensing graph

is given (an approach on how to select

discussed in detail in later sections), and define the set of neighbor states to be

{

j/i

∈V

}

(4)

The goal of the DPE is to estimate the augmented state vector

∈

, which is a column

concatenation of

Relative Pose Sensing and Measurement Models

Each spacecraft is assumed to carry a relative pose sensor which directly measures the relative

pose of neighboring sensors in its field-of-view. The sensor is assumed to be capable of measuring

multiple targets simultaneously. For example, a monocular camera and computer vision algorithms

with known target geometry can provide such an estimate.

We define the planar true pose of

-th

body frame with respect to the

-th body frame as

[

j/i

]

[

(

i/L

)

j/i

j/L

−

i/L

]

(5)

The second equality relates the relative pose between agent

and

in terms of the states estimated

by agent

. Note that

i/L

is an absolute measurement of agent

and is not a dependent variable,

hence both the absolute and relative measurement equations are linear. Then a relative measurement

-th agent with respect to

-th agent is given by following

j/i

(6)

where

j/i

and

j/i

is a zero-mean Gaussian noise with covariance

j/i

. The subscript

j/i

denotes it is a relative measurement associated with the

-th agent observing the

-th agent.

The same relative measurement may be used in DPE algorithms running on multiple agents.

Since each agent estimates the relative pose of neighbors with respect to itself, the relative states

representing an agent are different for each agent. Therefore, while

j/i

represents a measurement,

its measurement model may be expressed in different ways, depending on which agent the estima-

tion is done. Specifically, each relative measurement may take three different forms. Suppose that

the

-th agent is the reference spacecraft, on which the DPE algorithm is run. The first model is the

case where the relative measurement of the

-th agent is made by the

-th agent itself. This results

in the trivial case of Equation 6. The second is the relative measurement of the

-th agent made by

the

-th agent. Because the agent

has

i/j

and

j/L

as states, the relative measurement becomes

i/j

(7)

where

i/j

= (

)

−

and

(

)

−

[

−

(

j/i

)

j/i

−

j/i

]

[

−

(

j/L

)

j/i

i/L

−

j/L

]

(8)

Rotation matrix is defined as

(

j/i

) =

[

cos (

j/i

)

−

sin (

j/i

)

sin (

j/i

)

cos (

j/i

)

]

(9)

such that

for some two dimensional vector

. Finally, when the

-th agent uses the

relative measurements between two of its neighbor agents

j,k

∈ N

then the relative measurement

model becomes

k/j

(10)

where

k/j

= (

)

−

and

denotes the addition of successive planar coordinate transforma-

tions defined as

(

)

−

[

(

j/i

)

(

k/i

−

j/i

)

k/i

−

j/i

]

[

(

j/L

)

(

k/i

−

j/i

)

k/L

−

j/L

]

(11)

The Jacobians of each form of these measurement equations are written as

j/i

∂h

j/i

∂x

j/i

[

(

i/L

)

]

(12)

i/j

∂h

i/j

∂x

j/i





−

(

j/L

)

−

∂R

∂θ

∣

j/L

j/i

−





(13)

k/j

∂h

k/j

∂x

j/i





−

(

j/L

)

∂R

∂θ

∣

j/L

(

k/i

−

j/i

) 0

−





(14)

j/k

∂h

j/k

∂x

j/i

[

(

j/L

)

]

(15)

Communication and Augmented Sensing

At each communication step, each agent broadcasts all of the relative measurements that it makes

and the associated covariance matrices to its neighbors, according to the communication graph

topology

defined earlier. For each edge

(

i,j

)

∈ E

, the relative sensing information is defined

(

i/j

(

i,j

))

, which is a collection of the measurement, covariance, and edge element of the

graph. Then, define

to be a set of a relative sensing information for all the direct measurements

-th agent makes. That is

{

(

i/j

(

i,j

))

∈

×E

∈ N

}

where subscript

denotes that it is a neighbor in the relative sensing graph

. At each communication time

step, each agent broadcasts this

to its communication neighbors.

Each agent also collects the information broadcast by its neighbors. First define the list of all of

the agents that agent

is aware of from direct sensing or from the sensing of agents with which it

communicates. Denote this set of agents as

. This set is obtained by

⋃

∈N

(16)

Next, define

as a set of all the relative sensing edges collected by the

-th agent. That is

{

(

m,n

)

∈E

∈

∈N

}

(17)

Let

denote the graph

= (

)

. In general, this graph may not be connected. Let

⊆

be the set of agents in the local relative sensing graph for

-th agent, selected based on the relative

sensing and communication topologies. One of the later subsections discusses how to select

such that all of the states are observable. Once

is given, one can compute an induced sub-graph

= (

)

from

= (

)

. This sub-graph is defined as a local relative sensing graph. Note

that because

⊆

⊆V

and

⊆

⊆V

is a sub-graph of

Recall that the measurement equations are given in three forms, Equations 6, 7, and 10. These

equations describe any of the the relative measurements in the

. The augmented measurement

model for the augmented measurement vector can be defined as

(

) +

(18)

where

stacks Equations 6, 7, and 10 for all of the edges

(

j,k

)

∈ E

and corresponding measure-

ment Jacobian as

∂h

(

)

∂x

∣

(19)

Note that because all of the measurement models only depends only on one or two agent states at a

time, the Jacobian

will be sparse in the row direction when there are many agents in

Using the augmented measurement and the augmented state

, the dynamical system to estimate

for each agent becomes

(

) +

(20)

(

) +

(21)

where

→

and

→

. The process noise term

∈

has

Gaussian distribution

∼N

)

is a block diagonal with

. Let

be the Jacobian of

the possibly nonlinear dynamics

∂f

∂x

(22)

Because each agent’s equations of motion are decoupled, this augmented Jacobian is a block diag-

onal collection of the Jacobians with respect to each agent’s dynamics.

Estimation Filter

Once the augmented state dynamics and the augmented measurement are well defined, the dy-

namical system of Equations 20 and 21 are in the standard form. A nonlinear estimation technique

of choice may be used to estimate this system. For the purposes of this paper, an EKF is used but

any general nonlinear estimator may be used. Note that

and

are standard Jacobians linearized

around the prior estimate, defined as

∂f

∂x

∣

= ̄

and

∂h

∂x

∣

= ̄

(ˆ

)

(23)

(24)

(25)

−

(26)

= ̄

(

−

(ˆ

))

(27)

= (

−

)

(28)

Remark.

When cycles occur in the local graph, the DPE takes advantage of the constraint from rel-

ative pose graph, and joint estimation of the relative poses improve the observability and estimation

confidence. On the other hand, if any neighbors are not part of a cycle in the local relative sensing

graph, the terms for those neighbors in Local Pose Graph EKF (Algorithm 1) can be decoupled into

a separate pose EKF for each agent

∈N

Observability

The DPE requires each agent to know the set of agents

to be included in the local relative

sensing graph. One natural criteria for selecting the set is whether the states of those neighbor

agents are observable to the agent. If they are not observable, they are pruned from the measurement

set. In particular, we consider that the number of communication exchanges at each time step is

limited. Suppose that one communication exchange is allowed and assume there is no memory is

used to store the measurements from the previous time histories. In this case, any agent

6∈

not observable. This is straightforward as no relative measurements are functions of those agents’

states.

is not connected in general. The following lemma is used to determine subset of agents

that are observable. For this lemma, we first define

= (

)

as the undirected graph generated

from the directed graph, where

∪{

(

u,v

)

∈

(

v,u

)

∈

}

Lemma 1.

Select a set

such that

∈ V

if and only if there exists a path between

and

in the

undirected version of the graph

. Then the augmented state vector

, defined as a concatenation

j/i

for all

∈ V

, is observable. Furthermore, the measurements required to observe these

states are represented as

such that

= (

)

is an induced graph of

= (

)

This means that the

-th agent is observable to

-th agent, if the relative sensing graph available

-th agent is connected. Then the set of observable agents is defined as

{

∈

}

that are

connected to

-th agent on

. The proof is as follows.

Proof.

Recall the augmented measurement equation

and the measurement Jacobian

are given

by Equation 18 and Equation 19. Define

= [

;

]

. which is part of an observability matrix

constructed from a linearized system. We show that

is full rank and therefore the augmented

states are observable.

Let

be the size of

and

be the size of

. Without a loss of generality, sort

such that

{

,...,d,d

+ 1

,...,N

}

where an edge exists to

∀

∈{

,...,d

}

(

i,j

)

∈E

(

j,i

)

∈E

and an edge does not exist to

∀

∈{

+ 1

,...,N

}

(

i,j

)

6∈E

and

(

j,i

)

6∈E

. Consider the state

j/i

for

-th agent,

{

,...,d

}

. For these agents, there exists a direct measurement edge

(

i,j

)

(

j,i

)

, so denote this as

-th measurement in

. Consider the block rows corresponding

-th measurement on

, given by either

j/i

(Equation 12) or

i/j

(Equation 13). Define

= [

j/i

;

j/i

rel

]

= [

i/j

;

i/j

rel

]

, depending on whether

j/i

i/j

are avail-

able. Among block rows corresponding to the measurement

, the only non-zero block sub-matrix

the block corresponding to

j/i

. It is easy to see that

has full column rank, and

hence

has independent columns for the columns corresponding to

j/i

. Because this is true for

arbitrary

{

,...,d

}

, all the first

columns are linearly independent.

Next, we show that

has independent columns for

+ 1)

-th to

-th column, via induction.

For each

∈{

+ 1

,...,N

}

, there is a path from

. Let

(

α,β

)

∈

denote an edge along

this path, where

is closer to

than

in the path. For the sake of induction, assume that the states

of all agents on the path from

are observable.

α/i

(i.e. columns of

corresponding to

α/i

)

are independent. Because

m,n

∈

, we know at least one measurement exists between agents

and

, which are

α/β

β/α

. The measurement model is of the form of Equation 10 and its

Jacobians

α/β

and

β/α

are given by Equations 14 and 15. Define

mα

= [

α/β

;

α/β,α

rel

]

and

mβ

= [

β/α

;

β/α

rel

]

, which are sub-matrices of

corresponding to columns of

α,i

and

β,i

respectively. Whether

β/α

is taken from Equations 14 or 15, it is easy to see that

mβ

has full column rank. Now for agents

∈ {

+ 1

,...,N

}

, the rows of

corresponding to

has two non-zero block matrices

mα

and

mβ

. By the assumption of the induction, the columns

corresponding to

α,i

are already independent. In other words, there exist another rows

corresponding to the measurement

mα

is full rank and

mβ

= 0

. Therefore, following matrix

has full column rank.

[

mα

mβ

]

(29)

This is a sub matrix of

. Therefore the columns corresponding to all of the agents on the path

from

are shown to be linearly independent. We know that the first agent in the path is

observable as it is part of

,...,d

agent. Therefore, by induction, we conclude that the columns of

corresponding to the states of all agents along the path, including

, are linearly independent.

This is true for arbitrary

∈

+ 1

,...,N

, therefore this concludes that

has a full column rank.

SIMULATION RESULTS

The DPE is applied to example scenarios in simulations for formation flying spacecraft. The first

example considers four spacecraft in low Earth orbit (LEO) with a 300 km altitude, that are placed

in a passive relative orbit (PRO) as shown in Figure 3(a). Figure 3(b) shows the assumed relative

sensing and communication network graphs. The relative sensing graph is described by the directed

sensor adjacency matrix







0 1 1 0

0 0 1 1

1 0 0 1

1 1 0 0







(30)

Transnational dynamics of each agent are given by relative orbital mechanics, the HCW equa-

tions. The attitude is represented by a double integrator, defined by a constant angular rotation at

the same rate as mean motion of the absolute orbit, so that the positive

axis of the each spacecraft’s

body frame points towards, but not necessarily aligned to, the center of the formation. The relative

measurements are obtained as described by Equation 6. The measurement noise for the position is

assumed to have a larger uncertainty in the radial direction (same as relative position vector from

observer to target) than that in a transverse direction (perpendicular to the radial direction). The

simulation is run for 3,000 sec, which is approximately half of an orbit.

(a) PRO Trajectory

(b) Sensor and Communication Graphs

Figure 3. Example scenario considered in the simulation

As a point of comparison, we also implement a single-target EKF, denoted ST. The ST only

uses the direct measurement of one target that the observer is estimating. This is equivalent to

the best estimate obtainable when the agents are not cooperating. Each agent may run multiple

copies of the ST, one for each target that it observes. The performance of the DPE and the ST are

compared in Figures 4(a) and 4(b). In Figure 4(a), the black triangles denote the absolute poses

of all of the spacecraft. The blue and orange triangles denote the relative pose estimates made

by spacecraft 1 (bottom), which is the observer spacecraft. A position error ellipse corresponding

to each position estimate is plotted based on the position covariance. Figure 4(a) shows that the

position error covariance for the DPE is much smaller than that of the ST. Especially the uncertainty

in the transverse direction is reduced significantly. This improvement is expected, as the DPE

estimates multiple relative poses of the neighbor spacecraft and utilizes the structure of the relative

pose network over its local graph. Also, the DPE can estimate the states for all three neighboring

spacecraft, even though there are only two direct relative measurements available to spacecraft 1.

Figure 4(b) shows the estimation error of spacecraft 2 as seen by the spacecraft 1 for both the DPE

and the ST. The DPE estimation error is smaller than that of the ST for both position and rotation

consistently.

(a) Rel. pose estimate of DPE and ST w.r.t. spacecraft 1

(b) Estimation errors of spacecraft 2 w.r.t. spacecraft 1

Figure 4. Four spacecraft simulation results for the DPE (blue) and ST (orange), with

the covariances represented as ellipses and the estimates represented as arrows

The second example considers sixty agents in LEO, where additional spacecraft are placed on

PRO trajectories at four radii. Each agent in the outer formation ring observes one neighbor space-

craft in the same ring in clockwise direction and one closest agent in the inner ring. This relative

sensing topology is given such that it represents the camera field of view and range constraints. Each

agent communicates with one closest neighbors from inner ring, two closest neighbors from same

ring, and two closest neighbors from the outer ring. Simulation results are shown in Figures 5(a)

and 5(b). Figure 5(a) shows that the DPE can estimate the states of five of its neighbor spacecraft,

even though it directly measures the relative poses of only two of them. In Figure 5(b), the statistics

of the total estimation error of the DPE and the ST are studied. For each sensing edge in the sensing

graph

∈{

,...,m

}

, define the position estimation error as

δp

(

) = ˆ

(

)

−

(

)

(31)

where

(

)

and

(

)

are the relative pose estimate (from either the DPE or the ST) and the true

state at time step

. The averaged error distance is defined as

δd

(

) =

∑

δp

(

)

(32)

The

δd

(

)

is computed for the DPE and the ST are plotted in the Figure 5(b). Figure 5(b) shows

that the position estimation error of the DPE is smaller than that of the ST on average.

EXPERIMENTAL RESULTS

The DPE algorithm is also verified in robotic experiments on-board the Caltech’s robotic space-

craft simulators, the Multi-Spacecraft Testbed for Autonomy Research (M-STAR).

Each spacecraft

is equipped with one monocular camera with a high field-of-view (FOV) lens and visual markers

(ArUco markers

) on each side of the spacecraft simulator as seen in Figure 6. Using the images

from the monocular camera on-board each spacecraft, a standard computer vision algorithm

com-

bined with additional bias calibration is used to detect and estimate the full pose of the markers, as

shown in Figure 7. The spacecraft pose is computed from the pose of the markers by transforming