Consensus SE(3)-Constrained Extended Kalman Filter for Close Proximity Orbital Relative Pose Estimation

Abstract

In this paper, a recently proposed $SE\left(3\right)$-constrained extended Kalman filter (EKF) is extended to formulate a strategy for relative orbit estimation in a space-based sensor network. The resulting consensus $SE\left(3\right)$-constrained EKF utilizes space-based sensor fusion and is applied to the problem of spacecraft proximity operations and formation flying. The proposed filter allows for the state (i.e., pose and velocities) estimation of the deputy satellite while accounting for measurement error statistics using the rotation matrix to represent attitude. Via a comparison with a conventional filter in the literature, it is shown that the use of the proposed consensus $SE\left(3\right)$-constrained EKF can improve the convergence performance of the existing filter for satellite formation flying. Moreover, the benefits of faster convergence and consensus speed by using communication networks with more connections are illustrated to show the significance of the proposed sensor fusion strategy in spacecraft proximity operations.

1. Introduction

In recent years, there has been significant interest in satellite formation flying missions that have close proximity operations. The advantages of such missions over conventional missions are having higher redundancy as well as increased mission flexibility. Moreover, such missions have a lower injection payload mass, leading to reduced launch costs [1]. The application of such missions are frequent Earth observations for remote sensing applications, monitoring geographic regions of interest, telescopic formation for solar or deep space observations, computer tomography of clouds for better weather modeling, etc.

The spacecraft rotational and translational dynamic estimation is important for close proximity operations. In the literature, the significance of such estimation schemes for formation flying missions is documented [2,3,4]. Conventionally, the translation states are estimated from linearized models [5], while rotational motion states are estimated using Euler angles [6], quaternions [7], or other attitude parameterizations. However, minimal parameterizations such as Euler angles and classical and modified Rodrigues parameters always have a singularity, while quaternions are not unique and can suffer from the unwinding phenomenon when used for attitude control and estimation. Because of these demerits in the conventional approach [8], researchers are interested in estimation algorithms using rotation matrices on the Lie group $SO\left(3\right)$ [9,10].

A more essential and practical approach is the simultaneous estimation of translational and rotational motion states. In spacecraft formation flying missions, this is more important because the different types of torques, including gravity gradient torques, atmospheric drag, and solar radiation pressure, result in a coupling of the attitude and orbital dynamics. Thus, in order to have a compact formulation that includes both attitude and translational dynamics, the special Euclidean group $SE\left(3\right)$ [11] is useful. This nonlinear manifold is obtained from the semi-direct product of the Lie group of rotational motions $SO\left(3\right)$ with ${\Re }^3$ [12]. This formulation results in singularity-free unambiguous global representations of the rotational system and a compact description of the rigid body pose.

In the literature, a estimation technique based on Lie group $SE\left(3\right)$ is proposed by [13] that guarantees almost global asymptotic stability. Rigid body motion state estimation without known dynamic models is proposed by [14] using the Lagrange–d’Alembert principle. However, the stochastic properties of measurements are not taken into account in these techniques, unlike in the Kalman filtering method. Also, these methods are not capable of handling consensus feedback of the sensor information, which is highly advantageous for close proximity operations.

Researchers have proposed a few techniques that can handle consensus estimation of the states during formation flying missions. The initial research direction is in the design of the consensus Luenberger observer [15] and subsequently the design of a consensus Kalman filter for linear systems [16,17,18]. In the literature, the extended Kalman filter has been proposed by [19,20,21,22] in a consensus framework for handling measurements from multiple sensors within a communication network. However, these methods do not estimate the rotational and translational states in a single framework, which is crucial for close proximity operations and in formation flying missions.

Recently, an $SE\left(3\right)$-constrained EKF for rigid body pose estimation was proposed by the authors [23]. However, the methodology does not handle pose consensus estimation using information sharing between multiple space-based sensors; moreover, there are very few papers that address this requirement based on Lie group $SE\left(3\right)$. In the literature, using Lie algebra $\mathfrak{se}\left(3\right)$, a feedback filter with consensus design is available.

The novelty of this paper is in obtaining an $SE\left(3\right)$-constrained extended Kalman filter using consensus philosophy. It is applicable to any rigid body coupled estimation of rotational and translational states. To the authors’ knowledge, this paper represents the first attempt on sensor fusion of pose estimation. Moreover, the proposed filter in this paper is more complex and differs from the existing $SO\left(n\right)$-constrained filter [24] in that only a subset of configuration states is constrained, while it is not viable for sensor fusion in a communication network.

This paper is organized in the following manner: The spacecraft dynamics in the Hill frame along with an introduction to the $SE\left(3\right)$ formalism is provided in Section 2. The measurement architecture along with the assumed mathematical sensor models are provided in Section 3. The proposed consensus $SE\left(3\right)$-constrained extended Kalman filter in a continuous-discrete time framework ($SE\left(3\right)$-DEKF) is derived in Section 4 and the discretized variational integration used for numerical simulation is provided in Section 5. The proposed algorithm is validated in Section 6. Finally, the concluding remarks are provided in Section 7.

2. Spacecraft Dynamics on $\mathit{SE}\mathbf{\left(}\mathbf{3}\mathbf{\right)}$

Consider an inertial frame $\mathbb{O}$ ($XYZ$) in which spacecraft are orbiting around Earth, as shown in Figure 1. For simplicity, consider that the chief and deputy satellite are defined in a Hill reference frame $\mathbb{H}$ centered on a chief satellite. The x axis of this frame, i.e., $e_x$, is oriented along the radius vector of chief satellite $r_c$, which is from the center of Earth to the center of this frame. The z axis, i.e., $e_z$, points in the direction along the orbital angular momentum vector, which is perpendicular to the plane of the chief satellite orbit. Finally, the y axis, i.e., $e_y$, completes the triad [25,26,27,28]. Also, consider a body-fixed frame $\mathbb{B}$ ($xyz$), as shown in Figure 1, attached to the body of the deputy satellite.

2.1. Pose Kinematics

The rotation matrix C is defined to be the rotation matrix from the $\mathbb{H}$ frame to the $\mathbb{B}$ frame. It belongs to real special orthogonal group $SO\left(3\right)=\left\{{\Re }^{3\times 3};C^{T}C=I_{3\times 3};detC=1\right\}$. Let $p\in {\Re }^3$ denote the co-ordinates of the origin of $\mathbb{B}$ with respect to origin of $\mathbb{H}$ resolved in the $\mathbb{H}$ frame.

The special Euclidean group $SE\left(3\right)={\Re }^3⋉SO\left(3\right)$ is a six-dimensional Lie group representing the configuration space of a deputy satellite pose, which is given by the $4\times 4$ matrix, as follows:

$$\begin{array}{c}\hfill g=\left[\begin{array}{cc}C& 0_{3\times 1}\\ p^T& 1\end{array}\right]\in SE\left(3\right)\end{array}$$

(1)

The translational and rotational kinematics are

$$\begin{array}{c}\hfill \begin{array}{c}\dot{C}=-{\omega }^{\times }C,{\dot{p}}^T=v^{T}C\hfill \end{array}\end{array}$$

