Autonomous Rendezvous and Docking with

Tumbling, Uncooperative, and Fragile Targets

under Uncertain Knowledge
by
Hailee Elida Hettrick
B.S., Cornell University (2017)
Submitted to the Department of Aeronautics and Astronautics
in partial fulfillment of the requirements for the degree of
Master of Science in Aeronautics and Astronautics
at the
MASSACHUSETTS INSTITUTE OF TECHNOLOGY
June 2019
© Massachusetts Institute of Technology 2019. All rights reserved.

Author . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Department of Aeronautics and Astronautics
May 10, 2019

Certified by . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
David W. Miller
Professor, Aeronautics and Astronautics
Thesis Supervisor

Accepted by . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Sertac Karaman
Chairman, Department Committee on Graduate Theses
2
 Autonomous Rendezvous and Docking with Tumbling,
Uncooperative, and Fragile Targets under Uncertain
Knowledge
by
Hailee Elida Hettrick

Submitted to the Department of Aeronautics and Astronautics

on May 10, 2019, in partial fulfillment of the
requirements for the degree of
Master of Science in Aeronautics and Astronautics

Abstract
As efforts to expand humanity’s presence in space continue to increase, a need for
spacecraft to autonomously perform in-space close proximity maneuvers without a
human operator increases, as well. Such in-space close proximity maneuvers include
active debris removal, satellite servicing, and in-space assembly. Active debris removal
will facilitate the continued use and access to low Earth orbit, mitigating the expo-
nential debris growth occurring due to decrepit satellites and rocket bodies colliding.
Satellite servicing will provide the capability to repair and refurbish spacecraft, elon-
gating the lifetime of valuable assets both locally orbiting Earth and on routes further
out in the solar system. In-space assembly is the means by which large space struc-
tures are developed in orbit. Currently, such feats occur with the help of astronauts
and robotic arms (i.e. the continued development of the International Space Station).
However, for increased benefit, in-space assembly must occur autonomously, without
a human in-the-loop, in order to create large structures in locations unideal for hu-
mans or with a non-negligible communication latency. These three reference missions
need the software enabling autonomous rendezvous and docking to reach a technical
readiness level to be employed with confidence. In-space close proximity maneuvers
share a standard sequence of events described in this thesis. The focus of this thesis
address the terminal approach trajectory to soft docking, the contact dynamics of
docking between two spacecraft, the optimization of the detumble procedure to bring
the Target to stabilization, and adaptive control techniques to handle uncertainties
in spacecraft knowledge. The software developed in support of these subproblems is
included in the appendices and is largely based on implementation with the Synchro-
nized Position Hold Engage Reorient Experimental Satellites (SPHERES) platform
or with the characteristics of SPHERES considered.

Thesis Supervisor: David W. Miller

Title: Professor, Aeronautics and Astronautics

3
4
Acknowledgments
This research was conducted primarily under the support of the Massachusetts Insti-
tute of Technology Department of Aeronautics and Astronautics through a research
assistantship. The author thanks the department for its support and Dr. David
Miller for his guidance through the research. Additional gratitude is expressed to
the SPHERES team and former postdoc Danilo Roascio for assistance with using the
SPHERES testbed.
Lastly, many thanks to the never-ending support and wisdom from family and the
friendship and community provided by Jess, Alexa, Cadence, and Golda.

5
6
Contents

1 Introduction 21
1.1 Motivation for Autonomous Rendezvous & Docking . . . . . . . . . . 21
1.1.1 History of Rendezvous and Docking . . . . . . . . . . . . . . . 21
1.1.2 Reference Missions for Autonomous Rendezvous & Docking . . 24
1.2 SPHERES Platform . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
1.2.1 Status of Testbed . . . . . . . . . . . . . . . . . . . . . . . . . 32
1.3 Relative Operations for Autonomous Maneuvers . . . . . . . . . . . . 33
1.4 Research Objective . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
1.5 Thesis Roadmap . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

2 Literature Review 37
2.1 Trajectory Optimization . . . . . . . . . . . . . . . . . . . . . . . . . 37
2.2 Nonlinear Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
2.2.1 Sliding Mode Control . . . . . . . . . . . . . . . . . . . . . . . 39
2.2.2 Adaptive Control . . . . . . . . . . . . . . . . . . . . . . . . . 39
2.3 Attitude Stabilization under Actuator Loss-of-Effectiveness . . . . . . 43
2.4 Soft Docking & Stabilization of a Tumbling Target . . . . . . . . . . 44

3 Trajectory Optimization 47
3.1 Stage Description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
3.2 Synchronous Radial Approach via Second Order Differential Equation 48
3.3 Differential Equation Propagator Architecture . . . . . . . . . . . . . 56
3.3.1 R-Bar Alignment . . . . . . . . . . . . . . . . . . . . . . . . . 57

7
 3.3.2 Forward Propagation . . . . . . . . . . . . . . . . . . . . . . . 59
3.3.3 Determine Final Position . . . . . . . . . . . . . . . . . . . . . 59
3.3.4 Backwards Propagation . . . . . . . . . . . . . . . . . . . . . 60
3.3.5 Radial Shift . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

4 Adaptive Control for Thrust and Inertia Uncertainties 67

4.1 Thruster Degradation Characterization . . . . . . . . . . . . . . . . . 67
4.1.1 Thruster Characterization in Test Session 97 . . . . . . . . . . 68
4.1.2 Thruster Force Characterization . . . . . . . . . . . . . . . . . 70
4.1.3 Maintenance Session . . . . . . . . . . . . . . . . . . . . . . . 73
4.2 In-Flight Adaptive PID Sliding Mode Position and Attitude Controller 82
4.2.1 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . 83
4.2.2 Software and Hardware Implementation . . . . . . . . . . . . 88
4.2.3 Results & Discussion . . . . . . . . . . . . . . . . . . . . . . . 90

5 Docked Dynamics 101

5.1 Contact Dynamics of Docking . . . . . . . . . . . . . . . . . . . . . . 101
5.2 Modeled Docked Dynamics . . . . . . . . . . . . . . . . . . . . . . . . 105
5.3 Discrete Solver for Docked Dynamics . . . . . . . . . . . . . . . . . . 110
5.4 Detumble Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . 112
5.4.1 Indirect Optimization on Angular Acceleration Profiles . . . . 113
5.4.2 Direct Optimization Approach . . . . . . . . . . . . . . . . . . 117
5.4.3 Comparison of Optimization Results . . . . . . . . . . . . . . 120

6 Conclusions and Future Work 123

6.1 Summary of Thesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123
6.2 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
6.3 Recommendations for Future Work . . . . . . . . . . . . . . . . . . . 127

A Trajectory Optimization 129

A.1 Fuel-Optimal Trajectory Optimization Functions . . . . . . . . . . . . 129
A.2 Synchronous Radial Approach Propagator . . . . . . . . . . . . . . . 135

8
B Adaptive Control for Uncertainty 149
B.1 Thruster Characterization Plots . . . . . . . . . . . . . . . . . . . . . 149
B.2 Thruster Characterization Functions . . . . . . . . . . . . . . . . . . 150
B.3 Adaptive PID SMC Controller . . . . . . . . . . . . . . . . . . . . . . 165

C Docked Dynamics Functions 171

C.1 Contact Dynamics of Docking Event . . . . . . . . . . . . . . . . . . 171
C.2 Setting up Target and Chaser . . . . . . . . . . . . . . . . . . . . . . 174
C.3 Discrete Solver . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 177
C.4 Direct Optimization Using GPOPS . . . . . . . . . . . . . . . . . . . 184

9
10
List of Figures

1-1 Remote Manipulator System on STS-125, Hubble berthed to Shuttle [8] 23

1-2 Orbital Express Mission: ASTRO and NEXT [13] . . . . . . . . . . . 23
1-3 Effective number of LEO objects larger than 10 cm [15] . . . . . . . . 25
1-4 Conceptual diagram of potential capturing methods [17] . . . . . . . 26
1-5 Conceptual diagram of potential removal methods [17] . . . . . . . . 26
1-6 13-year construction progress of the International Space Station . . . 27
1-7 Astronauts install set of corrective lenses for flawed mirror in HST on
first servicing mission (STS-61) [25] . . . . . . . . . . . . . . . . . . . 28
1-8 Canadarm2 post-releasing Dragon spacecraft [27] . . . . . . . . . . . 29
1-9 Two SPHERES equipped with UDPs performing docking maneuver
onboard the ISS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
1-10 Expanded view of SPHERES . . . . . . . . . . . . . . . . . . . . . . 32
1-11 Spatial representation of the stages of ROAM . . . . . . . . . . . . . 33

2-1 Diagram of a generic adaptive control system configuration from Lan-

dau’s textbook [44] . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
2-2 Diagram of a model reference adaptive control system . . . . . . . . . 41
2-3 Diagram of a self-tuning control system . . . . . . . . . . . . . . . . . 42
2-4 Diagram of a gain scheduling control system . . . . . . . . . . . . . . 42

3-1 Reference frames used in discussion of trajectory generation . . . . . 48

3-2 Sequence of optimizations and function-fitting that led to a trajectory-
generation differential equation from [38] . . . . . . . . . . . . . . . . 49
3-3 Δ𝑉 of five trajectory optimizations with fixed final time . . . . . . . 50

11
3-4 Euclidean norm of acceleration components as fraction of the Euclidean
norm of total acceleration . . . . . . . . . . . . . . . . . . . . . . . . 51
3-5 Sum of Euclidean norm of radial and tangential acceleration compo-
nents as fraction of the Euclidean norm of total acceleration . . . . . 52
3-6 Vector elements of acceleration components for each generated trajectory 54
3-7 Chaser’s trajectory from call to ode45 using Listing 3.1 . . . . . . . . 56
3-8 Illustrating the necessary initial conditions for the propagator i.e. the
Chaser is at the desired radius and the Chaser is along the Target’s
R-bar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
3-9 Timeline illustrating the Chaser’s intersections with the Target’s R-bar
at the desired initial radial length away from the Target . . . . . . . . 58
3-10 Forward propagate Target’s orientation and rotation rate for 𝑛 periods 59
3-11 Generated trajectory yielded from 𝑡𝑓 from forward propagation prior
to radial shift to align initial position with Chaser’s current position . 63
3-12 Generated trajectory after radial shift with initial position aligned with
Chaser’s current position . . . . . . . . . . . . . . . . . . . . . . . . . 64
3-13 Illustration of timeline shift after shifting the radial approach to align
with the Chaser’s current position . . . . . . . . . . . . . . . . . . . . 65

4-1 Location and indices of thrusters on satellite with noted exhaust direction 68
4-2 Thruster characterization: raw accelerometer and gyroscope data from
IMU in test session 97 . . . . . . . . . . . . . . . . . . . . . . . . . . 69
4-3 Thruster characterization: 200 ms impulse accelerometer data sepa-
rated by axis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
4-4 Calculated force magnitude of each thruster on Blue and Orange satellites 73
4-5 Test session 101: calculated force of each thruster and each satellite . 75
4-6 Test 11: closed-loop x-translation telemetry . . . . . . . . . . . . . . 77
4-7 Test 12: closed-loop y-translation telemetry . . . . . . . . . . . . . . 78
4-8 Test 13: closed-loop z-translation telemetry . . . . . . . . . . . . . . . 79
4-9 Test 14: closed-loop x-rotation telemetry . . . . . . . . . . . . . . . . 80

12
4-10 Test 15: closed-loop y-rotation telemetry . . . . . . . . . . . . . . . . 80
4-11 Test 16: closed-loop z-rotation telemetry . . . . . . . . . . . . . . . . 81
4-12 Flowchart of the in-flight adaptive PID sliding mode controller as im-
plemented in C-code . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
4-13 A comparison of commanded position and attitude vs. measured teleme-
try from simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92
4-14 Adaptive PID SMC time histories corresponding to position and atti-
tude from simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . 93
4-15 Trajectories plotted in the 𝑥-𝑦 plane from simulation . . . . . . . . . 95
4-16 Trajectories plotted in the 𝑥-𝑦 plane from hardware tests . . . . . . . 96
4-17 A comparison of commanded position and attitude vs. measured teleme-
try from hardware tests . . . . . . . . . . . . . . . . . . . . . . . . . . 97
4-18 Adaptive PID SMC time histories corresponding to position and atti-
tude from hardware tests . . . . . . . . . . . . . . . . . . . . . . . . . 98

5-1 Chaser and Target during instance before docking occurs denoted with
separate position, velocity, and attitude vectors . . . . . . . . . . . . 102
5-2 Chaser and Target in the instance after docking denoted with aggre-
gated translational and attitude velocity . . . . . . . . . . . . . . . . 103
5-3 Chaser and Target as docked, aggregated system with denoted body-
fixed frame at Target’s center of mass and the force and torque direc-
tions of the Chaser . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
5-4 Free body diagram of the Chaser displaying Chaser’s abilities to impart
force and torques and its internal loads and moments from interacting
with the Target . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107
5-5 Expected angular velocity time histories from integration of selected
angular acceleration functional forms . . . . . . . . . . . . . . . . . . 114
5-6 Angular acceleration profiles as axisymmetric spacecraft initially rotate
about all three axes . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
5-7 Angular acceleration profiles as spacecraft initially rotate about 𝜓-axis 121

13
5-8 Angular acceleration profiles as axisymmetric spacecraft initially nutate 121

B-1 Thruster Characterization: 50 ms and 100 ms Impulses . . . . . . . . 149

14
List of Tables

3.1 Inputs to propagateToTarget function with descriptions . . . . . . 61

4.1 Thruster force and torque directions in SPHERES body-fixed frame . 69

4.2 Calculated thruster forces for each satellite and ratios of calculated
thruster force divided by expected force for each satellite . . . . . . . 74
4.3 Test session 101 descriptive list of tests executed . . . . . . . . . . . . 75
4.4 Sequential series of maneuvers and corresponding commanded position
and quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89

5.1 Notation for contact dynamics of two spacecraft docking to one another 102
5.2 fmincon optimization options used in the indirect optimization of an-
gular acceleration profiles . . . . . . . . . . . . . . . . . . . . . . . . . 113
5.3 Method 1 optimization results of optimal Δ𝑡 and associated cost . . . 116
5.4 Method 2 optimization results of optimal Δ𝑡 and associated cost . . . 119

15
16
Nomenclature

Acronyms

ATV Agena Target Vehicle

CM Center of mass

DART Demonstration of Autonomous Rendezvous Technology

DOF Degree-of-freedom

EVA Extravehicular Activity

GN&C Guidance, Navigation, & Control

HST Hubble Space Telescope

IMU Inertial measurement unit

ISS International Space Station

JEM Japanese Exploration Module

LEO Low Earth Orbit

MIT Massachusetts Institute of Technology

MRAC Model reference adaptive control

PADS Position and Attitude Determination System

PID Proportional-Integral-Derivative

17
ROAM Relative Operations for Autonomous Manev

SLV Satellite Launch Vehicle

SMC Sliding mode control

SPHERES Synchronized Position Hold Engage Reorient Experimental Satellites

SSL Space Systems Laboratory

SSRMS Space Station Remote Manipulator System

TS Test Session

UDP Universal Docking Port

VERTIGO Visual Estimation for Relative Tracking and Inspection of Generic Ob-
jects

XSS Experimental Satellite System

Coordinate Frames

CHA Chaser body-fixed frame

INT Inertial frame

TAR Target body-fixed frame

Variables

Δ𝑉 Fuel consumption metric represented by sum of velocity of a maneuver

𝜆 Wavelength

R𝐵2𝐼 Sliding variable associated with state 𝑝

𝜑 Roll angle

𝜓 Yaw angle

18
𝜃 Pitch angle

𝛼
⃗ Angular acceleration vector

𝜔
⃗ Angular velocity vector

⃗𝑟 Position vector

⃗𝑢 Control input vector

𝑞 Quaternion

𝑠𝑝 Sliding variable associated with state 𝑝

D Aperture diameter

19
20
Chapter 1

Introduction

1.1 Motivation for Autonomous Rendezvous & Dock-

ing
As manned and unmanned space missions become more complex, the need for reli-
able autonomous rendezvous and docking capabilities becomes paramount. Missions
that will benefit from autonomous rendezvous and docking include in-space assembly,
satellite servicing, and active debris removal. Mankind’s history of space missions is
already rich with human-assisted instances of in-space assembly and satellite servic-
ing, hinting at a wider set of capabilities that will be enabled by removing humans
from the loop.

1.1.1 History of Rendezvous and Docking

Initial interest in spacecraft rendezvous coincided with the beginning of the Space Age
in the late 1950s [1]. Technical adeptness at in-space rendezvous improved through
the course of the Gemini program in preparation for the Apollo program. Gemini 6A
and 7 achieved the first space rendezvous using a concentric orbit rendezvous plan [2].
Gemini 8’s sequences were a repeat of the previous mission with the Agena Target
Vehicle (ATV) with the addition of humans-in-the-loop for docking the two vehicles
[3]. Additional rendezvous sequences were tested in missions 9 through 12 of the

21
Gemini program, proving in-space rendezvous feasible in preparation for the Apollo
missions.
Rendezvous and capture were necessary functionalities for the Apollo program to
satisfy its mission objectives, as the Lunar Excursion Module had to rendezvous and
dock with the Command Service Module [4]. The coelliptic rendezvous scheme was
utilized for Apollo 11 and 12 [5], and the lessons learned from those missions as well
as 7, 9, and 10 led to the utilization of a shorter rendezvous profile in missions 14
through 17 that increased time on the lunar surface [6]. In this manner, rendezvous
and docking were essential maneuvers that facilitated visiting the Moon.
The era of the Space Shuttle ushered in new rendezvous, capture, and docking
approaches necessitated by the target spacecraft. Shuttle’s onboard guidance, naviga-
tion, and control (GN&C) subsystem autonomously performed rendezvous functions
but required the crew to be in-the-loop for docking maneuvers [4]. Additionally, the
Shuttle program was the first to make use of a robotic arm (the Remote Manipulator
System) for capture and berthing (see Figure 1-1) [7]. Therefore, the Shuttle did
not only advance the software capabilities critical for autonomous rendezvous and
docking, but it also had a significant impact on the hardware that facilitates such
maneuvers.
The Demonstration of Autonomous Rendezvous Technology (DART) was the first
US effort to perform rendezvous and docking without an astronaut piloting the ma-
neuver. Its mission objectives included demonstrating R-bar, V-bar, and circumnavi-
gation approach methods [9]. Launched in April 2005, the rendezvous maneuvers were
completed successfully. Unfortunately, DART’s mission was not entirely successful
as consequence of a sensor anomaly resulting in more fuel usage and a collision with
the secondary spacecraft, indicating necessary system engineering improvements for
a fully autonomous rendezvous mission [10].
XSS-10 and Orbital Express represented a focus shift towards satellite servicing.
The Air Force Research Laboratory’s XSS-10, which was deployed from a Delta II
vehicle in January 2003, successfully semi-autonomously maneuvered around the sec-
ond stage, performing close-proximity inspection of the stage [11]. Launched in March

22
Figure 1-1: Remote Manipulator System on STS-125, Hubble berthed to Shuttle [8]

2007, DARPA’s Orbital Express demonstrated the hardware and software abilities to
autonomously capture the Target and to autonomously service the Target [12]. Or-
bital Express was a key milestone in validating these technologies, illustrating the
technical feasibility of satellite servicing [10].

Figure 1-2: Orbital Express Mission: ASTRO and NEXT [13]

23
1.1.2 Reference Missions for Autonomous Rendezvous & Dock-
ing

Technology demonstration missions for autonomous rendezvous and docking have so

far not demonstrated the ability to handle non-ideal conditions, such as would be
expected for realistic in-space close proximity operations. These types of missions
include in-space assembly, satellite servicing, and active debris removal. Of these
types of missions, it is expected that the target spacecraft or object would not be
well-understood and is likely to be uncooperative.

Active Debris Removal

In 1978, Kessler predicted that by the year 2000, existing pieces of small debris will
exponentially increase via collisions even without additional debris being added from
orbit. As of 2010, Kessler’s prediction has proven true – the exponential increase
of small debris from collisions is coined the Kessler Syndrome [14]. Specifically, the
debris population of low Earth orbit (LEO) is at risk of reaching a capacity of decrepit
spacecraft and launch vehicle stages that satellites will no longer be able to be placed
in LEO [15]. Kessler also emphasized the need to implement mitigation practices in
the wake of inadequate collision avoidance by not leaving future payloads and rocket
bodies in orbit once deprecated. Figure 1-3 illustrates the effective number of objects
in LEO that are 10 cm or larger based on simulation. "Effective number" refers to the
fraction of time per orbit period than an object spends within the altitude range of
200 km to 2000 km [15]. Figure 1-3 indicates the steady growth of collision fragments
as collision-created debris causes more collisions.
Motivated by the dire situation the Kessler Syndrome presents, software develop-
ment has begun to make feasible active debris removal. Bonnal et al. highlight the
general high-level functions agnostic to the Chaser that are necessary for de-orbiting
debris. In summary, these functions are

1. Far-range rendezvous between Chaser and debris

24
 Figure 1-3: Effective number of LEO objects larger than 10 cm [15]

2. Short-range rendezvous between Chaser and debris – a potentially more complex

task based on the nature of the debris object (i.e. non-cooperative and tumbling)

3. Mechanical interfacing – docking, grappling, electromagnets, net

4. Control, detumbling, and re-orientation of the debris

5. De-orbitation

Details of each function is further elaborated in Bonnal et al’s paper. In addition to

outlining these necessary functionalities for an active debris removal mission, Bonnal
also stresses the need for more technical studies that investigate realistic solutions
and the political minefield that is inherent in using such technology [16].
Despite being past the tipping point of spacecraft density in LEO, no active debris
removal missions have launched. The literature provides a thorough review of existing
technology or concepts that would support such missions. Figure 1-4 illustrates the
proposed capture methods, and Figure 1-5 presents the proposed removal methods
[17].

25
 Figure 1-4: Conceptual diagram of potential capturing methods [17]

Figure 1-5: Conceptual diagram of potential removal methods [17]

In-Space Assembly

Most spacecraft are limited in size and functionality by the size of the payload fairing
of the launch vehicle. One workaround for this issue are utilizing complex deployable
systems, a methodology the James Webb Space Telescope has employed [18]. In-space
assembly with a human operator has occurred and is a continued possibility; however,
such an operation has inherent limitations. The International Space Station (ISS) is
a noteworthy example of in-space assembly with humans-in-the-loop (see Figure 1-
6). Lessons learned from the assembly of the ISS emphasized the importance of
offloading the work placed on the EVA astronaut as advances in computer vision,
remote operations, and autonomy occur [19].
According to Mohan, the feasibility of future space missions depends on the tech-
nical advancement of in-space assembly to facilitate the next-generation of space

26
 (a) Zarya and Unity on December 13, 1998 (b) ISS on March 7, 2011 [21]
[20]

Figure 1-6: 13-year construction progress of the International Space Station

telescopes, fuel depots, and solar power stations [22]. The next-generation of space
telescopes presents the most pressing case for robotic in-space assembly. Complex
deployable systems present a high risk to the mission, and desirable locations for
space telescopes are not necessarily in locations accessible by astronauts [23]. For
space telescopes (and other payload equipment that utilize apertures), the angular
resolution improves as the aperture diameter improves according to 𝜃 = 1.22 𝐷𝜆 , where
𝐷 is the aperture diameter and 𝜆 is the wavelength [24]. A higher angular resolution
is desirable as it allows for more precise imaging.
The disadvantages of human-assisted assembly are communication latencies (for
tele-operation), a lengthy lead time of roughly a decade before a mission occurs,
limited locations based on where humans can travel, and increased costs of assembly
due to human involvement. Robotic in-space assembly offers a low-cost and low-
risk alternative; however, technological progress in the use of space robotics and
autonomous rendezvous and docking methodologies is necessary for feasibility [22].

Satellite Servicing

Satellite servicing offers several benefits scientifically and economically. As humanity

expands outward, infrastructure must exist for refueling, repairing, and refurbishing
spacecraft. Not only would satellite servicing offer the means to extend the lifetime
of satellites that have already been significantly invested in, but it is reasonable to

27
claim it will also be a necessity as spacecraft systems become more complicated [10].
Astronaut-assisted satellite servicing has already proven its use with respect to re-
pairing Skylab and the Hubble Space Telescope (HST). The first instance of servicing
occurred shortly after Skylab’s launch in 1973. The micrometeroid shield failed and a
main solar array panel failed to deploy. The crew performed a series of extravehicular
activities (EVAs) to replace the shield. More notorious were the issues with HST,
which was designed to support on-orbit servicing via astronaut support. On initial
checkout, an optical flaw was discovered in HST’s primary mirror and it experienced
thermal jittering during orbital sunrise and sunset. Five servicing missions were de-
signed and executed to repair, replace, or augment various aspects of HST, the first
of which is depicted in Figure 1-7 [10].

Figure 1-7: Astronauts install set of corrective lenses for flawed mirror in HST on
first servicing mission (STS-61) [25]

The Space Station Remote Manipulator System (SSRMS), also known as Canadarm2

28
(see Figure 1-8), is a fully operational, 17-meter long robotic arm attached to the Inter-
national Space Station that offers seven degrees of freedom and is capable of handling
payloads up to 100,000 kg of mass [26]. Canadarm2 assists in maintaining the ISS,
grappling resupply spacecraft and berthing them to the ISS, and moving equipment.
Not only is Canadarm2 an example of the use of robotics in satellite servicing, but
it is also the recipient of servicing from astronauts. For example, in 2002, one of its
wrist roll joints was replaced and it received new latching end effectors in 2017 and
2018 [27]. Robotic arms are expected to continue and serve in a critical capacity for
future satellite servicing missions.

Figure 1-8: Canadarm2 post-releasing Dragon spacecraft [27]

Designing spacecraft to be serviced has already shown promising results, how-

ever, that is only part of the satellite servicing equation. Fully autonomous satellite
servicing requires the continued development of hardware and software capable of
autonomous rendezvous and docking as well as the advancement of utilizing robotic
manipulators. Removing the astronaut involvement from the satellite servicing sce-
nario will permit servicing on assets deployed in locations currently unreachable by
astronauts. Additionally, a dependence on such autonomy will be integral as our
space exploration aspirations become more complex and occur over distances where
communication latencies are a hindrance.

29
1.2 SPHERES Platform

SPHERES (see Figure 1-9) is a platform for the development and validation of for-
mation flying, docking, control, estimation, navigation, and guidance algorithms for
separated and aggregated spacecraft systems [28], [29]. There are three testbeds for
SPHERES – the Japanese Exploration Module (JEM) in the International Space
Station (ISS), the glass table in the Space Systems Laboratory (SSL) at the Mas-
sachusetts Institute of Technology (MIT), and the flat floor facility at MIT. The
testbed aboard the ISS allows for six degree-of-freedom testing because of the micro-
gravity environment – a unique feature this program offers for spacecraft algorithm
development. The two testbeds at MIT – the glass table and the flat floor – offer
three degree-of-freedom environments for SPHERES testing. Although this is a more
constrained environment, these testbeds are easier to access and all intended ISS test
sessions are first implemented on either the glass table or the flat floor.

Figure 1-9: Two SPHERES equipped with UDPs performing docking maneuver on-
board the ISS

There are three satellites aboard the ISS and three satellites in the SSL at MIT.
Each satellite is equipped with 12 CO2 thrusters that provide full translational and ro-
tational actuation. For ground-based testing, the satellites are placed on air carriages

30
that use CO2 to produce a thin film of air underneath the pucks. In this manner,
the satellites can translate in 𝑥 and 𝑦 and rotate about 𝑧 nearly frictionlessly. These
coordinates are defined in the body-fixed reference frame of each satellite. The origin
is defined as the geometric center of the satellite. The +𝑥 axis goes through the ex-
pansion port, +𝑦 is opposite of the control panel, and +𝑧 goes through the regulator
knob.

The satellites were designed to limit astronaut and research accessibility to only
the battery compartment, the CO2 tank, the expansion port, and the regulator
knob. The internal structure is an aluminum frame that all the internal devices
are mounted to. The external assembly is a polycarbonate shell, called out in Figure
1-10. The 12 thrusters operate based on on/off commands and offer full controllabil-
ity in six degrees-of-freedom. Each thruster produces a nominal force of 0.1 N, and
these thrusters are not throttleable. The satellite determines its metrology via the
Position and Attitude Determination System (PADS). Five ultrasound beacons are
wall-mounted about the test volume, and each satellite is equipped with ultrasound
receivers enabling global position knowledge. Additionally, each satellite has three
single-axis rate gyroscopes to measure body-axis angular rates and three single-axis
accelerometers to measure its linear acceleration. In this manner, each satellite is
controllable and observable.

Over the course of the satellites’ lifetime, several peripherals have been developed
and validated, expanding the capabilities of the satellites aboard the ISS. These pe-
ripherals include Halo, Universal Docking Port (UDP), and Visual Estimation for
Relative Tracking and Inspection of Generic Objects (VERTIGO). Halo is a struc-
tural attachment that expands the single expansion port of SPHERES to six expan-
sion ports, enabling a single satellite to use both UDP and VERTIGO at the same
time. The UDP is an attachment that permits unidirectional docking between two
satellites. Prior to the development of UDP, the satellites "docked" via Velcro straps.
With UDP, the satellites can autonomously dock and undock via the metal lance and
opening. The UDP is equipped with a photosensor that determines when the lance
is fully inserted. Then, a motor closes two metal cams around the lance, locking it in

31
 Figure 1-10: Expanded view of SPHERES

place. Moreover, the UDP has a camera and fiducial marker parallel to the lance and
opening to enable autonomous docking maneuvers. Lastly, VERTIGO enables vision-
based navigation and estimation using a 1.2 GHz x86Linux computer and two stereo
cameras. These peripherals enable more science to be conducted with SPHERES but
also change the well-understood inertial properties of the satellites [30].

1.2.1 Status of Testbed

The SPHERES platform on ISS has been operating for 13 years and has almost
performed 110 test sessions. As a consequence of this usage, several thrusters across
the three satellites have become degraded. An analysis and initial attempt at a
workaround are documented in this thesis. It is also hypothesized that the knowledge
of inertial properties of the satellite with an attachment may be inaccurate based on
the poor test results yielded from autonomous docking tests. The inability for the
satellites to dock to one another may be a consequence of both these issues though.
In order to address the possibility of inaccurate knowledge of inertial properties of an

32
aggregated system, a controller resilient to mass properties for position and attitude
of a free-flyer was developed.

1.3 Relative Operations for Autonomous Maneu-

vers
The majority of the research described herein developed from subproblems of the
SSL’s Relative Operations for Autonomous Maneuvers (ROAM) project. Figure 1-
11 illustrates Stages zero through three of a typical in-space close proximity mission
between a Chaser spacecraft and a Target. This research developed from focusing on
Stage 2, the terminal approach to soft landing, and the beginning of Stage 3, joint
maneuvering of the aggregated system.

Figure 1-11: Spatial representation of the stages of ROAM

Stage 2 warrants trajectory generation to facilitate the Chaser’s terminal approach

to the Target’s docking port. This is addressed via some observations of previous tra-
jectory optimization work performed in the lab in addition to the necessary software
framework for implementation with SPHERES. Both Stage 2 and 3 are vulnerable to
uncertainties in either actuator force or torque and inertial properties knowledge of
the individual or aggregated systems. An existing case with SPHERES is analyzed

33
and an adaptive control methodology to overcome such uncertainties is presented.
Lastly, since active debris removal is a type of in-space close proximity missions are
a problem of interest, a fragile Target at risk of breaking apart further is the subject
of the beginning of Stage 3 – detumbling the uncooperative Target. Therefore, in
addition to modeling the docked dynamics of the aggregated system, two different
optimization methods are employed to minimize a cost function of both fuel and
imparted loads and moments via optimizing the angular acceleration profile.

1.4 Research Objective

While facilitating in-space close proximity maneuvers encapsulate a breadth of engi-

neering disciplines, the focus of the research described herein is only concerned with
control, guidance, and autonomy algorithms with respect to specific subproblems.
Several supporting engineering disciplines are applicable with other fields and prob-
lems, as well, whereas the listed topics are discussed specifically with respect to such
proximity maneuvers.
The objective of this thesis is summarized as follows:

• To contribute towards the feasibility of regular and repeatable in-space close

proximity missions where a Chaser spacecraft is interacting with an uncooper-
ative, tumbling Target

• By utilizing adaptive control and optimization to enable in-flight parameter

estimation, trajectory generation, and detumbling procedures

• Using Chaser’s tracking error and information regarding the Target’s pose from
estimation

• While ensuring stability and both minimum fuel and minimum loads and mo-
ments imparted on the Target.

34
1.5 Thesis Roadmap
Six chapters constitute this thesis. Chapter 1 motivated the research documented
herein; provided a literature review of existing autonomous rendezvous and dock-
ing work; and outlined the research objective. Chapter 2 formulated the in-space
close proximity operations problem and gave an overview of the approaches em-
ployed. Chapter 3 detailed observations regarding existing trajectory optimization
research and described the framework for a trajectory propagator. Chapter 4 dis-
cussed an in-flight adaptive proportional-integral-derivative sliding mode controller
and the thruster degradation analysis of the SPHERES aboard the International
Space Station. Chapter 5 described modeling of contact dynamics from dancing, dy-
namics of two docked spacecraft, and the optimization of the detumbling procedure.
Lastly, Chapter 6 summarized the result of the aforementioned chapters and outlined
future work to be completed.

35
36
Chapter 2

Literature Review

2.1 Trajectory Optimization

Aspects of Stage 2, the terminal approach to soft docking, of the ROAM mission has
previously been discussed in the literature, although it remains the topic of ongoing
research. It is desired for such a terminal approach to be optimal and ensure the
safety of both the Chaser and the Target. Often, the terminal approach trajectory
is optimized to minimize fuel, time, thruster impingement, or a combination of these
factors. DiGirolamo proposed an RRT* algorithm as an offline trajectory planner that
minimized fuel and time and avoided thruster plume impingement from the Chaser
on the Target [31]. Jackson developed an offline planner that used an A* node search
to optimize a trajectory that minimized the combined plume and fuel cost for a close
proximity maneuver [32]. Miele et al. investigated several variants of the optimal
trajectory for the terminal approach of two spacecraft including time-optimal and
fuel-optimal [33]. Additionally, Lee described the development and evaluation of
an integrated guidance, navigation, and control system for autonomous proximity
operations and docking from a Chaser to a cooperative Target using linear quadratic
Gaussian controls and a V-bar hopping approach. This V-bar hopping approach had
the Chaser approach the Target from the −V-bar direction, occasionally moving to a
lower altitude orbit in order to speed up, and then shifting back to the same altitude
as the Target to station-keep briefly, ensuring the approach scheme was advancing

37
as expected [34]. These examples of existing research in the literature made use of
offline methods that are too computational expensive to occur in-flight.

Research in the literature has also focused on employing path planning techniques
that can produce trajectories more rapidly. Virgili-Llop et al. presented an onboard
algorithm to capture a tumbling Target via solving a series of convex programming
problems [35]. Munoz developed a rapid path-planning algorithm for the terminal
approach trajectory via an adaptive artificial potential function methodology that
produced a near-optimal trajectory yielding a near-minimum time or fuel cost [36].
Moreover, Zappulla et al. validated a real-time trajectory planner for spacecraft ren-
dezvous via harmonic potential functions and an RRT* algorithm for fuel optimality
[37]. In addition to the aforementioned literature, research in the SSL at MIT has also
focused on developing and validating in-flight optimal trajectory planners. Sternberg
investigated the autonomous docking approach problem that is complicated by an
uncooperative and tumbling Target, uncertainty in the knowledge of the tumble, and
required the Chaser to use an accelerated approach trajectory. Sternberg produced
a computationally efficient solver for determining a fuel-optimal approach trajectory
robust against uncertainty in the Target’s tumble. This solver utilized a synchronous
R-bar (where R-bar is defined as the docking port axis) approach for docking with
the rigid body Target [38]. However, there were some fundamental issues with the
technique Sternberg employed that are discussed at length later in this thesis.

2.2 Nonlinear Control

Driven by the need of application to real-world systems, nonlinear control theory

was applied in the research documented herein. Nonlinear control theory utilizes
rigorous mathematical techniques for nonlinear differential equations (i.e. Lyapunov
stability theory and limit cycle theory). Sliding mode control and adaptive control are
discussed in detail in this section, as they were employed in the research completed.

38
2.2.1 Sliding Mode Control

Sliding mode control is a straightforward technique of robust control that consists

of a nominal feedback controller and additional terms that handle the uncertainty of
the system. Such uncertainties can either be unmodelled dynamics or inaccuracies
in the system order. Sliding mode control allows 𝑛𝑡ℎ -order systems to be replaced
by 1𝑠𝑡 -order problems, which are generally easier to control. Therefore, after trans-
forming such problems, "perfect" tracking performance can be achieved in the face
of uncertainties at the cost of high control effort. This control technique simplifies
(︁ )︁𝑛−1
𝑑
the system dynamics by using the sliding variable 𝑠 = 𝑑𝑡
+𝜆 𝑥˜, where 𝑛 is the
order of the system, 𝑥˜ = 𝑥 − 𝑥𝑑 is the tracking error, and 𝜆 is a user-defined positive
constant [39]. Often, a boundary layer is applied to the sliding mode control scheme
in order to trade some tracking performance to eliminate chattering and lower control
cost [40]. Chapter 4 contains a detailed derivation of a sliding mode controller.
The literature contains several applications of sliding mode control for different
disciplines. A proportional-integral sliding mode control was applied to an active
suspension system and proved to be more robust compared a linear quadratic reg-
ulator [41]. A proportional-integral-derivative sliding mode controller was used to
control the speed of an electromechanical plant and showed superior performance
compared to a conventional PID controller [42]. Time-varying terminal sliding mode
techniques have been employed for attitude control of rigid spacecraft with inertia
uncertainties and external disturbances [43]. These examples represent a sample of
the existing research regarding applying sliding mode control to ensure robustness to
uncertainties.

2.2.2 Adaptive Control

Adaptive control offers a set of control methods for handling uncertainty in either sys-
tem parameters or the control parameters. This uncertainty arises from both a lack
of knowledge and from a system whose parameters vary with time. Therefore, adap-
tive control techniques provide the means for performing online parameter estimation

39
that consequently decreases a controller’s tracking error [44]. A basic adaptive control
setup is displayed in Figure 2-1. Only nondual adaptive control techniques were con-
sidered for the research documented in this thesis. Such control techniques include
model reference adaptive control (MRAC), self-tuning control, gain scheduling, and
adaptive sliding mode.

Figure 2-1: Diagram of a generic adaptive control system configuration from Landau’s
textbook [44]

MRAC systems are composed of a plant with unknown system parameters, a

reference model encompassing the desired output response, a feedback control law
with tunable parameters, and an adaptation law that updates the tunable parameters.
The unique elements of this adaptive control system are the reference model and
adaptation law. The reference model is designed with performance specifications in
mind (e.g. rise time, overshoot, settling time) that the system will be driven towards.
The adaptation law updates the tunable parameters based on the commanded state
to drive the tracking error to zero. The necessary adaptation laws for an MRAC
system fall out of applying Lyapunov stability theory to the closed-loop adaptive
control system [39]. A block diagram of a typical MRAC system is shown in Figure
2-2.

40
 Figure 2-2: Diagram of a model reference adaptive control system

The literature is rich with applications of MRAC systems and discussions of

MRAC’s robustness and stability under varying conditions [45], [46], [47], [48]. For
instance, MRAC was implemented on SPHERES to perform on-board parameter
estimation and yielded improved control on the SPHERES-Halo configuration [49].
Additionally, adaptive PD and PID MRAC controllers have been developed for an
open loop unstable Satellite Launch Vehicle (SLV) system. Both controllers yielded
improved tracking performance during the atmospheric flight stage over their gain-
scheduled counterparts due to their ability to account for unmodelled dynamics [50].

A self-tuning controller, depicted in Figure 2-3, consists of a controller and an

estimator that create a closed-loop with a plant. The estimator updates the un-
known parameters 𝑎
^ for a system that is continuously time-varying. These unknown
parameters may be of the plan (an indirect approach) or of the controller (a direct
approach) [51]. Different choices may be selected for both the controller and the es-
timator. For instance, the most straightforward estimator option would be recursive
least squares. A sample control method is deterministic pole placement but may also
be proportional-derivative [52]. The goal of self-tuning control is to minimize the
data-fitting error in input/output measurements whereas MRAC attempts to drive
the tracking error performance to zero [53]. MRAC and self-tuning control are similar

41
in several respects despite their varying motivations, as illustrated by their respective
block diagrams (Figures 2-2 and 2-3).

Figure 2-3: Diagram of a self-tuning control system

Gain scheduling represents another category of adaptive control. Gain schedul-

ing makes use of auxiliary variables correlated to the plant dynamics. By changing
the controller parameters as a function of the auxiliary variables, gain scheduling
minimizes the effect of variations of those plant parameters [51]. Knowledge of the
dynamics of the plant helps in determining scheduling variables. The key advantage
of gain scheduling is that plant parameters can be changed rapidly in response to
output changes. However, gain scheduling has the drawback of a time-consuming
design process and performing open-loop.

Figure 2-4: Diagram of a gain scheduling control system

42
 The adaptive control scheme employed and documented later in this thesis is
adaptive sliding mode control. Adaptive sliding mode control uses the sliding mode
variable previously described in combination with adaptation laws that fall out of Lya-
punov stability theory. Slotine and Coetsee detail how coupling adaptive control with
sliding mode control offers improved performance using the boundary layer concept to
limit control authority and chattering [54], [55]. Research furthering the robustness of
adaptive sliding mode control utilizes estimating the boundary of perturbations and
applying a low-pass filter in addition to the boundary layer described by Slotine [56].
Technical details of this type of controller and a derivation of necessary adaptation
laws is discussed in Chapter 4.

2.3 Attitude Stabilization under Actuator Loss-of-

Effectiveness

Formation flying has motivated the research to develop methods for persisting through
sensor or actuator failures. Godard and Kumar develop and present two adaptive
fault-tolerant control laws – continuous sliding mode and nonsingular terminal slid-
ing mode – as a software solution to an onboard sensor and actuator failure in order
to maintain a spacecraft clusters ability to fly in formation. They show via sim-
ulation that both methodologies were able to stabilized the formation and execute
maneuvers despite the failures; specifically, the nonsingular terminal sliding mode
had a faster convergence rate, was more effective in minimizing the tracking erros,
and limited fuel consumption compared to the continuous sliding mode [57]. Li et al.
demonstrate the robustness of a second-order sliding mode control algorithm against
sensor and actuator failure but with limited performance [58]. Moreover, Hu and Xiao
apply a sliding mode controller to a flexible spacecraft experiencing actuator loss-of-
effectiveness and demonstrate its ability to recover from the fault and achieve mission
objectives [59]. Henceforth, the existing literature suggests sliding mode control is a
promising method for stabilizing a spacecraft under actuator loss-of-effectiveness.

43
2.4 Soft Docking & Stabilization of a Tumbling
Target

At the end of the terminal approach stage (Stage 2) of ROAM occurs the soft dock,
after which both spacecraft act as a single system. Sternberg’s soft dock stage contin-
ued with the approach along the Target’s R-bar and assumed a mechanical clasping
between the docking ports of the Chaser and the Target [38]. Contrastingly, the
Orbital Express Demonstration System used a robotic arm to capture its stable and
cooperative Target and berth it to the Chaser for servicing [12]. Similarly, the Japan
Aerospace Exploration Agency’s Kounotori Integrated Tether Experiment is a low-
cost active debris removal system that uses an electrodynamic tether to capture an
object of debris and deorbit it [60]. Nguyen-Huynh developed an adaptive algorithm
for a space manipulator during the capture of an unknown, tumbling, rigid object to
create a reactionless motion, thus, perturbing the base of the manipulator minimally
during the transient period between capture and stabilizing the Target [61]. Zhang et
al. utilized an inter-satellite electromagnetic force and proposed a nonlinear 6-DOF
control (via linear quadratic regulator and an extended state observer) coupled with
the method of soft docking [62]. These different methods are appropriate for their
relative missions of capturing and becoming one system with a rigid Target.
Once a Chaser has docked to an uncooperative and tumbling Target, the Chaser
must stabilize the tumble that is occurring. Katz investigated the stabilization of an
uncooperative, flexible Target in 2D using an adaptive control law, which had better
performance than a PD controller and partial feedback linearization controller [63].
Aghili presented a closed-form solution for time-optimal controlling of a tumbling,
rigid Target that satisfied a constraint to maintain the norm of the braking torques
below a specified value correlating to the limits of an actuator’s torque [64]. Avanzini
and Giuletti developed a B-dot control law to detumble a spacecraft solely using mag-
netic actuators and confirmed its validity and convergence via Monte Carlo analysis
[65]. Coverstone-Carroll discussed a variable structure controller used to detumble
underactuated spacecraft, validated through detumbling simulations [66]. Several of

44
these methods have been posed as detumbling means for a spacecraft to detumble
itself, rather than for a Chaser docked to a Target and needing to detumble the ag-
gregated system. The same methodologies can be applied to this scenario. However,
the primary concern would be the limited actuator force available to the Chaser since
the Target is assumed to be uncooperative.

45
46
Chapter 3

Trajectory Optimization

This chapter will focus on the trajectory optimization approach of the in-space close
proximity operations mission (i.e. Stage 2 of ROAM). Stage 2 is the terminal ap-
proach to soft docking that begins when the Chaser is approximately 10 meters from
the Target spacecraft. It occurs after the angles-only rendezvous that gets the Chaser
into the desired proximity of the Target before a terminal approach with an end state
of the Chaser docking to or grappling the Target. An expansion of previous trajec-
tory optimization research was attempted, but that attempt led to some noteworthy
observations of the previous work, documented in this chapter. Additionally, the at-
tempt at the expansion resulted in a software framework for finding the waypoints of
a trajectory by propagating a given differential equation.

3.1 Stage Description

The Chaser approaches the Target along its R-bar, maintaining a synchronous radial
approach to the Target’s docking port. In the context of this chapter, R-bar refers to
the body-fixed, radial axis of the Target’s docking port expressed in the UDP reference
frame. In discussing the dynamics of the Chaser and Target, four reference frames are
used – INT, the inertially-fixed frame; TAR, the Target’s body-fixed frame centered
at its center of mass; UDP, the body-fixed frame located at the docking port’s lance;
and CHA, the body-fixed frame of the Chaser centered at its center of mass. These

47
reference frames are illustrated in Figure 3-1.

Figure 3-1: Reference frames used in discussion of trajectory generation

It is assumed that the Target is tumbling and uncooperative. The Chaser de-
termines the Target’s orientation and rotation rate via a model delivered by the
estimation stage of the mission. Additional assumptions include rigid body dynamics
and that the Chaser has full control authority to translate and rotate. This stage
begins when the Chaser is relatively close to the Target (on the order of magnitude
of 10 meters away) and has zero initial velocity.

3.2 Synchronous Radial Approach via Second Or-

der Differential Equation
Initially, the trajectory optimization subproblem focused on utilizing the contributions
from Sternberg’s Sc.D. thesis. This section will overview Sternberg’s contributions
and hypotheses and the corresponding observations and reasons his work could not
be implemented.
To support his efforts of finding an optimal, minimum-fuel synchronous radial
approach of a Chaser to a Target, Sternberg developed three hypotheses:

1. All possible tumbles are a function of inertia ratios and angular rate components;
therefore, a minimum parametrization exists.

48
 2. A fuel optimal approach is analogous with the tangential and radial acceleration
components of a trajectory being equivalent between initial and final transients.

3. A reduction in optimization complexity falls out as a consequence of this equal-

ity and minimum parametrization.

The first hypothesis is well supported by Sternberg [38]; the second and third hy-
potheses will be discussed in-depth here. Following Sternberg’s optimization process
detailed in his thesis and illustrated by Figure 3-2, the following plots were produced
using a final time array of [1, 20, 33, 55, 240] seconds. The spacecraft has an inertia
ratio of [1, 1, 1] and an angular velocity of [0, 0, 5] deg
s
. The optimization options uti-
lized were: OptimalityTolerance = 1e-3, MaxFunctionEvaluationss = 1e6,
MaxIterations = 1e7, StepTolerance = 1e-8, and Algorithm = 'sqp'. Two
of the primary functions for running this optimization are included in this thesis in
Appendix A.1. Contact the author for access to the numerous supporting functions.

Figure 3-2: Sequence of optimizations and function-fitting that led to a trajectory-

generation differential equation from [38]

49
 Figure 3-3: Δ𝑉 of five trajectory optimizations with fixed final time

Figure 3-3 illustrates the resulting Δ𝑉 of the trajectory optimizations correspond-

ing the value to the fixed final time. The lowest Δ𝑉 is 2.183 ms , yielded from the op-
timization with 𝑡𝑓 = 33 seconds. This figure indicates that the optimal minimum fuel
trajectory exists in the middle of the duration spectrum of final times. Intuitively, it
is reasonable that slower trajectories require more fuel as the Chaser circumnavigates
the Target repeatedly; rapid trajectories burn fuel at a faster rate to meet the desired
final time. According to Sternberg’s second hypothesis, this trajectory’s tangential
and radial acceleration components should be equivalent. At least, these components
should be closer to equivalent to the others when taking into consideration that the
optimization process used here likely is not identical to Sternberg’s. The acceleration
components are labeled and determined as such:

Linear: ⃗𝑎𝑙𝑖𝑛 = ⃗𝑟¨𝑇𝐶𝐻𝐴

𝐴𝑅 (3.1a)
𝑇 𝐴𝑅 𝑇 𝐴𝑅
Centripetal: ⃗𝑎𝑐𝑒𝑛𝑡 = 𝜔
⃗ 𝐼𝑁 𝑇 × (⃗
𝜔𝐼𝑁 𝑟𝑇𝐶𝐻𝐴
𝑇 ×⃗ 𝐴𝑅 ) (3.1b)

⃗˙ 𝐼𝑁
Angular: ⃗𝑎𝑎𝑛𝑔 = 𝜔 𝑇 𝐴𝑅
𝑟𝑇𝐶𝐻𝐴
𝑇 ×⃗ 𝐴𝑅 (3.1c)
𝑇 𝐴𝑅
Coriolis: ⃗𝑎𝑐𝑜𝑟 = 2(⃗𝜔𝐼𝑁 𝑟˙𝑇𝐶𝐻𝐴
𝑇 ×⃗ 𝐴𝑅 ). (3.1d)

The radial acceleration components are the linear and centripetal accelerations and
the tangential acceleration components are angular and Coriolis.

50
 In Figure 3-4, the Euclidean norm of each component as a fraction of the Euclidean
norm of the total acceleration is represented. Since the Target is in a flat spin with
angular velocity [0, 0, 5] deg
s
, the angular acceleration component is zero throughout
⃗˙ 𝐼𝑁
the time duration since 𝜔 𝑇 𝐴𝑅 ⃗
𝑇 (𝑡) = 0. The time histories of the remaining three

acceleration components differ based on the fixed final time inputted in the trajectory
optimization for minimizing Δ𝑉 . The corresponding Δ𝑉 is labeled at the top of each
plot.

(a) 𝑡𝑓 = 1 s (b) 𝑡𝑓 = 20 s (c) 𝑡𝑓 = 33 s

(d) 𝑡𝑓 = 55 s (e) 𝑡𝑓 = 240 s

Figure 3-4: Euclidean norm of acceleration components as fraction of the Euclidean

norm of total acceleration

Figure 3-5 illustrates the sum of the radial acceleration components with respect
to the sum of the tangential acceleration components. According to the second hy-
pothesis, these sums should be equivalent between initial and final transients for the
fuel-optimal trajectory. The lowest Δ𝑉 cost is when 𝑡𝑓 = 33 seconds, corresponding
to Figure 3-6c. Clearly, the radial and tangential accelerations are not equivalent;
however, that may be a consequence of the optimization setup. Thus far, Stern-
berg’s results supporting his second hypothesis have not been able to be reproduced,
which is a matter that requires further investigation to yield any decisive conclusions.

51
Regardless, these plots display a sum of the norms of the acceleration components
rather than a comparison of the vector elements. A comparison of the vector ele-
ments of the acceleration components is a more physically intuitive relationship to
turn to in hopes of deriving any physical relationship between fuel optimality and the
acceleration components.

(a) 𝑡𝑓 = 1 s (b) 𝑡𝑓 = 20 s (c) 𝑡𝑓 = 33 s

(d) 𝑡𝑓 = 55 s (e) 𝑡𝑓 = 240 s

Figure 3-5: Sum of Euclidean norm of radial and tangential acceleration components
as fraction of the Euclidean norm of total acceleration

It is evident in Figure 3-6 that it is impossible for the vector tangential acceleration
to be equivalent to the vector radial acceleration, regardless of if the trajectory is fuel-
optimal or not. For a synchronous radial approach, the Chaser is along the Target’s
R-bar corresponding to its docking port axis. Thus,
⎡ ⎤
¨ ⎥
⎢ 𝑥
⎢ ⎥
⃗𝑎𝑙𝑖𝑛 = ⃗𝑟¨𝑇𝐶𝐻𝐴
𝐴𝑅 = ⎢ ⎥. (3.2)
⎢ 0 ⎥
⎢ ⎥
⎣ ⎦
0

52
The centripetal acceleration for a flat spin is calculated as

𝑇 𝐴𝑅 𝑇 𝐴𝑅
⃗𝑎𝑐𝑒𝑛𝑡 = 𝜔
⃗ 𝐼𝑁 𝑇 × (⃗
𝜔𝐼𝑁 𝑟𝑇𝐶𝐻𝐴
𝑇 ×⃗ 𝐴𝑅 ) (3.3a)
⎛⎡ ⎤ ⎡ ⎤⎞
⎜⎢ 0 ⎥ ⎢ 𝑥 ⎥⎟
⎜⎢ ⎥ ⎢ ⎥⎟
𝑇 𝐴𝑅
⃗𝑎𝑐𝑒𝑛𝑡 = 𝜔
⃗ 𝐼𝑁 𝑇 × ⎜⎢
⎜⎢ 0 ⎥×⎢
⎥ ⎢
0 ⎥
⎥⎟
⎟ (3.3b)
⎜⎢ ⎥ ⎢ ⎥⎟
⎝⎣ ⎦ ⎣ ⎦⎠
𝜔𝑧 0
⎡ ⎤ ⎡ ⎤ ⎡ ⎤
2
⎢ 0 ⎥ ⎢ 0 ⎥ ⎢ −𝜔𝑧 𝑥 ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ 0 ⎥ × ⎢ 𝜔𝑧 𝑥
⃗𝑎𝑐𝑒𝑛𝑡 = ⎢ =⎢ 0 (3.3c)
⎥ ⎢ ⎥ ⎥
⎥ ⎢ ⎥
⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣ ⎦ ⎣ ⎦
𝜔𝑧 0 0

Therefore, the radial acceleration components (linear and centripetal) are only nonzero
in 𝑥. It has been established that the angular acceleration is zero for all time
(⃗𝑎𝑎𝑛𝑔 = ⃗0). The Coriolis acceleration is displayed in the following equations.

𝑇 𝐴𝑅
⃗𝑎𝑐𝑜𝑟 = 2(⃗𝜔𝐼𝑁 𝑟˙𝑇𝐶𝐻𝐴
𝑇 ×⃗ 𝐴𝑅 ) (3.4a)
⎡ ⎤
⎢ 0 ⎥
⎢ ⎥
⃗𝑎𝑐𝑜𝑟 = ⎢
⎢ 𝜔𝑧 𝑥˙
⎥
⎥ (3.4b)
⎢ ⎥
⎣ ⎦
0

Hence, the tangential acceleration is only nonzero in 𝑦. Since the elements of the
vectors are not equivalent, it cannot be deduced that the tangential and radial accel-
eration vectors are equivalent for a fuel-optimal trajectory.

The last set of plots (Figure 3-6) were not presented in Sternberg’s Sc.D. thesis –
the vector representation of the radial and tangential accelerations were not shown.
The plots in Figure 3-6 indicate that the elements of the radial and tangential ac-
celerations are not equivalent for any of the optimizations. In the absence of this
information, Sternberg developed his third hypothesis that the first two hypotheses
lead to a reduction in optimization complexity. The reduction he refers to is a vector
second order differential equation equating the tangential and radial accelerations,
shown in Equation 3.5 (equivalent representation in Equation 3.6). The appealing

53
 (a) 𝑡𝑓 = 1 s (b) 𝑡𝑓 = 20 s (c) 𝑡𝑓 = 33 s

(d) 𝑡𝑓 = 55 s (e) 𝑡𝑓 = 240 s

Figure 3-6: Vector elements of acceleration components for each generated trajectory

nature of this hypothesis was that a second order differential equation is more com-
putationally efficient to solve in-flight than an optimization. Therefore, while the
equation was promising in terms of in-flight feasibility, it is fundamentally incorrect.

(︁ )︁
⃗𝑟¨𝑇𝐶𝐻𝐴
𝐴𝑅 + 𝜔
𝑇 𝐴𝑅
⃗ 𝐼𝑁 𝑇 × 𝜔
𝑇 𝐴𝑅
⃗ 𝐼𝑁 𝑟𝑇𝐶𝐻𝐴
𝑇 ×⃗ ⃗˙ 𝐼𝑁
𝐴𝑅 = 𝜔
𝑇 𝐴𝑅
𝑟𝑇𝐶𝐻𝐴
𝑇 ×⃗ 𝐴𝑅 + 2(⃗
𝑇 𝐴𝑅
𝜔𝐼𝑁 𝑟˙𝑇𝐶𝐻𝐴
𝑇 ×⃗ 𝐴𝑅 ) (3.5)

To further illustrate that this equation does not produce a fuel-optimal, radial
synchronous approach from the Chaser to the Target, a simple simulation was exe-
cuted via MATLAB’s ode45. The right-hand side function is shown in Listing 3.1
with the angular velocity hard-coded since the example is a flat spin case and the
angular acceleration is zero.

54
 ⎡ ⎤
⎢ 0 −𝜔3 𝜔2 ⎥
⎢ ⎥
¨𝐶𝐻𝐴
I3⃗𝑟𝑇 𝐴𝑅 − 2 ⎢
⎢
𝜔3 0
⎥ ˙ 𝐶𝐻𝐴
−𝜔1 ⎥ ⃗𝑟 + ...
⎢ ⎥ 𝑇 𝐴𝑅
⎣ ⎦
−𝜔2 𝜔1 0
⎡ ⎤
⎢ −𝜔32 − 𝜔22 𝜔˙ 3 + 𝜔1 𝜔2 −𝜔˙ 2 + 𝜔1 𝜔3 ⎥
⎢ ⎥
⎢
−𝜔˙ 3 + 𝜔1 𝜔2 −𝜔12 − 𝜔32
⎥ 𝐶𝐻𝐴
𝜔˙ 1 + 𝜔2 𝜔3 ⎥ ⃗𝑟 = ⃗0 (3.6)
⎥ 𝑇 𝐴𝑅
⎢
⎢
⎣ ⎦
𝜔˙ 2 + 𝜔1 𝜔3 −𝜔˙ 1 + 𝜔2 𝜔3 −𝜔12 − 𝜔22

Listing 3.1: Right-hand side function for call to ode45 to demonstrate the second-
order differential equation
1 function zdot = RHS(t,z)
2 omega_TAR_INT=deg2rad( 5/norm([0 0 1])*[0 0 1]');
3 w1 = omega_TAR_INT(1); w2 = omega_TAR_INT(2); w3 = omega_TAR_INT(3);
4 w1dot = 0; w2dot = 0; w3dot = 0;
5

6 w_skew = [0 -w3 w2 ; w3 0 -w1 ; -w2 w1 0];

7 wdot_skew = [0 -w3dot w2dot ; w3dot 0 -w1dot ; -w2dot w1dot 0];
8

9 B = 2*w_skew;
10 C = (wdot_skew - w_skew*w_skew);
11

12 A = [zeros(3), eye(3);
13 C B];
14

15 zdot = A*z;

The initial translational position was [10, 0, 0] and the initial translational velocity
was [0, 0, 0]. The simulation was run for 100 seconds and produced the following
trajectory in Figure 3-7. The trajectory spirals away from the Target at (0, 0). This
example shows that Equation 3.5 does not produce a synchronous radial approach
for a Chaser with respect to a tumbling Target.

55
 Figure 3-7: Chaser’s trajectory from call to ode45 using Listing 3.1

3.3 Differential Equation Propagator Architecture

A supporting propagator architecture was developed to yield the waypoints of the

Chaser’s trajectory to a tumbling Target along the Target’s R-bar provided by solving
a given differential equation. This architecture was motivated by Sternberg’s differ-
ential equation. Although Equation 3.5 has been proved to be invalid, the propagator
architecture is generalizable to any differential equation that results in a synchronous
radial approach trajectory. The propagator architecture is documented in this section
and was developed to run in the SPHERES simulation. The propagator architecture
consists of five sequential steps and outputs the waypoints the Chaser needs to follow.
The following subsections elaborate on each of the five steps. In short, the propagator
architecture (1) determines when the Chaser should hop along the Target’s R-bar,
(2) creates future knowledge of the Target’s rotation in the absence of estimation

56
software, (3) determines the Chaser’s final position of the end of the approach, (4)
backwards propagates the differential equation from the final position, and then (5)
radially shifts the trajectory of waypoints such that the initial position aligns with
the Chaser’s current position. This all occurs before the Chaser aligns itself with the
Target’s R-bar and begins the approach. The propagator architecture is encompassed
in the file propagator.c, shown in Appendix A.3.

3.3.1 R-Bar Alignment

Figure 3-8 illustrates the necessary initial conditions for starting the propagator: (1)
the Chaser is at the desired radial length from the Target, and (2) the Chaser is along
the Target’s R-bar. If provided with a model of the Target’s rotation, the propagator
can use this information for identifying intersections of the Target’s R-bar with the
Chaser at the desired radial length. Alternatively, if the Chaser is not approaching in
the correct plane, this step also identifies the future intersection location that requires
minimal state change for the Chaser.
Provided with a model of the Target’s rotation based on the Chaser’s estimation
capabilities, the Chaser can create a future history of the Target’s rotation (Figure
3-9). This future history is multiplied by 180 deg about the Target’s docking axis and
is compared with the Chaser’s static quaternion. When the difference between these
two quaternions is zero, the Chaser will be aligned with the Target’s R-bar for the
associated points in time. For example, if the Target’s docking port is on its x-axis,
the propagator finds the quaternion rotated about the z-axis via Equation 3.7.

*,𝐶𝐻𝐴 𝑇 𝐴𝑅
𝑞𝐼𝑁 𝑇 = 𝑞𝐼𝑁 𝑇 ⊗ [0, 0, −1, 0] (3.7)

*,𝐶𝐻𝐴 0,𝐶𝐻𝐴
The times associated with the minimum of 𝑞𝐼𝑁 𝑇 − 𝑞𝐼𝑁 𝑇 indicate the future
intersection times – these are the times in which the Chaser should start moving
synchronously with the Target’s R-bar. The times associated with the intersection
locations are graphically illustrated in Figure 3-9. In the current implementation of
the propagator architecture, the propagator develops a future history rotation model

57
Figure 3-8: Illustrating the necessary initial conditions for the propagator i.e. the
Chaser is at the desired radius and the Chaser is along the Target’s R-bar

Figure 3-9: Timeline illustrating the Chaser’s intersections with the Target’s R-bar
at the desired initial radial length away from the Target

of the Target in the absence of the estimation software. Therefore, instead of com-
paring the Chaser’s quaternion with each future Target quaternion, the propagator
⃒ ⃒
*,𝐶𝐻𝐴 *,𝐶𝐻𝐴 0,𝐶𝐻𝐴 ⃒
identifies the value of 𝑞𝐼𝑁 𝑇 and checks at 1 Hz if ⃒⃒𝑞𝐼𝑁 𝑇 − 𝑞𝐼𝑁 𝑇 ⃒ < 𝛿, where 𝛿 is

a user-defined margin. The next step starts once this is satisfied.

58
3.3.2 Forward Propagation

For the full in-space close proximity operations mission, the estimation portion of the
software infrastructure will work in-the-loop with this trajectory generator. Based
on its observations, it will develop a rotation model of the Target that includes the
expected future 𝜔 ⃗˙ , 𝑞, 𝑞.
⃗,𝜔 ˙ In the absence of that rotation model, the propagator
currently creates its own forward propagation of the Target (up to three periods)
based on the Target’s current rotation rate. This forward propagation is graphically
depicted by Figure 3-10.

Figure 3-10: Forward propagate Target’s orientation and rotation rate for 𝑛 periods

The future estimates of the Target’s rotation are found by numerically solving the
following equations for up to three periods of rotation.

⃗˙ 𝐼𝑁
𝜔 𝑇 𝐴𝑅
𝑇 = 𝐼
−1 𝑇 𝐴𝑅
(𝑀 − (⃗𝜔𝐼𝑁 𝑇 × 𝐼⃗
𝑇 𝐴𝑅
𝜔𝐼𝑁 𝑇 )) (3.8a)
⎛⎡ ⎤ ⎞
𝑇 𝐴𝑅
𝑇 𝐴𝑅 1 ⎜⎢ 𝜔
⃗ 𝐼𝑁 𝑇 ⎥ 𝑇 𝐴𝑅 ⎟
𝑞˙𝐼𝑁 𝑇 = ⎝⎣ ⎦ ⊗ 𝑞𝐼𝑁 𝑇 ⎠ (3.8b)
2 0

𝑇 𝐴𝑅 𝑇 𝐴𝑅
For each time step, Euler integration solves the associated 𝜔
⃗ 𝐼𝑁 𝑇 and 𝑞𝐼𝑁 𝑇 values. At

the end of this step in the propagator architecture, there are three periods worth of
𝑇 𝐴𝑅 ˙ 𝑇 𝐴𝑅 𝑇 𝐴𝑅 𝑇 𝐴𝑅
future estimates of 𝜔
⃗ 𝐼𝑁 𝑇, 𝜔
⃗ 𝐼𝑁 𝑇 , 𝑞𝐼𝑁 𝑇 , and 𝑞˙𝐼𝑁 𝑇 .

3.3.3 Determine Final Position

If the Target has a complex tumble, it may take more than one period of rotation for
the Chaser to be able to dock to it. However, for flat spins, the Chaser should be able
to dock to the Target in less than one period of rotation. Based on this information,

59
a desired final time (𝑡𝑓 ) is selected. The corresponding final Target quaternion is
𝑇 𝐴𝑅
𝑞𝐼𝑁 𝑇 (𝑡𝑓 ). Therefore, the corresponding final position of the Chaser is determined by

𝐶𝐻𝐴 𝑇 𝐴𝑅
⃗𝑟𝐼𝑁 𝑇 (𝑡𝑓 ) = 𝐷𝐶𝑀 (𝑞𝐼𝑁 𝑇 (𝑡𝑓 ))⃗
𝑟𝑠𝑡𝑎𝑛𝑑𝑜𝑓 𝑓 (3.9)

where ⃗𝑟𝑠𝑡𝑎𝑛𝑑𝑜𝑓 𝑓 is the user-defined desired position away from the Target at 𝑡𝑓 . The
variable 𝐷𝐶𝑀 denotes the operation that yields a direction cosine matrix provided
a quaternion. The relative position of the Chaser to the Target is

⃗𝑟𝑇𝐶𝐻𝐴 𝑇 𝐴𝑅
𝐴𝑅 (𝑡𝑓 ) = 𝑞𝐼𝑁 𝑇 (𝑡𝑓 ) ⊗ ⃗
𝐶𝐻𝐴
𝑟𝐼𝑁 𝑇 (𝑡𝑓 ) (3.10)

The final position of the Chaser relative to the Target is a necessary boundary con-
dition for the following step.

3.3.4 Backwards Propagation

Now that the necessary boundary conditions have been determined, ⃗𝑟𝑇𝐶𝐻𝐴 𝑟𝑇𝐶𝐻𝐴
𝐴𝑅 (0), ⃗ 𝐴𝑅 (𝑡𝑓 ),

⃗𝑟˙𝑇𝐶𝐻𝐴
𝐴𝑅 (𝑡𝑓 ) = 0, the backwards propagation – the part of the propagator software that

generates the trajectory – is ready to be utilized. This part of the software is currently
implemented as a C-function named propagateToTarget and takes eight arrays as
inputs. These inputs are described in Table 3.1; the "to-be-trimmed" arrays are short-
ened to match the filled-in length of the future Target histories since all arrays were
initialized with a length of 2000.
After trimming the quaternion and denoting the index corresponding to the filled-
in length, the function uses ctrlState and tgtState to compute the Chaser’s position
relative to the Target at the current time step. The expected final Target quaternion is
converted to a direction cosine matrix and is multiplied by the desired radial standoff
of the Chaser during the last time step. The resulting array is the desired ⃗𝑟𝑓 of the
Chaser with respect to the inertial frame.
Having denoted the desired final position and the initial position, the function
enters into a while loop that it can only exit once the radial position is greater

60
 Table 3.1: Inputs to propagateToTarget function with descriptions

Input Name Description

ctrlState 13-element state array of Chaser
tgtState 13-element state array of Target
To-be-trimmed 2000 by 3 array of
radial_profile
radial positions along the trajectory
To-be-trimmed 2000 by 3 array of
velocity_profile
velocity commands along the trajectory
To-be-trimmed 2000 by 4 array of Target’s
q_TAR_INT_array
future quaternions from forward propagation
To-be-trimmed 2000 by 4 array of Target’s
omega_TAR_INT_array
future angular velocity from forward propagation
To-be-trimmed 2000 by 4 array of Target’s
q_dot_TAR_INT_array
future quaternion derivatives from forward propagation
To-be-trimmed 2000 by 4 array of Target’s
omega_dot_TAR_INT_array
future angular acceleration from forward propagation

than 𝑟0 (the initial radial distance between the Target and Chaser) or the counter of
iterations has exceeded the user-defined maximum. The procedure that occurs in the
while loop is

1. Update local variables associated with the Target’s quaternion, quaternion

derivative, angular velocity, and angular acceleration using the arrays computed
from forward propagation.

2. The skew symmetric matrices for the Target’s angular velocity and angular
acceleration are constructed.

3. The A, B, and C matrices from Equation 3.5 are constructed as

𝐵 = 2𝜔× (3.11a)

𝐶 = 𝜔˙ × − 𝜔× 𝜔× (3.11b)
⎡ ⎤
0 I ⎥
𝐴=⎢
⎣ ⎦ (3.11c)
𝐶 𝐵

where the subscript × indicates the skew-symmetric matrix.

61
 4. The state vector ⃗𝑥 is populated using the current iteration of the Chaser’s posi-
[︁ ]︁ 𝑇
𝐶𝐻𝐴 ˙ 𝐶𝐻𝐴
tion and velocity with respect to the inertial frame, such that ⃗𝑥 = ⃗𝑟𝐼𝑁 𝑟𝐼𝑁 𝑇 .
𝑇 ,⃗

5. The computation ⃗𝑥˙ = 𝐴⃗𝑥 occurs to populate ⃗𝑥˙ . Euler integration then creates
𝐶𝐻𝐴
an updated ⃗𝑟𝐼𝑁 𝑟˙𝐼𝑁
𝑇 and ⃗
𝐶𝐻𝐴
𝑇 .

6. To find the corresponding ⃗𝑟𝑇𝐶𝐻𝐴 𝑟˙𝑇𝐶𝐻𝐴

𝐴𝑅 and ⃗
𝑇 𝐴𝑅
𝐴𝑅 , the quaternion conjugate of 𝑞𝐼𝑁 𝑇 is

determined and used:

𝑟𝑇𝐶𝐻𝐴 𝐼𝑁 𝑇 𝐶𝐻𝐴
𝐴𝑅 = 𝑞𝑇 𝐴𝑅 ⊗ 𝑟𝐼𝑁 𝑇 (3.12a)

𝑟˙𝑇𝐶𝐻𝐴 𝐼𝑁 𝑇 𝐶𝐻𝐴
𝐴𝑅 = 𝑞𝑇 𝐴𝑅 ⊗ 𝑟˙𝐼𝑁 𝑇 (3.12b)

7. The updated ⃗𝑟𝑇𝐶𝐻𝐴 𝑟˙𝑇𝐶𝐻𝐴

𝐴𝑅 and ⃗ 𝐴𝑅 values are stored in the variables radial_profile

and velocity_profile to be implemented as commands for the Chaser.

3.3.5 Radial Shift

An essential point to consider in implementing the propagator architecture is the

choice of the "final states" of the Target based on the future time history constructed
in the forward propagation step. For simple rotations (i.e. flat spin), this choice
does not matter. However, for complex rotations, this choice becomes more selective
in order to maintain minimal fuel cost (an idea left for future work). For simple
rotations, the propagator picks the final state from forward propagation. However,
backwards propagating from this state will not necessarily yield a radial approach that
aligns with the Chaser’s current position (see Figure 3-11). A radial shift must occur
in order to align the trajectory’s initial position with the Chaser’s current position.
The propagator’s initial position and the Chaser’s initial position are vectors in
the inertial frame. The following procedure occurs within the propagateToTarget
function to find the rotation matrix. Let the propagator’s position vector be denoted
as 𝐴 and the Chaser’s current position vector be denoted as 𝐵.

62
Figure 3-11: Generated trajectory yielded from 𝑡𝑓 from forward propagation prior to
radial shift to align initial position with Chaser’s current position

1. Find the unit vector of 𝐴 and 𝐵.

𝐴
𝐴^ = (3.13a)
||𝐴||
^= 𝐵
𝐵 (3.13b)
||𝐵||

2. Calculate the dot product and the magnitude of the cross product of the unit
vectors.

𝐴^ · 𝐵
^ = 𝑎1 𝑏 1 + 𝑎2 𝑏 2 + 𝑎3 𝑏 3 (3.14a)
⎡ ⎤
⎢ 𝑎2 𝑏 3 − 𝑎3 𝑏 2 ⎥
⎢ ⎥
𝐴^ × 𝐵
^= 𝑎3 𝑏 1 − 𝑎1 𝑏 3 ⎥ (3.14b)
⎢ ⎥
⎢
⎢ ⎥
⎣ ⎦
𝑎1 𝑏 2 − 𝑎2 𝑏 1
√︁
||𝐴^ × 𝐵||
^ = (𝑎2 𝑏3 − 𝑎3 𝑏2 )2 + (𝑎3 𝑏1 − 𝑎1 𝑏3 )2 + (𝑎1 𝑏2 − 𝑎2 𝑏1 )2 (3.14c)

63
 3. Construct the matrix 𝐺.
⎡ ⎤
⎢ 𝐴^ · 𝐵
^
−||𝐴^ × 𝐵||
^ 0 ⎥
⎢ ⎥
𝐺=
⎢
⎢ ^ ^
||𝐴 × 𝐵|| ^
𝐴·𝐵 ^ 0 ⎥
⎥
(3.15)
⎢ ⎥
⎣ ⎦
0 0 1

4. Construct the basis change matrix 𝐹 .

[︂ ]︂−1
^ 𝐴·^ 𝐵)
^ 𝐴^
𝐹 = 𝐴^ 𝐵−(
^ 𝐴·
||𝐵−( ^ 𝐵)
^ 𝐴||
^ 𝐴^ × 𝐵
^ (3.16)

5. Calculate the rotation matrix 𝑈 = 𝐹 −1 𝐺𝐹 .

6. Find 𝐴* = 𝑈 𝐴, where 𝐴* is the updated trajectory position (𝑟𝐼𝑁

𝐶𝐻𝐴 *
𝑇 (𝑡) =
𝐶𝐻𝐴
𝑈 𝑟𝐼𝑁 𝑇 (𝑡)).

Figure 3-12: Generated trajectory after radial shift with initial position aligned with
Chaser’s current position

This is repeated for each position in the radial profile array and is the final output
for the propagateToTarget function. Figure 3-12 illustrates the radially-shifted
trajectory. Figure 3-13 depicts what this radial shift means in the scheme of the

64
timeline of the synchronous radial approach. This radially-shifted trajectory is used
as a series of commanded positions for the Chaser to follow along with the velocity
profile that commands the corresponding velocities at each position.

Figure 3-13: Illustration of timeline shift after shifting the radial approach to align
with the Chaser’s current position

65
66
Chapter 4

Adaptive Control for Thrust and

Inertia Uncertainties

The motivation for the topics discussed in this chapter is the inability for the SPHERES
to dock to one another with the UDPs attached aboard the ISS. Moreover, this is-
sue is relevant to the ROAM project as a practical application to one of the three
reference missions – active debris removal. Realistically, an effective active debris
removal effort will involve interacting with unknown Targets or Targets whose well-
characterized properties will have changed due to collisions with other debris. This
chapter first discusses the test design, results, and analysis of thruster characteriza-
tion for the SPHERES free-flyers on the ISS. Second, this chapter provides a detailed
description of an adaptive controller robust to uncertainty in knowledge of inertial
properties.

4.1 Thruster Degradation Characterization

The addition of the UDP as a peripheral to the SPHERES program inspired hopes of
demonstrating the ability for the SPHERES to dock and undock without astronaut
involvement. However, the SPHERES team has been unable to demonstrate this
capability since checking out the UDPs on station in Test Session 78 in September
2015. The SPHERES team has been investigating this matter and there are two

67
favored possibilities for what is causing the issue: 1) the inertial properties of the
aggregated system are incorrect and/or 2) the thrusters have degraded significantly
enough that it is causing nontrivial performance issues. This section will focus on the
investigation into possible thruster degradation on the satellites on station.

4.1.1 Thruster Characterization in Test Session 97

The thruster dance – a series of commanded thrusts used to characterize the cur-
rent performance – was conducted as Test 18 on Test Session 97 (TS97) using the
SPHERES platform on the ISS. The thruster dance consisted of pulsing each thruster
individually and then the pairs that create torques for three time segments: 50 ms,
100 ms, and 200 ms. As mentioned previously, each satellite has 12 thrusters and are
indexed from 0 to 11. The location of each thruster is indicated by its index on Figure
4-1. The thruster force and torque directions (and torque couplings) are shown in
Table 4.1.

Figure 4-1: Location and indices of thrusters on satellite with noted exhaust direction

The raw data from this test is shown in the following Figures 4-2a and 4-2b. To
better understand and gauge if each thruster is producing a similar level of thrust,
the raw accelerometer data was separated by axis and by the duration of the thrust
impulse.
Figures 4-3a and 4-3b are a series of segregated data plots, separated by satellite,
by direction, and by impulse series. The associated thruster with a commanded

68
 Table 4.1: Thruster force and torque directions in SPHERES body-fixed frame

Thruster Force Direction Torque Direction

Index x y z x y z
0 1 0 0 0 1 0
1 1 0 0 0 -1 0
2 0 1 0 0 0 1
3 0 1 0 0 0 -1
4 0 0 1 1 0 0
5 0 0 1 -1 0 0
6 -1 0 0 0 -1 0
7 -1 0 0 0 1 0
8 0 -1 0 0 0 1
9 0 -1 0 0 0 -1
10 0 0 -1 -1 0 0
11 0 0 -1 1 0 0

Raw IMU data for Sphere logical ID 1 Raw IMU data for Sphere logical ID 1
0.1 0.1
Accelerometers [m s -2 ]

Accelerometers [m s -2 ]

x x
y y
0.05 z
0.05 z

0 0

-0.05 -0.05

-0.1 -0.1
10 20 30 40 50 60 70 10 20 30 40 50 60 70

0.2 0.2
Gyroscopes [rad/s]

Gyroscopes [rad/s]

0.1 0.1

0 0

x x
-0.1 y -0.1 y
z z

-0.2 -0.2
10 20 30 40 50 60 70 10 20 30 40 50 60 70
Test time, s Test time, s

(a) SPH1 (Blue) (b) SPH2 (Orange)

Figure 4-2: Thruster characterization: raw accelerometer and gyroscope data from
IMU in test session 97

impulse is denoted on each plot by its thruster index (described in Table 4.1). There
are significant spikes in data associated with another thruster’s impulse – this is a
consequence of the impulse ringing the system and can effectively be ignored.
The 200 ms accelerometer data provides the cleanest impulse image; the following
deductions are based off of the 200 ms plots (Figures 4-3a and 4-3b). Qualitatively,
half of the thrusters produce a similar impulse in both Blue and Orange. Orange’s
thrusters 0, 6, and 7 produce an impulse larger than their counterparts on Blue; Blue’s

69
 Z-Accelerometers [m s -2 ]Y-Accelerometers [m s -2 ] X-Accelerometers [m s -2 ]

Z-Accelerometers [m s -2 ]Y-Accelerometers [m s -2 ] X-Accelerometers [m s -2 ]

Raw IMU data for Sphere logical ID 1 Raw IMU data for Sphere logical ID 2
0.05 0 1 0.05 0 1

0 0

-0.05 6 7 -0.05 6 7
38 40 42 44 46 48 38 40 42 44 46 48

0.05 2 3 0.05 2 3

0 0

-0.05 8 9 -0.05 8 9
38 40 42 44 46 48 38 40 42 44 46 48

0.05 4 5 0.05 4 5

0 0

-0.05 10 11 -0.05 10 11
38 40 42 44 46 48 38 40 42 44 46 48
Test time, s Test time, s

(a) SPH1 (Blue) (b) SPH1 (Orange)

Figure 4-3: Thruster characterization: 200 ms impulse accelerometer data separated

by axis

thrusters 3, 4, and 9 produce an impulse larger than their counterparts on Orange.

Based on these results, it was concluded that the thrust levels varied between thrusters
and more characterization tests were required to more accurately determine the force
per thruster.

4.1.2 Thruster Force Characterization

In order to characterize each thruster’s force, existing MATLAB functions developed

by the SPHERES team originally used for characterizing the inertial properties of
SPHERES satellites with attached peripherals were modified to output the measured
force of each thruster of a stand-alone satellite.

Methodology

Three MATLAB functions were used to characterize thruster force: pulse_extraction.m,

process_pulses_sph.m, and plot_pulses.m. Additionally, a function revise_pulses.m
was created to allow the user to re-do the processing function for specific pulses rather
than the whole set. These functions can be found in Appendix B.2.
Within pulse_extraction.m, the raw IMU data from Test 18 in TS97 was
extracted from its .mat file and placed in various data arrays and organized by

70
its satellite number. The accelerometer data was interpolated to match the length
of the gyroscope data and both accelerometer and gyroscope data were smoothed.
The thruster commands were extracted from the test data, as well, – the thruster
commands are a series of on and off commands to each thruster. Based on this
information, time history vectors of when the thrusters were commanded on were
constructed for each satellite. The function then determines the maximum pulse
length across all the thrusters to use for extracting each pulse. It does so by identifying
six stages of the pulse: pre-pulse start, pre-pulse end, during pulse start, during pulse
end, post-pulse start, and post-pulse end. The user is then presented with a series of
plots of the accelerometer data and is asked to select the start and end of the peak of
the impulse (the pulse period). Contrastingly, the function uses the gyroscope data
without user input.
The function pulse_extraction.m determines acceleration, angular accelera-
tion, angular velocity, and change in angular velocity. The average of the accelerom-
eter data in the user-selected pulse period is the acceleration (⃗𝑎𝑚𝑒𝑎𝑠 ). The angular
acceleration (⃗
𝛼) is determined as the difference between the final gyroscope data and
the initial gyroscope data divided by the time period during the user-selected pulse
period. The angular velocity (⃗𝜔 ) is simply the average of the gyroscope data, and the
change in the angular velocity (Δ⃗𝜔 ) is the average of the difference of gyroscope data
before the pulse and after. This information is stored in a struct for each pulse for
each thruster and outputted as a .mat file for each satellite.
The function process_pulses_sph.m takes in the pulse .mat files that were
created in pulse_extraction.m and outputs the force of each thrusters for each
satellite. In order to find the force 𝐹𝑡ℎ𝑟𝑢𝑠𝑡 = 𝑚 *⃗𝑎, ⃗𝑎 must be computed. The function
iterates through each pulse and separates this problem into solving for the acceleration
in each direction. Thus, for each pulse (𝑝), the following is computed (using only the
𝑥-direction in this example):

𝑏𝑥 (𝑝) = 𝑎𝑚𝑒𝑎𝑠,𝑥 − (𝛼× + 𝜔 × 𝜔 × )(1,:)⃗𝑟𝑔𝑐,𝑥 (4.1a)

𝐴𝑥 (𝑝, :) = 𝐴𝑡ℎ𝑟𝑢𝑠𝑡 (1, [1, 2, 7, 8]) (4.1b)

71
where row 1 of 𝐴𝑥 (𝑝, :) corresponds to the 𝑥-direction and the columns correspond to
thrusters acting in 𝑥, 𝐴𝑡ℎ𝑟𝑢𝑠𝑡 ∈ R3×12 with a 1 corresponding to the thruster direction
and zeros otherwise, 𝛼× is the skew symmetric matrix of 𝛼
⃗ 𝑝 from pulse_extraction.m,
𝜔 × is the skew symmetric matrix of Δ⃗𝜔
2 𝑝
from pulse_extraction.m, and ⃗𝑟𝑔𝑐,𝑥 is the
position of the 𝑥-axis accelerometer in SPHERES. Once each pulse is looped through,
there are three completed matrices (𝐴𝑥 , 𝐴𝑦 , 𝐴𝑧 ∈ R12×4 ) and three completed vectors
(𝑏𝑥 , 𝑏𝑦 , 𝑏𝑧 ∈ R12×1 ). The calculated acceleration is found via

𝑥¨ = 𝐴−1
𝑥 𝑏𝑥 ∈ R
4×1
(4.2)

where the first two elements correspond to the thruster’s imparted force and the
second two elements correspond to the thruster’s imparted torque. This calculation
occurs for each direction and the first two elements are extracted into a vector of
forces associated with the corresponding thruster indices. Additionally, 𝐴𝑥 , 𝐴𝑦 , 𝐴𝑧 ,
𝑏𝑥 , 𝑏𝑦 , 𝑏𝑧 , 𝑎𝑚𝑒𝑎𝑠 , 𝛼𝑚𝑒𝑎𝑠 , and 𝜔𝑚𝑒𝑎𝑠 are placed in a struct and are outputs of this
function.
Lastly, the function plot_pulses.m takes the results struct created in
process_pulses_sph.m and plots the thruster force for each satellite according to
thruster index. If the user has low confidence in the result for a thruster or for a
handful but does not wish to re-do the entire extraction process, they can utilize
revise_pulses.m and re-extract and re-process a selected number of pulses.

Results

Using the aforementioned MATLAB functions and test data from Test 18 in TS97,
Figure 4-4 was produced. It is evident that each satellite has thrusters that are per-
forming poorly. Each computed thruster force and the ratio of computed to expected
is shown in Table 4.2. (Note: the expected thruster force is 0.098 N.) Ratios of less
than 0.5 are highlighted in red on the table and ratios between 0.5 and 0.75 are high-
lighted in yellow. Of note are the thrusters whose performance is denoted by the
red.

72
 Thruster Force
0.1

0.09

0.08

0.07

0.06
Thrust [N]

0.05

0.04

0.03

0.02

0.01
Blue
Orange
0
0 1 2 3 4 5 6 7 8 9 10 11
Thruster Number

Figure 4-4: Calculated force magnitude of each thruster on Blue and Orange satellites

For Blue, this means that two +𝑦 torques, two −𝑥 forces, one +𝑥 force, and one
−𝑦 torque are performing poorly. For Orange, this indicates that one −𝑧 torque, one
+𝑧 torque, one −𝑦 force, and one +𝑦 force are under-performing. These results are
concerning since they imply that the satellites have an insignificant amount of control
authority in the noted directions. Based on these conclusions, a maintenance session
was designed to perform a thruster characterization test for Blue, Orange, and Red in
addition to testing new mixers (via a handful of translational and attitude sequences)
the SPHERES team designed based on knowledge of thruster characterization before
the session.

4.1.3 Maintenance Session

Three types of tests were designed and run during the maintenance session: open-loop
thruster characterizations, closed-loop translational and orientation maneuvers with
new mixers, and a thruster manifold test. For this test session, three satellites were
used (Blue, Orange, Red). The goals of the maintenance session were threefold:

(1) to produce a secondary thruster characterization for Blue and Orange and a

73
Table 4.2: Calculated thruster forces for each satellite and ratios of calculated thruster
force divided by expected force for each satellite

Force Torque Blue: Blue: Orange: Orange:

Thr. #
Direction Direction Force [N] Ratio Force [N] Ratio
0 +x +y 0.020 0.204 0.0851 0.868
1 +x -y 0.0853 0.870 0.0903 0.921
2 +y +z 0.0794 0.810 0.0681 0.695
3 +y -z 0.0873 0.891 0.0089 0.091
4 +z +x 0.0993 1.1013 0.0907 0.925
5 +z -x 0.0784 0.800 0.0766 0.782
6 -x -y 0.0223 0.228 0.0709 0.723
7 -x +y 0.0402 0.410 0.0795 0.811
8 -y -z 0.0759 0.774 0.0711 0.725
9 -y +z 0.0946 0.965 0.0150 0.153
10 -z -x 0.0935 0.954 0.0895 0.913
11 -z +x 0.0632 0.645 0.0563 0.574

primary one for Red since it had not been utilized in several years

(2) to evaluate the performance of new mixers designed for each satellite based on
previous thruster characterization

(3) to evaluate if the thruster manifolds in each satellite were behaving as expected.

In this section, the first two points are discussed but no analysis has occurred for
the thruster manifold test at this time. This maintenance session occurred as Test
Session 101 (TS101) in June 2018 and consisted of 16 tests, described in Table 4.3.

Thruster Characterization

Tests 1-9 were simply thruster characterization tests split into the separate planes and
for three different lengths of time: 50 ms, 100 ms, and 200 ms. The SPHERES team
performed the analysis on the resulting sets of data using the functions described
in Section 4.1.2. This analysis culminated in Figure 4-5, which shows the measured
thruster force per thruster per satellite and references the nominal force as 0.088 N (an
already degraded value from the thruster’s datasheet value of 0.098 N). Illustratively,
it is clear that each satellite suffers from at least one seriously degraded thruster. For
Red, thruster 0 outputted 0.002 N, meaning its ability to thrust in the +𝑥 direction

74
 Table 4.3: Test session 101 descriptive list of tests executed

Test # Description
1 Open-Loop X-Y Plane (200 ms)
2 Open-Loop X-Z Plane (200 ms)
3 Open-Loop Y-Z Plane (200 ms)
4 Open-Loop X-Y Plane (100 ms)
5 Open-Loop X-Z Plane (100 ms)
6 Open-Loop Y-Z Plane (100 ms)
7 Open-Loop X-Y Plane (50 ms)
8 Open-Loop X-Z Plane (50 ms)
9 Open-Loop Y-Z Plane (50 ms)
10 Thruster Manifold Test
11 Closed-Loop X Translate
12 Closed-Loop Y Translate
13 Closed-Loop Z Translate
14 Closed-Loop X Rotate
15 Closed-Loop Y Rotate
16 Closed-Loop Z Rotate

and torque about +𝑦 is severely crippled. For Orange, thruster 3 outputted 0.008 N
and thruster 9 outputted 0.014 N – its ability to thrust in +𝑦 and −𝑦 is limited and
is crippled for torquing about −𝑧. For Blue, thruster 0 outputted 0.023 N, thruster 6
outputted 0.015 N, and thruster 7 outputted 0.032 N, meaning it has limited ability
to thrust in +𝑥 and −𝑥 and is at a disadvantage for torquing about −𝑦 and +𝑦.
These conclusions are based on the force and torque directions outlined in Table 4.2.

Figure 4-5: Test session 101: calculated force of each thruster and each satellite

75
Mixer Validation

Based on the thruster characterization results prior to this test session, the SPHERES
team developed an individual mixer for Blue, Orange, and Red. Similar to the pre-
vious standard mixer, these individualized mixers took in the desired control effort
from the controller and outputted the necessary thruster on/off commands. However,
the team modified each one to consider the limited performance of certain thrusters
on the different satellites, thereby changing the force matrix for each one. Hypothet-
ically, this would help in achieving the desired position or orientation. In order to
test these mixers, a series of closed-loop tests for each translation and orientation
direction were completed in TS101.

Each closed-loop test consisted of four maneuvers. For all tests, maneuver 1 was
10 seconds allotted for estimator convergence and the satellite’s initial position was
copied to memory as the position to return to in maneuver 4. For all tests, maneuver
2 commanded each satellite to hold its initial position and orientation. Maneuver 3
varied for each test as it consisted of the translational or reorientation command. For
the translation tests, maneuver 3 commanded each satellite to translate by +0.3 m in
the respective direction of the test. For the rotation tests, maneuver 3 commanded
each satellite to rotate +90 deg about the respective rotation axis. For all tests,
maneuver 4 commanded each satellite back to its original position and orientation.

The following portion of this section will discuss the results of each closed-loop
test in turn. First, two instances of the closed-loop translation in 𝑥 were run, both
of which yielded poor results. The best results for each satellite are presented in
the following figures. From Figure 4-6a, it is shown that Blue did not reach the
commanded 𝑥-position in the allotted time. By the end of maneuver 3, it should have
moved +0.3 m from its initial position in maneuver 1, and then returned to that initial
position by the end of maneuver 4. At the end of maneuver 4, it is still increasing
in +𝑥 and does not return to its initial position. This indicates that the mixer was
not able to compensate for the thruster degradation in thruster 0, as illustrated in
Figure 4-5. Orange’s performance during this test is shown in Figure 4-6b. During

76
maneuver 3, it is hypothesized that the satellite would have been able to reach the
commanded position if more time had been permitted. Not surprisingly based on
Figure 4-5, Red struggled to move in the +𝑥 direction but quickly translated in −𝑥
(Figure 4-6c), indicating that the mixer was not able to compensate for its crippled
+x thruster (thruster 0). The results for the closed-loop test for translation in 𝑥 were
underwhelming in part due to too little time allotted for the maneuver and crippled
thrusters. It would be worthwhile to investigate the performance with more permitted
time per maneuver.

(a) Blue: position telemetry (b) Orange: position telemetry

(c) Red: position telemetry

Figure 4-6: Test 11: closed-loop x-translation telemetry

The closed-loop translation in 𝑦 test yielded more promising results. Figure 4-7a
illustrates the results for Blue. Blue was able to translate +0.3 m in the 𝑦-direction
and then −0.3 m back. This is not surprising based on the thruster characterization
results indicating no issues with Blue’s ±𝑦 thrusting prior to the new mixer. Or-
ange’s results are shown in Figure 4-7b and indicate that the new mixer was able
to overcome Orange’s limited thrust capability in +𝑦 but not in −𝑦, evident by no
indication that Orange started to move in −𝑦 before the test ended. Lastly, Red’s
performance is illustrated by Figure 4-7c, which indicates Red was able to complete

77
the commanded maneuvers without any issue – unsurprising since its ±𝑦 thrusters
were without significant degradation.

(a) Blue: position telemetry (b) Orange: position telemetry

(c) Red: position telemetry

Figure 4-7: Test 12: closed-loop y-translation telemetry

Similarly, the closed-loop translation in 𝑧 test yielded acceptable results as ex-

pected since there was minimal degradation in any of the ±𝑧 thrusters; the mixers
were modified minimally. Figure 4-8a indicates that Blue completed the +0.3 m
translation and −0.3 m translation back to its original position without issue in the
allotted maneuver time. According to Figure 4-8b, Orange smoothly followed the
+0.3 m command but struggled to return to its initial position. Since the thruster
characterization did not indicate a significant thruster degradation in Orange’s −𝑦
thrusters, a potential solution is to tweak Orange’s mixer appropriately. Lastly, Fig-
ure 4-8c indicates that Red fell short of meeting the commanded position due to
the maneuver ending, but it was able to return to its initial position in the allotted
time. Overall, these results were acceptable and a validation of each mixer in terms
of 𝑧-translation capabilities.
For each rotation test, each satellite was commanded to rotate +90 deg about the
relevant axis and then −90 deg back to its original orientation. From Figure 4-9a,

78
 (a) Blue: position telemetry (b) Orange: position telemetry

(c) Red: position telemetry

Figure 4-8: Test 13: closed-loop z-translation telemetry

it is evident that Blue had no issue performing the desired roll maneuver, indicating
that with its new mixer it is able to overcome any torque deficiencies from thruster
loss-of-effectiveness. Similarly, Orange completed the roll maneuver in the allotted
time with slightly more trouble than Blue – an issue that can be resolved by revising
its mixer (Figure 4-9b). Figure 4-9c also displays that Red had no issue meeting the
commanded roll maneuver in the allowed time. Overall, all three satellites were able
to complete roll maneuvers with their individualized mixers.
The next rotation test was the pitch maneuver. Based on the thruster character-
ization, Blue (prior to the mixer revision) was expected to perform poorly. However,
Figure 4-10a indicates that the updated mixer helped Blue complete the ±90 deg
pitch maneuver. Orange’s position and attitude telemetry in Figure 4-10b suggests a
collision or manual impulse occurred at the beginning of the test. However, Orange
still satisfactorily completed the commanded attitude changes in maneuvers 3 and 4
of this test. Red’s performance during the pitch maneuver proved to be particularly
interesting. As displayed on Figure 4-5, Red’s thruster 0 practically produced no
thrust, which is of concern for a pitch maneuver since it is one of the +𝑦 torquers.

79
 (a) Blue: attitude telemetry (b) Orange: attitude telemetry

(c) Red: attitude telemetry

Figure 4-9: Test 14: closed-loop x-rotation telemetry

Despite this handicap and in combination with Red’s updated mixer, Red completed
the pitch maneuver within the timeframe of the maneuvers (Figure 4-10c).

(a) Blue: attitude telemetry (b) Orange: attitude telemetry

(c) Red: attitude telemetry

Figure 4-10: Test 15: closed-loop y-rotation telemetry

80
 The last closed-loop test conducted on TS101 was the yaw maneuver, in which
each satellite was commanded to rotate ±90 deg about its 𝑧-axis. Figure 4-11a shows
that Blue performed the yaw maneuver well and finished with a few seconds to spare.
Unfortunately, Orange performed poorly, taking a longer period of time to reach the
+90 deg rotation and overshooting the command, leaving little time to return to the
original orientation (Figure 4-11b). This suggests that Orange’s thruster degradation
cannot be compensated for by a modified mixer. Lastly, Red’s yaw maneuver is shown
in Figure 4-11c, which performed exceptionally.

(a) Blue: attitude telemetry (b) Orange: attitude telemetry

(c) Red: attitude telemetry

Figure 4-11: Test 16: closed-loop z-rotation telemetry

Conclusively, the modified mixers proved to help complete desired maneuvers de-
spite thruster degradation for Blue and Red in all tests except 𝑥-translation. Orange,
however, struggled in several of the maneuvers despite the new mixer. From this test
session, Orange was elected to return to ground for thruster replacement.

81
4.2 In-Flight Adaptive PID Sliding Mode Position
and Attitude Controller

Future spacecraft operations, such as debris capture and autonomous in-orbit ser-
vicing, will require spacecraft to interact with objects with poorly defined inertial
properties which introduce a large degree of uncertainty into the dynamics of such
operations. To handle this uncertainty, this section describes the development and
validation of a mass property-resilient controller for position and attitude control of a
free-flying spacecraft. Specifically, the proportional-integral-derivative (PID) sliding
mode controller (SMC) was developed to account for inaccurate knowledge of mass
properties of small spacecraft, using sliding mode variables for each axis and an adap-
tive determination of the appropriate integral and derivative gains to achieve a com-
manded motion. The controller was validated both in simulation and in ground-based
tests on the SPHERES (Synchronized Position Hold Engage Reorient Experimental
Satellites) platform, small free-floating autonomous satellites used to study precision
navigation and maneuvering. The SPHERES was commanded to follow a specified
series of maneuvers using the PID SMC, to assess the convergence time of the sliding
variables, gain values and commanded state parameters in each maneuver.
In order to provide resilience to mass properties, sliding mode control and adaptive
control were utilized. By using sliding mode and adaptive control techniques, the
controller gains and the characteristic coefficients of the system’s equations of motion
were found in-flight using adaptation equations (differential equations that update the
gains and coefficients based on a sliding variable). These values updated throughout
the course of each test to converge the system to the desired state [39].
The controller detailed in this section was designed using the linearized SPHERES’s
translational dynamics (equivalent to that of a mass-spring-damper system) and the
linearized SPHERES’s attitude dynamics (a double integrator). Although this con-
troller was designed specifically for SPHERES, a similar derivation process can be
performed based on the dynamics of the system in question. Additionally, this type
of controller has wider applications in the field of autonomous free-flying spacecraft

82
beyond the SPHERES program. Autonomous spacecraft proximity operations such
as docking, in-orbit servicing, and space debris capture, will comprise a large part of
future space operations. In such cases, inertial properties of the spacecraft or compo-
nent being interacted with may not be fully defined and will impact the dynamics of
the free-flying satellite. Use of a mass property resilient-controller allows the system
to adaptively adjust its gains to account for this uncertainty.
To illustrate the performance of the PID SMC under unknown or uncertain mass
property knowledge, the static PID controller with incorrect mass property knowledge
was used for the same tests. This comparison for both the simulation and hardware
tests emphasizes that the PID SMC offers significantly better performance over a
static PID controller under the circumstance of incorrect/uncertain mass property
knowledge since the PID SMC does not rely on this information.

4.2.1 Problem Formulation

In constructing a mass property-resilient controller, an intuitive approach was to apply

sliding mode control since SMCs reduce a system to first order. SMC forces a system’s
trajectory to slide along a subspace 𝑠 ≡ 0, where 𝑠 is the sliding mode variable of the
form 𝑠 = ( 𝑑𝑡𝑑 +𝜆)𝑛−1 𝑥˜ and 𝑛 is the order of the system. The variable 𝑥˜ is the difference
between the current position and the desired position and 𝜆 is a strictly positive,
user-defined constant that is experimentally selected. Once the sliding variable is
equivalent to zero, the system continues to slide along that subspace until a new
state is commanded. Moreover, adaptive control used with SMC is advantageous
since it allows for updating controller gain values and mass property parameters [39].
The derivation for the controllers was completed for a six degree of freedom en-
vironment, since SPHERES has a microgravity testbed aboard the ISS, as discussed
previously. The following derivation used a proportional-integral-derivative (PID)
control law and yielded three adaptation laws for both position and attitude control,
resulting in a total of eight laws implemented into the final aggregated control system.
Each law has a tunable, positive parameter selected via repetitive experimentation
to determine which value provided rapid convergence. Stability of the control and

83
adaptation laws was verified by applying Barbalat’s Lemma to a candidate Lyapunov
function [39]. In this section, the derivation of the eight laws are detailed as well as
the verification that stability is ensured.

Position Control

The translation dynamics in 𝑥, 𝑦, and 𝑧 of a SPHERES satellite are described by the

following system (𝑚⃗𝑎 = 𝑢 decomposed into axis-specific equations), where 𝑢 repre-
sents the control input. The 𝛼 and 𝛽 constants represent the spring and dampening
coefficients (normalized by the mass) for a SPHERES’s satellite.

𝑥¨ + 𝛼𝑥 𝑥˙ + 𝛽𝑥 𝑥 = 𝑢𝑥 (4.3a)

𝑦¨ + 𝛼𝑦 𝑦˙ + 𝛽𝑦 𝑦 = 𝑢𝑦 (4.3b)

𝑧¨ + 𝛼𝑧 𝑧˙ + 𝛽𝑧 𝑧 = 𝑢𝑧 (4.3c)

As seen from the above equations of motion, this system is order 𝑛 = 2, so the general
form of the sliding variable is

𝑠 = 𝑥˜˙ + 𝜆˜
𝑥 = 𝑥˙ − 𝑥˙𝑟 (4.4)

where 𝑥˜ is the difference between the current position and the desired position (𝑥𝑑 )
and 𝑥˙𝑟 = 𝑥˙ 𝑑 − 𝜆˜
𝑥. The following derivation is for the translation in the 𝑥-axis, so 𝑠
is denoted as 𝑠𝑥 .

𝑠˙ 𝑥 = 𝑥¨ − 𝑥¨𝑟 (4.5a)

𝑠˙ 𝑥 = −𝛼𝑥 𝑥˙ − 𝛽𝑥 𝑥 + 𝑢𝑥 − 𝑥¨𝑟 (4.5b)

𝑠˙ 𝑥 = 𝑢𝑥 − 𝑥¨𝑟 − 𝑌𝑥⃗𝑎𝑥 (4.5c)

⎡ ⎤
𝛼𝑥 ⎥
where 𝑌𝑥 = [𝑥,
˙ 𝑥] and ⃗𝑎𝑥 = ⎣
⎢
⎦
𝛽𝑥

In order to drive 𝑠˙ 𝑥 to 0, let 𝑢^𝑥 = 𝑌𝑥 𝑎

^𝑥 + 𝑥¨𝑟 to counteract the unknown constants.

84
Additionally, a PID control law is desired, so let

𝑢𝑥 = 𝑢^𝑥 − 𝑢𝑥,𝑃 𝐼𝐷 (4.6)

∫︁
^ 𝑖,𝑥
𝑢𝑥,𝑃 𝐼𝐷 = 𝐾𝑝,𝑥 𝑠𝑥 + 𝐾 ^ 𝑑,𝑥 𝑠˙ 𝑥
𝑠𝑥 𝑑𝑡 + 𝐾 (4.7)

^ 𝑖,𝑥 and 𝐾
In Equation 4.7, 𝐾𝑝,𝑥 is a positive constant, but 𝐾 ^ 𝑑,𝑥 can be either positive

or negative and change via adaptation laws that are yielded from the Lyapunov
candidate function. This simplifies 𝑠˙ 𝑥 to Equation 4.8.

˜𝑥 − 𝑢𝑥,𝑃 𝐼𝐷
𝑠˙ 𝑥 = 𝑌𝑥⃗𝑎 (4.8)

A candidate Lyapunov function for this system and its derivative are derived
below. This Lyapunov function needs to be positive definite in order to use Barbalat’s
Lemma to analyze the system stability.

1 1 ˜ 2 + 1 𝛾 −1 𝐾 ˜ 2 + 1𝑎
𝑉𝑥 = 𝑠2𝑥 + 𝛾1−1 𝐾 𝑖,𝑥 2 𝑑,𝑥 ˜𝑇𝑥 Γ−1
1 𝑎 ˜𝑥 (4.9a)
2 2 2 ∫︁
2
˜𝑥 − 𝐾𝑝,𝑥 𝑠2 − 𝐾
𝑉˙ 𝑥 = 𝑠𝑥 𝑌𝑥⃗𝑎 ^ 𝑠
𝑖,𝑥 𝑥 𝑠𝑥 𝑑𝑡 − 𝐾 ^ 𝑑,𝑥 𝑠𝑥 𝑠˙ 𝑥 + ...
𝑥

𝛾1−1 𝐾 ^˙ 𝑖,𝑥 + 𝛾 −1 𝐾
˜ 𝑖,𝑥 𝐾 ^˙ 𝑑,𝑥 + 𝑎
˜ 𝑑,𝑥 𝐾 ^˙ 𝑇𝑥 Γ−1 ˜𝑥 (4.9b)
2 1 𝑎

^˙ 𝑥 = −Γ1 𝑌 𝑇 𝑠𝑥
=⇒ Adaptation Law 1: ⃗𝑎 (4.9c)
𝑥
∫︁
=⇒ 𝑉˙ 𝑥 = −𝐾𝑝,𝑥 𝑠2𝑥 − 𝐾
^ 𝑖,𝑥 𝑠𝑥 ^ 𝑑,𝑥 𝑠𝑥 𝑠˙ 𝑥 + ...
𝑠𝑥 𝑑𝑡 − 𝐾

𝛾1−1 𝐾 ^˙ 𝑖,𝑥 + 𝛾 −1 𝐾
˜ 𝑖,𝑥 𝐾 ^˙ 𝑑,𝑥
˜ 𝑑,𝑥 𝐾 (4.9d)
2

To use Barbalat’s Lemma, 𝑉˙ 𝑥 needs to be negative definite. To ensure this is always

true, the following two adaptation laws are derived, as well.

^˙ 𝑖,𝑥 = 𝛾1 𝑠𝑥
∫︁
=⇒ Adaptation Law 2: 𝐾 𝑠𝑥 𝑑𝑡 (4.10a)

^˙ 𝑑,𝑥 = 𝛾2 𝑠𝑥 𝑠˙ 𝑥
=⇒ Adaptation Law 3: 𝐾 (4.10b)

^𝑥 , 𝐾
When the three adaptation laws are used to update the values of ⃗𝑎 ^ 𝑑 , and 𝐾
^ 𝑖 , 𝑉˙ 𝑥

is guaranteed to be negative definite.

85
 Since 𝑉˙ 𝑥 is negative definite and 𝑉𝑥 is positive definite, it is determined that 𝑉𝑥
˜ 𝑖,𝑥 , 𝐾
is bounded, and therefore, 𝑠𝑥 , 𝐾 ˜ 𝑑,𝑥 , and 𝑎
˜𝑥 are bounded. It is straightforward
to show that 𝑉¨𝑥 is bounded based on the previously determined bounded variables
from 𝑉𝑥 . By Barbalat’s Lemma, 𝑉˙ 𝑥 → 0 as 𝑡 → ∞, which implies that 𝑠𝑥 → 0, as
well [39]. Thus, these three adaptation laws in addition to the PID control provide a
stable sliding mode controller solution for position control.
The same proof can be used for the 𝑦 and 𝑧 position dynamics and would produce
the same result for appropriately subscripted terms unique to 𝑦 and 𝑧.

Attitude Control

⃗¨ = ⃗𝜏 , where 𝛼
The attitude dynamics of the satellite are described by 𝐽 𝛼 ⃗ = [𝜓, 𝜃, 𝜑]𝑇
(which correspond to roll, pitch, and yaw, respectively) and ⃗𝜏 is the control input.
This is decomposed into the following equations per axis.

𝐽11 𝜓¨ + 𝐽12 𝜃¨ + 𝐽13 𝜑¨ = 𝜏𝜓 (4.11a)

𝐽21 𝜓¨ + 𝐽22 𝜃¨ + 𝐽23 𝜑¨ = 𝜏𝜃 (4.11b)

𝐽31 𝜓¨ + 𝐽32 𝜃¨ + 𝐽33 𝜑¨ = 𝜏𝜑 (4.11c)

The following derivation is for the attitude about the 𝑧-axis, denoted by 𝜑. Similar
to the derivation for position, this system has order 𝑛 = 2, so the sliding variable is
defined as 𝑠𝜑 = 𝜑˜˙ + 𝜆𝜑˜ = 𝜑˙ − 𝜑˙ 𝑟 . For increased simplicity in the derivation, the term
𝐽33 𝑠𝜑 is used in the derivation.

𝐽33 𝑠˙ 𝜑 = 𝐽33 𝜑¨ − 𝐽33 𝜑¨𝑟 (4.12a)

𝐽33 𝑠˙ 𝜑 = 𝜏𝜑 − 𝐽31 𝜓¨ − 𝐽32 𝜃¨ − 𝐽33 𝜑¨𝑟 (4.12b)

𝐽33 𝑠˙ 𝜑 = 𝜏𝜑 − 𝑌𝜑 𝐽⃗𝜑 (4.12c)

⎡ ⎤
⎢ 𝐽31 ⎥
[︁ ]︁ ⎢ ⎥
¨
where 𝑌𝜑 = 𝜓, ¨
𝜃, 𝜑¨𝑟 and 𝐽⃗𝜑 =
⎢
⎢ 𝐽32 ⎥
⎥
⎢ ⎥
⎣ ⎦
𝐽33

86
 ^
In order to drive 𝑠˙ 𝜑 to 0, let 𝜏^𝜑 = 𝑌𝜑 𝐽⃗𝜑 . Since a PID control law is desired, let

𝜏𝜑 = 𝜏^𝜑 − 𝜏𝜑,𝑃 𝐼𝐷 (4.13)

∫︁
^ 𝑖,𝜑
𝜏𝜑,𝑃 𝐼𝐷 = 𝐾𝑝,𝜑 𝑠𝜑 + 𝐾 ^ 𝑑,𝜑 𝑠˙ 𝜑
𝑠𝜑 𝑑𝑡 + 𝐾 (4.14)

^ 𝑖,𝜑 and 𝐾
In Equation 4.14, 𝐾𝑝,𝜑 is a positive constant, but 𝐾 ^ 𝑑,𝜑 can be either positive

or negative and change via adaptation laws yielded from the Lyapunov candidate
function.

𝐽33 𝑠˙ 𝜑 = 𝜏^𝜑 − 𝜏𝜑,𝑃 𝐼𝐷 − 𝑌𝜑 𝐽⃗𝜑 (4.15a)

^
𝐽33 𝑠˙ 𝜑 = 𝑌𝜑 𝐽⃗𝜑 − 𝑌𝜑 𝐽⃗𝜑 − 𝜏𝜑,𝑃 𝐼𝐷 (4.15b)
˜
𝐽33 𝑠˙ 𝜑 = 𝑌𝜑 𝐽⃗𝜑 − 𝜏𝜑,𝑃 𝐼𝐷 (4.15c)

A candidate Lyapunov function for this system and its derivative are derived
below.

1 1 ˜ 𝑖,𝜑 1 ˜ 𝑑,𝜑 1˜ ⃗˜
𝑉𝜑 = 𝐽33 𝑠2𝜑 + 𝛾3−1 𝐾 2
+ 𝛾4−1 𝐾 2
+ 𝐽⃗𝜑𝑇 Γ−1
2 𝐽𝜑 (4.16a)
2 2 2 ∫︁ 2
˜
𝑉˙ 𝜑 = 𝑠𝜑 𝑌𝜑 𝐽⃗𝜑 − 𝐾𝑝,𝜑 𝑠𝜑 − 𝑠𝜑 𝐾
2 ^ 𝑖,𝜑 𝑠𝜑 𝑑𝑡 − 𝑠𝜑 𝐾^ 𝑑,𝜑 𝑠˙ 𝜑 + ...
˙
𝛾3−1 𝐾 ^˙ 𝑖,𝜑 + 𝛾4−1 𝐾
˜ 𝑖,𝜑 𝐾 ^˙ 𝑑,𝜑 + 𝐽⃗^𝜑𝑇 Γ−1
˜ 𝑑,𝜑 𝐾 ⃗˜
2 𝐽𝜑 (4.16b)
˙
=⇒ Adaptation Law 1: 𝐽^𝜑 = −Γ2 𝑌𝜑𝑇 𝑠𝜑 (4.16c)
∫︁
^ 𝑖,𝜑
=⇒ 𝑉˙ 𝜑 = −𝐾𝑝,𝜑 𝑠2𝜑 − 𝑠𝜑 𝐾 ^ 𝑑,𝜑 𝑠˙ 𝜑 + ...
𝑠𝜑 𝑑𝑡 − 𝑠𝜑 𝐾

𝛾3−1 𝐾 ^˙ 𝑖,𝜑 + 𝛾4−1 𝐾

˜ 𝑖,𝜑 𝐾 ^˙ 𝑑,𝜑
˜ 𝑑,𝜑 𝐾 (4.16d)

To ensure that 𝑉˙ 𝜑 is negative definite, the following two adaptation laws are derived,
as well.

^˙ 𝑖,𝜑 = 𝛾3 𝑠𝜑
∫︁
=⇒ Adaptation Law 2: 𝐾 𝑠𝜑 𝑑𝑡 (4.17a)

^˙ 𝑑,𝜑 = 𝛾4 𝑠𝜑 𝑠˙ 𝜑
=⇒ Adaptation Law 3: 𝐾 (4.17b)

87
When the three adaptation laws are used to update the values of 𝐽^𝜑 , 𝐾
^ 𝑖,𝜑 , and 𝐾
^ 𝑑,𝜑 ,

𝑉˙ 𝜑 is guaranteed to be negative definite. Therefore, by the same logic as from the

position control derivation, these three adaptation laws in addition to the PID control
law provide a stable sliding mode controller solution for attitude control.

The same proof can be used for 𝜓 and 𝜃 attitude dynamics and would produce
the same result with appropriately subscripted terms unique to 𝜓 and 𝜃.

4.2.2 Software and Hardware Implementation

To validate the control and adaptation laws, a sequence of maneuvers, described in

Table 4.4, were defined and conducted both in simulation and on a ground-based
testbed with three degrees of freedom (two translational and one rotational). Each
maneuver commanded the SPHERES to reach a certain (𝑥, 𝑦) position on the glass
table, while maintaining a commanded attitude, before moving on to the next ma-
neuver. To allow flexibility in how long the satellite took to reach the maneuver’s
endpoint, the software terminated a current maneuver and commanded the next ma-
neuver only if the satellite stayed within 2.5 cm of the desired endpoint (in both
𝑥- and 𝑦-directions) for 5 seconds. Each maneuver also had an upper time limit,
after which the maneuver was terminated and the satellite was commanded to the
next position. Note that Maneuver 1 was an estimator convergence maneuver, where
the satellite remained stationary to allow for accurate localization of its position and
attitude before beginning the commanded maneuvers.

An extensive sweep of possible parameter values via numerous simulation runs was
used to determine the final parameter values used for the simulation and hardware
testing based on which provided the best performance. The following parameters
yielded the quickest response time without overshooting in simulation. Note that
in the 3-DOF implementation of this controller, the user-defined position control

88
Table 4.4: Sequential series of maneuvers and corresponding commanded position
and quaternion

Maneuver Position (x,y) [m] Quaternion

1 N/A (1, 0, 0, 0)
2 (0, -0.25) (1, 0, 0, 0)
3 (0, 0) (1, 0, 0, 0)
4 (0, 0.25) (1, 0, 0, 0)
5 (0.25, 0.25) (1, 0, 0, 0)
6 (0.25, 0) (1, 0, 0, 0)
7 (0, -0.25) (1, 0, 0, 0)
8 (0.125, 0) (1, 0, 0, 0)
9 (0.25, 0.25) (1, 0, 0, 0)

parameters were used for both the 𝑥 and 𝑦 translation.

Position Control

𝜆1 = 0.085, 𝛾1 = 60, 𝛾2 = 80,

⎡ ⎤
⎢ 10 0 0 ⎥
⎢ ⎥
𝐾𝑝,𝑥 = 1000, Γ1 = ⎢
⎢ 0 10 0 ⎥
⎥
⎢ ⎥
⎣ ⎦
0 0 10

Attitude Control

𝜆2 = 0.10, 𝛾3 = 4, 𝛾4 = 2,
⎡ ⎤
⎢ 10 0 0 ⎥
⎢ ⎥
𝐾𝑝,𝜑 = 50, Γ2 = 0 10 0 ⎥
⎢ ⎥
⎢
⎢ ⎥
⎣ ⎦
0 0 10

The control and adaptation laws described in the previous section were imple-
mented within the SPHERES framework as a substitution for the default controller.
The sequence of maneuvers described in Table 4.4 was defined as a test file that in-
corporated the standard infrastructure of the SPHERES programming. Therefore,
during the execution of this test, the standard infrastructure handled the satellite’s
state vector and computed the state error. Once the state error was calculated, a call
to the PID SMC was made. A high-level description of the sequence of computations

89
that occurred in the controller is displayed in Figure 4-12. In this figure (and in
the subsequent sections of this paper), 𝑝 is used as a generic variable representing
𝑥, 𝑦, and 𝜑 for brevity. Note that the flowchart provides the numerical equations
implemented for computing the integral term of the sliding variable as well as the
derivative. Specifically, the integral terms are not integrated in addition to the nu-
merical integration equation, but are provided as a reference to the terms derived in
the previous section.

4.2.3 Results & Discussion

This section describes the results yielded from the previously derived position and
attitude PID SMC with the above parameters. First, the results from simulation
are shown and discussed, and then the results from hardware testing are shown and
discussed.

Simulation

Simulation tests were run using the SPHERES simulation and flight software together
with the implemented PID SMC. The simulation was limited to a three degrees of
freedom test using a mass model that incorporated the air bearing stand that the
SPHERES are held on for ground tests. Thus, the simulation aligns with the hardware
ground test as closely as possible.
Figure 4-13a shows the 𝑥-position, 𝑦-position, and 𝑦𝑎𝑤 attitude commands and
response of the satellite during the simulation. The vertical magenta lines indicate a
new maneuver has been commanded (i.e. a new commanded position and attitude).
As described in the previous section, the satellite is permitted to move on to the
next maneuver once it stays within a 2.5 cm tolerance of the commanded position for
5 seconds. In the simulation the satellite was consistently able to meet this condition,
converging to the commanded position and attitude and subsequently terminating
each maneuver after 5 seconds. This is evident in Figure 4-13a where the satellite
takes approximately 20 seconds to converge to the commanded state and holding for

90
 Start Controller Call

Convert quaternions
to Euler angles

Compute
Δt
sp dt = sp dt + (sp,j−2 + sp,j−1 )
(∫ ) (∫ )
j j−1 2

Compute
.

sp,j = p̃ j + λ p̃ j

Compute
. .

p̃ j − p̃ j−1 .
.
sp,j = + λ p̃ j
Δt

Construct Yp matrix

Compute
. .
.

ˆ ˆ
â p , K i,p , K d,p

Numerically integrate to
find
ˆ ˆ
â p , K i,p , K d,p

Compute control laws

up = û p − up,PID

End Controller Call

Figure 4-12: Flowchart of the in-flight adaptive PID sliding mode controller as im-
plemented in C-code

5 seconds before moving on.

In order to verify that the PID SMC provides a better control solution to the
scenario in which uncertain mass properties exist, the PID SMC results are com-
pared against results from the static PID controller given incorrect mass properties.
The position and attitude results from such a scenario are shown in Figure 4-13b.

91
 Time vs. X-Position
0.5

Position [m]
0

Measured
Commanded
Maneuver
-0.5
0 50 100 150 200 250 300 350 400
Time [s]
Time vs. Y-Position
0.5

Position [m]
0

-0.5
0 50 100 150 200 250 300 350 400
Time [s]
Time vs. Yaw
200

150

Attitude [deg]
100

50

-50

0 50 100 150 200 250 300 350 400

Time [s]

(a) Adaptive PID SMC (b) Static PID

Figure 4-13: A comparison of commanded position and attitude vs. measured teleme-
try from simulation

Note that with this static PID controller, the test took roughly 425 seconds to reach
completion — 200 seconds longer than it took for the PID SMC. Additionally, the
position and attitude overshoot on every new command.

Recall that the sliding variable is found via

𝑠𝑝 = 𝑝˜˙ + 𝜆˜
𝑝 (4.18)

where 𝑝˜ = 𝑝 − 𝑝𝑑 .

Additionally, recall that the goal of the sliding mode controller is for the sliding
variable to converge to zero (indicating that the system has converged to the desired
position/attitude). Thus, when viewing the time history of the sliding variables (i.e.
Figure 4-14a), the goal is for the variable to converge to zero by the end of the
maneuver. When the position or attitude command between maneuvers changes, it
is expected for the sliding variable to jump to a new value. Naturally, this change is
proportional in magnitude and direction as the new command compared to the old
command.

92
 (a) Sliding Variables (b) Gain Values

Figure 4-14: Adaptive PID SMC time histories corresponding to position and attitude
from simulation

Figure 4-14a illustrates how the 𝑠𝑝 variable changes throughout the course of the
test. Since the sliding variable was not used during the first maneuver (estimator
convergence) of the test, this maneuver is not included in the figure. The sliding
variables associated with the 𝑥 and 𝑦 positions are consistent with the desired behavior
whereas the sliding variable associated with the 𝑦𝑎𝑤 attitude is not. The sliding
variables 𝑠𝑥 and 𝑠𝑦 converge to zero by the end of each maneuver and reset as expected
per new command at a new maneuver. However, 𝑠𝜑 exhibits significant chattering
about 𝑠𝜑 = 0, indicating that the variable overshoots zero and enters a limit cycle as
it repeats the overshoot. This behavior was not fixable via parameter tuning, but it is
expected that if the attitude sliding mode controller was re-designed with a boundary
layer in the sliding variable, this behavior would subside.

The adaptation laws are of the following form:

^˙ 𝑝 = −Γ𝑌 𝑇 𝑠𝑝
⃗𝑎 (4.19)
𝑝

^˙ 𝑖,𝑝 = 𝛾𝑖 𝑠𝑝
∫︁
𝐾 𝑠𝑝 𝑑𝑡 (4.20)

93
 ^˙ 𝑑,𝑝 = 𝛾𝑑 𝑠𝑝 𝑠˙ 𝑝 .
𝐾 (4.21)

Each adaptation law contains the sliding variable; therefore, as the sliding variable
converges to zero, it is expected that these derivative terms converge to zero, meaning
the adaptation term and gain values converge to some constant value per command.
^ values change for each degree of freedom
Figure 4-14b illustrates how the 𝐾
through the course of the test. Convergence to a constant value is evident for the
^ 𝑑 and 𝐾
𝐾 ^ 𝑖 values for the 𝑥 and 𝑦 positions. However, for the 𝑦𝑎𝑤 attitude gains,
^ 𝑑 value keeps increasing across all maneuvers. This issue is correlated with the
the 𝐾
chattering behavior in Figure 4-14a and is discussed in more detail later in this paper.
The 𝑥-𝑦 plot in Figure 4-15 shows the satellite’s trajectory as it attempted to
reach each position for both the PID SMC and the default PID controller. In the
figure, the asterisk markers indicate the satellite’s position at the beginning of each
maneuver. Although the commands are only associated with specific positions the
satellite is to reach by the end of each maneuver without explicitly defining the
trajectory, there exists an implied commanded trajectory, i.e. the shortest distance
between the position of the previous maneuver with that of the current maneuver.
With the PID SMC, the satellite closely matches this implicit trajectory as it reaches
each commanded position. However, when the satellite is equipped with the default
PID controller, it overshoots each commanded endpoint, illustrating that the PID
SMC offers better performance.

Hardware

Hardware tests were run using the glass table for SPHERES testing in the Space
Systems Laboratory. The satellite was fastened to an air bearing stand in order
to minimize friction with the table. The same flight software was loaded on the
SPHERES as was used in the simulation. Again, this setup is restricted to three
degrees of freedom (𝑥, 𝑦 and 𝑦𝑎𝑤). From this testing, four datasets were produced,
all of which are shown in Figure 4-16a; however, only dataset 4 is discussed in detail
presently as it yielded the best results (and the corresponding data is shown in Figures

94
 X vs. Y
0.4
PID SMC
Default PID
Commanded

0.3
Man 5
ManMan
6 6
Man 5

0.2

0.1

Man 4
Man 9

Y [m]
0 Man
Man 22 Man 7
Man 4 Man 9
Man 7

-0.1

-0.2

Man 8
Man 3
Man Man
8 3
-0.3

-0.4
-0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4
X [m]

Figure 4-15: Trajectories plotted in the 𝑥-𝑦 plane from simulation

4-17a, 4-18a, and 4-18b).

It is clear from Figure 4-16b that the static PID controller performed poorly on
the table in comparison to the adaptive PID SMC in Figure 4-16a. It was expected
that the performance would be poor based on the simulation results. However, it
is hypothesized that the imperfections of the glass table coupled with the inferior
controller resulted in the inability of the PID controller to meet the desired endpoints.

Figure 4-17a illustrates the 𝑥-position, 𝑦-position, and 𝑦𝑎𝑤 attitude commands
and response of the satellite during the hardware test corresponding to dataset 4.
The same time tolerance as the simulation case was set for completing a maneuver.
For this hardware test, the maximum time the satellite was allowed to spend in each
maneuver was 100 seconds, before the satellite would terminate its effort to converge
and move on to the next maneuver. In maneuvers 4, 5 and 9 it can be seen that
while the satellite converges to the commanded 𝑥-position and 𝑦𝑎𝑤, it struggles to
converge to the commanded 𝑦-position. From Figure 4-16a it is evident that this
inability to reach the commanded 𝑦-position in maneuvers 4, 5 and 9 (𝑦 = 0.25 cm)
occurred across all four hardware tests. However, this was clearly not an issue in
the simulation tests. There are several possible explanations for this result. The
simulation likely has a poor model of the true system, particularly with respect to

95
 X vs. Y
0.4
Default PID
Commanded

0.3

0.2

0.1

Man 7
Man 8
Man 6

Y [m]
0 Man 5 Man 9

Man 4
Man 3

-0.1

-0.2

-0.3

Man 2

-0.4
-0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4
X [m]

(a) Four datasets from adaptive PID SMC (b) Static PID

Figure 4-16: Trajectories plotted in the 𝑥-𝑦 plane from hardware tests

thruster performance. The simulation assumes all thrusters thrust at a constant value
over time, however in practice this does not occur. Some thrusters on the hardware
have suffered degradation over the years and therefore thrust inconsistently. The
air bearings that move the SPHERES across the glass table require an extremely
smooth surface to operate, so any small imperfections in the glass table could result in
additional friction when moving the SPHERES in that area. As the problem seems to
be consistent with a particular 𝑦 position across the table, the more likely explanation
is that the glass table was not perfectly level at that point and consequently the
thrusters had difficulty moving the satellite in this region.

In all four datasets, maneuvers 4, 5 and 9 timed-out (seen in Figure 4-17a where
the maneuvers automatically terminate after 100 seconds). It is also important to
note that for datasets 1 and 2, the final maneuver was not completed. This was due
to depletion of the CO2 tanks used for thruster propellant before the final maneuver.
Lastly, the large jumps in data in Figure 4-16a is indicative of a combination of some
data dropping across communications in addition to some estimation error rather
than the satellite performing large oscillations as it translated.

96
 Time vs. X-Position
0.5

Position [m]
0

Measured
Commanded
Maneuver
-0.5
0 100 200 300 400 500 600 700 800
Time [s]
Time vs. Y-Position
0.5

Position [m]
0

-0.5
0 100 200 300 400 500 600 700 800
Time [s]
Time vs. Yaw
200

150

Attitude [deg]
100

50

-50

0 100 200 300 400 500 600 700 800

Time [s]

(a) Adaptive PID SMC (b) Static PID

Figure 4-17: A comparison of commanded position and attitude vs. measured teleme-
try from hardware tests

Figure 4-17b illustrates the position and attitude time history of the satellite
completing the test maneuvers using the static PID controller with incorrect mass
knowledge. It is evident that the controller struggles to complete the test, timing
out in each maneuver rather than meeting the desired endpoint. Comparing Figure
4-17b with Figure 4-17a supports the conclusion that the PID SMC offers superior
performance to the default PID controller when there exists incorrect mass property
knowledge because the adaptive PID SMC controller is able to meet the commands
whereas the defualt PID controller cannot.
Figure 4-18a depicts the 𝑠𝑝 sliding variable time history for dataset 4. As men-
tioned previously, since the sliding variable was not used during the first maneuver
(estimator convergence), the maneuver is not included in the figure. Recall that for
each maneuver, the sliding variable should converge to 𝑠𝑝 = 0, corresponding to the
system converging to the commanded position (or attitude). Compared to the time
history of sliding variables in Figure 4-14a, the sliding variables for 𝑥 and 𝑦 positions
appear to chatter slightly around 𝑠𝑝 = 0. This behavior is consistent with the slow
convergence to the commanded positions in Figure 4-17a. In fact, the slow conver-

97
 (a) Sliding Variables (b) Gain Values

Figure 4-18: Adaptive PID SMC time histories corresponding to position and attitude
from hardware tests

gence to the commanded position is a consequence of this sliding variable behavior.

Of greater significance is the chattering of the 𝑦𝑎𝑤 sliding variable. As mentioned
in the discussion of the simulation results, this chattering indicates that the sliding
variable is limit cycling about 𝑠𝑝 = 0. An attempt to resolve this issue with parame-
ter tuning was not fruitful, and it is expected that adding a boundary layer into the
attitude controller’s sliding variable would alleviate the issue.

Figure 4-18b shows the time history of the derivative and integral gains for the 𝑥
^
and 𝑦 positions and the 𝑦𝑎𝑤 attitude. Recall that the desired behavior of these 𝐾
values is convergence to some constant value per commanded position; however, this
convergence is dependent on the sliding variable converging to 𝑠𝑝 = 0. Of particular
interest is the sliding variable time history for the 𝑦 position during maneuvers 4 and
5. From Figure 4-17a, it is clear that the satellite moves on to the next maneuver due
to reaching the maximum time limit rather than spending 5 seconds within a tolerance
of 2.5 cm of the commanded position. Thus, the corresponding time history in Figure
4-18b indicates that the slowly changing gains were insufficient for controlling the
satellite to its desired position. Similar to the simulation results, the gain values

98
 ^ 𝑖 value does not manage to
for the yaw attitude exhibit poor convergence. The 𝐾
converge to a constant value during the maneuvers; however, the range over which
the value changes is an order of magnitude smaller than the corresponding position
integral gain values. This indicates a negligible impact on the system. Again, the
^ 𝑑 value is a consequence of the chattering 𝑦𝑎𝑤 sliding variable that
ever-increasing 𝐾
is discussed in more depth in the following section.

Considerations & Conclusions

When conducting hardware tests of the PID sliding mode attitude and position con-
trollers, the decision was made to increase the test from five maneuvers to nine in
order to add waypoints between particularly distant endpoints of maneuvers. This
was an attempt to force the satellite to more closely follow the trajectory that the
simulation mapped out (i.e. a straight line between endpoints). Prior to these ad-
ditional waypoints, the satellite followed semicircular-like curves between endpoints
during hardware testing due to imperfections in the glass table that inadvertently af-
fected the satellite’s path. The addition of these waypoints decreased the initial value
of the sliding variable per new command, which translates into smaller necessary gain
values per maneuver.
In order for a sliding mode controller to operate correctly, chattering must be
eliminated in order to limit the necessary control activity. As mentioned previously,
chattering of the 𝑦𝑎𝑤 attitude sliding variable occurred in both simulation and in
hardware testing. A thin boundary layer can be applied to 𝑠𝑝 = 0 in an attempt
to smooth out this chattering. The addition of a thin boundary layer to the sliding
variable trades "perfect" tracking for tracking with a guaranteed precision proportional
to the width of the boundary layer. This implementation changes the definition
of the sliding variable and the subsequent derivation of the adaptation laws. The
implementation of a boundary layer in the attitude controller is future work for this
research.
Both hardware and simulation tests were used to validate the PID sliding mode
controllers for position and attitude. Results show that with experimentally tuned

99
parameters, the adaptive controller enabled the satellite to sufficiently converge to
commanded positions and attitudes over a series of maneuvers. The performance
of the PID SMC was compared with a PID controller designed with incorrect mass
property knowledge (the SPHERES’s default PID controller). The results of both
simulation and hardware tests for each controller supports the conclusion that the
PID SMC provides resilience to incorrect mass properties knowledge and superior
performance. Future work will address the chattering of the attitude controller via
the addition of a boundary layer to the sliding variable (and the subsequent effects
that will have on the adaptation laws). Altogether, the PID SMC is a promising
control scheme for offering resilience to systems with uncertainty in mass properties,
and continued efforts are necessary to more finely tune its capabilities.

100
Chapter 5

Docked Dynamics

The focus of this chapter is the beginning of Stage 3 of the ROAM project – soft
docking between the Chaser and the Target and the detumbling procedure that brings
the aggregated system to a standstill. In this stage, the Chaser has instantaneously
docked to the Target by maintaining synchronous motion on its approach. The goal
is for the Chaser to detumble the Target while minimizing fuel, loads, and moments
imparted on the Target. This chapter will overview the contact dynamics of docking,
the docked dynamics of maintaining synchronous motion, and an optimization for
the detumble procedure. Because active debris removal presents scenarios in which
Targets will be fragile, the imparted loads and moments on the Target are considered
in the cost of the detumbling maneuver.

5.1 Contact Dynamics of Docking

For the scenario in which the Target spacecraft is fragile, the user is interested in de-
termining the loads and moments imparted on the Target during the docking process
and detumbling maneuver. The contact dynamics of docking are provided from the
literature and applied to this problem [67].
Figure 5-1 indicates the Chaser and Target the instant before docking, in which
both spacecraft still have their separate linear and angular velocity. Refer to Table
5.1 regarding the notation used in the figure. This problem is more constrained than

101
Figure 5-1: Chaser and Target during instance before docking occurs denoted with
separate position, velocity, and attitude vectors

the original problem cited in the literature [67], since the trajectory approach of the
mission is limited to a synchronous radial approach. This implies that the angular
velocity of the Chaser is equivalent to that of the Target. Figure 5-2 illustrates the
aggregated system immediately after docking.

Table 5.1: Notation for contact dynamics of two spacecraft docking to one another

Variable Description
⃗𝑣𝑡 , ⃗𝑣𝑐 Independent linear velocity
𝜔
⃗ 𝑡, 𝜔 ⃗𝑐 Independent angular velocity
⃗𝑟𝑡 , ⃗𝑟𝑐 Position from system center of mass
⃗𝑟𝑡𝑐 Distance from Target’s CM to Chaser’s CM
⃗𝑟𝑙𝑡 Distance from docking interface to Target’s CM
𝑚𝑡 , 𝑚𝑐 Spacecraft mass
⃗ 𝐶𝑀 𝐶
Ω System angular velocity
𝑉⃗𝐶𝑀 𝐶 System linear velocity
𝜖= 𝑚 𝑚𝑡
𝑐
Mass ratio

In order to yield expressions for the impulsive force and torque imparted during
docking, several geometric relationships are defined. The position vectors are related
via ⃗𝑟𝑐 = ⃗𝑟𝑡 +⃗𝑟𝑡𝑐 . The definition of the center of mass of the aggregated system implies
that 𝑚𝑐⃗𝑟𝑐 + 𝑚𝑡⃗𝑟𝑡 = 0. Therefore, both position vectors can be expressed in terms of

102
Figure 5-2: Chaser and Target in the instance after docking denoted with aggregated
translational and attitude velocity

𝜖 and ⃗𝑟𝑡𝑐 .

⃗𝑟𝑐 = (1 + 𝜖)−1⃗𝑟𝑡𝑐 (5.1)

⃗𝑟𝑡 = −(1 + 𝜖)−1 𝜖⃗𝑟𝑡𝑐 (5.2)

Differentiating Equation 5.2 yields an expression for ⃗𝑟˙𝑡 .

⃗𝑟˙𝑡 = ⃗𝑣𝑡 − 𝑉⃗𝐶𝑀 𝐶 (5.3a)

⃗𝑟˙𝑡 = −(1 + 𝜖)−1 𝜖⃗𝑟˙𝑡𝑐 (5.3b)

Thus, the independent velocity of the Target relative to the aggregated system linear
velocity after docking is found by equating the two ⃗𝑟˙𝑡 expressions and rearranging.

⃗𝑣𝑡 = 𝑉⃗𝐶𝑀 𝐶 − (1 + 𝜖)−1 𝜖⃗𝑟˙𝑡𝑐 (5.4)

By conservation of linear momentum, the linear momentum of the Chaser and the
Target before docking must be equivalent to the linear momentum of the aggregated
system after docking. From this relationship, an expression for the linear velocity of

103
the aggregated system is determined.

𝑚𝑐⃗𝑣𝑐 + 𝑚𝑡⃗𝑣𝑡 = (𝑚𝑐 + 𝑚𝑡 )𝑉⃗𝐶𝑀 𝐶 (5.5a)

𝑚𝑐⃗𝑣𝑐 + 𝑚𝑡⃗𝑣𝑡
=⇒ 𝑉⃗𝐶𝑀 𝐶 = (5.5b)
𝑚𝑐 + 𝑚𝑡

The Target’s impulsive velocity due to docking is

⃗ 𝐶𝑀 𝐶 × ⃗𝑟𝑡 .
𝑉⃗𝑡+ = 𝑉⃗𝐶𝑀 𝐶 + Ω (5.6)

Using the velocity relationships already defined, the impulsive force is determined in
Equation 5.7c.

𝐹⃗𝐼 = 𝑚𝑡 (𝑉⃗𝑡+ − 𝑉⃗𝑡 ) (5.7a)

⃗ 𝐶𝑀 𝐶 × ⃗𝑟𝑡 + (1 + 𝜖)−1 𝜖⃗𝑟˙𝑡𝑐 )
𝐹⃗𝐼 = 𝑚𝑡 (Ω (5.7b)

𝐹⃗𝐼 = 𝑚𝑐 (1 + 𝜖)−1 (⃗𝑟˙𝑡𝑐 − Ω

⃗ 𝐶𝑀 𝐶 × ⃗𝑟𝑡𝑐 ) (5.7c)

Equation 5.7c indicates that ⃗𝑟˙𝑡𝑐 , Ω

⃗ 𝐶𝑀 𝐶 , and ⃗𝑟𝑡𝑐 are the state variables contributing

to the impulsive force.

Conservation of angular momentum of the system before and after docking is
⃗ 𝐶𝑀 𝐶 ).
utilized for determining the angular velocity of the system (Ω

⃗ 𝑡 +⃗𝑟𝑡 × 𝑚𝑡⃗𝑟˙𝑡 + 𝐻
𝐻 ⃗ 𝑐 + ⃗𝑟𝑐 × 𝑚𝑐⃗𝑟˙𝑐 = 𝐼𝐶𝑀 𝐶 Ω
⃗ 𝐶𝑀 𝐶 (5.8a)
[︁ ]︁
⃗ 𝐶𝑀 𝐶 = 𝐼 −1 𝐻
Ω ⃗ 𝑐 + 𝑚𝑐 (1 + 𝜖)−1⃗𝑟𝑡𝑐 × ⃗𝑟˙𝑡𝑐
⃗𝑡 + 𝐻 (5.8b)
𝐶𝑀 𝐶

⃗ 𝑡 = 𝐼𝑡 𝜔
𝐻 ⃗𝑡
⃗ 𝑐 = 𝐼𝑐 𝜔
𝐻 ⃗𝑐

The impulsive torque is expressed in Equation 5.9.

⃗ 𝐶𝑀 𝐶 − 𝜔
⃗𝜏𝐼 = 𝐼𝑡 (Ω ⃗ 𝑡 ) − ⃗𝑟𝑙𝑡 × 𝐹⃗𝐼 (5.9)

⃗ 𝐶𝑀 𝐶 , 𝜔
Equation 5.9 indicates that Ω ⃗ 𝑡 , ⃗𝑟𝑙𝑡 , and the variables from Equation 5.7c are

104
the state variables contributing to the impulsive torque imparted on the spacecraft.
Based on the aforementioned analysis of the impulsive forces and torques imparted
in docking, it is evident that, in the scenario of a fragile Target, the Chaser must con-
tinue to thrust such that the spin axis remains at the Target’s center of mass. These
equations illustrated that with the constraint that the Chaser radially approaches the
Target while maintaining synchronicity, no state variable in this process can be mod-
ified to limit the impulses. This is a consequence of the synchronous radial approach
constraint offering no flexibility in the state variables that constitute Equations 5.7c
and 5.9. Therefore, a solution is to command the Chaser to thrust while docking such
that the angular velocity of the system is moved to be about the Target’s center of
mass as the Chaser starts the detumble procedure.
A script that symbolically solves for the impulsive force and torque can be found
in Appendix C.1, Listing C.1. A function to numerically solve for the impulsive force
and torque based on a given Chaser and Target is given in Appendix C.1, Listing C.2.

5.2 Modeled Docked Dynamics

The body-fixed frame is located at the Target’s center of mass (CM), with the axis 𝑒^𝑟
going through its docking port, and axes 𝑒^𝜃 and 𝑒^𝑧 mutually perpendicular creating a
right-handed system (see Figures 5-3a and 5-3b). The inertial frame is the mutually
perpendicular, right-handed system 𝑥^, 𝑦^, 𝑧^ that is equivalent to the body-fixed frame
at the start of the docked stage. It is assumed that the Chaser has thrusters that
exert force in the 𝑒^𝜃 and 𝑒^𝑧 directions and a reaction wheel to torque about 𝑒^𝑟 .
Since the Chaser must be synchronous with the Target’s rotation, the Chaser’s
translational dynamics in this stage are sufficiently characterized by the following
equation.
⃗𝑟¨𝐼 = R𝐵2𝐼 [(⃗𝜔 × (⃗𝜔 × ⃗𝑟𝐵 ) + (𝜔
⃗˙ × ⃗𝑟𝐵 )] (5.10)

In Equation 5.10, ⃗𝑟𝐵 is the distance from the Target’s CM to the Chaser’s CM, which
does not change in the body frame since the spacecraft stays docked. The variable 𝜔
⃗
is the angular velocity expressed in the body axes of the Target and Chaser, and R𝐵2𝐼

105
 (a) Top-down view (b) Side view

Figure 5-3: Chaser and Target as docked, aggregated system with denoted body-fixed
frame at Target’s center of mass and the force and torque directions of the Chaser

is the rotation matrix from the body frame to the inertial frame. While this equation
sufficiently defines the translation motion of the Chaser, it can be equivalently ex-
pressed via the following linear momentum balance, which illustrates the tensile and
shear loads applied to the Target during the detumbling procedure.

Σ𝐹⃗ = 𝑚⃗𝑟¨𝐵 (5.11a)

𝑚⃗𝑟¨𝐵 = 𝑚(⃗𝜔 × (⃗𝜔 × ⃗𝑟𝐵 ) + (𝜔

⃗˙ × ⃗𝑟𝐵 )) (5.11b)

Σ𝐹⃗ = (𝐹𝑟 + 𝑇 )^
𝑒𝑟 + (𝐹𝜃 + 𝑆𝜃 )^
𝑒𝜃 + (𝐹𝑧 + 𝑆𝑧 )^
𝑒𝑧 (5.11c)

Equating Equation 5.11b with Equation 5.11c and representing the relationship in
matrix form yields Equation 5.12.
⎡ ⎤ ⎡ ⎤
2 2
⎢ 𝐹𝑟 + 𝑇 ⎥ ⎢ −𝑟𝑟 (𝜔𝜃 + 𝜔𝑧 ) + 𝑟𝜃 (𝜔𝑟 𝜔𝜃 − 𝜔
˙ 𝑧 ) + 𝑟𝑧 (𝜔𝑟 𝜔𝑧 + 𝜔˙ 𝜃 ) ⎥
⎢ ⎥ ⎢ ⎥
𝐹 𝜃 + 𝑆𝜃 ⎥ = 𝑚 ⎢ 𝑟𝑟 (𝜔𝑟 𝜔𝜃 + 𝜔˙ 𝑧 ) − 𝑟𝜃 (𝜔𝑟2 + 𝜔𝑧2 ) + 𝑟𝑧 (𝜔𝜃 𝜔𝑧 − 𝜔˙ 𝑟 ) ⎥ (5.12)
⎢ ⎥ ⎢ ⎥
⎢
⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣ ⎦
𝐹 𝑧 + 𝑆𝑧 𝑟𝑟 (𝜔𝑟 𝜔𝑧 − 𝜔˙ 𝜃 ) + 𝑟𝜃 (𝜔𝜃 𝜔𝑧 + 𝜔˙ 𝑟 ) − 𝑟𝑧 (𝜔𝑟2 + 𝜔𝜃2 )

In Equation 5.12, 𝑇 , 𝑆𝜃 , and 𝑆𝑧 are the tensile and shear loads expressed in the
body frame of the docking interface. This linear momentum balance uses the free
body diagram of the Chaser only (Figure 5-4b and 5-4a). In order to determine the
values of 𝑇 , 𝑆𝜃 , and 𝑆𝑧 , the necessary torques for changing the Target’s rotation in
the desired manner are considered.
Now, the angular momentum balance for the Target and Chaser combined sys-
tem is constructed to determine the desired torques to detumble the Target. Let 𝜌

106
 (a) Top-down view of Chaser (b) Side view of Chaser

Figure 5-4: Free body diagram of the Chaser displaying Chaser’s abilities to impart
force and torques and its internal loads and moments from interacting with the Target

represent the distance between the Target CM and the Chaser CM. The following
equations constitute the angular momentum balance.

Σ𝜏𝑇 = 𝐻˙ 𝐶𝑀 𝐶/𝑇 𝐴𝑅 (5.13a)

⃗˙ + (⃗𝜔 × [𝐼]𝐶𝑀 𝐶/𝑇 𝐴𝑅 𝜔

Σ𝜏𝑇 = [𝐼]𝐶𝑀 𝐶/𝑇 𝐴𝑅 𝜔 ⃗) (5.13b)

𝑒𝑟 × 𝐹𝜃 𝑒^𝜃 + 𝜌^
Σ𝜏𝑇 = 𝑀𝑥 𝑒^𝑟 + 𝜌^ 𝑒𝑟 × 𝐹𝑧 𝑒^𝑧 (5.13c)

⃗˙ + (⃗𝜔 ×[𝐼]𝐶𝑀 𝐶/𝑇 𝐴𝑅 𝜔

[𝐼]𝐶𝑀 𝐶/𝑇 𝐴𝑅 𝜔 ⃗ ) = 𝑀𝑥 𝑒^𝑟 − 𝜌𝐹𝑧 𝑒^𝜃 + 𝜌𝐹𝜃 𝑒^𝑧 (5.13d)

Expanding Equation 5.13d into matrix form yields the following Equation 5.14.
⎡ ⎤ ⎡ ⎤
⎢ 𝑀𝑥 ⎥ ⎢ 𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝑟𝑟 𝜔˙ 𝑟 + (𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝑧𝑧 − 𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝜃𝜃 )𝜔𝜃 𝜔𝑧 ⎥
⎢ ⎥ ⎢ ⎥
−𝜌𝐹𝑧 ⎥ = 𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝜃𝜃 𝜔˙ 𝜃 + (𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝑟𝑟 − 𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝑧𝑧 )𝜔𝑟 𝜔𝑧 ⎥ (5.14)
⎢ ⎥ ⎢ ⎥
⎢ ⎢
⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣ ⎦
𝜌𝐹𝜃 𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝑧𝑧 𝜔˙ 𝑧 + (𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝜃𝜃 − 𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝑟𝑟 )𝜔𝑟 𝜔𝜃

In the above equations, [𝐼]𝐶𝑀 𝐶/𝑇 𝐴𝑅 is defined as

[𝐼]𝐶𝑀 𝐶/𝑇 𝐴𝑅 = [𝐼]𝐶 + [𝐼]𝑇 + (𝑚𝐶 + 𝑚𝑇 )[I3 (⃗𝑞𝐵 · ⃗𝑞𝐵 ) − ⃗𝑞𝐵 ⊗ ⃗𝑞𝐵 ] (5.15)

and ⃗𝑞𝐵 is the displacement vector from the aggregated center of mass to the Target’s
CM. In general, this is not a diagonal matrix. In the context of ROAM, only inertia
ratios are provided from the estimation phase, so [𝐼]𝐶𝑀 𝐶/𝑇 𝐴𝑅 is a diagonal matrix.

107
 Therefore, given a prescribed angular acceleration, the necessary forces and mo-
ments that the Chaser must produce are easily determined. Combining these results
with Equation 5.12 permits the ability to determine the values of 𝑆𝜃 and 𝑆𝑧 . This re-
lationship also illustrates that 𝐹𝑟 is not restricted by the angular momentum balance
of the Target-Chaser combined system. Therefore, 𝐹𝑟 can supply all of the necessary
force on the Chaser in the 𝑒^𝑟 direction, implying that 𝑇 = 0 for detumbling.

In order to determine 𝑀𝑥 , the angular momentum balance of the Chaser is con-

structed.
⃗˙ + (⃗𝜔 × [𝐼]𝐶 𝜔
[𝐼]𝐶 𝜔 ⃗ ) = (𝑀𝑥 + 𝑀𝐵,𝑟 )^
𝑒𝑟 + 𝑀𝐵,𝜃 𝑒^𝜃 + 𝑀𝐵,𝑧 𝑒^𝑧 (5.16)

⎡ ⎤ ⎡ ⎤
⎢ 𝑀𝑥 + 𝑀𝐵,𝑟 ⎥ ⎢ 𝐼𝐶,𝑟𝑟 𝜔˙ 𝑟 + (𝐼𝐶,𝑧𝑧 − 𝐼𝐶,𝜃𝜃 )𝜔𝜃 𝜔𝑧 ⎥
⎢ ⎥ ⎢ ⎥
𝑀𝐵,𝜃 = 𝐼𝐶,𝜃𝜃 𝜔˙ 𝜃 + (𝐼𝐶,𝑟𝑟 − 𝐼𝐶,𝑧𝑧 )𝜔𝑟 𝜔𝑧 ⎥ (5.17)
⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢
⎢ ⎥ ⎢ ⎥
⎣ ⎦ ⎣ ⎦
𝑀𝐵,𝑧 𝐼𝐶,𝑧𝑧 𝜔˙ 𝑧 + (𝐼𝐶,𝜃𝜃 − 𝐼𝐶,𝑟𝑟 )𝜔𝑟 𝜔𝜃

The variable [𝐼]𝐶 is the Chaser’s inertia matrix. The variable 𝑀𝐵,𝜃 is the bending
moment at the docking interface about 𝑒^𝜃 ; 𝑀𝐵,𝑧 is the bending moment at the docking
interface about 𝑒^𝑧 . Torsion about the docking axis is represented by 𝑀𝐵,𝑟 . Lastly,
𝑀𝑥 is the applied moment that occurs about the Chaser’s CM that acts about the 𝑒^𝑟
axis. Thus, the angular momentum balance of the Chaser provides the expressions
for the torsion and bending moments that act at the docking port interface.

This exercise in analyzing the dynamics of the aggregated system and the Chaser
(cut away from the Target) has provided the relationships between the loads and
moments imparted on the Target. There are three internal loads (𝑇 , 𝑆𝜃 , 𝑆𝑧 ) and
three moments (𝑀𝐵,𝑟 , 𝑀𝐵,𝜃 , 𝑀𝐵,𝑧 ). Expressions for the internal loads are derived
from Equations 5.12 and 5.14. As mentioned previously, the internal tension is merely

𝑇 =0 (5.18)

since the applied force 𝐹𝑟 does not show up in the system’s angular momentum
balance. An expression for 𝐹𝜃 and 𝐹𝑧 in terms of inertias and angular velocities and

108
accelerations fall out of Equation 5.14. Applying these expressions to 5.12 yields the
following.
(︃ )︃
1
𝑆𝜃 = 𝜔𝑟 𝜔𝜃 (𝑚𝑟𝑟 − (𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝜃𝜃 − 𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝑟𝑟 )) + ...
𝜌
(︃ )︃ (5.19)
1
𝜔˙ 𝑧 (𝑚𝑟𝑟 − 𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝑧𝑧 ) − 𝑚(𝑟𝜃 (𝜔𝑟2 + 𝜔𝑧2 ) − 𝑟𝑧 (𝜔𝜃 𝜔𝑧 − 𝜔˙ 𝑟 ))
𝜌

1
𝑆𝑧 = 𝜔𝑟 𝜔𝑧 (𝑚𝑟𝑟 + (𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝑟𝑟 − 𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝑧𝑧 )) + ...
𝜌
(5.20)
1
𝜔˙ 𝜃 ( (𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝜃𝜃 − 𝑚𝑟𝑟 ) + 𝑚(𝑟𝜃 (𝜔𝜃 𝜔𝑧 + 𝜔˙ 𝑟 ) − 𝑟𝑧 (𝜔𝑟2 + 𝜔𝜃2 ))
𝜌

Therefore, the internal loads during the detumbling procedure are expressed by Equa-
tions 5.18, 5.19, and 5.20.

Now, expressions for the internal moments are determined from the previous dy-
namics analysis. From Equation 5.17, expressions for 𝑀𝐵,𝜃 and 𝑀𝐵,𝑧 are clear.

𝑀𝐵,𝜃 = 𝐼𝐶,𝜃𝜃 𝜔˙ 𝜃 + (𝐼𝐶,𝑟𝑟 − 𝐼𝐶,𝑧𝑧 )𝜔𝑟 𝜔𝑧 (5.21)

𝑀𝐵,𝑧 = 𝐼𝐶,𝑧𝑧 𝜔˙ 𝑧 + (𝐼𝐶,𝜃𝜃 − 𝐼𝐶,𝑟𝑟 )𝜔𝑟 𝜔𝜃 (5.22)

Equation 5.14 yields the necessary expression for 𝑀𝑥 that simplifies the relationship
between 𝑀𝐵,𝑟 and the angular velocities and angular accelerations.

𝑀𝐵,𝑟 = 𝐼𝐶,𝑟𝑟 𝜔˙ 𝑟 + (𝐼𝐶,𝑧𝑧 − 𝐼𝐶,𝜃𝜃 )𝜔𝜃 𝜔𝑧 − 𝑀𝑥 (5.23a)

𝑀𝐵,𝑟 = 𝐼𝐶,𝑟𝑟 𝜔˙ 𝑟 + (𝐼𝐶,𝑧𝑧 − 𝐼𝐶,𝜃𝜃 )𝜔𝜃 𝜔𝑧 − ...

(𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝑟𝑟 𝜔˙ 𝑟 + (𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝑧𝑧 − 𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝜃𝜃 )𝜔𝜃 𝜔𝑧 ) (5.23b)

𝑀𝐵,𝑟 = (𝐼𝐶,𝑟𝑟 − 𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝑟𝑟 )𝜔˙ 𝑟 + ...

(𝐼𝐶,𝑧𝑧 − 𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝑧𝑧 − 𝐼𝐶,𝜃𝜃 + 𝐼𝐶𝑀 𝐶/𝑇 𝐴𝑅,𝜃𝜃 )𝜔𝜃 𝜔𝑧 (5.23c)

Therefore, the internal moments during the detumbling procedure are expressed by
Equations 5.21, 5.22, and 5.23c.

109
5.3 Discrete Solver for Docked Dynamics
The discrete solver takes as inputs the maximum time allotted for the maneuver,
the desired profile of the angular acceleration time history, a struct containing the
Chaser’s parameters, and a struct containing the Target’s parameters. The Chaser
and Target structs are constructed by separate functions before the discrete solver
is called. Copies of these functions are in Appendix C.2, Listings C.3 and C.4. The
functions that setup the Chaser and Target structs define important lengths of
the spacecraft with respect to their centers of mass, define the principal axis inertia
matrix with origin at the center of mass, and set up the initial conditions of both
translation and attitude.
Given these inputs, the discrete solver enters a while loop that it can only exit once
the magnitude of every element of the angular velocity is less than a certain tolerance
(tol = 0.001). A time step occurs at each iteration of the loop. If the maximum
time allotted is exceeded, the loop is exited and deemed unsuccessful. Otherwise, the
time span that actually occurred is recorded and outputted to the user, as well as are
the time histories of all the states and their derivatives.
During each iteration of the loop, the solver executes the following procedure:

1. Determine the current angular acceleration of the docked Chaser-Target system

based on the previous iteration’s angular velocity.

𝐼𝑦𝑦 − 𝐼𝑧𝑧 ˙ ˙
𝜑¨ = 𝜃𝜓 (5.24)
𝐼𝑥𝑥
𝐼𝑧𝑧 − 𝐼𝑥𝑥 ˙ ˙
𝜃¨ = 𝜑𝜓 (5.25)
𝐼𝑦𝑦
𝐼𝑥𝑥 − 𝐼𝑦𝑦 ˙ ˙
𝜓¨ = 𝜑𝜃 (5.26)
𝐼𝑧𝑧

2. Determine if the Target has the potential to be nutating.

(a) If the Target may be nutating, the solver counteracts the roll angular
velocity first (before dealing with the pitch and yaw angular velocities) by
applying the functional form of the angular acceleration (user-defined) in

110
 the roll direction. Once the roll angular velocity has magnitude less than
the solver’s tolerance, the solver will counteract the pitch and yaw angular
velocities in the same manner.

(b) If the Target cannot be nutating, the solver counteracts the angular ve-
locity in all three directions simultaneously via the user-defined functional
form of the angular acceleration.

3. If the Target has any translational acceleration, the solver counteracts it using
a constant acceleration functional form.

4. The solver takes the change in the three rotation angles from the previous it-
eration and finds the quaternion from the last iteration using a call to quat2dcm
and transposing it. This DCM is transposed and allocated as the CurrentB2IRot
for the current iteration. This matrix is R𝐵2𝐼 mentioned in Section 5.2. Thus,
this step updates the rotation matrix relating the body frame to the inertial
frame.

5. To determine what translational acceleration the Chaser needs to maintain to

stay synchronous with the Target, the solver determines the centripetal and
angular acceleration terms based on the current angular velocity and angular
acceleration of the Target (and Chaser). Since this stage of the mission starts
at when the spacecraft are already docked, the linear and Coriolis accelera-
tion terms are nonzero and ignored. The equations for centripetal and angular
acceleration are respectively

⃗ 𝐵 × (⃗𝜔𝐵 × ⃗𝑟𝐵 )
⃗𝑎𝑐𝑒𝑛𝑡 = 𝜔 (5.27)

⃗ 𝐵 × ⃗𝑟𝐵
⃗𝑎𝑎𝑛𝑔 = 𝛼 (5.28)

6. The centripetal and angular accelerations are rotated into the inertial frame us-
ing CurrentB2IRot and summed. These inertial accelerations are also summed
with the desired translational deceleration from Step 3.

111
 7. The solver calculates the angular momentum balance of the Target with respect
to the aggregated system and solves for 𝑀𝑥 , 𝐹𝑧 , and 𝐹𝜃 to be used in determining
the imparted loads and moments on the Target (see Equation 5.14).

8. Now that the solver has determined the effect to the translation and angu-
lar accelerations, it numerically integrates to solve for translation and angular
velocity and position for the current iteration.

9. The current iteration ends and the exit conditions in the while loop are checked.

This discrete solver is shown in its complete form in Appendix C.5.

5.4 Detumble Optimization

This section will cover the two optimization methodologies that were employed for
determining the optimal angular acceleration time history to bring the Target to
standstill and the results from the optimization processes. Both optimization methods
had an objective function of

∫︁ 𝑡𝑓 (︂ √︁ )︂
𝐽= 𝑆𝜃 (𝑡) + 𝑆𝑧 (𝑡) + 𝑀𝐵,𝑟 (𝑡) + 𝑀𝐵,𝜃 (𝑡) + 𝑀𝐵,𝑧 (𝑡) + 𝜑¨2 (𝑡) + 𝜃¨2 (𝑡) + 𝜓¨2 (𝑡) dt
0
(5.29)
where each term is equally weighted. It is worth noting that each load and moment
term can either be positive or negative, implying some magnitude cancellations may
occur depending on the nature of the Target’s tumble. Future work could include re-
fining this objective value to use absolute values of each load and moment. Moreover,
an objective function that seeks to minimize the maximum loads and moments of the
detumble would be a worthwhile case to investigate for fragile Targets.
Both methods, which are discussed in detail in the following sections, minimized
the objective function by optimizing the angular acceleration profiles. However, the
first method achieved this through an indirect approach while the second method
accomplished this directly.

112
5.4.1 Indirect Optimization on Angular Acceleration Profiles

The indirect optimization of the angular acceleration profiles minimized Equation

5.29 by optimizing the desired detumble time interval (Δ𝑡𝑑𝑒𝑠 ), which acted as an
essential input for the angular acceleration profiles for each run. Thus, the objective
function for the indirect optimization is shown by Equation 5.30.

∫︁ 𝑡𝑓
min 𝐽 = ( 𝑆𝜃 (𝑡) + 𝑆𝑧 (𝑡) + 𝑀𝐵,𝑟 (𝑡) + 𝑀𝐵,𝜃 (𝑡) + 𝑀𝐵,𝑧 (𝑡) + ...
Δ𝑡𝑑𝑒𝑠 0
√︁
¨ Δ𝑡𝑑𝑒𝑠 )2 + 𝜃(𝑡,
𝜑(𝑡, ¨ Δ𝑡𝑑𝑒𝑠 )2 + 𝜓(𝑡,
¨ Δ𝑡𝑑𝑒𝑠 )2 ) dt (5.30)

Four functional forms of the angular acceleration profiles were utilized in the
optimization and the results were compared against one another to determine which
provided the lowest cost. A description of these functional forms in provided in the
subsequent section.
This indirect optimization process used the MATLAB function fmincon. The
discrete solver previously described was the function input for the call to fmincon.
Thus, the optimization iteratively called the discrete solver whilst changing (Δ𝑡𝑑𝑒𝑠 ) to
minimize the objective function. For the sake of re-creation, the optimization settings
are detailed in Table 5.2. The following section describes the angular acceleration
functional forms.

Table 5.2: fmincon optimization options used in the indirect optimization of angular
acceleration profiles

Algorithm TolFun MaxFunEvals MaxIter TolX TolCon

sqp 1e-16 1e6 1e7 1e-16 1e-16

Angular Acceleration Functional Forms

In forming this optimization problem, angular acceleration functional forms were first
selected. Simple functions for the four desired forms were implemented in the discrete
solver function. These forms are denoted as Constant, Positive Linear, Negative

113
Linear, and Negative Quadratic. These functions determine the change in angular
acceleration that the Chaser needs to provide to counteract the Target’s angular
velocity. The effective time history in angular velocity that these functional forms
provide is qualitatively illustrated in Figures 5-5a-5-5d. In these figures, 𝜔𝑖𝑛𝑖𝑡𝑖𝑎𝑙 is the
initial angular velocity, 𝑡𝑤𝑎𝑖𝑡 is a user-defined variable indicating a desired time to
wait before starting the detumble procedure, and Δ𝑡𝑑𝑒𝑠𝑖𝑟𝑒𝑑 is the desired time for the
maneuver to complete. The angular acceleration functional forms are defined using
the given variables by Equations 5.31-5.34.

(a) Constant angular acceleration (b) Positive linear angular acceleration

(c) Negative linear angular acceleration (d) Negative quadratic angular acceler-
ation

Figure 5-5: Expected angular velocity time histories from integration of selected
angular acceleration functional forms

114
 𝜔
⃗ 𝑖𝑛𝑖𝑡𝑖𝑎𝑙
⃗˙ 𝑐𝑜𝑛𝑠𝑡𝑎𝑛𝑡 = −
𝜔 (5.31)
Δ𝑡𝑑𝑒𝑠𝑖𝑟𝑒𝑑
𝜔
⃗ 𝑖𝑛𝑖𝑡𝑖𝑎𝑙
⃗˙ 𝑃 𝐿 = 2
𝜔 (𝑡 − (Δ𝑡𝑑𝑒𝑠𝑖𝑟𝑒𝑑 + 𝑡𝑤𝑎𝑖𝑡 )) (5.32)
(Δ𝑡𝑑𝑒𝑠𝑖𝑟𝑒𝑑 )2
𝜔
⃗ 𝑖𝑛𝑖𝑡𝑖𝑎𝑙
⃗˙ 𝑁 𝐿 = −2
𝜔 (𝑡 − 𝑡𝑤𝑎𝑖𝑡 ) (5.33)
(Δ𝑡𝑑𝑒𝑠𝑖𝑟𝑒𝑑 )2
𝜔
⃗ 𝑖𝑛𝑖𝑡𝑖𝑎𝑙
⃗˙ 𝑁 𝑄 = −3
𝜔 3
(𝑡 − 𝑡𝑤𝑎𝑖𝑡 )2 (5.34)
(Δ𝑡𝑑𝑒𝑠𝑖𝑟𝑒𝑑 )

These additional angular acceleration profiles are solved for on each iteration
of the discrete solver. Which profile the solver uses is selected by the user – it is
an input to the function call of the discrete solver. The use of pre-defined angular
acceleration profiles distinguish this optimization method from the secondary method
and necessitates that the optimization occurs on the variable Δ𝑡𝑑𝑒𝑠 .

Results

Using the aforementioned objective function (Equation 5.29), the following nested
for loops was executed to produce the results in Table 5.3. The potential spacecraft
shapes were spherical, rectangular prism, or cylindrical. The pre-defined angular
velocities for each type of spin were: [0, 0, 5] 𝑑𝑒𝑔
𝑠
for flat spin, [2, 2, 2] 𝑑𝑒𝑔
𝑠
for 3D spin,
[1, 2, 3] 𝑑𝑒𝑔
𝑠
for nutation.

Algorithm 1 Method 1 Optimization Loop

1: procedure OptimizationLoop
2: shapeLen ← length of {shapes}
3: angVelLen ← length of angVelMatrix
4: deltaTLen ← length of deltaTArray
5: loop:
6: for q=1:shapeLen do
7: ShapeType = shapes{q}
8: for r=1:angVelLen do
9: angVel = angVelMatrix(:,r)
10: for j=1:deltaTLen do
11: deltaT = deltaTArray(j)
12: Execute optimization.

115
 Although two values were used in Algorithm 1 for Δ𝑡, the results presented in Ta-
ble 5.3 are the results from providing an initial guess of Δ𝑡 = 150𝑠. The optimization
routine outputted the optimal Δ𝑡 corresponding to the lowest cost. Only the results
for the initial guess of Δ𝑡 = 150𝑠 are displayed because the initial guess of Δ𝑡 = 5𝑠
occasionally produced a nonsensically large cost, suggesting an issue with the discrete
solver attempting to detumble the Target in such a short period of time.

Table 5.3: Method 1 optimization results of optimal Δ𝑡 and associated cost

Ang. Acceleration Form Δ𝑡[𝑠] Cost

Constant 56.14 -0.034616
Sphere: Positive Linear 5.00 -0.034595
flat spin Negative Linear 149.91 -0.034608
Negative Quadratic 150.00 -0.034620
Constant 148.35 -0.046129
Sphere: Positive Linear 5.00 -0.046126
3D spin Negative Linear 149.91 -0.046144
Negative Quadratic 150.00 -0.046160
Constant 483.00 0.0151
Rect. prism: Positive Linear 150.01 0.046790
flat spin Negative Linear 150.01 0.046774
Negative Quadratic 483.00 0.001457
Constant 381.84 0.0255
Rect. prism: Positive Linear 150.01 0.063002
3D spin Negative Linear 150.01 0.062967
Negative Quadratic 381.84 0.003932
Constant 13.41 0.0308
Rect. prism: Positive Linear 96.267 0.0150
nutation Negative Linear 62.10 0.301
Negative Quadratic 37.69 0.167418
Constant 483.00 0.00375
Cylinder: Positive Linear 150.01 0.0116
flat spin Negative Linear 483.00 0.00116
Negative Quadratic 483.00 0.00036
Constant 381.84 0.00632
Cylinder: Positive Linear 150.01 0.0156
3D spin Negative Linear 381.84 0.00248
Negative Quadratic 381.84 0.00097
Constant 38.92 0.0944
Cylinder: Positive Linear 82.91 -0.0205
nutation Negative Linear 26.98 0.0871
Negative Quadratic 43.29 0.1827

116
 In Table 5.3, the cost numerics nominally have three significant figures except
to distinguish the difference between costs for the same shape and spin. For the
spherical spacecraft, the positive linear angular acceleration functional form provides
the lowest cost with a Δ𝑡 = 5𝑠 for both the flat and 3D spins. For the rectangular
prism spacecraft, the negative quadratic angular acceleration functional form provided
the lowest cost for the flat and 3D spins (yielding the largest Δ𝑡 for both). However,
for the nutating spin, the positive linear form provided the lowest cost (half of the
second lowest cost). Similarly, for the cylindrical spacecraft, the negative quadratic
form yielded the lowest cost for the flat and 3D spins, and the positive linear form
yielded the lowest cost for the nutating spin. This data suggests that if a Target is
nutating, regardless of its shape, a positive linear angular acceleration profile is the
optimal profile form for minimizing Equation 5.29. Additionally, these results suggest
that for an axisymmetric spacecraft that is not nutating (i.e. it has a flat spin or a
general 3D spin), a negative quadratic angular acceleration profile is the optimal form
for minimizing Equation 5.29.

5.4.2 Direct Optimization Approach

GPOPS-II (General Purpose Optimal Control Software) was employed to produce

a direct optimization of the angular acceleration profile for minimizing the objec-
tive function in the subproblem of the Chaser detumbling the Target once already
docked. GPOPS-II is a user-friendly numerical optimization software that uses Gaus-
sian quadrature integration, works within MATLAB, and offers options for setting
up the problem (NLP solver, derivative supplier, scaling, etc) [68]. For this detumble
optimization, IPOPT was used with a tolerance of 10 × 10−7 [69].

117
Problem Formulation

This problem was constructed as one phase in GPOPS with the following mathemat-
ical form.

∫︁ 𝑡𝑓
min 𝐽 = ( 𝑆𝜃 (𝑡) + 𝑆𝑧 (𝑡) + 𝑀𝐵,𝑟 (𝑡) + 𝑀𝐵,𝜃 (𝑡) + 𝑀𝐵,𝑧 (𝑡) + ...
𝛼
⃗ (𝑡) 0
√︁ )︂
¨ 2 + 𝜃(𝑡)
𝜑(𝑡) ¨ 2 + 𝜓(𝑡)
¨ 2 dt (5.35)

where
⎡ ⎤
¨
⎢ 𝜑(𝑡) ⎥
⎢ ⎥
⎢ ¨
⃗ (𝑡) = ⎢ 𝜃(𝑡)
𝛼 (5.36)
⎥
⎥
⎢ ⎥
¨
⎣ ⎦
𝜓(𝑡)

subject to the dynamic constraints

⎡ ⎤
¨ ⎥
⎢ 𝑥
⎢ ⎥ (︁ )︁
⎢
⎢ 𝑦¨ ⎥
⎥
= R𝐵2𝐼 𝜔 ⃗˙ × ⃗𝑟𝐵
⃗ × (⃗𝜔 × ⃗𝑟𝐵 ) + 𝜔 (5.37)
⎢ ⎥
⎣ ⎦
𝑧¨

where ⃗𝑟𝐵 is the position vector from the Target’s CM to the Chaser’s CM in the body frame
⎡ ⎤
⎢ 𝑢𝜑 ⎥
⎢ ⎥
𝛼 ⃗˙ = ⎢
⃗ =𝜔 ⎢ 𝑢𝜃 ⎥
⎥
(5.38)
⎢ ⎥
⎣ ⎦
𝑢𝜓
⎡ ⎤ ⎡ ⎤
𝑞𝑤 ⎥ ˙ 𝑥
− 12 (𝜑𝑞 + 𝜃𝑞˙ 𝑦 + 𝜓𝑞˙ 𝑧)
⎢ ⎢ ⎥
⎢ ⎥ ⎢ ⎥
1 ˙ ˙ ˙
𝑑
⎢
𝑞𝑥 ⎥ ⎢
( 𝜑𝑞 𝑤 + 𝜃𝑞 𝑧 − 𝜓𝑞 𝑦 ) ⎥
⃗𝑞˙ = 2
⎢ ⎥ ⎢ ⎥
⎢ ⎥ = ⎢ ⎥ (5.39)
𝑑𝑡 ⎢
⎢
⎥
𝑞𝑦 ⎥
⎢ 1
˙ 𝑧
⎢ (−𝜑𝑞 ˙ 𝑤 + 𝜓𝑞
+ 𝜃𝑞 ˙ 𝑥) ⎥
⎥
⎢ ⎥ ⎢ 2 ⎥
⎣ ⎦ ⎣ ⎦
1 ˙ ˙ 𝑥 + 𝜓𝑞
˙ 𝑤)
𝑞𝑧 2
(𝜑𝑞𝑦 − 𝜃𝑞
⎡ ⎤
2 2 2 2
⎢ 𝑞𝑤 + 𝑞𝑥 − 𝑞𝑦 − 𝑞𝑧 2(𝑞𝑥 𝑞𝑦 − 𝑞𝑧 𝑞𝑤 ) 2(𝑞𝑥 𝑞𝑧 + 𝑞𝑦 𝑞𝑤 ) ⎥
⎢ ⎥
R𝐵2𝐼 = 2(𝑞𝑥 𝑞𝑦 + 𝑞𝑧 𝑞𝑤 ) 𝑞𝑤2 − 𝑞𝑥2 + 𝑞𝑦2 − 𝑞𝑧2 2(𝑞𝑦 𝑞𝑧 − 𝑞𝑥 𝑞𝑤 ) (5.40)
⎢ ⎥
⎢ ⎥
⎢ ⎥
⎣ ⎦
2(𝑞𝑥 𝑞𝑧 − 𝑞𝑦 𝑞𝑤 ) 2(𝑞𝑦 𝑞𝑧 + 𝑞𝑥 𝑞𝑤 ) 𝑞𝑤2 − 𝑞𝑥2 − 𝑞𝑦2 + 𝑞𝑧2

118
subject to the inequality path constraints

√︁
𝜁 − 1 × 10−4 ≤ 𝑥(𝑡)2 + 𝑦(𝑡)2 + 𝑧(𝑡)2 ≤ 𝜁 + 1 × 10−4 (5.41)
√︁
𝜁= 𝑥(0)2 + 𝑦(0)2 + 𝑧(0)2

In order to mimic the construction of the indirect optimization process, the control
was constrained to be [−0.007, −0.007, −0.007] ≤ ⃗𝑢 ≤ [0.007, 0.007, 0.007] rad
𝑠2
.

Results

Table 5.4 indicates the resultant Δ𝑡 and cost from the direct optimization process. In
this instance, the cost is calculated via cumtrapz in order to parallel the computation
from the indirect method. GPOPS-II was not able to converge to a solution for the 3D
spin of the spherical spacecraft. The hypothesized root cause is that the optimization
was not able to determine which axis to prioritize detumbling since there was no
difference in the axes’s inertia ratios or in the spin rate. Therefore, the optimization
ends having only exceeded its iteration limits rather than finding the optimal solution.

Table 5.4: Method 2 optimization results of optimal Δ𝑡 and associated cost

Spacecraft Shape Spin Δ𝑡 [𝑠] Cost

flat spin 14.807 -0.0518
Spherical 3D spin N/A N/A
nutation N/A N/A
flat spin 14.230 0.382
Rectangular
3D spin 34.217 -0.00455
Prism
nutation 20.721 0.000930
flat spin 14.317 0.330
Cylindrical 3D spin 29.875 -0.0521
nutation 20.564 -0.0351

For the 3D spin, the rectangular prism and cylindrical spacecraft are initialized
with an angular velocity vector [2, 2, 2]𝑇 deg
s
. The spacecraft have the same dimensions
and mass, but their moments of inertias are intrinsically different. Such a difference
accounts for the slight variation in the control time history, illustrated in Figures 5-6a
and 5-6b, as well as the time duration and associated cost.

119
 (a) Rectangular prism spacecraft (b) Cylindrical spacecraft

Figure 5-6: Angular acceleration profiles as axisymmetric spacecraft initially rotate

about all three axes

For the three spacecraft – spherical, rectangular prism, cylindrical – the control
effort about the 𝜓 axis follows a similar pattern over the time duration. The control 𝑢𝜓
initially starts at the minimum −0.007, increases in the positive direction (decreases
in negative magnitude), and returns to the minimum −0.007 around 𝑡 = 4 seconds.
The control history for the 𝜑 and 𝜃 axes of the rectangular prism and cylindrical
spacecraft are nearly identical, varying slightly at a handful of time steps (see Figures
5-7b and 5-7c). This is expected since they are both axisymmetric and equivalent in
mass and dimensions, yet they vary in their moment of inertia. Contrastingly, the
spherical spacecraft is trisymmetric, and the control history for 𝜑 and 𝜃 are noticeably
different than that of its counterparts (see Figure 5-7a).
While not identical, the control history results of the rectangular prism and cylin-
drical spacecraft are similar. Both are initialized with an angular velocity vector of
[1, 2, 3]𝑇 deg
s
. Similar to the 3D spin, the difference in the control time history is due
to the variation in the inertia ratios, displayed in Figures 5-8a and 5-8b, as well as
the time duration and cost.

5.4.3 Comparison of Optimization Results

Generally, the optimal Δ𝑡 from the direct optimization was smaller across the board
than that yielded from the indirect optimization. However, the minimal cost from
the indirect optimization method was nominally on the order of magnitude of 10−2 or

120
 (a) Spherical spacecraft (b) Rectangular prism spacecraft

(c) Cylindrical spacecraft

Figure 5-7: Angular acceleration profiles as spacecraft initially rotate about 𝜓-axis

(a) Rectangular prism spacecraft (b) Cylindrical spacecraft

Figure 5-8: Angular acceleration profiles as axisymmetric spacecraft initially nutate

smaller (23 out of 24 results) whereas the direct optimization yielded five out of seven
results with a cost 10−2 or smaller. This suggests that the indirect optimization can
provide an optimal or near-optimal angular acceleration profile for less computation
time. Additionally, this suggests that the patterns recognized from the indirect opti-
mization’s yielded results would be applicable as a near-optimal angular acceleration

121
profile – no longer necessitating an optimization to occur in-flight. To further vali-
date this point, more functional forms of the angular acceleration should be tested
using the indirect optimization method and more experimental spacecraft should be
utilized for comparison.

122
Chapter 6

Conclusions and Future Work

6.1 Summary of Thesis

In summary, the work in this thesis proposes solutions to subproblems that constitute
the overall problem of autonomous rendezvous and docking with a tumbling and un-
cooperative Target spacecraft. These subproblems include the soft-docking terminal
approach when the Chaser is within 10 meters of the Target, an optimized detumble
procedure of the Target once the Chaser is docked to it so the aggregated system
can maneuver jointly, and handling uncertain knowledge with respect to both the
Chaser and Target spacecraft. The goal is for all the solutions and software presented
herein to be integrated with other aspects of the ROAM project, creating a cohesive
software framework for in-space close proximity operations. Such a framework would
assist in the technological progress towards fully autonomous rendezvous and docking
procedures for satellite servicing, active debris removal, and in-space assembly.
In Chapter 1, the interest in autonomous rendezvous and docking for satellite
servicing, active debris removal, and in-space assembly was motivated through a dis-
cussion of the avenues of science and exploration fully autonomy of each reference
mission would yield. Satellite servicing would elongate the lifetime of valuable assets
in orbit and facilitate further expansion outward from Earth by providing infrastruc-
ture for spacecraft repair and maintenance. Active debris removal is necessary to
diminish the effects of the Kessler Syndrome and allow uninhibited access to low-

123
Earth orbit. In-space assembly would encourage science missions that require space
telescopes than what can reasonably be stowed in a payload fairing of a launch vehicle
and develop orbiting stations like the ISS. Moreover, autonomous in-space assembly
would allow for such developments in remote locations unaccessible by astronauts.
Chapter 1 also covered a discussion of the current status of the SPHERES testbed,
as that motivated the work outlined in Chapter 4. Additionally, the ROAM project
is briefly described, as it was the impetus for the research detailed in Chapters 3 and
5. The research objective was formulated as as contributing towards the feasibility of
regular and repeatable in-space close proximity missions where a Chaser spacecraft
is interacting with an uncooperative, tumbling Target.

In Chapter 2, a literature review was discussed on trajectory optimization for a

Chaser approaching a tumbling Target, nonlinear control methodologies relevant for
handling uncertainties arising from inertial properties knowledge or actuation failure,
and existing literature regarding soft docking and stabilizing a tumbling spacecraft.
Several trajectory optimization techniques exist that satisfy minimum time, minimum
fuel, or thruster characterizations that are primarily solved offline, although progress
has occurred with respect to rapid path-planning algorithms. Popular nonlinear con-
trol methods include sliding mode control, which simplifies an n𝑡ℎ -order system to
1𝑠𝑡 -order, and adaptive control, notably model reference adaptive control where the
user designs the frequency domain characteristics the system should achieve. A few
methods for soft docking to a Target were discussed in addition to existing means for
detumbling a spacecraft. Much of the existing literature focuses on interfacing with
cooperative Targets. The research areas highlighting uncooperative Targets have not
performed hardware validation tests – a necessary implementation to increase the
technological readiness levels and advance towards autonomous in-space close prox-
imity operations.

In Chapter 3, observations were constructed regarding a potential trajectory-

generating differential equation that Sternberg proposed in [38] and a propagator
was developed to support implementing that differential equation in flight. An ar-
gument was constructed disproving that second-order differential equation proposed

124
by Sternberg that was hypothesized based on evidence that a fuel-optimal radially
synchronous trajectory of a Chaser approaching a Target was one in which the mag-
nitudes of radial and tangential acceleration components were equal. While that
conclusion may be true, a vectorized differential equation that equates the radial
and tangential acceleration components is not valid as shown by the simple flat spin
case. Additionally, it was shown that a trajectory produced from solving this equa-
tion with ode45 yielded an outward bound path. Before this conclusion was drawn,
a software framework for propagating this differential equation was developed in C
code for use on SPHERES. Thus, this chapter details the execution sequence of var-
ious critical points in order to produce a series of waypoints that encapsulate the
trajectory for a Chaser to follow. This propagator software can be applied to any
trajectory-generating differential equation.

In Chapter 4, the state of thruster degradation of the ISS SPHERES free-flyers

was analyzed; an in-flight adaptive PID sliding mode controller was developed and
validated for use in scenarios of uncertainty due to thruster degradation or uncertainty
in inertia properties. With respect to the thruster degradation issue, the thruster
forces of Blue and Orange were characterized from a single thruster dance, which
yielded concerning results. Thus, new mixers were developed for each satellite to
accommodate the failing thrusters and closed-loop tests were designed to measure
their performance. In the maintenance session (Test Session 101), Blue, Orange,
and Red all performed more thruster characterization tests and their new mixers
yielded promising results. Each satellite has at least one seriously crippled thruster,
hindering the controllability of the system. Due to the issues the satellites had been
experiencing during docking test sessions, an in-flight adaptive PID sliding mode
controller was designed and validated in 3-DOF simulation and ground testing that is
robust to uncertainty in mass properties knowledge and is a potential solution to the
thruster degradation issue. This controller should be expanded to 6-DOF and tested
on station to evaluate if it is a feasible way to overcome the actuator failures.

In Chapter 5, Stage 3 of the ROAM mission was modeled as the Chaser already
docked with the Target. This modeling included the fleshed out development of the

125
contact dynamics that occur while docking. This is of particular interest for the sce-
nario in which the Target is fragile, since it is undesirable to break the Target up
by applying too much force or torque. This fragility is not only a concern for the
contact dynamics but also when the Chaser detumbles the Target to bring it to a
standstill. The Chaser maintains synchronous motion about the Target’s center of
mass once it has docked, but it exerts an angular acceleration in order to bring the
aggregated standstill. These dynamics were setup in a discrete solver in MATLAB.
Two optimization methods were employed to optimize the angular acceleration profile
in order to minimize a cost function of fuel consumption and the loads and moments
imparted on the Target. Results indicated that the indirect optimization method
produced near-optimal results relative to the direct optimization process. These re-
sults suggested that further offline optimizations for different angular acceleration
functional forms and characteristic spacecraft inertias would provide a useful lookup
table for providing near-optimal in-flight solutions.

6.2 Contributions

This section briefly lists the contributions presented in this thesis, as follows:

• Correction regarding matched acceleration differential equation with respect to

in-flight trajectory optimization from [38]

• Software framework for handling a trajectory-generating differential equation

in C code (specifically developed with SPHERES’s core software in mind)

• A fleshed-out timeline and sequence of executions for a trajectory-generating

differential equation

• MATLAB functions to quantify thruster force from SPHERES’s IMU data by

smoothing, locating impulses, employing user verification, and outputting mea-
sured force versus expected force

126
 • Closed-loop unit tests for validating updated mixers designed with respect to
specific thruster degradation

• Analysis regarding effectiveness of new mixers and state of thruster degradation

of Blue, Orange, and Red SPHERES onboard the ISS

• A validated 3-DOF (with straightforward extension to 6-DOF) in-flight, adap-

tive PID sliding mode controller for SPHERES that is robust to uncertainty in
inertial properties knowledge

• Implementation of contact dynamics from [67] that output the impulsive force
and torque a tumbling Target experiences during docking

• A (MATLAB-coded) discrete solver of a docked Chaser-Target system tumbling

synchronously about the Target’s center of mass with a user-defined input to
detumble the system

• An optimization methodology that indirectly optimizes the angular acceleration

profile by optimizing the desired time interval of the detumble procedure in order
to minimize an objective function of fuel consumption and imparted loads and
moments on the Target via looping over three different functional forms of the
angular acceleration profile

• An optimization methodology that directly optimizes the angular acceleration

profile in order to minimize an objective function of fuel consumption and im-
parted loads and moments on the Target using direct collocation via GPOPS

6.3 Recommendations for Future Work

This section lists recommended areas that may be extensions of the work presented
in this thesis:

• Invest more time in investigating the relationship Sternberg was onto regarding
tangential and radial acceleration magnitudes being equivalent for fuel-optimal
R-bar approaches

127
• Implement and validate the trajectory-generating differential equation propa-
gator on SPHERES

• Design and implement sliding mode control methods on SPHERES on station

to overcome the thruster degradation

• Employ techniques from underactuated robotics for in-flight trajectory opti-

mization for SPHERES under thruster failure

• Validate the 6-DOF in-flight adaptive PID sliding mode controller in simulation
and then on SPHERES onboard the ISS

• Complete Monte Carlo simulations of the in-flight adaptive PID sliding mode
controller to evaluate how robustly it performs

• Test more general functional forms of angular acceleration profiles for the indi-
rect optimization of the detumbling procedure between a docked Chaser-Target
system

• Create a multi-objective optimization of Δ𝑉 and imparted loads and moments

on the Target as a function of the angular acceleration profile

• From further testing, develop a lookup table of near-optimal angular accelera-

tion profiles that can be validated in simulation testing and in-flight

128
Appendix A

Trajectory Optimization

A.1 Fuel-Optimal Trajectory Optimization Func-

tions

Listing A.1: TrajectoryOptimizer_v1.m

1 % This script is my re-creation of Sternberg's optimization ...
script he
2 % described in his ScD thesis. It performs the following:
3 % 1) An optimization to find optimal delta time spacing for a ...
fuel-optimal
4 % approach. This yields an optimal time history associated with a
5 % logarithmically-spaced radial distribution.
6 % 2) Fits a parameterized approximation to the fuel optimal ...
trajectory from
7 % number 1
8 % 3) Finds the 2-term Fuel Optimal Exponential Trajectory
9 % 4) Fitting the exponential optimal trajectory!
10
11 %% Set-up
12 clc;
13 close all;
14
15 %% Initialize cells and struct arrays
16 fit_error_exponential = [];
17 computational_time_exponential = [];
18 computational_DV_exponential = [];
19 run_num = 1;
20 run_num_exponential = 1;
21 RESULTS_exponential = [];
22 optimal_time_history = [];
23 optimal_radial_distribution = [];
24

25 %% Set variables
26 num_dt = 100; % Number of time steps
27 NUM_TERMS = 6; % Number of terms to use in function fitting

129
28 omega = 5/norm([0 0 1])*[0 0 1]'; % Initial angular velocity of ...
Target
29 r0 = 10; % Initial radial distance between Chaser and Target
30 rf = 1; % Final radial distance between Chaser and Target
31 inertia_ratio = [1 1 1]'; % Diagonal of inertia matrix
32 fit_type = {'exponential'}; % Options: ...
{'polynomial','exponential','logistic'}
33 %Tend = 33; % Final times for optimization
34 Tend = [1 20 33 55 240];
35 for j=1:length(Tend)
36 %% 1. Find the Fuel Optimal Trajectory
37
38 % Setup for fmincon call
39 A = ones(num_dt);
40 b = Tend(j)*ones(num_dt,1);
41 Aeq = ones(1,num_dt);
42 beq = Tend(j);
43 lb = zeros(num_dt,1);
44 ub = Tend(j)*ones(num_dt,1);
45

46 % Initial guess = uniform time step

47 x0 = Tend(j)/num_dt * ones(num_dt,1);
48 % fmincon options
49 tol = 1e-8;
50 options = optimoptions(@fmincon,'OptimalityTolerance',1e-3,...
51 'MaxFunctionEvaluations', 1e6, 'MaxIterations',1e7,...
52 'Algorithm','sqp','StepTolerance',tol);
53 % Objective function for fmincon
54 f = @(dt_list)DeltaV_dt(dt_list,omega,r0,rf,inertia_ratio);
55 [dt_out, DV_out,exitflag,output] = fmincon(f,x0,A,b,Aeq,...
56 beq,lb,ub,[],options);
57 disp(['Exitflag: ',num2str(exitflag)]);
58 disp(['Minimum DV=',num2str(DV_out),'m/s'])
59
60 % -----Optimal Time History-----
61 t_out = zeros(length(dt_out),1);
62 for i = 1:length(dt_out)
63 t_out(i+1) = t_out(i) + dt_out(i);
64 end
65
66 % -----Radial Distribution-----
67 r = logspace(log10(r0),log10(rf),length(t_out)); r(end) = rf;
68
69 figure();
70 plot(t_out,r,'-*b');
71 title({'Optimal Radial Distance ...
Profile';['\DeltaV=',num2str(DV_out),'m/s']})
72 xlabel('Time [s]')
73 ylabel('Radius [m]')
74

75 optimal_time_history = t_out;
76 optimal_radial_distribution = r;
77

130
 78 DV_computed = ...
DeltaV_dt_r_omega0(dt_out,optimal_radial_distribution,...
79 omega,inertia_ratio,1)
80 % Create struct
81 DV_computed_struct.dt_out(:,j) = dt_out;
82 DV_computed_struct.DV(j) = DV_computed;
83 DV_computed_struct.DV_out(j) = DV_out;
84 DV_computed_struct.Tend(j) = Tend(j);
85 DV_computed_struct.Alin(:,:,j) = Alin;
86 DV_computed_struct.Acen(:,:,j) = Acen;
87 DV_computed_struct.Acor(:,:,j) = Acor;
88 DV_computed_struct.Aang(:,:,j) = Aang;
89 end
90 save('DV_computed_final.mat','DV_computed_struct');
91 keyboard
92 % Currently, the below code is not designed with the for loop, so ...
it will
93 % just use the last optimal radial distribution and time ...
distribution.
94 %% 2. Fit a parameterized approximation to the fuel optimal ...
trajectory
95 [fcnfit_opt,fit_error,B_list]=fit_param_approx(...
96 optimal_radial_distribution, optimal_time_history, r0, rf,...
97 fit_type,NUM_TERMS, num_dt);
98 DV_fit1 = ...
DeltaV_dt_r_omega0(dt_out,fcnfit_opt(2,:),omega,inertia_ratio,1)
99
100 %% 3. Find the 2-term Fuel Optimal Exponential Trajectory
101 t_vec = linspace(0,Tend,num_dt+1);
102 dt = diff(t_vec);
103
104 % Nonlinear constrainted optimization function with argument b - ...
the coefficients.
105 nonlcon = @(b)TrajectoryOptimizerVariableStudy_nonlconfunc_B(...
106 t_vec,b,r0,rf,'exponential');
107
108 scaling_vec = [1; 1; 1; 1];
109

110 % f is the objective function

111 f = @(exp_coefficients)DeltaV_dt(dt,omega,r0,rf,inertia_ratio,...
112 exp_coefficients./scaling_vec);
113 x0 = B_list{2}; % selecting results from step 2 as initial guess
114
115 opts = optimoptions(@fmincon,'Algorithm','interior-point');
116 problem = createOptimProblem('fmincon','objective',f,...
117 'x0',x0,'Aeq',repmat([1,0],1,2),'beq',r0,'lb',[],...
118 'ub',[],'nonlcon',nonlcon,'options',opts);
119 gs = GlobalSearch;
120 [exp_coeff_out,DV_out2] = run(gs,problem);
121 % exp_coeff_out is the value yielding DV_out (global minimum)
122
123 disp(['Minimum DV=',num2str(DV_out2),'m/s'])
124
125 t_out = zeros(length(dt),1);

131
126 for i = 1:length(dt)
127 t_out(i+1) = t_out(i) + dt(i);
128 end
129
130 % Calculates radial profile of Chaser by time history done for ...
optimization
131 % of the coefficients - 2-term exponential
132 r = exp_coeff_out(1)*exp(-exp_coeff_out(2)*t_out) + ...
133 exp_coeff_out(3)*exp(-exp_coeff_out(4)*t_out);
134
135 x = t_out; % Re-naming variable
136 yx = r; % Re-naming variable; this is the optimized exp ...
trajectory
137 % This is the 100 points along optimal 2-term exponential trajectory
138 figure();
139 plot(t_out,r);
140 title({'Optimal Radial Distance ...
Profile';['\DeltaV=',num2str(DV_out2),'m/s']})
141 xlabel('Time [s]')
142 ylabel('Radius [m]')
143
144 % This is finding the deltaV from the optimal exp trajectory
145 DeltaV_Optimal = DeltaV_dt_r_omega0(dt,yx',omega,inertia_ratio,1);
146
147 %% 4. Fitting the exponential optimal trajectory!
148 [fcnfit_exp_opt,fit_error,B_list]=fit_param_approx(yx', x, r0, ...
rf, fit_type,NUM_TERMS, num_dt);
149
150 num_terms = NUM_TERMS;
151 for i = 1:num_terms
152 switch i
153 case 1
154 fit_coeffs = B_list{1}(:,end);
155 r_fit = fit_coeffs(1).*exp(-fit_coeffs(2).*x);
156 case 2
157 fit_coeffs = B_list{2}(:,end);
158 r_fit = fit_coeffs(1).*exp(-fit_coeffs(2).*x) + ...
fit_coeffs(3).*exp(-fit_coeffs(4).*x);
159 case 3
160 fit_coeffs = B_list{3}(:,end);
161 r_fit = fit_coeffs(1).*exp(-fit_coeffs(2).*x) + ...
fit_coeffs(3).*exp(-fit_coeffs(4).*x) + ...
fit_coeffs(5).*exp(-fit_coeffs(6).*x);
162 case 4
163 fit_coeffs = B_list{4}(:,end);
164 r_fit = fit_coeffs(1).*exp(-fit_coeffs(2).*x) + ...
fit_coeffs(3).*exp(-fit_coeffs(4).*x) + ...
fit_coeffs(5).*exp(-fit_coeffs(6).*x) +...
165 fit_coeffs(7).*exp(-fit_coeffs(8).*x);
166 case 5
167 fit_coeffs = B_list{5}(:,end);
168 r_fit = fit_coeffs(1).*exp(-fit_coeffs(2).*x) + ...
fit_coeffs(3).*exp(-fit_coeffs(4).*x) + ...
fit_coeffs(5).*exp(-fit_coeffs(6).*x) +...

132
169 fit_coeffs(7).*exp(-fit_coeffs(8).*x) + ...
fit_coeffs(9).*exp(-fit_coeffs(10).*x);
170 case 6
171 fit_coeffs = B_list{6}(:,end);
172 r_fit = fit_coeffs(1).*exp(-fit_coeffs(2).*x) + ...
fit_coeffs(3).*exp(-fit_coeffs(4).*x) + ...
fit_coeffs(5).*exp(-fit_coeffs(6).*x) +...
173 fit_coeffs(7).*exp(-fit_coeffs(8).*x) + ...
fit_coeffs(9).*exp(-fit_coeffs(10).*x) + ...
fit_coeffs(11).*exp(-fit_coeffs(12).*x);
174 end
175
176 R_FIT2(:,i) = r_fit;
177 DeltaV_Fit(i) = ...
DeltaV_dt_r_omega0(dt,r_fit',omega,inertia_ratio,0);
178
179 end
180
181 DeltaV_Fit_Normalized = DeltaV_Fit/DeltaV_Optimal; % optimal exp ...
trajectory
182
183 % Deducing that 2-term is fit, plot radial profile using two-terms
184 figure()
185 plot(x,R_FIT2(:,2));
186 grid on;
187 xlabel('Time [s]')
188 ylabel('Radius [m]')
189 title('Radial Distance of Exponential Fits')
190
191 figure()
192 plot(DeltaV_Fit_Normalized);
193 grid on;
194 xlabel('Number of Terms')
195 ylabel('Normalized \DeltaV')
196 title('Fit \DeltaV Normalized by Optimal \DeltaV')
197
198 DeltaV_dt_r_omega0(dt,R_FIT2(:,2)',omega,inertia_ratio,1)
199

200 %% Plot all 4

201 figure
202 plot(optimal_time_history,optimal_radial_distribution,'-*b'); ...
hold on;
203 plot(optimal_time_history,fcnfit_opt(2,:),'-ok');
204 plot(t_out, yx,'-.g');
205 plot(t_out, fcnfit_exp_opt(2,:),'--r'); grid on;
206 xlabel('Time [s]','FontSize',18);
207 ylabel('Radius [m]','FontSize',18);
208 legend({'100-Point Optimal Trajectory','2-Term Fit of ...
Optimal','100 Points Along Optimal 2-Term Exponential ...
Trajectory',...
209 '2-Term Fit to 2-Term Exponential Trajectory'},'FontSize',12);

133
 Listing A.2: DeltaV_dt.m
1 function DeltaV = ...
DeltaV_dt(dt_list,omega,r0,rf,inertia_ratio,exp_coefficients)
2 % This function is used in optimizations to determine the optimal ...
dt_list
3
4 if nargin < 6
5 work_with_exponentials = 0;
6 else
7 work_with_exponentials = 1;
8 end
9
10 %Clear the data vectors
11 r_SVC_INT = [r0; 0; 0]; % [m]
12 r_SVC_TAR = r_SVC_INT; % A convenient assumption
13

14 I_TAR = diag(inertia_ratio);
15 omega_TAR_INT = deg2rad(omega); % rate of target in inertial frame
16
17 %linear velocity of servicer relative to static target in ...
inertial frame
18 v_SVC_INT = fast_cross(omega_TAR_INT,r_SVC_INT);
19 % Make sure this is consistent with other deltaV function
20
21 q_TAR_INT = [0; 0; 0; 1]; %quaternion from inertial frame to ...
target frame
22 q_UDP_TAR = [0; 0; 0; 1]; %quaternion from target frame to ...
inertial frame
23
24 % Initialize variables
25 initialize_zeros
26 num_iter = length(dt_list);
27
28 % create the time history
29 for i=1:num_iter
30 t(i+1) = t(i) + dt_list(i);
31 end
32
33 % Define radial distribution
34 if work_with_exponentials
35 rad_dist = exp_coefficients(1)*exp(-exp_coefficients(2)*t) + ...
36 exp_coefficients(3)*exp(-exp_coefficients(4)*t);
37 else % i.e. for full optimization
38 rad_dist = logspace(log10(r0),log10(rf),num_iter+1);
39 end
40 r = r0;
41
42 % Propagate the radial approach
43 for i = 1:num_iter
44 if r(i) < rf
45 break;
46 end
47

134
48 dt = dt_list(i);
49 tumbling_dynamics
50
51 % Calculate accelerations
52 acc_DV_calculations
53
54 % Normalize r to compare against rf
55 r(i+1) = norm(r_SVC_TAR(:,i+1));
56
57 t(i+1) = t(i) + dt;
58 end % end loop of simulating time until end of spiral
59
60 assignin('base','funcT',t)
61 assignin('base','funcR',rad_dist);
62
63 DV_TOT = DV_total(end);
64
65 DeltaV = DV_TOT;
66
67 end

A.2 Synchronous Radial Approach Propagator

Listing A.3: propagator.c

1 /∗
2 ∗ propagator . c
3 ∗
4 ∗ SPHERES Guest S c i e n t i s t Program custom s o u r c e code t e m p l a t e .
5 ∗
6 ∗ MIT Space Systems L a b o r a t o r y
7 ∗ SPHERES Guest S c i e n t i s t Program
8 ∗ h t t p : / / s s l . mit . edu / s p h e r e s /
9 ∗
10 ∗ Copyright 2018 M a s s a c h u s e t t s I n s t i t u t e o f Technology
11 ∗
12 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
13 ∗ Pro paga tor F u n c t i o n s ∗
14 ∗ By : H a i l e e H e t t r i c k ∗
15 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
16 ∗ Last m o d i f i e d : 06/17/18 ∗
17 ∗ ∗
18 ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗
19 ∗/
20
21 #i n c l u d e " propagator . h"
22 #i n c l u d e " spheres_constants . h"
23 #i n c l u d e " spheres_physical_parameters . h"
24 #i n c l u d e " math_quat . h "
25 #i n c l u d e " math_matrix . h "

135
26 #i n c l u d e <math . h>
27

28 #i f d e f SPH_MATLAB_SIM
29 #i n c l u d e " . . / S i m u l a t i o n / v a s S i m u l a t o r . h "
30 #i n c l u d e " mex . h "
31 #d e f i n e DEBUG( a r g ) m ex Pri nt f a r g
32 #e l s e
33 #d e f i n e DEBUG( a r g )
34 #e n d i f
35
36 // Real i n f o
37 f l o a t I_TAR [ 3 ] [ 3 ] = { { 0 . 0 2 8 8 5 6 0 9 3 5 8 3 8 8 3 0 0 0 f , - 0 . 0 0 0 2 7 1 7 6 0 4 2 6 0 2 9 4 2 1 f ...
, 0.001061424114444470 f } ,
38 { - 0 . 0 0 0 2 7 1 7 6 0 4 2 6 0 2 9 4 2 1 f , 0 . 0 5 5 9 8 6 6 0 2 5 4 4 4 4 9 5 0 0 f , ...
0.000183704942897512 f } ,
39 { 0 . 0 0 1 0 6 1 4 2 4 1 1 4 4 4 4 4 7 0 f , 0 . 0 0 0 1 8 3 7 0 4 9 4 2 8 9 7 5 1 2 f , ...
0.058822298355749900 f }};
40 // TODO: c a l c u l a t e t h i s i n v e r s e i n f u n c t i o n
41 f l o a t inv_I_TAR [ 3 ] [ 3 ] = { { 3 4 . 6 7 9 3 6 7 7 9 5 3 4 7 2 1 1 f , 0 . 1 7 0 3 8 9 6 1 1 6 0 6 0 6 6 f , ...
-0.626307024637702 f } ,
42 { 0.170389611606066 f , 17.862436235713918 f , -0.058859880828906 f } ,
43 { - 0 . 6 2 6 3 0 7 0 2 4 6 3 7 7 0 2 f , - 0 . 0 5 8 8 5 9 8 8 0 8 2 8 9 0 6 f , 1 7 . 0 1 1 8 4 1 0 5 6 9 0 8 0 8 6 f ...
}};
44
45 // a n g u l a r a c c e l e r a t i o n i n i t i a l
46 f l o a t omega_dot_TAR_INT[ 3 ] = { 0 . 0 f , 0 . 0 f , 0 . 0 f } ;
47
48 // q_dot i n i t i a l
49 f l o a t q_dot_TAR_INT[ 4 ] = { 1 . 0 f , 0 . 0 f , 0 . 0 f , 0 . 0 f } ;
50
51 float I_by_omega [ 3 ] ;
52 float new_omega_TAR_INT [ 3 ] ;
53 float new_omega_dot_TAR_INT [ 3 ] ;
54 float new_q_TAR_INT [ 4 ] ;
55 float new_q_dot_TAR_INT [ 4 ] ;
56 float int_new_q_dot_TAR_INT [ 4 ] ;
57 float cross_prod [ 3 ] ;
58 float int_q_dot [ 4 ] ;
59 float q_dot_TAR_INT [ 4 ] ;
60 float quat_term [ 4 ] ;
61 float quat_term3 [ 3 ] ;
62 float quat_term_vec_norm ;
63 float temp ;
64 float quat_exponential_term [ 4 ] ;
65 float quat_exp_sin [ 3 ] ;
66 float q_TAR_INT_conj [ 4 ] ;
67 float new_q_TAR_INT_conj [ 4 ] ;
68 float array_with_zero [ 4 ] ;
69 float dt_setup = - 1 . 0 f ;
70

71 // Back p r o p a g a t e
72 f l o a t rdot_SVC_TAR [ 3 ] = { 0 . 0 1 f , 0 . 0 f , 0 . 0 f } ; // s l o w a r r i v a l
73 float B[ 3 ] [ 3 ] ;
74 float C[ 3 ] [ 3 ] ;

136
 75 i n t i , j , k , l ,m;
76 float A[ 6 ] [ 6 ] ;
77 float x [6];
78 f l o a t drdot [ 3 ] ;
79 f l o a t dxdt [ 6 ] ;
80 f l o a t new_r_SVC_TAR [ 3 ] ;
81 f l o a t new_rdot_SVC_TAR [ 3 ] ;
82 f l o a t new_r_SVC_INT [ 3 ] ;
83 f l o a t new_rdot_SVC_INT [ 3 ] ;
84 int counter ;
85 f l o a t q_UDP_INT [ 4 ] ;
86 float G[ 3 ] [ 3 ] ;
87 f l o a t dotproduct ;
88 f l o a t crossproduct ;
89 f l o a t temp_radial_profile [ 2 0 0 0 ] [ 3 ] ;
90 f l o a t x_factor ;
91 i n t non_zero =0;
92
93 /∗ f o r quat check ∗/
94 f l o a t z_rot [ 4 ] = { 0 . 0 f , 0 . 0 f , - 1 . 0 f , 0 . 0 f } ;
95 f l o a t checkTargetQuat [ 4 ] ;
96 f l o a t compWDesired ;
97 // f l o a t old_compWDesired = 0 . 0 f ;
98
99 /∗ ∗∗∗∗∗∗ Check i f q u a t e r n i o n i s a l i g n e d ∗∗∗∗∗∗ ∗/
100 void quaternionCheck ( f l o a t ∗ c t r l S t a t e , f l o a t ∗ tgtState , i n t f l a g ) {
101 // I s Chaser a l m o s t a l o n g r - bar o f Target ? I f not , i t w a i t s .
102 quatMult_noNorm ( checkTargetQuat , &c t r l S t a t e [QUAT_1] , z_rot ) ;
103
104 // Compare checkTargetQuat with t g t S t a t e [QUAT_2]
105 compWDesired = checkTargetQuat [ 1 ] - t g t S t a t e [QUAT_2 ] ; // TODO: ...
update t h i s s o t h a t i t i s f l e x i b l e f o r a x i s r - bar i s a l o n g
106
107 i f ( ( compWDesired < 0 . 0 5 f ) && ( compWDesired > - 0 . 0 5 f ) ) {
108 flag = 1;
109 }
110 }
111

112
113 /∗ ∗∗∗∗∗∗ Forward Propagate f o r TAR ' s q , qdot , omega , omegadot i n f o ...
∗∗∗∗∗∗ ∗/
114 v o i d f o r w a r d P r o p a g a t e ( f l o a t ∗ t g t S t a t e , f l o a t q_TAR_INT_array...
[ 2 0 0 0 ] [ 4 ] , f l o a t omega_TAR_INT_array [ 2 0 0 0 ] [ 3 ] , f l o a t ...
q_dot_TAR_INT_array [ 2 0 0 0 ] [ 4 ] , f l o a t omega_dot_TAR_INT_array...
[2000][3]){
115 // Forward p r o p a g a t e q_TAR_INT and omega_TAR_INT a s p e c i f i e d ...
number o f time c o n s t a n t s t o d e t e r m i n e when backwards ...
propagation should s t a r t
116 // I n p u t s : c u r r e n t Target ' s q u a t e r n i o n and a n g u l a r r a t e . I n e r t i a ...
from e s t i m a t i o n p r o c e s s .
117 // Outputs : f u t u r e q u a t e r n i o n and a n g u l a r r a t e s f o r s p e c i f i e d ...
number o f p e r i o d s
118
119 // r o t a t i o n r a t e

137
120 f l o a t omega_TAR_INT [ 3 ] ;
121 memcpy(&omega_TAR_INT [ 0 ] , &t g t S t a t e [RATE_X] , s i z e o f ( f l o a t ) ∗ 3 ) ; ...
// [ rad / s ]
122 omega_TAR_INT_array [ 0 ] [ 0 ] = omega_TAR_INT [ 0 ] ;
123 omega_TAR_INT_array [ 0 ] [ 1 ] = omega_TAR_INT [ 1 ] ;
124 omega_TAR_INT_array [ 0 ] [ 2 ] = omega_TAR_INT [ 2 ] ;
125
126 f l o a t q_TAR_INT [ 4 ] ;
127 memcpy(&q_TAR_INT [ 0 ] , &t g t S t a t e [QUAT_1] , s i z e o f ( f l o a t ) ∗ 3 ) ;
128 q_TAR_INT_array [ 0 ] [ 0 ] = q_TAR_INT [ 0 ] ;
129 q_TAR_INT_array [ 0 ] [ 1 ] = q_TAR_INT [ 1 ] ;
130 q_TAR_INT_array [ 0 ] [ 2 ] = q_TAR_INT [ 2 ] ;
131 q_TAR_INT_array [ 0 ] [ 3 ] = q_TAR_INT [ 3 ] ;
132

133 f o r ( i =0; i <3; i ++){

134 omega_dot_TAR_INT_array [ 0 ] [ i ] = omega_dot_TAR_INT [ i ] ;
135 }
136
137 f o r ( i =0; i <4; i ++){
138 q_dot_TAR_INT_array [ 0 ] [ i ] = q_dot_TAR_INT [ i ] ;
139 }
140
141 /∗ Determine p e r i o d o f r o t a t i o n ∗/
142 f l o a t T, T1 , T2 , T3 ;
143 T1 = abs ( 2 . 0 f ∗ 3 . 1 4 f /omega_TAR_INT [ 0 ] ) ;
144 T2 = abs ( 2 . 0 f ∗ 3 . 1 4 f /omega_TAR_INT [ 1 ] ) ;
145 T3 = abs ( 2 . 0 f ∗ 3 . 1 4 f /omega_TAR_INT [ 2 ] ) ; // [ s ] time p e r i o d o f ...
rotation
146
147 i f (T1 < T2 && T1 < T3) {
148 T = T1 ;
149 } e l s e i f (T2 < T1 && T2 < T3) {
150 T = T2 ;
151 } e l s e i f (T3 < T1 && T3 < T2) {
152 T = T3 ;
153 }
154
155 f l o a t n_T ;
156 n_T = 0 . 9 0 f ∗T ;
157
158 // M and tau w i l l be i n p u t s e v e n t u a l l y
159 f l o a t M_TAR[ 3 ] = { 0 . 0 f , 0 . 0 f , 0 . 0 f } ;
160 f l o a t tau_TAR [ 3 ] = { 0 . 0 f , 0 . 0 f , 0 . 0 f } ;
161 f l o a t dt = - dt_setup ;
162 f l o a t t = 0.0 f ;
163
164 int c = 1;
165
166 w h i l e ( t < n_T) {
167 // C l e a r memory
168 memset(&I_by_omega , 0 , s i z e o f ( f l o a t ) ∗ 3 ) ;
169 memset(&new_omega_dot_TAR_INT , 0 , s i z e o f ( f l o a t ) ∗ 3 ) ;
170 memset(&new_q_dot_TAR_INT, 0 , s i z e o f ( f l o a t ) ∗ 4 ) ;
171 memset(&new_q_TAR_INT, 0 , s i z e o f ( f l o a t ) ∗ 4 ) ;

138
172 memset(&new_omega_TAR_INT, 0 , s i z e o f ( f l o a t ) ∗ 3 ) ;
173

174 /∗ C o n s t r u c t omegadot e q u a t i o n : omegadot = I ^ ( - 1 ) ( - omega c r o s s ...

( I ∗omega + M) + tau ) ∗/
175 f o r ( i =0; i <3; i ++){
176 f o r ( j =0; j <3; j ++){
177 I_by_omega [ i ] += I_TAR [ i ] [ j ] ∗ omega_TAR_INT [ j ] ; // I ∗omega
178 }
179 I_by_omega [ i ] += M_TAR[ i ] ; // I ∗omega + M
180 }
181
182 c r o s s _ p r o d [ 0 ] = -omega_TAR_INT [ 1 ] ∗ I_by_omega [ 2 ] +omega_TAR_INT...
[ 2 ] ∗ I_by_omega [ 1 ] + tau_TAR [ 0 ] ; // - omega c r o s s ( I ∗omega + M) + ...
tau
183 c r o s s _ p r o d [ 1 ] = -omega_TAR_INT [ 2 ] ∗ I_by_omega [ 0 ] +omega_TAR_INT...
[ 0 ] ∗ I_by_omega [ 2 ] + tau_TAR [ 1 ] ;
184 c r o s s _ p r o d [ 2 ] = -omega_TAR_INT [ 0 ] ∗ I_by_omega [ 1 ] +omega_TAR_INT...
[ 1 ] ∗ I_by_omega [ 0 ] + tau_TAR [ 2 ] ;
185
186 f o r ( i =0; i <3; i ++){
187 f o r ( j =0; j <3; j ++){
188 new_omega_dot_TAR_INT [ i ] += inv_I_TAR [ i ] [ j ] ∗ c r o s s _ p r o d [ j ] ; ...
// omegadot = I ^ ( - 1 ) ( - omega c r o s s ( I ∗omega + M) + tau )
189 }
190 }
191

192 //New omega v a l u e u s i n g omegadot e q u a t i o n

193 f o r ( i =0; i <3; i ++){
194 new_omega_TAR_INT [ i ] = omega_TAR_INT [ i ] + omega_dot_TAR_INT [ ...
i ] ∗ ( dt ) ;
195 }
196

197 /∗ C o n s t r u c t qdot e q u a t i o n ∗/
198 f o r ( i =0; i <3; i ++){
199 array_with_zero [ i ] = omega_TAR_INT [ i ] ;
200 }
201 array_with_zero [ 3 ] = 0 . 0 f ;
202 quatMult_noNorm ( int_q_dot , q_TAR_INT, array_with_zero ) ;
203
204 f o r ( i =0; i <4; i ++){
205 new_q_dot_TAR_INT [ i ] = 0 . 5 f ∗ int_q_dot [ i ] ;
206 int_new_q_dot_TAR_INT [ i ] = ( dt ) ∗new_q_dot_TAR_INT [ i ] ;
207 }
208

209 f o r ( i =0; i <3; i ++){

210 q_TAR_INT_conj [ i ] = -q_TAR_INT[ i ] ;
211 }
212 q_TAR_INT_conj [ 3 ] = q_TAR_INT [ 3 ] ;
213 quatMult_noNorm ( quat_term , q_TAR_INT_conj , int_new_q_dot_TAR_INT...
);
214
215 f o r ( i =0; i <3; i ++){
216 quat_term3 [ i ]= quat_term [ i ] ;
217 }

139
218
219 // Normalize quat_term3
220 temp = 0 ;
221 f o r ( j =0; j <3; j ++){
222 temp += quat_term3 [ j ] ∗ quat_term3 [ j ] ;
223 }
224 quat_term_vec_norm = s q r t f ( temp ) ;
225

226 f o r ( k=0; k <3; k++){

227 quat_exp_sin [ k ] = quat_term3 [ k ] ∗ s i n ( quat_term_vec_norm ) /...
quat_term_vec_norm ;
228 quat_exponential_term [ k ] = exp ( quat_term [ 3 ] ) ∗ quat_exp_sin [ k...
];
229 }
230 quat_exponential_term [ 3 ] = exp ( quat_term [ 3 ] ) ∗ c o s ( ...
quat_term_vec_norm ) ;
231 quatMult_noNorm (new_q_TAR_INT, q_TAR_INT, quat_exponential_term ) ...
;
232
233 f o r ( i =0; i <3; i ++){
234 omega_TAR_INT_array [ c ] [ i ] = new_omega_TAR_INT [ i ] ;
235 }
236
237 f o r ( i =0; i <4; i ++){
238 q_TAR_INT_array [ c ] [ i ] = new_q_TAR_INT [ i ] ;
239 }
240
241 f o r ( i =0; i <3; i ++){
242 omega_dot_TAR_INT_array [ c ] [ i ] = new_omega_dot_TAR_INT [ i ] ;
243 }
244
245 f o r ( i =0; i <4; i ++){
246 q_dot_TAR_INT_array [ c ] [ i ] = new_q_dot_TAR_INT [ i ] ;
247 }
248
249 memcpy(&q_TAR_INT [ 0 ] , &new_q_TAR_INT [ 0 ] , s i z e o f ( f l o a t ) ∗ 4 ) ;
250 memcpy(&omega_TAR_INT [ 0 ] , &new_omega_TAR_INT [ 0 ] , s i z e o f ( f l o a t ) ...
∗3) ;
251 memcpy(&omega_dot_TAR_INT [ 0 ] , &new_omega_dot_TAR_INT [ 0 ] , ...
s i z e o f ( f l o a t ) ∗3) ;
252 memcpy(&q_dot_TAR_INT [ 0 ] , &new_q_dot_TAR_INT [ 0 ] , s i z e o f ( f l o a t ) ...
∗4) ;
253
254 t = t + dt ;
255 c++; // c o u n t e r f o r a r r a y s
256 }
257 }
258
259 /∗ ∗∗∗∗∗∗ Back Propagate t o TAR ' s i n i t i a l p o s i t i o n ∗∗∗∗∗∗ ∗/
260 v o i d propagateToTarget ( f l o a t ∗ c t r l S t a t e , f l o a t ∗ t g t S t a t e , f l o a t ...
r a d i a l _ p r o f i l e [ 2 0 0 0 ] [ 3 ] , f l o a t v e l o c i t y _ p r o f i l e [ 2 0 0 0 ] [ 3 ] , f l o a t ...
q_TAR_INT_array [ 2 0 0 0 ] [ 4 ] , f l o a t omega_TAR_INT_array [ 2 0 0 0 ] [ 3 ] , ...
f l o a t q_dot_TAR_INT_array [ 2 0 0 0 ] [ 4 ] , f l o a t ...
omega_dot_TAR_INT_array [ 2 0 0 0 ] [ 3 ] ) {

140
261
262 // This s h o u l d o c c u r a f t e r e v e r y e s t i m a t i o n s t a g e .
263 // I n i t i a l l y , s e t u p d o e s not p r o v i d e e s t i m a t i o n knowledge o f ...
Target s o we ' l l u s e t r u e knowledge f i r s t
264 // Output : P r o f i l e f o r Chaser t o f o l l o w (r_SVC_INT)
265
266 // Trim a r r a y s down
267 f o r ( i =0; i <1999; i ++){
268 i f ( ( q_TAR_INT_array [ i ] [ 0 ] ! = 0 ) | | (q_TAR_INT_array [ i ] [ 1 ] ! = 0 ) ...
| | (q_TAR_INT_array [ i ] [ 2 ] ! = 0 ) | | (q_TAR_INT_array [ i ] [ 3 ] ! = 0 ) ) {
269 non_zero++;
270 }
271 }
272 i n t i d = non_zero - 1 ;
273
274 f l o a t dt = dt_setup ;
275
276 // r _ i n i t i a l = Chaser ' s c u r r e n t p o s i t i o n o f f s e t by t h e t a r g e t ' s ...
position
277 float r_initial [3];
278 f o r ( i =0; i <3; i ++){
279 r_initial [ i ] = ctrlState [ i ] - tgtState [ i ] ;
280 }
281
282 f l o a t q_TAR_INT [ 4 ] ;
283 memcpy(&q_TAR_INT [ 0 ] , &q_TAR_INT_array [ i d ] [ 0 ] , s i z e o f ( f l o a t ) ∗ 4 ) ;
284
285 // r o t a t i o n r a t e
286 f l o a t omega_TAR_INT [ 3 ] ;
287
288 // DCM t r a n s f o r m a t i o n f o r d e s i r e d o f f s e t from Target ' s CG
289 f l o a t dcm_init [ 3 ] [ 3 ] ;
290 dcm_from_quat ( dcm_init , q_TAR_INT) ;
291 f l o a t r_SVC_INT [ 3 ] ;
292 f l o a t r_SVC_TAR [ 3 ] ;
293 f l o a t vec [ 3 ] = { 0 . 4 f , 0 . 0 f , 0 . 0 f } ; // r _ s t a n d f o f
294 f o r ( i =0; i <3; i ++){
295 r_SVC_INT [ i ] = 0 . 0 f ;
296 f o r ( j =0; j <3; j ++){
297 r_SVC_INT [ i ] += dcm_init [ i ] [ j ] ∗ vec [ j ] ;
298 }
299 }
300 vecRotbyQuat (q_TAR_INT,r_SVC_TAR, r_SVC_INT) ;
301

302 // q u a t e r n i o n from s t a t e v e c t o r i s r o t a t i o n from g l o b a l frame t o ...

body frame
303 f l o a t q_UDP_TAR[ 4 ] = { 0 , 0 , 0 , 1 } ; // q u a t e r n i o n from UDP frame...
t o Target frame - o b s e r v e d i n e s t i m a t i o n
304
305 // r ( i +1) = norm (r_SVC_TAR( : , i +1) ) ;
306 f l o a t r_temp = 0 ;
307 f l o a t r0_temp = 0 ;
308 float r ;
309 f o r ( i =0; i <3; i ++){

141
310 r_temp += r_SVC_INT [ i ] ∗ r_SVC_INT [ i ] ;
311 r0_temp += r _ i n i t i a l [ i ] ∗ r _ i n i t i a l [ i ] ; // aka s t o p back ...
p r o p a g a t i o n once you g e t t o Chaser ' s c u r r e n t p o s i t i o n
312 }
313 r = s q r t f ( r_temp ) ;
314 f l o a t r0 ;
315 r 0 = s q r t f ( r0_temp ) ;
316

317 // This w i l l be t h e Chaser ' s f i n a l move towards Target

318 f o r ( i =0; i <3; i ++){
319 r a d i a l _ p r o f i l e [ 1 9 9 9 ] [ i ] = r_SVC_INT [ i ] ;
320 }
321
322 // Weird attempt t o g e t S t e r n b e r g ' s e q u a t i o n t o work
323 f l o a t norm_rate = s q r t f ( omega_TAR_INT_array [ i d ] [ 0 ] ∗ ...
omega_TAR_INT_array [ i d ] [ 0 ] + omega_TAR_INT_array [ i d ] [ 1 ] ∗ ...
omega_TAR_INT_array [ i d ] [ 1 ] + omega_TAR_INT_array [ i d ] [ 2 ] ∗ ...
omega_TAR_INT_array [ i d ] [ 2 ] ) ;
324 f l o a t r a t e = norm_rate ∗ 1 8 0 . 0 f / 3 . 1 4 f ;
325 x_factor = 0.1∗ rate /5;
326
327 counter = 0;
328 w h i l e ( ( r < r 0 ) && ( c o u n t e r < 2 0 0 0 ) ) {
329
330 // quatMult_noNorm (q_UDP_INT,q_UDP_TAR,q_TAR_INT) ; // r e t u r n s ...
q_UDP_INT
331 // quatMult_noNorm (q_UDP_INT, q_TAR_INT,q_UDP_TAR) ;
332 //q_SVC_INT = quatConj (q_UDP_INT) ; // math_quat . c
333
334 // C l e a r memory
335 memset(&dxdt , 0 , s i z e o f ( f l o a t ) ∗ 6 ) ;
336 memset(&drdot , 0 , s i z e o f ( f l o a t ) ∗ 3 ) ;
337 memset(&new_r_SVC_INT , 0 , s i z e o f ( f l o a t ) ∗ 3 ) ;
338 memset(&new_r_SVC_TAR, 0 , s i z e o f ( f l o a t ) ∗ 3 ) ;
339 memset(&new_rdot_SVC_TAR , 0 , s i z e o f ( f l o a t ) ∗ 3 ) ;
340
341 // Update Target ' s o r i e n t a t i o n i n f o r m a t i o n
342 f o r ( i =0; i <3; i ++){
343 omega_TAR_INT [ i ] = omega_TAR_INT_array [ id - c o u n t e r ] [ i ] ;
344 omega_dot_TAR_INT [ i ] = omega_dot_TAR_INT_array [ id - c o u n t e r ] [ i ...
];
345 }
346
347 f o r ( i =0; i <4; i ++){
348 q_TAR_INT[ i ] = q_TAR_INT_array [ id - c o u n t e r ] [ i ] ;
349 new_q_TAR_INT [ i ] = q_TAR_INT_array [ id - c o u n t e r - 1 ] [ i ] ;
350 q_dot_TAR_INT [ i ] = q_dot_TAR_INT_array [ id - c o u n t e r ] [ i ] ;
351 }
352
353 // Now, s e t up A, B, C m a t r i c e s
354 f l o a t w_skew [ 3 ] [ 3 ] = {
355 { 0 , -omega_TAR_INT [ 2 ] , omega_TAR_INT [ 1 ] } ,
356 {omega_TAR_INT [ 2 ] , 0 , -omega_TAR_INT [ 0 ] } ,
357 { -omega_TAR_INT [ 1 ] , omega_TAR_INT [ 0 ] , 0}

142
358 };
359 f l o a t wdot_skew [ 3 ] [ 3 ] = {
360 { 0 , -omega_dot_TAR_INT [ 2 ] , omega_dot_TAR_INT [ 1 ] } ,
361 {omega_dot_TAR_INT [ 2 ] , 0 , -omega_dot_TAR_INT [ 0 ] } ,
362 { -omega_dot_TAR_INT [ 1 ] , omega_dot_TAR_INT [ 0 ] , 0}
363 };
364
365 f o r ( i =0; i <3; i ++){
366 f o r ( j =0; j <3; j ++){
367 B [ i ] [ j ] = 2∗w_skew [ i ] [ j ] ;
368 C [ i ] [ j ] = ( - wdot_skew [ i ] [ j ] + w_skew [ i ] [ j ] ∗ w_skew [ i ] [ j ] ) /...
x_factor ;
369 }
370 }
371 f o r ( k=0; k <3; k++){
372 f o r ( l =0; l <3; l ++){
373 A[ k ] [ l ]= 0 ;
374 }
375 f o r (m=3; m<6; m++){
376 A[ k ] [m] = 1 ;
377 }
378 }
379 f o r ( k=3; k <6; k++){
380 f o r ( l =0; l <3; l ++){
381 A[ k ] [ l ]= C [ k - 3 ] [ l ] ;
382 }
383 f o r (m=3; m<6; m++){
384 A[ k ] [m] = B [ k - 3 ] [ m- 3 ] ;
385 }
386 }
387
388 f o r ( i =0; i <3; i ++){
389 x [ i ] = r_SVC_TAR[ i ] ;
390 x [ i +3] = rdot_SVC_TAR [ i ] ;
391 }
392
393 // dxdt = A∗x ;
394 f o r ( j =0; j <6; j ++){
395 f o r ( k=0; k <6; k++){
396 dxdt [ j ] += A[ j ] [ k ] ∗ x [ k ] ;
397 }
398 }
399
400 f o r ( i =0; i <3; i ++){
401 d r d o t [ i ] = dxdt [ i + 3 ] ;
402 }
403
404 //new_r_SVC_TAR = r_SVC_TAR + d r d o t ∗ dt
405 //rdot_SVC_TAR ( : , i +1) = (r_SVC_TAR( : , i ) - r_SVC_TAR( : , i +1) ) / ...
( dt ) ;
406 i =0;
407 f o r ( i =0; i <3; i ++){
408 new_r_SVC_TAR [ i ] = r_SVC_TAR[ i ] + d r d o t [ i ] ∗ ( dt ) ;

143
409 new_rdot_SVC_TAR [ i ] = (r_SVC_TAR[ i ] - new_r_SVC_TAR [ i ] ) / ( dt ) ...
;
410 }
411
412 f o r ( i =0; i <3; i ++){
413 new_q_TAR_INT_conj [ i ] = -new_q_TAR_INT [ i ] ;
414 }
415 new_q_TAR_INT_conj [ 3 ] = new_q_TAR_INT [ 3 ] ;
416
417 //r_SVC_INT ( : , i +1) = t r a n s _ v e c ( conj_quat (q_TAR_INT ( : , i +1) ) , ...
r_SVC_TAR( : , i +1) ) ;
418 vecRotbyQuat (new_q_TAR_INT_conj , new_r_SVC_INT , new_r_SVC_TAR) ;
419 vecRotbyQuat (new_q_TAR_INT_conj , new_rdot_SVC_INT ,
420 new_rdot_SVC_TAR) ;
421
422 c o u n t e r ++;
423 // TODO: add t g t S t a t e back h e r e i f no r a d i a l s h i f t i s r e q u i r e d
424 f o r ( i =0; i <3; i ++){
425 r a d i a l _ p r o f i l e [ 1 9 9 9 - c o u n t e r ] [ i ] = new_r_SVC_INT [ i ] ;
426 v e l o c i t y _ p r o f i l e [ 1 9 9 9 - c o u n t e r ] [ i ] = new_rdot_SVC_INT [ i ] ;
427 }
428
429 memcpy(&r_SVC_INT [ 0 ] , &new_r_SVC_INT [ 0 ] , s i z e o f ( f l o a t ) ∗ 3 ) ;
430 memcpy(&r_SVC_TAR [ 0 ] , &new_r_SVC_TAR [ 0 ] , s i z e o f ( f l o a t ) ∗ 3 ) ;
431 memcpy(&rdot_SVC_TAR [ 0 ] , &new_rdot_SVC_TAR [ 0 ] , s i z e o f ( f l o a t ) ...
∗3) ;
432
433 // r ( i +1) = norm (r_SVC_TAR( : , i +1) ) ;
434 f l o a t r_temp = 0 ;
435 f o r ( i =0; i <3; i ++){
436 r_temp += new_r_SVC_TAR [ i ] ∗new_r_SVC_TAR [ i ] ;
437 }
438 r = s q r t f ( r_temp ) ;
439 }
440
441
442 /∗ ∗∗∗∗∗∗∗∗∗ T r a j e c t o r y S h i f t ∗∗∗∗∗∗∗∗∗ ∗/
443 // TODO: Add e l s e s t a t e m e n t f o r i f s h i f t i s n ' t n e c e s s a r y but t o ...
add t g t S t a t e back i n
444 // 1999 - c o u n t e r e q u a l s CHASER ' s f i r s t s t e p . This n e e d s t o e q u a l ...
Chaser ' s c u r r e n t p o s i t i o n .
445 i f ( r a d i a l _ p r o f i l e [ 1 9 9 9 - c o u n t e r ] [ 0 ] != c t r l S t a t e [ 0 ] - t g t S t a t e [ 0 ] ) {...
// then f i n d r o t a t i o n matrix
446 // Make them u n i t v e c t o r s f i r s t
447 f l o a t rad_profile_mag = s q r t f ( r a d i a l _ p r o f i l e [ 1 9 9 9 - c o u n t e r ] [ 0 ] ∗
448 r a d i a l _ p r o f i l e [ 1 9 9 9 - c o u n t e r ] [ 0 ] + r a d i a l _ p r o f i l e [ 1 9 9 9 - ...
counter ] [ 1 ] ∗
449 r a d i a l _ p r o f i l e [ 1 9 9 9 - c o u n t e r ] [ 1 ] + r a d i a l _ p r o f i l e [ 1 9 9 9 - c o u n t e r ...
][2]∗
450 r a d i a l _ p r o f i l e [1999 - counter ] [ 2 ] ) ;
451 f l o a t unit_rad_profile [ 3 ] = { r a d i a l _ p r o f i l e [1999 - counter ] [ 0 ] /
452 rad_profile_mag , r a d i a l _ p r o f i l e [ 1 9 9 9 - c o u n t e r ] [ 1 ] / ...
rad_profile_mag ,
453 r a d i a l _ p r o f i l e [ 1 9 9 9 - c o u n t e r ] [ 2 ] / rad_profile_mag } ;

144
454 f l o a t ctrlState_mag = s q r t f ( ( c t r l S t a t e [ 0 ] ) ∗( c t r l S t a t e [ 0 ] ) +
455 ( c t r l S t a t e [ 1 ] ) ∗ ( c t r l S t a t e [ 1 ] ) + ( c t r l S t a t e [ 2 ] ) ∗ ( c t r l S t a t e ...
[2]) ) ;
456 f l o a t u n i t _ c t r l S t a t e [ 3 ] = { ( c t r l S t a t e [ 0 ] ) / ctrlState_mag ,
457 ( c t r l S t a t e [ 1 ] ) / ctrlState_mag , ( c t r l S t a t e [ 2 ] ) / c t r l S t a t e _ m a g } ;
458
459 // dot p r o d u c t
460 d o t p r o d u c t =0;
461 f o r ( i =0; i <3; i ++){
462 d o t p r o d u c t += u n i t _ r a d _ p r o f i l e [ i ] ∗ ( u n i t _ c t r l S t a t e [ i ]) ;
463 }
464 // c r o s s p r o d u c t magnitude
465 f l o a t s1 = unit_rad_profile [ 1 ] ∗ ( u n i t _ c t r l S t a t e [ 2 ] ) - ...
unit_rad_profile [ 2 ] ∗ ( unit_ctrlState [ 1 ] ) ;
466 f l o a t s2 = unit_rad_profile [ 2 ] ∗ ( u n i t _ c t r l S t a t e [ 0 ] ) - ...
unit_rad_profile [ 0 ] ∗ ( unit_ctrlState [ 2 ] ) ;
467 f l o a t s3 = unit_rad_profile [ 0 ] ∗ ( u n i t _ c t r l S t a t e [ 1 ] ) - ...
unit_rad_profile [ 1 ] ∗ ( unit_ctrlState [ 0 ] ) ;
468 crossproduct = s q r t f ( s1 ∗ s1 + s2 ∗ s2 + s3 ∗ s3 ) ;
469

470 // c o n s t r u c t G matrix
471 G[ 0 ] [ 0 ] = dotproduct ;
472 G[ 0 ] [ 1 ] = - crossproduct ;
473 G[ 0 ] [ 2 ] = 0.0 f ;
474 G[ 1 ] [ 0 ] = crossproduct ;
475 G[ 1 ] [ 1 ] = dotproduct ;
476 G[ 1 ] [ 2 ] = 0.0 f ;
477 G[ 2 ] [ 0 ] = 0.0 f ;
478 G[ 2 ] [ 1 ] = 0.0 f ;
479 G[ 2 ] [ 2 ] = 1.0 f ;
480
481 // C o n s t r u c t F , Finv
482 f l o a t midterm [ 3 ] = { u n i t _ c t r l S t a t e [ 0 ] - d o t p r o d u c t ∗...
unit_rad_profile [ 0 ] ,
483 u n i t _ c t r l S t a t e [ 1 ] - dotproduct ∗ unit_rad_profile [ 1 ] ,
484 u n i t _ c t r l S t a t e [ 2 ] - dotproduct ∗ unit_rad_profile [ 2 ] } ;
485 f l o a t midterm_mag = s q r t f ( midterm [ 0 ] ∗ midterm [ 0 ] +
486 midterm [ 1 ] ∗ midterm [ 1 ] + midterm [ 2 ] ∗ midterm [ 2 ] ) ;
487 float F[ 3 ] [ 3 ] ;
488 f l o a t Finv [ 3 ] [ 3 ] = {{ u n i t _ r a d _ p r o f i l e [ 0 ] , midterm [ 0 ] / ...
midterm_mag , s 1 } ,
489 { u n i t _ r a d _ p r o f i l e [ 1 ] , midterm [ 1 ] / midterm_mag , s 2 } ,
490 { u n i t _ r a d _ p r o f i l e [ 2 ] , midterm [ 2 ] / midterm_mag , s 3 } } ;
491 m a t r i x I n v e r s e (F , Finv ) ;
492
493 // c o n s t r u c t U
494 float U[ 3 ] [ 3 ] ;
495 f l o a t int_prod [ 3 ] [ 3 ] ;
496 f o r ( i =0; i <3; i ++){
497 f o r ( j =0; j <3; j ++){
498 int_prod [ i ] [ j ] = 0 . 0 f ;
499 f o r ( k=0; k <3; k++){
500 int_prod [ i ] [ j ] += G[ i ] [ k ] ∗ F [ k ] [ j ] ;
501 }

145
502 }
503 }
504 f o r ( i =0; i <3; i ++){
505 f o r ( j =0; j <3; j ++){
506 U[ i ] [ j ] = 0 . 0 f ;
507 f o r ( k=0; k <3; k++){
508 U[ i ] [ j ] += Finv [ i ] [ k ] ∗ int_prod [ k ] [ j ] ;
509 }
510 }
511 }
512
513
514 // c r e a t e temporary a r r a y t o h o l d c u r r e n t r a d i a l p r o f i l e
515 f o r ( i =0; i <2000; i ++){
516 f o r ( j =0; j <3; j ++){
517 temp_radial_profile [ i ] [ j ] = r a d i a l _ p r o f i l e [ i ] [ j ] ;
518 }
519 }
520
521 // re - w r i t e r a d i a l p r o f i l e with t h e r o t a t i o n i n p l a c e
522 f o r ( i =0; i <c o u n t e r ; i ++){
523 f o r ( j =0; j <3; j ++){
524 r a d i a l _ p r o f i l e [ 1 9 9 9 - c o u n t e r+i ] [ j ] = 0 . 0 f ;
525 f o r ( k=0; k <3; k++){
526 r a d i a l _ p r o f i l e [ 1 9 9 9 - c o u n t e r+i ] [ j ] += U[ j ] [ k ] ∗
527 t e m p _ r a d i a l _ p r o f i l e [ 1 9 9 9 - c o u n t e r+i ] [ k ] ;
528 }
529 r a d i a l _ p r o f i l e [ 1 9 9 9 - c o u n t e r+i ] [ j ] =
530 r a d i a l _ p r o f i l e [ 1 9 9 9 - c o u n t e r+i ] [ j ]+ t g t S t a t e [ j ] ;
531 }
532 }
533 }
534
535 }
536
537 v o i d quatMult_noNorm ( f l o a t ∗q3 , f l o a t ∗q1 , f l o a t ∗ q2 ) {
538 // q u a t e r n i o n m u l t i p l i c a t i o n ( s u c c e s s i v e r o t a t i o n ) , q2 onto q1
539 q3 [ 0 ] = q1 [ 0 ] ∗ q2 [ 3 ] + q1 [ 1 ] ∗ q2 [ 2 ] - q1 [ 2 ] ∗ q2 [ 1 ] + q1 [ 3 ] ∗ q2 [ 0 ] ;
540 q3 [ 1 ] = - q1 [ 0 ] ∗ q2 [ 2 ] + q1 [ 1 ] ∗ q2 [ 3 ] + q1 [ 2 ] ∗ q2 [ 0 ] + q1 [ 3 ] ∗ q2 [ 1 ] ;
541 q3 [ 2 ] = q1 [ 0 ] ∗ q2 [ 1 ] - q1 [ 1 ] ∗ q2 [ 0 ] + q1 [ 2 ] ∗ q2 [ 3 ] + q1 [ 3 ] ∗ q2 [ 2 ] ;
542 q3 [ 3 ] = - q1 [ 0 ] ∗ q2 [ 0 ] - q1 [ 1 ] ∗ q2 [ 1 ] - q1 [ 2 ] ∗ q2 [ 2 ] + q1 [ 3 ] ∗ q2 [ 3 ] ;
543 }
544
545 v o i d m a t r i x I n v e r s e ( f l o a t minv [ 3 ] [ 3 ] , f l o a t m[ 3 ] [ 3 ] ) {
546 // For 3 x3 m a t r i c e s
547 // F i r s t c a l c u l a t e t h e d e t e r m i n a n t o f m
548 f l o a t d e t = m[ 0 ] [ 0 ] ∗ (m[ 1 ] [ 1 ] ∗m[ 2 ] [ 2 ] - m[ 2 ] [ 1 ] ∗m[ 1 ] [ 2 ] ) -
549 m[ 0 ] [ 1 ] ∗ (m[ 1 ] [ 0 ] ∗m[ 2 ] [ 2 ] - m[ 1 ] [ 2 ] ∗m[ 2 ] [ 0 ] ) +
550 m[ 0 ] [ 2 ] ∗ (m[ 1 ] [ 0 ] ∗m[ 2 ] [ 1 ] - m[ 1 ] [ 1 ] ∗m[ 2 ] [ 0 ] ) ;
551

552 f l o a t i n v d e t = 1/ d e t ;
553
554 // f l o a t minv [ 3 ] [ 3 ] ;
555 minv [ 0 ] [ 0 ] = (m[ 1 ] [ 1 ] ∗ m[ 2 ] [ 2 ] - m[ 2 ] [ 1 ] ∗ m[ 1 ] [ 2 ] ) ∗ i n v d e t ;

146
556 minv [ 0 ] [ 1 ] = (m[ 0 ] [ 2 ] ∗ m[ 2 ] [ 1 ] - m[ 0 ] [ 1 ] ∗ m[ 2 ] [ 2 ] ) ∗ invdet ;
557 minv [ 0 ] [ 2 ] = (m[ 0 ] [ 1 ] ∗ m[ 1 ] [ 2 ] - m[ 0 ] [ 2 ] ∗ m[ 1 ] [ 1 ] ) ∗ invdet ;
558 minv [ 1 ] [ 0 ] = (m[ 1 ] [ 2 ] ∗ m[ 2 ] [ 0 ] - m[ 1 ] [ 0 ] ∗ m[ 2 ] [ 2 ] ) ∗ invdet ;
559 minv [ 1 ] [ 1 ] = (m[ 0 ] [ 0 ] ∗ m[ 2 ] [ 2 ] - m[ 0 ] [ 2 ] ∗ m[ 2 ] [ 0 ] ) ∗ invdet ;
560 minv [ 1 ] [ 2 ] = (m[ 1 ] [ 0 ] ∗ m[ 0 ] [ 2 ] - m[ 0 ] [ 0 ] ∗ m[ 1 ] [ 2 ] ) ∗ invdet ;
561 minv [ 2 ] [ 0 ] = (m[ 1 ] [ 0 ] ∗ m[ 2 ] [ 1 ] - m[ 2 ] [ 0 ] ∗ m[ 1 ] [ 1 ] ) ∗ invdet ;
562 minv [ 2 ] [ 1 ] = (m[ 2 ] [ 0 ] ∗ m[ 0 ] [ 1 ] - m[ 0 ] [ 0 ] ∗ m[ 2 ] [ 1 ] ) ∗ invdet ;
563 minv [ 2 ] [ 2 ] = (m[ 0 ] [ 0 ] ∗ m[ 1 ] [ 1 ] - m[ 1 ] [ 0 ] ∗ m[ 0 ] [ 1 ] ) ∗ invdet ;
564
565 }
566
567 v o i d dcm_from_quat ( f l o a t dcm [ 3 ] [ 3 ] , f l o a t ∗ quat ) {
568 // Find t h e d i r e c t i o n c o s i n e matrix from a q u a t e r n i o n .
569 f l o a t q1 = quat [ 0 ] ;
570 f l o a t q2 = quat [ 1 ] ;
571 f l o a t q3 = quat [ 2 ] ;
572 f l o a t q0 = quat [ 3 ] ; // s c a l a r v a l u e
573
574 dcm [ 0 ] [ 0 ] = q0 ∗ q0+q1 ∗q1 - q2 ∗q2 - q3 ∗ q3 ;
575 dcm [ 0 ] [ 1 ] = 2 ∗ ( q1 ∗ q2+q0 ∗ q3 ) ;
576 dcm [ 0 ] [ 2 ] = 2 ∗ ( q1 ∗q3 - q0 ∗ q2 ) ;
577 dcm [ 1 ] [ 0 ] = 2 ∗ ( q1 ∗ q2 - q0 ∗ q3 ) ;
578 dcm [ 1 ] [ 1 ] = ( q0 ∗q0 - q1 ∗ q1+q2 ∗q2 - q3 ∗ q3 ) ;
579 dcm [ 1 ] [ 2 ] = 2 ∗ ( q2 ∗ q3+q0 ∗ q1 ) ;
580 dcm [ 2 ] [ 0 ] = 2 ∗ ( q1 ∗ q3+q0 ∗ q2 ) ;
581 dcm [ 2 ] [ 1 ] = 2 ∗ ( q2 ∗q3 - q0 ∗ q1 ) ;
582 dcm [ 2 ] [ 2 ] = ( q0 ∗q0 - q1 ∗q1 - q2 ∗ q2+q3 ∗ q3 ) ;
583
584 }

147
148
Appendix B

Adaptive Control for Uncertainty

B.1 Thruster Characterization Plots

Z-Accelerometers [m s -2 ]Y-Accelerometers [m s -2 ] X-Accelerometers [m s -2 ]

Z-Accelerometers [m s -2 ]Y-Accelerometers [m s -2 ] X-Accelerometers [m s -2 ]

Raw IMU data for Sphere logical ID 1 Raw IMU data for Sphere logical ID 2
0.05 0 1 0.05 0 1

0 0

-0.05 6 7 -0.05 6 7
14 16 18 20 22 24 14 16 18 20 22 24

0.05 2 3 0.05 2 3

0 0

-0.05 8 9 -0.05 8 9
14 16 18 20 22 24 14 16 18 20 22 24

0.05 4 5 0.05 4 5

0 0

-0.05 10 11 -0.05 10 11
14 16 18 20 22 24 14 16 18 20 22 24
Test time, s Test time, s

(a) SPH1 (Blue) 50 ms (b) SPH2 (Orange) 50 ms

Z-Accelerometers [m s -2 ]Y-Accelerometers [m s -2 ] X-Accelerometers [m s -2 ]

Z-Accelerometers [m s -2 ]Y-Accelerometers [m s -2 ] X-Accelerometers [m s -2 ]

Raw IMU data for Sphere logical ID 1 Raw IMU data for Sphere logical ID 2
0.05 0 1 0.05 0 1

0 0

-0.05 6 7 -0.05 6 7
26 28 30 32 34 36 26 28 30 32 34 36

0.05 2 3 0.05 2 3

0 0

-0.05 8 9 -0.05 8 9
26 28 30 32 34 36 26 28 30 32 34 36

0.05 4 5 0.05 4 5

0 0

-0.05 10 11 -0.05 10 11
26 28 30 32 34 36 26 28 30 32 34 36
Test time, s Test time, s

(c) SPH1 (Blue) 100 ms (d) SPH1 (Orange) 100 ms

Figure B-1: Thruster Characterization: 50 ms and 100 ms Impulses

149
B.2 Thruster Characterization Functions

Listing B.1: pulse_extraction.m

1 function pulses = pulse_extraction(sphnum)
2 % Specifically designed for T18 of TS97
3 % Run this file first. After running this, run process_pulses_sph.m
4
5 load('saved_imu_1.mat');
6 load('saved_imu_2.mat');
7
8 user_input = 1;
9

10 if sphnum == 1
11 time = imu_sph_1.gyro{2}.Time;
12 time_ind = find(time≥37.25&time≤50.7);
13 sdata(:,2) = time(time_ind);
14 time_x = imu_sph_1.gyro{1}.Time;
15 sph1_gyro_x = interp1(time_x,imu_sph_1.gyro{1}.Data,time);
16 sdata(:,3) = smooth(sph1_gyro_x(time_ind),7);
17 sph1_gyro_y = imu_sph_1.gyro{2}.Data;
18 sdata(:,4) = smooth(sph1_gyro_y(time_ind),7);
19 time_z = imu_sph_1.gyro{3}.Time;
20 sph1_gyro_z = interp1(time_z,imu_sph_1.gyro{3}.Data,time);
21 sdata(:,5) = smooth(sph1_gyro_z(time_ind),7);
22 time_x_a = imu_sph_1.accel{1}.Time;
23 sph1_accel_x = interp1(time_x_a,imu_sph_1.accel{1}.Data,time);
24 sdata(:,6) = smooth(sph1_accel_x(time_ind),7);
25 time_y_a = imu_sph_1.accel{2}.Time;
26 sph1_accel_y = interp1(time_y_a,imu_sph_1.accel{2}.Data,time);
27 sdata(:,7) = smooth(sph1_accel_y(time_ind),7);
28 time_z_a = imu_sph_1.accel{3}.Time;
29 sph1_accel_z = interp1(time_z_a,imu_sph_1.accel{3}.Data,time);
30 sdata(:,8) = smooth(sph1_accel_z(time_ind),7);
31 elseif sphnum == 2
32 time = imu_sph_2.gyro{2}.Time;
33 time_ind = find(time≥37.25&time≤50.7);
34 sdata(:,2) = time(time_ind);
35 time_x = imu_sph_2.gyro{1}.Time;
36 sph2_gyro_x = interp1(time_x,imu_sph_2.gyro{1}.Data,time);
37 sdata(:,3) = smooth(sph2_gyro_x(time_ind),7);
38 sph2_gyro_y = imu_sph_2.gyro{2}.Data;
39 sdata(:,4) = smooth(sph2_gyro_y(time_ind),7);
40 time_z = imu_sph_2.gyro{3}.Time;
41 sph2_gyro_z = interp1(time_z,imu_sph_2.gyro{3}.Data,time);
42 sdata(:,5) = smooth(sph2_gyro_z(time_ind),7);
43 time_x_a = imu_sph_2.accel{1}.Time;
44 sph2_accel_x = interp1(time_x_a,imu_sph_2.accel{1}.Data,time);
45 sdata(:,6) = smooth(sph2_accel_x(time_ind),7);
46 time_y_a = imu_sph_2.accel{2}.Time;
47 sph2_accel_y = interp1(time_y_a,imu_sph_2.accel{2}.Data,time);
48 sdata(:,7) = smooth(sph2_accel_y(time_ind),7);

150
49 time_z_a = imu_sph_2.accel{3}.Time;
50 sph2_accel_z = interp1(time_z_a,imu_sph_2.accel{3}.Data,time);
51 sdata(:,8) = smooth(sph2_accel_z(time_ind),7);
52 end
53
54 load('2018_01_17_13_18_53_P526_T18_Rcv');
55
56 raw_data = data{1,sphnum}.DebugUnsignedVal; % thruster data
57 thr_data_on = [];
58 thr_data_off = [];
59 pulses = struct();
60 % Sort thruster data into on and off
61 for c = 1:length(raw_data)
62 if 40932 ̸= raw_data(1,c)
63 neg_thr = dec2bin(raw_data(14,c),6);
64 thr_data_on(13,end+1) = raw_data(1,c)/10;
65 thr_data_off(13,end+1) = raw_data(1,c)/10;
66 for thr = 1:6
67 if strcmp(neg_thr(end+1-thr),'0')
68 thr_data_on(thr,end) = ...
(raw_data(2*thr,c))/1000+raw_data(1,c)/10;
69 thr_data_off(thr,end) = ...
(raw_data(2*thr+1,c))/1000+raw_data(1,c)/10;
70 thr_data_on(thr+6,end) = raw_data(1,c)/10;
71 thr_data_off(thr+6,end) = raw_data(1,c)/10;
72 else
73 thr_data_on(thr,end) = raw_data(1,c)/10;
74 thr_data_off(thr,end) = raw_data(1,c)/10;
75 thr_data_on(thr+6,end) = ...
(raw_data(2*thr,c))/1000+raw_data(1,c)/10;
76 thr_data_off(thr+6,end) = ...
(raw_data(2*thr+1,c))/1000+raw_data(1,c)/10;
77 end
78 end
79 end
80 end
81
82

83
84 for t = 75:2:97 %Hard-coded for longest duration of thrust pulses ...
in T18 of TS97
85 x = [-inf*ones(1,3) ones(1,3)*inf];
86 thrusters = zeros(12,1);
87 base_data = [];
88 for thr = 1:12
89 if thr_data_on(thr,t)̸=thr_data_off(thr,t)
90 thrusters(thr) = 1;
91 % Modify this according to typical pulse length
92 x(1) = max(x(1),thr_data_on(thr,t) + 0); %Pre-pulse start
93 x(2) = max(x(2),thr_data_on(thr,t) + 0.2); %Pre-pulse end
94 x(3) = max(x(3),thr_data_on(thr,t) + 0.2); %During ...
pulse start
95 x(4) = min(x(4),thr_data_off(thr,t) + 0.1); %During ...
pulse end

151
 96 x(5) = min(x(5),thr_data_off(thr,t)+ 0.1); ...
%Post-pulse start
97 x(6) = min(x(6),thr_data_off(thr,t) + 0.3); ...
%Post-pulse end
98 end
99 end
100
101 if sum(x̸=[-inf*ones(1,3) ones(1,3)*inf])==6
102 pulse_time = (sdata(:,2)≥x(1))&(sdata(:,2)≤x(6));
103 pulse_data = sdata(pulse_time,:);
104 if ¬isempty(pulse_data)
105 pre_pulse_ind = ...
find(pulse_data(:,2)≥x(1)&pulse_data(:,2)≤x(2));
106 during_pulse_ind = ...
find(pulse_data(:,2)≥x(3)&pulse_data(:,2)≤x(4));
107 post_pulse_ind = ...
find(pulse_data(:,2)≥x(5)&pulse_data(:,2)≤x(6));
108 if ¬isempty(pre_pulse_ind) && ¬...
isempty(during_pulse_ind) && ¬isempty(post_pulse_ind)
109

110 base_data(:,1) = pulse_data(:,1);

111 base_data(:,2) = pulse_data(:,2);
112
113 % Find line to describe base data
114 %Gyro data
115 for i = 3:5
116 p = polyfit(pulse_data(pre_pulse_ind,2),...
117 pulse_data(pre_pulse_ind,i),1);
118 base_data(:,i) = p(1)*pulse_data(:,2) + p(2);
119 end
120 %Accel data
121 for i = 6:8
122 p = polyfit([pulse_data(pre_pulse_ind,2);...
123 pulse_data(post_pulse_ind,2)],...
124 [pulse_data(pre_pulse_ind,i);...
125 pulse_data(post_pulse_ind,i)],1);
126 base_data(:,i) = p(1)*pulse_data(:,2) + p(2);
127 end
128
129 if user_input %only plot clean accel data
130 old_pulse_data_omega = pulse_data(:,3:5);
131
132 figure
133 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
134 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
135 during_pulse_ind;post_pulse_ind],6),...
136 'Color',[0 0.5 0])
137 hold on
138 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
139 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
140 during_pulse_ind;post_pulse_ind],7),...
141 'Color',[0.5 0 0])
142 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
143 post_pulse_ind],2),pulse_data([pre_pulse_ind;...

152
144 during_pulse_ind;post_pulse_ind],8),...
145 'Color',[0 0 0.5])
146 title('Cleaned Accelerometer Data')
147 xlabel('Time (s)')
148 ylabel('Measured Acceleration (m/s^2)')
149 legend({'a_x','a_y','a_z'},...
150 'Location','EastOutside')
151 [p1,z] = ginput(1);
152 fake_time = [p1 p1+.16];
153 fake_z = [z z];
154 plot(fake_time,fake_z,'-*k');
155 [p2,y] = ginput(1);
156 else
157 figure
158 subplot(2,2,1)
159 plot(pulse_data(:,2),pulse_data(:,3),...
160 'Color',[0 1 0])
161 hold on
162 plot(base_data(:,2),base_data(:,3),...
163 'Color',[0 0.5 0])
164 plot(pulse_data(:,2),pulse_data(:,4),...
165 'Color',[1 0 0])
166 plot(base_data(:,2),base_data(:,4),...
167 'Color',[0.5 0 0])
168 plot(pulse_data(:,2),pulse_data(:,5),...
169 'Color',[0 0 1])
170 plot(base_data(:,2),base_data(:,5),...
171 'Color',[0 0 0.5])
172 title('Raw Gyro Data and Baseline Estimate')
173 xlabel('Time (s)')
174 ylabel('Measured Angular Velocity (rad/s)')
175 legend({'\omega_x','\omega_x base','\omega_y',...
176 '\omega_y base','\omega_z','\omega_z ...
base'},...
177 'Location','EastOutside')
178
179 subplot(2,2,2)
180 plot(pulse_data(:,2),pulse_data(:,6),...
181 'Color',[0 1 0])
182 hold on
183 plot(base_data(:,2),base_data(:,6),...
184 'Color',[0 0.5 0])
185 plot(pulse_data(:,2),pulse_data(:,7),...
186 'Color',[1 0 0])
187 plot(base_data(:,2),base_data(:,7),...
188 'Color',[0.5 0 0])
189 plot(pulse_data(:,2),pulse_data(:,8),...
190 'Color',[0 0 1])
191 plot(base_data(:,2),base_data(:,8),...
192 'Color',[0 0 0.5])
193 title('Raw Accelerometer Data and Baseline ...
Estimate')
194 xlabel('Time (s)')
195 ylabel('Measured Acceleration (m/s^2)')

153
196 legend({'a_x','a_x base','a_y','a_y base',...
197 'a_z','a_z base'},'Location','EastOutside')
198
199 %extract the raw angular rate data (assuming ...
no bias)
200 raw_omega = pulse_data(during_pulse_ind,3:5);
201
202

203 pulse_data(:,3:8) = pulse_data(:,3:8) - ...

base_data(:,3:8);
204
205 subplot(2,2,3)
206 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
207 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
208 during_pulse_ind;post_pulse_ind],3),...
209 'Color',[0 0.5 0])
210 hold on
211 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
212 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
213 during_pulse_ind;post_pulse_ind],4),...
214 'Color',[0.5 0 0])
215 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
216 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
217 during_pulse_ind;post_pulse_ind],5),...
218 'Color',[0 0 0.5])
219 title('Cleaned Gyro Data')
220 xlabel('Time (s)')
221 ylabel('Measured Angular Velocity (rad/s)')
222 legend({'\omega_x','\omega_y','\omega_z'},...
223 'Location','EastOutside')
224 subplot(2,2,4)
225 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
226 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
227 during_pulse_ind;post_pulse_ind],6),...
228 'Color',[0 0.5 0])
229 hold on
230 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
231 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
232 during_pulse_ind;post_pulse_ind],7),...
233 'Color',[0.5 0 0])
234 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
235 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
236 during_pulse_ind;post_pulse_ind],8),...
237 'Color',[0 0 0.5])
238 title('Cleaned Accelerometer Data')
239 xlabel('Time (s)')
240 ylabel('Measured Acceleration (m/s^2)')
241 legend({'a_x','a_y','a_z'},...
242 'Location','EastOutside')
243 set(gcf,'Position',[100, 100, 1200, 700])
244 %valid = questdlg('Was this pulse accurately ...
extracted?');
245 end
246 if user_input

154
247 during_pulse_ind_g = ...
find(pulse_data(:,2)≥p1&pulse_data(:,2)≤p2);
248 pre_pulse_ind_g = ...
find(pulse_data(:,2)≥x(1)&pulse_data(:,2)≤p1);
249 post_pulse_ind_g = ...
find(pulse_data(:,2)≥p2&pulse_data(:,2)≤x(6));
250
251 raw_omega_g = ...
old_pulse_data_omega(during_pulse_ind_g,:);
252 end
253
254 if user_input
255 pulses(end+1).thruster = thrusters;
256 % accel = average of accel data "during pulse ...
period"
257 pulses(end).accel = ...
mean(pulse_data(during_pulse_ind_g,6:8));
258 % alpha = (final gyro data - initial gyro ...
data)/(tf -
259 % ti) for "during pulse period"
260 pulses(end).alpha = ...
(pulse_data(during_pulse_ind_g(end),3:5)...
261 -pulse_data(during_pulse_ind_g(1),3:5))/...
262 (pulse_data(during_pulse_ind_g(end),2)-...
263 pulse_data(during_pulse_ind_g(1),2));
264 %delta_omega = avg of gryo data before pulse ...
- avg of
265 %gyro data after pulse
266 pulses(end).delta_omega = ...
mean(pulse_data(post_pulse_ind_g,3:5))-...
267 mean(pulse_data(pre_pulse_ind_g,3:5));
268 pulses(end).omega = mean(raw_omega_g);
269 else
270 pulses(end+1).thruster = thrusters;
271 pulses(end).accel = ...
mean(pulse_data(during_pulse_ind,6:8));
272 pulses(end).alpha = ...
(pulse_data(during_pulse_ind(end),3:5)...
273 -pulse_data(during_pulse_ind(1),3:5))/...
274 (pulse_data(during_pulse_ind(end),2)-...
275 pulse_data(during_pulse_ind(1),2));
276 pulses(end).delta_omega = ...
mean(pulse_data(post_pulse_ind,3:5))-...
277 mean(pulse_data(pre_pulse_ind,3:5));
278 pulses(end).omega = mean(raw_omega);
279 end
280 end
281 end
282 end
283 end
284
285 pulses(1) = [];
286 if sphnum == 1
287 filename = 'T18';

155
288 else
289 filename = 'T18_sph2';
290 end
291
292 save(['pulses_' filename],'pulses','filename')

Listing B.2: process_pulses_sph.m

1 function results = process_pulses_sph()
2 % Run this after running pulse_extraction.m
3 % After this, run plot_pulses.m
4
5 [filename, folder] = uigetfile('*.mat','Pick pulse data file');
6 load([folder filename])
7
8 %% Computations
9 % SPHERES properties
10 mass = 4.43; % [kg]
11 % accelerometer locations
12 rgc_xaccel = [0.0519, 0.0217, 0.0327]';
13 rgc_yaccel = [-0.0266, 0.0335, 0.0330]';
14 rgc_zaccel = [0.0328, -0.0437, 0.0335]';
15
16 % Thruster locations
17 Rgc =[-0.0516 , 0.0 , 0.0965;
18 -0.0516 , 0.0 , -0.0965;
19 0.0965 , -0.0516 , 0.0;
20 -0.0965 , -0.0516 , 0.0;
21 0.0 , 0.0965 , -0.0516;
22 0.0 , -0.0965 , -0.0516;
23 0.0516 , 0.0 , 0.0965;
24 0.0516 , 0.0 , -0.0965;
25 0.0965 , 0.0516 , 0.0;
26 -0.0965 , 0.0516 , 0.0;
27 0.0 , 0.0965 , 0.0516;
28 0.0 , -0.0965 , 0.0516];
29 % Force directions
30 Force =[1,0,0;
31 1,0,0;
32 0,1,0;
33 0,1,0;
34 0,0,1;
35 0,0,1;
36 -1,0,0;
37 -1,0,0;
38 0,-1,0;
39 0,-1,0;
40 0,0,-1;
41 0,0,-1];
42 R_thr = Rgc;
43 Torque = cross(R_thr,Force); % torque directions
44 Minv = [Force';Torque']; %Scale by the estimated thrust available
45

156
46 a_meas = zeros(length(pulses),3);
47 alpha_meas = zeros(length(pulses),3);
48 omega_meas = zeros(length(pulses),3);
49 A_thrust_x = zeros(length(pulses),4);
50 A_thrust_y = zeros(length(pulses),4);
51 A_thrust_z = zeros(length(pulses),4);
52 Ax = zeros(length(pulses),4);
53 Ay = zeros(length(pulses),4);
54 Az = zeros(length(pulses),4);
55 bx = zeros(length(pulses),1);
56 by = zeros(length(pulses),1);
57 bz = zeros(length(pulses),1);
58 for p = 1:length(pulses)
59 a_meas(p,:) = pulses(p).accel;
60 if sum(pulses(p).thruster)>0
61 % This extracts which force direction the specific pulse ...
was in.
62 A_thrust = 0.94^(sum(pulses(p).thruster)-1)*...
63 (pulses(p).thruster*ones(1,3))'.*Minv(1:3,:);
64 else
65 A_thrust = zeros(3,12);
66 end
67 alpha_meas(p,:) = pulses(p).alpha; ...
%pulses(p).delta_omega/0.36; %
68 omega_meas(p,:) = pulses(p).delta_omega/2;
69 alpha_cross = skewSym(pulses(p).alpha); ...
%skewSym(pulses(p).delta_omega/0.36); %
70 omega_cross = skewSym(pulses(p).delta_omega/2);
71 A_cross = alpha_cross + omega_cross*omega_cross;
72 prod_x = A_cross(1,:)*rgc_xaccel;
73 prod_y = A_cross(2,:)*rgc_yaccel;
74 prod_z = A_cross(3,:)*rgc_zaccel;
75
76 A_thrust_x(p,:) = A_thrust(1,[1,2,7,8]);
77 A_thrust_y(p,:) = A_thrust(2,[3,4,9,10]);
78 A_thrust_z(p,:) = A_thrust(3,[5,6,11,12]);
79
80 Ax(p,:) = A_thrust_x(p,:);
81 Ay(p,:) = A_thrust_y(p,:);
82 Az(p,:) = A_thrust_z(p,:);
83 bx(p,1) = a_meas(p,1) - prod_x;
84 by(p,1) = a_meas(p,2) - prod_y;
85 bz(p,1) = a_meas(p,3) - prod_z;
86 end
87
88 %% Find the solution
89 %For the x-axis accelerometer
90 sol_xaccel = Ax\bx;
91 thrust_xaccel = sol_xaccel*mass;
92

93 %For the y-axis accelerometer

94 sol_yaccel = Ay\by;
95 thrust_yaccel = sol_yaccel*mass;
96

157
 97 %For the z-axis accelerometer
98 sol_zaccel = Az\bz;
99 thrust_zaccel = sol_zaccel*mass;
100
101 %% Compute the thruster force from individual axes estimation
102 thrust = [thrust_xaccel(1:2); thrust_yaccel(1:2);...
103 thrust_zaccel(1:2); thrust_xaccel(3:4);...
104 thrust_yaccel(3:4); thrust_zaccel(3:4)];
105
106 %% Create results structure
107 results.Ax = Ax;
108 results.Ay = Ay;
109 results.Az = Az;
110 results.bx = bx;
111 results.by = by;
112 results.bz = bz;
113 results.a_meas = a_meas;
114 results.alpha_meas = alpha_meas;
115 results.omega_meas = omega_meas;
116 results.thrust = thrust;
117
118 %% Save results
119 save(['results_' filename],'results')

Listing B.3: plot_pulses.m

1 function plot_pulses()
2 % Run this after running process_pulses_sph.m
3 % If you want to modify a few pulses, run revise_pulses.m
4 load('results_T18.mat')
5 results_sph1 = results;
6 load('results_T18_sph2.mat')
7 results_sph2 = results;
8
9 %Plot thrust
10 thrust_sph1 = (results_sph1.thrust);
11 thrust_sph2 = (results_sph2.thrust);
12 num = 0:1:11;
13
14 figure
15 plot(num,thrust_sph1,'*b'); hold on;
16 plot(num,thrust_sph2,'or');
17 yL = ylim;
18 xL = xlim;
19 line([0 0], yL,'Color','black'); %x-axis
20 line(xL,[0 0],'Color','k');
21 xlabel('Thruster Number');
22 ylabel('Thrust [N]');
23 title('Thruster Force')
24 legend('Blue','Orange','Location','Southeast');
25 xticks([0,1,2,3,4,5,6,7,8,9,10,11]); grid on;
26
27 end

158
 Listing B.4: revise_pulses.m
1 function revise_pulses(sphnum)
2 % Allows user input to re-do the pulse extraction for selected ...
pulses.
3

4 user_input = 1;
5
6 if sphnum == 1
7 load('saved_imu_1.mat');
8 load('pulses_T18.mat');
9 time = imu_sph_1.gyro{2}.Time;
10 time_ind = find(time≥37.25&time≤50.7);
11 sdata(:,2) = time(time_ind);
12 time_x = imu_sph_1.gyro{1}.Time;
13 sph1_gyro_x = interp1(time_x,imu_sph_1.gyro{1}.Data,time);
14 sdata(:,3) = smooth(sph1_gyro_x(time_ind),7);
15 sph1_gyro_y = imu_sph_1.gyro{2}.Data;
16 sdata(:,4) = smooth(sph1_gyro_y(time_ind),7);
17 time_z = imu_sph_1.gyro{3}.Time;
18 sph1_gyro_z = interp1(time_z,imu_sph_1.gyro{3}.Data,time);
19 sdata(:,5) = smooth(sph1_gyro_z(time_ind),7);
20 time_x_a = imu_sph_1.accel{1}.Time;
21 sph1_accel_x = interp1(time_x_a,imu_sph_1.accel{1}.Data,time);
22 sdata(:,6) = smooth(sph1_accel_x(time_ind),7);
23 time_y_a = imu_sph_1.accel{2}.Time;
24 sph1_accel_y = interp1(time_y_a,imu_sph_1.accel{2}.Data,time);
25 sdata(:,7) = smooth(sph1_accel_y(time_ind),7);
26 time_z_a = imu_sph_1.accel{3}.Time;
27 sph1_accel_z = interp1(time_z_a,imu_sph_1.accel{3}.Data,time);
28 sdata(:,8) = smooth(sph1_accel_z(time_ind),7);
29 else
30 load('saved_imu_2.mat');
31 load('pulses_T18_sph2.mat');
32 time = imu_sph_2.gyro{2}.Time;
33 time_ind = find(time≥37.25&time≤50.7);
34 sdata(:,2) = time(time_ind);
35 time_x = imu_sph_2.gyro{1}.Time;
36 sph2_gyro_x = interp1(time_x,imu_sph_2.gyro{1}.Data,time);
37 sdata(:,3) = smooth(sph2_gyro_x(time_ind),7);
38 sph2_gyro_y = imu_sph_2.gyro{2}.Data;
39 sdata(:,4) = smooth(sph2_gyro_y(time_ind),7);
40 time_z = imu_sph_2.gyro{3}.Time;
41 sph2_gyro_z = interp1(time_z,imu_sph_2.gyro{3}.Data,time);
42 sdata(:,5) = smooth(sph2_gyro_z(time_ind),7);
43 time_x_a = imu_sph_2.accel{1}.Time;
44 sph2_accel_x = interp1(time_x_a,imu_sph_2.accel{1}.Data,time);
45 sdata(:,6) = smooth(sph2_accel_x(time_ind),7);
46 time_y_a = imu_sph_2.accel{2}.Time;
47 sph2_accel_y = interp1(time_y_a,imu_sph_2.accel{2}.Data,time);
48 sdata(:,7) = smooth(sph2_accel_y(time_ind),7);

159
49 time_z_a = imu_sph_2.accel{3}.Time;
50 sph2_accel_z = interp1(time_z_a,imu_sph_2.accel{3}.Data,time);
51 sdata(:,8) = smooth(sph2_accel_z(time_ind),7);
52 end
53
54 load('2018_01_17_13_18_53_P526_T18_Rcv');
55
56 raw_data = data{1,sphnum}.DebugUnsignedVal; % This is thruster data!
57 thr_data_on = [];
58 thr_data_off = [];
59 counter = 1;
60
61 % Sort thruster data into on and off
62 for c = 1:length(raw_data)
63 if 40932 ̸= raw_data(1,c)
64 neg_thr = dec2bin(raw_data(14,c),6);
65 thr_data_on(13,end+1) = raw_data(1,c)/10;
66 thr_data_off(13,end+1) = raw_data(1,c)/10;
67 for thr = 1:6
68 if strcmp(neg_thr(end+1-thr),'0')
69 thr_data_on(thr,end) = ...
(raw_data(2*thr,c))/1000+raw_data(1,c)/10;
70 thr_data_off(thr,end) = ...
(raw_data(2*thr+1,c))/1000+raw_data(1,c)/10;
71 thr_data_on(thr+6,end) = raw_data(1,c)/10;
72 thr_data_off(thr+6,end) = raw_data(1,c)/10;
73 else
74 thr_data_on(thr,end) = raw_data(1,c)/10;
75 thr_data_off(thr,end) = raw_data(1,c)/10;
76 thr_data_on(thr+6,end) = ...
(raw_data(2*thr,c))/1000+raw_data(1,c)/10;
77 thr_data_off(thr+6,end) = ...
(raw_data(2*thr+1,c))/1000+raw_data(1,c)/10;
78 end
79 end
80 end
81 end
82

83 % Ask user which thrusters they want to revise

84 prompt = 'Select thrusters to revise (1-12 scale) in a vector:';
85 thr_to_revise = input(prompt);
86
87 for t = 75:2:97
88 x = [-inf*ones(1,3) ones(1,3)*inf];
89 thrusters = zeros(12,1);
90 base_data = [];
91 for thr = thr_to_revise
92 if thr_data_on(thr,t)̸=thr_data_off(thr,t)
93 thrusters(thr) = 1;
94 x(1) = max(x(1),thr_data_on(thr,t) + 0); %Pre-pulse start
95 x(2) = max(x(2),thr_data_on(thr,t) + 0.2); %Pre-pulse end
96 x(3) = max(x(3),thr_data_on(thr,t) + 0.2); %During ...
pulse start
97 x(4) = min(x(4),thr_data_off(thr,t) + 0.1); %During ...

160
 pulse end
98 x(5) = min(x(5),thr_data_off(thr,t)+ 0.1); ...
%Post-pulse start
99 x(6) = min(x(6),thr_data_off(thr,t) + 0.3); ...
%Post-pulse end
100 end
101 end
102

103 if sum(x̸=[-inf*ones(1,3) ones(1,3)*inf])==6

104 pulse_time = (sdata(:,2)≥x(1))&(sdata(:,2)≤x(6));
105 pulse_data = sdata(pulse_time,:);
106 if ¬isempty(pulse_data)
107 pre_pulse_ind = ...
find(pulse_data(:,2)≥x(1)&pulse_data(:,2)≤x(2));
108 during_pulse_ind = ...
find(pulse_data(:,2)≥x(3)&pulse_data(:,2)≤x(4));
109 post_pulse_ind = ...
find(pulse_data(:,2)≥x(5)&pulse_data(:,2)≤x(6));
110 if ¬isempty(pre_pulse_ind) && ¬...
isempty(during_pulse_ind) && ¬isempty(post_pulse_ind)
111
112 base_data(:,1) = pulse_data(:,1);
113 base_data(:,2) = pulse_data(:,2);
114
115 % Find line to describe base data
116 %Gyro data
117 for i = 3:5
118 p = polyfit(pulse_data(pre_pulse_ind,2),...
119 pulse_data(pre_pulse_ind,i),1);
120 base_data(:,i) = p(1)*pulse_data(:,2) + p(2);
121 end
122 %Accel data
123 for i = 6:8
124 p = polyfit([pulse_data(pre_pulse_ind,2);...
125 pulse_data(post_pulse_ind,2)],...
126 [pulse_data(pre_pulse_ind,i);...
127 pulse_data(post_pulse_ind,i)],1);
128 base_data(:,i) = p(1)*pulse_data(:,2) + p(2);
129 end
130
131 if user_input %only plot clean accel data
132 old_pulse_data_omega = pulse_data(:,3:5);
133 %pulse_data(:,3:8) = pulse_data(:,3:8) - ...
base_data(:,3:8);
134 figure
135 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
136 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
137 during_pulse_ind;post_pulse_ind],6),...
138 'Color',[0 0.5 0])
139 hold on
140 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
141 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
142 during_pulse_ind;post_pulse_ind],7),...
143 'Color',[0.5 0 0])

161
144 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
145 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
146 during_pulse_ind;post_pulse_ind],8),...
147 'Color',[0 0 0.5])
148 title('Cleaned Accelerometer Data')
149 xlabel('Time (s)')
150 ylabel('Measured Acceleration (m/s^2)')
151 legend({'a_x','a_y','a_z'},'Location','EastOutside')
152 [p1,z] = ginput(1);
153 fake_time = [p1 p1+.16];
154 fake_z = [z z];
155 plot(fake_time,fake_z,'-*k');
156 [p2,y] = ginput(1);
157 else
158 figure
159 subplot(2,2,1)
160 plot(pulse_data(:,2),pulse_data(:,3),'Color',[0 ...
1 0])
161 hold on
162 plot(base_data(:,2),base_data(:,3),'Color',[0 ...
0.5 0])
163 plot(pulse_data(:,2),pulse_data(:,4),'Color',[1 ...
0 0])
164 plot(base_data(:,2),base_data(:,4),'Color',[0.5 ...
0 0])
165 plot(pulse_data(:,2),pulse_data(:,5),'Color',[0 ...
0 1])
166 plot(base_data(:,2),base_data(:,5),'Color',[0 ...
0 0.5])
167 title('Raw Gyro Data and Baseline Estimate')
168 xlabel('Time (s)')
169 ylabel('Measured Angular Velocity (rad/s)')
170 legend({'\omega_x','\omega_x base','\omega_y',...
171 '\omega_y base','\omega_z','\omega_z ...
base'},...
172 'Location','EastOutside')
173
174 subplot(2,2,2)
175 plot(pulse_data(:,2),pulse_data(:,6),'Color',[0 ...
1 0])
176 hold on
177 plot(base_data(:,2),base_data(:,6),'Color',[0 ...
0.5 0])
178 plot(pulse_data(:,2),pulse_data(:,7),'Color',[1 ...
0 0])
179 plot(base_data(:,2),base_data(:,7),'Color',[0.5 ...
0 0])
180 plot(pulse_data(:,2),pulse_data(:,8),'Color',[0 ...
0 1])
181 plot(base_data(:,2),base_data(:,8),'Color',[0 ...
0 0.5])
182 title('Raw Accelerometer Data and Baseline ...
Estimate')
183 xlabel('Time (s)')

162
184 ylabel('Measured Acceleration (m/s^2)')
185 legend({'a_x','a_x base','a_y','a_y base',...
186 'a_z','a_z base'},'Location','EastOutside')
187
188 %extract the raw angular rate data (assuming ...
no bias)
189 raw_omega = pulse_data(during_pulse_ind,3:5);
190

191 pulse_data(:,3:8) = pulse_data(:,3:8) - ...

base_data(:,3:8);
192
193 subplot(2,2,3)
194 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
195 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
196 during_pulse_ind;post_pulse_ind],3),...
197 'Color',[0 0.5 0])
198 hold on
199 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
200 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
201 during_pulse_ind;post_pulse_ind],4),...
202 'Color',[0.5 0 0])
203 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
204 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
205 during_pulse_ind;post_pulse_ind],5),...
206 'Color',[0 0 0.5])
207 title('Cleaned Gyro Data')
208 xlabel('Time (s)')
209 ylabel('Measured Angular Velocity (rad/s)')
210 legend({'\omega_x','\omega_y','\omega_z'},...
211 'Location','EastOutside')
212 subplot(2,2,4)
213 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
214 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
215 during_pulse_ind;post_pulse_ind],6),...
216 'Color',[0 0.5 0])
217 hold on
218 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
219 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
220 during_pulse_ind;post_pulse_ind],7),...
221 'Color',[0.5 0 0])
222 plot(pulse_data([pre_pulse_ind;during_pulse_ind;...
223 post_pulse_ind],2),pulse_data([pre_pulse_ind;...
224 during_pulse_ind;post_pulse_ind],8),...
225 'Color',[0 0 0.5])
226 title('Cleaned Accelerometer Data')
227 xlabel('Time (s)')
228 ylabel('Measured Acceleration (m/s^2)')
229 legend({'a_x','a_y','a_z'},...
230 'Location','EastOutside')
231 set(gcf,'Position',[100, 100, 1200, 700])
232 %valid = questdlg('Was this pulse accurately ...
extracted?');
233 end
234 if user_input

163
235 during_pulse_ind_g = ...
find(pulse_data(:,2)≥p1&pulse_data(:,2)≤p2);
236 pre_pulse_ind_g = ...
find(pulse_data(:,2)≥x(1)&pulse_data(:,2)≤p1);
237 post_pulse_ind_g = ...
find(pulse_data(:,2)≥p2&pulse_data(:,2)≤x(6));
238
239 raw_omega_g = ...
old_pulse_data_omega(during_pulse_ind_g,:);
240 end
241
242 if user_input %strcmp(valid,'Yes')
243 pulses(thr_to_revise(counter)).thruster = ...
thrusters;
244 pulses(thr_to_revise(counter)).accel = ...
mean(pulse_data(during_pulse_ind_g,6:8));
245 pulses(thr_to_revise(counter)).alpha = ...
(pulse_data(during_pulse_ind_g(end),3:5)...
246 -pulse_data(during_pulse_ind_g(1),3:5))/...
247 (pulse_data(during_pulse_ind_g(end),2)-...
248 pulse_data(during_pulse_ind_g(1),2));
249 pulses(thr_to_revise(counter)).delta_omega = ...
250 mean(pulse_data(post_pulse_ind_g,3:5))- ...
251 mean(pulse_data(pre_pulse_ind_g,3:5));
252 pulses(thr_to_revise(counter)).omega = ...
mean(raw_omega_g);
253 counter = counter+1;
254 else
255 pulses(thr_to_revise(counter)).thruster = ...
thrusters;
256 pulses(thr_to_revise(counter)).accel = ...
mean(pulse_data(during_pulse_ind,6:8));
257 pulses(thr_to_revise(counter)).alpha = ...
(pulse_data(during_pulse_ind(end),3:5)...
258 -pulse_data(during_pulse_ind(1),3:5))/...
259 (pulse_data(during_pulse_ind(end),2)-...
260 pulse_data(during_pulse_ind(1),2));
261 pulses(thr_to_revise(counter)).delta_omega = ...
262 mean(pulse_data(post_pulse_ind,3:5))-...
263 mean(pulse_data(pre_pulse_ind,3:5));
264 pulses(thr_to_revise(counter)).omega = ...
mean(raw_omega);
265 counter = counter+1;
266 end
267 end
268 end
269 end
270 end
271
272 % Re-save pulses file
273 if sphnum == 1
274 filename = 'T18';
275 else
276 filename = 'T18_sph2';

164
277 end
278 save(['pulses_' filename],'pulses','filename')

B.3 Adaptive PID SMC Controller

Listing B.5: ctrl_pid_smc.c

1 /*
2 Copyright (c) 2013, Massachusetts Institute of Technology Space ...
Systems Laboratory
3 All rights reserved.
4
5 Redistribution and use in source and binary forms, with or ...
without modification, are permitted provided that the ...
following conditions are met:
6 Redistributions of source code must retain the above copyright ...
notice, this list of conditions and the following disclaimer.
7 Redistributions in binary form must reproduce the above copyright ...
notice, this list of conditions and the following disclaimer ...
in the documentation and/or other materials provided with the ...
distribution.
8 Neither the name of the Massachusetts Institute of Technology, ...
the MIT Space Systems Laboratory, nor the names of its ...
contributors may be used to endorse or promote products ...
derived from this software without specific prior written ...
permission.
9
10 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND ...
CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, ...
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF ...
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE ...
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR ...
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, ...
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT ...
NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; ...
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) ...
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN ...
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR ...
OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, ...
EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
11 */
12
13 /*
14 * ctrl_pid_smc.c
15 *
16 * This file contains a library of standard "PID"-type position ...
controllers.
17 *
18 * MIT Space Systems Laboratory
19 * SPHERES Guest Scientist Program v1.0

165
20 * http://ssl.mit.edu/spheres/
21 *
22 * Copyright 2018 Massachusetts Institute of Technology
23 *
24 **********************************************************
25 * Sliding Mode Controller - PID *
26 * By: Hailee Hettrick *
27 **********************************************************
28 * Last modified: 10/14/2018 *
29 * *
30 **********************************************************
31 */
32
33 #include <math.h>
34 #include "ctrl_pid_smc.h"
35 #include "spheres_constants.h"
36 #include "ctrl_position.h"
37 #include "spheres_physical_parameters.h"
38 #include "control.h"
39

40 #ifdef SPH_MATLAB_SIM
41 #include "mex.h"
42 #define DEBUG(arg) mexPrintf arg
43 #else
44 #define DEBUG(arg)
45 #endif
46
47 // Constants
48 float lambda = 0.085f;
49 float lambda_att = 0.1f;
50 float gamma1 = 60.0f;
51 float gamma2 = 80.0f;
52 float gamma3 = 4.0f;
53 float gamma4 = 2.0f;
54 float Kp = 1000.0f;
55 float Kp_att = 50.0f;
56 float P = 10.0f;
57 float P_att = 10.0f;
58 int i, j;
59 int n = 2;
60
61 // zero initialization
62 float u_hat[2] = {0.0f, 0.0f};
63 float u_pid[2] = {0.0f, 0.0f};
64 float xr_ddot[2] = {0.0f,0.0f};
65 float Y[4] = {0.0f, 0.0f, 0.0f, 0.0f};
66 float Y_att[3] = {0.0f, 0.0f, 0.0f};
67 float theta_tilde[3] ={0.0f, 0.0f, 0.0f};
68 float theta[3] ={0.0f, 0.0f, 0.0f};
69 float u_hat_att[3] = {0.0f, 0.0f,0.0f};
70 float u_pid_att[3] = {0.0f, 0.0f,0.0f};
71 float u_pd_att[3] = {0.0f, 0.0f, 0.0f};
72
73 // saturation

166
 74 float p = 0.001f; // defines the boundary layer //this helps ...
with chattering but the corresponding translation parameters ...
don't work with it
75 float dist = 1.0f;
76 float k = 2.0f;
77 float delta[3] = {0.0f, 0.0f, 0.0f};
78
79 void sat(float, float, float);
80
81 // x, y translation
82 void ctrl_PID_SMC_position(float s[], float s_dot[], float ...
s_int[], float old_s[], float delta_t, float Ki_hat_dot[], ...
float old_Ki_hat_dot[], float Kd_hat_dot[], float ...
old_Kd_hat_dot[], float a_hat_dot[], float old_a_hat_dot[], ...
float *ctrlStateError, float *prev_ctrlStateError, float ...
*ctrlControl, float *ctrlState, float Ki_hat[], float ...
Kd_hat[], float a_hat[]){ //add more inputs
83
84 // compute s_int
85 for(i=0; i<n; i++){
86 s_int[i] = s_int[i] + delta_t*(old_s[i] + s[i])/2;
87 }
88
89 // compute s
90 for(i=0; i<n; i++){
91 s[i] = -(ctrlStateError[i+VEL_X] + lambda*ctrlStateError[i]);
92 }
93
94 // compute s_dot
95 for(i=0; i<n; i++){
96 s_dot[i] = -((ctrlStateError[i+VEL_X] - ...
prev_ctrlStateError[i+VEL_X])/(delta_t) + ...
lambda*ctrlStateError[i+VEL_X]);
97 }
98
99 // construct Y for x
100 Y[0] = ctrlState[VEL_X];
101 Y[1] = ctrlState[POS_X];
102 // construct Y for y
103 Y[2] = ctrlState[VEL_Y];
104 Y[3] = ctrlState[POS_Y];
105
106 // adaptation laws
107 for(i=0; i<n; i++){
108 Ki_hat_dot[i] = gamma1*s[i]*s_int[i];
109 Kd_hat_dot[i] = gamma2*s[i]*s_dot[i];
110 }
111
112 //is below correct?
113 for(i=0; i<2; i++){
114 a_hat_dot[i] = 0.0f;
115 a_hat_dot[i+2] = 0.0f;
116 for(j=0; j<2; j++){
117 // for x

167
118 a_hat_dot[i] = a_hat_dot[i] - P*Y[j]*s[0];
119 // for y
120 a_hat_dot[i+2] = a_hat_dot[i+2] - P*Y[j+2]*s[1];
121 }
122 }
123
124 // integrate
125 for(i=0; i<n; i++){
126 Ki_hat[i] = Ki_hat[i] + delta_t*(old_Ki_hat_dot[i] + ...
Ki_hat_dot[i])/2;
127 Kd_hat[i] = Kd_hat[i] + delta_t*(old_Kd_hat_dot[i] + ...
Kd_hat_dot[i])/2;
128 }
129

130 for(i=0; i<2; i++){

131 a_hat[i] += delta_t*(old_a_hat_dot[i] + a_hat_dot[i])/2;
132 a_hat[i+2] += delta_t*(old_a_hat_dot[i+2] + ...
a_hat_dot[i+2])/2;
133 }
134

135 // find xr_ddot

136 for(i=0; i<n; i++){
137 xr_ddot[i] = lambda*ctrlStateError[i+VEL_X];
138 }
139
140 // u_hat = xr_ddot + Y*a_hat
141 u_hat[0] = Y[0]*a_hat[0] + Y[1]*a_hat[1] + xr_ddot[0];
142 u_hat[1] = Y[2]*a_hat[2] + Y[3]*a_hat[3] + xr_ddot[1];
143
144 // construct u_pid
145 for(i=0; i<n; i++){
146 u_pid[i] = Kp*(s[i])+ Kd_hat[i]*(s_dot[i])+ ...
Ki_hat[i]*(s_int[i]);
147 }
148
149 // update u = u_hat - u_pid
150 ctrlControl[FORCE_X] = 0;
151 ctrlControl[FORCE_Y] = 0;
152 ctrlControl[FORCE_X] = u_hat[0] - u_pid[0];
153 ctrlControl[FORCE_Y] = u_hat[1] - u_pid[1];
154
155 }
156
157 void ctrl_PID_SMC_attitude(float s_att[], float s_dot_att[], ...
float s_int_att[], float old_s_att[], float delta_t, float ...
Ki_hat_dot_att[], float old_Ki_hat_dot_att[], float ...
Kd_hat_dot_att[], float old_Kd_hat_dot_att[], float ...
a_hat_dot_att[], float old_a_hat_dot_att[], float ...
*ctrlStateError, float *prev_ctrlStateError, float ...
*ctrlControl, float *ctrlState, float *ctrlStateTarget, float ...
Ki_hat_att[], float Kd_hat_att[], float a_hat_att[]){
158
159 // convert to Euler angles

168
160 theta_tilde[0] = atan2( ...
2*(ctrlStateError[QUAT_4]*ctrlStateError[QUAT_1] + ...
ctrlStateError[QUAT_2]*ctrlStateError[QUAT_3]), ...
1-2*(ctrlStateError[QUAT_1]*ctrlStateError[QUAT_1] + ...
ctrlStateError[QUAT_2]*ctrlStateError[QUAT_2]) );
161 theta_tilde[1] = asin( ...
2*(ctrlStateError[QUAT_4]*ctrlStateError[QUAT_2] - ...
ctrlStateError[QUAT_3]*ctrlStateError[QUAT_1]) );
162 theta_tilde[2] = atan2( ...
2*(ctrlStateError[QUAT_4]*ctrlStateError[QUAT_3] + ...
ctrlStateError[QUAT_1]*ctrlStateError[QUAT_2]), ...
1-2*(ctrlStateError[QUAT_2]*ctrlStateError[QUAT_2] + ...
ctrlStateError[QUAT_3]*ctrlStateError[QUAT_3]) );
163

164 theta[0] = atan2( 2*(ctrlState[QUAT_4]*ctrlState[QUAT_1] + ...

ctrlState[QUAT_2]*ctrlState[QUAT_3]), ...
1-2*(ctrlState[QUAT_1]*ctrlState[QUAT_1] + ...
ctrlState[QUAT_2]*ctrlState[QUAT_2]) );
165 theta[1] = asin( 2*(ctrlState[QUAT_4]*ctrlState[QUAT_2] - ...
ctrlState[QUAT_3]*ctrlState[QUAT_1]) );
166 theta[2] = atan2( 2*(ctrlState[QUAT_4]*ctrlState[QUAT_3] + ...
ctrlState[QUAT_1]*ctrlState[QUAT_2]), ...
1-2*(ctrlState[QUAT_2]*ctrlState[QUAT_2] + ...
ctrlState[QUAT_3]*ctrlState[QUAT_3]) );
167
168

169 // compute s_int_att

170 for(i=0; i<3; i++){
171 s_int_att[i] = s_int_att[i] + delta_t*(old_s_att[i] + ...
s_att[i])/2;
172 }
173

174 // compute s_att

175 for(i=0; i<3; i++){
176 s_att[i] = -(ctrlStateError[i+RATE_X] + ...
lambda_att*theta_tilde[i]);
177 }
178

179 // compute s_dot_att

180 for(i=0; i<3; i++){
181 s_dot_att[i] = -((ctrlStateError[i+RATE_X] - ...
prev_ctrlStateError[i+RATE_X])/(delta_t) + ...
lambda_att*ctrlStateError[i+RATE_X]);
182 }
183
184 // construct Y_att = theta_ddot_desired - lambda*theta_tilde
185 // theta_ddot_desired = 0
186 for(i=0; i<3; i++){
187 Y_att[i] = lambda_att*theta_tilde[i];
188 }
189
190 // adaptation laws
191 for(i=0; i<3; i++){
192 Ki_hat_dot_att[i] = gamma3*s_att[i]*s_int_att[i];

169
193 Kd_hat_dot_att[i] = gamma4*s_att[i]*s_dot_att[i];
194 }
195
196 a_hat_dot_att[0] = -P_att * Y_att[0] * s_att[0];
197 a_hat_dot_att[1] = -P_att * Y_att[1] * s_att[0];
198 a_hat_dot_att[2] = -P_att * Y_att[2] * s_att[0];
199 a_hat_dot_att[3] = -P_att * Y_att[0] * s_att[1];
200 a_hat_dot_att[4] = -P_att * Y_att[1] * s_att[1];
201 a_hat_dot_att[5] = -P_att * Y_att[2] * s_att[1];
202 a_hat_dot_att[6] = -P_att * Y_att[0] * s_att[2];
203 a_hat_dot_att[7] = -P_att * Y_att[1] * s_att[2];
204 a_hat_dot_att[8] = -P_att * Y_att[2] * s_att[2];
205
206 // integrate
207 for(i=0; i<3; i++){
208 Ki_hat_att[i] = Ki_hat_att[i] + ...
delta_t*(old_Ki_hat_dot_att[i] + Ki_hat_dot_att[i])/2;
209 Kd_hat_att[i] = Kd_hat_att[i] + ...
delta_t*(old_Kd_hat_dot_att[i] + Kd_hat_dot_att[i])/2;
210 a_hat_att[i] += delta_t*(old_a_hat_dot_att[i] + ...
a_hat_dot_att[i])/2;
211 }
212
213 // u_hat = Y_att*a_hat_att
214 u_hat_att[0] = Y_att[0]*a_hat_att[0] + Y_att[0]*a_hat_att[1] ...
+ Y_att[0]*a_hat_att[2];
215 u_hat_att[1] = Y_att[1]*a_hat_att[3] + Y_att[1]*a_hat_att[4] ...
+ Y_att[1]*a_hat_att[5];
216 u_hat_att[2] = Y_att[2]*a_hat_att[6] + Y_att[2]*a_hat_att[7] ...
+ Y_att[2]*a_hat_att[8];
217
218 // construct u_pid
219 for(i=0; i<3; i++){
220 u_pid_att[i] = Kp_att*(s_att[i])+ ...
Kd_hat_att[i]*(s_dot_att[i])+ ...
Ki_hat_att[i]*(s_int_att[i]);
221 }
222

223 // update u = u_hat - u_pid

224 ctrlControl[TORQUE_X] = 0;
225 ctrlControl[TORQUE_Y] = 0;
226 ctrlControl[TORQUE_Z] = 0;
227 ctrlControl[TORQUE_X] = u_hat_att[0] - u_pid_att[0];
228 ctrlControl[TORQUE_Y] = u_hat_att[1] - u_pid_att[1];
229 ctrlControl[TORQUE_Z] = u_hat_att[2] - u_pid_att[2];
230 }
231
232 void sat(float n, float lower, float upper) {
233
234 if(n > upper){ n = upper; }
235 if( n < lower) { n = lower; }
236 }

170
Appendix C

Docked Dynamics Functions

C.1 Contact Dynamics of Docking Event

Listing C.1: FindImpulsiveFT.m

1 % Inertial frame's origin is defined at the point where the ...
satellites dock
2 % Body-fixed frame of each sat has positive x through docking port
3
4 syms m rho x y z vx vy vz theta3 theta3d theta3dd dt real
5 syms theta1d theta2d real
6
7 %%% Chaser setup
8 % Inputs
9 MassChaser_kg = m;
10 PosDPtoChaserCMInInertial_m = [x;y;z];
11 AngVelChaserInCHA_rps = [theta1d; theta2d; theta3d];
12 AngChaserCMInInertial = [0; 0; theta3 + pi];
13 QuatInertialToCHA = EulerAnglesZYXToQuat(AngChaserCMInInertial);
14 QuatCHAToInertial = quatconj(QuatInertialToCHA);
15 VelChaserInCHA_mps = cross(AngVelChaserInCHA_rps, ...
16 QuatRotate(QuatInertialToCHA, PosDPtoChaserCMInInertial_m')');
17 % Computations
18 % simplification for a sphere
19 MOIChaser_kgm2 = (2/5) * MassChaser_kg * rho^2 * eye(3);
20 AngMomChaserInCHA_kgm2ps = MOIChaser_kgm2 * AngVelChaserInCHA_rps;
21
22 %%% Target setup
23 % Inputs
24 MassTarget_kg = m;
25 PosDPtoTargetCMInInertial_m = [-x; -y; z];
26 VelTargetInitInTAR_mps = [0; 0; 0];
27 AngVelTargetInTAR_rps = AngVelChaserInCHA_rps;
28 AngTargetCMInInertial = [0; 0; theta3];
29 QuatInertialToTAR = EulerAnglesZYXToQuat(AngTargetCMInInertial);
30 QuatTARToInertial = quatconj(QuatInertialToTAR);
31 % Computations

171
32 % simplification for a sphere
33 MOITarget_kgm2 = (2/5) * MassTarget_kg * rho^2 * eye(3);
34 PosTargetToChaserInInertial_m = PosDPtoChaserCMInInertial_m - ...
PosDPtoTargetCMInInertial_m;
35 VelTargetToChaserInInertial_mps = QuatRotate(QuatCHAToInertial, ...
VelChaserInCHA_mps')' ...
36 - QuatRotate(QuatTARToInertial,VelTargetInitInTAR_mps')';
37 AngMomTargetInTAR_kgm2ps = MOITarget_kgm2 * AngVelTargetInTAR_rps;
38
39 % Aggregated system
40 PosDPtoCMCInInertial_m = (MassChaser_kg * ...
PosDPtoChaserCMInInertial_m + ...
41 MassTarget_kg * PosDPtoTargetCMInInertial_m) / (MassChaser_kg ...
+ MassTarget_kg);
42 RSkewSymMatrixChaser_m = [0, -PosDPtoChaserCMInInertial_m(3), ...
PosDPtoChaserCMInInertial_m(2); ...
PosDPtoChaserCMInInertial_m(3), 0, ...
-PosDPtoChaserCMInInertial_m(1); ...
43 -PosDPtoChaserCMInInertial_m(2), ...
PosDPtoChaserCMInInertial_m(1), 0];
44 RSkewSymMatrixTarget_m = [0, -PosDPtoTargetCMInInertial_m(3), ...
PosDPtoTargetCMInInertial_m(2); ...
PosDPtoTargetCMInInertial_m(3), 0, ...
-PosDPtoTargetCMInInertial_m(1); ...
45 -PosDPtoTargetCMInInertial_m(2), ...
PosDPtoTargetCMInInertial_m(1), 0];
46 MOICMC_kgm2 = (MOIChaser_kgm2 - MassChaser_kg * ...
RSkewSymMatrixChaser_m*RSkewSymMatrixChaser_m) + ...
47 (MOITarget_kgm2 - MassTarget_kg * ...
RSkewSymMatrixTarget_m*RSkewSymMatrixTarget_m);
48
49 PosCMCtoTargetCMInInertial_m = -PosDPtoCMCInInertial_m + ...
PosDPtoTargetCMInInertial_m;
50
51 % Constants
52 MassRatio = MassChaser_kg / MassTarget_kg;
53
54 % Relationships
55 VelocityCMCInInertial_mps = (MassChaser_kg * ...
QuatRotate(QuatCHAToInertial, VelChaserInCHA_mps')' + ...
56 MassTarget_kg * QuatRotate(QuatTARToInertial, ...
VelTargetInitInTAR_mps')') / ...
57 (MassChaser_kg + MassTarget_kg);
58 %VelTarget_mps = VelTargetInitial_mps - VelocityCMC_mps;
59
60 %%% After dock
61 AngVelCMCInInertial_rps = (MOICMC_kgm2) \ ...
(QuatRotate(QuatTARToInertial, AngMomTargetInTAR_kgm2ps')' ...
62 + QuatRotate(QuatCHAToInertial, AngMomChaserInCHA_kgm2ps')' + ...
63 cross(MassChaser_kg * (1 + MassRatio)^(-1) * ...
PosTargetToChaserInInertial_m, ...
VelTargetToChaserInInertial_mps));
64 VelPostDockTargetInInertial_mps = VelocityCMCInInertial_mps + ...
65 cross(AngVelCMCInInertial_rps, PosCMCtoTargetCMInInertial_m);

172
66
67 % want to minimize this impulsive force if Target is fragile
68 ForceImpulsiveInInertial_N = simplify(MassTarget_kg * ...
(VelPostDockTargetInInertial_mps - ...
69 QuatRotate(QuatTARToInertial, VelTargetInitInTAR_mps')') / dt);
70 TorqueImpulsiveInInertial_Nm =simplify( MOITarget_kgm2 * ...
(AngVelCMCInInertial_rps - ...
71 QuatRotate(QuatTARToInertial, AngVelTargetInTAR_rps')') / dt ...
- ...
72 cross(PosDPtoTargetCMInInertial_m, ...
ForceImpulsiveInInertial_N) );

Listing C.2: SolveImpulsiveForcesAndTorques.m

1 function [ForceImpulsiveInInertial_N,...
2 TorqueImpulsiveInInertial_Nm] = ...
SolveImpulsiveForcesAndTorques(Chaser, Target, dt)
3
4 %%% Chaser setup
5 PosDPtoChaserCMInInertial_m = [Chaser.x2_0; Chaser.y2_0; ...
Chaser.z2_0];
6 AngVelChaserInCHA_rps = [Target.roll1d_0; Target.pitch1d_0; ...
Target.yaw1d_0];
7 QuatInertialToCHA = EulerAnglesZYXToQuat(Chaser.EulerAngles_rad);
8 QuatCHAToInertial = quatconj(QuatInertialToCHA);
9 VelChaserInCHA_mps = cross(AngVelChaserInCHA_rps, ...
10 QuatRotate(QuatInertialToCHA, PosDPtoChaserCMInInertial_m')');
11 AngMomChaserInCHA_kgm2ps = Chaser.MOI_kgm2 * AngVelChaserInCHA_rps;
12
13 %%% Target setup
14 PosDPtoTargetCMInInertial_m = [Target.x1_0; Target.y1_0; ...
Target.z1_0];
15 VelTargetInitInTAR_mps = [Target.x1d_0; Target.y1d_0; Target.z1d_0];
16 AngVelTargetInTAR_rps = [Target.roll1d_0; Target.pitch1d_0; ...
Target.yaw1d_0];
17 QuatInertialToTAR = EulerAnglesZYXToQuat(Target.EulerAngles_rad);
18 QuatTARToInertial = quatconj(QuatInertialToTAR);
19 % Computations
20 PosTargetToChaserInInertial_m = PosDPtoChaserCMInInertial_m - ...
PosDPtoTargetCMInInertial_m;
21 VelTargetToChaserInInertial_mps = QuatRotate(QuatCHAToInertial, ...
VelChaserInCHA_mps')' ...
22 - QuatRotate(QuatTARToInertial,VelTargetInitInTAR_mps')';
23 AngMomTargetInTAR_kgm2ps = Target.MOI_kgm2 * AngVelTargetInTAR_rps;
24
25 % Aggregated system
26 PosDPtoCMCInInertial_m = (Chaser.mass_kg * ...
PosDPtoChaserCMInInertial_m + ...
27 Target.mass_kg * PosDPtoTargetCMInInertial_m) / ...
(Chaser.mass_kg + Target.mass_kg);
28 RSkewSymMatrixChaser_m = [0, -PosDPtoChaserCMInInertial_m(3), ...
PosDPtoChaserCMInInertial_m(2); ...
PosDPtoChaserCMInInertial_m(3), 0, ...

173
 -PosDPtoChaserCMInInertial_m(1); ...
29 -PosDPtoChaserCMInInertial_m(2), ...
PosDPtoChaserCMInInertial_m(1), 0];
30 RSkewSymMatrixTarget_m = [0, -PosDPtoTargetCMInInertial_m(3), ...
PosDPtoTargetCMInInertial_m(2); ...
PosDPtoTargetCMInInertial_m(3), 0, ...
-PosDPtoTargetCMInInertial_m(1); ...
31 -PosDPtoTargetCMInInertial_m(2), ...
PosDPtoTargetCMInInertial_m(1), 0];
32 MOICMC_kgm2 = (Chaser.MOI_kgm2 - Chaser.mass_kg * ...
RSkewSymMatrixChaser_m*RSkewSymMatrixChaser_m) + ...
33 (Target.MOI_kgm2 - Target.mass_kg * ...
RSkewSymMatrixTarget_m*RSkewSymMatrixTarget_m);
34

35 PosCMCtoTargetCMInInertial_m = -PosDPtoCMCInInertial_m + ...

PosDPtoTargetCMInInertial_m;
36
37 % Constants
38 MassRatio = Chaser.mass_kg / Target.mass_kg;
39

40 % Relationships
41 VelocityCMCInInertial_mps = (Chaser.mass_kg * ...
QuatRotate(QuatCHAToInertial, VelChaserInCHA_mps')' + ...
42 Target.mass_kg * QuatRotate(QuatTARToInertial, ...
VelTargetInitInTAR_mps')') / ...
43 (Chaser.mass_kg + Target.mass_kg);
44
45 %%% Post dock
46 AngVelCMCInInertial_rps = (MOICMC_kgm2) \ ...
(QuatRotate(QuatTARToInertial, AngMomTargetInTAR_kgm2ps')' ...
47 + QuatRotate(QuatCHAToInertial, AngMomChaserInCHA_kgm2ps')' + ...
48 cross(Chaser.mass_kg * (1 + MassRatio)^(-1) * ...
PosTargetToChaserInInertial_m, ...
VelTargetToChaserInInertial_mps));
49 VelPostDockTargetInInertial_mps = VelocityCMCInInertial_mps + ...
50 cross(AngVelCMCInInertial_rps, PosCMCtoTargetCMInInertial_m);
51
52 % Impulsive Force and Torque
53 ForceImpulsiveInInertial_N = (Target.mass_kg * ...
(VelPostDockTargetInInertial_mps - ...
54 QuatRotate(QuatTARToInertial, VelTargetInitInTAR_mps')') / dt);
55 TorqueImpulsiveInInertial_Nm = ( Target.MOI_kgm2 * ...
(AngVelCMCInInertial_rps - ...
56 QuatRotate(QuatTARToInertial, AngVelTargetInTAR_rps')') / dt ...
- ...
57 cross(PosDPtoTargetCMInInertial_m, ...
ForceImpulsiveInInertial_N) );

C.2 Setting up Target and Chaser

174
 Listing C.3: SetupTargetParams.m
1 function Target = SetupTargetParams(mass, CM2DPDistance, ...
CMToEdgeY_m, CMToEdgeZ_m, TargetShapeType, AngVelVec_rad)
2

3 % Coordinates
4 i = [1 0 0];
5 j = [0 1 0];
6 k = [0 0 1];
7
8 % Target mass
9 Target.mass_kg = mass;
10 % Distance from Target's CM to the docking port interface
11 Target.CM2DPinX_m = CM2DPDistance;
12
13 % Distances from CM to edges
14 Target.CMToEdgeZ_m = CMToEdgeZ_m;
15 Target.CMToEdgeY_m = CMToEdgeY_m;
16 Target.CMToEdgeX_m = Target.CM2DPinX_m;
17
18 % Set up inertia matrix based on shape type
19 if strcmp(TargetShapeType, 'cylinder')
20 Target.MOI_kgm2 = [1/2*Target.mass_kg*Target.CMToEdgeY_m^2, ...
0, 0;...
21 0, 1/12*Target.mass_kg*(3*Target.CMToEdgeY_m^2 + ...
Target.CMToEdgeX_m^2), 0;...
22 0, 0, 1/12*Target.mass_kg*(3*Target.CMToEdgeY_m^2 + ...
Target.CMToEdgeX_m^2)];
23 elseif strcmp(TargetShapeType, 'sphere')
24 Target.MOI_kgm2 = ...
(2/5)*Target.mass_kg*Target.CMToEdgeX_m^2*eye(3);
25 elseif strcmp(TargetShapeType, 'rect prism')
26 Target.MOI_kgm2 = (1/12)*Target.mass_kg*...
27 [((2*Target.CMToEdgeY_m)^2+(2*Target.CMToEdgeZ_m)^2), 0, ...
0;...
28 0, ((2*Target.CMToEdgeZ_m)^2+(2*Target.CMToEdgeX_m)^2), 0;...
29 0, 0, ((2*Target.CMToEdgeY_m)^2+(2*Target.CMToEdgeX_m)^2)];
30 end
31
32 % Simplified notation for following calcultions
33 Ixx = Target.MOI_kgm2(1,1); Iyy = Target.MOI_kgm2(2,2); Izz = ...
Target.MOI_kgm2(3,3);
34
35 % Initial Conditions
36 % Initial angles should always be equal to 0 since the assumption ...
is that
37 % the global frame = the body frame at the start of this phase; ...
so they are
38 % not included as an option for ICs
39 Target.roll1d_0 = AngVelVec_rad(1); %wx
40 Target.pitch1d_0 = AngVelVec_rad(2); %wy
41 Target.yaw1d_0 = AngVelVec_rad(3); %wz
42
43 % Initial angular accelerations

175
44 Target.roll1dd_0 = (Iyy-Izz)*Target.pitch1d_0*Target.yaw1d_0/Ixx;
45 Target.pitch1dd_0 = (Izz-Ixx)*Target.roll1d_0*Target.yaw1d_0/Iyy;
46 Target.yaw1dd_0 = (Ixx-Iyy)*Target.pitch1d_0*Target.roll1d_0/Izz;
47
48 % Angular velocity construction in Target
49 Target.BodyToInertialRot = FindRotMat(0, 0, 0);
50 omega_123 = [Target.roll1d_0; Target.pitch1d_0; Target.yaw1d_0];
51 Target.omega_xyz = Target.BodyToInertialRot*omega_123;
52
53 % Notation in inertial frame for determining ICs
54 %r1_xyz = [(-Target.CM2DPinX_m);0;0];
55 r1_xyz = [0;0;0];
56 v1_xyz = cross(Target.omega_xyz, r1_xyz);
57

58 Target.x1_0 = dot(r1_xyz,i);
59 Target.x1d_0 = dot(v1_xyz,i); % Change to 0 for no translation
60 Target.y1_0 = dot(r1_xyz,j);
61 Target.y1d_0 = dot(v1_xyz,j); % Change to 0 for no translation
62 Target.z1_0 = dot(r1_xyz,k);
63 Target.z1d_0 = dot(v1_xyz,k); % Change to 0 for no translation
64
65 Target.shape = TargetShapeType;
66
67 end

Listing C.4: SetupChaserParams.m

1 function Chaser = SetupChaserParams(mass, CM2DPDistance, ...
CMToEdgeY_m, CMToEdgeZ_m, ChaserShapeType, Target)
2
3 % Coordinate frame
4 i = [1 0 0];
5 j = [0 1 0];
6 k = [0 0 1];
7
8 % Chaser mass
9 Chaser.mass_kg = mass;
10 % Distance from Chaser's CM to docking port interface
11 Chaser.CM2DPinX_m = CM2DPDistance;
12
13 % Distances from CM to edges
14 Chaser.CMToEdgeZ_m = CMToEdgeZ_m;
15 Chaser.CMToEdgeY_m = CMToEdgeY_m;
16 Chaser.CMToEdgeX_m = Chaser.CM2DPinX_m;
17
18 % Set up inertia matrix
19 if strcmp(ChaserShapeType, 'cylinder')
20 Chaser.MOI_kgm2 = [1/2*Chaser.mass_kg*Chaser.CMToEdgeY_m^2, ...
0, 0;...
21 0, 1/12*Chaser.mass_kg*(3*Chaser.CMToEdgeY_m^2 + ...
Chaser.CMToEdgeX_m^2), 0;...
22 0, 0, 1/12*Chaser.mass_kg*(3*Chaser.CMToEdgeY_m^2 + ...
Chaser.CMToEdgeX_m^2)];

176
23 elseif strcmp(ChaserShapeType, 'sphere')
24 Chaser.MOI_kgm2 = ...
(2/5)*Chaser.mass_kg*Chaser.CMToEdgeX_m^2*eye(3);
25 elseif strcmp(ChaserShapeType, 'rect prism')
26 Chaser.MOI_kgm2 = (1/12)*Chaser.mass_kg*...
27 [((2*Chaser.CMToEdgeY_m)^2+(2*Chaser.CMToEdgeZ_m)^2), 0, ...
0;...
28 0, ((2*Chaser.CMToEdgeZ_m)^2+(2*Chaser.CMToEdgeX_m)^2), 0;...
29 0, 0, ((2*Chaser.CMToEdgeY_m)^2+(2*Chaser.CMToEdgeX_m)^2)];
30 end
31
32 % Set up initial conditions
33 R = (Target.CM2DPinX_m+Chaser.CM2DPinX_m);
34 r2_xyz = (Target.BodyToInertialRot*[R;0;0]);
35 v2_xyz = cross(Target.omega_xyz, r2_xyz);
36
37 Chaser.x2_0 = dot(r2_xyz,i) + Target.x1_0;
38 Chaser.x2d_0 = dot(v2_xyz,i) + Target.x1d_0;
39 Chaser.y2_0 = dot(r2_xyz,j) + Target.y1_0;
40 Chaser.y2d_0 = dot(v2_xyz,j)+ Target.y1d_0;
41 Chaser.z2_0 = dot(r2_xyz,k) + Target.z1_0;
42 Chaser.z2d_0 = dot(v2_xyz,k) + Target.z1d_0;
43 Chaser.shape = ChaserShapeType;
44
45 end

C.3 Discrete Solver

Listing C.5: AggregatedDynamicsDiscreteSolver.m

1 function [out,tspan] = ...
AggregatedDynamicsDiscreteSolver(Target,Chaser,tspan,term_type)
2
3 n = length(tspan);
4 % initial conditions + setup
5 qw = zeros(2,1); qw(1) = 1;
6 qx = zeros(2,1); qy = zeros(2,1); qz = zeros(2,1);
7 qwdot = zeros(2,1); qxdot = zeros(2,1);
8 qydot = zeros(2,1); qzdot = zeros(2,1);
9 yaw1 = zeros(2,1); yaw1d = zeros(2,1);
10 yaw1d(1) = Target.yaw1d_0; yaw1dd = zeros(2,1); yaw1dd(1) = ...
Target.yaw1dd_0;
11 pitch1 = zeros(2,1); pitch1d = zeros(2,1);
12 pitch1d(1) = Target.pitch1d_0; pitch1dd = zeros(2,1); pitch1dd(1) ...
= Target.pitch1dd_0;
13 roll1 = zeros(2,1); roll1d = zeros(2,1);
14 roll1d(1) = Target.roll1d_0; roll1dd = zeros(2,1); roll1dd(1) = ...
Target.roll1dd_0;
15 x1 = zeros(2,1); x1(1) = Target.x1_0; x1d = zeros(2,1);
16 x1d(1) = Target.x1d_0; x1dd = zeros(2,1); x1dd(1) = 0;

177
17 y1 = zeros(2,1); y1(1) = Target.y1_0; y1d = zeros(2,1);
18 y1d(1) = Target.y1d_0; y1dd = zeros(2,1); y1dd(1) = 0;
19 z1 = zeros(2,1); z1(1) = Target.z1_0; z1d = zeros(2,1);
20 z1d(1) = Target.z1d_0; z1dd = zeros(2,1); z1dd(1) = 0;
21 x2 = zeros(2,1); x2(1) = Chaser.x2_0; x2d = zeros(2,1);
22 x2d(1) = Chaser.x2d_0; x2dd = zeros(2,1); x2dd(1) = 0;
23 y2 = zeros(2,1); y2(1) = Chaser.y2_0; y2d = zeros(2,1);
24 y2d(1) = Chaser.y2d_0; y2dd = zeros(2,1); y2dd(1) = 0;
25 z2 = zeros(2,1); z2(1) = Chaser.z2_0; z2d = zeros(2,1);
26 z2d(1) = Chaser.z2d_0; z2dd = zeros(2,1); z2dd(1) = 0;
27 BodyToInertialRot = zeros(3,3,n); BodyToInertialRot(:,:,1) = ...
FindRotMat(0, 0, 0);
28 CurrentB2IRot = BodyToInertialRot;
29 CentAccInBody_mpsps = zeros(3,2);
30 AngAccInBody_mpsps = zeros(3,2);
31 Mx = zeros(2,1); Fz = zeros(2,1); Fth = zeros(2,1);
32 if isfield(Chaser,'roll1dd_user')
33 roll1dd_user = Chaser.roll1dd_user;
34 pitch1dd_user = Chaser.pitch1dd_user;
35 yaw1dd_user = Chaser.yaw1dd_user;
36 end
37
38 t_wait = 1;
39 i = 1;
40 tol = 0.001;
41 h = tspan(2)-tspan(1); %assumes linspace
42 R = (Target.CM2DPinX_m + Chaser.CM2DPinX_m);
43 Ixx = Target.MOI_kgm2(1,1); Iyy = Target.MOI_kgm2(2,2); Izz = ...
Target.MOI_kgm2(3,3);
44 rollstopped = 0;
45 RInBody = R*[1;0;0];
46

47 while (abs(yaw1d(i)) ≥ tol || abs(pitch1d(i)) ≥ tol || ...

abs(roll1d(i)) ≥ tol)
48
49 % Determine double dot terms
50 if i̸=1
51 roll1dd(i) = (Iyy-Izz)*pitch1d(i)*yaw1d(i)/Ixx;
52 pitch1dd(i) = (Izz-Ixx)*roll1d(i)*yaw1d(i)/Iyy;
53 yaw1dd(i) = (Ixx-Iyy)*pitch1d(i)*roll1d(i)/Izz;
54 end
55 xdd_trans = 0; ydd_trans = 0; zdd_trans = 0;
56 if tspan(i) ≥ t_wait % start the process of stopping the Target
57 %If nutating, we want to stop roll1d first before ...
stopping yaw1d,
58 %pitch1d
59 if strcmp(term_type, 'user_defined')
60 roll1dd(i) = roll1dd_user(i) + roll1dd(i);
61 pitch1dd(i) = pitch1dd_user(i) + pitch1dd(i);
62 yaw1dd(i) = yaw1dd_user(i) + yaw1dd(i);
63 else
64 if (Ixx == Iyy) && (Iyy == Izz)
65 % Target is a sphere; no nutation
66 if strcmp(term_type, 'NegQuadratic')

178
 67 yaw1dd_slowdown = ...
NegativeQuadraticTerm(Target,...
68 Target.yaw1d_0, t_wait, tspan(i));
69 roll1dd_slowdown = ...
NegativeQuadraticTerm(Target,...
70 Target.roll1d_0, t_wait, tspan(i));
71 pitch1dd_slowdown = ...
NegativeQuadraticTerm(Target,...
72 Target.pitch1d_0, t_wait, tspan(i));
73 elseif strcmp(term_type, 'PosLinear')
74 yaw1dd_slowdown = PositiveLinearTerm(Target,...
75 Target.yaw1d_0, t_wait, tspan(i));
76 roll1dd_slowdown = PositiveLinearTerm(Target,...
77 Target.roll1d_0, t_wait, tspan(i));
78 pitch1dd_slowdown = PositiveLinearTerm(Target,...
79 Target.pitch1d_0, t_wait, tspan(i));
80 elseif strcmp(term_type, 'NegLinear')
81 yaw1dd_slowdown = NegativeLinearTerm(Target,...
82 Target.yaw1d_0, t_wait, tspan(i));
83 roll1dd_slowdown = NegativeLinearTerm(Target,...
84 Target.roll1d_0, t_wait, tspan(i));
85 pitch1dd_slowdown = NegativeLinearTerm(Target,...
86 Target.pitch1d_0, t_wait, tspan(i));
87 elseif strcmp(term_type, 'Constant')
88 yaw1dd_slowdown = ConstantAccTerm(Target,...
89 Target.yaw1d_0);
90 roll1dd_slowdown = ConstantAccTerm(Target,...
91 Target.roll1d_0);
92 pitch1dd_slowdown = ConstantAccTerm(Target,...
93 Target.pitch1d_0);
94 end
95 roll1dd(i) = roll1dd_slowdown + roll1dd(i);
96 pitch1dd(i) = pitch1dd_slowdown + pitch1dd(i);
97 yaw1dd(i) = yaw1dd_slowdown + yaw1dd(i);
98 else
99 %Target may be nutating
100 if abs(roll1d(i)) > tol/10
101 if strcmp(term_type, 'NegQuadratic')
102 roll1dd_slowdown = ...
NegativeQuadraticTerm(Target,...
103 Target.roll1d_0, t_wait, tspan(i));
104 elseif strcmp(term_type, 'PosLinear')
105 roll1dd_slowdown = ...
PositiveLinearTerm(Target,...
106 Target.roll1d_0, t_wait, tspan(i));
107 elseif strcmp(term_type, 'NegLinear')
108 roll1dd_slowdown = ...
NegativeLinearTerm(Target,...
109 Target.roll1d_0, t_wait, tspan(i));
110 elseif strcmp(term_type, 'Constant')
111 roll1dd_slowdown = ConstantAccTerm(Target,...
112 Target.roll1d_0);
113 end
114 roll1dd(i) = roll1dd_slowdown + roll1dd(i);

179
115 end
116 if abs(roll1d(i)) ≤ tol/10
117 % slow down ang vel in other axes now
118 if rollstopped == 0
119 % save off these ang vels
120 Target.yaw1d_0 = yaw1d(i);
121 Target.pitch1d_0 = pitch1d(i);
122 end
123 if abs(pitch1d(i)) > tol
124 if strcmp(term_type, 'NegQuadratic')
125 pitch1dd_slowdown = ...
NegativeQuadraticTerm(Target,...
126 Target.pitch1d_0, t_wait, tspan(i));
127 elseif strcmp(term_type, 'PosLinear')
128 pitch1dd_slowdown = ...
PositiveLinearTerm(Target,...
129 Target.pitch1d_0, t_wait, tspan(i));
130 elseif strcmp(term_type, 'NegLinear')
131 pitch1dd_slowdown = ...
NegativeLinearTerm(Target,...
132 Target.pitch1d_0, t_wait, tspan(i));
133 elseif strcmp(term_type, 'Constant')
134 pitch1dd_slowdown = ...
ConstantAccTerm(Target,...
135 Target.pitch1d_0);
136 end
137 pitch1dd(i) = pitch1dd_slowdown + ...
pitch1dd(i);
138 end
139 if abs(yaw1d(i)) > tol
140 if strcmp(term_type, 'NegQuadratic')
141 yaw1dd_slowdown = ...
NegativeQuadraticTerm(Target,...
142 Target.yaw1d_0, t_wait, tspan(i));
143 elseif strcmp(term_type, 'PosLinear')
144 yaw1dd_slowdown = ...
PositiveLinearTerm(Target,...
145 Target.yaw1d_0, t_wait, tspan(i));
146 elseif strcmp(term_type, 'NegLinear')
147 yaw1dd_slowdown = ...
NegativeLinearTerm(Target,...
148 Target.yaw1d_0, t_wait, tspan(i));
149 elseif strcmp(term_type, 'Constant')
150 yaw1dd_slowdown = ...
ConstantAccTerm(Target,...
151 Target.yaw1d_0);
152 end
153 yaw1dd(i) = yaw1dd_slowdown + yaw1dd(i);
154 end
155 rollstopped = 1;
156 end
157 end
158 end
159

180
160 % Always occur post t_wait
161 if abs(y1d(i)) > tol
162 ydd_trans = ConstantAccTerm(Target, Target.y1d_0);
163 end
164 if abs(x1d(i)) > tol
165 xdd_trans = ConstantAccTerm(Target, Target.x1d_0);
166 end
167 if abs(z1d(i)) > tol
168 zdd_trans = ConstantAccTerm(Target, Target.z1d_0);
169 end
170 end
171
172 % Quaternion to DCM
173 if i̸=1
174 BodyToInertialRot(:,:,i) = quat2dcm([qw(i), qx(i), qy(i), ...
qz(i)]);
175 BodyToInertialRot(:,:,i) = BodyToInertialRot(:,:,i)';
176 CurrentB2IRot(:,:,i) = BodyToInertialRot(:,:,i);
177 end
178 omega_123 = [roll1d(i); pitch1d(i); yaw1d(i)];
179 alpha_123 = [roll1dd(i); pitch1dd(i); yaw1dd(i)];
180
181 % Necessary for maintaining synchronous motion
182 CentAccInBody_mpsps(:,i) = cross(omega_123, cross(omega_123, ...
RInBody));
183 AngAccInBody_mpsps(:,i) = cross(alpha_123, RInBody);
184
185 CentAccInInertial_mpsps = ...
CurrentB2IRot(:,:,i)*CentAccInBody_mpsps(:,i);
186 AngAccInInertial_mpsps = ...
CurrentB2IRot(:,:,i)*AngAccInBody_mpsps(:,i);
187 TotalAccInInertial_mpsps = CentAccInInertial_mpsps + ...
AngAccInInertial_mpsps;
188
189 x2dd(i) = TotalAccInInertial_mpsps(1) + xdd_trans;
190 y2dd(i) = TotalAccInInertial_mpsps(2) + ydd_trans;
191 z2dd(i) = TotalAccInInertial_mpsps(3) + zdd_trans;
192

193 x1dd(i) = xdd_trans;

194 y1dd(i) = ydd_trans;
195 z1dd(i) = zdd_trans;
196
197 qwdot(i) = -1/2 * (roll1d(i).*qx(i) + pitch1d(i).*qy(i) + ...
yaw1d(i).*qz(i));
198 qxdot(i) = 1/2 * (roll1d(i).*qw(i) + pitch1d(i).*qz(i) - ...
yaw1d(i).*qy(i));
199 qydot(i) = 1/2 * (pitch1d(i).*qw(i) + yaw1d(i).*qx(i) - ...
roll1d(i).*qz(i));
200 qzdot(i) = 1/2 * (yaw1d(i).*qw(i) + roll1d(i).*qy(i) - ...
pitch1d(i).*qx(i));
201
202 % results from TARGET AMB (wrt aggregated system)
203 Hdot = Target.MOICombowrtTargetCM_kgm2*alpha_123 + ...
204 cross(omega_123, Target.MOICombowrtTargetCM_kgm2*omega_123);

181
205 Mx(i) = Hdot(1);
206 Fz(i) = -Hdot(2)/R;
207 Fth(i) = Hdot(3)/R;
208
209 % Update timer
210 i = i+1;
211 if i>n
212 break;
213 end
214
215 % update velocity terms
216 x1d(i) = x1d(i-1) + x1dd(i-1)*h; y1d(i) = y1d(i-1) + y1dd(i-1)*h;
217 z1d(i) = z1d(i-1) + z1dd(i-1)*h;
218 nextstep = CurrentB2IRot(:,:,i-1)*cross(omega_123, RInBody);
219 x2d(i) = x1d(i) + nextstep(1); y2d(i) = y1d(i) + nextstep(2);
220 z2d(i) = z1d(i) + nextstep(3);
221
222 % Update position terms
223 x1(i) = x1(i-1) + x1d(i)*h; y1(i) = y1(i-1) + y1d(i)*h;
224 z1(i) = z1(i-1) + z1d(i)*h;
225 nextstep = CurrentB2IRot(:,:,i-1)*RInBody;
226 x2(i) = x1(i) + nextstep(1); y2(i) = y1(i) + nextstep(2);
227 z2(i) = z1(i) + nextstep(3);
228
229 % update angular velocity
230 yaw1d(i) = yaw1d(i-1) + yaw1dd(i-1)*h;
231 pitch1d(i) = pitch1d(i-1) + pitch1dd(i-1)*h;
232 roll1d(i) = roll1d(i-1) + roll1dd(i-1)*h;
233
234 %update angles, delta angle
235 yaw1(i) = yaw1(i-1) + yaw1d(i)*h;
236 pitch1(i) = pitch1(i-1) + pitch1d(i)*h;
237 roll1(i) = roll1(i-1) + roll1d(i)*h;
238
239 %update quaternion
240 qw(i) = qw(i-1) + qwdot(i-1)*h; qx(i) = qx(i-1) + qxdot(i-1)*h;
241 qy(i) = qy(i-1) + qydot(i-1)*h; qz(i) = qz(i-1) + qzdot(i-1)*h;
242

243 end
244 if i≤n
245 tspan = tspan(1:i);
246 end
247
248 % Pack output
249 out.roll1 = roll1; out.pitch1 = pitch1; out.yaw1 = yaw1;
250 out.roll1d = roll1d; out.pitch1d = pitch1d; out.yaw1d = ...
yaw1d;
251 out.roll1dd = roll1dd; out.pitch1dd = pitch1dd; out.yaw1dd = ...
yaw1dd;
252 out.x1 = x1; out.y1 = y1; out.z1 = z1;
253 out.x2 = x2; out.y2 = y2; out.z2 = z2;
254 out.x1d = x1d; out.y1d = y1d; out.z1d = z1d;
255 out.x2d = x2d; out.y2d = y2d; out.z2d = z2d;
256 out.x1dd = x1dd; out.y1dd = y1dd; out.z1dd = z1dd;

182
257 out.x2dd = x2dd; out.y2dd = y2dd; out.z2dd = z2dd;
258 out.CurrentB2IRot = CurrentB2IRot;
259 out.qw = qw; out.qx = qx; out.qy = qy; out.qz = qz;
260
261 %% Calculate loads/moments
262 TotalForceInBody_N = Chaser.mass_kg*(CentAccInBody_mpsps + ...
263 AngAccInBody_mpsps);
264 out.ShearLoadY_N = TotalForceInBody_N(2,:) - Fth';
265 out.ShearLoadZ_N = TotalForceInBody_N(3,:) - Fz';
266
267 out.Torsion_Nm = Chaser.MOI_kgm2(1,1)*roll1dd - ...
268 (Chaser.MOI_kgm2(2,2)-Chaser.MOI_kgm2(3,3)).*...
269 pitch1d(1:length(roll1dd)).*yaw1d(1:length(roll1dd)) - Mx;
270 out.BendingMomY_Nm = Chaser.MOI_kgm2(2,2)*pitch1dd - ...
271 (Chaser.MOI_kgm2(3,3)-Chaser.MOI_kgm2(1,1)).*...
272 roll1d(1:length(pitch1dd)).*yaw1d(1:length(pitch1dd));
273 out.BendingMomZ_Nm = Chaser.MOI_kgm2(3,3)*yaw1dd - ...
274 (Chaser.MOI_kgm2(1,1)-Chaser.MOI_kgm2(2,2)).*...
275 pitch1d(1:length(yaw1dd)).*roll1d(1:length(yaw1dd));
276

277 p = length(out.ShearLoadY_N);
278 out.int.ShearLoadY_N = cumtrapz(tspan(1:p), abs(out.ShearLoadY_N));
279 out.int.ShearLoadZ_N = cumtrapz(tspan(1:p), abs(out.ShearLoadZ_N));
280 out.int.Torsion_Nm = cumtrapz(tspan(1:p), abs(out.Torsion_Nm));
281 out.int.BendingMomY_Nm = cumtrapz(tspan(1:p), ...
abs(out.BendingMomY_Nm));
282 out.int.BendingMomZ_Nm = cumtrapz(tspan(1:p), ...
abs(out.BendingMomZ_Nm));
283 out.int.TotalLM = out.int.ShearLoadY_N(end) + ...
out.int.ShearLoadZ_N(end) + ...
284 out.int.Torsion_Nm(end) + out.int.BendingMomY_Nm(end)...
285 + out.int.BendingMomZ_Nm(end);
286
287 %% Calculate deltaV
288 n = length(yaw1dd);
289 t = tspan(1:n);
290
291 out.DeltaVRoll_mps = cumtrapz(t, roll1dd);
292 out.DeltaVPitch_mps = cumtrapz(t, pitch1dd);
293 out.DeltaVYaw_mps = cumtrapz(t, yaw1dd);
294 out.TotalDeltaV_mps = out.DeltaVRoll_mps(end) + ...
295 out.DeltaVPitch_mps(end) + out.DeltaVYaw_mps(end);
296
297 end
298
299 function current_acc = ConstantAccTerm(p,init_vel)
300 current_acc = (0 - init_vel) / p.deltaT;
301 end
302
303 function current_acc = ...
NegativeLinearTerm(p,init_vel,t_wait,current_time)
304 a = init_vel/(p.deltaT)^2;
305 current_acc = -2*a*(current_time-t_wait);
306 end

183
307
308 function current_acc = ...
PositiveLinearTerm(p,init_vel,t_wait,current_time)
309 a = init_vel/(p.deltaT)^2;
310 current_acc = 2*a*(current_time-(p.deltaT+t_wait));
311 end
312
313 function current_acc = ...
NegativeQuadraticTerm(p,init_vel,t_wait,current_time)
314 a = init_vel/(p.deltaT)^3;
315 current_acc = -3*a*(current_time-t_wait)^2;
316 end

C.4 Direct Optimization Using GPOPS

Listing C.6: detumble_gpops.m

1 %---Detumbling Target---%
2 clear all; close all; clc
3
4 %--- Provide Auxiliary Data for Problem ---%
5
6 TargetShapeType = 'cylinder'; %options: 'sphere', 'rect prism', ...
'cylinder'
7 ChaserShapeType = TargetShapeType;
8
9 %Target = SetupTargetParams(mass, CM2DPDistance, CMToEdgeY_m, ...
CMToEdgeZ_m, TargetShapeType, AngVelVec_rad)
10 Target = SetupTargetParams(5, 0.10, 0.35, 0.35, TargetShapeType, ...
deg2rad([1 2 3]));
11 % Forcing Target to have no translation
12 Target.x1d_0 = 0;
13 Target.y1d_0 = 0;
14 Target.z1d_0 = 0;
15 Chaser = SetupChaserParams(5, 0.10, 0.35, 0.35, ChaserShapeType, ...
Target);
16
17 disp_vec = [-Target.CM2DPinX_m; 0; 0]; %from CMC to new point
18 Target.MOICombowrtTargetCM_kgm2 = Target.MOI_kgm2 + ...
Chaser.MOI_kgm2 + ...
19 (Target.mass_kg+Chaser.mass_kg)*(dot(disp_vec,disp_vec)*eye(3) ...
- disp_vec*disp_vec');
20
21 auxdata.I_C = Chaser.MOI_kgm2;
22 auxdata.mass_C = Chaser.mass_kg;
23 auxdata.I_T = Target.MOI_kgm2;
24 auxdata.mass_T = Target.mass_kg;
25 auxdata.I_CMCwrtTAR = Target.MOICombowrtTargetCM_kgm2;
26 auxdata.R = (Target.CM2DPinX_m + Chaser.CM2DPinX_m);
27 RInBody = auxdata.R*[1;0;0];

184
28
29

30 %--- Provide All Bounds for Problem ---%

31 t0min = 0; t0max = 0;
32 tfmin = 0; tfmax = 250;
33 % Orientation
34 quat0 = [1 0 0 0];
35 quatfmin = [-1 -1 -1 -1]; quatfmax = [1 1 1 1];
36 quatmin = quatfmin; quatmax = quatfmax;
37 % Angular velocity
38 omega0 = [Target.roll1d_0, Target.pitch1d_0, Target.yaw1d_0];
39 omegaf=[0 0 0];
40 omegamin = deg2rad([-5 -5 -5]); omegamax = deg2rad([5 5 5]);
41 r0 = [Chaser.x2_0, Chaser.y2_0, Chaser.z2_0];
42 rfmin = [Target.x1_0, Target.y1_0, Target.z1_0] - auxdata.R;
43 rfmax = [Target.x1_0, Target.y1_0, Target.z1_0] + auxdata.R;
44 rdot0= [Chaser.x2d_0, Chaser.y2d_0, Chaser.z2d_0];
45 rdotfmin = 0.1*[-1 -1 -1]; rdotfmax = 0.1*[1 1 1];
46 rmin = rfmin; rmax = rfmax;
47 rdotmin = 0.5*[-1 -1 -1]; rdotmax = 0.5*[1 1 1];
48 % control is angular acceleration
49 umin = -0.007*ones(1,3); umax = -umin;
50
51 %--- Setup for Problem Bounds ---%
52 bounds.phase.initialtime.lower = t0min;
53 bounds.phase.initialtime.upper = t0max;
54 bounds.phase.finaltime.lower = tfmin;
55 bounds.phase.finaltime.upper = tfmax;
56 bounds.phase.initialstate.lower = [quat0, omega0, r0, rdot0];
57 bounds.phase.initialstate.upper = [quat0, omega0, r0, rdot0];
58 bounds.phase.state.lower = [quatmin, omegamin, rmin, rdotmin];
59 bounds.phase.state.upper = [quatmax, omegamax, rmax, rdotmax];
60 bounds.phase.finalstate.lower = [quatfmin, omegaf, rfmin, rdotfmin];
61 bounds.phase.finalstate.upper = [quatfmax, omegaf, rfmax, rdotfmax];
62 bounds.phase.control.lower = [umin];
63 bounds.phase.control.upper = [umax];
64 bounds.phase.integral.lower = 0;
65 bounds.phase.integral.upper = [50];
66 % Path constraint for Chaser to stay docked to Target
67 bounds.phase.path.lower = sqrt(Chaser.x2_0^2 + Chaser.y2_0^2 + ...
Chaser.z2_0^2)-0.001/10;
68 bounds.phase.path.upper = sqrt(Chaser.x2_0^2 + Chaser.y2_0^2 + ...
Chaser.z2_0^2)+0.001/10;
69

70 %--- Provide Guess of Solution ---%

71 tGuess = [t0min; 50];
72 quatGuess = [quat0; quatfmax];
73 omegaGuess = [omega0; omegaf];
74 rGuess = [r0; rfmax];
75 rdotGuess = [rdot0; rdotmax];
76 uGuess = [umin; umin];
77 guess.phase.state = [quatGuess, omegaGuess, rGuess, rdotGuess];
78 guess.phase.control = [uGuess];
79 guess.phase.time = [tGuess];

185
 80 guess.phase.integral = 0.01;
81

82 %---Provide Mesh Refinement Method and Initial Mesh ---%

83 mesh.method = 'hp-PattersonRao';
84 % desired mesh error tolerance for optimal solution
85 mesh.tolerance = 1e-5;
86 mesh.maxiterations = 20;
87 mesh.colpointsmin = 4;
88 mesh.colpointsmax = 10;
89 mesh.phase.colpoints = 4;
90
91 %--- Assemble Information into Problem Structure ---%
92 setup.mesh = mesh;
93 setup.name = 'Detumble';
94 setup.functions.endpoint = @detumbleEndpoint;
95 setup.functions.continuous = @detumbleContinuous;
96 setup.displaylevel = 2;
97 setup.auxdata = auxdata;
98 setup.bounds = bounds;
99 setup.guess = guess;
100 setup.nlp.solver = 'ipopt';
101 setup.derivatives.supplier = 'sparseCD';
102 setup.derivatives.derivativelevel = 'second';
103 setup.method = 'RPM-Differentiation';
104 setup.scales.method = 'automatic-bounds';
105 setup.nlp.ipoptoptions.maxiterations = 2000;
106 setup.nlp.ipoptoptions.tolerance = 10e-7;
107
108 %--- Solve Problem Using GPOPS2 ---%
109 output = gpops2(setup);

Listing C.7: detumbleContinuous.m

1 function phaseout = detumbleContinuous(input)
2 % Update dynamics and objective function
3
4 %% Dynamics - make actual equations of motion
5 % states order: [quat, omegadot, rmin, rdot];
6 qw = input.phase.state(:,1);
7 qx = input.phase.state(:,2);
8 qy = input.phase.state(:,3);
9 qz = input.phase.state(:,4);
10 rolldot = input.phase.state(:,5);
11 pitchdot = input.phase.state(:,6);
12 yawdot = input.phase.state(:,7);
13 x = input.phase.state(:,8);
14 y = input.phase.state(:,9);
15 z = input.phase.state(:,10);
16 xdot = input.phase.state(:,11);
17 ydot = input.phase.state(:,12);
18 zdot = input.phase.state(:,13);
19 u1 = input.phase.control(:,1); %rollddot
20 u2 = input.phase.control(:,2); %pitchddot

186
21 u3 = input.phase.control(:,3); %yawddot
22

23 % dynamics
24 qwdot = -1/2 * (rolldot.*qx + pitchdot.*qy + yawdot.*qz);
25 qxdot = 1/2 * (rolldot.*qw + pitchdot.*qz - yawdot.*qy);
26 qydot = 1/2 * (pitchdot.*qw + yawdot.*qx - rolldot.*qz);
27 qzdot = 1/2 * (yawdot.*qw + rolldot.*qy - pitchdot.*qx);
28 quatdot = [qwdot, qxdot, qydot, qzdot];
29
30 rollddot = u1;
31 pitchddot = u2;
32 yawddot = u3;
33 angacc = [rollddot, pitchddot, yawddot];
34

35 dx = xdot;
36 dy = ydot;
37 dz = zdot;
38 dtrans = [dx, dy, dz];
39
40 % Need to solve for accelerations
41 RInBody = input.auxdata.R*[1;0;0];
42 omega = [rolldot'; pitchdot'; yawdot'];
43 alpha = [rollddot'; pitchddot'; yawddot'];
44 [¬, c] = size(omega);
45 for i=1:c
46 CentAccInBody_mpsps(:,i) = cross(omega(:,i), ...
cross(omega(:,i), RInBody));
47 AngAccInBody_mpsps(:,i) = cross(alpha(:,i), RInBody);
48 dcm = quat2dcm([qw(i) qx(i) qy(i) qz(i)]);
49 CentAccInInertial_mpsps = dcm*CentAccInBody_mpsps(:,i);
50 AngAccInInertial_mpsps = dcm*AngAccInBody_mpsps(:,i);
51 TotalAccInInertial_mpsps(:,i) = CentAccInInertial_mpsps + ...
AngAccInInertial_mpsps;
52 end
53
54 xddot = TotalAccInInertial_mpsps(1,:)';
55 yddot = TotalAccInInertial_mpsps(2,:)';
56 zddot = TotalAccInInertial_mpsps(3,:)';
57 ddtrans = [xddot, yddot, zddot];
58
59 phaseout.dynamics = [quatdot, angacc, dtrans, ddtrans];
60
61 % Path constraint
62 phaseout.path = sqrt(x.^2 + y.^2 + z.^2);
63 %% Find integrand
64 t = input.phase.time;
65 n = length(t);
66
67 for i=1:n
68 normU(i) = norm([u1(i), u2(i), u3(i)]);
69 end
70
71 %% Find Hdot
72 R = input.auxdata.R;

187
73 for i=1:n
74 omega_123 = [rolldot(i); pitchdot(i); yawdot(i)];
75 alpha_123 = [rollddot(i); pitchddot(i); yawddot(i)];
76 int_product = input.auxdata.I_CMCwrtTAR*omega_123;
77 Hdot = input.auxdata.I_CMCwrtTAR*alpha_123 + cross(omega_123, ...
int_product);
78 Mx(i) = Hdot(1);
79 Fz(i) = -Hdot(2)/R;
80 Fth(i) = Hdot(3)/R;
81 end
82
83 %% Find Loads - dims are wrong
84 TotalForceInBody_N = input.auxdata.mass_C*(CentAccInBody_mpsps + ...
AngAccInBody_mpsps);
85 ShearLoadY_N = TotalForceInBody_N(2,:)' - Fth';
86 ShearLoadZ_N = TotalForceInBody_N(3,:)' - Fz';
87
88 %% Find Moments
89 I_C = input.auxdata.I_C;
90 Torsion = I_C(1,1)*rolldot - (I_C(2,2) - ...
I_C(3,3))*pitchdot.*yawdot - Mx';
91 BendingMomY_Nm = I_C(2,2)*pitchddot - ...
(I_C(3,3)-I_C(1,1))*rolldot.*yawdot;
92 BendingMomZ_Nm = I_C(3,3)*yawddot - ...
(I_C(1,1)-I_C(2,2))*pitchdot.*rolldot;
93

94 % Integrand
95 phaseout.integrand = normU' + (ShearLoadY_N) + ...
96 (ShearLoadZ_N) + (Torsion) + (BendingMomY_Nm) + (BendingMomZ_Nm);
97
98 end

Listing C.8: detumbleEndpoint.m

1 function output = detumbleEndpoint(input)
2
3 % Inputs
4 % input.phase(phasenumber).initialstate -- row
5 % input.phase(phasenumber).finalstate -- row
6 % input.phase(phasenumber).initialtime -- scalar
7 % input.phase(phasenumber).finaltime -- scalar
8 % input.phase(phasenumber).integral -- row
9 % input.parameter -- row
10 % input.auxdata = auxiliary information
11

12 % Output
13 % output.objective -- scalar
14 % output.eventgroup(eventnumber).event -- row
15
16 output.objective = input.phase.integral;
17

18 end

188
Bibliography

[1] A. Vogeley and R. Brissenden, “Survey of rendezvous progress,” in Guidance and

Control Conference, p. 353, 1964.
[2] J. Mayer and R. Parten, “Development of the gemini operational rendezvous
plan.,” Journal of spacecraft and rockets, vol. 5, no. 9, pp. 1023–1028, 1968.
[3] G. Lunney, “Summary of gemini rendezvous experience,” in Simulation and Sup-
port Conference, p. 272, 1967.
[4] D. Zimpfer, P. Kachmar, and S. Tuohy, “Autonomous rendezvous, capture and
in-space assembly: past, present and future,” in 1st Space exploration conference:
continuing the voyage of discovery, p. 2523, 2005.
[5] K. A. Young and J. D. Alexander, “Apollo lunar rendezvous,” Journal of Space-
craft and Rockets, vol. 7, p. 1083, 1970.
[6] J. L. Goodman, “History of space shuttle rendezvous and proximity operations,”
Journal of Spacecraft and Rockets, vol. 43, no. 5, pp. 944–959, 2006.
[7] B. LOGAN, JR, “Shuttle payload deployment and retrieval system,” in Guidance
and Control Conference, p. 1580, 1982.
[8] “View of the hst berthed to the shuttle atlantis : Nasa : Free download, borrow,
and streaming.” https://archive.org/details/s125e007257, May 2009.
[9] T. E. Rumford, “Demonstration of autonomous rendezvous technology (dart)
project summary,” in Space Systems Technology and Operations, vol. 5088,
pp. 10–20, International Society for Optics and Photonics, 2003.
[10] G. S. Center, “On-orbit satellite servicing study project report,” tech. rep., Tech-
nical report, 2010.
[11] T. M. Davis and D. Melanson, “Xss-10 microsatellite flight demonstration pro-
gram results,” in Spacecraft platforms and infrastructure, vol. 5419, pp. 16–26,
International Society for Optics and Photonics, 2004.
[12] A. Ogilvie, J. Allport, M. Hannah, and J. Lymer, “Autonomous satellite servicing
using the orbital express demonstration manipulator system,” in Proc. of the 9th
International Symposium on Artificial Intelligence, Robotics and Automation in
Space (i-SAIRAS’08), pp. 25–29, 2008.

189
[13] “Orbital express astro.” http://www.astronautix.com/o/
orbitalexpressastro.html.

[14] D. J. Kessler, N. L. Johnson, J. Liou, and M. Matney, “The kessler syndrome:

implications to future space operations,” Advances in the Astronautical Sciences,
vol. 137, no. 8, p. 2010, 2010.

[15] J.-C. Liou and N. L. Johnson, “Risks in space from orbiting debris,” 2006.

[16] C. Bonnal, J.-M. Ruault, and M.-C. Desjean, “Active debris removal: Recent
progress and current trends,” Acta Astronautica, vol. 85, pp. 51–60, 2013.

[17] M. Shan, J. Guo, and E. Gill, “Review and comparison of active space debris
capturing and removal methods,” Progress in Aerospace Sciences, vol. 80, pp. 18–
32, 2016.

[18] J. P. Gardner, J. C. Mather, M. Clampin, R. Doyon, M. A. Greenhouse, H. B.

Hammel, J. B. Hutchings, P. Jakobsen, S. J. Lilly, K. S. Long, et al., “The james
webb space telescope,” Space Science Reviews, vol. 123, no. 4, pp. 485–606, 2006.

[19] R. Rembala and C. Ower, “Robotic assembly and maintenance of future space
stations based on the iss mission operations experience,” Acta Astronautica,
vol. 65, no. 7-8, pp. 912–920, 2009.

[20] “Image of international space station.” https://spaceflight.nasa.gov/

gallery/images/station/assembly/hires/s88e5157.jpg.

[21] “Image of international space station.” https://spaceflight.nasa.gov/

gallery/images/station/assembly/hires/s133e010447.jpg.

[22] S. Mohan, Reconfiguration methods for on-orbit servicing, assembly, and opera-
tions with application to space telescopes. PhD thesis, Massachusetts Institute of
Technology, 2007.

[23] C. M. Jewison, Reconfigurable thruster selection algorithms for aggregative space-

craft systems. PhD thesis, Massachusetts Institute of Technology, 2014.

[24] W. J. Larson and J. R. Wertz, “Space mission analysis and design,” tech. rep.,
Torrance, CA (United States); Microcosm, Inc., 1992.

[25] R. Garner, “Hubble servicing missions overview.” https://www.nasa.gov/

mission_pages/hubble/servicing/index.html, 2018.

[26] D. King, “Space servicing: past, present and future,” in Proceedings of the 6th
International Symposium on Artificial Intelligence and Robotics & Automation
in Space: i-SAIRAS, pp. 18–22, 2001.

[27] C. S. Agency, “About canadarm2.” http://www.asc-csa.gc.ca/eng/iss/

canadarm2/about.asp, 2018.

190
[28] C. Jewison, B. McCarthy, D. Sternberg, D. Strawser, and C. Fang, “Resource
Aggregated Reconfigurable Control and Risk-Allocative Path Planning for On-
orbit Assembly and Servicing of Satellites,” tech. rep., AIAA SciTech 2014, 2013.

[29] A. Saenz-Otero, Design Principles for the Development of Space Technology Mat-
uration Laboratories Aboard the International Space Station. PhD thesis, Mas-
sachusetts Institute of Technology, Cambridge, MA, 2005.

[30] J. James, C. Jewison, D. Sternberg, W. Sanchez, D. Roascio, and A. Saenz-Otero,

“Recent SPHERES Docking and Reconfiguration Experiments on the ISS.” 2016.

[31] L. J. DiGirolamo, K. A. Hacker, A. H. Hoskins, and D. B. Spencer, “A hybrid

motion planning algorithm for safe and efficient, close proximity, autonomous
spacecraft missions,” in AIAA/AAS astrodynamics specialist conference, p. 4130,
2014.

[32] M. C. Jackson, “A six degree of freedom, plume-fuel optimal trajectory planner

for spacecraft proximity operations using an a* node search,” Master’s thesis,
Massachusetts Institute of Technology, 1994.

[33] A. Miele, M. Weeks, and M. Ciarcia, “Optimal trajectories for spacecraft ren-
dezvous,” Journal of optimization theory and applications, vol. 132, no. 3,
pp. 353–376, 2007.

[34] D.-R. Lee and H. Pernicka, “Integrated system for autonomous proximity oper-
ations and docking,” International Journal of Aeronautical and Space Sciences,
vol. 12, no. 1, pp. 43–56, 2011.

[35] J. Virgili-Llop, C. Zagaris, R. Zappulla, A. Bradstreet, and M. Romano, “A

convex-programming-based guidance algorithm to capture a tumbling object on
orbit using a spacecraft equipped with a robotic manipulator,” The International
Journal of Robotics Research, vol. 38, no. 1, pp. 40–72, 2019.

[36] J. D. Munoz, Rapid path-planning algorithms for autonomous proximity opera-

tions of satellites. PhD thesis, University of Florida, 2011.

[37] R. Zappulla, J. Virgili-Llop, and M. Romano, “Near-optimal real-time spacecraft

guidance and control using harmonic potential functions and a modified rrt,” in
27th AAS/AIAA Space Flight Mechanics Meeting, San Antonio, TX, 2017.

[38] D. C. Sternberg et al., Optimal docking to tumbling objects with uncertain prop-
erties. PhD thesis, Massachusetts Institute of Technology, 2017.

[39] J.-J. E. Slotine and W. Li, Applied Nonlinear Control. Prentice-Hall Upper
Saddle River, NJ, 1 ed., 1991.

[40] K. D. Young, V. I. Utkin, and U. Ozguner, “A control engineer’s guide to sliding

mode control,” in Proceedings. 1996 IEEE International Workshop on Variable
Structure Systems.-VSS’96-, pp. 1–14, IEEE, 1996.

191
[41] Y. M. Sam, J. H. Osman, and M. R. A. Ghani, “A class of proportional-integral
sliding mode control with application to active suspension system,” Systems &
control letters, vol. 51, no. 3-4, pp. 217–223, 2004.

[42] I. Eker, “Sliding mode control with pid sliding surface and experimental applica-
tion to an electromechanical plant,” ISA transactions, vol. 45, no. 1, pp. 109–118,
2006.

[43] L. Zhao and Y. Jia, “Finite-time attitude tracking control for a rigid spacecraft
using time-varying terminal sliding mode techniques,” International Journal of
Control, vol. 88, no. 6, pp. 1150–1162, 2015.

[44] I. D. Landau, R. Lozano, M. M’Saad, and A. Karimi, Adaptive control: algo-

rithms, analysis and applications. Springer Science & Business Media, 2011.

[45] G. Kreisselmeier and B. Anderson, “Robust model reference adaptive control,”

IEEE Transactions on Automatic Control, vol. 31, no. 2, pp. 127–133, 1986.

[46] S. Nicosia and P. Tomei, “Model reference adaptive control algorithms for indus-
trial robots,” Automatica, vol. 20, no. 5, pp. 635–644, 1984.

[47] R. Kumar, S. Das, P. Syam, and A. K. Chattopadhyay, “Review on model ref-

erence adaptive system for sensorless vector control of induction motor drives,”
IET Electric Power Applications, vol. 9, no. 7, pp. 496–511, 2015.

[48] A. Shekhar and A. Sharma, “Review of model reference adaptive control,” in

2018 International Conference on Information, Communication, Engineering
and Technology (ICICET), pp. 1–5, IEEE, 2018.

[49] A. T. Espinoza and W. Sanchez, “On-board parameter learning using a model

reference adaptive position and attitude controller,” in 2017 IEEE Aerospace
Conference, pp. 1–8, IEEE, 2017.

[50] A. P. Nair, N. Selvaganesan, and V. Lalithambika, “Lyapunov based pd/pid in

model reference adaptive control for satellite launch vehicle systems,” Aerospace
Science and Technology, vol. 51, pp. 70–77, 2016.

[51] K. J. Åström, “Theory and applications of adaptive controlâĂŤa survey,” Auto-

matica, vol. 19, no. 5, pp. 471–486, 1983.

[52] K. J. Åström and B. Wittenmark, Adaptive control. Courier Corporation, 2013.

[53] D. Clarke and P. Gawthrop, “Self-tuning control,” in Proceedings of the Institu-

tion of Electrical Engineers, vol. 126, pp. 633–640, IET, 1979.

[54] J.-J. Slotine and J. Coetsee, “Adaptive sliding controller synthesis for non-linear
systems,” International Journal of Control, vol. 43, no. 6, pp. 1631–1651, 1986.

192
[55] J.-J. Slotine and S. S. Sastry, “Tracking control of non-linear systems using
sliding surfaces, with application to robot manipulators,” International journal
of control, vol. 38, no. 2, pp. 465–492, 1983.

[56] F. Plestan, Y. Shtessel, V. Bregeault, and A. Poznyak, “New methodologies for

adaptive sliding mode control,” International journal of control, vol. 83, no. 9,
pp. 1907–1919, 2010.

[57] G. Godard and K. D. Kumar, “Fault tolerant reconfigurable satellite formations

using adaptive variable structure techniques,” Journal of guidance, control, and
dynamics, vol. 33, no. 3, pp. 969–984, 2010.

[58] J. Li, Y. Pan, and K. D. Kumar, “Formation flying control of small satellites,”
in AIAA Guidance, Navigation, and Control Conference, p. 8296, 2010.

[59] Q. Hu and B. Xiao, “Fault-tolerant sliding mode attitude control for flexible
spacecraft under loss of actuator effectiveness,” Nonlinear Dynamics, vol. 64,
no. 1-2, pp. 13–23, 2011.

[60] S. Kawamoto, Y. Ohkawa, H. Okamoto, K. Iki, and T. Okumura, “Current status

of research and development on active debris removal at jaxa,” in 7th European
Conference on Space Debris, 2017.

[61] T.-C. Nguyen-Huynh and I. Sharf, “Adaptive reactionless motion for space ma-
nipulator when capturing an unknown tumbling target,” in 2011 IEEE Interna-
tional Conference on Robotics and Automation, pp. 4202–4207, IEEE, 2011.

[62] Y.-w. Zhang, L.-p. Yang, Y.-w. Zhu, H. Huang, and W.-w. Cai, “Nonlinear 6-
dof control of spacecraft docking with inter-satellite electromagnetic force,” Acta
Astronautica, vol. 77, pp. 97–108, 2012.

[63] J. G. Katz, “Estimation and control of flexible space structures for autonomous
on-orbit assembly,” Master’s thesis, Massachusetts Institute of Technology, 2009.

[64] F. Aghili, “Time-optimal detumbling control of spacecraft,” Journal of guidance,

control, and dynamics, vol. 32, no. 5, pp. 1671–1675, 2009.

[65] G. Avanzini and F. Giulietti, “Magnetic detumbling of a rigid spacecraft,” Jour-

nal of guidance, control, and dynamics, vol. 35, no. 4, pp. 1326–1334, 2012.

[66] V. Coverstone-Carroll, “Detumbling and reorienting underactuated rigid space-

craft,” Journal of Guidance, Control, and Dynamics, vol. 19, no. 3, pp. 708–710,
1996.

[67] C. Grubin, “Docking dynamics for rigid-body spacecraft,” AIAA Journal, vol. 2,
no. 1, pp. 5–12, 1964.

193
[68] M. A. Patterson and A. V. Rao, “Gpops-ii: A matlab software for solving
multiple-phase optimal control problems using hp-adaptive gaussian quadrature
collocation methods and sparse nonlinear programming,” ACM Transactions on
Mathematical Software (TOMS), vol. 41, no. 1, p. 1, 2014.

[69] A. Waechter, C. Laird, F. Margot, and Y. Kawajir, “Introduction to ipopt: A

tutorial for downloading, installing, and using ipopt,” Revision, 2009.

194