In this tutorial we will develop a coordinated control strategy to keep a group of satellites in a minimum-thrust formation.

This will demonstrate several use-cases and useful features of Collimator:

- Developing a system-of-systems orbital mechanics simulation
- Abstracting functionality into reusable Groups and Submodels
- Vectorizing identical subsystems with Replicators
- Partitioning a control strategy into top-level coordination in Python and decentralized low-level controllers
- Using the python-control library to design LQR controllers

In addition, the tutorial shows how the block diagram abstraction can be used to construct a simulation of a relatively complex controller.

We will implement the algorithm described in Ref [1], which deals with maintaining a local satellite formation undergoing several configuration changes.

Maintaining a formation of satellites is difficult in general because satellites following elliptic orbits do not maintain fixed relative positions.

However, it is possible to determine certain special formations in which the relative positions can be maintained with minimal control effort [2].

For instance, a simple "in-plane" formation consists of two satellites on the same orbit but with different anomalies.

Given such a formation, the Clohessy-Wiltshire-Hill (CWH) equations can then be used to determine initial conditions and predict the relative motion.

The CWH equations are based on a linearization of the equations of motion in the local (noninertial) reference frame about a circular "reference" orbit.

Typically this represents the orbit of some other satellite, in which case it is natural to consider planning rendezvous maneuvers in this local frame.

However, the reference orbit can simply represent the notional "center" of the formation, whether or not there is a physical satellite at the local origin.

In either case, we will work with the equations of motion in the local frame.

Suppose the reference orbit in an Earth-fixed (i.e. approximately inertial) reference frame is

$$ X_\mathrm{ref}(t) = r_0 \cos \omega t, \qquad Y_\mathrm{ref}(t) = r_0 \sin \omega t, $$

with $\omega = \sqrt{\mu / r_0^3}$ and $\mu = 3.986 \times 10^{14}~\mathrm{m}^3/\mathrm{s}^2$.

This coordinate system is defined so that the $X-Y$ axes are in the plane of the reference orbit, with the $Z$ axis normal.

Next, define a local (non-inertial) reference frame $xyz$ centered on $(X_\mathrm{ref}(t), Y_\mathrm{ref}(t))$, with $y$ the along-track direction and $x$ pointing away from the center of Earth.

The equations of motion for a satellite moving in this local frame are

$$ \begin{gathered} \ddot{x} - 2 \omega \dot{y} - \omega^2 (r_0 + x) \left\{ 1 - r_0^3 \left[ (r_0 + x)^2 + y^2 + z^2 \right]^{-3/2} \right\} = u_1 \\ \ddot{y} + 2 \omega \dot{x} - \omega^2 y \left\{ 1 - r_0^3 \left[ (r_0 + x)^2 + y^2 + z^2 \right]^{-3/2} \right\} = u_2 \\ \ddot{z} + \omega^2 r_0^3 z \left[ (r_0 + x)^2 + y^2 + z^2 \right]^{-3/2} = u_3, \end{gathered} \tag{1} $$

where $\mathbf{u} = \begin{bmatrix} u_1 & u_2 & u_3 \end{bmatrix}^T$ are the control inputs.

Although satellites will follow elliptical orbits in the inertial frame, this is not necessarily the case in the local frame, unless the orbits are designed carefully and controlled.

The CWH equations are the linearization of (1):

$$ \begin{gathered} \ddot{x} - 2 \omega \dot{y} - 3 \omega^2 x = u_1 \\ \ddot{y} + 2 \omega \dot{x} = u_2 \\ \ddot{z} + \omega^2 z = u_3, \end{gathered} \tag{2} $$

which can also be written in the first-order form $\dot{\mathbf{x}} = A \mathbf{x} + B \mathbf{u}$, with

$$ A = \begin{bmatrix} 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ 3\omega^2 & 0 & 0 & 0 & 2\omega & 0 \\ 0 & 0 & 0 & -2\omega & 0 & 0 \\ 0 & 0 & -\omega^2 & 0 & 0 & 0\end{bmatrix}, \qquad B = \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 0 \end{bmatrix} \tag{3} $$

The formation design approach taken in Ref [2] is to look for solutions to the CWH equations that satisfy certain constraints corresponding to desired formation geometries.