(2)

where $\omega$ is the angular velocity and v is the translational velocity of the deputy satellite expressed in the $\mathbb{B}$ frame. We define the cross product operator for the vector $x=\left[\begin{array}{ccc}{x}_1& x_2& x_3\end{array}\right]$ as

$$\begin{array}{c}\hfill x^{\times }=\left[\begin{array}{ccc}0& -x_3& x_2\\ x_3& 0& -x_1\\ -x_2& x_1& 0\end{array}\right]\in \mathfrak{so}\left(3\right)\end{array}$$

(3)

The kinematics in Equation (2), using Equation (1), can be rewritten as

$$\begin{array}{c}\hfill \dot{g}=-V^{\vee }g\end{array}$$

(4)

where the augmented velocity is defined as $V\triangleq {\left[\begin{array}{cc}{\omega }^T& v^T\end{array}\right]}^T$ and the wedge operator is defined as

$$\begin{array}{c}\hfill V^{\vee }=\left[\begin{array}{cc}{\omega }^{\times }& 0_{3\times 1}\\ -v^T& 0\end{array}\right]\in \mathfrak{se}\left(3\right)\end{array}$$

(5)

Define the following mathematical terminology for the sake of brevity: Consider the matrix $A=\left[\begin{array}{ccc}{a}_1& \dots & a_n\end{array}\right]\in {\Re }^{n\times n}$ with $a_i\in {\Re }^n$. Then, defining $vec\left(A\right)\triangleq {\left[\begin{array}{ccc}{a}_1^T& \dots & a_n^T\end{array}\right]}^T\in {\Re }^{n^2}$ as the vector with stacked columns of A, the converse operation is defined such that $mat\left(vec\left(A\right)\right)\triangleq A\in {\Re }^{n\times n}$. Hence, the $4\times 4$ matrix g in Equation (1) is rewritten as

$$\begin{array}{c}\hfill \begin{array}{c}g=\left[\begin{array}{cccc}{g}_1& g_2& g_3& g_4\end{array}\right]\hfill \end{array}\end{array}$$

(6)

where $g_i$ denote the ith column of g. Since the last column of g does not need to be estimated, the other desired $12\times 1$ vector is defined as

$$\begin{array}{c}\hfill g_{vec}=vec\left(\left[\begin{array}{ccc}{g}_1& g_2& g_3\end{array}\right]\right)\end{array}$$

(7)

Hence, the $g_{vec}$ and g relations can be written as

$$\begin{array}{c}\hfill g^a\left(g_{vec}\right)\triangleq mat\left(\left[\begin{array}{c}{g}_{vec}\\ 0_{3\times 1}\\ 1\end{array}\right]\right)=g\end{array}$$

(8)

Finally, the vectorized pose kinematics, using Equation (7) in Equation (4), are as follows:

$$\begin{array}{c}\hfill {\dot{g}}_{vec}=-diag\left\{V^{\vee },V^{\vee },V^{\vee }\right\}{g}_{vec}\end{array}$$

(9)

2.2. Satellite Dynamics

The rotational and translational dynamics of the deputy satellite of mass m and inertia matrix J are expressed as

$$\begin{array}{c}\hfill \begin{array}{c}J\dot{\omega }=-{\omega }^{\times }J\omega +\tau ,m\dot{v}=-{\omega }^{\times }mv+\varphi \hfill \end{array}\end{array}$$

(10)

where $\tau \in {\Re }^3$ and $\varphi \in {\Re }^3$ represent the gravitational torque and force acting in the body frame of the deputy satellite, respectively.

While there are multiple possible sources of orbit/attitude coupling, only the gravity gradient coupling effects are present in the simulation in this paper, while the effects of not including it are also illustrated. The gravity gradient torque acting on the deputy satellite is approximated to the second order as [29]

$$\begin{array}{c}\hfill \tau ={\displaystyle \frac{3\mu }{\left|\right|r_d{\left|\right|}^5}}({\overline{r}}_d\times J{\overline{r}}_d)\end{array}$$

(11)

The effect of rigid body inertia and attitude on the gravitational acceleration of the deputy’s center of mass location ${\overline{r}}_d$ is also approximated to the second order as [29]

$$\begin{array}{c}\hfill {\overline{a}}_d=-{\displaystyle \frac{\mu }{r_d^3}}\left(\left(1+{\displaystyle \frac{3}{2m{r}_d^2}}\left(tr\left(J\right)-5{\widehat{r}}_d^{T}J{\widehat{r}}_d\right)\right)I_{3x3}+{\displaystyle \frac{3}{m{r}_d^2}}J\right){\overline{r}}_d\end{array}$$

(12)

where ${\widehat{r}}_d={\overline{r}}_d/r_d$ and the standard two-body gravity is obtained if the inertia (which is fixed only in the spacecraft body frame) vanishes. Noting that the chief has position vector ${\overline{r}}_c$ and does not experience gravity gradient torque, the relative position defined by $\Delta \overline{r}={\overline{r}}_d-{\overline{r}}_c$ has the dynamics obtained by subtracting the chief’s (unperturbed) acceleration from the deputy’s acceleration, yielding

$$\begin{array}{ccc}\hfill \Delta \ddot{\overline{r}}& =& -{\displaystyle \frac{\mu }{{\left|{\overline{r}}_c+\Delta \overline{r}\right|}^3}}\left({\overline{r}}_c+\Delta \overline{r}\right)+{\displaystyle \frac{\mu }{{r_c}^3}}{\overline{r}}_c\hfill \\ & & -{\displaystyle \frac{3\mu }{m{\left|{\overline{r}}_c+\Delta \overline{r}\right|}^5}}\left({\displaystyle \frac{1}{2}}\left(tr\left(J\right)-{\displaystyle \frac{5}{{\left|{\overline{r}}_c+\Delta \overline{r}\right|}^2}}{\left({\overline{r}}_c+\Delta \overline{r}\right)}^{T}J\left({\overline{r}}_c+\Delta \overline{r}\right)\right)I_{3x3}+J\right)\left({\overline{r}}_c+\Delta \overline{r}\right)\hfill \end{array}$$

(13)

Using the binomial expansion, this can be linearized as

$$\begin{array}{ccc}\hfill \Delta \ddot{\overline{r}}& \approx & -{\displaystyle \frac{\mu }{{r_c}^3}}\left[\begin{array}{c}\left(1+{\displaystyle \frac{3}{2m{r_c}^2}}\left(tr\left(J\right)-5{\widehat{r}}_c^{T}J{\widehat{r}}_c\right)\right)I_{3x3}\hfill \\ -3\left(1+{\displaystyle \frac{5}{2m{r_c}^2}}\left(tr\left(J\right)-7{\widehat{r}}_c^{T}J{\widehat{r}}_c\right)\right){\widehat{r}}_c{\widehat{r}}_c^T\hfill \\ +{\displaystyle \frac{3}{m{r_c}^2}}\left(J-5\left(J{\widehat{r}}_c{\widehat{r}}_c^T+{\widehat{r}}_c{\widehat{r}}_c^{T}J\right)\right)\hfill \end{array}\right]\Delta \overline{r}\hfill \\ & & -{\displaystyle \frac{3\mu }{m{r_c}^5}}\left({\displaystyle \frac{1}{2}}\left(tr\left(J\right)-5{\widehat{r}}_c^{T}J{\widehat{r}}_c\right)I_{3x3}+J\right){\overline{r}}_c\hfill \end{array}$$

