Decentralized Formation Pose Estimation for
Spacecraft Swarms
Kai Matsuka
a,
∗
, Aaron O. Feldman
a
, Elena S. Lupu
a
, Soon-Jo Chung
a,b
, Fred
Y. Hadaegh
b
a
1200 E. California Blvd, Pasadena, CA, USA 91125
b
4800 Oak Grove Dr, Pasadena, CA, USA 91109
Abstract
For spacecraft swarms, the multi-agent localization algorithm must scale well
with the number of spacecraft and adapt to time-varying communication and
relative sensing networks. In this paper, we present a decentralized, scalable al-
gorithm for swarm localization, called the Decentralized Pose Estimation (DPE)
algorithm. The DPE considers both communication and relative sensing graphs
and defines an observable local formation. Each spacecraft jointly localizes its
local subset of spacecraft using direct and communicated measurements. Since
the algorithm is local, the algorithm complexity does not grow with the number
of spacecraft in the swarm. As part of the DPE, we present the Swarm Reference
Frame Estimation (SRFE) algorithm, a distributed consensus algorithm to co-
estimate a common Local-Vertical, Local-Horizontal (LVLH) frame. The DPE
combined with the SRFE provides a scalable, fully-decentralized navigation so-
lution that can be used for swarm control and motion planning. Numerical
simulations and experiments using Caltech’s robotic spacecraft simulators are
presented to validate the effectiveness and scalability of the DPE algorithm.
Keywords:
swarm localization, spacecraft swarm, large scale estimation, and
decentralized estimation.
1. Introduction
Spacecraft swarms have the potential to revolutionize the space industry
by enabling missions such as distributed aperture telescopes, space structure
assemblies, and cooperative deep space explorations (Chung and Hadaegh, 2011;
Cash, 2006; Gdoutos et al., 2018). These multi-spacecraft missions have several
advantages over monolithic satellite missions, such as robustness to individual
spacecraft loss and improved science return (Hadaegh et al., 2016; Brown et al.,
2009). Since ground-in-the-loop control of individual spacecraft is prohibitive
∗
Corresponding author
Preprint submitted to Advances in Space Research
June 10, 2020
for swarm missions, the spacecraft swarm must autonomously estimate their
states and maintain their formation.
An essential prerequisite for swarm motion planning and collision-avoidance
algorithms (Morgan et al., 2016) is swarm localization. Swarm localization is
a multi-agent localization problem where the number of agents is so large that
no single agent can maintain the information for all of the agents in the swarm.
Swarm localization is challenging for several reasons. First, in some multi-agent
localization algorithms for small-scale swarms, the time complexity scales at
least linearly with the formation size (Kamal et al., 2012; A ̧cıkme ̧se et al.,
2014). Hence, these algorithms are not suitable for large-scale swarms.
Another challenge is the requirement that each spacecraft must estimate
the absolute orbit of a reference spacecraft in order to define a common Local-
Horizontal, Local-Vertical (LVLH) frame estimate (see Figure 1). This estima-
tion is challenging for large-scale swarms as some of the spacecraft in the swarm
may not make a direct measurement of this reference spacecraft (Chung et al.,
2018; Bandyopadhyay and Chung, 2018). Any algorithm suitable for swarm
localization requires a novel approach that explicitly addresses these challenges.
9
3
4
2
6
8
5
7
Relative
Sensing
Comms
3
4
1
2
Observer
Local
Observable Subset
O
rbital
M
otion
푁
푅
푇
Common
LVLH Frame
푲
푰
푱
Earth
-
Centered
Inertial Frame
Figure 1: A spacecraft swarm and its relative sensing and communication networks. The
DPE estimates the spacecraft poses in the local observable subset with respect to the common
Local-Horizontal, Local-Vertical (LVLH) frame.
In this paper, we solve the swarm localization problem by proposing a local
algorithm called the Decentralized Pose Estimation (DPE) algorithm. Using the
DPE, each spacecraft estimates the states of only a local subset of the swarm.
Neighbor spacecraft cooperate to improve the pose estimates by sharing their
measurements. The DPE uses the result of nonlinear observability analysis to
determine the local observable subset of spacecraft given the
ad hoc
relative
sensing and communication graphs for each spacecraft. Then it jointly localizes
the spacecraft in the local observable subset by fusing the measurements that
are collected over the communication network. The DPE offers advantage over
the standard pose estimation without communication by improving the estima-
tion accuracy and by increasing the number of observable spacecraft. For our
2
specific implementation, we represent the attitude of spacecraft with quaternion
while the Extended Kalman Filter (EKF) estimates the error attitude state in
minimal coordinate at each time step. As part of the DPE, we also present
the Swarm Reference Frame Estimation (SRFE) algorithm, which allows each
spacecraft to co-estimate the common LVLH frame of the swarm in a decen-
tralized manner. The SRFE applies the decentralized consensus filter (Kamal
et al., 2013; Bandyopadhyay and Chung, 2018) to estimate the reference space-
craft that may be visible to only a subset of the spacecraft. The DPE combined
with the SRFE provides a fully decentralized navigation solution that can be
used in swarm motion planning.
The DPE algorithm was verified in simulations and real-time robotic ex-
periments. The DPE performance was compared against that of an Individual
EKF, wherein each spacecraft uses only its measurements to estimate only those
spacecraft it directly measures, and a Centralized EKF, which has access to all
the information in the swarm. The robotic experiment was conducted on Cal-
tech’s robotic spacecraft dynamics simulators, the Multi-Spacecraft Testbed for
Autonomy Research (M-STAR) (Nakka et al., 2018; Foust et al., 2018). The rel-
ative poses of spacecraft were estimated using vision on-board each spacecraft.
We validated the DPE estimate against the ground truth obtained from a mo-
tion capture system. In summary, this paper presents a scalable, decentralized
algorithm for swarm localization that is appropriate for on-board implementa-
tion.
1.1. Related Work
The literature review is divided into the following parts: the multi-agent
localization problem for a team of robotic systems, some domain-specific large-
scale estimation in other applications, estimation of the common reference frame
for a swarm, and experimental validation of vision-based relative pose estimation
algorithms.
The research in multi-agent localization, a problem of estimating poses of
multiple robots in a group, has advanced over the years in the robotics com-
munity as interest in multi-agent coordination (Yan et al., 2013) has increased.
While there already exists a variety of multi-agent localization algorithms in
the literature, they predicate on assumptions specific to the respective applica-
tion. Some research assumes that the team of agents rendezvous infrequently
and that the robots exchange information upon close encounters, such as Multi-
agent Simultaneous Localization and Mapping (Fox et al., 2000; Carlone et al.,
2010; Zhou and Roumeliotis, 2006; Aragues et al., 2011). In contrast, other
research on multi-agent localization assumes that the team of robots maintains
communication links over a possibly time-varying network (Martinelli et al.,
2005; Franchi et al., 2013; Roumeliotis and Bekey, 2002). Some prior work
explicitly considers joint pose estimation over a network of relative measure-
ments (Martinelli et al., 2005; Marelli and Fu, 2015; Kekatos and Giannakis,
2012; Blackmore and Hadaegh, 2009; Bezouska and Barnhart, 2019). However,
this work assumes all-to-all communication, so it is not scalable for swarm ap-
plications. In another perspective, multi-agent localization can be seen as a
3
distributed estimation problem where the states to be estimated are the poses
of the agents themselves. For a small-scale estimation problem, decentralized
consensus filters have been shown to be highly effective (Kamal et al., 2013;
Bandyopadhyay and Chung, 2018). However, these algorithms require all of the
nodes to have the same set of states. Since swarm localization is a large-scale
estimation problem, these algorithms also do not scale well with the number of
robots in the formation.
Large-scale estimation can be made tractable by limiting each node to esti-
mate only some subset of the swarm. In the context of networked power systems,
this problem has been studied for recursive estimation of static states (Kekatos
and Giannakis, 2012; Marelli and Fu, 2015). Another proposed decentralized
local algorithm addressed dynamical systems with an L-banded network struc-
ture (Khan and Moura, 2008), such as dynamics governed by partial differential
equations. While this approach yields a near-optimal result for the specific
problem, the problem assumes that each agent has a predetermined ordering of
the graph nodes. This is not valid for swarm localization problems where the
network topology is
ad hoc
. A scalable swarm localization algorithm requires
a new approach in which local estimation accommodates
ad hoc
networks in
terms of communication and relative sensing.
A separate but critical sub-component for swarm localization is the estima-
tion of a common reference frame for the swarm. Motion planning algorithms
typically require a local reference frame with respect to which the swarm poses
are defined. Some multi-agent robotics literature discusses the estimation of
a common reference frame (Nagavalli et al., 2014; Franceschelli and Gasparri,
2013; L ́opez-Lim ́on et al., 2014). For formation flying spacecraft applications,
the goal of the common reference frame estimation is to obtain knowledge of the
absolute orbital state. The estimated state vector is small, so the decentralized
consensus filter (Kamal et al., 2013; Bandyopadhyay and Chung, 2018) can be
applied to solve this problem.
Some previous literature on vision-based relative pose estimation involves the
implementation of the proposed algorithm on hardware. While there are few
in-flight experiments for vision-based relative navigation (Fourie et al., 2014),
most of the vision-based pose estimation algorithms were validated through
simulations (Capuano et al., 2020) or on the ground using high-fidelity testbeds.
For example, some research work studies the pose estimation for a cooperative
single target (Romano et al., 2007; Tweddle and Saenz-Otero, 2015; Zhang et al.,
2014) or an uncooperative one (De Jongh et al., 2020). However, there has not
been an experimental validation on a vision-based pose estimation for spacecraft
swarms.
1.2. Contributions
In this paper, we present a novel, decentralized swarm localization algorithm
called the Decentralized Pose Estimation (DPE) algorithm. The main contri-
butions of this paper are as follows. First, the DPE estimates the states of only
a local subset of spacecraft, so the algorithm complexity on each spacecraft
does not grow with the swarm size. Based on the
ad hoc
relative sensing and
4
communication graphs, the DPE uses the observability criteria to determine
the set of observable spacecraft. Second, we provide a strategy for estimat-
ing the common reference frame using a distributed consensus filter wherein
we apply the decentralized consensus filter for common LVLH frame estima-
tion for spacecraft swarms. We validated the algorithm in numerical simulation
and experiments. To the authors’ knowledge, the experimental validation of a
cooperative pose estimation algorithm using a swarm of spacecraft simulators
equipped with electro-optical sensors is also novel.
This paper extends our previous work (Matsuka et al., 2019) in the following
ways:
•
we modified the DPE such that the estimated states are poses with respect
to a common LVLH frame and the new measurement model includes ab-
solute pose measurements available from the spacecraft’s communication
neighbors,
•
we formulated the DPE for full six degrees-of-freedom, rather than the
planar dynamics considered in (Matsuka et al., 2019),
•
we developed the SRFE algorithm to estimate a common LVLH frame
using the decentralized consensus filter,
•
we provided more thorough numerical analyses illustrating the advantages
of the DPE,
•
we experimentally validated the algorithm in time-varying relative sensing
and communication networks.
The outline of the remainder of the paper is as follows. We first define nota-
tions and review the relative orbital dynamics in Section 2. Next, we describe
the DPE algorithm in detail in Section 3 and the SRFE algorithm in Section 4.
Then we describe the numerical simulations in Section 5 and the robotic exper-
iment results in Section 6. The conclusion is presented in Section 7.
2. Preliminaries
Let
G
s
= (
V
,
E
s
) denote a directed graph that describes the
relative sensing
graph
, with
V
=
{
1
,...,N
}
the set of spacecraft and
E
s
the set of edges. An
edge (
i,j
) is in
E
s
when the
i
-th spacecraft measures the relative pose of the
j
-th spacecraft. Similarly, let
G
c
= (
V
,
E
c
) denote the
communication graph
, an
undirected graph for the communication topology. We say (
i,j
)
∈E
c
if there is
a communication link between the
i
-th and the
j
-th spacecraft. Note that the
measurement graph and the communication graph may be different in general.
The out-neighbors of a node
i
in a graph
G
are defined as
N
i
=
{
j
∈V |
(
i,j
)
∈
E
(
G
)
}
and we use subscripts
s
and
c
to distinguish the neighbors for relative
sensing and communication graphs respectively. The neighborhood of node
i
is defined as
̄
N
i
=
N
i
∪{
i
}
. The degree of a node in a graph is defined as
d
i
=
∑
N
j
=1
A
ij
where
A
is the adjacency matrix of the graph and the maximum
degree ∆ of a graph is defined as ∆ = max (
d
i
).
A column concatenation of vectors
x
1
,...,
x
n
is written as
x
= [
x
1
;
...
;
x
n
]
or
x
=
‖
i
=1
,...,n
x
i
, where the bar over the variable denotes an augmented vari-
able defined as a concatenation of variables. The position and velocity of an
5
object
a
in frame
b
are denoted
p
a,b
,
v
a,b
. The attitude and angular rate of the
frame
a
with respect to the frame
b
are denoted
q
a,b
and
ω
a,b
. The function
R
(
·
) maps a quaternion onto a rotation matrix such that
x
b
=
R
(
q
a,b
)
x
a
where
x
a
,
x
b
are vectors expressed in frame
a
and
b
respectively. We use ˆ above
variables to denote the expectation.
The relative states of formation flying spacecraft are defined using a local-
vertical, local-horizontal (LVLH) frame that is attached to the common reference
spacecraft as shown in Figure 1. The LVLH frame, denoted by subscript
L
, is
defined as follows: the
x
direction,
R
, is along the orbit position vector to
the spacecraft; the
z
direction,
N
, is along the angular momentum vector of the
spacecraft’s orbit; and the
y
direction,
T
, completes the right-handed coordinate
system. The origin of the LVLH frame coincides with the center of gravity of
the reference spacecraft.
2.1. Relative Orbital Dynamics
This section reviews the equation of motion for the relative orbital dynamics.
For the rest of the paper, we assume that each spacecraft is in a near-circular
orbit, there are no perturbations and that all the spacecraft are in proximity
such that the relative orbital dynamics can be linearized to the Hill-Clohessy-
Wiltshire (HCW) equations (Schaub and Junkins, 2005). There exist some
dynamics models that include eccentricity or other perturbation effects (Ya-
manaka and Ankersen, 2002; Morgan et al., 2012; Sullivan et al., 2017); however,
we choose the HCW model dynamics to illustrate more clearly the decentral-
ized aspects of the algorithm. Suppose
p
i,L
,
v
i,L
are the position and velocity
vectors of the
i
-th spacecraft with respect to an LVLH frame that is commonly
known among all the spacecraft in the swarm. The dynamics of translational
states using the HCW equations are described by
[
̇
p
i,L
̇
v
i,L
]
=
A
t
[
p
i,L
v
i,L
]
+
B
t
w
t
(1)
where state matrix
A
t
and actuation matrix
B
t
are given by
A
t
=
[
0
3
×
3
I
3
A
vp
A
vv
]
,
B
t
=
[
0
3
×
3
I
3
]
A
vp
=
3
n
2
0
0
0
0
0
0
0
−
n
2
,
A
vv
=
0
2
n
0
−
2
n
0
0
0
0
0
(2)
where
n
is the mean anomaly of the reference spacecraft orbit and
w
t
∼
N
(
0
3
×
1
,
W
t
) is assumed to be a zero mean Gaussian process noise.
2.2. Attitude Dynamics
For specific implementation of the DPE, we choose quaternions to represent
the attitude components of the spacecraft state (Markley and Crassidis, 2014).
A quaternion is defined as
q
= [
q
v
;
q
s
] where
q
v
∈
R
3
and
q
s
∈
R
are the vector
6
and scalar components of the quaternion, respectively. A unit quaternion
q
∈
S
3
satisfies the constraint
q
>
q
= 1. Quaternion multiplication is denoted with the
group operator
⊗
and is defined as
q
′
⊗
q
=
[
q
′
s
q
v
+
q
s
q
′
v
−
q
′
v
×
q
v
q
′
s
q
s
−
q
′
v
·
q
v
]
.
(3)
The inverse of a quaternion is defined as
q
−
1
=
1
||
q
||
[
−
q
v
q
s
]
.
(4)
A small attitude perturbation
δ
q
∈
S
3
can be represented in a minimal co-
ordinate
a
∈
R
3
where the mapping from
a
∈
R
3
to
δ
q
is defined as follows
δ
q
(
a
) =
1
2
[
a
√
4
−
a
>
a
]
.
(5)
Suppose
q
i,I
and
q
ref
i,I
for the
i
-th spacecraft denote true and reference attitude,
respectively. Then attitude error
a
i,I
and angular rate error
δ
ω
i,I
are defined
such that
δ
q
(
a
i,I
) =
q
i,I
⊗
(
q
ref
i,I
)
−
1
,
(6)
δ
ω
i,I
=
ω
i,I
−
ω
ref
i,I
.
(7)
The kinematic differential equation for the attitude quaternion for the
i
-th
spacecraft body frame can be expressed in two equivalent forms
̇
q
i,I
=
1
2
Ω(
ω
i,I
)
q
i,I
=
1
2
Θ(
q
i,I
)
ω
i,I
(8)
where Ω(
ω
) and Θ(
q
) are matrices defined as
Ω(
ω
) =
[
−
ω
×
ω
−
ω
>
0
]
,
Θ(
q
) =
[
q
s
I
3
+
q
×
v
−
q
>
v
]
.
(9)
where the superscript
×
denotes a skew symmetric matrix. The attitude rate
of the
i
-th spacecraft is propagated with the following
̇
ω
i,I
=
−
J
−
1
ω
i,I
×
J
ω
i,I
+
J
−
1
w
a
(10)
where
J
is the inertia tensor of the spacecraft in the body frame and
w
a
∼
N
(
0
3
×
1
,
W
a
) is the attitude process noise modeled as a torque perturbation
with zero-mean white Gaussian noise.
2.3. Review of Error State Estimation
Because the standard EKF does not strictly enforce the manifold constraint
for quaternion, we estimate the error state estimation for the attitude compo-
nents. This is similar to the conventional attitude estimation techniques such as
7
the Multiplicative EKF (MEKF) (Markley, 2003). The main idea is to estimate
attitude error in a minimal coordinate at each step while using a quaternion
to provide a nonsingular attitude representation overall. The filtering involves
three steps: time update, measurement update, and reset. This section reviews
the time update of the error state and its covariance, as well as the reset step.
The state variables of the
i
-th spacecraft
x
i
are defined as
x
i
= [
p
i,L
;
v
i,L
;
q
i,I
;
ω
i,I
]
(11)
where
p
i,L
and
v
i,L
are relative positions and velocities of the
i
-th spacecraft
with respect to the swarm reference LVLH frame. Attitude parameters
q
i,I
and
ω
i,I
are the quaternion and the angular rate of the
i
-th spacecraft with respect
to the Earth-Centered Inertial (ECI) frame, respectively. Precisely speaking,
translational states and rotational states are expressed with respect to different
frames (i.e. the LVLH and the ECI frames). This is a convenient choice made
to simplify the relative orbital and attitude dynamics models.
The attitude parameters are decomposed into some reference and error
q
i,I
=
δ
q
(
a
i,I
)
⊗
q
ref
i,I
and
ω
i,I
=
ω
ref
i,I
+
δ
ω
i,I
, as defined in Eqs. (6) and (7). Non-
singular representation of
x
i
is denoted by the reference state vector
x
ref
i
= [
p
i,L
;
v
i,L
;
q
ref
i,I
;
ω
ref
i,I
]
.
(12)
At each filtering time step, the actual state to be estimated is the minimal
coordinate representation of state with respect to
q
ref
i,I
and
ω
ref
i,I
defined as
x
min
i
= [
p
i,L
;
v
i,L
;
a
i,I
;
δ
ω
i,I
]
.
(13)
We refer to this as the minimal state vector of the
i
-th spacecraft, denoted by
the superscript min. At each step,
q
ref
i,I
and
ω
ref
i,I
are selected such that the prior
estimate of
a
i,I
and
δ
ω
i,I
are identically zero.
The state vector
x
i
resides on a manifold
M
=
R
6
×
S
3
×
R
3
and we can
extend the notion of group operator to states in
M
as follows. Suppose
x
′
,
x
∈
M
. Then the group operator
is defined as
x
′
x
=
p
′
+
p
v
′
+
v
q
′
⊗
q
ω
′
+
ω
.
(14)
Suppose the error state between two states is defined as ∆
x
=
x
′
x
−
1
∈ M
,
whose components are denoted by ∆
x
= [∆
p
; ∆
v
; ∆
q
; ∆
ω
]. This error
state can be parameterized by a minimal state error ∆
χ
∈
R
12
defined as
∆
χ
= [∆
p
; ∆
v
;
a
(∆
q
); ∆
ω
]
.
(15)
Finally two states
x
,
x
′
and ∆
χ
are related by
x
′
= ∆
x
(∆
χ
)
x
(16)
8
where ∆
x
(
·
) :
R
12
→M
is the map from ∆
χ
to ∆
x
. The DPE applies Eq. (16)
at each reset step to apply the correction ∆
χ
, expressed in minimal coordinate,
to a prior reference state vector to finally obtain the posterior state vector for
each spacecraft.
Next, we derive the equation of motion for attitude error states, which is
necessary for computing the covariance time update. Taking the time derivative
of the error quaternion given by Eq. (6) and substituting Eq. (8), one can obtain
2
̇
δ
q
(
a
i,I
) =
[
ω
i,I
0
]
⊗
δ
q
(
a
i,I
)
−
δ
q
(
a
i,I
)
⊗
[
ω
ref
i,I
0
]
.
(17)
Substituting this into Eq. (5) leads to the equation of motion for attitude error
̇
a
i,I
=
(
2
̇
δ
q
(
a
i,I
)
)
1:3
=
1
2
(
(4
−
a
>
i,I
a
i,I
)
1
2
δ
ω
i,I
−
(2
ω
ref
i,I
+
δ
ω
i,I
)
×
a
i,I
)
.
(18)
Similarly, the equation of motion for angular rate error can be derived from
Eqs. (7) and (10)
̇
δ
ω
i,I
=
−
J
−
1
(
δ
ω
×
i,I
J
(
ω
ref
i,I
+
δ
ω
i,I
) +
ω
ref
i,I
×
J
δ
ω
i,I
)
+
J
−
1
w
a
.
(19)
Eqs. (18) and (19) together represent the attitude error dynamics. The Jacobian
of error attitude dynamics in Eqs. (18) and (19) with respect to error attitude
variables is given by
∂
̇
a
i,I
∂
a
i,I
∣
∣
∣
∣
a
i,I
,δ
ω
i,I
=0
=
(
−
δ
ω
>
i,I
a
i,I
2(4
−
a
>
i,I
a
i,I
)
1
2
−
ω
ref
i,I
×
−
1
2
δ
ω
×
i,I
)
∣
∣
∣
∣
a
i,I
,δ
ω
i,I
=0
=
−
ω
ref
i,I
×
(20)
∂
̇
a
i,I
∂δ
ω
i,I
∣
∣
∣
∣
a
i,I
=0
=
(
1
2
(4
−
a
>
i,I
a
i,I
)
1
2
I
3
+
a
×
i,I
)
∣
∣
∣
∣
a
i,I
=0
=
I
3
(21)
∂
̇
δ
ω
i,I
∂
a
i,I
=
0
3
×
3
(22)
∂
̇
δ
ω
i,I
∂δ
ω
i,I
∣
∣
∣
∣
δ
ω
i,I
=0
=
−
J
−
1
(
δ
ω
×
i,I
J
−
(
J
(
ω
ref
i,I
+
δ
ω
i,I
))
×
+
ω
ref
i,I
×
J
)
∣
∣
∣
∣
δ
ω
i,I
=0
=
J
−
1
(
(
J
ω
ref
i,I
)
×
−
ω
ref
i,I
×
J
)
(23)
Finally, covariance can be computed by solving the differential Lyapunov equa-
tion
̇
P
=
AP
+
PA
>
+
BWB
>
(24)
where
A
is Jacobian of propagation of states.
A
=
[
A
t
0
6
×
6
0
6
×
6
A
a
]
,
B
=
[
B
t
0
6
×
3
0
6
×
3
B
a
]
,
W
=
[
W
t
0
3
×
3
0
3
×
3
W
a
]
(25)
9
where
A
t
and
B
t
are given by HCW equations in Eq. (2) and
A
a
and
B
a
are
given by
A
a
=
[
−
ω
ref
i,I
×
I
3
0
3
×
3
J
−
1
(
(
J
ω
ref
i,I
)
×
−
ω
ref
i,I
×
J
)
]
,
B
a
=
[
0
3
×
3
J
−
1
]
(26)
Because the translational and rotational dynamics are decoupled, the state ma-
trix preserves a block diagonal structure.
3. Decentralized Pose Estimation (DPE) Algorithm
In this section, we delineate the DPE algorithm. The DPE estimates the
poses of a local observable subset of spacecraft in a swarm, given the relative
sensing and communication network topologies. First, each spacecraft measures
the poses of itself and its neighbors. Each spacecraft then communicates its
measurements and the associated measurement noise covariances to its commu-
nication neighbors
j
∈N
c
i
. Based on the available communication and relative
sensing networks at the given time, the augmented state vector is modified to
add newly detected spacecraft and subtract the spacecraft that became unob-
servable. Finally, each spacecraft jointly estimates the poses of the local space-
craft. This algorithm is summarized in Algorithm 1 and the following sections
explain the steps in detail. A copy of the same algorithm is implemented on
each spacecraft. First, we introduce the following definition to clearly define the
set of spacecraft to be estimated by the
i
-th spacecraft.
Definition 1.
The local observable set
V
i
∈V
for the
i
-th spacecraft is defined
as the union of the sensing neighborhood over the communication neighborhood.
That is
V
i
:
=
⋃
j
∈
̄
N
c
i
̄
N
s
j
.
(27)
This is the set of agents detected by the
i
-th spacecraft either via communi-
cation or via relative sensing in one communication step. The goal of the DPE
is for each spacecraft
i
∈ V
to estimate the state for each detected spacecraft
j
∈ V
i
. Suppose the cardinality of the local observable set is
N
i
= card(
V
i
).
We define the reference augmented state vector
x
ref
i
∈ M
N
i
and the minimal
state augmented vector
x
min
i
∈
R
12
N
i
for the
i
-th spacecraft as the column
concatenation of all states for
j
∈V
i
. That is
x
ref
i
=
‖
j
∈V
i
x
ref
j
,
x
min
i
:
=
‖
j
∈V
i
x
min
j
(28)
where
x
ref
j
and
x
min
j
correspond to the full and minimal state for the
j
-th space-
craft as defined in Eq. (11) and 13.
10
3.1. Absolute and Relative Measurement Models
Each spacecraft
i
∈V
is assumed to have an absolute pose measurement
y
i
with respect to the Earth-Centered Inertial (ECI) frame.
y
i
=
h
a
(
x
i
,
p
L,I
,
q
L,I
) +
ψ
a
i
=
[
R
(
q
L,I
)
p
i,L
+
p
L,I
q
i,I
]
+
ψ
a
i
(29)
where
ψ
a
i
denotes measurement noise.
p
L,I
and
q
L,I
describe the LVLH to ECI
transformation and are treated as fixed parameters known from the SRFE algo-
rithm. This measurement is available from GPS and a star tracker. We denote
the position and the attitude components of this observation as
y
i
= [
p
obs
i,I
;
q
obs
i,I
].
Since the attitude measurement is given as a quaternion, it is convenient to
transform the observation to a pseudo-measurement form (Markley, 2003)
̃
y
i
=
[
R
(
q
L,I
)(
p
obs
i,I
−
p
L,I
)
2
(
q
obs
i,I
⊗
(
q
i,I
)
−
1
)
1:3
]
.
(30)
Then this measurement can be modeled as
̃
y
i
=
̃
h
a
(
x
min
i
) +
̃
ψ
a
i
=
[
p
i,L
a
i,I
]
+
̃
ψ
a
i
(31)
where
̃
ψ
a
i
∼N
(
0
6
×
,
̃
Ψ
i
) is the absolute pseudo-measurement noise vector.
In addition to its absolute measurement, each spacecraft may have relative
measurements, possibly multiple at a given time. In this paper, each relative
measurement is assumed to be a pose measurement provided by a monocular
camera (Garrido-Jurado et al., 2014). The availability of relative measurements
depends on the physical constraints of the given sensors such as range, field-
of-view (FOV), and lighting. This information is captured by the edges in
the relative sensing graph
G
s
. The relative measurement is assumed to give
the relative pose of the observed spacecraft with respect to the observer. Let
y
j,i
= [
p
obs
j,i
;
q
obs
j,i
] denote the pose of the
j
-th spacecraft relative to the
i
-th
spacecraft. The relative attitude can be written in terms of reference and error
attitude as follows
q
j,i
=
q
j,I
⊗
(
q
i,I
)
−
1
=
δ
q
(
a
j,I
)
⊗
q
ref
j,i
⊗
δ
q
(
−
a
i,I
)
(32)
where
q
ref
j,i
=
q
ref
j,I
⊗
(
q
ref
i,I
)
−
1
. In the same way as absolute measurement, it is
more convenient to transform the relative measurement to a minimal parame-
terization. We define the relative pseudo measurement by
̃
y
j,i
=
[
p
obs
j,i
a
obs
j,i
]
(33)
where
a
obs
j,i
= 2
(
q
obs
j,i
⊗
(
q
ref
j,i
)
−
1
)
1:3
. Therefore, the minimal relative pose mea-
surement of the
j
-th spacecraft with respect to the
i
-th spacecraft is given by
̃
y
j,i
=
̃
h
r
(
x
min
i
,
x
min
j
,
x
ref
i
)
+
̃
ψ
r
j,i
.
(34)
11
where
̃
ψ
r
j,i
is relative pseudo-measurement noise and the measurement model is
given by
̃
h
r
(
x
min
i
,
x
min
j
,
x
ref
i
)
=
[
R
(
δ
q
(
a
i,I
))
R
(
q
ref
i,I
)
R
(
q
L,I
)
>
(
p
j,L
−
p
i,L
)
2
(
δ
q
(
a
j,I
)
⊗
q
ref
j,I
⊗
δ
q
(
−
a
i,I
)
⊗
(
q
ref
j,I
)
−
1
)
1:3
]
.
(35)
From this, the Jacobian of relative pseudo-measurement with respect to each
minimal state variable can be computed as follows
∂
̃
h
r
∂
p
i,L
=
[
−
R
(
q
ref
i,I
)
R
(
q
L,I
)
>
0
3
×
3
]
(36)
∂
̃
h
r
∂
p
j,L
=
[
R
(
q
ref
i,I
)
R
(
q
L,I
)
>
0
3
×
3
]
(37)
∂
̃
h
r
∂
a
i,I
=
[
[
R
(
q
ref
i,I
)
R
(
q
L,I
)
>
(
p
j,L
−
p
i,L
)
]
×
−
R
(
q
ref
j,i
)
]
(38)
∂
̃
h
r
∂
a
j,I
=
[
0
3
×
3
I
3
]
(39)
3.2. Communication and Augmented Sensing
At every communication step, each spacecraft broadcasts its sensing infor-
mation including both the absolute and the relative measurements. Absolute
sensing information is defined as
M
a
i
= (
y
i
,
Ψ
i
,i
). For each edge in the relative
sensing graph
G
s
, the relative sensing information is defined as (
y
j,i
,
Ψ
j,i
,
(
i,j
)).
Then, we define the set
M
r
i
to be the set of relative sensing information for all
of the direct measurements the
i
-th spacecraft makes:
M
r
i
=
{
(
y
j,i
,
Ψ
j,i
,
(
i,j
))
|
j
∈N
s
i
}
(40)
where
N
s
i
denotes the neighbors of the
i
-th spacecraft in the relative sensing
graph
G
s
. At each communication time step, each spacecraft broadcasts
M
a
i
and
M
r
i
to its communication neighbors.
Each spacecraft also collects the information broadcast by its neighbors. The
set of all of the relative sensing edges collected by the
i
-th spacecraft is
̄
E
s
i
:
=
{
(
j,k
)
∈E
s
|
j
∈
̄
N
c
i
}
.
(41)
The augmented relative observation, measurement model, and noise are defined
as
y
r
i
:
=
‖
(
j,k
)
∈
̄
E
s
i
̃
y
k,j
,
h
r
i
(
x
min
i
,
x
ref
i
)
:
=
‖
(
j,k
)
∈
̄
E
s
i
̃
h
r
(
x
min
j
,
x
min
k
,
x
ref
j
)
ψ
r
i
:
=
‖
(
j,k
)
∈
̄
E
s
i
̃
ψ
r
k,j
(42)
which are column concatenations over all of the relative sensing edges available
to the
i
-th spacecraft. Similarly, each spacecraft collects all of the absolute
measurements
y
a
i
:
=
‖
j
∈
̄
N
c
i
̃
y
j
,
h
a
i
(
x
min
i
)
:
=
‖
j
∈
̄
N
c
i
̃
h
a
(
x
min
j
)
,
ψ
a
i
:
=
‖
j
∈
̄
N
c
i
̃
ψ
a
j
(43)
12
The total augmented measurement is the collection of all of the relative and
absolute measurements. That is,
y
i
= [
y
a
i
;
y
r
i
],
h
i
= [
h
a
i
;
h
r
i
], and
ψ
i
= [
ψ
a
i
;
ψ
r
i
],
such that the augmented measurement equation becomes
y
i
=
h
i
(
x
min
i
,
x
ref
i
) +
ψ
i
.
(44)
The corresponding Jacobian linearized around the estimates
ˆ
̄
x
min
i
and
ˆ
̄
x
ref
i
be-
comes
H
i
=
∂
h
i
(
x
min
i
,
ˆ
̄
x
ref
i
)
∂
x
min
i
∣
∣
∣
∣
x
min
i
=
ˆ
̄
x
min
i
.
(45)
Since all of the measurement models depend only on one or two spacecraft states
at a time, each row of
H
i
will be sparse.
For each spacecraft
i
∈ V
, we have the propagation models for the full and
the minimal state vectors
̇
x
ref
i
=
f
(
x
ref
i
)
(46)
̇
x
min
i
=
f
min
(
x
min
i
,
x
ref
i
)
(47)
where the reference state model is given by collecting Eqs. (2), (8), and (10) and
the minimal propagation model is given by Eqs. (2), (18), and (19). Recall the
augmented state for the
i
-th spacecraft is
x
ref
i
. Then, the augmented dynamical
system for all spacecraft
j
∈V
i
is given by
̇
x
ref
i
=
f
i
(
x
ref
i
)
:
=
‖
j
∈V
i
f
(
x
ref
j
)
(48)
̇
x
min
i
=
f
min
i
(
x
min
i
,
x
ref
i
)
:
=
‖
j
∈V
i
f
min
(
x
min
j
,
x
ref
j
)
.
(49)
Integrating Eq. (48) propagates the previous prior estimate
ˆ
x
ref+
i
(
t
−
1) to the
current posterior estimate
ˆ
x
ref
−
i
(
t
), where the superscript
−
and + denotes prior
and posterior, respectively. Eq. (49), instead, is used to define the augmented
Jacobian
A
i
=
∂
f
min
i
(
x
min
i
,
ˆ
̄
x
ref
i
)
∂
x
min
i
∥
∥
∥
∥
x
min
i
=
ˆ
̄
x
min
i
.
(50)
Since the propagation of each state is decoupled, Eq. (50) is a block diagonal
where the diagonal block corresponding to
j
∈ V
i
is given by
A
(
x
ref
j
) from
Eq. (26). Using these equations, the augmented posterior covariance from the
previous step can be updated to the current prior covariance using
̇
P
i
=
A
i
P
i
+
P
i
A
>
i
+
B
i
W
i
B
>
i
.
(51)
While the augmented state matrix
A
i
and the process noise covariance term
B
i
W
i
B
>
i
are block diagonal, Eq. (51) has to be solved simultaneously because
P
i
is not diagonal.
13
At the measurement update of the DPE, the Kalman gain, the posterior co-
variance, and the state correction terms are computed similarly to the standard
EKF:
K
i
=
P
−
i
H
>
i
(
H
i
P
−
i
H
>
i
+
Ψ
i
)
−
1
(52)
P
+
i
=
(
I
−
K
i
H
i
)
P
−
i
(53)
∆
̄
χ
i
=
K
i
(
y
i
−
h
i
(
x
min
i
,
x
ref
i
))
(54)
Because the correction term ∆
̄
χ
i
is expressed in the minimal coordinate
R
12
N
i
,
we apply the reset step to the augmented reference state to recover the posterior
estimate of the state
x
ref
i
+
= ∆
̄
x
i
(∆
̄
χ
i
)
x
ref
i
−
.
(55)
The definition of the group operator
and the mapping between the tangent
space are extended to those for the augmented vector by simply applying the
operations for each of
j
∈V
i
.
For the purposes of this paper, we implemented the DPE algorithm with
specific definitions for the state, measurement, and dynamics models. However,
the strategy of defining the augmented state vector and measurements in this
paper can be extended to different scenarios.
3.3. Adding and Subtracting Nodes to Set
V
i
The local observable set
V
i
from Eq. (27) may vary at each time step, based
on the relative sensing and communication graphs
G
s
and
G
c
at the given time.
The DPE modifies the augmented state vector
x
ref
i
and its associated covariance
P
i
if the set
V
i
has changed over time.
A new spacecraft
j
is added to
V
i
at time
t
if a measurement of the new
spacecraft becomes available at the new time step. A new measurement becomes
available to an agent either when (i) the agent itself or one of its communication
neighbors detects a new spacecraft or (ii) the measurement becomes available
through the addition of a new communication link. The DPE waits for two
consecutive pose measurements, such that the velocities of the new spacecraft
are computed by numerical differentiation of the two pose measurements.
The DPE adds the new spacecraft states for
j
∈ V
i
to
ˆ
x
i
by adding a
new state
x
ref
j
directly computed from the positions and velocities. A block
column and a block row are added to the covariance matrix when initializing
the state. Assuming that
x
ref
j
is independent of
ˆ
x
i
at the previous time state,
the augmented covariance matrix is created by adding a new set of rows and
columns with a prescribed specified initial uncertainty. The off-diagonals are
zeros since
x
ref
j
and
ˆ
x
i
are independent.
The observer spacecraft may also stop estimating a spacecraft if a previously
estimated spacecraft becomes unobservable. Depending on the application, the
dynamics of the unobservable states may be propagated by the dynamics model
without the measurement updates for a fixed maximum number of rounds. If
a new measurement becomes available for the spacecraft before the maximum
14
number of rounds, the measurement update is applied and the count is reset.
The spacecraft state and associated covariance blocks are deleted if the count
exceeds the specified maximum number.
The DPE algorithms explained in the above sections can be summarized in
Algorithm 1.
Algorithm 1:
DPE Algorithm
Result:
Estimate
ˆ
x
ref+
i
(
t
) and
P
+
i
(
t
)
1
Initialize
ˆ
x
ref+
i
(0) and
P
+
i
(0)
2
while
true
do
3
ˆ
x
ref+
i
(
t
−
1),
P
+
i
(
t
−
1) = Reassign(
ˆ
x
ref+
i
(
t
),
P
+
i
(
t
))
4
Get measurements
M
a
i
,
M
r
i
5
for
j
∈N
c
i
do
6
Exchange measurements (
M
a
i
,
M
r
i
) and (
M
a
j
,
M
r
j
)
7
end
8
Collect measurements:
̄
M
a
i
=
⋃
j
∈
̄
N
c
i
M
a
j
,
̄
M
r
i
=
⋃
j
∈
̄
N
c
i
M
r
j
9
Update (
ˆ
x
ref+
i
(
t
−
1),
P
+
i
(
t
−
1)) according to
V
i
10
ˆ
x
ref
−
i
(
t
),
P
−
i
(
t
) = Time Update(
ˆ
x
ref+
i
(
t
−
1),
P
+
i
(
t
−
1))
11
∆
̄
χ
i
,
P
+
i
(
t
) = Measurement Update(
ˆ
x
−
i
,
P
−
i
,
̄
M
a
i
,
̄
M
r
i
)
12
ˆ
x
ref+
i
(
t
) = Reset(∆
̄
χ
i
,
ˆ
x
ref
−
i
)
13
end
3.4. Nonlinear Observability
Assuming that the swarm has limited sensing and limited communication, it
is important to determine which subset of spacecraft in the swarm is observable.
The observer system for the
i
-th spacecraft in terms of
x
i
is constructed from
Eqs. (44) and (48). As usual, the following observability analysis assumes the
deterministic nonlinear observer system.
{
̇
x
i
=
f
i
(
x
i
)
y
i
=
h
i
(
x
i
)
(56)
We analyze Eq. (56) to determine its nonlinear observability. First, we define
the terminology to make the discussion more concrete.
Definition 2.
Suppose
i,j
∈ V
. We say agent
j
is observable to agent
i
if
j
∈V
i
and
x
j
, a subset of state vector
x
i
, is observable to
i
.
Definition 3.
We say a set of agents
S
i
⊆ V
is an observable set with respect
to agent
i
if agent
j
is observable to agent
i
for all
j
∈S
i
Recall that
x
i
=
‖
j
∈V
i
x
j
where
V
i
⊆V
. Any agent
j
6∈V
i
is not observable
to agent
i
because it is not a part of the local dynamical system. Therefore
j
∈ V
i
is a necessary condition for agent
j
to be observable to agent
i
. Using
this definition, we have the following proposition.
15
Proposition 1.
Suppose
j
∈
̄
N
c
i
and
k
∈
̄
N
s
j
for some
i
∈ V
. Then agents
j
and
k
are observable to agent
i
.
Proof.
We have that
j,k
∈ V
i
by Definition 1, so
x
j
and
x
k
are both parts of
the state
x
i
estimated by
i
-th agent in the nonlinear system Eq. (56). Now
we consider the part of Eq. (56) pertaining to agents
j
and
k
. We define
w
=
[
x
j
;
x
k
].
{
̇
w
=
f
p
(
w
)
z
=
h
p
(
w
)
(57)
where
f
p
(
w
) = [
f
(
x
j
);
f
(
x
k
)] and
h
p
(
w
) = [
h
a
(
x
j
);
h
r
(
x
j
,
x
k
)]. The measure-
ment model can be written as
h
p
(
w
) =
R
(
q
L,I
)
>
p
j,L
+
p
L,I
q
j,I
R
(
q
j,I
)
R
(
q
L,I
)
>
(
p
k,L
−
p
j,L
)
q
k,I
⊗
(
q
j,I
)
−
1
(58)
where
q
L,I
and
p
L,I
are known fixed parameters. The zeroth- and first-order
Lie derivatives of
h
p
are given by
L
0
h
p
(
w
) =
h
p
(
w
)
(59)
L
1
f
p
h
p
(
w
) =
∇
w
h
p
(
w
)
·
f
p
(
w
)
(60)
Based on the Lie derivatives above, the observability matrix is defined as follows
O
=
{
∇
w
L
l
f
p
h
p
(
w
)
|
l
∈
N
}
(61)
The observability rank condition (Hermann and Krener, 1977) states that if the
observability matrix
O
is full column rank, the nonlinear system Eq. (57) is
locally weakly observable. One can compute the gradient of the zeroth-order
Lie derivative to get
∇
w
L
0
h
p
(
w
) =
∇
w
h
p
(
w
)
=
R
(
q
L,I
)
>
0
3
×
3
0
3
×
4
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
4
0
3
×
3
0
4
×
3
0
4
×
3
I
4
0
4
×
3
0
4
×
3
0
4
×
3
0
4
×
4
0
4
×
3
−
R
(
q
i,I
)
R
(
q
L,I
)
>
0
3
×
3
Φ
1
0
3
×
3
R
(
q
i,I
)
R
(
q
L,I
)
>
0
3
×
3
0
3
×
4
0
3
×
3
0
4
×
3
0
4
×
3
Φ
2
0
4
×
3
0
4
×
3
0
4
×
3
Φ
3
0
4
×
3
(62)
where Φ
1
(
p
j,L
,
p
k,L
,
q
j,I
) and Φ
2
(
q
k,I
) are some functions that are generally
non-zero and Φ
3
= Φ
3
(
q
j,I
) is given by
Φ
3
(
q
) =
[
q
s
I
3
+
q
×
v
−
q
v
q
>
v
q
s
]
(63)
16
The gradient of the first-order Lie derivative is
∇
w
L
1
f
p
h
p
=
0
3
×
3
R
(
q
L,I
)
>
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
∗
1
2
Θ(
q
j,I
)
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
∗ ∗ ∗ ∗ ∗
R
(
q
j,I
)
R
(
q
L,I
)
T
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
3
∗ ∗
0
3
×
3
0
3
×
3
∗
1
2
Φ
3
(
q
j,I
)Θ(
q
k,I
)
(64)
where an asterisk denotes some non-zero block element of matched dimensions.
Because the nonlinear system Eq. (57) is infinitely smooth,
O
has an infinite
number of rows in general. However, it is sufficient to show that a finite number
of rows are linearly independent to determine local weak observability. With
this in mind, we consider only the rows corresponding to the zeroth and first
Lie derivatives.
O
=
[
∇
w
L
0
h
p
∇
w
L
1
f
p
h
p
]
(65)
After applying block row elimination,
O
reduces to
R
(
q
L,I
)
>
0
3
×
3
0
3
×
4
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
4
0
3
×
3
0
4
×
3
0
4
×
3
I
4
0
4
×
3
0
4
×
3
0
4
×
3
0
4
×
4
0
4
×
3
0
3
×
3
0
3
×
3
0
3
×
4
0
3
×
3
R
(
q
j,I
)
R
(
q
L,I
)
>
0
3
×
3
0
3
×
4
0
3
×
3
0
4
×
3
0
4
×
3
0
4
×
4
0
4
×
3
0
4
×
3
0
4
×
3
Φ
3
(
q
j,I
)
0
4
×
3
0
3
×
3
R
(
q
L,I
)
>
0
3
×
4
0
3
×
3
0
3
×
3
0
3
×
3
0
3
×
4
0
3
×
3
0
4
×
3
0
4
×
3
0
4
×
4
1
2
Θ(
q
j,I
)
0
4
×
3
0
4
×
3
0
4
×
4
0
4
×
3
0
3
×
3
0
3
×
3
0
3
×
4
0
3
×
3
0
3
×
3
R
(
q
j,I
)
R
(
q
L,I
)
>
0
3
×
4
0
3
×
3
0
4
×
3
0
4
×
3
0
4
×
4
0
4
×
3
0
4
×
3
0
4
×
3
0
4
×
4
1
2
Φ
3
(
q
j,I
)Θ(
q
k,I
)
(66)
Given
||
q
||
= 1,
R
(
q
), Φ
3
(
q
), and Θ(
q
) have full column rank. Therefore
O
has full column rank for arbitrary
j
∈
̄
N
c
i
and
k
∈
̄
N
s
j
. The observability rank
condition (Hermann and Krener, 1977) tells that the nonlinear system from
Eq. (57) is locally weakly observable.
Finally, we arrive at the following theorem.
Theorem 1.
Suppose the detected set of agents
V
i
⊆ V
for agent
i
is defined
as in Eq. (27). Then
V
i
is the largest observable set in
V
.
Proof.
Suppose
j
∈
̄
N
c
i
. Proposition 1 implies that
̄
N
s
j
is an observable set with
respect to the agent
i
. Moreover since
V
i
is defined as the union of all
̄
N
s
j
over
∀
j
∈
̄
N
c
i
,
V
i
is also an observable set with respect to agent
i
. Recall that
j
∈V
i
is a necessary condition for agent
j
to be observable to agent
i
because it has
no information on
l
∈V \V
i
. Therefore, we conclude that
j
∈V
i
is a necessary
and sufficient condition for agent
j
to be observable to agent
i
.
Theorem 1 states that all the detected sets of spacecraft
V
i
are observable
in this problem formulation. Moreover, no other spacecraft
j
∈ V \V
i
is ob-
servable to the
i
-th spacecraft, given the measurement models and the one-hop
17
communication limitation assumed in this problem. In other words, the DPE
algorithm estimates the states for all the agents in the largest local observable
subset
j
∈V
i
.
4. Consensus Estimation of Swarm Reference Frame
This section details the Swarm Reference Frame Estimation (SRFE) algo-
rithm. Sophisticated motion planning algorithms typically require a common
local reference (e.g. LVLH frame); however, finding such a reference frame is
a non-trivial estimation task. All of the spacecraft in the swarm must have an
estimate, and the swarm must reach a consensus on the common local reference
frame. We apply an information consensus filter (Kamal et al., 2013; Bandy-
opadhyay and Chung, 2018), which is a decentralized algorithm where a sensor
network co-estimates a state vector using the consensus algorithm (Olfati-Saber
and Murray, 2004).
In development of the SRFE algorithm, we make the following assumptions:
•
the communication graph
G
= (
V
,
E
c
) is undirected and connected at each
time step
•
the subset of agents in the swarm has the measurements of the absolute
pose of the reference spacecraft
•
the degree of the communication graph is upper bounded by a finite bound
d
max
. That is Card(
N
c
i
)
< d
max
for all
i
∈V
for some
d
max
<
+
∞
.
The assumption that the communication graph is undirected may be relaxed
so long as the graph is balanced (Olfati-Saber and Murray, 2004; Bandyopad-
hyay and Chung, 2018). To estimate the common LVLH frame, the state that
needs to be estimated is the absolute position and velocity of a reference space-
craft in the ECI frame. Denote
ξ
to be the reference spacecraft translational
state where
ξ
= [
p
L,I
;
v
L,I
] and
p
L,I
and
v
L,I
are the position and velocity
of the reference spacecraft in the ECI. Suppose a subset of spacecraft
W ⊆ V
measures the absolute pose of the reference spacecraft. These absolute pose
measurements may be obtained by combining GPS and relative pose measure-
ments, which are assumed to be available for the DPE. Then, the discrete-time
dynamics for the whole swarm are given by the following set of equations.
ξ
(
t
+ 1) =
f
s
(
ξ
(
t
)) +
w
s
, t
= 1
,
2
,...
ξ
(0) =
ξ
0
(67)
η
i
(
t
) =
H
s
ξ
+
ψ
s
i
, i
∈W
,
(68)
where
H
s
= [
I
3
0
3
×
3
] is the absolute measurement model for measurement
η
i
. Propagation is modeled by a nonlinear function
f
s
. The process and mea-
surement noises are denoted as
w
s
∼ N
(
0
6
×
1
,
W
s
) and
ψ
s
i
∼ N
(
0
3
×
1
,
Ψ
s
)
respectively and they are assumed to be independent. We define the estimate
of
ξ
by
ˆ
ξ
i
for a spacecraft
i
∈V
. The objective of the SRFE is to estimate
ˆ
ξ
i
in
an optimal fashion for all the spacecraft. To this end, we apply the information
consensus filter (Kamal et al., 2013) to the distributed dynamical system given
by Eqs. (67) and (68).
18
The SRFE first computes the proposal information vector
u
0
i
and the pro-
posal information matrix
U
0
i
, according to Eqs. (71) and (72).
N
denotes the
size of the swarm
N
= Card(
V
). This is computed on each spacecraft
i
∈ V
from the prior estimate
ˆ
ξ
−
i
and the information matrix
J
−
i
. Next, the swarm
communicates the consensus proposals to its neighbors and iteratively applies
consensus
K
times. For each iteration
k
, the SRFE uses the consensus to
compute a posterior information vector and information matrix according to
Eqs. (73) and (74) respectively. The consensus coefficient
must satisfy a stable
upper bound
<
1
/d
where
d
(
G
c
) is the maximum degree of the communication
graph. By the assumption that the communication graph has a finite degree,
we have
d
(
G
c
)
< d
max
. Then, we choose
such that
<
1
/d
max
to guarantee
convergence. The posteriori information state and information matrix are com-
puted according to Eqs. (75) and (76). The algorithm is modified such that it
uses nonlinear dynamics for time propagation of state. The Jacobian of
f
s
(
ξ
)
around
ˆ
ξ
i
is defined to be
F
i
=
∂
f
s
(
ξ
)
∂
ξ
∣
∣
∣
ξ
=
ˆ
ξ
i
and this is used for the covariance
time propagation. The SRFE algorithm is summarized in Algorithm 2.
The SRFE has multiple properties that make it advantageous for the com-
mon LVLH estimation. First, the information consensus filter asymptotically
approaches the optimal centralized estimate as
K
→ ∞
, assuming the dynam-
ical system is linear (Kamal et al., 2013). The optimal centralized estimate
refers to the Kalman filter solution given that the centralized nodes have access
to all the measurements Eq. (68), where
f
s
in Eq. (67) is linear. In practice, it
is known that the information consensus filter achieves near-optimal value even
if
K
is small (Kamal et al., 2013). The algorithm is strictly local and decentral-
ized, such that the spacecraft only requires local information exchange. Each
of the agents has an estimate of the reference trajectory even if some of the
spacecraft do not make a direct measurement. Also, this approach is agnostic
to whether the reference spacecraft or target is cooperative or uncooperative, so
long as some of the spacecraft in the swarm can measure the absolute position of
the reference in the ECI frame. In addition, the LVLH estimation has a unique
requirement that the reference trajectory obeys the orbital dynamics. This is
because the relative orbital dynamics such as HCW assume that the reference
trajectory follows the modeled dynamics. The SRFE algorithm estimates the
orbital states of an actual orbiting body; therefore, an unbiased estimate of
ξ
will also satisfy the orbital dynamics.
5. Validation of DPE and SRFE by Numerical Simulations
In the following three subsections, we discuss how the performance of the
DPE and the SRFE was verified. Using a satellite inspection mission scenario
as an example, we first illustrate the estimator convergence. Second, we quan-
titatively compare the computational time and the estimation error for an in-
creasing number of spacecraft in the swarm. Finally, the DPE was implemented
in a robotic experiment using Caltech’s robotic spacecraft simulators called the
19
Algorithm 2:
SRFE Algorithm
Result:
Estimate
ˆ
ξ
+
i
and
J
+
i
for each
i
∈V
1
Input:
ˆ
ξ
i
(0) =
ˆ
ξ
i
0
,
J
i
(0) =
J
i
0
2
while
true
do
3
Propagate dynamics
ˆ
ξ
−
i
(
t
) =
f
s
(
ˆ
ξ
+
i
(
t
−
1))
(69)
J
−
i
(
t
) =
(
F
i
(
J
+
i
(
t
−
1))
−
1
F
>
i
+
W
s
)
−
1
(70)
4
Get measurements
η
i
5
Compute consensus proposal vector
u
0
i
and matrix
U
0
i
u
0
i
=
1
N
J
−
i
(
t
)
ˆ
ξ
−
i
+
H
s
>
Ψ
s
η
i
(71)
U
0
i
=
1
N
J
−
i
(
t
) +
H
s
>
Ψ
s
H
s
(72)
Perform consensus on
u
0
i
and
U
0
i
6
for
k
= 1
to
K
do
7
Communicate
u
k
i
and
U
k
i
to all neighbors
j
∈N
c
i
8
Update:
u
k
i
=
u
k
−
1
i
+
∑
j
∈N
c
i
(
u
k
−
1
j
−
u
k
−
1
i
)
(73)
U
k
i
=
U
k
−
1
i
+
∑
j
∈N
c
i
(
U
k
−
1
j
−
U
k
−
1
i
)
(74)
9
end
10
Compute a posteriori state and information matrix
ˆ
ξ
+
i
(
t
) = (
U
K
i
)
−
1
u
K
i
(75)
J
+
i
(
t
) =
N
U
K
i
(76)
11
end
Multi-spacecraft Testbed for Autonomy Research (M-STAR). The experiment
considers time-varying relative sensing and communication graphs.
5.1. Numerical Simulation Example
This section verifies the performance of the DPE algorithm in a 6DOF nu-
merical simulation example. We consider an example mission scenario in a
circular, Low Earth Orbit (LEO) where three spacecraft cooperatively inspect
20