Although the true nonlinear dynamics will disturb the analytic trajectories, the idea is that these formations will still only require minimal formation-keeping effort.

One such solution corresponds to a circular orbit in the local frame (but elliptic in the Earth-referenced frame) inclined at $30^\circ$ to the cross-track/along-track plane, in which case the solution to the CWH equations is

$$ \begin{aligned} x(t) &= \frac{\dot{x}_0}{\omega} \sin \omega t + x_0 \cos \omega t \\ y(t) &= \frac{2\dot{x}_0}{\omega} \cos \omega t - 2x_0 \sin \omega t \\ z(t) &= \frac{\sqrt{3}\dot{x}_0}{\omega} \sin \omega t + \sqrt{3} x_0 \cos \omega t, \end{aligned} \tag{4} $$

which in turn fully specifies the initial conditions $\mathbf{x}_0 = \begin{bmatrix} x_0 & y_0 & z_0 & \dot{x}_0 & \dot{y}_0 & \dot{z}_0 \end{bmatrix}^T$.

Before we get into coordinated control of a multi-satellite formation, let's first tackle the problem of maintaining *_one_* satellite in this orbit.

We'll start by simulating the linearized equations (2) to check that the solution matches what we expect, then compare with a simulation of the fully nonlinear equations (1).

We expect that the satellite will depart from the desired circular trajectory, so after simulating the natural dynamics we'll turn to the problem of developing a feedback controller.

The easiest way to simulate a linear time-invariant (LTI) system like (2) and (3) is to define the arrays in Python in a script in the Collimator project, then choose that script as the "init" script for the model:

Then the arrays and parameters will be available in the model and we can simulate the linearized system using a StateSpace block:

As expected, the solutions are periodic

To visualize the trajectory in local coordinates, let's use the Python API to run a simulation remotely and retrieve the results

We can also do some quick checks that the result from the StateSpace block matches the analytic solution result, for instance:

The difference between the analytic and numerical solutions is around $10^{-6}$, which is consistent with the tolerance of the continuous-time solver.

Next we'll add the nonlinear dynamics alongside the linear state-space model for comparison. SatDynamics3D implements equation (1) in blocks, but is abstracted into a submodel so we can reuse it easily.

As with the analytic solution, we can compare the linearized and nonlinear simulations using an Adder block. For the most part the relative errors are on the order of 0.1%, but the satellite does drift away from its target trajectory over time, especially in the local $y$-direction.

In order to stabilize the satellite in its target trajectory, we will develop a feedback controller.

However, before we get to that, let's take a moment to zoom out and consider how this relates to the larger goal of distributed control of the satellite formation.

We started by defining a circular "reference trajectory" around the Earth and using as our basic model the nonlinear equations of motion for a satellite in the local reference frame defined by that reference trajectory.

Ultimately our goal is to define a multi-satellite formation in the local frame which can be maintained using minimal thrust.

In order to find such a formation, we looked for initial conditions for the Clohessy-Wiltshire-Hill equations corresponding to circular solutions.

These initial conditions define a particular elliptical orbit around the Earth which appears as a circular trajectory in the local frame.

If the satellite is initialized perfectly with these initial conditions, it will stay relatively close to its target trajectory.

However, we will want to introduce feedback control for three reasons:

- Even if the satellite is initialized perfectly, it will still drift over time due to nonlinearities ignored by the CWH equations
- We want to be able to compensate for imperfect initialization (e.g. from release after launch) and unmodeled disturbances
- We can use the individual satellite controller as the low-level subsystem control in our larger distributed control scheme