(14)

Using the transport theorem $\Delta \ddot{\overline{r}}={}^{\mathbb{H}}{\displaystyle \frac{d^2}{d{t}^2}}\overline{r}+2{\overline{\omega }}_{\mathbb{H}}\times \left({}^{\mathbb{H}}{\displaystyle \frac{d}{dt}}\overline{r}\right)+{\overline{\omega }}_{\mathbb{H}}\times \left({\overline{\omega }}_{\mathbb{H}}\times \overline{r}\right)$ for the rotating Hill frame of the chief and resolving all terms into this frame, where $n_c^2=\mu /{r_c}^3$, yields the perturbed relative force as

$$\begin{array}{ccc}\hfill {\varphi }_{perturbed}^{\mathbb{H}}& =& \left(\begin{array}{c}2n_c\dot{y}\\ -2n_c\dot{x}\\ 0\end{array}\right)+n_c^2\left(\begin{array}{c}x\\ y\\ 0\end{array}\right)+\left(\begin{array}{c}3n_c^2\left(1+{\displaystyle \frac{5}{2m{r_c}^2}}\left(tr\left(J\right)-7{\widehat{r}}_c^{T}J{\widehat{r}}_c\right)\right)x\\ 0\\ 0\end{array}\right)\hfill \\ & & -n_c^2\left(\begin{array}{c}\left(1+{\displaystyle \frac{3}{2m{r_c}^2}}\left(tr\left(J\right)-5{\widehat{r}}_c^{T}J{\widehat{r}}_c\right)\right)I_{3x3}\hfill \\ +{\displaystyle \frac{3}{m{r_c}^2}}\left(J-5\left(J{\widehat{r}}_c{\widehat{r}}_c^T+{\widehat{r}}_c{\widehat{r}}_c^{T}J\right)\right)\hfill \end{array}\right)\left(\begin{array}{c}x\\ y\\ z\end{array}\right)\hfill \\ & & -{\displaystyle \frac{3\mu }{m{r_c}^5}}\left(\begin{array}{c}{\displaystyle \frac{1}{2}}\left(tr\left(J\right)-5{\widehat{r}}_c^{T}J{\widehat{r}}_c\right)I_{3x3}+J\hfill \end{array}\right)\left(\begin{array}{c}{r}_c\\ 0\\ 0\end{array}\right)\hfill \end{array}$$

(15)

It can be seen that if all inertias are set to zero, the standard HCW equations

$$\begin{array}{c}\hfill {\varphi }_{unperturbed}^{\mathbb{H}}=\left(\begin{array}{c}2n_c\dot{y}+3n_c^{2}x\\ -2n_c\dot{x}\\ -n_c^{2}z\end{array}\right)\end{array}$$

(16)

are obtained. The terms ${\widehat{r}}_c^{T}J{\widehat{r}}_c,J{\widehat{r}}_c{\widehat{r}}_c^T,{\widehat{r}}_c{\widehat{r}}_c^{T}J$, and J are attitude-dependent terms, where the rotation matrix C is used explicitly as ${\widehat{r}}_c^T{C}^{T}JC{\widehat{r}}_c,C^{T}JC{\widehat{r}}_c{\widehat{r}}_c^T,{\widehat{r}}_c{\widehat{r}}_c^T{C}^{T}JC$, and $C^{T}JC$. The dominant effect of the perturbed version is to change the deputy’s orbital rate from that of the unperturbed case.

Define the $6\times 6$ mass tensor as

$$\begin{array}{c}\hfill \mathbb{I}\triangleq \left[\begin{array}{cc}J& 0_{3\times 3}\\ 0_{3\times 3}& m{I}_{3\times 3}\end{array}\right]\end{array}$$

(17)

Thus, the deputy satellite dynamics in Equation (10) are rewritten as

$$\begin{array}{c}\hfill \mathbb{I}\dot{V}=a{d}_V^{*}\mathbb{I}V+u\end{array}$$

(18)

where $u\triangleq {\left[\begin{array}{cc}{\tau }^T& {\varphi }^T\end{array}\right]}^T$ and the co-adjoint operator is defined as

$$\begin{array}{c}\hfill a{d}_V^{*}=-\left[\begin{array}{cc}{\omega }^{\times }& v^{\times }\\ 0_{3\times 3}& {\omega }^{\times }\end{array}\right]\end{array}$$

(19)

Hence, the above-mentioned 12-dimensional tangent bundle $TSE\left(3\right)=SE\left(3\right)\times {\Re }^6$ is the state space associated with the six degrees of freedom model. Note that ${\varphi }^{\mathbb{H}}$ is resolved in the Hill frame, which has to be transformed into the deputy’s body frame $\mathbb{B}$ in order to be used in Equation (18) using $\varphi =C{\varphi }^{\mathbb{H}}$. While certain relevant perturbations may be included based on the orbital regime and mission requirements, they were not included in this paper since only gravity gradient torque was assumed in the relative orbital model.

2.3. Communication Graph

Let us assume that there are N spacecraft in a network defined by undirected communication links. To model this communication topology, a connected undirected graph is used, i.e., $\mathcal{G}\triangleq (\mathcal{V},\mathcal{E})$, where $\mathcal{V}=\{1,\dots ,N\}$ is the node set and $\mathcal{E}\subseteq \mathcal{V}\times \mathcal{V}$ is the set of edges. Here, the edge of $\mathcal{G}$ is denoted by $e_{ij}=({\mathcal{v}}_i,{\mathcal{v}}_j)\in \mathcal{E}$, which implies an undirected communication link between nodes i and j. Let $\mathcal{A}=\left[{\mathcal{a}}_{ij}\right]\in {\Re }^{N\times N}$ be the adjacency matrix associated with $\mathcal{G}$, in which the element ${\mathcal{a}}_{ij}=1$ if agents i and j are connected; otherwise, ${\mathcal{a}}_{ij}=0$. The degree matrix of the graph $\mathcal{G}$ is a diagonal matrix $\mathcal{D}$, where

$$\begin{array}{c}\hfill {\mathcal{d}}_{ii}=\sum _{k=1,k\ne i}^N{\mathcal{a}}_{ik}\end{array}$$

(20)

Accordingly, the graph Laplacian matrix is written as $\mathcal{L}=\mathcal{D}-\mathcal{A}$. The representation of line and complete graphs for a four-node undirected network is shown in Figure 2.

3. Problem Statement for Proximity Operations

Without a loss of generality, assume the following consensus estimation scenario of multiple chief spacecraft cooperatively estimating the state of the deputy satellite:

(i)

Four chief satellites and one deputy satellite is in formation, as shown in Figure 3.

(ii)

Chief satellite orbital elements and its attitude are known perfectly at all times.

(iii)

The objective is to estimate the deputy satellite—$SE\left(3\right)$ information—using the filters that are running in each chief satellite.

(iv)

The deputy satellite has attitude sensors for estimating C in Equation (1). This information along with its velocities is shared with each chief satellite for running the filter.

(v)

Chief satellites have a LIDAR sensor, which measures its range from the deputy satellite. This is used to estimate the p in the g-matrix.

(vi)

For consensus estimation, each chief satellite $SE\left(3\right)$ state is shared in the communication network.

Figure 3. Consensus estimation scenario.

Figure 3. Consensus estimation scenario.

The dynamic model can be expressed as

$$\begin{array}{ccc}\hfill {}^i\dot{x}& =& {}^{i}f\left(x\right)+{}^{i}G{}^i\mathfrak{w}\hfill \end{array}$$

(21)

$$\begin{array}{ccc}\hfill {}^{i}y& =& {}^{i}h\left(x\right)+{}^{i}G{}^i\mathfrak{v}\hfill \end{array}$$

(22)

where ${}^i\dot{x}$ denotes the state of the deputy satellite relative to the $i^{\mathbb{H}}$ frame (Hill frame corresponding to the chief satellite i); ${}^{i}f\left(x\right)$ corresponding to the terms explained in Section 2.1; ${}^i\mathfrak{w}$ denotes the corresponding process noise with $E\left[{}^i\mathfrak{w}\left(t\right){}^i\mathfrak{w}^T\left(t^{\prime }\right)\right]={}^{i}Q\delta (t-t^{\prime })$; $\delta (t-t^{\prime })$ is the Dirac delta function; ${}^{i}Q$ is the process noise covariance matrix; and ${}^{i}G$ denotes the input matrix associated with ${}^i\mathfrak{w}$. In addition, ${}^{i}y$ denotes the measurement for chief satellite as explained in Section 3, and ${}^i\mathfrak{v}$ denotes the corresponding measurement noise with $E\left[{}^i\mathfrak{v}\left(t\right){}^i\mathfrak{v}^T\left(t^{\prime }\right)\right]={}^{i}R\delta (t-t^{\prime })$, in which ${}^{i}R$ is the measurement noise covariance matrix.

One must note that there is only one deputy; however, its state is expressed in the different Hill frames of each chief. Hence, these states are transformed to the corresponding Hill frame by using the following equation:

$$\begin{array}{ccc}\hfill {}_j{}^{i}C& =& {}^{i}C_{\mathbb{I}}^T{}^{j}C_{\mathbb{I}}{}^{j}C\hfill \end{array}$$

(23)

$$\begin{array}{ccc}{}_j{}^{i}p& =& {}^{i}C_{\mathbb{I}}^T{}^{j}C_{\mathbb{I}}{}^{j}p+{}^{i}C_{\mathbb{I}}^T({}^{j}r-{}^{i}r)\hfill \end{array}$$

(24)

where ${}^{i}C_{\mathbb{I}}$ is the known rotation matrix from the inertial frame $\mathbb{I}$ to the Hill frame $\mathbb{H}$ of the ith chief spacecraft, ${}^{j}C_{\mathbb{I}}$ is the known rotation matrix from the inertial frame $\mathbb{I}$ to the Hill frame $\mathbb{H}$ of the jth chief spacecraft, and the vectors ${}^{j}r$, ${}^{i}r$, ${}^{i}p$, and ${}^{j}p$ are defined in Figure 4.

Measurement Model

Assume that the deputy satellite is equipped with attitude sensors, which need a minimum number of vector measurements, i.e., ≥2, for the attitude to be determined uniquely at any instant [30].

$$\begin{array}{c}\hfill {}^{i}S_{\mathbb{B},n_i}^m={}^{i}C{}^{i}C_{\mathbb{I}}{S}_{\mathbb{I}}+\eta \end{array}$$

(25)

where ${}^{i}S_{\mathbb{B},n_i}^m$ is the vector measured by the $n_i$ attitude sensor in the body frame corresponding to the vector $S_{\mathbb{I}}$ in the inertial frame, with $\eta$ being additive measurement noise.

Additionally, consider that the chief satellite is mounted with a LIDAR sensor, which measures the range to the deputy satellite in the Hill frame. Range measurements can be represented by the magnitude of the relative position vector, i.e.,

$$\begin{array}{c}\hfill {}^{i}p^m=\left|\right|{}^{i}p\left|\right|+\varsigma \end{array}$$

(26)

where $\varsigma$ is measurement noise [31]. Note that the velocity is assumed to be known for the sake of brevity. However, the proposed estimation architecture can be easily augmented with the velocity measurement, as demonstrated in the [23].

4. Consensus Continuous-Discrete $\mathit{SE}\mathbf{\left(}\mathbf{3}\mathbf{\right)}$-Constrained Extended Kalman Filter

In this section, a practical consensus $SE\left(3\right)$-constrained extended Kalman filter in continuous-discrete time fashion (i.e., continuous dynamics and discrete measurements) is proposed. The consensus extended Kalman filter techniques can be categorized into two general types. The first type which includes the feedback of both estimated state and covariance matrix is known as the optimal consensus EKF, while the second one includes only the estimated state and is therefore suboptimal [19]. However, it must be noted that the exchange of covariance in the optimal filter type leads to a high computational burden in the communication network [32]. Hence, though the performance of the optimal filter is slightly better than in a suboptimal case, the latter one is recommended and adopted in this paper for satellite formation flying.

4.1. Continuous-Discrete Time SE(3)-EKF Structure

The vector measurements at time instant k is

$$\begin{array}{c}\hfill {}^{i}y_k={\left[\begin{array}{cc}{}^{i}S_{\mathbb{B},n_i,k}^{m,T}& {}^{i}p_k^m\end{array}\right]}^T\end{array}$$

(27)

where i represents the chief satellite under consideration, and the computed a priori estimates is

$$\begin{array}{c}\hfill {}^i\widehat{y}_k^{-}={}^{i}H_k{}^i\widehat{g}_{vec,k}^{-}\end{array}$$

(28)

where

$$\begin{array}{c}\hfill {}^{i}H_k=\left[\begin{array}{cccccc}{S}_{\mathbb{I},k}^1\left(1\right)I_{3\times 3}& 0_{3\times 1}& S_{\mathbb{I},k}^1\left(2\right)I_{3\times 3}& 0_{3\times 1}& S_{\mathbb{I},k}^1\left(3\right)I_{3\times 3}& 0_{3\times 1}\\ S_{\mathbb{I},k}^2\left(1\right)I_{3\times 3}& 0_{3\times 1}& S_{\mathbb{I},k}^2\left(2\right)I_{3\times 3}& 0_{3\times 1}& S_{\mathbb{I},k}^2\left(3\right)I_{3\times 3}& 0_{3\times 1}\\ 0_{3\times 3}& I_{3\times 3}(:,1)& 0_{3\times 3}& I_{3\times 3}(:,2)& 0_{3\times 3}& I_{3\times 3}(:,3)\end{array}\right]\end{array}$$

(29)