Following Ref [1], we will use a strategy of feedback linearization to stabilize this orbit. First, we define a nonlinear control law $\mathbf{u} = \mathbf{u}(\mathbf{x}, \mathbf{u}')$ such that the dynamics *_become_* the CWH equations $\dot{\mathbf{x}} = A \mathbf{x} + B \mathbf{u}'$, with $A$ and $B$ given by equation (3). The linearization control is

$$ \begin{aligned} u_1 &= -\omega^2 (r_0 + x) \tilde{r} + 3 \omega^2 x + u_1' \\ u_2 &= -\omega^2 y \tilde{r} + u_2' \\ u_3 &= -\omega^2 z \tilde{r} + u_3', \end{aligned} $$

where $\tilde{r} = \left\{ 1 - r_0^3 \left[ (r_0 + x)^2 + y^2 + z^2 \right]^{-3/2} \right\}$.

Substituting this into the nonlinear equations of motion (1) results in the much simpler LTI system.

We still need to determine the secondary control inputs $\mathbf{u}'$, but now we can use any linear control design technique.

For this case we will use a simple LQR controller with a strong actuation penalty. This is easy to do with the `python-control` library. All we have to do is add a few lines to our "init" script:

Now the secondary controls $\mathbf{u}'$ can be determined from the linear feedback law $\mathbf{u}' = -K \left(\mathbf{x} - \mathbf{x}_d(t) \right)$, where $\mathbf{x}_d(t)$ is the "desired" trajectory given by equation (4).

Here's what the whole feedback linearization controller looks like:

Note that the state is labeled `q` in the block diagram to distinguish from the variable $x$.

The TargetTrajectory block generates the analytic solution `q_ref` using equation (4).

The LQRController3D block then uses the LQR feedback law `up = -K @ (q - q_ref)` to determine the secondary control inputs $\mathbf{u}'$.

Finally, the FeedbackLinearization3D block combines the secondary controls $\mathbf{u}'$ with the state to determine the actual control inputs $\mathbf{u}$ that are fed back to the system.

This feedback linearization approach is a convenient way to define an effectively nonlinear controller using basic linear systems tools like LQR design.

Since the target state is time-varying, there is a persistent ~0.1% relative tracking error (which equates to a few meters of absolute error).

Between this error and the fact that the thrust is not tightly regulated, the LQR controller is not ideal for real applications.

However, it is a good illustration of a subsystem-level controller that can be combined with the top-level coordination we will explore next.

Now the controlled nonlinear trajectory (yellow) stabilizes to the target trajectory (blue):

Now that we understand how to keep a satellite on a particular trajectory in the local reference frame, we can consider the problem of formation control.

Let's say we have a group of $N$ satellites that we want to maintain in a distributed, low-thrust, collision-free configuration.

One approach to achieving this is to use a centralized planning approach such as trajectory optimization.

Nonlinear trajectory optimization has many advantages, including the ability to explicitly set constraints such as minimum pairwise distances and maximum thrust commands.

However, a proper implementation of this type of centralized open-loop control can be difficult, particularly if there is any state uncertainty or model error.

Coordinated control offers a straightforward alternative that requires minimal communication between the subsystems (individual satellites) and allows for the use of standard feedback control at the subsystem level.

The approach described by Ref [1] is to use a "reference projection" that essentially moves the target trajectory from being absolute (e.g. referenced to time) to being relative (e.g. referenced to a phase difference).

As a simple concrete example, take a circular formation of two satellites A and B, where B is intended to follow A with a phase lag of $\phi$.

We can extract the phase $\theta_A$ of satellite A in the circular trajectory with

$$ \theta_A = \tan^{-1} \left( \frac{x_A + \sqrt{3} z_A}{2 y_A} \right) $$

and likewise for $\theta_B$.

Tracking an absolute target trajectory involves feedback control of phase differences like $(\theta_A - \omega t)$ and $(\theta_B - \omega t + \phi)$.

On the other hand, suppose we replace the absolute target phase $\omega t (+ \phi)$ in each phase difference with reference projections $\gamma$, where $\gamma_A = \theta_A $ and $\gamma_B = \theta_A - \phi$.

The reference-projected phase difference vanishes for satellite A since $\theta_A - \gamma_A = 0$, and for satellite B the phase difference simplifies to $\theta_B - (\theta_A - \phi)$.

In other words, satellite A is the "leader" of the formation in that it may have any phase and no feedback control is applied, but satellite B uses feedback control to approach the target phase difference of $\phi$.

The formation is therefore controlled without centralized planning of the dynamics and control of each subsystem.

Instead, the reference projection simply acts as a top-level coordination between the subsystems that can be used to easily specify different formations.

This approach can be extended to any number of satellites, with any number of leader/follower groups defining sub-formations.

Since the target trajectory is a circle, the reference projection requires both a phase and a radius variable.

A simple way to define a variety of formations is to define the target radius variables $\boldsymbol{\gamma}_r$ as absolutes and the phase variables $\boldsymbol{\gamma}_\theta$ relative to a leader satellite: $\boldsymbol{\gamma}_\theta = Y \boldsymbol{\theta} + \mathbf{b}$, where $Y$ is a matrix that defines the phase combinations.

In the simple two-satellite example above, the radius of the target circular orbit is $x_0$, so $\boldsymbol{\gamma}_r = \begin{bmatrix} x_0 & x_0 \end{bmatrix}^T $ and

$$ \boldsymbol{\gamma}_\theta = \begin{bmatrix} 1 & 0 \\ -1 & 1 \end{bmatrix} \begin{bmatrix} \theta_A \\ \theta_B \end{bmatrix} + \begin{bmatrix} 0 \\ \phi \end{bmatrix} $$

This approach can also be used to switch between multiple formations by defining $\{Y^{(i)}, \mathbf{b}^{(i)}, \boldsymbol{\gamma}_r^{(i)}\}$ for each stage and switching between formation specifications using a StateMachine or predefined transition times.

The subsystem-level LQR controllers will then use feedback control based on the reference projections to steer the satellites into the new formation.

Extending the single-satellite model to multi-satellite formation control in Collimator is straightforward, but it does require a few modifications.

Here's what the full model looks like:

Let's break down what each component is doing.

The Replicator block copies a Group or Submodel $N$ times.

In our case the Group contains essentially the same model as we used above for the single satellite with feedback linearization and LQR control.

Each copy represents an individual satellite model:

- The inputs are the current state `q` and reference projection `gamma`
- TargetTrajectory computes the target state (called `q_ref` above) from equation (4) above, but instead of using the initial radius $x_0$ and true phase $\omega t$, it uses the reference-projected radius and phase variables $\gamma_r$ and $\gamma_\theta$ as described above.
- LQRController3D computes secondary controls `up` based on the state `q` and target state `q_ref`.
- FeedbackLinearization3D uses nonlinear feedback linearization control law along with the state `q` and secondary controls `up` to compute the true actuation commands `u`
- Finally, SatDynamics3D uses the current state `q` and actuation `u` to compute the time derivative of the state `dq`

Note that the Submodel blocks LQRController3D, FeedbackLinearization3D, and SatDynamics3D are all reused from the single-satellite control case we developed above.

Importantly, the information fed to each replicated satellite model is only about that individual satellite and not global information about the entire formation.

The Replicator block expects inputs with leading dimension $N$ and then feeds the $i$-th entry of the inputs to the $i$-th copy.

To understand this, it is helpful to turn on the "Show dimensions" layer in Collimator:

Here we have $N=6$ satellites. Both the state `q` and reference projections `gamma` that are fed to the Replicator have leading dimension $N$, so that `q` has shape `(6,6)` since there are 6 degrees of freedom per satellite.

Similarly `gamma` has shape `(6,2)` since there are two reference projection variables.

Inside the Replicator, each subsystem only sees its own state and reference projection, so `q` has shape `(6)` and `gamma` has shape `(2)`.

If you're familiar with machine learning frameworks like PyTorch and Jax, you can think of this as a "batch dimension" over which the Replicator is vectorized.

The Replicator therefore nicely mirrors the structure of distributed or system-of-systems control problems, where each subsystem is primarily only aware of its own local conditions and there is only a limited amount of top-level coordination.

A simple PythonScript block specifies the desired formation in terms of the configuration $\{Y^{(i)}, \mathbf{b}^{(i)}, \boldsymbol{\gamma}_r^{(i)}\}$ defined above.

In a more realistic setting, we might use a StateMachine to transition between formations depending on task-dependent criteria (e.g. completed rendezvous or orbit-change maneuver), but for this tutorial we just predefine a series of transition times between seven formation stages:

- Even distribution in the same orbit. All satellites track a single leader with staggered phase offsets.
- Move satellite $N$ to the reference orbit (the center of the formation), simulating a rendezvous or docking operation with some station or satellite following the reference orbit. The other $N-1$ satellites continue following the leader, but redistribute themselves evenly around the orbit.
- Move satellite $N-1$ to a circular "parking orbit" with radius $\gamma_r = 1.5 x_0$. Again, the remaining satellites evenly distribute in the original circular target orbit.
- Move satellite $N-2$ to the parking orbit and follow satellite $N-1$ with a phase offset.
- Move satellite $N$ from the center of the formation back to the original target orbit. All satellites in the target orbit should redistribute to make room for the new arrival.
- Move satellite $N-1$ from the parking orbit back to the target orbit. Again, all satellites in the target orbit should redistribute themselves.
- Move satellite $N-2$ back to the target orbit for an identical formation to Stage 1.

These stages are defined with standard Python code in the Formation block, which returns the configuration specification `Y`, `b`, and `r`.

This Group block implements the calculation of the reference-projected variables $\gamma_\theta$ and $\gamma_r$ based on the configuration specification $\{Y \mathbf{b}, \boldsymbol{\gamma}_r\}$ and the current state.

This is the only step that requires full knowledge of the state of all satellites, and must be vectorized in order to pass the "batched" signal with leading dimension $N$ to the Replicator block.

For the sake of simplicity, the calculation of the phases $\theta$ is carried out in a simple PythonScript block. As with the Formation block, this could easily be rewritten as a CFunction for deployment.

Because we have represented the subsystem dynamics in the Replicator block, the state signal is matrix-valued. This is not a problem for the Integrator block, but the Pandas-based data collection and visualization tools in Collimator are not currently compatible with matrix-valued signals.

In order to collect the data locally for visualization, we can just Demux the signal along the first dimension, which produces $N$ vector signals corresponding to the individual satellites.

Similarly, we can calculate useful global observables like the local radius $r_i = \sqrt{x_i^2 + y_i^2 + z_i^2}$.

We can clearly see the stage transitions happening, as satellite 5 moves to the center $r_5 = 0$, then satellite 4 moves to the parking orbit, etc.

The non-monotonic convergence to new radial positions is a reflection of the nonlinear control law.

Intuitively, this is a result of working in a non-inertial reference frame.

Since each satellite is on an elliptic trajectory in the inertial frame, the most efficient path to the desired trajectory in the non-inertial frame may look unexpected.

Similarly, when the satellites redistribute themselves in a new stage, the configuration will temporarily depart from the circular trajectory in order to redistribute its phases.

To get a better visualization we can also run the simulation via the Python API as in the single-satellite case and use Matplotlib animation tools here.

If you want to build on this demo and experiment more with Collimator and system-of-systems control, here are a few things you could try:

As noted by Ref [1], the LQR controller directly penalizes the *_secondary_* control inputs $u'$, but not the actual thrust commands $u$, which are modified by the feedback linearization.

However, fuel consumption is among the most important considerations when planning control strategies for orbital mechanics, so a more realistic subsystem control strategy would account for fuel use directly. To explore this, try:

- Researching more efficient control strategies, for instance that ensure that fuel usage stays within some reasonable limit.
- Implement a new control strategy for the single-satellite problem and compare fuel consumption with the LQR+feedback linearization controller
- Move the new low-level controller into the Replicator block to experiment with modular design of system-of-systems controllers

Here we have constructed a control strategy based on a desired circular formation in the local reference frame.

However, Ref [[2]](https://arc.aiaa.org/doi/10.2514/2.3681) describes three other formations: in-plane, in-track, and projected circular.

Depending on the application, a formation like in-track may be more relevant than the circular formation.

To explore this try:

- Designing a single-satellite controller to keep a satellite in one of the other formations (e.g. in-track)
- Derive an appropriate reference projection based on the analytic solution of the CWH equations for this formation
- Modify the distributed controller to use the new reference projection and perform some relevant formation maneuver, such as replacing a satellite while ensuring ground-track coverage.

The equations of motion (1) are highly idealized, neglecting many important astrodynamics effects.

While they may be reasonable for prototyping and controller design, it is often necessary to simulate at higher fidelity, for instance to quantify formation-keeping costs.

To explore this, try:

- Adding physics such as a geopotential, atmospheric drag model, or three-body effects from the sun and moon. How much does the performance of a controller designed based on the idealized model degrade under these effects?
- Replacing the point-mass satellite approximation with a more realistic 6-dof model where the actuators provide both forces and torques (see the RigidBody block).
- Removing the assumption of full-state knowledge and model noisy limited measurements with state estimation. Is the distributed controller more or less robust to incomplete information compared to a centralized controller?