represents the output Jacobian matrix derived based on the measurement model explained in Section 3. Note that $S_{\mathbb{I},k}\left(1\right),S_{\mathbb{I},k}\left(2\right),S_{\mathbb{I},k}\left(3\right)$ are the components of the vector $S_{\mathbb{I},k}\in {\Re }^3$ in the inertial frame at time instant $t_k$.

The time update for the estimates is

$$\begin{array}{ccc}\hfill {}^i\dot{\widehat{g}}_{vec}& =& -diag\left\{{}^{i}V^{\vee },{}^{i}V^{\vee },{}^{i}V^{\vee }\right\}{}^i\widehat{g}_{vec}\hfill \end{array}$$

(30)

and the consensus measurement update equation [19] for the a posteriori estimates in terms of the residual error ${}^i\tilde{y}_k=\left[{}^{i}y_k-{}^i\widehat{y}_k^{-}\right]$ is

$$\begin{array}{c}\hfill {}^i\widehat{g}_{vec,k}^{+}={}^i\widehat{g}_{vec,k}^{-}+{}^{i}K_k{}^i\tilde{y}_k-{\gamma }_k{\displaystyle \frac{{}^{i}P_k^{-}}{1+\left|\right|{}^{i}P_k^{-}\left|\right|}}\sum _{i\sim j}\left({}^i\widehat{g}_{vec,k}^{-}-{}_j{}^i\widehat{g}_{vec,k}^{-}\right)\end{array}$$

(31)

where ${}^{i}K_k\in {\Re }^{12\times (3n_i+3)}$ is the gain matrix to be determined, ${\gamma }_k\ge 0$ denotes the scalar consensus feedback gain, and $j\sim i$ are the neighbors of i in the network.

4.2. Continuous-Discrete Time SE(3)-EKF Error Dynamics

The estimation state vector is

$$\begin{array}{c}\hfill \begin{array}{c}{}^{i}x={}^{i}g_{vec}\hfill \end{array}\end{array}$$

(32)

and the corresponding estimated state vector is $\widehat{x}$, with the estimation error being defined as ${}^i\tilde{x}={}^{i}x-{}^i\widehat{x}$. The linearized estimation error dynamics are

$$\begin{array}{c}\hfill {}^i\dot{\tilde{x}}={}^{i}A{}^i\tilde{x}+{}^{i}G{}^i\mathfrak{w}\end{array}$$

(33)

The matrices ${}^{i}A\in {\Re }^{12\times 12}$ and ${}^{i}G\in {\Re }^{12\times 3n_i}$ can be derived. The error covariance is defined as

$$\begin{array}{c}\hfill {}^{i}P=E\left\{{}^i\tilde{x}{}^i\tilde{x}^T\right\}\end{array}$$

(34)

and the covariance propagation is carried out as follows:

$$\begin{array}{c}\hfill {}^i\dot{P}={}^{i}A{}^{i}P+{}^{i}P{}^{i}A^T+{}^{i}G{}^{i}Q{}^{i}G^T\end{array}$$

(35)

For more details on the filter, one can refer to [33,34].

4.3. Discrete Time SE(3)-EKF Gain Selection

The gain for an $SE\left(3\right)$-constrained filter is found by

$$\begin{array}{c}\hfill \underset{{}^{i}K_k}{min}J={\displaystyle \frac{1}{2}}\sum _{i=1}^{n}trace\left[{}^{i}P_k^{+}\right]\end{array}$$

(36)

and is subjected to the orthogonality constraint and determinant constraint. In other words, the a posteriori estimate of ${}^{i}g_{vec}$ should satisfy the following constraint:

$$\begin{array}{c}\hfill {}^i\widehat{g}_{vec,k}^{+}\in SE\left(3\right)\end{array}$$

(37)

The a posteriori covariance matrix can be derived [35] as

$$\begin{array}{ccc}\hfill {}^{i}P_k^{+}& \triangleq & E\left[{}^i\tilde{x}_k^{+}{}^i\tilde{x}_k^{+,T}\right]\hfill \\ & =& \left(I-{}^{i}K_k{}^{i}H_k\right){}^{i}P_k^{-}{\left(I-{}^{i}K_k{}^{i}H_k\right)}^T+{}^{i}K_k{}^{i}R_k{}^{i}K_k^T\hfill \\ & =& \left(I-{}^{i}K_k{}^{i}H_k\right){}^{i}P_k^{-}\hfill \end{array}$$

(38)

where ${}^{i}P_k^{-}\triangleq E\left[{}^i\tilde{x}_k^{-}{}^i\tilde{x}_k^{-,T}\right]=E\left[\left({}^{i}x_k-{}^i\widehat{x}_k^{-}\right){\left({}^{i}x_k-{}^i\widehat{x}_k^{-}\right)}^T\right]$ is the a priori covariance matrix. At measurement time $t_k$, the a priori state estimate and the estimated state error covariance matrix for the ith $SE\left(3\right)$-constrained EKF are denoted by ${}^i\widehat{x}_k^{-}={}^i\widehat{x}\left(t_k\right)$ and ${}^{i}P_k^{-}={}^{i}P\left(t_k\right)$. The transformation of variables is defined as

$$\begin{array}{c}\hfill {}^{i}K_k={}^{i}K_k^{unc}+{}^i\Delta Kk\end{array}$$

(39)

where ${}^{i}K_k^{unc}$ is the unconstrained optimal gain, which can be derived as

$$\begin{array}{ccc}\hfill {}^{i}K_k^{unc}& =& {}^{i}P_k^{-}{}^{i}H_k^T{\left[{}^{i}H_k{}^{i}P_k^{-}{}^{i}H_k^T+{}^{i}R_k\right]}^{-1}\hfill \end{array}$$

(40)

where ${}^{i}R_k$ is the measurement noise covariance matrix. Note that ${}^i\Delta K_k$ becomes the new design variable for a constrained $SE\left(3\right)$ formulation. Substituting Equation (39) in Equation (38) gives the a posteriori covariance matrix as

$$\begin{array}{c}\hfill {}^{i}P_k^{+}={}^{i}P_k^{unc,+}+{}^i\Delta K_k\left({}^{i}H_k{}^{i}P_k^{-}{}^{i}H_k^T+{}^{i}R_k\right){}^i\Delta K_k^T\end{array}$$

(41)

where

$$\begin{array}{c}\hfill {}^{i}P_k^{unc,+}=\left(I-{}^{i}K_k^{unc}{}^{i}H_k\right){}^{i}P_k^{-}\end{array}$$

(42)

is a posteriori covariance. One can see from Equations (38) and (41) that the terms $-{}^{i}K_k{}^{i}H_k{}^{i}P_k^{-}$ and ${}^i\Delta K_k\left({}^{i}H_k{}^{i}P_k^{-}{}^{i}H_k^T+{}^{i}R_k\right){}^i\Delta K_k^T$ are equivalent. Moreover, substituting Equation (39) in Equation (31) gives the a posteriori estimate as

$$\begin{array}{ccc}\hfill {}^i\widehat{g}_{vec,k}^{+}& =& {}^i\widehat{g}_{vec,k}^{unc,+}+{}^i\Delta K_k{}^i\tilde{y}_k\hfill \end{array}$$

(43)

where

$$\begin{array}{c}\hfill {}^i\widehat{g}_{vec,k}^{unc,+}={}^i\widehat{g}_{vec,k}^{-}+{}^{i}K_k^{unc}{}^i\tilde{y}_k-{\gamma }_k{\displaystyle \frac{{}^{i}P_k^{-}}{1+\left|\right|{}^{i}P_k^{-}\left|\right|}}\sum _{i\ne j}\left({}^i\widehat{g}_{vec,k}^{-}-{}_j{}^i\widehat{g}_{vec,k}^{-}\right)\end{array}$$

(44)

Using the covariance Equation (41), the optimal $\Delta$-constrained gain is found with the following minimization problem:

$$\begin{array}{cc}\underset{{}^i\Delta K_k}{min}\Delta J_g={\displaystyle \frac{1}{2}}\sum _{i=1}^{n}trace\left[{}^i\Delta K_k\left({}^{i}H_k{}^{i}P_k^{-}{}^{i}H_k^T+{}^{i}R_k\right){}^i\Delta K_k^T\right]\hfill & \\ \hfill \dots \mathrm{subject}\mathrm{to}{g}^a\left({}^i\widehat{g}_{vec,k}^{+}\right)\in SE\left(3\right)\end{array}$$

(45)

Note that the cost function Equation (45) is convex; however, the feasible set ${}^i\Delta K_k$ is non-convex due to the fact that $SE\left(3\right)$ is nonconvex. Therefore, it is difficult to find a globally minimizing solution. To solve this problem, the filter gain was restricted to a special form, for which there is an analytical global solution. Moreover, the restriction of the form of gain provided no loss of generality for the Kalman filter state-estimate correction step, in the sense that all points in $SE\left(3\right)$ were accessible in the correction step with this form of gain. As said, an alternate approach [36] is taken to find the optimal $\Delta$-constrained gain ${}^i\Delta K_k$ by restricting its form to

$$\begin{array}{c}\hfill {}^i\Delta K_k={\displaystyle \frac{{}^i\Delta \overline{K}_k{}^i\tilde{y}_k^T{\left({}^{i}H_k{}^{i}P_k^{-}{}^{i}H_k^T+{}^{i}R_k\right)}^{-1}}{{}^i\tilde{y}_k^T{\left({}^{i}H_k{}^{i}P_k^{-}{}^{i}H_k^T+{}^{i}R_k\right)}^{-1}{}^i\tilde{y}_k}}\end{array}$$

(46)

where ${}^i\Delta \overline{K}_k$ becomes the design variable, and the a posteriori estimate in Equation (43) becomes

$$\begin{array}{c}\hfill {}^i\widehat{g}_{vec,k}^{+}={}^i\widehat{g}_{vec,k}^{unc,+}+{}^i\Delta \overline{K}_k\end{array}$$

(47)

or, equivalently,

$$\begin{array}{c}\hfill g^a\left({}^i\widehat{g}_{vec,k}^{+}\right)=g^a\left({}^i\widehat{g}_{vec,k}^{unc,+}\right)+mat\left({}^i\Delta \overline{K}_k\right)\end{array}$$

(48)

It has to be noted that the term ${}^i\widehat{g}_{vec,k}^{unc,+}$ or $g^a\left({}^i\widehat{g}_{vec,k}^{unc,+}\right)$ has the consensus term for sensor fusion as in Equation (44). Moreover, by using Equation (46) in Equation (45), the cost function is rewritten as

$$\begin{array}{ccc}\hfill \Delta J_g& =& {}^i\Lambda \sum _{i=1}^{n}trace\left[{}^i\Delta \overline{K}_k{}^i\Delta \overline{K}_k^T\right]={}^i\Lambda \sum _{i=1}^n{}^i\Delta \overline{K}_k^T{}^i\Delta \overline{K}_k\hfill \\ & =& {}^i\Lambda \sum _{i=1}^{n}trace\left[mat{\left({}^i\Delta \overline{K}_k\right)}^{T}mat\left({}^i\Delta \overline{K}_k\right)\right]\hfill \end{array}$$

(49)

where ${}^i\Lambda \triangleq {\displaystyle \frac{1}{2{}^i\tilde{y}_k^T{\left({}^{i}H_k{}^{i}P_k^{-}{}^{i}H_k^T+{}^{i}R_k\right)}^{-1}{}^i\tilde{y}_k}}$ Using Equation (48), the cost function in Equation (49) can be rewritten as

$$\begin{array}{ccc}\hfill \Delta J_g& =& {}^i\Lambda \sum _{i=1}^{n}trace\left[{}^i\tilde{g}^{a,T}{}^i\tilde{g}^a\right]\hfill \end{array}$$

(50)

where ${}^i\tilde{g}^a\triangleq \left[g^a\left({}^i\widehat{g}_{vec,k}^{+}\right)-g^a\left({}^i\widehat{g}_{vec,k}^{unc,+}\right)\right]$. Hence, the minimization problem in Equation (45) is equivalent to

$$\begin{array}{c}\hfill \underset{g^a\left({}^i\widehat{g}_{vec,k}^{+}\right)\in SE\left(3\right)}{min}{\Delta J}_g^{unc}={\displaystyle \frac{1}{2}}\sum _{i=1}^{n}trace\left[{}^i\tilde{g}^{a,T}{}^i\tilde{g}^a\right]\end{array}$$

(51)

This is recognized as a Procrustes-type problem, and the optimal constrained estimate $g^a\left({}^i\widehat{g}_{vec,k}^{+}\right)$ is the least-square projection of the unconstrained update comprising the consensus term, i.e., $g^a\left({}^i\widehat{g}_{vec,k}^{unc,+}\right)$ onto $SE\left(3\right)$. However, it is known that

$$\begin{array}{c}\hfill g^a\left({}^i\widehat{g}_{vec,k}^{unc,+}\right)=\left[\begin{array}{cc}{}^i\widehat{C}_k^{unc,+}& 0_{3\times 1}\\ {}^i\widehat{p}_k^{+,T}& 1\end{array}\right]\end{array}$$

(52)

The solution for Equation (51) can be obtained as

$$\begin{array}{c}\hfill g^a\left({}^i\widehat{g}_{vec,k}^{+}\right)=\left[\begin{array}{cc}\mathfrak{V}diag(I_{2\times 2},det\mathfrak{V}det\mathfrak{U}){\mathfrak{U}}^T& 0_{3\times 1}\\ {}^i\widehat{p}_k^T& 1\end{array}\right]\end{array}$$

(53)

where the singular value decomposition of ${}^i\widehat{C}_k^{unc,+}$ is calculated as $\mathfrak{V}\Sigma {\mathfrak{U}}^T$, in which $\mathfrak{V},\mathfrak{U}\in {\Re }^{n\times n}$ are the orthogonal matrices and $\Sigma$ is diagonal matrix containing the singular values of ${\widehat{C}}_k^{unc,+}$ in descending order. The optimal $\Delta$-constrained gain can be calculated using Equation (47) in Equation (46) as

$$\begin{array}{c}\hfill {}^i\Delta K_k={\displaystyle \frac{{}^i\tilde{g}{}^i\tilde{y}_k^T{\left({}^{i}H_k{}^{i}P_k^{-}{}^{i}H_k^T+{}^{i}R_k\right)}^{-1}}{{}^i\tilde{y}_k^T{\left({}^{i}H_k{}^{i}P_k^{-}{}^{i}H_k^T+{}^{i}R_k\right)}^{-1}{}^i\tilde{y}_k}}\end{array}$$

(54)

where ${}^i\tilde{g}\triangleq \left({}^i\widehat{g}_{vec,k}^{+}-{}^i\widehat{g}_{vec,k}^{unc,+}\right)$. Finally, using Equation (54), the a posteriori covariance matrix Equation (41) becomes

$$\begin{array}{c}\hfill {}^{i}P_k^{+}={}^{i}P_k^{unc,+}+diag\left\{{\displaystyle \frac{{}^i\tilde{g}{}^i\tilde{g}^T}{{}^i\tilde{y}_k^T{\left({}^{i}H_k{}^{i}P_k^{-}{}^{i}H_k^T+{}^{i}R_k\right)}^{-1}{}^i\tilde{y}_k}}\right\}\end{array}$$

(55)

As was mentioned at the start of this section, the covariance propagation algorithm of the optimal consensus Kalman filter has a high computational burden and is not scalable in n with an all-to-all communication topology [32]. Hence, a suboptimal filter was utilized in terms of the approximate covariance update of Equation (38) by neglecting the rightmost term in Equation (31). This approximation of the suboptimal filter assumes a small consensus gain ${\gamma }_k$, as explained in [19]. For more details on the appropriate choice of the consensus gain based on Lyapunov-based stability analysis, as well as the full covariance update without this approximation, one can refer to [19].

5. Discretized Variational Integration

Lie group variational integrators preserve the structure of the configuration space. Variational integration obtained from the discrete Langrange–d’Alembert principle [37] is used to obtain the states at time $k+1$ in terms of time k as

$$\begin{array}{ccc}\hfill \Im & =& {\displaystyle \frac{1}{2}}trace\left[J\right]I_{3\times 3}-J\hfill \\ \hfill {\left(J{\omega }_k\right)}^{\times }& =& {\displaystyle \frac{1}{\Delta t}}\left(F_k\Im -\Im F_k^T\right)\hfill \\ \hfill C_{k+1}& =& {\left[C_k^T{F}_k\right]}^T\hfill \\ \hfill \mathbb{I}{V}_{k+1}& =& diag(F_k^T,F_k^T)\mathbb{I}{V}_k+\Delta tdiag(F_k^T,F_k^T)u_k\hfill \end{array}$$

(56)

where ℑ is the modified inertia matrix in terms of the inertia matrix J, and $\Delta t$ is the time step used for the simulation. Note that Equation (56) is an implicit equation, and it is solved for $F_k$ using the Newton–Raphson method. Moreover, the propagation of position and the covariance matrix is carried out using Runge–Kutta 78 method. These equations are used to compute the evolution of the pose and the velocities of the rigid body in Equation (18).

6. Numerical Simulation

In this part of this paper, the proposed filter is validated via a numerical simulation by considering the simulation setup explained in Section 3. One of the common ways to represent the state of the satellite is by using six orbital elements, namely semimajor axis a, eccentricity e, inclination i, the argument of ascending node $\Omega$, the argument of periapsis $\omega$, and true anomaly $\nu$. Moreover, in the case of satellite formation flying, there exist various ways to denote the relative orbits between the chief and deputy satellites. One option is to use orbit element differences, i.e., the difference of the orbital elements between the spacecraft. Close proximity operations are generally carried out in low Earth orbits of altitudes in the range of 500–1000 km [38]. Hence, without a loss of generality, it is assumed that there exists a reference chief spacecraft with the following orbital elements for the simulation: a = 7100 km, e = 0.0, i = 1.0$°$, $\Omega$ = 0.0$°$, $\omega$ = 90$°$, $\nu$ = 0.0$°$. Furthermore, the orbital element differences of the other three chief satellites and a deputy satellite assumed from the reference chief spacecraft are tabulated in

Table 1. Orbital element differences for the simulation scenario.

Satellite	$\mathit{\delta }\mathit{a}$, km	$\mathit{\delta }\mathit{e}$	$\mathit{\delta }{\mathit{i}}^{\circ }$	$\mathit{\delta }{\mathbf{\Omega }}^{\circ }$	$\mathit{\delta }{\mathit{\omega }}^{\circ }$	$\mathit{\delta }{\mathit{M}}_0^{\circ }$
Second, chief	0	0	$0.16$	0	0	$0.8$
Third chief	0	0	$0.2$	0	0	$0.7$
Four chief	0	0	$-0.2$	0	0	$-0.7$
Deputy	0	$0.004$	$-0.2$	0	0	$-0.7$

. Note that the initial mean anomaly difference $\delta M_0$ is adopted in Table 1 instead of the true anomaly difference $\delta {\nu }_0$ for the sake of convenience. It is assumed that there exists a communication network using the line graph topology, as shown in Figure 2, and the configuration of the deputy and the four chief satellites in the inertial frame is shown in Figure 3.

The mass of the deputy satellite is $m=100$ kg and its inertia tensor is given by

$$\begin{array}{c}\hfill J=diag(51.2,60.2,59.6)\mathrm{kg}·{\mathrm{m}}^2\end{array}$$

(57)

The initial attitudes of the deputy satellite in each Hill frame are assumed as

$$\begin{array}{ccc}\hfill {}^{1}C_0& =& exp{m}_{so\left(3\right)}\left({\left({\displaystyle \frac{\pi }{8}}\times {\left[\begin{array}{ccc}{\displaystyle \frac{3}{7}}& {\displaystyle \frac{-6}{7}}& {\displaystyle \frac{2}{7}}\end{array}\right]}^T\right)}^{\times }\right)\hfill \\ \hfill {}^{2}C_0& =& exp{m}_{so\left(3\right)}\left({\left({\displaystyle \frac{\pi }{10}}\times {\left[\begin{array}{ccc}{\displaystyle \frac{3}{7}}& {\displaystyle \frac{-6}{7}}& {\displaystyle \frac{2}{7}}\end{array}\right]}^T\right)}^{\times }\right)\hfill \\ \hfill {}^{3}C_0& =& exp{m}_{so\left(3\right)}\left({\left({\displaystyle \frac{\pi }{9}}\times {\left[\begin{array}{ccc}{\displaystyle \frac{3}{7}}& {\displaystyle \frac{-6}{7}}& {\displaystyle \frac{2}{7}}\end{array}\right]}^T\right)}^{\times }\right)\hfill \\ \hfill {}^{4}C_0& =& exp{m}_{so\left(3\right)}\left({\left({\displaystyle \frac{\pi }{7}}\times {\left[\begin{array}{ccc}{\displaystyle \frac{3}{7}}& {\displaystyle \frac{-6}{7}}& {\displaystyle \frac{2}{7}}\end{array}\right]}^T\right)}^{\times }\right)\hfill \end{array}$$

(58)

Similarly, the initial angular velocity of the deputy satellite is

$$\begin{array}{ccc}\hfill {\omega }_0& =& {\left[\begin{array}{ccc}0.0& 0.0& 0.0\end{array}\right]}^T\mathrm{rad}/\mathrm{s}\hfill \end{array}$$

(59)

Using these conditions, the nominal scenario was simulated as shown in Figure 5. These figures show the relative trajectory of the same deputy satellite in the Hill frame of each chief satellite considered. Figure 5a represents the trajectory propagation for nearly two orbital periods using the model in Equation (16) without gravity gradient torque. To visualize the effect of gravity gradient torque, trajectory propagation was carried out with Equation (15). As shown in Figure 5b, the deputy satellite relative trajectory was deviated within half of the orbital time period. Next, the trajectory was considered with gravity gradient torque in order to simulate conditions close to reality.

For the simulation, the attitude sensor measurement noise covariances are assumed as $E\left\{i_1{i}_1^T\right\}=r_1{I}_{3\times 3}$, $E\left\{i_2{i}_2^T\right\}=r_2{I}_{3\times 3}$, $E\left\{i_1{i}_2^T\right\}=0_{3\times 3}$, respectively, where $r_1=1\mathrm{m}$ and $r_2=1\mathrm{m}$. The noise covariance in the LIDAR sensor measurement is assumed as $E\left\{p_1{p}_1^T\right\}=r_3{I}_{3\times 3}$, where $r_3=100{\mathrm{m}}^2$. Finally, the process noise in position is assumed as $E\left\{w_p{w}_p^T\right\}=q_p{I}_{3\times 3}$, where $q_p=1{\mathrm{m}}^2$ [39]. The initial estimation error covariance matrix is $P_0=diag(5\times {10}^{-4}{I}_{3\times 3},1\times {10}^{-1},5\times {10}^{-4}{I}_{3\times 3},1\times {10}^{-1},5\times {10}^{-4}{I}_{3\times 3},1\times {10}^{-1})$. The initial estimates of the states are assumed with $10\%$ error with respect to their corresponding nominal values. Note that the filter propagation is carried out using the linear HCW model in Equation (16). For the simulation, the measurement rates for all the sensors are assumed to be available at every $0.1$ s. The setup of each EKF for the following simulations is the same as the above-mentioned scenario, unless otherwise noted. Moreover, for plotting, the errors between the true and estimate states are represented as

$$\begin{array}{ccc}\hfill {}^i\varphi _{err}& =& {cos}^{-1}\left({\displaystyle \frac{trace\left({}^{i}C{}^i\widehat{C}^T\right)-1}{2}}\right)\hfill \end{array}$$

(60)

$$\begin{array}{ccc}\hfill {}^{i}p_{err}& =& {\left|\right|{}^{i}p-{}^i\widehat{p}||}_2\hfill \end{array}$$

(61)

Without a loss of generality, the numerical simulation is carried out in the MATLAB R2024a platform. For comparison, the same problem is solved using the $SO\left(3\right)$ filter from ref. [24] for attitude estimation and a Kalman filter for position estimation, which are the conventional techniques adopted for proximity operations. Hence, the results are categorized as two sets—nominal and consensus. The nominal solution corresponds to the existing $SO\left(3\right)$ and position filter in the literature, while the latter corresponds to the novel consensus $SE\left(3\right)$-constrained filter proposed in Section 4.

First, the overall summary of the results is discussed, which were obtained using the filters running in the continuous-discrete time domain. These results, which are shown in Figure 6, are based on the nominal as well as the line graph network topologies. Using Equation (60), the principal rotation angle (PRA) of the attitude error for the nominal as well as consensus design were plotted, as shown in Figure 6a. From Figure 6a, one can observe that the continuous-discrete time filter for the nominal design has satisfactory filter performance. However, the filter in each chief satellite converges to its true value as per its own characteristics. Next, the significance of the proposed sensor fusion design via the $SE\left(3\right)$ filter’s performance is shown in Figure 6a. As seen, the PRA error in each filter changes to a consensus value before that of the nominal design, as indented. Next, the position loop in the $SE\left(3\right)$ estimator, which is shown in Figure 6b, is discussed. One can notice the significance of the sensor fusion architecture in the proposed $SE\left(3\right)$ filter very clearly here. The position error history reaches a consensus value and approaches zero faster than the nominal design. More importantly, the increase in computational load is only ${10}^{-3}$ s greater in each time step than in the nominal design for each satellite. Note that this performance comparison was generated in the HP Z840 workstation with the following specifications: Intel(R) Xeon (R) CPU E5-2609 V4 processor (Intel, Santa Clara, CA, USA) of $1.7$ GHz speed using 64 GB RAM. The novelty of these results lies in the fact that although various estimation strategies are widely available in the literature, they were addressed in the consensus SE(3) framework for the first time in this paper.

Effects of Additional Communication Links

In this section, the objective is to study the effects of using line and complete graph-connected topologies in the consensus estimation of relative spacecraft orbits as in Figure 2. To achieve this objective, the performance in terms of filter convergence speed and consensus speed is studied for the relative orbit scenarios explained in Section 3.

To comprehensively understand the effects of using the consensus estimation system along with the additional communication links, the root mean square of the $SE\left(3\right)$ state error is used, which is defined as follows:

$$\begin{array}{ccc}\hfill {}^{rms}\varphi _{err}& =& {\displaystyle \frac{1}{N}}\sqrt{\sum _{i,j=1;j\ne i}^N∥{}^i\varphi _{err}-{}^j\varphi _{err}∥}\hfill \end{array}$$

(62)

$$\begin{array}{ccc}\hfill {}^{rms}p_{err}& =& {\displaystyle \frac{1}{N}}\sqrt{\sum _{i,j=1;j\ne i}^N∥{}^{i}p_{err}-{}^{j}p_{err}∥}\hfill \end{array}$$

(63)

Since the communication topology based on the line graph is already discussed, the results based on the complete graph topology are assessed in Figure 7. The PRA and position error of the consensus relative orbit scenario is presented in Figure 7a. The filter’s performance based on the complete graph topology is significantly better than the line graph topology. In order to appreciate this, the root mean square error calculated using Equation (62) based on both topologies is plotted in Figure 7b. One can easily appreciate the filter convergence speed and consensus speed, which is notably faster for a complete graph because of the additional communication links among the satellites.

7. Conclusions

The advantages of $SE\left(3\right)$-based estimation are seen primarily in the presence of orbit/attitude coupling effects (gravity gradients, SRP, drag, thrust vector misalignments, etc.) and in the control or estimation design in which a single controller or estimator is required rather than two separate ones, as is normally the case. For handling such practical applications, a novel consensus $SE\left(3\right)$-constrained extended Kalman filter was proposed in this paper, allowing for the coupled estimation of the pose (rotational and translational configurations) while accounting for measurement error statistics and using the rotation matrix instead of attitude parameterization. The performance of the filter was validated through the modeled environment using numerical simulations for satellite formation flying. Moreover, a comparison with a conventional estimation strategy provided credence to the importance of the proposed filter. Hence, the proposed $SE\left(3\right)$-constrained extended Kalman filter is an essential option where the coupling between the rotational and translational dynamics is of utmost importance.