System Characterization and Online Mass Property Identification
of the SPHERES Formation Flight Testbed
By
Dustin S. Berkovitz
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
MASSACHUSSETS INSTITUTE OF TECHNOLOGY
FEBRUARY 2008
©2008 Massachusetts Institute of Technology. All rights reserved.
Signature of Author:
Department of Aeronautics and Astronautics
/
September 4, 2007
Certified by:
David W. Miller
Professor
Thesis Supgryisor
Accepted by:
I3avid.)Dahnmofal
Associate Professor, Department of Aeronautics and Kstronautics
Chairman, Graduate Committee
..
_8"
MASACHUSETM-tNSTITUT
OF TECHNOLOGY
MAR 2 5 2008
LIB.RARIES
ARCHNES
System Characterization and Online Mass Property Identification
of the SPHERES Formation Flight Testbed
By
Dustin S. Berkovitz
Submitted to the Department of Aeronautics and Astronautics
on September 4, 2007, in partial fulfillment of the
requirements for the degree of Master of Science in Aeronautics and Astronautics
Abstract
The National Aeronautics and Space Administration (NASA) and other entities in the
aerospace industry have recently been considering distributed architectures for many
space applications, such as space-based interferometry. Whether the craft in such a
system are structurally connected or flown in tight formation, distribution allows for
higher redundancy in case of failures as well as reducing the minimum payload footprint
for launch. Designed to fly in precise formation, the SPHERES satellites rely on accurate
system characteristics such as thruster strength and vehicle mass and inertia. The
SPHERES testbed is described and the applications for formation flight are presented.
Mass properties of the SPHERES satellites are examined because of their impact on
control determination, with comparison between CAD model estimates and empirically
determined values. The sensor and actuator suite, essential for closed-loop control, are
also identified and characterized. A recursive least squares algorithm for determining
mass properties in real time is explained and implemented both offline and online with
results from test flights aboard NASA's KC-135 micro-gravity aircraft (Reduced Gravity
Airplane, RGA).
Thesis Supervisor: David W. Miller
Title: Professor
Acknowledgments
The mass property identification algorithm discussed in this thesis is derived and
formulated by Edward Wilson, Chris Lages, and Robert Mah of NASA Ames Research
Center. Some data and results using the AMES implementation of the algorithm are
included and referenced.
I'd like to thank my friends and family, my parents and Katie Shirley for supporting me
through this lengthy process. I also am grateful to my graduate advisor, David Miller, for
putting up with me for the past 6 years. Also, thanks to my lab mates in the Space
Systems Lab for making work fun; especially Alvar Saenz-Otero and Simon Nolet for
reviewing this thesis on their own time.
Contents
A bstract
.............................................
List of Figures ...............
...................
.....
9
...........................
... ................................
12
List of Tables .................................................................
Nomenclature ...................
Chapter 1
....
.........................
1,2 SPHERES Hardware Overview.........................
.........
1.2. I
Inertial Sensors ..........
1.2.2
Propulsion System ........
1.2.3
Ultrasound-based Positioning System ..................
15
................. 17
.............
....... ...................... 17
.............................
1.3 Current Programs ......................................
................ 18
.................. 18
............................19
1.4 System Characterization Motivation .....................
SPHERES Mass Properties ..............
2.1 Mass Properties Overview ......
13
.................................
15
Introduction ................
1.1 SPHERES Background ........................................................ ........
Chapter 2
3
........... 21
.......................
24
........................................... 24
......................... 26
2.2 Measuring Center of Mass ............................
2.2.1
CoM Error Sources .............
..............
................
2.3 M easuring M oment of Inertia ..................................
27
............... 27
......
.......................28
2.3.1
Torsional Pendulum Modeling ........................
2.3.2
Test Procedure .....
2.3.3
Products of Inertia ..............
2.3.4
Results ..................................
2.3.5
Alternate Method for Measuring Inertia ..................................... 34
2.3.6
Inertia Measurement Error Analysis .......
2.3.7
Empirical Mass Properties Conclusion ...............
Chapter 3
..................... ......
....... .................
............. 32
.............
............................. 33
.....
................
35
............ 42
....
................. 44
Thruster Characterization .................
............ 44
3.1 Propulsion System Overview ............................... ..........
....................................... 45
3.2 Propulsion Hardware ..............
3.3 Pulse-width Modulation Scheme ..............
............ 47
....................
3.4 Propulsion Limitations ......................................................
.......... 51
51
3.5 Measuring Nominal Thrust Vector .....................................
3.6 Thrust Latency ...............................
3.7 Multiple Thruster Pressure Drop .............
.....................
..............
3.8 Sensor Characterization ...........................................
Chapter 4
32
........
53
.............. 56
58
Mass Property Identification ............. ................................... 61
4.1 Online M ass Property Identification .................................................... 61
4.2 Theory and Equations ..............
........................................
4.3 KC-135 Micro-gravity Testing.....................
6
............ 63
.................. 65
4.4 Algorithm Development ........................................
........
.......... 69
4.4.1
Gyro Filtering and Interpolation ..................
4.4.2
Angular Acceleration Estimation.............................71
4.4.3
Mass Property Identification .......
4.4.4
C Code Implementation ................. ........
Chapter 5
.... ...
Conclusion .................................
5.1 Thesis Summary ...........................
......
,....
............... 86
Tuning Fork Gyroscopes................... .....
A.2 Modeling Rate Sensing .................
..............
93
................................ 93
.....
........................ 94
...
..................... 96
Fuel Slosh ..............................................
B. 1 Propellant Properties ..................................
.
........................
99
.......................99
.................
B.3 Modeling Fuel Slosh .................................................
B.4 Modeling Input ..................
89
.
A.3 Application to SPHERES ......................
B.2 Fuel Slosh on SPHERES
............. 87
.........................................
.
84
...................... 84
............................................
A. Background Physics ...................
Appendix B
.................... 79
.
Bibliography ........................
Appendix A
....... ........................... 76
...........................
5.2 Conclusions ..............................................
5.3 Future Work ......................
..............70
100
101
...............
............... 103
B .5 Resu lts ....................................................................................... 104
Appendix C
SPHERES Satellite Properties ................
...
............... 107
C. 1 CoM Offset .............................................. ................... 107
C.2 M oment of Inertia...................
................................................... 108
C.3 Thrust Strengths ...................
................
C.4 Thrust Directions ......
.........................
Appendix D
MATLAB Code ..........
.................... ............
............
........
......................
108
109
110
D. 1 Mass Property ID Code .................................................... 110
D.2 Interpolation Scheme ...................................................
D.3 Gyro Filter ......................................
D.4 Angular Acceleration Estimator .......
...
113
........
..................
...........
114
.............. 115
D.5 Batch Mass Property ID ......................................
117
D.6 Recursive Mass Property ID .........
119
Appendix E
C Code ...............
...............................
.......................... . .......
E.1 Mass Property ID Code .................................
E.2 Angular Acceleration Kalman Filter .................
E.3 Mass ID - low level .
.............
.................. 126
................126
.............141
........................................... 146
List of Figures
1.1
SPHERES Flight Satellite
1.2
Ultrasound Beacon
1.3
Standard Feedback Control Representation
2.1
SPHERES Satellite Hanging in a Torsional Pendulum
2.2
Model of One String of the Torsional Pendulum
2.3
Equal Arc-lengths Assumption
2.4
X-Gyro Data from SPHERE SN 2 on the Torsional Pendulum
3.1
Solenoid Valve Thruster 2
3.2
PWM Mixer Code Block Diagram
3.3
Thrust Correction for Simultaneous Firings
3.4
SPHERES Body Axes
3.5
Thrust Latency in Solenoid Valve
3.6
Applied Impulse From Difference in Thruster Latency
3.7
Sample of 6ms Solenoid Delay
3.8
System Pressure Propagation Test Setup
3.9
Pressure Change Propagation Data
3.10
Inertial Measurement Unit Specifications
3.11
Filtered 338 Hz Gyro Noise
3.12
FFT of Gyro Ringing
4.1
Ideal Maneuvers for Inertia and CoM Identification
4.2
Three-Axis Gyro Data From RGA Test
4.3
Concatenated RGA Gyro Data
4.4
AMES/Intellization Mass Property RGA Results [Wilson, 2003]
4.5
Filtered Gyro Data Standard Deviation
4.6
Kalman Filter Process
4.7
Angular Acceleration Estimation Comparison
4.8
Estimation Routine Variances
4.9
Sample Ixx Convergence
4.10
MATLAB vs. Visual C Acceleration Estimation Results
4.11
Visual C vs. Online Embedded C Mass ID Results
4.12
Z-Axis Gyro Data From Online Mass ID Test
4.13
Online Mass Property Estimate Convergence
A. 1
Tuning Fork Gyro
A.2
Gyro Sensing Model
A.3
12 Hz Gyro Noise
A.4
38 Hz Gyro Noise
B. 1
Observed Fuel Slosh in Gyro Data
B.2
Linearized Fuel Slosh Model
B.3
Simplified Fuel Slosh Model
B.4
Fuel Slosh Model Input
B.5
Model Output
List of Tables
2.1
Measured CM Offset vs. CAD Values
2.2
Measured Moment of Inertia vs. CAD Values
2.3
Nonlinear Spring Constant Values
3.1
Thruster Dependencies [Chen, 2002]
3.2
Thruster Locations w.r.t. Geometric Center (GC)
3.3
Thruster Strength and Direction for SPHERE SN 2
4.1
RGA Mass Property Results
4.2
Standard Deviations for Line-fits of Varying Order
B.1
Worst-case Effect of Fuel Slosh on Mass Properties
C.1
CoM Offset
C.2
Moment of Inertia
C.3
Thrust Strengths
C.4
Thrust Directions
Nomenclature
DSP
Digital Signal Processor
ISS
International Space Station
MIT
Massachusetts Institute of Technology
NASA
National Aeronautics and Space Administration
PWM
Pulse-width Modulation
SPHERES
Synchronized Position Hold, Engage, Reorient, Experimental Satellites
SSL
Space Systems Laboratory
CM/CoM
Center of Mass
GC
Geometric Center
DOF
Degrees of Freedom
CAD
Computer-aided Design
C02
Carbon Dioxide
ID
Identification (as in "Mass Property Identification")
(R)LS
(Recursive) Least Squares
IMU
Inertial Measurement Unit
KC
NASA's micro-gravity aircraft, KC-135
KF
Kalman Filter
P
Estimate error covariance
Q
Process error covariance
R
Measurement error covariance
I
Moment of inertia
g
Damping ratio
K
Spring constant
b
Damping coefficient
E
Young's modulus
o"
Stress (Hooke's Law); standard deviation
Pseudo-inverse operator
0, c, a
Angular acceleration
i
Estimate of true value, x
Chapter 1
Introduction
1.1
SPHERES Background
SPHERES (Synchronized Position-Hold, Engage, Reorient Experimental Satellites) is a
high fidelity test-bed designed to help develop and mature algorithms for formation flight
and autonomous docking. The test-bed includes three satellites with full six degrees of
freedom (6-DOF) control authority and a relative and absolute positioning system.
SPHERES is meant to be a low risk platform for testing experimental spacecraft
navigation [10, 15, 16].
The SPHERES program began as an undergraduate design course in the Department of
Aeronautics and Astronautics at the Massachusetts Institute of Technology (MIT).
Juniors and seniors were asked to design and build a platform for testing formation flight
algorithms over a span of three academic terms. Because of the growing interest in
formation flying satellites in the aerospace industry, many interested parties such as the
Defense Advanced Research Projects Agency (DARPA), National Aeronautics and Space
Administration (NASA), and Lockheed Martin invested in SPHERES as a graduate
research program.
The SPHERES satellites were redesigned and rebuilt with the
intention of operating for an extended period of time aboard the International Space
Station (ISS). They have been tested extensively at the MIT Space System Laboratory's
(SSL) float table (2D configuration) and in micro-gravity aboard NASA's Reduced
Gravity Airplane (RGA). Three SPHERES satellites and accompanying test equipment
were launched to ISS throughout 2006.
Figure 1.1 SPHERES Flight Satellite
This thesis first presents an overview of the SPHERES testbed hardware, including the
satellites themselves as well as the ultrasonic positioning system equipment. Next, it
presents the offline and online characterization of key system parameters of the
SPHERES testbed. First, mass properties of the SPHERES satellites are examined, with
comparison between CAD model estimates and empirically determined values (Chapter
2). Next, experiments performed to characterize the SPHERES satellite's propulsion
system and sensor suite are detailed (Chapter 3).
Finally, online mass property
identification algorithm is introduced and compared with previous results (Chapter 4).
1.2
SPHERES Hardware Overview
The SPHERES hardware was designed and built collaboratively by the MIT Space
Systems Laboratory and Payload Systems Inc. There are five identical satellites in
operation (two for ground testing at MIT, three for experiments aboard the ISS). Each
SPHERES satellite has an aluminum truss-work skeleton that provides mounting points
for the avionics, propulsion system, and sensors. A color-coded, translucent plastic shell
protects this equipment and helps provide easy identification.
The subsystems relevant to this thesis are described below. Other subsystems include the
CPU and signal processing, power, communications, and the control laptop with the GUI,
but will not be introduced here. Refer to [13, 16] for more information.
1.2.1
Inertial Sensors
Each SPHERES satellite has six inertial sensors, three accelerometers and three
gyroscopes. The three Honeywell accelerometers are mounted in three orthogonal axes
in the body frame. They are designed to measure a fine resolution of linear acceleration
produced by the onboard thrusters. While this is ideal in micro-gravity, accelerometer
data is unreliable at the 2D MIT facility (the accelerometers are saturated by the presence
of gravity when the satellite is tilted by more than one degree).
Similarly, three
gyroscopes are mounted on each axis to provide angular rate information.
1.2.2
Propulsion System
SPHERES satellites accelerate using a cold-gas propulsion system (carbon dioxide,
CO2). The multi-phase C02 is stored at 860 psi in a tank located in the middle of the
satellite. The gas is regulated to 35 psi and fed through an expansion capacitor to the 12
solenoid-actuated valve thrusters. When a command to fire is sent through the Digital
Signal Processor (DSP) to the firing circuit, the corresponding thruster solenoid is
actuated, allowing the pressurized CO2 to escape and provide thrust. The 12 thrusters
can be commanded either ON or OFF (no intermediate settings or throttle) and are
controlled via pulse-width modulation (PWM). They are located around the satellite so
as to provide full, uncoupled control in 6 Degrees of Freedom (6-DOF).
1.2.3
Ultrasound-based Positioning System
Most space systems have some way to measure absolute position (e.g., star-trackers or
GPS). A localized system involving ultrasonics was developed for SPHERES in order to
operate inside the ISS. Given a priori knowledge of the pre-programmed delays in the
beacons, the DSP computes the time-of-flight of the ultrasound pings, which travel at the
speed of sound.
The time-of-flight is used by an Extended Kalman Filter (EKF) to
estimate absolute translational and angular position and velocity.
Figure 1.2 Ultrasound Beacon
Each SPHERES satellite is equipped with its own onboard beacon to allow for satelliteto-satellite relative ranging and bearing angles (more accurate for close proximity
maneuvers).
1.3
Current Programs
SPHERES is designed as a flexible platform for testing autonomous spacecraft
algorithms, specifically targeting rendezvous and proximity operations (RPO) and
formation flight. Related ideas include such topics as servicing spacecraft [14], spacetethers [6], formation flying interferometers [11] and space-network topology [19]. The
SPHERES project offers an opportunity to do research in this area as a low cost, low risk,
and high fidelity hardware simulation.
By operating inside the ISS, the SPHERES
testbed exploits the micro-gravity environment to represent the dynamics of docking
missions while protecting itself from getting lost in space when real or simulated failures
occur.
Space-based interferometry is an especially interesting topic because of the consistent
demand for higher resolution pictures in space. In order to get better quality images from
orbiting telescopes like Hubble, the diameter of the primary mirror would have to grow in
size. However, a telescope with a larger mirror would not fit in any launch vehicle
available today.
Rather than designing and building a launch vehicle with a higher
payload volume, interferometry can be used to get comparable quality images using
multiple smaller telescopes arranged in a precise pattern.
Spacecraft autonomy is a very powerful concept. Without the need for human assistance,
satellites can operate efficiently for longer lifetimes in orbit. Fault Detection Isolation
and Recovery (FDIR) is a big step towards fully autonomous spacecraft [20]. Systems
with FDIR can detect failures in their components and adjust accordingly to fulfill their
primary objectives. A more specific example of this applied to SPHERES is thruster
FDIR. Using the firing commands given by the software and the measured dynamics
from the onboard sensors, an FDIR algorithm can isolate thruster failures and alter future
firing commands with this failure taken into account.
Another, more basic fundamental of autonomy is estimation. The more accurate the
information is in the state vector, the better the control performance can be. The goal in
autonomous estimation is to identify relevant system parameters online (especially
changing ones like fuel mass).
A mass property identification algorithm has been
developed for testing on the SPHERES testbed [21].
It has the ability to give a
continually updating estimate of the Center of Mass (CoM) and Moment of Inertia by
using thruster commands and gyroscope data. These estimated values then impact how
commanded forces and torques are manifested as thruster ON/OFF times.
This wide variety of research topics is the focus of the SPHERES project. Many
algorithms have gone through multiple levels of iteration, with offline testing in
MATLAB and other computational tools, and online testing on the SPHERES satellites at
the 2D testbed at MIT as well as several flights on NASA's RGA micro-gravity
"laboratory" and aboard the ISS.
1.4
System Characterization Motivation
It is an inevitable consequence of the manufacturing process that misalignments,
asymmetries, and other minor flaws are created that affect the performance of any
assembly. Though all SPHERES satellites are meant to be identical, there are slight
inconsistencies that cause each to behave differently. Correcting or accounting for these
errors can be addressed in multiple ways, including but not limited to using stochastic
values or designing controllers to compensate for sensor noise or uncertainty in specific
parameters of the plant model.
Regardless of which methods are employed, the
underlying truth holds that the more that is known about the system, the easier it will be
to control and improve performance. Efficient use of consumables (pressurized gas and
batteries) is a universally important issue for space applications, and is crucial for
extending the life of SPHERES as test-bed aboard the ISS, as well as countless other
space vehicles.
The first part of our strategy for improving system performance is to make SPHERES
behave the best possible in open loop. This is accomplished by fully characterizing the
propulsion system (Actuator) and the vehicle itself (Plant). This system characterization
is used to calibrate and tune the control software and allow the SPHERES satellite to
perform open loop maneuvers, like flying straight, with better accuracy.
However, there is only so much we can accomplish with characterization and calibration
while SPHERES is on the ground. Some system parameters like CoM and Inertia will
change as fuel is depleted (or in cases of other satellites, when components are deployed
or jettisoned or docking with another spacecraft has occurred). Online estimation of
these parameters can extend mission lifetime by allowing more accurate maneuvers and
by using the on-board fuel more efficiently. An online mass property identification
algorithm developed at the NASA AMES Research Center [Lages, Wilson, Mah] [21]
has been implemented for use on SPHERES to identify CoM and Inertia. In addition, the
Inertial Measurement Units (Sensor) used to close the loop are examined.
Figure 1.3 depicts a standard control-loop representation with the feedback as a dashed
line to signify an open loop system. The input to the control software is in the form of
desired state (position, attitude, velocity and angular rates). This is transformed to body
forces and torques and then passed through a "mixer" to get thruster ON/OFF times for
each of the 12 thrusters. The thrusters apply forces and moments on the plant and the
inertial sensors measure angular rates and linear acceleration.
.... .... ... .... ... .... ... .... .... ... .... ... .... ... .... ...
S0
X
!
:
... ... .
Controller
0
--0
. .. .. . .. .. . .. .. . .. .. . .. .. .. . .. ..
4,--
aI
. .. .. . .. ..
Output
I
Jr
Figure 1.3 Standard Control-Loop Representation
We use this representation as a model for the organization of the SPHERES System
Characterization in Chapters 2 and 3.
The Plant, Sensors, and Actuators are fixed
(already built/purchased), so our only form of correction is in the control software. After
presenting predicted and empirically measured values for key system parameters, online
mass property identification is introduced and compared with previous results (Chapter
4).
Chapter 2
SPHERES Mass Properties
2.1
Mass Properties Overview
Modem spacecraft are designed using computer-aided design tools such as ProEngineer
and Solidworks.
Many CNC workshop machines can replicate parts directly from
imported CAD files. This allows for a high level of fidelity between the conceived
design and the ultimate product. However accurate the machine, there are bound to be
inconsistencies between "identical" pieces. CAD estimated values for mass properties
could be unreliable because of this assumption.
Also, some parts (like electronics
boards) are modeled crudely and thus do not truly represent their effect on mass, CoM,
and Inertia of the assembled satellite.
Still other errors can arise from hollow
components that are approximated as solid objects. The most important error source
from CAD drawings is assembly of components.
Most of the time, fixation devices
(screws, rivets, bolts, nuts, soldering joints, etc.) are not modeled in a CAD drawing, and
the assembled parts are assumed to be perfectly assembled, when in fact we know that
there is always clearance in a bolt hole and the bolt is likely not to be centered in the hole.
These error sources, along with discrepancies introduced from the manufacture and
assembly processes, indicate that there is plenty of room for improvement in the accuracy
of our mass property estimates.
Table 2.1 shows the discrepancy between predicted
(CAD modeling) and measured CoM offset of one the SPHERES satellites.
I
CAD Model
Measured
0.49
-1.24
3.98
0.37
-1.52
3.32
0.48
-1.19
1.08
0.82
-0.99
1.07
Table 2.1 Measured CM Offset vs. CAD Values
These tests were repeated for all five SPHERES satellites with a full C02 tank (Wet) and
an empty tank (Dry). How the gas settles and sloshes does have an impact on CoM and
Inertia but will not be discussed in detail here (refer to Appendix B).
2.2
Measuring Center of Mass (CoM)
To get a more reliable estimate of the SPHERES satellite mass properties (Plant
parameters), a test stand was built that holds a SPHERES satellite rigid, while keeping its
center of geometry in a very precise, known location (Figure 2.1). The stand consists of
an aluminum plate with various mounting points and four rectangular posts that allow
access to screw into the frame of the SPHERES satellite. A SPHERES satellite can be
mounted along any primary axis to this stand. It was designed to attach to the top of a 6DOF load cell (for measuring thruster strengths and CoM) and to a torsional pendulum
harness (for measuring moment of inertia).
The primary function of the load cell is to measure forces and torques applied to it.
Before each CoM test, the signals from all six channels (corresponding to each DOF)
were recorded using a SigLab Data Acquisition System.
These measurements were
averaged and saved as bias estimates. The same was done with the empty test stand. By
positioning the center of geometry of the SPHERES satellite directly over the center of
the load cell, we can determine the torque applied by the CoM offset of the satellite alone
(removing the other recorded biases).
From the two horizontal moments applied on the load cell, we get the CoM offset in two
axes. For the
3 rd
coordinate, the SPHERES satellite is rotated so it's mounted on its side
and the moments are measured again.
2.2.1
CoM Error Sources
When measuring offsets on the order of millimeters, any slight misalignment will drown
out the true value. The test stand was meticulously designed and each test carefully
performed, but errors on that scale of magnitude are difficult to avoid completely. The
four screws that secure the test stand to the load cell are M4x.7 (Metric screws, 4.0mm in
diameter, 0.7 threads per mm). The through-holes on the test plate were made with the
recommended clearance of 4.5mm, leaving 0.5mm of play. With CoM values ranging
from 0-3mm, this is a significant source of error. By assuming the error bias is close to
zero mean, averaging over many tests: can provide reasonable estimates.
2.3
Measuring Moment of Inertia
The principal moments of inertia were measured using a trifilar torsional pendulum. An
aluminum mounting plate supports a satellite while hanging by three strings from another
plate bolted to the ceiling.
A specially modified outer shell was used to allow a
SPHERES satellite to be mounted in six different orientations. The two aluminum plates
were designed to allow leveling in both horizontal axes.
The top plate is mounted in two locations to a metal rail secured to the ceiling. By
adjusting these two mounting points, the top plate can be leveled along the axis
perpendicular to the rail. To level the other horizontal axis, there are two thumbscrews
that can be torqued into the ceiling, raising or lowering either side of the plate. The three
8 ft. long strands of fishing line are tied to steel bolts screwed into the ceiling-mounted
plate. They are threaded through a fine hole drilled through the length of each bolt to
minimize lateral strain on the strings.
The opposite end of each string is tied to aluminum mounting adapters. Similar to the
steel bolts at the top, the strings are threaded through a small hole in the center of the
adapter. The bottom of the adapter is threaded with a standard %-20 thread, which is
used to secure it to the bottom plate where the SPHERES satellite is mounted, as shown
in Figure 2.1. The threaded adapter also allows for independent leveling of the bottom
plate, which is important for inertia tests. Three mounting points, though coupled, can
still allow the leveling of both horizontal axes.
Figure 2.1 SPHERES Satellite
Hanging in a Torsional
2.3.1
Torsional Pendulum Modeling
The motion of a trifilar torsional pendulum is a nonlinear problem because the strings
restrict each other's movement and are therefore coupled. This causes the test object to
lift slightly as the pendulum oscillates. The test equipment and conditions were designed
to minimize the effect of these phenomena on the measured inertia (long strings of >8 ft;
string length >> mounting plate radius; small oscillations of <200 amplitude). In order to
determine the equations of motion, a simplified conceptual model will be used. Figure
2.2 is a force diagram representation of one of the three suspension strings modeled as a
simple pendulum in 2-D.
Force equilibrium along the axes parallel and normal to the string:
YF = T -mg cos 0= 0
(2.1)
SF
1 = mg sin 0 = fm0
y
T
m9
Figure 2.2 Model of One String of the
Torsional Pendulum
Solving for T: T = mg cos b (assumes even distribution of weight between the three
strings). Since the string is attached to a plate with negligible vertical mobility, Fx is
assumed to be the torque applied to the mounting plate.
xFx =Tsin
= mgcosOsinO
(2.2)
From the rotational equations of motion: rxF = 1M, where r is the radius of the circle
defined by the three string mounting points, I is the rotational inertia of the entire rotating
assembly, and 0 is the angle through which the mounting plate has turned. Substituting
for F and including a damping term gives:
-mgr cos 0 sin 0 - !; = MI
(2.3)
Since the strings are so long and the angle that the pendulum rotates through is so small,
the arcs of the two angles 0 and 0 are approximated as equal: O==Or.
approximation is shown in Figure 2.3:
These arc-lengths
assumed equivalent
Figure 2.3 Equal Arc-lengths Assumption
'
r
This
This produces a non-linear damped harmonic oscillator:
1 ++g0+mgrcos r0 sin(
) =0
(2.4)
The type of string used was a long, thin monofilament line with very small energy
dissipation (assumed massless). Also, energy loss to drag from the ambient air is small
due to the circular profile of the satellite and the low angular velocities. For these
reasons, the damping term was considered negligible.
Inputs will be of the form of an initial displacement 40 from the pendulum's equilibrium
point such that 0= 00cos ( . Substituting for 0,and solving for I:
I=
mgr
"o62COS( C)
cos( 0 cos(C)
(.
sin r90 cos(C)
)
(2.5)
(t 1
As was mentioned earlier, this test is designed to run for small values of 0 = 0ocos a .
Using small angle approximations for sine and cosine (~ << 1 and
r9
O<<1), Equation
(2.5) becomes:
m gr
I = mg
I0
For this particular setup, the values are:
m - mass = 7.172kg
g - 9.8m s2
r -radius = 0.146 1m
I -length - 2.47m
I0 -baselnertia= 0.0448kgm2
o- frequency
(2,6)
2.3.2
Test Procedure
The test: object is weighed before every experiment. After weighing, it is secured on the
mounting plate, then the plate is leveled using a dual axis high precision level. The last
pre-test procedure is measuring the length of the strings.
Finally, an oscillation is
initiated by turning the pendulum through a slight angle and releasing, being careful not
to impart any translational motion. Using Equation (2.6), it is possible to determine the
inertia of the object about that axis by measuring the frequency of oscillation. Inertia
measurements with only the test stand were performed to remove the bias of the stand.
2.3.3
Products of Inertia
This method was helpful to determine the primary moments of inertia, but not the offaxis terms. It is possible to use the same experimental setup to find the off-axis terms
using Mohr's Circle [3]. The Principal Axes of an object are defined as the axes around
which the products of inertia are zero. If a test object were mounted so that it could be
incrementally rotated around the x-axis, and inertia estimates were taken for many
different combinations of axes in the Y-Z plane, the resulting values would be sinusoidal.
The maximum and minimum values of this sinusoid (in 3-D) correspond to the Principal
Axes. If the object were rotated through an angle B in the Y-Z plane, the measured
inertia is related to the product of inertia by Equation (2.7).
IBrz = Iz sin 2 B+ Iy, cos 2 B-IPz sin2B
(2.7)
The minimum number of measurements necessary to determine products of inertia is six;
assuming three orthogonal axes are used for a basis. One measurement about X, Y, and
Z is needed as well as one each about axes 45 degrees between the X-Y, Y-Z, and Z-X
axes.
Unfortunately this: method requires more mounting configurations than are available on
the current test stand, and so the products of inertia were not measured empirically.
Since the SPHERES satellites are designed to be symmetric, the off-axis terms are a
couple orders of magnitude below the primary inertias (according to the CAD predictions
and online mass property results) and thus much more prone to mounting errors and
mathematical assumptions.
2.3.4 Results
Table (2.2) shows sample inertia data for one satellite, comparing CAD predictions to
empirically measured values. As seen in the table, they are similar but vary by nontrivial
amounts. The discrepancy is expected because of the inherent inaccuracy of CAD
predictions for heterogeneous objects. The error analysis described in Section 2.3.6 and
the results from the online mass property identification algorithm verify that the empirical
values are better candidates for measures of truth.
I
Measured
CAD Model
2.30
2.19
2.57
2.45
10.5%
2.42
2.31
2.25
2.15
7.6%
2.14
2.13
2.03
2.03
5.4%
0.01
0.01
n/a
n/a
n/a
-0.03
-0.03
n/a
n/a
n/a
-0
-0
n/a
n/a
n/a
Table 2.2 Measured Moment of Inertia vs. CAD Values
2.3.5
Alternate Method for Measuring Inertia
An alternate method for measuring inertia was attempted. The strategy is to approximate
the entire torsional pendulum as a torsional spring with spring constant K.
r =10 =K-O
9= A sin(ot)
I=
(2.8)
K
The procedure in this case is to measure K by testing an object of known inertia and use
that K value for all future tests. It turns out however that this is an over-simplification of
the torsional spring since, when compared to Equation (2.6), K depends on m and m
which is not constant:
K=
mngr
2
(2.9)
If the spring were linear, it would exhibit strain displacement proportional to applied
stress and K would be constant.
behavior.
Table 2.3 shows data supporting this nonlinear
The first row includes data from a torsional pendulum test with just the
mounting plate.
The second row is from a test with a SPHERES satellite mounted
(inertia values are validated from CAD models and online estimation).
0.0448
0.0674
2.286
2.933
0.2341
0.5802
Table 2.3 Nonlinear Spring Constant Values
The estimates for off-axis terms of inertia have come from running an offline version of
AMES/Intellizations's
gyro-based
mass
property
identification
algorithm
[21].
Gyroscope data collected in micro-gravity on the RGA provided the necessary
information to determine off axis inertias. The algorithm itself is discussed in full detail
in Chapter 4.
2.3.6
Inertia Measurement Error Analysis
The moment of inertia characterization is vulnerable to many error sources ranging from
the background math and physics to the test implementation and execution.
Mathematical Approximation (equal arc lengths, small angle, no damping)
To properly reduce Equation (2.4), the arc lengths Of and Or are assumed equal. This is
a slightly less "risky" assumption than using small angle approximations for 0 and (b.
Since 0 is the larger angle, it has the highest potential for error under the small angle
assumption. Showing that the small angle assumption for 0 is negligible will then prove
the equal arc-lengths assumption negligible as well. As part of the test procedure, the
maximum value of 0 is 20 degrees (0.349 rad). The length of the arc defined by 0 is
rO = 0.025499m.
2r sin -
The length of the chord that approximates this arc is
= 0.025467m, which is an error of 0.000259m (0.5%). Inertia varies linearly
with this error, so none of the angle assumptions will affect inertia by more than 0.5%.
The equations used also assume that the damping is negligible. The damping ratio can be
determined by examining the rate of descent of the oscillation amplitude.
I
; = 2 •ln(at1
(2.10)
The percent error in measured inertia is equal to 100;2 [3]. The data in Figure 2.4 was
taken for the X-axis inertia of SPHERE SN 2.
SN 2 lxx Measurement Data
2400
2300
2200
.4
4
2100
.4.
2000
4*
4
1900
,i.
1800
1 7n
0
o
I
I
500
1000
Update Number (50 Hz)
1500
Figure 2.4 X-Gyro Data From SPHERES SN 2 on the Torsional Pendulum
Over these 13 oscillations, the amplitude dropped from 306 to 299 counts:
n=13
a0 =306
a = 299
S26In 30
(2.11)
99) = 2.833e
error = 100ý 2 = 8.0267e-6%
There would need to be much more severe damping to have a noticeable impact on
inertia measurement.
Mounting error and string load distribution
There are a number of reasons why the axis of rotation might not coincide with the CoM
of the test object. Among them are mounting error, offset between the geometric center
and the CoM, and string load distribution. The transformation for inertia about parallel
axes is used to characterize the effect of this misalignment:
I B = I + mrA,
(2.12)
According to the "Mass Properties Measurement Handbook" [Boynton, Wiener] [3],
offsets around 1% of the radius of gyration are negligible, which is 1.461mm. The most
obvious source of misalignment is mounting error. The mounting plate and test stand
were: designed in CAD and machined to 0.001 in tolerances (0.254 mm) to hold a
SPHERES satellite securely in the middle of the three strings. Another possibility is that
the CoM of the satellite is not at the center of geometry. Most CoM measurements have
fallen in the range of 0-3 mm and are on the order of negligible errors. A third source
comes from the load distribution in the strings. If the tension in the strings is not equal
the axis of gyration will not be centered, imitating a CoM offset error. This is why care is
taken before each test to level the mounting plate. Having one comer of the mounting
plate lower than the others would tilt the measurement axis and could change the
pendulum's equilibrium point.
Such errors would cause the inertia to vary by md 2 (Equation 2.12) where d is the
distance between the CoM and the axis of gyration. A conservative estimate of 1 cm for
d results in an inertia error of SI = 0.000717kg .m 2 (1.025%).
Elasticity in the string
The inertia measurement equation is a function of the length of the strings holding the
test plate. Since the strings are stretching and contracting through these oscillations, this
length is not truly a constant. The string used was nylon fishing line with a Young's
Modulus of approximately 3 GPa.
A nominal value for load would be 23.52 N
m 1
(7.2kgx9.8 -- x-, assuming equal load on all three strings). Using a diameter of 2mm,
s 3
the cross-sectional area of the string is 3.14e-6m2 . From Hooke's law:
E=
E
= 0.002496
(2.13)
1
According to Equation (2.6), inertia varies with string length as -. In the range of values
of e used for these tests, the relationship is close to linear (-1% change in £ leads to
-1% change in I). For example, using a nominal string length of 2.6m, S&= 0.0065m.
This translates into a change of I equal to SI =0.000175kg.m2 , or a 0.7% error. As
shown earlier, the strain in the strings does not vary linearly with the applied stress,
making Hooke's law invalid. However, since the error due to string stretching is so
small, the effect of this linear approximation is negligible.
Measurement Error (weight, string length, oscillation period)
The period was measured by using a stopwatch to time 10, 20, and 50 oscillations. The
time at each of these checkpoints is used to estimate the period and data is averaged. The
measured time on the stopwatch (T)is related to w by the number of oscillations. For a
test over 20 oscillations:
2r
40"
ST= -;=T
20
(2.14)
2
I=
1600n
. T
•
(2.15)
Using nominal values for m and £, I changes by I = 0.0007696kg. m2 (1.194%) with a
measurement error of ST = 0.25s. This error source was addressed by collecting data
from tests as long as 50 periods as well as averaging over 10-20 tests per SPHERES
satellite, per axis, and per wet/dry configuration. Inertia sensitivity to changes in mass
would be less than sensitivity to time errors since inertia varies linearly with mass.
Sensitivity to changes in string length was discussed earlier, with respect to string
elasticity.
Orthogonal Rotational/Translational Motion
If the SPHERES satellite is rotating or translating in any direction outside of the desired
axis (vertical rotation), more complicated equations of motion would be needed to
accurately describe this behavior. The equations of motion used assume no translational
movement, and care was taken during the experiment to minimize the amount of this
motion.
Because of the coupling in a trifilar torsional pendulum, a CoM misalignment causes
sideways motion.
The energy in the translational oscillation is not included in the
rotation frequency (and therefore not observable in inertia measurements), and the
nonlinearity of the pendulum is amplified.
Consider that the torsional pendulum acts as a simple harmonic oscillator in both degrees
of freedom of interest:
A) a disc of mass m and radius r that oscillates about its center (amplitude of 0=20')
B) a mass m on a string of length f that swings at very small amplitude (0.01m)
causing unwanted side-to-side motion
The total energy of a simple harmonic oscillator is:
1
E= K.A2
(2.16)
2
where K is the spring constant and A is the oscillation amplitude.
1
In Case A, E A = -K AA
A where
2A
2
A IcoA=
(2.17)
AA =
r
For a circular disk, I = mr
2.
From Equation (2.6),
2
ac
ingr
it
2
Substituting into
Equation (2.16) yields:
EA
1
In Case B, E, =~KA where
2
2
202
(2.18)
KB = mfLoW
K'~
A, = O.Olmeter
For a simple pendulum, co2 = g
= r
(2.19)
Substituting into Equation (2.16) yields:
1 mg(2
SMg (0.01)2
2f
(2.20)
Assuming a translational amplitude of 1cm ( A = 0.01) and a rotational amplitude of 20'
( A = 0.349 rad),
E A = 0.037005J
E, = 0.001423J
(2.21)
SI = 0.002799kg -nm2
This constitutes an inertia error of 3.845%. Alternatively, if A, = 0.005 the inertia error
is 0.961%.
2.3.7 Empirical Mass Properties Conclusion
There are two goals for empirically measuring SPHERES satellite mass properties. The
first is simply to collect accurate information about the vehicle so that the controller can
be as efficient as possible. The obvious advantage to this is more precise maneuvering
and better fuel efficiency. This plant characterization continues in Chapter 3 with an indepth characterization of the CO 2 thrusters.
The second goal is to establish a truth measure for the online mass property identification
described in Chapter 4. From section 2.3.6, almost all the error sources in inertia
calculation are around or below 1%. Adding them together as a worst-case scenario still
amounts to <5% error. Because of this, the empirical inertia values serve as adequate
truth measures for online mass ID. However, because of the small magnitude of CM
offset, it would not be surprising to see deviations on the order of the values themselves,
despite how meticulously the mount was created and the tests were performed. While
averaging over several tests can help to remove some of the error bias, the CM
measurements are not exceedingly reliable as truth measures.
Chapter 3
Thruster Characterization
3.1
Propulsion System Overview
SPHERES satellites maneuver using twelve ON/OFF solenoid valve thrusters that
provide%independent control in three axes of rotation and three axes of translation. The
fuel used is cold gas (C02) and is stored in mixed liquid/gaseous phase in a tank at the
center of the satellite.
Since it is multiphase C02, the pressure inside the gas tank
remains constant at 860 psi until all the liquid evaporates. Directly downstream of the
tank is a variable pressure regulator calibrated to provide 35 psi C02 when the regulator
is completely opened. After the regulator is a small gas capacitor that allows any liquid
CO2 to expand before reaching the plastic tubing and the thrusters themselves.
The
capacitor also helps maintain constant pressure downstream when thrusters are opened.
The C02 is then fed through a series of manifolds and tube branches to supply all 12
thrusters with gas.
3.2
Propulsion Hardware
The fuel tank was placed near the center of the satellite along the Z-axis to minimize the
error induced by fuel depletion and slosh on the CoM and moment of inertia. While the
fuel is mixed phase, the pressure inside the tank remains constant at 860 psi. Once all the
liquid has expanded into gas (occurs with -28g of C02 remaining), the tank pressure
begins to drop as more C02 is expelled. Since this only occurs when the tank is almost
empty, we can assume a constant pressure supply for almost all tests.
The gas capacitor serves two purposes: allowing solid and liquid C02 to expand to
gaseous form and to provide a small buffer for changes, in supply pressure or demand on
the regulator. Through continuous firing, the regulator will start to get cold. When this
happens, its more likely that liquid C02 will make it into the piping past the regulator,
which can damage the tubes and thrusters or produce off-nominal behavior. This has
been seen even with a warm regulator when the tank is upside-down in 1-g and in microgravity. Also, there is a time constant associated with the regulator's ability to react to
changes in demand for C02, which occurs whenever a thruster is commanded on. As gas
is expelled through that thruster, the pressure drop propagates up the system back to the
regulator, which opens wider to maintain a constant 35 psi.
compensate for this delay since it acts a small reservoir of fuel.
The capacitor helps
Figure 3.1 Solenoid Valve Thruster 2
As per the control model representation in Figure 1.3, the thrusters (Actuators) are the
most important part of the propulsion system to characterize. The thrust exerted by the
solenoid valve thrusters depends on a number of things. A summary of applied thrust
dependencies obtained by [Chen, 2002] [5] is listed in Table 3.1.
Always set to 35 psi (regulator modified with slip
ring to "bottom out" at 35 psi)
Measured with an in-line contour projector
Solenoid variation negligible.
System
Pressure
Nozzle Area
Solenoid
Mechanics
# Open
Thrusters
Temperature
Firing
Duration
Thrust varies linearly with # of open thrusters
(R2=0.9955)
Negligible
Negligible - slightly positive slope in thrust profile
(<5% change after 70 sec)
Table 3.1 Thrust Dependencies [Chen, 2002] [5]
The location of each thruster was taken from the engineering CAD model used to build
the SPHERES satellites.
Table 3.2 has the nominal positions of each thruster in
centimeters relative to the geometric center of the vehicle:
-5.16
-5.16
0.00
0.00
9.65
-9.65
-5.16
0.00
-9.65
-9.65
-5.16
0.00
-9.65
-5.16
0.00
0.00
-9.65
-5.16
0.0 -. 65
-5.65
5.16
0.00
-9.65
9.65
5.16
0.00
-9.65
5.6
0.00
0.00
9.65
5.16
0.00
-9.65
5.16
Table 3.2 Thruster Locations wrt Geometric Center (GC)
3.3
Pulse-Width Modulation Scheme
In most of the control schemes run on SPHERES, the outputs of controllers are forces
and torques.
For variable-force thrusters and reaction wheels this is a fairly
straightforward command. However, since the SPHERES thrusters are binary, a PulseWidth Modulation (PWM) scheme is used [10]. This is accomplished using a mixing
matrix to transform forces and torques to thruster ON/OFF times. Figure 3.2 describes
the mixer process end-to-end.
Controller
Command
Mixing Matrix
(MMT )-
M* =M
Resolve Opposing Thrust
Resolve Negative Values
Scale by
Maximum
xT
Dead Zone
Pulse Centering
Ii
Multiple Thruster
Correction
.I
Check
TON >0
TOFF < T
Schedule
Propulsion
Interrupt
Srigure 3.L WIVwI MViixer •ole block Diagram
The mixing matrix that transforms the command vector into thruster ON/OFF times is
constructed using the calibrated force vectors for each thruster. First, the top half of a 6
by 12 matrix is populated with the three force vector components from each thruster. On
the bottom half, instead of force there is the cross product of force and location, leaving a
vector of torques for each thruster.
F2
...
F-2y
F1z F
F2g --. F12Z
MM=MT
Next, the pseudo inverse of this matrix is obtained:
=M
(2.1)
When a command
vector is multiplied by the matrix MA, the result is a vector of length 12 of commanded
forces normalized by each thruster's nominal force (i.e. a "2" means the controller wants
that thruster to fire twice as hard as it actually can). Since SPHERES uses pulse width
modulation, these numbers are actually fractions of the control period rather than
fractions of the desired force. If any of these values is greater than 1, then they are all
scaled down so that the net direction of the commanded firing will remain the same. If
all values are below 1,then net impulse can be conserved as well.
The next step is to take the vector of thrust durations and center all the pulses about the
middle of the control period. The algorithm then accounts for multiple-thruster pressure
drop by starting with the thruster with the smallest pulse and calculating how much
impulse is lost on that thruster and extending the pulse accordingly (using the linear
relationship between number of open valves and drop in thrust [5]). From here, the
algorithm works outwards, finishing with the longest pulse. Ideally, this would be an
iterative process, but for now our mixer does this just once.
The final step is to
implement a deadband of 10 ms (minimum time needed for thrusters to open) and round
up pulse durations that are very close to the entire period.
Figure 3.3 shows a simple example of how the commanded firing durations of two
thrusters are altered to account for strength differences and the drop in system pressure
when multiple valves are open. Since the thrusters are pulse-width modulated, the firing
duration of the thruster with strength F 1 is shortened and then the pulses are centered so
that there is no residual torque applied. The total area would be kept constant as well to
have the same applied impulse.
Corrected
Thrust command is pulse
Uncorrected
c
Thri .cqf
width mnrli ilat••
F2
F2
-
hnrf~n
F
pulse to account for higher
thrust and multiple thruster
pressure drop
,F
Thrust
F2
t
Figure 3.3 Thrust Correction for Simultaneous Firings
t
3.4
Propulsion Limitations
There are a couple of limitations that complicate the SPHERES PWM system.
The
solenoid actuators have a mechanical limitation that requires a 10ms opening transient.
This means that 10ms is the minimum impulse bit for thruster commands. Also, because
of how fast the propulsion software interrupt runs, the resolution with which a thruster
can be turned off is 1 ms.
Lastly, thrusters cannot be on during any ultrasound
measurement period since they can be misinterpreted as ultrasound signals used in the
position and attitude determination system.
This requires scheduling between global
position updates and command periods.
3.5
Measuring Nominal Thrust Vector
Nominal forces from the thrusters are key parameters for SPHERES system
characterization. Each thruster was turned on and off for one second while testing on the
load cell. The measured forces and torques over time in each axis were collected with a
SigLab data acquisition system and processed with MATLAB scripts to filter out teststand vibrations, change coordinate frames, and decouple forces and moments. Force
components were averaged over the one-second pulse to create a nominal vector of
applied force in all three axes for each thruster. From this, the resultant thrust magnitude
and direction were determined.
This characterized both the nominal thrust of each
solenoid valve as well as its misalignment with the satellite body axes. These nominal
values were averaged over 200-500 tests per SPHERES satellite.
The same 6 DOF load cell and test stand used to measure the Center of Mass was used to
measure nominal thrust strengths. A test program was written to cycle through all 12
thrusters in order and loaded onto a SPHERES satellite beforehand. This vehicle, with a
full tank and batteries, was mounted upright on the test stand with the regulator knob
pointing up. The power was turned on, the-1ropulsion system was pressurized and data
collection on SigLab begun right before the "start test" command was initiated from the
ground station laptop. Each test was run 5-10 times for repeatability.
_IV.
+Z
Af
Y
Figure 3.4 SPHERES Body Axes
Because of the geometry of the SPHERES satellite and the test stand, the two thrusters
pointing down would not register correctly on the load cell. This is because the force
from those thrusters is cancelled by itself since it reacts against the test stand. Therefore,
like with the CoM tests, the satellite was rotated 90 degrees and the tests performed again
to get the last two thruster strengths.
Since the load cell is 6 DOF, the output data
included forces and torques in three axes, so the misalignment of the mounted thrusters
could be examined.
Table 3.3 shows sample thrust strength and direction numbers for SPHERE SN 2. These
tests were conducted on all five of the flight satellites. As expected, these thrust strength
2
numbers are reasonably correlated with nozzle area (R = 0.42). "Dx," "Dy," and "Dz"
form the components of a unit vector in the direction of force for a given thruster. As the
chart shows, most thrusters have a unit vector direction aligned to at least 0.996 along the
principal body axes.
0.134
0.004
0.998 -0.022 0.025
0.130
0.002
0.999 -0.001 -0.025
0.122
0.006
0.059
0.996 0.032
0.130
0.004
-0.018
0.999 0.004
0.114
0.005
0.011
0.034 0.999
0.124
0.006
-0.022 -0.012 0.999
0.134
0.004
-0.999 -0.008 0.044
0.130
0.003
-0.998 -0.009 -0.037
0.128
0.006
0.042 -0.999 0.009
0.132
0.005
-0.019 -1.000 0.004
0.126
0.006
0.004 0.060 -0.998
0.110
0.005 I -0.002 -0.030 -0.999
Table 3.3 Thruster Strength and Direction for SPHERE SN 2
3.6
Thruster Latency
Each solenoid has a latency of about 6 ms. If we assume that thruster firings will be
much longer than 6 ms (say 1-2 orders of magnitude), then the difference in delay
I
between thrusters should not be a concern. Some tests were conducted on the prototype
SPHERES satellite to get an idea for this "relative" delay between thrusters.
For
example, two thrusters in the same direction were commanded to fire and the produced
torque was measured on a load cell.
Figures 3.6 shows some samples of the torque data from the load cell. The horizontal
axis is in seconds. Every two seconds both parallel thrusters were commanded to fire for
one second. The first maneuver at t = 0 seconds had Thruster A firing 3 ms before
Thruster B. Two seconds later, Thruster A fired 2 ms before Thruster B, and so on. The
middle maneuver at t = 6 seconds is when both thrusters fired simultaneously.
By
looking for the shortest impulse in the first figure (Z-moment) or the intercept in the
second figure (X-moment), the relative delay appears to be less than 1 ms.
V
/
-
20V -
Signal to
solenoid
5V io
F
t
Id
1Oms
i
/ \' T
FO
I
I
Applied
force
t
I
54
t
Figure 3.5 Thrust Latency in Solenoid Valve
Figure 3.6 Applied Impulse From Difference in Thruster Latency
The flight SPHERES satellites have access to accelerometer data at 1 kHz, which can be
used to get a better idea of the absolute delays for each solenoid. Figure 3.7 shows an
example of solenoid delay on the order of 6ms.
TMr.DdY
I
Figure 3.7 Sample of 6ms Solenoid Delay
The possibility of using the JR3 load cell to measure the solenoid delays of the flight
SPHERES thrusters was explored. The JR3 has a bandwidth of 8 kHz and in order to
resolve a 6ms delay in thruster actuation, an effective bandwidth of 2 kHz is desired (20
counts over a 10ms period). When a mass is placed on top of the load cell, q drops:
ofg=
=
Vm
(2.2)
The stiffness of the load cell to forces in the X and Y directions is c = 18x10 3 in English
units. With a typical SPHERES satellite weight of 7.7 lb, the bandwidth of the entire
system is approximately 48 Hz. The response time of the combined SPHERES and JR3
system will drown out the solenoid latency.
3.7
Multiple Thruster Pressure Drop
Since the capacitor is not infinite in size, the supply pressure coming out of the regulator
is not constant, but dependent on the flow rate. As such, the more thrusters that are open
at a time, the lower the pressure is at each nozzle. The lower supply pressure results in a
lower produced thrust. The first SPHERES mixer assumed constant thrust, while newer
iterations, including one described in [Blanc-Pacques '05] [1] compensate for the thrust
reduction, resulting in more accurate open (and closed) loop control. This new thruster
mixer was developed using the previous thruster characterizations.
Tests to see this
effect were carried out on a mock propulsion system, pictured in Figure 3.8. This setup
includes all the components of the propulsion systems on the flight SPHERES arrayed on
a plywood board [5].
P1
P4
Thruster 1
Figure 3.8 System Pressure Propagation Test Setup
Pressure transducers were placed at 4 locations in the system to measure the propagation
of pressure loads. Figure 3.9 shows the thrust commands and the corresponding pressure
from the four transducers. From this test, it appears that the reduction in thrust is not
very dependent on how close the thrusters are in the tubing tree. The effect of the gas
capacitor can be seen here where the upstream pressure does not change much with
increased pressure demand.
System Pressure
Thruster Configuration
LJ.H
.U.LUKL
r:i
· · *·'"·'
0;
'
'··--.·!-.··"·~-;:·
•
5
~·-~····I·--`'·"-
•
rWn
-o
.
N•n
I ..
,r,*
o
10
20
30
time (s)
40
50
6o
-
10
............
"
20
30
Figure 3.9 Pressure Change Propagation Data
40
time (s)
F -upstream
P
S P downstream
i70
60
50 P4 -downstream
3.8
Sensor Characterization
Having adequately examined the Plant (SPHERES satellite) and actuators (propulsion
system and thrusters), all that remains is to look at the sensors (inertial gyroscopes and
accelerometers) to complete our system characterization. The two inertial sensors are the
BEI Gyrochip II tuning-fork gyroscope and the Honeywell Q-Flex 750 accelerometer.
There are three of each on the flight SPHERES, providing telemetry for all axes. While
the accelerometers are mentioned here for completeness, they are not used to close the
loop, and so are irrelevant to the topics in this thesis. The specifications for these sensors
are summarized in Figure 3.10.
Max Update Rate
Range
1000 Hz
+25.6 mg
Max Update Rate
1000 Hz
Range
+83 deg/s
Resolution
12.5 [tg/count
Resolution
0.0407 deg/s/count
Bandwidth
300 Hz
Bandwidth
300 Hz
Noise (0 to 10 Hz)
< 7 tg rms
Noise (10 to 500 Hz)
< 70 pg rms
Noise (0 to 100 Hz)
< 0.05 deg/s/(Hz)1/2
Figure 3.10 Inertial Measurement Unit Specifications
·
In order to take full advantage of these sensors in closed-loop control, it is beneficial to
examine the noise characteristics. In addition to the noise listed under the manufacturer's
specification, there also appears to be a high frequency ringing associated with the
gyroscope when thrusters are opened or closed. As a tuning fork gyro, the Gyrochip II
vibrates one set of tines at a "drive frequency." The other set of tines (the "pickup fork")
oscillate at a sense or "pickup frequency" only when the gyro is rotating (Coriolisinduced torque) [17]. The difference between these frequencies is known as the "deltaF"
of the gyro, which in our case is about 338 Hz. This phenomenon is explained in more
detail in Appendix A.
When this frequency is excited, say by the small impulse of opening/closing a solenoid
valve, there is a decaying oscillation of 338 Hz in the gyro data. Since this noise is tonal,
it can be filtered out fairly easily with a notch filter. Figure 3.11 shows gyro data where
this high frequency noise is especially apparent. Since this data is sampled at 1 kHz and
the noise in question is about 338 Hz, the effect of only having three samples per period
is apparent (Nyquist requires >2). Figure 3.12 shows a Fast Fourier Transform of gyro
data sampled at 1 kHz to isolate this noise peak.
25D
1......
Fii
Figure 3.11 Filtered 338 Hz Gyro Noise
A couple key characteristics of these sensors are the biases and scale factors used to
convert from counts to rad/sec and m/s 2. The biases and scale factors of each gyroscope
were computed with the help of a rate table, while the biases and scale factors for the
accelerometers were taken from manufacturer specifications.
These values are stored in
flash memory on the corresponding satellites and are automatically used when IMU
readings are converted.
FFT of signal with Ts = 0.001. DC :comrn
ponent:(0.00038837) is not shown,
0.5
0.45
0.4
0.35
0.3
0•.25
p 0.2
E
0.15
0.1
0.05
O
0
1
50
100"
A,
150
200 250
300
frequency [Hz]
350
400
450
500
Figure 3.12 FFT of Gyro Ringing
With gyro-specific scale factors and biases and online filtering of the 338 Hz noise, the
gyro output is very accurate, which is important for closed-loop control and for the online
mass property identification algorithm.
Chapter 4
Mass Property Identification
4.1
Online Mass Property Identification
Thruster values and relevant sensor calibration information remains the same over normal
operation, but mass properties of the SPHERES satellites change as fuel is depleted.
While previously measured empirical values for CoM and Inertia provide a good start for
control algorithms using these values, having them update online is preferred for more
accurate control performance.
The mass-property algorithm and equations ((4.1) through (4.8)) are taken from "On-line,
gyro-based, mass-property identification for thruster-controlled spacecraft using recursive
least squares," [Wilson, Lages, Mah] [21]. The general approach to online mass ID is to
use gyro information about rotational rate to estimate angular acceleration and system
characterization information (especially about the thrusters) to estimate applied torque.
With this information, Euler's dynamical equation is used to find inertia assuming a
center of mass offset, or find the moment arm (hence center of mass offset) assuming a
value for inertia.
The center of mass offsets can be identified with better accuracy with pure translations
along a single axis since the unbalanced effect of these offsets is much easier to see when
the range of the gyro data is small. The two major error contributors in flying along a
straight path (pure translation) are the thrust strength variation (which we've
characterized) and the center of mass offset. While identifying center of mass, the
difference in torques provided by parallel thrusters is the "signal" that provides the
algorithm with the necessary information to identify CoM.
If there are rotational
maneuvers occurring, this signal is easily lost in much higher magnitude "noise" of largescale rotations.
On the other hand, pure rotations about a single axis, provide the best data for estimating
the inertia matrix elements. The inertia associated with that axis depends on the estimated
angular acceleration during the maneuver, which is most accurate during pure rotations
since the effect of nonzero center of mass offset in the gyro data is minimized. Figure 4.1
shows a representation of these two scenarios.
z
= moment x Jorce = la
'
=J
F
I
T.i. 1
r
Lfj
p
_
mome
moment
Figure 4.1 Ideal Maneuvers for Inertia and CoM Identification
4.2
Theory and Equations
The first step in formulating the equations behind the mass ID algorithm is to take Euler's
dynamical equation, and write it in a form that allows least squares (LS) analysis, with
gyro data as our measured quantity and with CM offset and inertia as our unknowns. In
the following equations, L is the thruster locations measured from the CoM, D is the
nominal thrust directions (unit vector), F is the nominal thrust, B is the blowdown effect
from decaying thrust, and Tis a binary on/off value for each thruster.
(o= I1 ' (' - @ICo)
(3.1)
)
(3.2)
S= thrusters + Zdisturb = (L x D F + zitub
Fk = B(Fom +Fbias +
(3.3)
adom,k )Tk
The algorithm is equipped to handle disturbance torques, random variability in the
thrusters and blowdown, although for our purposes at this stage, these quantities are
considered negligible. Equations (4.1), (4.2), and (4.3) are combined to give:
Co = I' ((LxD)B(Fom +F
+
Fbi' ,ndom,k Tk
+
diturb
- OX(I
O))
(3.4)
The next step is converting this Equation (4.4) into the standard least squares form,
Ax - b. For the center of mass identification, there are a few more definitions: C is the
true center of mass and A is the CoM offset.
C = Cnom +A
L=Lom -A[1
(3.5)
. . . 1]
(3.6)
Refer to [Wilson, Lages, Mah] [21] for more detailed derivations. The final least squares
form is:
Ak
=I-1
0
-c3
2
C3
0
-C1
-- C 2
C1
0
1
x
k
bk = +-1 1(cox(+I))-I-' (LnomxD)FnomTk
A2
A3
Center of Mass LS Formula
ck - DFnoT
(3.7)
k
The vector of center of mass offsets fits well into the standard least squares form.
However, the inertia matrix is a symmetric 3x3 matrix with 6 independent terms and
needs a special transformation to conform to Ax _ b.
Also, the inverse of the inertia
matrix is identified because of the form of the equations of motion:
a,
Ak =0
0
0
a2 a3 0
I
a2
0
a1
0
0
a1 a2
I-I2
33
-0 0
a3
a3
x=
bk
k
12
ak = (L xD ) FomTk
S(LD)F)
-o
1131
-
I I3nInertia LS Formula
(3.8)
For Equations (4.8) and (4.9), the inverse inertia matrix and center of mass offsets are
identified by using the standard batch LS solution,
= (ATA) -1ATb. This is acceptable for
"off-line" data processing for computing mass property estimates.
For on-line
estimation, a recursive algorithm is used, incorporating the covariance and state update
equations:
Qk+ -1 +=Tk-IAk+1
= Xk + Qk+lAIZR+l
4.3
Ak-I
[bk+-Ak+lk-]
(3.9)
(3.10)
RGA Micro-gravity Testing
From February 5-7, 2002, the SPHERES team conducted tests aboard NASA's RGA.
Mass property identification tests were run to help with development of the algorithm as
well as to provide good, independent estimates of inertia and center of mass offsets for
each of the flight units. Two types of tests were performed to help identify the mass
properties of each SPHERES satellite. One set of tests was composed of 12 different
maneuvers: pure rotations and pure translations in each axis in both directions (2 x (3+3)
= 12). Essentially, pairs of thrusters were turned on for one second to produce pure
rotations/translations in both the negative and positive directions. The IMU data was
sampled at 100 Hz, which was the most data that could pass through the communications
system in real time.
The other set of tests had seven maneuvers, which involved turning on one thruster at a
time for one second each (maneuvers 1-6 each tested 2 different thrusters). These tests
had a sampling rate of 1 kHz and required a period of about 30 seconds to download the
IMU data afterwards. The last maneuver in this set of tests was created with the intention
of testing the robustness of the mass ID algorithm. It consisted of a series of firings of
duration varying from 0.01-0.02 seconds. Some firings included orthogonal thrusters to
excite motion around multiple axes.
Figure 4.2 shows some sample filtered gyro data taken during a test on the RGA. The
plot on the left shows a pure rotation in the Y-axis and then an equal and opposite
rotation. This data is ideal for identifying Iyy, Ixy, and lyz. The plot on the right is from a
pure translation test along the +Y-axis. As is expected, the angular rate values do not
change much over the course of the two firings. This data is ideal for identifying the
center of mass offset along the x-axis. Assuming we know the thruster misalignments,
the nonzero change in angular rate in the figure is purely due to CoM offset and is easily
observable. It is this information the algorithm uses to identify CoM offset.
..
...
.
" ............• ...........
...........
.. * . ... .
I-... ... .. ..............
...........
..
. ··.
·...
..........
...
......
....
I
i;
··· ·-I----· ..........
. ..........
,
··.
....................
r-·- ; -,;···;·~1I·-;I······
.........
.
Y"o
-
ma
ma m
"o
0
...........
............. ..................
...........
s
o s
o 0
10
Figure 4.2 Three-Axis Gyro Data From RGA Test
Every data file collected during the RGA flights was analyzed and catalogued. Each
SPHERES satellite ran from 2-8 successful iterations of each maneuver type (translations
and rotations in x, y, and z), totaling in 15-40 micro-gravity tests for each SPHERES
satellite. Continuous, undisturbed data was extracted from as many tests as possible and
run through the offline MATLAB implementation of the mass property identification
algorithm. Mass property estimates for each SPHERES satellite were separated and
averaged to produce nominal values. These results are shown in Table 4.1. As expected,
the values are similar across all the satellites.
0.967
0.0284
-2.68
-0.0000837
0.0268
0.55
0.000014
-0.00029
-0.33
0.02795
-0.23
0.000108
0.0238
0.737
0.000157
0.0000123
3.517
0.0260
0.771
-0.0000082
0.0229
-1.87
0.000149
-0.000221
Table 4.1 RGA Mass Property Results
0.0236
0.0204
0.0208
All the usable data was also concatenated to test the convergence of the mass property
estimates over long tests. Figure 4.3 shows an example of all the Z-axis rotation data
(measured by the gyro along the X-axis) for one SPHERES satellite and the
corresponding angular acceleration estimate. SPHERE SN 1 performed 5 Z-axis rotation
maneuvers (5 seconds each) and 3 "combo" maneuvers that excited all axes, totaling 34
seconds of usable concatenated data.
Gyro Data
'"^
Angular Ac celeraion Estimate
^"
--... ... ....
... .. ----
...
--- --
-4
0.4
3O2
S
I
I
2500
IL
II
Ii
II
I
II
II
1 1
I
I
0.2
2600
.....
....
...
..
.....
......
..
".....
. ... ....
..
..
..
...
.....
...
.. .....
. ... ..
..
..
.....
1600
...
................
....
..................
..........
*
*
oomaoy
-
.
I
z g"yro
I
J
LA
2
22
--L - ,
,
-0.4
0
70.6
n00
.e
222
2.4
2.6
time (ms)
268
3
3.2
34
1.8,
2.4
x 10
2.86
time (ms)
2.8
3
3.2
3.4
x 10
Figure 4.3 Concatenated RGA Gyro Data
NASA Ames/Intellization also conducted some tests with SPHERES aboard the RGA.
As with the tests above, raw IMU data was collected and post-processed using a
MATLAB version of their mass property identification algorithm [20]. The green lines
in Figure 4.4 indicate when the RGA pulled out of its freefall. Data telemetry collection
was paused during these periods using real-time commands from the control laptop. This
pause/resume feature was implemented to keep the data stream free of disturbances
caused by the satellite bumping the wall or being handled by the test operators. On the
thirtieth parabola, a known mass was attached to one side of the vehicle. By adding a
measured proof mass with a precisely known mass, CoM, and inertia, it was possible to
determine if the mass property identification algorithm was accurate in measuring
changes in these parameters. Around the fiftieth parabola, a near empty tank of C02 was
used to replace the current tank. This tank had been depleted to the point that all the
liquid C02 inside had expanded to gas form. Since the pressure inside the tank is no
longer constant as gas is expelled, the resulting force from the thrusters is no longer
constant as well.
-
IDeadinertia
X10
1
p
2.s
.5-
ID~ed
dolt&C
.
A1
4'C
Is.
1!
i5,
-0.5-
1
pI0010ro
a.
-..
-'5---
between parabolas
500d
~c.5..
-2.3-
~
~
o4~
-0.55'.
----0
~20
10
F"
It dolt.c
S.-
-3-
)3z
7
k~
S-1.5
dltaC.
deltacy
-3.5ý
$
30
-4
40
50
Ime [sconds]
60
70
0
10
20
30
40
50
tm e [econds)
o
70
80
Figure 4.4 AMES/Intellization Mass Property RGA Results [Wilson, 2003] [20]
The RGA testing provided high-fidelity data for post-processing. The mass property
identification algorithm was validated offline by estimating mass properties similar to
empirically measured values. The NASA Ames tests succeeded in identifying known
changes in mass properties from the addition of a proof mass.
4.4
Algorithm Development
The first few iterations of the mass property ID code were developed in MATLAB using
a batch least squares approach described in "On-line gyro-based mass-property
•(
identification for thruster-controlled spacecraft using recursive least squares" [21] to
post-process data. Later revisions included simulated recursive LS convergence, varying
methods for estimating di,offline C code that used mock data, and finally an online realtime version that ran on the SPHERES Embedded C++ operating system.
The first hurdle to clear when implementing the mass-property identification algorithm
was to characterize the key system parameters, which is what most of the preceding
chapters are dedicated to outlining. Once the thrust strengths, directions, and moment
arms were identified and nominal mass properties were determined, the only missing
piece was how to estimate ;)from the onboard gyroscopes. The crucial part of this
problem was that 6)is the measurement vector for the RLS mass property equations, and
its variance must be small enough to make the inertia and CoM estimates meaningful.
Also, differentiation is an inherently noisy process, where a simple difference can raise
noise by a factor of1 (1,000 at 1 kHz). This is why careful attention was paid to both
At
the noise of wo and noise created by the process of estimating (0.
4.4.1
Gyro Filtering and Interpolation
As explained in Section 3.8 and in greater detail in Appendix A, the SPHERES
gyroscopes have a non-trivial noise component at 338 Hz. Raw gyro data is always sent
through a low pass-filter, either online before it is sent across the communications
channel or during post processing in MATLAB (Figure 3.11 shows unfiltered gyro data).
In the latter case, a zero-phase forward-backward filter with two poles at 50 Hz is used.
Even before the rate data is filtered, it is sent through an interpolator to account for lost
data points due to dropped telemetry packets (not an issue for online testing). This makes
the signal even smoother and allows a uniform sample rate assumption for the filter. This
filter was chosen because it sufficiently reduced the noise (approximately -20DB)
without softening the corners in ) where rate: changes occur, and because it produced
acceptable values for the variance of gyro data. Figure 4.5 shows a sample of filtered rate
data collected during one of SPHERES micro-gravity RGA tests. This test consisted of
two one-second long torques (equal and opposite) around the X-axis. Also shown are the
post-filter calculated values for R0, the standard deviation of the gyro signals.
Fietarg
X -GyFQ
D0*taSN 1)
'i
i
..
,.
..
): : ~36
i·
i
2000
rr
:·
i,
r-;·~···:l.·····;·-.
1900
i·
i
·;r·-r·..,.,.--·.·1·-·)··-~::···i··1
:
i·;
·L·····-··'.'6"'.'l"Z
:
·li
6
:·
:·
*
i
rl
1700
:
i:
i
r
i
i1600
•.m
· · ·.· ;·······.-
j
i
:
.... t~t.~-TI · · · ~i·?t;·.-jli
::
:i
1
*··
::
i
S. 600
. 1000500... 2W
:2500
30003500
Updnte
Pointrms)
9.5661e -7
=0
0
0
2.7365e -7
0
0
4000
0
0
rad s
2.3044e- 7
Figure 4.5 Filtered Gyro Data Standard Deviation
4.4.2 Angular Acceleration Estimation
A number of different candidate filters were considered for estimating b.
Finite
differences are very light on computation, but produce unacceptable noise levels in the
output. First order line-fits are suggested [Wilson, Lages, Mah, 2002] [21], and many of
these were used and analyzed.
The window of a line-fit determines how many data
points to take into account for each estimate of 6, which is equal to the filter order for
discrete filters. Two different line-fits are compared in Table 4.2, a non-overlapping linefit and an overlapping ("sliding") line-fit, for varying window sizes. Both of these linefits are "centered" (the 6) estimate is centered around an odd number of Co values) and
are therefore acausal and unusable in real-time. A backward line-fit that only operates on
previous values should be used for online estimation.
Table 4.2 Standard Deviations for Line-fits of Varying Order
Sliding line-fits of high order clearly provide the lowest variance estimates, but not low
enough. Using these variance values for the measurement vector in the mass-property
identification algorithm led to uncertainties in inertia and CoM on the order of the values
themselves.
The approach that is currently implemented to estimate 6is a discrete Kalman Filter.
The propagation and measurement update equations are derived from:
k+l= kXk + Wk(3.11)
Zk = Hkxk +vk
I
In Equation (4.11), x is the estimate of the true value x, wk is the process noise from
propagating x, 0 is the propagation matrix, z is the measurement, v is the
measurement noise, and k is the update number. Figure 4.6 shows the form of the
general Kalman Filter equations as they were implemented in code.
6ýý
Measurement update
+H Rk Hk
= (Pk'
Kk
Xk
7
P
kHIRk
k
= Xk +Kk (zk -Hk
k kXk-
I
41,ý
ii
- Update number
- (superscript) indicates "a priori" knowledge
- Covariance matrix of estimate error
- Matrix relating measurement and state
vector
- Measurement error covariance matrix
- Kalman gain
- Estimate vector
- Measurement vector
- Propagation matrix
- Process noise covariance matrix
Figure 4.6 Kalman Filter Process
Assuming constant thrust and no energy loss due to drag, dc is a constant during any
given thruster firing configuration. By using a Kalman Filter to identify a constant [18],
all the previous information during a given thruster firing is used to determine that
constant. Each successive measurement update increases the confidence in this value,
leading to very low values of variance. Since the estimate vector is a constant and the
derivative of the measurement vector:
0=1
Qk=4Ewk
R
Rk
1
(3.12)
E[VkVk.]
=
Hk =tk
This Kalman Filter has been shown to converge well for initial conditions Xj =0 and
(PO-
= 0 as well as for more accurate starting guesses.
This technique allows the variance: of angular acceleration to decrease to a level that
gives reasonable convergence values for the mass properties.
configuration changes, the Kalman Filter resets.
Whenever the firing
The initial estimate of angular
acceleration for the Kalman Filter is set to: a number calculated using nominal thrust and
inertia, and the covariance of the initial estimate is reset to a very conservative value.
The gyro data in Figure 4.5 was sent through the oh Kalman Filter as well as line-fits of
varying order for comparison. Figure 4.7 shows plotted angular acceleration for a nonoverlapping line-fit and a sliding line-fit (both of order 15) and the Kalman Filter.
Angular Acceleration Estimation
-1overapping
1.9
2
2.1
22
2.3
Not2.4-verlppin
2. 5
4
x 10
-1
1.9
2
2.1
2.2
2.3
1.9
2
21
22
Time (ms)
2.3
2.4
2
X104
Kalman Filter
2.4
25
4
X 10
Figure 4.7 Angular Acceleration Estimation Comparison
The trade-off between the Kalman Filter and the line-fits is that the KF estimate can be
fairly uncertain at the start of each new firing, whereas its variance becomes much
smaller after a few updates.
The KF estimate variance at the end of this data set is
5.4508e- rad/s4 . Figure 4.8 shows the variance profiles for each estimation method.
Using the Kalman Filter over the line-fits led to much better convergence of mass
properties.
Innr•
Omega Dot Variance Comparison
(U
U)
U
C
(U
Update Point(ms)
Figure 4.8 Estimation Routine Variances
4.4.3
Mass Property Identification
At this point, the thrust characteristics and nominal mass properties are known, the gyro
data has been interpolated and filtered, and an co estimate with low covariance has been
calculated.
xk+A= (A
The MATLAB code performs a batch LS over all the data using
A)- A'b to give an estimate of inertia and CoM. Then the calculation is done
recursively to show the convergence of the mass property estimates. Depending on the
length of the test, the initial guesses for the recursive estimator are provided by a batch
LS of the first two firings.
This initial batch calculation is to help avoid a poorly conditioned estimate covariance
matrix. Many of the test maneuvers consist of single axis translation or rotation since
they are the easiest way to make specific mass properties observable. Unfortunately, this
means that little to no information about the other values in the estimate vector is being
provided, so the difference in the covariance values is very large. An ill-conditioned
matrix is extremely prone to digitization error, especially when inverted.
Another
procedure for avoiding this problem is that the MATLAB code can scale down the
Recursive Least Squares RLS equations, (4.7) and (4.8), depending on which axes have
been excited.
For example, a moment about the X-axis will only exist if thruster 5, 6, 11, or 12 is fired.
If only these thrusters are fired, I~, I , and I7 are unobservable. If running the mass
property algorithm on this data merely leads to radical values of those inertias, this is not
a problem, but if it causes matrix inversion errors from a high condition number, it will
pollute the value being measured, I:. If those inertias are assumed 0, Equation (4.8)
reduces to:
Ax
a, 02 a2
3
0 a, 0 1I-=
0 0 a, I
=b
(3.13)
Though this eliminates the poorly conditioned Q problem, it is impractical for tests with
maneuvers about multiple axes. This would require dynamically changing sizes for all
the vectors and matrices, which is difficult and inefficient to implement. Of course, if
there are different maneuvers about multiple axes, Q will be well-conditioned and the full
order equations are ideal.
Using the pseudo-inverse instead ('pinv' command in
MATLAB) will usually be able to force a solution regardless of poor conditioning, but
I
the resulting values can still be distorted. In the actual algorithm, the first two firings are
used for a batch estimate. Chances are enough axes are excited to avoid these issues, and
subsequent updates occur with each new measurement.
Once an initial estimate with reasonable covariance has been provided, the RLS
equations can calculate the convergence of the estimated inertia and CoM. The final
value of the recursion should equal the total batch results since the two processes are
mathematically equivalent. Figure 4.9 shows an example of I, convergence.
bxxRecursive Estimate
0.1
iI
* l
•
II
I
I
I
I
I
I
II
II
II
I
I
I
0.08
r --I
- ----- - -- 1 ------ -
0.06
E
I
S0.04
-
I
1i
I
-
,
I
0.02
i
1.........
........ I ....
.
I
Il
I
I
I
I
I
II
............
II
I
I
I
e
Ij
I
II
I
0
0
1000 2000 3000 4000 5000 6000
update #
7000 8000 9000 10000
Figure 4.9 Sample Ixx Convergence
The green and red lines are la boundaries based on the estimate error covariance. The
regions where the a envelope closes rapidly are during rotations induced about the Xaxis, when I, is most observable. Similarly, the areas where the confidence envelope is
flat correspond to times when the SPHERES satellite is free-floating or firing about
orthogonal axes, which provides very little information that can be used to improve
confidence in the I, estimate.
4.4.4
C Code Implementation
Once a fully operational offline mass, property identification algorithm was functional,
the next goal was to transport it to the SPHERES operating system for online use, which
would allow the satellite control routines to take advantage of mass properties updated in
real-time. This process was made easier by separating the task into two parts, each with
its own complications.
Porting the mass ID algorithm from MATLAB to C Code
required thinking about memory allocation (local stacks, static and volatile variables).
Also, computation in C is handled differently than MATLAB, especially with matrix
inversion, matrix multiplication, and indexing. Once the code is functional in C, the
language barrier is crossed, but changing the code to work on a real-time DSP is nontrivial. The various threads of the SPHERES flight code operate on prioritized periodic
interrupts, which require careful scheduling of processor time. Thread scheduling places
constraints on when a piece of code can retrieve its inputs, when it can make its
calculations, and when it can send data over the communications channel. Another
important issue that doesn't exist in the offline implementation is limitations on processor
power and communication bandwidth. Inverting a 300x300 matrix at 1 kHz or sending
every RLS variable over the wireless channel is not feasible.
The first step was writing the basic functions in Visual Studio C++. These functions
were tested by taking previously collected telemetry data and writing them to a text file in
MATLAB.
The C Code would read these files as inputs, calculate mass property
estimates, and write the output to other text files, which would be read by MATLAB
scripts and compared to the outputs of the original software.
Figure 4.10 compares
angular acceleration from the MATLAB and offline C Code. The error is probably due
to the different precisions of matrix inversion processes. Since both still converge to the
same value, the initial error is assumed negligible.
0.56
0.54
'664
-- . - .
Angular Acceleration Estimate
I
I - --- -4-_---
I.
. .
. - -
11
,II*.
II
I
-
MATLAB
-
0.52
aos,
-.
048
0.481
.t .
II
1
I
II
0.42
~~
. .~
. . - -- -- --
I
1|j
1,9
---
-
1.95
--
I
I
-•
. - TI T---------1-------I --.
-
I
II
-I
0.44
0.4
- -----II --- ---c
I
I
~--'-iI
I
i
-. . - ,-I . . . .- :--
I
I
2
time (ms)
I
I
2.06
-- - -
I
I
2.1
x 10
Figure 4.10 MATLAB vs. Visual C Acceleration Estimation Results
After validating the functionality of the C code algorithm, tests were conducted using the
system clock to try to simulate real-time conditions.
The highest frequency interrupt on the SPHERES DSP is the propulsion interrupt at 1
kHz. This defines the minimum thruster command and the maximum rate at which the
sensors can be sampled.
From an estimation accuracy standpoint, it is desirable to
operate at the maximum update frequency. However, running the ID algorithm at 1 kHz
caused problems with processor time, so the algorithm was changed to trigger off the less
frequent interrupts. For more specific information on the online C Code implementation,
refer to Appendix E.
Figures 4.11 shows test results from the online mass ID algorithm. Gyro data was also
downloaded and fed into the offline C version to compare outputs.
IntersInertia
(Izz)- Sphere
Online
Inversnertia(Izz)- VisualStudio
Ju
I
I
I
I
I
I
I
I
I
I
I
I
- - -
I
-
-I-
-I-
-I--;:
'+----i-
I
I
I
1I
I
I
I
I
I
I
r-.....r--
I
I
I.
II
I-
I
I
-I
L~l
0
0
10
I
-- I
20
I
T.
I
I
I
I
I
I
I
I
I
I
I
I
I·---_-.
30
I
1I
I.-.-~....~..~.
1
I
I
I
I
-
I
I
40
50
update
#
I
I
i
I
I
I
7-II
I
----
i
-I
i
I1
60. 70
I
I
I
80
nl
0
90
10
I
I
20
I
I
~
..-I
I
I
I
11
~
~
I
I
I
I
I
II
I
I
I
~
I
I
I
30 40
I
I
I
I
I.
I
50
•
II
I
I
II
II
I
I
I
I
80
I
I
I
70
80
90
update
#
Figure 4.11 Visual Cvs. Online Embedded CMass ID Results
Figure 4.12 shows the Z-axis gyro data for a maneuver designed to test the full
functionality of the online mass ID code. The test included 4 pure rotations about Z
designed to identify I, followed by four pure translations designed to identify XCM.
81
Z-Gyro Data
08
,
-0.4 - 0.64 --0.46
-
•+I
-- - I
0.28
L
---I
-
I
-"-____I l
l
-/
4.6
4.8
-2Figure
4.12
4.6
4.8
axisCM
-0.8offset
5
Z-Axis
l
5.2
5.4
Gyro
5
Data
5.2
-
I
I
I
L
- I
I
58
6
--
--
ii
n
56
From
,
Online
5.4
5.6
time (ms)
5.8
l
62
Mass
6
6.4
ID
l
6.2
Test
z esmate
6.4
4
X 10
Figure 4.12 Z-Axis Gyro Data From Online Mass ID Test
The resulting RLS estimates are plotted in Figures 4.13.
Izz esbtmate
X-axis CM offset
II
I .I
1
I
-0.2
--
-0.48
-
--
--
S
+
I
----
t
-t
I
I
---
I
I
--
III----4I
II I
I
-- -
I
o
--
T .
I
.
.1--.
.
.
.
4II ---
SI
-1.2
•
0
0.05
4-
0.04
I
I .I.. -
.
I
-
I.
--I
- ----- --
*I
I
0.0
0.06
------
I
-
I
I
T-1
I
.
I
I -
--
I
I
I
4
I
I
I
20
40
60
80
100
update#
.
.
-T - I
I
4.
7- I1
.
I
4
I
-I
I -
4
I
I
I
I
I
120
140
160
--
0.02
180
0
4--
20
-----
-----4
40
1I
--
--III-
I----
60
- T- Ir
4-
-
80
- i - I
-r
-- I I
-----
100
120
140
-
--II4--
-
-- [.
.
160
180
update#
Figure 4.13 Online Mass Property Estimate Convergence
As expected, the inertia estimate converges quickly during the pure rotations while the
CoM offset jumps wildly. During the pure translations, the inertia covariance does not
improve, but the CoM offset converges quickly. The primary goal of this test was to
show that the online algorithm converged to the same value as the offline MATLAB
code, which it did. The actual estimated mass properties are incorrect, probably because
of drag on the air-table or gas escaping from the air-bearing in a non-uniform way. The
online algorithm in real-time C has been shown to be equivalent to the offline C++
version as well as the offline MATLAB version, which from RGA tests was shown to
estimate mass properties correctly.
Chapter 5
Conclusion
5.1
Thesis Summary
The first chapter introduces SPHERES as a low risk, high fidelity platform for
developing algorithms for autonomous spacecraft.
The various components of
SPHERES hardware are examined. The motivation behind creating SPHERES to test
autonomous rendezvous, docking, and formation flight is explained and related to current
projects and areas of active research in the field of aerospace. Also, the motivation
behind system characterization as a candidate algorithm on SPHERES is provided.
Results of empirical tests to establish a truth measure more reliable than CAD estimates
by obtaining the mass properties of the SPHERES satellites are presented.
The test
procedure and error sources for CoM measurements are briefly outlined. Since moment
of inertia isn't measured directly like CoM, a model is introduced to approximate a
torsional pendulum. Equations and assumptions are presented and results are shown and
compared to CAD values. Error sources for inertia measurement are analyzed in detail
and the procedure behind measuring products of inertia is explained.
Empirical
measurements are proven to be more reliable than CAD predictions.
Having characterized the SPHERES satellite itself (Plant), the thrusters (Actuators) and
gyroscopes (Sensors) are analyzed. The hardware associated with the propulsion system
and sensors is detailed as well as the thruster locations and thrust dependencies. The
PWM routine is explained in detail, including a block diagram of the mixer code. The
test setup for measuring thruster strengths and directions is outlined and results are
displayed.
Different measures of thruster latency are presented and the difference in
delay deemed negligible. Data depicting the multiple thruster pressure drop phenomenon
is shown and observations made. The sensor specifications are reviewed and the noise
characteristics of the gyroscopes are characterized.
Finally, a mass property identification algorithm [21] is introduced to improve mass
estimates on line, thereby allowing even finer control than offline estimates. The theory
and equations behind the algorithm are presented, and results from micro-gravity
experiments on the RGA are presented.
The development of the mass property ID
algorithm from an offline batch LS in MATLAB to an online recursive LS in real-time C
is described.
This included a discussion of gyro filtering, a comparison of multiple
candidate algorithms for finding angular acceleration, and a description of the Kalman
Filter that was chosen. The transformation from MATLAB to C++ to Embedded realtime C is explained, with data comparisons between the different versions provided.
5.2
Conclusions
Offline characterization can be used to improve control performance for autonomous
spacecraft. Having accurate knowledge of important system parameters can lead to more
efficient maneuvers.
While characterizing mass properties "offline" is helpful, online
estimation offers another level of fine control because of shifting parameters like fuel
mass.
This is even more helpful for drastic changes in mass properties caused by
deploying extendable parts or docking with other vehicles.
The CoM tests performed do serve a purpose, but two non-trivial error sources exist: the
play in the mount for the load cell and the effect of the liquid fuel. The 0.5mm play in
the mounting configuration is unavoidable with the available manufacturing tolerances.
While the fuel mass shouldn't affect CoM measurements while statically mounted to the
load cell, it can change the CoM by a factor of 1 or more when in motion. The inertia
measurements proved more robust, as all the error sources were resolved to around 1% or
less of the measured value.
Ideally, the absolute thruster latency would be known so that the controller and mass ID
algorithm could account for the "lost" impulse. Knowing only that the delays are similar
does still help with centering pulses in the PWM routine. The results from the multiple
thruster pressure drop tests can help in future characterization. Since the effect seems to
be independent of thruster location, a simple model should suffice.
The gyro-based mass property identification algorithm was validated against CAD and
empirical values with test data taken aboard the RGA. A Kalman Filter estimator for
angular acceleration was proven to be the best routine.
The mass ID algorithm was
proven to function in an online implementation, allowing for real-time controller
adjustments.
Future missions can use this technology to continually update plant models and allow for
tighter control.
Not only could they benefit from being able to track changing mass
properties due to fuel depletion and other dynamic effects, but algorithms like these can
also help compensate for poorly measured or rough system parameters.
Better plant
knowledge and tighter control can have effects as minor as marginally improving fuel
efficiency and as major as saving a mission from failure, and has applicability to any and
all space vehicles.
5.3
Future Work
Recommendations for future work are as follows:
* Integrate accelerometers into the mass property identification algorithm either as a
way to check thruster strengths or to estimate mass change from fuel depletion.
Since the accelerometers are not at the CoM, there also might be redundant
information about the angular rate that could be fused with gyro data.
* Perform online mass ID tests with 2 SPHERES satellites docked. Both vehicles
could provide independent estimates using their own gyros. Care would have to
be taken to ensure there is no plume impingement and that both satellites are
aware of all thruster commands.
* Restrict mass property ID to favorable maneuvers. This might be accomplished
by assigning a signal-to-noise ratio threshold for CoM and inertia estimation,
dependent on the commanded thruster profile. This could include a dynamically
changing A matrix to minimize any singularity distortions.
* Account for fuel slosh in the algorithm. While deemed mostly negligible, it may
be possible to excite a mode along the long dimension of the fuel tank, causing a
greater disturbance. Including a model of how the fuel slosh should affect gyro
readings as a function of how much fuel mass remains could improve results.
Bibliography
[1]
Blanc-Paques, P., "Thruster Calibration for the SPHERES Spacecraft Formation
Flight Testbed," Master's Thesis, MIT, 2005
[2]
Bloomberg, J.J., Layne, C.S., McDonald, P.V., Peters, B.T., Huebner, W.P.,
Reschke, M.F., Berthoz, A., Glasauer, S., Newman, D.J. and D.K. Jackson, "Effects
of Space Flight on Locomotor Control," in Extended Duration Orbiter Medical
Project, Final Report 1989-1995, NASA SP-1999-534, NASA Johnson Space
Center, 1999.
[3]
Boynton, R.; Wiener, K. "Mass properties measurement handbook - part 3". SA WE,
Wichita, 1998
[4]
Brown, R.G., and Hwang, P.Y., "Introduction to Random Signals and Applied
Kalman Filtering," 1997.
[5]
Chen, A., "Propulsion System Characterization for the SPHERES Formation Flight
and Docking Testbed," Master's Thesis, MIT, 2000.
[6]
Chung, S-J, Kong, E. M., Miller, D., "Dynamics and Control of Tethered Formation
Flight Spacecraft Using the SPHERE Testbed." 2005 AIAA Guidance, Navigation
and Control Conference, San Francisco, CA, August 2005.
[7]
Dahleh, M., Dahleh, M., and Verghese G., Lectures on Dynamic Systems and
Control, 2002.
[8]
"Demonstration
of
Autonomous
Rendezvous
Technology
www.nasa.gov/centers/marshall/news/backgrounds/factsheets.htm,
[9]
(DART)",
September 2004
Enright, J., et al., "The SPHERES Guest Scientist Program: Collaborative Science
on the ISS," IEEE Aerospace Conference, Big Sky, Montana, 2004
[10] Hilstad, M., "A Multi-Vehicle
Testbed and Interface Framework for the
Development and Verification of Separated Spacecraft Control Algorithms,"
Master's Thesis, MIT, 2002.
[11] Kong, E. M., Hilstad, M., Nolet, S., Miller, D., "Development and verification of
algorithms for spacecraft formation flight using the SPHERES testbed: application
to TPF," SPIE Astronomical Telescope Conference 2004, Glasgow, Scotland, June
2004
[12] Kong, E.M., Saenz-Otero, A., Nolet, S., Berkovitz, D., Miller, D., "SPHERES as a
Formation Flight Algorithm Development and Validation Testbed: Current Progress
and Beyond."
2 nd
International Symposium on Formation Flying Missions &
Technologies, Washington, DC, September 2004
[13] Miller, David W., et al, SPHERES Critical Design Review, February 2002
[14] Nolet, S., Kong, E., Miller, D., "Design of an algorithm for autonomous docking
with a freely tumbling target." SPIE Defense and Security Symposium 2005, Vol.
5799-16, Orlando, FL, March 2005
[15] Saenz-Otero, "The SPHERES Satellite Formation Flight Testbed: Design and Initial
Control", Master's Thesis, MIT, 2000
[16] Saenz-Otero, A., Miller, D., "The SPHERES ISS Laboratory for Rendezvous and
Formation Flight," 5 th InternationalESA Conference on Guidance,Navigation and
ControlSystems, Frascati, Italy, October 2002
[17] Shkel, A., "Type I and Type II Micromachined Vibratory Gyros," IEEE, 2006
[18] Strang, G., "Introduction to Applied Mathematics," 1986.
[19] Tillerson M., Breger L., and How J. P., "Distributed Coordination and Control of
Formation Flying Spacecraft," IEEEAmericanControl Conference, June 2003
[20] Wilson, E., et al., "Motion-based System Identification and Fault Detection and
Isolation Technologies for Thruster Controlled Spacecraft," JANNAF 3 rd Modeling
and Simulation Joint Subcommittee Meeting, Colorado Springs, CO, December
2003
[21] Wilson, E., Lages, C., and Mah, R., "On-line gyro- based mass-property
identification for thruster-controlled spacecraft using recursive least squares."
4 5 th
IEEE InternationalMidwest Symposium on Circuits and Systems, Tulsa, Oklahoma,
Aug 2002.
[22] Wu, M., Lectures in "MEMS Physics and Design," UCLA Electrical Engineering
course.
Appendix A
Tuning-Fork Gyroscopes
A. 1
Background Physics
The angular rate sensors used in SPHERES are BEI GyroChip II vibrating quartz tuningfork gyroscopes. They operate by taking advantage of the Coriolis force, measuring its
effect with piezoelectric elements, and using that to determine rotation rate. The Coriolis
force acts on rotating objects that are translating in a direction orthogonal to their spin.
F, = 2mvxQl
(A.1)
Tuning-fork gyros like the GyroChip II drive one set of tines to wobble in a direction
radially outward from the spin axis. When the gyro spins about this axis, the Coriolis
force acts on the driven tines. Since these are always driven in opposite directions, the
effect of the Coriolis force is an oscillating torque applied to the rest of the tuning fork.
This causes the other set of tines to wobble in a direction orthogonal to both the drive
axis and the spin axis. Figure A. 1 summarizes this process.
Figure A.1 Tuning Fork Gyro
This out-of-plane motion can be picked up by optics or, in the case of the GyroChip II,
piezoelectric sensing. The sensed oscillation is then amplified and demodulated withreference to the drive signal to produce the output voltage, which is proportional to the
angular rate.
A.2
Modeling Rate Sensing
A simplified model of tuning fork tines is two proof masses suspended by springs and
dashpots, as shown in Figure A.2. The spring resistances in the x-axis and y-axis are kx
and ky respectively, and the damping coefficients are bx and by.
The simplified
equations of motions can be described as:
Ali = -bxi-kxx+M(Q2x+ 2x + 2y)+ Fx
AM/ = -byj -kyy +M((2y
-
(A.2)
202i - Ox)+ F,
Figure A.2 Gyro Sensing Model 1
The Q term is assumed to be 0 since we're considering small angular accelerations. The
2'
term is also assumed negligible with respect to the other larger terms. F, = 0 since
the drive force is along the x-axis only.
MA.= -byj -kyy-
2MDvk
(A.3)
As a damped second order system, this can be expressed in terms of natural frequencies
and damping ratios (which are assumed equal for both axes):
j + 2;ao,,) +Wy + 22k0= 0
(A.4)
The position of the proof masses on the x-axis is assumed to be +xo +a sin(odt), where
x0 is the rest position and cod is the drive frequency. The velocity of the masses along
the x-axis is:
x = ac cos(odlt)
(A.5)
Assuming that the drive frequency and natural frequency are equivalent, the equation of
motion along the y-axis becomes:
j + 2g.cwy + ouy + 22cawo cos(ot) = 0
The solution
to this
second
ordinary
differential
equation
(ODE)
is
sin(ao)t). The constants a, Wc, and ; are known or calibrated and y is sensed,
y = --
which allows solving for Q2.
deflection,
A.3
order
(A.6)
Q
=
a
If the gyroscope measures just the amplitude of the y-
y.
Application to SPHERES
In this simplified model, the drive frequency and sense frequency are exactly the same
but in practice, this is rarely the case. The natural frequency of a vibrating structure
gyroscope is a function of its geometry and mass distribution. Even if it were possible to
match both the drive and sense frequency to the natural frequency, the sensing elements
are often highly sensitive to temperature. Small changes in temperature can move the
resonant peaks of the drive and sense tines of the tuning fork far enough away to
drastically affect signal-to-noise ratio (SNR). These gyroscopes are typically tuned so
that they are driven at their natural frequency, but usually need force feedback to ensure
consistent amplitude oscillation along the drive axis.
Figure A.3 is an FFT of gyroscope data sampled at 50 Hz from a test on MIT's float
table.
n r1
..
ls6
061
a
'UMS
1.'r
ian
.L
(iSM
a
~
· -------------
-
---
· ·-
-i-----·-
· -- :--··;--·--
is
Figure A.3 12 Hz Gyro Noise
Originally, this 11.45 Hz noise was assumed to be vibration from the building
propagating through the float table. During more recent tests aboard the RGA where the
sampling rate was 100 Hz, a 38.33 Hz noise signal was observed instead (shown in
Figure A.4). After later sampling the gyro at 1 kHz and also performing an FFT of the
analog signal directly from the gyro output pins, it was determined that these noise spikes
were merely aliasing of noise at 338 Hz.
alB2
r f
Xi a'
UM
rj ,"
a
I
.e
s
r'
a
r
*r
"
*i
rt1
acvis
I
*
r·~=
"
i~~
r
.*
vr
s
h"
0
"% 9 ""lril+
`4
~·i
"
i
°
"*
r
tli
i
r·
*
*
r
?.
·
n
1
'
r
Figure A.4 38 Hz Gyro Noise
From contacting the manufacturer, it was determined that this noise at 338 Hz was a
result of a difference between the drive and sense frequencies of the gyro. As explained
above, this means that the input and output resonances are different, causing the
difference between the frequencies to pass through the demodulation as noise. Since this
noise is very frequency-specific, filtering it out has been trivial.
98
Appendix B
Fuel Slosh Analysis
B. 1
Propellant Properties
The propellant used to maneuver the SPHERES satellites is gaseous C02. When stored
in the onboard tank, it is in a mixed-phase state with liquid and gas at 860 psi (vapor
pressure of C02) and at room temperature.
The volume containing the propellant
remains constant, and the temperature is assumed constant as well (there is no external
heat source, only the ambient air around the tank). As gaseous C02 is expelled from the
tank, some of the liquid vaporizes to reestablish phase equilibrium.
Nominal fuel mass for a "full" tank is 172g.
The C02 exists in dual-phase until
approximately 28g remains in the tank. After this point, the tank pressure starts to drop
as more gas is expelled.
Since the supply pressure changes significantly during this
period, the regulator is unable to maintain a stable system pressure of 35 psi downstream
and the resultant force provided by the thrusters is unreliable. Because of this, navigation
experiments are usually concluded at or shortly after this point. This means that a high
percentage of SPHERES tests are conducted with liquid-phase C02 present in the fuel
tank, and are subject to the effects of fuel slosh.
B.2
Fuel Slosh on SPHERES
Fuel slosh has been shown to have a negligible effect on online mass property estimation
for SPHERES when compared to other noise sources such as thruster variability and gyro
ringing. Tests conducted by NASA AMES/Intellization produced these values for CM
with a full tank (maximum slosh) and a near empty tank (no slosh):
CM (empty)
=
[-0.000015665375 -0.000821387572 0.003081773236]
CM (full) = [-0.000054495248 -0.000810686405 0.000244119942 ]
Figure B.1 is an example of gyro data gathered during a 3 DOF SPHERES test on the
float table at MIT that exhibits some evidence of fuel slosh.
From the figure, the
amplitude of the disturbance is greater and the frequency smaller when the SPHERES
satellite is rotating slower. The angular rate of the satellite is directly proportional to the
applied rotational impulse: I r = Jo).
Hence, for this model we treat the applied rotational
impulse as the input and the resulting sinusoidal disturbance as the output.
100
I
Filtered Z-axis Gyro
..
0.2 - 01
Fi
. "ue
...
iO.u
....
.....
:3
-0.6 . . ......... ..... ...
: ....... .. ......
................
. . ........... .....
•
time [sec]
Figure B.I Observed Fuel Slosh in Gyro Data
B.3
Modeling Fuel Slosh
Fuel slosh is known to be a non-linear phenomenon, but for the purposes of
understanding it and recognizing its effect, a simplified linear model will suffice. Figure
B.2 depicts a simple representation of fuel slosh: a solid mass M connected to each side
of the tank by a spring and a dashpot, constrained to one DOF.
Figure B.2 Linearized Fuel Slosh Model
101
Since the mirror image sides do not add any mathematical significance, the model will be
further simplified to:
X
Figure B,3 Simplified Fuel Slosh Model
The linear motion of this mass will represent the sloshing of liquid C02, which in turn
affects the angular velocity of the satellite. This system acts as a damped harmonic
oscillator, which satisfies the second order differential equation:
b
k
2+x + - x = u(t)
MM
where u(t) is the driving force. The states in this system are X =
Sk
M
b .+I u(t)
M
102
(B.1)
]so
(B.2)
oi]
Y=[1
J
(B.3)
This is a state-space representation with A, B, C, and D defined as:
0
A=
1
k
b
B=M[
B= 0
[01
C=[1 0]
(B.4)
D=0
A value of 0.05kg was used for M, while k and b were manipulated so the output
disturbance resembled the plotted gyro data (k=0.3, b=0.001). For this analysis, just the
shape of the output is important, not the numerical scale. These values lead to a system
transfer function of:
H(s)=
2
s2
1
+0.02s+6
(B.5)
which is a complex pair of poles at -0.01 + 2.45i.
B.4
Modeling Input
Now with the model characterized, the next step is to see its response to a rotational
impulse input, modeled as a unit height rectangle impulse (Figure B.4).
103
Rotational Impulse Input
2 sec
4 sec
....
..................................
....... ..... ....
038
0.6
..
..
...
....
....
0.4
0.2
0
' -"
0
1000
1500
sec/100
2000
2500
3000
Figure B.4 Fuel Slosh Model Input
In order to do this, the fuel slosh transfer function must be discretized. Because of the
expected input, a zero-order hold method was used for discretization, along with a sample
time of 0.01.
H(z)=
B.5
4.999e-' z + 4.999e - 5
z2 -1.999z +0.9998
Results
Figure B.5 shows the responses to input impulses of width 2 sec and 4 sec.
104
CB.6)
Fuel Slosh Impulse Response
0
S00
1000
1500
sec/l00
2000
2500
3000
Figure B.5 Model Output
As expected, the amplitude of the response increases with the width of the input impulse.
However, unlike the sample fuel slosh data the output frequency remains unchanged,
which is to be expected from an LTI model (the mass M will always oscillate at the
natural frequency of the complex pair of poles). The discrepancy is most likely caused
by the true nonlinearity of the system.
According to the Department of Transportation guidelines for the safe packaging of
liquid C02, no more than 68% of the tank may be filled with liquid. The worst-case
scenario for fuel slosh affecting CoM and Inertia would be a full tank (172 g) with all the
liquid C02 at the bottom of the tank (Z-axis has highest moment arm potential for fuel).
Under these conditions, CoM values are highly unreliable (200%-1600% change). The
CoM change in the X and Y-axes is on the order of typical values while the change in the
105
Z-axis is over 5 times as large. Inertia is less affected, with the worst-case percentage
difference being 4.5% around the X or Y-axis.
Table B.1 Worst-case Effects of Fuel Slosh on Mass Properties
106
Appendix C
SPHERES Satellite Properties
C.1
CoM Offset
CoM offset in mm (from batch LS of mass property ID). or is about 0.08 in X and Y, 0.4
in Z.
0.5125
0.4549
-1.2502
-1.241
2.9484
0.8691
-1.2069
2.74191
0.63431
-1.6191
3.5379
0.7879
-1.3827
0.108
0.8115
-1.2044
-0.0985
0.5767
-1.6296
0.6975
0.7303
2.4472
0.4621
-1.3613
3.1517
-1.3824
-0.3932
0.4045
-1.3294
0.3113
107
C.2
Moment of Inertia
Inertia in kg-m^2 (from batch LS of mass property ID). a is about 0.002.
0.0275
0.024517
0.0242
0.0235
0.021529
0.0214
0.0194
0.0212
0.020309
-8.4E-05 0.000108 -8.2E-06
0.000014 0.000157 0.000149
0.00029 1.23E-05 -0.00022
C.3
0.023
0.0242
0.0242 0.021529
0.0212
0.0214
0.0001 -0.00022
-0.0003
0
0
0.0001
Thrust Strengths
U.1UbS
U.UU;3U
U.1Ubb
U.UUbU
U.11 b
U.UU44
U.1111
U.UUb3
0.1302 0.0023
0.1117
0.0018
0.1096
0.0031
0.1120
0.0023
0.1108
0.0032
0.1221
0.0063
0.1131
0.0090
0.1164
0.0097
0.1195
0.0098
0.1108
0.0080
0.1299 0.0042
0.1120
0.0070
0.1097
0.0057
0.1149
0.0074
0.1142
0.0049
0.1139 0.0047
0.1120
0.0025
0.1060
0.0097
0.1224
0.0026
0.1123
0.0075
0.1244 0.0056
0.1116
0.0025
0.1048
0.0070
0.1204
0.0014
0.1105
0.0050
0.1335 0.0040
0.1117 0.0043 0.1051
0.0050
0.1228 0.0075 0.1160 0.0060
0.1302 0.0027
0.1151
0.0053
0.1128
0.0055
0.1191
0.0071
0.1130
0.0047
0.1284 0.0059
0.1170
0.0078
0.1184
0.0083
0.1220
0.0089
0.1135
0.0066
0.1316 0.0051
0.1137
0.0064
0.1160
0.0059
0.1194
0.0074
0.1138
0.0047
0.1256 0.0056
0.1058
0.0022
0.1089
0.0093
0.1225
0.0023
0.1068
0.0073
0.1095 0.0050
0.1113
0.0026
0.1041
0.0072
0.1127
0.0021
0.1079
0.0063
U.133b
U.UU44
108
C.4
Thrust Directions
0.9982 0.9985
0.0014 0.0017
-0.0221 -0.0008
0.0471 0.0255
0.0249 -0.0248
0.0214 0.0438
0.0589 -0.0176
0.0330 0.0128
0.9961 0.9994
0.0044 0.0002
0.0324 0.0036
0.0506 0.0268
0.9988 0.9995
0.0009 0.0005
-0.0061 -0.0086
0.0299 0.0293
0.0356 -0.0018
0.0156 0.0095
0.0126 -0.0209 -0.0011 -0.0076 -0.9996 -0.9989 0.0391 -0.0292 0.0085 -0.0046
0.0278 0.0126 0.0168 0.0067 0.0003 0.0005 0.0040 0.0055 0.0064 0.0052
0.9994 0.9995 0.0244 -0.0278 0.0162 0.0374 -0.9989 -0.9992 0.0598 0.0075
0.0005 0.0002 0.0178 0.0087 0.0096 0.0059 0.0003 0.0004 0.0085 0.0090
0.0104 -0.0149 0.9994 0.9995 0.0137 -0.0166 0.0219 0.0239 -0.9981 -0.9999
0.0106 0.0119 0.0009 0.0002 0.0153 00227 00102 00117 00005 00001
0.9996 0.9986
0.0004 0.0009
-0.0091 -0.0247
0.0164 0.0152
0.0171 -0.0405
0.0166 0.0187
0.0100 -0.0393 0.0084
0.0252 0.0119 0.0243
0.9995 0.9988 -0.0087
0.0006 0.0003 0.0320
0.0009 0.0125 0.9992
0.0189 0.0228 0.0009
0.9990
0.0010
-0.0331
0.0227
0.0196
0.0123
0.0107 -0.0224 -0.9986 -0.9984 0.0418 -0.0194 0.0041 -0.0018
0.0075 0.0101 0.0010 0.0014 0.0107 0.0124 0.0069 0.0070
0.0337 -0.0121 -0.0077 -0.0095 -0.9990 -0.9997 0.0603 -0.0297
0.0224 0.0224 0.0249 0.0365 0.0005 0.0002 0.0070 0.0218
0.9991 0.9994 0.0445 -0.0367 0.0088 0.0044 -0.9981 -0.9994
0.0010 0.0005 0.0179 0.0225 00060 0_0085 0_0004 00009
-0.0166 -0.9988 -0.9983 0.0078 -0.0310
0.0204 0.0010 0.0012 0.0411 0.0283
-0.0835 0.0134 0.0162 -0.9990 -0.9990
0.0309 0.0116 0.0124 0.0009 0.0011
0.9957 0.0067 -0.0353 -0.0158 -0.0003
0.0033 0.0463 0.0428 0.0086 0.0181
0.9983 0.0264 -0.0455 -0.0128 -0.0136 -0.9996 -0.9988
0.0005 0.0209 0.0184 0.0272 0.0176 0.0005 0.0009
-0.0351 0.9992 0.9985 0.0264 -0.0660 0.0020 0.0149
0.0132 0.0012 0.0008 0.0156 0.0114 0.0093 0.0102
-0.0451 0.0085 -0.0212 0.9991 0.9975 0.0092 -0.0408
0.0032 0.0244 0.0108 0.0009 0.0005 0.0264 0.0224
-0.0258 0.0083
0.0132 0.0259
0.0426 -0.0462
0.0743 0.0381
-0.9961 -0.9979
0.0092 0.0024
0.0023 -0.0448 -0.0279 0.0231
0.0273 0.0233 0.0109 0.0090
-0.9995 -0.9986 0.0159 -0.0335
0.0005 0.0009 0.0236 0.0281
-0.0114 0.0079 -0.9992 -0.9988
0.0098 0.0122 0.0007 0.0007
0.9988 0.9987 0.0229 -0.0669 -0.0219 -0.0036 -0.9992 -0.9991 0.0090 -0.0450 -0.0108 0.0136
0.0005 0.0003 0.0120 0.0051 0.0223 0.0136 0.0003 0.0007 0.0385 0.0281 0.0070 0.0103
-0.0108 -0.0276 0.9993 0.9975 0.0272 -0.0662 0.0101 0.0110 -0.9989 -0.9985 0.0352 -0.0334
0.0100 0.0075 0.0003 0.0006 0.0086 0.0047 0.0258 0.0130 0.0010 0.0013 0.0265 0.0189
0.0452 -0.0406 -0.0202 -0.0225 0.9991 0.9977 0.0003 -0.0255 -0.0250 -0.0089 -0.9990 -0.9991
0.0136 0.0064 0.0214 0.0086 0.0007 0.0003 0.0303 0.0296 0.0087 0.0089 0.0011 0.0006
109
Appendix D
MATLAB Code
D. 1
Mass Property ID code (massidkcl.m)
This is the high-level function call for the mass property ID algorithm.
function [cm l, Il] = massidkcl(start,finish);
load('kc test.mat);
disp('Initializing...');
% SYSTEM PARAMETERS- Initialize system parametersdepending on which SPHERE
was used.
% ------------------------------------------------------------------------------
ifSph
Fnom = [0.13364, 0.13023, 0.12209, 0.12994, 0.11388, 0.12436, 0.13351, 0.13018,
0.1284, 0.13159, 0.12563, 0.10954];
D = [ 0.99824, 0.99853, 0.05895, -0.00176, 0.01074, -0.02244, -0.99856, -0.99845,
0.04179, -0.01939, 0.00412, -0.00184;...
-0.02206, -0.00082, 0.99608, 0.99944, 0.03370, -0.01207, -0.00774, -0.00946, 0.99902, -0.99971, 0.06026, -0.02973;...
110
0.02491, -0.02481, 0.03236, 0.00365, 0.99913, 0.99941, 0.04447, -0.03675,
0.00876, 0.00438, -0.99814, -0.99936];
Lnom = 0.01 .* [-5.16, -5.16, 9.65, -9.65, 0, 0, 5.16, 5.16, 9.65, -9.65, 0,
0;...
0, 0, -5.16, -5.16, 9.65, -9.65, 0, 0, 5.16, 5.16, 9.65, -9.65;...
9.65, -9.65, 0, 0, -5.16, -5.16, 9.65, -9.65, 0, 0, 5.16, 5.16];%m
9%I_nom = 0. 0335;% kg.mA2, for 2-D table experiments
Inom = [0.0229, 0.0000965, -0.000293; 0.0000965, 0.0242, -0.0000311; -0.000293, 0.0000311, 0.0214];
cm_nom = [0;0;0];
bias = [2055; 2079; 2051];
elseifSph == 2
Fnom = [0.11808, 0.11868, 0.11885, 0.11984, 0.11590, 0.11830, 0.11843, 0.12168,
0.11953, 0.11874, 0.11630, 0.11917]; %CalibratedPressureGauge
D = [0.99880, 0.99951, 0.01260, -0.02093, -0.00105, -0.00764, -0.99962, -0.99890,
0.03911, -0.02922, 0.00846, -0.00464;...
-0.00614, -0.00857, 0.99944, 0.99953, 0.02440, -0.02778, 0.01615, 0.03738, 0.99894, -0.99921, 0.05980, 0.00753;...
0.03561, -0.00181, 0.01040, -0.01492, 0.99941, 0.99953, 0.01372,-0.01662,
0.02191, 0.02388, -0.99812, -0.99991];
Lnom - 0.01 .*[-5.16, -5.16, 9.65, -9.65, 0, 0, 5.16, 5.16, 9.65, -9.65, 0,
0;...
0, 0, -5.16, -5.16, 9.65, -9.65, 0, 0, 5.16, 5.16, 9.65, -9.65;...
9.65, -9.65, 0, 0, -5.16, -5.16, 9.65, -9.65, 0, 0, 5.16, 5.16];% m
%I_nom = 0. 0335;% kg.mA2, for table experiments
I nom = [0.0229, 0.0000965, -0.000293; 0.0000965, 0.0242, -0.0000311; -0.000293, 0.0000311, 0.0214];
cm_nom = [0;0;0];
bias = [2077; 2070; 2058];
elseifSph == 3
Fnom = [0.10648965825739, 0.10958074078082, 0.11639991410096,
0.10972026118392, 0.10595235083091, 0.10476246795815, 0.10506642798593,
0.11278450399743, 0.11835632534687, 0.11600639008945, 0.10888722593497,
0.10413145765793];
D = [0.99824, 0.99833, 0.00140, -0.00010, -0.00308, -0.00958, -0.99861, -0.99757,
0.02145, -0.04382, -0.00456, 0.00586;...
0.01498, -0.01245, 0.99974, 0.99962, 0.04179, -0.02819, 0.02828, 0.02149, 0.99934, -0.99886, 0.04434, -0.02649;...
0.03182, -0.04666, -0.00102, 0.00158, 0.99907, 0.99953, 0.04307, -0.05952,0.01566, 0.01221, -0.99897, -0.99946];
111
Lnom = 0.01 .*[-5.16, -5.16, 9.65, -9.65, 0, 0, 5.16, 5.16, 9.65, -9.65, 0,
0;...
0, 0,-5.16, -5.16, 9.65, -9.65, 0, 0,5.16, 5.16, 9.65, -9.65;...
9.65, -9.65, 0, 0, -5.16, -5.16, 9.65, -9.65, 0, 0, 5.16, 5.16];% m
% Inom = 0.0335; % kg.mA2, for table experiments
I nom = [0.0229, 0.0000965, -0.000293; 0.0000965, 0.0242, -0.0000311; -0.000293, 0.0000311, 0.0214];
cm nom = [O;0;0];
bias = [2055; 2061; 2053];
end;
% % TESTPARAMETERS
% % ----------------------------------------
-----------------------------
rate raw = gyro;
% Convert rate
%conversion = 1713.14;% m V/(rad/s)
conversion = 1412. 79; % counts/(rad/s)
for n = 1:length(rate_raw)
rate(:, n) = (rate raw(:,n) - bias) ./ conversion;% rad/s
end;
% Interpolate missing telemetryfrom droppedcomm packets
disp('Interpolatemissing data.... );
[time, rate, accel, thrusters] = int(time, rate, accel,thrusters);
% Findbeginnings/endsoffirings
indtemp =find(diff(thrusters));
window = 1;
if mod(length(indtemp),2) == 1
indtemp = [indtemp' length(thrusters)];
end;
% T is the binary version of "thrusters"
T= de2bi(thrusters,12);
% Filter interpolatedgyro signal
disp('Filtergyro readings....)
rate = gyrofiltvar(rate,time, 50, 2);
% updateind is a vector with the indices of all the updates (i.e. which time points actually
have an associatedw dot estimate)
updateind=find(thrusters);
L = Lnom - cm nom *ones(1,12);
112
disp('EstimateAngular Acceleration....);
wdotkalman
%%%%%%%%%%%%%%%%%%%%%%
% Batch Least Squares %
0%%%%%%%%%%°1%% %%%%%%%
disp('BatchLS.... );
batchLS;
% Batch Results:
cm] = cm(:,end)
HI = I_mat{end)
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Recursive Least Squares %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('RecursiveLS....');
recursiveLS;
%%%%%%%%%%%%%
% Plots %
%%%%%%%%%%%%%
%%OMITTED%%
D.2
Interpolation Scheme (int.m)
This file is the first called by massid_kc.m. It fills in the missing data gaps with a linear
interpolation.
function [inttime, intgyro, intaccel,intthrusters]= int(time, gyro, accel, thrusters);
b =find(diff(time) == 0);
dt = floor(mean(diff(time(1:50))));
forj = J:length(b)
index = length(b) -j + 1;
time(b(index)+1) = [];
gyro(,b(index)+1) = [];
accel(.',b(index)+1) = [];
thrusters(b(index)+1) = [];
113
end;
x = time;
a =find(diff(time) - I *dt);
for i = 1:3
y = gyro(i,:);
xi = [];
forj = 1:length(a)
xi = [xi time(a(J))+1*dt :dt: time(a)(j+l)-l];
end;
yi = interpl(x, y, xi, 'linear");
A = [[xxi]' [yyi'];
B = sortrows(A,1);
intgyro(i,.) = B(:, 2)';
end;
inttime = B(:,1)';
y = thrusters;
yi = interpl(x,y, xi, 'nearest);
C = [[xxi]'[y yi'];
D = sortrows(C,1);
intthrusters = D(-,2);
intaccel = accel;
D.3
Gyro Filter (gyrofiltvar.m)
This file is called by massid_kcl.m to filter the gyros after missing data points are
interpolated.
functionfiltered_data= gyrofiltvar(gyro,time, freq, ord)
freq = freq*2*pi;
114
s =tf('s');
filter cont = freqAord/(s+freq)^ord;
dt = (time(2)-time(1))/l000;
filter_disc = c2d(ftltercont,dt ,'tustin);
filter struct = get(filter disc);
b =filterstruct.num{1};
a =filter struct.den{1};
filtered_data(],.) =filtfilt(b, a, gyro(1,:));
filtered_data(2,.) = fltfilt(b, a, gyro(2,:));
filtered data(3,:) = filtfilt(b, a, gyro(3, ));
D.4
Angular Acceleration Estimator (wdotkalman.m)
This script is called after interpolating and filtering to start the Kalman Filter for angular
acceleration estimation.
dt = floor(mean(diff(time(1:50))))/1000;
% Kalman FilterInitializations
phi = 1;
Fk = (Fnom-sum(T(updateind(1),:))*0.0106). * T(updateind(1),:);
x minus{1} = inv(I nom) *cross(L,D)*Fk';
P inv_
minus{1} = 2*eye(3,3);
Q = zeros(3,3);
H{1} = diag(zeros(1,3));
R = le-7 * [9.5661 0; 0 2.7365 0; 0 0 2.3044];
bias] = rate(:,updateind(1) - 1);
ifdt < 5/1000
updateind(1:10)= [];
%bias] = mean(rate(:,1:updateind(1)-10),2);
end;
% StartKalman Filter
j=1 *dt,
k=l;
while k< length(updateind)
115
H{k) = diag(fjjj])*1;
G =find(T(updateind(k),:));
P inv{k} = Pinvminusfk} + H{k}'*inv(R)*H{k};
P{k} = ivPinv(Pinv{k);
{k}k = Pk} *H{k} '*inv(R) ;
xfk} = x minusfk} + Kk} *(rate(:,updateind(k))-biasl - H{k} *x minus k});
xminus k+1} = phi*xfk};
P_minusk+1} = phi*P{k} *phi'+ Q;
P invminusfk+1} = inv(P minus{k+1});
if -(thrusters(updateind(k+1)) == thrusters(updateind(k)))
Fk = (Fnom-sum(T(updateind(k+1),)) *0.0106) . * T(updateind(k+ ),),
j = 0;
xminus lk+ 1) = inv(I nom) *cross(L,D)*Fk';
P inv minus k+1} = 2*eye(3,3);
bias] = rate(., updateind(k+1) - 1);
ifdt < 5/1000
updateind(k+ :k+11) = []
end;
end;
j=j+]*dt;
k=k+l;
end;
% Extract angularaccelerationand associatedstandarddeviation
for i = 1:length(x)
acc(.,i) = x{i};
acc xvar(i) = P{i}(1,1) ;
acc_yvar(i) = P{i}(2,2);
acczvar(i) = P{i}(3,3);
acc_Sigmax(i) = sqrt(acc xvar(i));
accSigmay(i) = sqrt(accyvar(i));
acc Sigmaz(i) = sqrt(acc zvar(i));
R acc{i} = diag(diag(P{i}));
end;
anew = zeros(3, length(time));
for i = 1:length(x)
anew (.,updateind(i)) = x{i};
end;
116
% Clear local variablessince this is a script, not afunction
clearH G x accxvar accyvar acczvar acc_Sigmay acc_Sigmaz P inyvminus Pinv P
D.5
Batch Mass Property ID (batchLS.m)
This script called after angular acceleration has been calculated.
% estI holds information about which axes have been excited. This allows simplification
ofLS equations ifnecessaryfor well-conditionedQ
cc=];
estI = zeros(1,3);
for bb=l:ength(updateind)
if thrusters(updateind(bb))> 0
if (T(updateind(bb),5) T(updateind(bb),6) T(updateind(bb),l) I
T(updateind(bb),12))
estI(1,1) = 1;
end;
if(T(updateind(bb),1) T(updateind(bb),2) T(updateind(bb),7)
T(updateind(bb),8))
estI(1,2) = 1;
end;
if (T(updateind(bb),3) T(updateind(bb),4) T(updateind(bb),9)
T(updateind(bb),10))
estI(1, 3) = 1;
end;
Fk = (Fnom-sum(T(updateind(bb),:))*0.0106) .* T(updateind(bb),:);
Ck = D*Fk';
Ak = cross(L,D) *Fk';
estldec = bi2de(estI)
All switch statements are uncommented if we are using estI to simplify equations.
switch estIdec
case bi2de([1 0 0])
Al(cc:cc+2,.) =inv(I nom) *[ 0 -Ck(3);...
Ck(3) 0 ;...
-Ck(2) Ck(])];
bl(cc:cc+2,.) = anew(:,updateind(bb))- inv(I nom) *cross(Lnom,D)*Fk';
117
0/o
0/o
A2(cc:cc+2,.) = [Ak(1) Ak(2) Ak(3) ;...
o
Ak(1) 0
o
o Ak(1)],
;...
b2(cc:cc+2,:) = anew(:,updateind(bb));
case bi2de([0 10])
Al (cc:cc+2,.) = inv(I nom) *[-Ck(3) Ck(2);...
o -Ck(]);...
Ck (1)0 ];
0/o
bl(cc:cc+2,:) = anew(:, updateind(bb)) - inv(I nonm) *cross(Lnom,D)*Fk';
A2(cc:cc+2,:) = [0 Ak(2) 0
Ak(2) Ak(1) Ak(3) ;...
O 0 Ak(2)];
0/o
0/6
b2(cc:cc+2,:) = anew(:,updateind(bb));
case bi2de([O 0 1])
A (cc:cc+2,.) = inv(I nom) *[ 0
Ck(2);...
Ck(3) -Ck(1);...
0/6
-Ck(2) 0 ],
bl(cc:cc+2,:) = anew (:, updateind(bb)) - inv(I non) *cross(Lnom,D)*Fk';
A2(cc:cc+2,:) = [0 Ak(3) 0
O
0
Ak(3) ;...
0/o
Ak(3) Ak(1) Ak(2)];
9~o
b2(cc:cc+2,:) = anew(:, updateind(bb));
9/o
case bi2de([1 1 0])
Al(cc:cc+2,) = inv(J nom)*[ 0
-Ck(3) Ck(2);...
Ck(3) 0 -Ck(1);...
-Ck(2) Ck(1) 0 ];
0/o
0/o
0/o
bl (cc:cc+2,:) = anew(:,updateind(bb)) - inv(I nom) *cross(Lnom,D)*Fk;
A2(cc:cc+2,:) = [Ak(1) 0 Ak(2) Ak(3) 0 ;...
o
Ak(2) Ak(1) 0
o
o0 0
Ak(3) ;...
Ak(l) Ak(2)];
b2(cc:cc+2,.) = anew(:,updateind(bb));
case bi2de([1 0 1])
Al(cc:cc+2,:) = inv(I nom)*[ 0 -Ck(3) Ck(2);...
Ck(3) 0 -Ck(l);...
-Ck(2) Ck(1) 0 1;
00
0/0
0/o
0/6
b](cc:cc+2,:) = anew(:, updateind(bb)) - inv(Inom) *cross(Lnom,D)*Fk';
A2(cc:cc+ 2,:) = [Ak(1) 0 Ak(2) Ak(3) 0 ;...
0 0 Ak(1) 0 Ak(3) ;...
0 Ak(3) 0 Ak(1) Ak(2)];
b2(cc:cc+2,.) = anew(:,updateind(bb));
case bi2de([0 1 1rJ7
-)
00
Al(cc:cc+2,:)
inv(I nom) *[ 0
-Ck(3) Ck(2);...
Ck(3) 0 -Ck(1);...
0/o
0/o
-Ck(2) Ck(1) 0 ];
bl(cc:cc+2,.) anew(:, updateind(bb)) - inv(Inom) *cross(Lnom,D)*Fk';
A2(cc:cc+2,:) S[0 0 Ak(2) Ak(3) O ;...
118
Ak(2) 0 Ak(1) 0 Ak(3) ;...
0 Ak(3) 0 Ak(1) Ak(2)];
%
%
b2(cc:cc+2,:) = anew(:,updateind(bb));
case bi2de([l 1])% FULL ORDER CASE
Al(cc:cc+2,:) = inv(I nom) *[ 0 -Ck(3) Ck(2);...
%
%
Ck(3) 0 -Ck(l);...
-Ck(2) Ck(J) 0 ];
bl(cc:cc+2,:) = anew(:,updateind(bb)) - inv(I nom) *cross(Lnom,D)*Fk';
A2(cc:cc+2,.) = [Ak(1) 0 0 Ak(2) Ak(3) 0 ;...
o Ak(2) 0 Ak(1) 0 Ak(3) ;...
o
%o
%
%
%
0 Ak(3) 0 Ak(1) Ak(2)];
b2(cc:cc+2,:) = anew(:,updateind(bb));
case bi2de([0 0 0])
disp('Error)
keyboard;
end
cc=cc+3;
% ID Estimatesindexed with thrusteron time (*NO* gaps between thrusterfirings)
ifbb = length(updateind)% Only taking batch after all datais included
cm(:,(cc-J)/3) = inv(AI'*A1) *Al'*bl;
I inv(.;,(cc-1)/3) = inv(A2 '*A22) *A2'*b2;
I_inv_mat (cc-1) 3} = [linv(1,(cc-1)/3) I inv(4, (cc-1)/3) I_inv(5, (cc-1)/3);...
I_inv(4, (cc-1)/3) I inv(2, (cc-1)/3) l_inv(6,(cc-1)/3);...
I inv(5, (cc-1)/3) I inv(6,(cc-1)/3) I_inv(3, (cc-1)/3)];
Imat{(cc-1)/3}
end,
end;
end;
D.6
=
inv(Iinvmat{(cc-1)/3});
Recursive Mass Property ID (recursiveLS.m)
This is the last script call executed from massidkc.m and it is where the recursive
equations are exercised.
119
% Batch overfirst twofirings
cc 1; % used to index rows ofA and b
count = 1; % how far we've gotten through our w dot updates
stop_batch = indtemp(3)+1; % thruster values change 4 times over two firings!
L = Lnom - cm nom *ones(1,12);
estI = zeros(1,3);
Rbatch = [];
while time(updateind(count)) < time(stop batch -floor(window/2)-1)
if (T(updateind(count),5) T(updateind(count),6) T(updateind(count),11)
T(updateind(count),12))
estI(1, 1) = 1;
end;
if (T(updateind(count),1) T(updateind(count),2) T(updateind(count),7)
T(updateind(count),8))
estI(1,2) = 1;
end;
if (T(updateind(count),3) I T(updateind(count),4) T(updateind(count),9)
T(updateind(count),10))
estI(1,3) = 1;
end;
Fk = (Fnom-sum(T(updateind(count),.))*0. 0106) .* T(updateind(count),.); % This
takes into account thrust drop with multiple thrustersopen
Ckrls = D*Fk';
Akrls = cross(L,D)*Fk';
estIdec = bi2de(estl);
switch estIdec
case bi2de([1 0 0])
Alrls(cc:cc+2,:) = inv(I nom) *[ 0
-Ckrls(3) ;...
Ckrls(3) 0
-Ckrls(2) Ckrls(1)];
blrls(cc:cc+2,:)= anew (:,updateind(count)) - inv(I_nom) *cross(Lnom,D)*Fk';
A2rls(cc:cc+2,:) =[Akrls(1) Akrls(2) Akrls(3) ;...
0
Akrls(1) 0
;...
0
0
Akrls(1)];
b2rls(cc:cc+2,:) = anew (, updateind(count));
case bi2de([O 1 0])
A lrls(cc:cc+2,.) inv(I nom) *[-Ckrls(3)Ckrls(2);...
o
-Ckrls(1);...
Ckrls(1) 0
];
blrls(cc:cc+2,.:) anew(:, updateind(count)) - inv(Inom) *cross(Lnom,D)*Fk';
Akrls(2) 0
A2rls(cc:cc+2,:) [0
Akrls(2) Akrls(1) Akrls(3) ;...
120
0
0
Akrls(2)];
b2rls(cc:cc+2,:) = anew(:,updateind(count));
case bi2de([0 Orl)
Alrls(cc:cc+2,.) = inv(I nom) *[ 0
Ckrls(2);...
Ckrls(3) -Ckrls(l);...
-Ckrls(2) 0
];
blrls(cc:cc+2,.) = anew (., updateind(count))- inv(I nom) *cross(Lnom,D)*Fk';
A2rls(cc:cc+2,.) = [0
Akrls(3) 0
0
0
Akrls(3) ;...
Akrls(3) Akrls(1) Akrls(2)];
b2rls(cc:cc+2,. - anew(-, updateind(count));
case bi2de([1 1 0])
Alrls(cc:cc+2,.) - inv(I nom) *[ 0
-Ckrls(3) Ckrls(2);...
Ckrls(3) 0
-Ckrls(]);...
-Ckrls(2) Ckrls(i) 0
];
blrls(cc:cc+
-2, ) = anew (:, updateind(count))- inv(I nom) *cross(Lnom,D)*Fk',
A2rls(cc:cc-+2,.) [Akrls(1) 0
Akrls(2) Akrls(3) 0
;...
0
Akrls(2) Akrls(1) 0
Akrls(3) ;...
O
0
0
Akrlsrl) Akrls(2)];
b2rls(cc:cc+2,:) = anew(., updateind(count));
case bi2de([1 0 1])
A lrls(cc:cc-+2,.) = inv(I nor) *[ 0
-Ckrls(3) Ckrls(2);...
Ckrls(3) 0
-Ckrls(1);...
-Ckrls(2) Ckrls() 0 ];
blrls(cc:cc+2,.:) anew(., updateind(count)) - inv(I nom) *cross(Lnonm,D)*Fk';
A2rls(cc:cc-+2,) = [Akrls(1) 0
Akrls(2) Akrls(3) 0
0
0
Akrls(1) 0
Akrls(3) ;...
o
Akrls(3) 0
Akrls(1) Akrls(2)];
b2rls(cc:cc+2,:) = anew(:,updateind(count));
case bi2de([0
A lrls(cc:cc-+2,.') = inv(I nom) *[0
-Ckrls(3) Ckrls(2);...
Ckrls(3) 0
-Ckrls(J);...
-Ckrls(2) Ckrls(l) 0 ];
blrls(cc:cc- 2,:) = anew (.,updateind(count)) - inv(I nom) *cross(Lnom,D) *Fk';
A2rls(cc:cc-+2,.) = [0
0
Akrls(2) Akrls(3) 0
Akrls(2) 0
Akrls(l) 0
Akrls(3) ;...
0
Akrls(3) 0
Akrls(1) Akrls(2)];
b2rls(cc:cc+2,:) = anew(:, updateind(count));
case bi2de([] 1 1])
Alrls(cc:cc+2,.) =inv(I nom) *[ 0
-Ckrls(3) Ckrls(2);...
Ckrls(3) 0
-Ckrls(1);...
-Ckrls(2) Ckrls(1) 0 ];
blrls(cc:cc+2,:) anew (:,updateind(count))- inv( non) *cross(Lnom,D)*Fk';
A2rls(cc:cc+2,:) S[Akrls(1) 0 0 Akrls(2) Akrls(3)
0
0
Akrls(2) 0
Akrls(1) 0
Akrls(3) ;...
0
0
Akrls(3) 0
Akrls(1) Akrls(2)];
121
b2rls(cc:cc+2,:)= anew(:,updateind(count));
%
case bi2de([O 0 0])
%
%
disp('Error)
keyboard;
end;
%
cc=cc+3;
Rbatch = diag([diag(R_batch);diag(Racc{count})]);
count = count + 1,
end;
temp = (count); % Remember how far we got through w dot updates...we want to start
our recursive where our batch left off
% Initialstate/covarianceestimates:
cm rls(:,l) = inv(A rls'*A rls) *Alrls'*blrls;
Qcm inv{1} = Airls' * (inv(R batch)) * A rls;
I inv rls(:,1)= inv(A2rls'*A2rls) *A2rls'*b2rls;
QI inv{1} = (A2rls' * (inv(R batch)) * A2rls);
clear A Irls A2rls blrls b2rls;
k=2; % k=l is our batch estimate, recursive starts with k=2
for bb=temp:length(updateind)-1
if (T(updateind(bb),5) T(updateind(bb),6) T(updateind(bb),11)
T(updateind(bb),12))
estI(1, 1) = 1;
end;
if (T(updateind(bb),1) T(updateind(bb),2) T(updateind(bb), 7) T(updateind(bb),8))
estI(1,2) = 1;
end;
if(T(updateind(bb),3) T(updateind(bb),4) T(updateind(bb),9) I T(updateind(bb),10))
estI(1,3) = 1;
end;
Fk = (Fnom-sum(T(updateind(bb),:))*0.010 6) . * T(updateind(bb),.);
Ckrls = D*Fk';
Akrls = cross(L,D)*Fk',;
estldec = bi2de(estl);
%
switch estldec
%
case bi2de([i 0 0])
%
A Irls = inv(I nom) *[ 0
%
Ckrls(3) 0
-Ckrls(3) ;.
;...
-Ckrls(2) Ckrls(i)];
122
..
%
%
%
%
%
%
bbrls = anew (:,updateind(bb))- inv(I nom) *cross(Lnom,D)*Fk';
A2rls = [Akrls(1) Akrls(2) Akrls(3) ;...
0
0
Akrls(1) 0
0
Akrls(1)];
b2rls = anew(:,updateind(bb));
case bi2de([0 1 0])
%
Alrls = inv(I_nom) *[-Ckrls(3)Ckrls(2);...
%
%
%
0
-Ckrls(1);...
Ckrls(1) 0 ];
birls = anew(:,updateind(bb))- inv(I nom) *cross(Lnom,D)*Fk';
%
%
A2rls = [0
Akrls(2) 0
Akrls(2) Akrls(1) Akrls(3) ;...
%
%
%
%
%
0
0
Akrls(2)];
b2rls = anew(.,updateind(bb));
case bi2de([O 0 1])
Alrls = inv(I nom) *[ 0
Ckrls(2);...
Ckrls(3) -Ckrls(1);...
%
-Ckrls(2) 0
%
%
%
%
%
%
birls = anew(:,updateind(bb))- inv(Inom) *cross(Lnom,D)*Fk';
A2rls = [0
Akrls(3) 0
0
0
Akrls(3) ;...
Akrls(3) Akrls(1) Akrls(2)];
b2rls = anew(:,updateind(bb));
case bi2de([1 1 0])
%
A Irls - inv(I nom) *[ 0
%
Ckrls(3) 0
%
%
%
];
-Ckrls(3) Ckrls(2);...
-Ckrls(1);...
-Ckrls(2) Ckrls(1) 0
%
0
Akrls(2) Akrls(1) 0
%
0
0
%
];
blrls = anew(:,updateind(bb))- inv(I nom) *cross(Lnom,D)*Fk';
A2rls = [Akrls(1) 0
Akrls(2) Akrls(3) 0
0
Akrls(3) ;...
Akrls(1) Akrls(2)];
b2rls = anew(:,updateind(bb));
%
%
case bi2de([1 0 1])
A rls inv(Inon) *[ 0
%
Ckrls(3) 0
%
-Ckrls(3) Ckrls(2);...
-Ckrls(1);...
-Ckrls(2) Ckrls(1) 0
];
%
bIrls = anew(:,updateind(bb))- inv(I nom) *cross(Lnom,D)*Fk';
%
A2rls - [Akrls(1) 0
%
%
%
%
%
%
%
%
0
0
Alkrls(2) Akrls(3) 0
Akrls(1) 0
Alkrls(3) ;...
0
Akrls(3) 0
Akrls(1) Akrls(2)];
b2rls = anew(.',updateind(bb));
case bi2de([0 1 ])
Alrls = inv(I nom)*[ 0
-Ckrls(3) Ckrls(2);...
Ckrls(3) 0
-Ckrls(1);...
-Ckrls(2) Ckrls(1) 0 ];
birls = anew(:,updateind(bb))- inv(I nom) *cross(Lnom,D)*Fk';
123
A2rls = [0
%
%
%
%
Akrls(2) Akrls(3) 0
0
Akrls(2) 0
%
Akrls(1) 0
Akrls(3) ;...
Akrls(1) Akrls(2)];
Akr/s(3) 0
0
anew(:,updateind(bb));
b2rls =
case bi2de([1 1 1])
-Ckrls(3) Ckrls(2);...
Alrls = inv(I nom) *[ 0
Ckrls(3) 0
-Ckrls(1);...
-Ckrls(2) Ckrls(]) 0 ];
blrls = anew(:,updateind(bb)) - inv(I nom) *cross(Lnom,D)*Fk';
0
A2rls = [Akrls(1) 0 0 Akrls(2) Akrls(3)
0
Akrls(2) 0
o
0
Akrls(3) ;...
Akrls(1) 0
Akrls(3) 0
Akrls(1) Akrls(2)];
b2rls = anew(:,updateind(bb));
%
case bi2de([O 0 0])
%
disp('Error')
%
%
keyboard;
end;
% State/Covarianceupdate:
Qcminvk} = Qcm inv{k-1} + Alrls' * (inv(R acc{temp-2+k})) *Alrls;
cm rls(:,k) = cm rls(:,k-1) + inv(Qcm inv{k}) * Alrls' * (inv(R acc{temp-2+k})) *
(birls - Alrls * cm rls(:,k-1));
QI inv'k}= QI inv{k-1} + A2rls' * (inv(R acc{temp-2+k})) * A2rls;
Iinvrls(.,k) = I inv r/s(:,k-1) + inv(nv(QI invk) * A2rls' * (inv(R acc{temp-2+k})) *
(b2rls - A2rls * Iinmvrls(:,k-1));
k=k+l;
end;
% %oPutinverse inertiaestimate into matrixform and invert it.
for i = i:size(I invrls,2)
I inv mat rlsi} = [I inv rls(1,i)I inv rls(4,i) I inv rls(5,i);...
I_ inv rls(4,i) I inv rls(2,i) I inv rls(6,i);...
I inv rls(5,i) I inv rls(6,i) I inv rls(3,i)];
I_mat rls{i} = inv(I inv mat rs{i});
end;
% Get varaince/stdev of lxx, Iyy, Izz over time
forj = i:length(QIinv)
QIxx(j) = 1/(QI inv/j}(1,1));
Sigmaxx() = sqrt(Qlxx());
Ixx rlso) = I mat rls{i}(1,1);
Qlyyo) = 1(QI invo{}(2,2));
Sigmayyo) = sqrt(QIyyo));
Iyy rls(j) = I mat rls{} (2,2);
124
QIzzo) 1/(QI invoj}(3,3));
Sigmazz() = sqrt(QIzz());
Izz ris) = Imat rls{j(3,3);
end;
125
Appendix E
C Code
E.1
Mass Property ID code (massid/gsp.c)
This is the highest layer of the online mass ID code.
/* Templatefile ofgsp.c
*/
#include <std.h>
#include <clk.h>
#include <sys.h>
#include <stdlib.h>
#include <string.h>
//#include "gsp.h"
#include "gsp internal.h"
#include "prop.h"
#include "pads.h"
#include "comm.h"
#include "commands.h"
#include "gsp task.h"
#include "spheresconstants.h"
#include "system.h"
#include "control.h"
126
#include "pads correct.h"
#include "comm datacomm.h"
#include "housekeeping.h"
#include "control internal.h"
#include "est ang acc.h"
#include "mass id.h"
#include "gsutil standard maneuvers.h"
#include "mix simple.h"
#include "mix simple_correct.h"
#include "math matrix.h"
#include "math LTIFilter.h"
#include "spheres types. h"
#include "utilserial_printfh"
//1kHz sampling declarations
#define SHORTS PERSAMPLE
#define FLOATS PER SAMPLE
#define SAMPLES PER SECOND
#define TESTDURATION
(8)
(4)
(1000)
(5)
staticfloat *sample_buffer=NULL;
staticfloat *current buffer = NULL;
staticfloat *BufferPos= NULL;
//static unsigned int sample_bufsize =sizeof(unsignedshort) * SHORTS PER SAMPLE *
SAMPLES PER SECOND * TEST DURATION;
static unsigned int sample_bufsize =sizeof(float) * FLOATS PER SAMPLE *
SAMPLES PER SECOND * TEST DURATION,
static defaultrfm
fpayloadpkt;
//omega dot KF declarations
staticfloat startgyro[3]= {0.0•];
staticfloat current_gyro[3] = 0.0f};
staticfloat gsp omega dot[3], gsp omega dot minus[3], lastomega dot[3],
gsp R acc[3], last Racc[3];
int k, p;
unsignedint test started = 1;
unsigned shortpast thrust, new thrust;
int down count = -1;
//mass id declarations
unsigned int numfirings = 0;
static mass ID input batch one = {0.O], batch two = {O.O], recursive one = {0.0;
static mass ID output batchresult= {0.O], propagate = {0.O;
int rls init - 1;
127
// gyrofilter declarations
staticfloat U_gyro[3] = {0. Of};
//debug declarations
unsigned int data num;
dbgfloat_packetDebugVec;
//dbgushort_packetDebugVec;
// control declarations
unsigned int current cycle
void gspPadslnertial(IMUsample *accel,IMU sample *gyro,unsigned int
numsamples)
//1 00 Hz sampling declarations
static int is second sample = 0;
int data start = 0;
int i;
padsCountsConvertGyros(gyro[0],current_gyro);
/*
//100 Hz data sampling
if (is second sample)
data start = 8;
else
data start = 0;
//Fill in the IMU data
((unsignedshort *)pkt)[data start] sysGetSpheresTime() & Oxffff"
for (i=O0;i<3;i++)
((unsignedshort *) pkt)[data start + 1 + i] = accel[O][i];
for (i=0;i<3;i++)
((unsigned short *)pkt)[datastart+ 4 +i] = gyro[O][i];
128
((unsignedshort *)pkt)[data start + 7] = propGetCurrentTrustersO;
if(is_second_sample)
I
commSendPacket(COAMM CHANNEL_STL, GROUND,sysIdentityGetO,COMM
CMD_IMU DATA, pkt, 0);
is_second sample = 0;
print int(1);
e
else
I}*
is second sample = 1;
void gspPadsGlobal(unsignedint beacon, beacon_measurement matrix measurements)
void gspldentitySetO
// The unique logical identity of this sphere:
/ SPHERE1, SPHERE2, SPHERE3, SPHERE4, or SPHERES.
sysIdentitySet(SPHERE1);
void gspInitProgram()
//Basic Initialization- These settings can be changedas neededfor each
experiment
padslnitializeFPGA(5);
129
//Set up comm TDMframes -- These are sphere-specific
/Set up comm TDMframes -- These are sphere-specific
commTdmaStandardlnit(COMAvM CHANNEL STS, sysIdentityGetO, 1);
commTdmaStandardlnit(COMM CHANNEL STL, sysldentityGetO, 1);
//Enable both communications channels
comm TdmaEnable(COMM CHANNEL STS);
commTdmaEnable(COMM CHANNEL STL);
//Allocate a single sample's worth of internalstorage
padsInertialAllocateBuffers(1);
//Sample andprocess inertialsensors at 10Hz
padslnertialPeriodSet(10, 10);
//Advanced Initialization- These settings should not need adjustmentfor early
experiments
//Turn on 1Hz background telemetry
commBackgroundPointerDefaultO);
/Set up flow controlfor data transfers
ctrlBypassEnable(1);
datacommTokenSetup(O, 18,8);
// kHz sampling init
//
sample buffer = (unsignedshort *) calloc(sample_bufsize,1);
sample buffer =(float *) calloc(sample bufsize, 1);
}
void gsplnitTest(unsignedint testnumber)
{
//Turn off spheres beacon
padsBeaconNumberSet(O);
test started = 1;
for (k= O;k<3;k++)
{
gsp omega dot minus[k] =0.Of
gsp_omegadot[k] = O.Of"
start_gyro[k] = current gyro[k];
past thrust = propGetCurrentThrusterso;
130
switch (test number)
case 1:
case 2:
//mass idpreset rotation
//firingstraight,correctfor induced acceleration
//Firstset the controlfrequency
ctrlPeriodSet(1000);
//setup the datastackpointer
current buffer = sample buffer;
memset(gsp_omega dot, 0, sizeof(gsp_omega dot));
memset(gsp_omega dot minus, 0, sizeof(gsp_omega_dot_minus));
memset(gsp R_acc, 0, sizeof(gsp R_acc));
memset(propagate.CM offset, 0, sizeof(propagate.CM offset));
memset(propagate.CM offsetcov, 0, sizeof(propagate.CM_offset cov));
memset(propagate.Inv_Inertia,0, sizeof(propagate.Inv_Inertia));
memset(propagate.Inv_Inertia_cov, 0, sizeof(propagate.InvInertiacov));
memset(batch result.CMoffset, 0, sizeof(batch result.CM offset));
memset(batch result.CMoffset_cov, 0,
sizeof(hatch_result.CMoffsetcov));
memset(batchresult.Inv Inertia,0, sizeof(batch result.Inv_Inertia));
memset(batch result.Inv Inertia cov, 0,
sizeof(batch_result.InvInertia cov));
memset(batchone.:omega, 0, sizeof(batch_one.omega));
memset(batch one.omega_dot, 0, sizeof(batch_one.omega dot));
memset(batch one.Racc, 0, sizeof(batchone.Racc));
batch one.thrust = 0;
memset(batch_two.omega, 0, sizeof(batchtwo.omega));
memset(batch two.omega_dot, 0, sizeof(batch two.omeg a dot));
memset(batch two.R_acc, 0, sizeof(batch two.R_acc)) ;
batch two.thrust = 0;
memset(recursive_one.omega, 0, sizeof(recursive_one.omega));
memset(recursive_one.omega dot, 0, sizeof(recursiveone.omega dot));
memset(recursive_one.R_acc, 0, sizeof(recursive one.Racc));
recursive one.thrust = 0;
mass_ID InitO;
break;
case 3:
//download 1 kHz data
ctrlPeriodSet(50);
break;
default:
current buffer = NULL;
131
}
BufferPos = current buffer;
}
void gsplnitTask()
I
taskTriggerMaskSet(PADS INERTIALTRIG);
void gspTaskRun(unsigned int gsp_task trigger,unsigned int extra data)
{
//
kHz sampling variables
float cur time;
unsigned int testnum;
//FilteringVariables
float xinput;
constfloatAcoeff_gyro[3] = {1.Of 0. 6498f 0. 1056f};
constfloat Bcoeff_ gyro[3] = {0.5975f 0.6498f 0.5081f•;
// Test 3 is data transfer
test num = ctrlTestNumGetO;
if(test num == 3)
{
return;
}
// Start off test resettingKF
if (test started)
reset KF((float *) current gyro, (float *) gsp omega dot minus);
test started = 0;
new thrust = propGetCurrentThrustersO;
x input = current_gyro[2];//TEMP!!, justfiltering z gyro for now
if (new thrust != 0) //If thruster(s)is on
pstthrust)/If thruster(s)just turned on
st
if (new_thrust=
{
down count = 1; //=lfor
132
1 kHz
numJfirings++;
down count--;
if (down count -= 0) //If 10 ms haspassed to avoid transient
for (k-0;k<3;k++)
startgyro[k] = current gyro[k];
reset KF((float *) start gyro, (float *)gsp_omega dot minus);
}
for (k=0;k<3;k++)//Maybe memcopy or atomic memcopy (forall vector
copies)?
last omegadot[k] = gsp_omega dotk]J;
last R acc[k] - gsp_R acc[k];
// We're past transient,update KF and run mass ID
if (down_count <= 0)
{
update KF((float *) currentgyro,(float*) gsp omega dot, (float
*) gsp omega dot minus, (float *) gspR acc);
if(numirings > = 3)
for (k=0;k<3;k++)
recursive_one.omega_dot[k]=
gsp omega_dot[k];
recursive one.omega[k] = current_gyro[k];
recursive one.R acc[k] = gsp R acc[k];
}
recursive one.thrust = new thrust;
if (rls init == 1)
rls init = 0;
propagate = mass ID_RLS(batch result,
recursive one);
}
else
133
propagate= mass ID RLS(propagate,
recursive one);
}
else //Thrusters are off
{
memset(gspomega dot,O,sizeof(gsp omega dot));
if (newthrust ! pastthrust) //Thruster(s)just turned off
switch (num_firings)//Need datafrom two differentfirings to start
of mass id
I-
case 0:
break,
case 1:
for (k=0O;k<3;k++)
batchone.omega dot[k] =
last omega_dot[k];
batch one.omega[k] = current gyro[k];
batch one.R acc[k]= last R acc[k];
}
batch_one.thrust = past thrust;
break;
case 2:
for (k=0O;k<3;k++)
{
batchtwo.omegadot[k] =
lastomega dot[k];
batch two.omega[k] = current gyro[k];
batch two.R acc[k] last R acc[k];
batch two. thrust = past thrust;
batch result = mass ID Batch(batch one,
batch two);
break;
default:
break;
134
I
past_thrust = new thrust;
//Range check. Make sure that we don't recordtoo many samples.
if (current buffer)
if ((BufferPos - current buffer) < (SHORTS PER SAMPLE *
SAMPLES PER SECOND * TESTDURATION * sizeof(short)))
/*
if ((BufferPos - currentbuffer) < (sample bufsize >> 2))
//Fill in the valuesfor this sample
cur time = (float)sysGetSpheresTime 0;
BufferPos[O] = 7.Of
BufferPos[l] = cur time;
//
//
BufferPos[2] = last omega dot[2];
BufferPos[2] = x input;
BufferPos[3] = gsp_omega dot minus[2];
BufferPos[3] = current_gyro[2];
BufferPos[4] = gyro[O][O];
BufferPos[5] = gyro[O][1];
BufferPos[6] = gyro[O][2];
BufferPos[7] = propGetCurrentThrustersO;
BufferPos += FLOATS PER SAMPLE;
}*/
data num++;
if (data num >= 20)
data num = 0;
void gspControl(unsignedint test number, unsignedint test_time, unsigned int
maneuvernumber,unsigned int maneuver time)
//FastlMUvariables
static
unsigned
int
started
flag
=
staticfloat *buffer_pos;
static unsigned int last length = 0;
135
// Controlvariablesfor mixer
const unsignedint minpulse = 20;
staticfloat defaultState[13] = {O.f
O0.0O
O0.0f
0.0f
0.of o. f o0.Of;
O0.0f
O.0f 0.f 0.Of, 0.Of 1.Of
float command[6] = 0. Of 0.2f 0. Of 0. Of 0.Of 0. Of;
float correcttorque[3];
proptimefiring times;
memset(firing_times.on time, 0, sizeof(firing times.on_time));
memset(firing times.off time, 0, sizeof(firing times.off time));
switch (test number)
{
case 1:
started flag = 0;
lastlength = (sample_bufsize >> 2);
switch (maneuver number)
{
case O.:
case 1:
DoStandardManeuver(1,1000);
ctrlManeuverNumSet(2);
break;
case 2:
DoStandardManeuver(12,1000);
ctrlManeuverNumSet(3);
break;
case 3:
DoStandardMvfaneuver(1,1000);
ctrlManeuverNumSet(4);
break;
case 4:
DoStandardManeuver(13,1000);
ctrlManeuverNumSet(5);
break;
case 5:
DoStandardManeuver(1,1000);
ctrlManeuverNumSet(6);
break;
case 6:
DoStandardManeuver(13,1000);
ctrlManeuverNumSet(7);
break;
case 7:
136
DoStandardfManeuver(1,1000);
ctrlManeuverNumSet(8);
break;
case 8:
DoStandardManeuver(12,1000);
ctrlManeuverNumSet(9);
break;
case 9:
DoStandardManeuver(1,1000);
ctrlManeuverNumnSet(10);
break;
case 10:
DoStandardManeuver(4,1000);
ctrlManeuverNumnSet(11);
break;
case 11:
DoStandardManeuver(1,1000);
ctrlManeuverNumSet(12);
break;
case 12:
DoStandardManeuver(5,1000);
ctrlManeuverNumSet(13);
break;
case 13:
DoStandardManeuver(1,1000);
ctrlManeuverNumSet(14);
break;
case 14:
DoStandardlMvaneuver(5,1000);
ctrlManeuverNumnSet(15);
break;
case 15:
DoStandardManeuver(1,1000);
ctrlManeuverNumSet(16);
break;
case 16:
DoStandard2
aneuver(4,1000);
ctrlManeuverNumnSet(17);
break;
case 17:
DoStandardManeuver(1,1000);
ctrlManeuverNumSet(22);
break;
case 18:
DoStandardManeuver(12,1000);
ctrlManeuverNumSet(19);
137
break;
case 19:
DoStandardManeuver(1,1000);
ctrlManeuverNumSet(20);
break;
case 20:
DoStandardManeuver(13,1000);
ctrlManeuverNumSet(21);
break;
case 21:
DoStandardManeuver(1,1000);
ctrlManeuverNumSet(22);
break;
/*
case 0:
case 1:
DoStandardManeuver(1,2000);
ctrlManeuverNumSet(2);
break;
case 2:
firing times.on time[2] = 0;
firing times.off time[2] = 2000;
ctrlManeuverNumSet(3);
break;
case 3:
DoStandardManeuver(1,2000);
ctrlManeuverNumSet(4);
break;
case 4:
firing_times.on time[3] = 0;
firing times.off time[3] = 2000;
ctrlManeuverNumSet(5);
break;
case 5:
DoStandardManeuver(1,2000);
ctrlManeuverNumSet(6);
break;
case 6:
firing times.on time[9] = 0;
firing times.off time[9] = 2000;
ctrlManeuverNumSet(7);
break;
case 7:
DoStandardManeuver(1,2000);
ctrlManeuverNumSet(8);
break;
case 8:
138
firing times.on time[9] = 0;
firing times.off time[9] = 2000;
ctrlManeuverNumSet(9);
break;
case 9:
DoStandardManeuver(1,2000);
ctrlManeuverNumSet(10);
break;
case 10:
firing times.on time[21 = 0;
firing times.off time[2] = 2000;
ctrlManeuverNumSet(11);
break;
case 11:
DoStandardManeuver(1,2000);
ctrlManeuverNumSet(22);
break;
case 12:
firing times.on time[3] = 0;
firing_times.off time[3] = 2000;
ctrlManeuverNumSet(13);
break;
case 13:
DoStandardManeuver(1,2000);
ctrlManeuverNumSet(22);
break; */
case 22:
ctrlTestTerminate(TEST RESULT NORMAL);
break;
default:
break;
}
//propSetThrusterTimes(&firing times);
break;
case 2:
startedflag = 0;
last length = (samplebufsize >> 2);
currentcycle++;
if(current cycle == 2)
{
ctrlMixSimple(command,min pulse, defaultState);
if(current cycle
4)
4=
{
139
mathMatVecMult((float *) correct torque, (float **) J nom, (float
*) last omega dot, 3, 3);
for (k=3;k<5;k++)
{
command[k] = command[k] - correct torque[k-3];
}
ctrlMixSimpleCorrect(command,minpulse, defaultState);
}
if (current cycle > 5)
{
ctrlTestTerminate(TEST RESULT NORMAL);
}
break;
case 3:
/Send the data buffer over the comm
if (!startedflag)
{
startedflag = 1;
bufferpos = samplebuffer;
}
else
//copy the datafrom the sample buffer into apacket, two samples
at a time
memcpy(pkt, buffer_pos,sizeof(default rfmn_
payload));
// increment memory pointer by two samples (16 shorts)
bufferpos += 8;
//put the packet in the STL queue
commSendPacket(COMM CHANNEL STS, GROUND,sysldentityGetO,COMMAI
CMD DBG FLOA T,(unsignedchar *)pkt,0);
// once all data is sent, stop transmissions
if ((bufferpos-samplebuffer) >= last length)
startedflag = 0;
ctrlTestTerminate(TEST RESULT NORMAL);
break;
default:
break;
I
140
E.2
Angular Acceleration Kalman Filter (est_angacc.c)
#include "est angacc.h"
#include <math.h>
#include "spheres constants.h"
#include "system.h"
#include "control.h"
#include "comm.h"
#include <assert.h>
#include "math lubksb.h"
#include "math ludcmp.h"
#include '"prop.h"
#include "math matrix.h"
#include "commands.h"
#include '"ads.h"
#include <string.h>
#include "util serialprintfh"
//#include "mass id constants.h"
//Declarations
constfloat dt = 0.Ol1/
float I nom[3][3] =
{ 0.0229f 0.0000965f -0.000293f},
{0.0000965f 0.0242f -0.0000311f},
[-0.000293f -0.0000311f 0.0214f },
};
constfloat I inv[3][3] =
{43.6765f -0.1734f 0.5977f},
{-0.1734f 41.3231f 0.0577f},
{0.5977f 0.0577f 46.7372f},
};
constfloatF nom[l2] = {0.12579f 0.13153f 0.13530f 0.13568f 0.12788f 0.12956f
0.12096f 0.13180f 0.13593f 0.14065f 0.12774f 0.12620]j;
static int part index[3] = {0};
staticfloat even odd;
staticfloat Pinv minus[3][3] = [0.Of};
constfloat R inv[3][3] =
141
{7.2307e4f0.f, 0.0f },
{0. Of 1.5119e4f 0.f },
{O.Of
O0.0 3.1776e4ji,
,.
unsigned int updatenum;
staticfloat bias[3] = {0. OJ•;
staticfloat hf3] = {0. Of};
staticfloat P[3][3] = {0. Qf;
staticfloat P inv[3][33] = {0. O•;
int i,j;
//Functions
void reset KF(float start_rate[3],float *omega dot minus)
unsigned short thruster quant;
staticfloat Fk[12] = 0. Of};
constfloat L cross D[3][12] =
{
{0.002128790f -0.000079130f, -0.001669776f -0.000188340f
0.098154965f -0.097065877f 0.000746910f -0.000912890f 0.000452016f
0.000226008f -0.099429926f 0.097972308jf,
{0.097615516f -0.097638341f -0.003122740f 0.000352225f 0.000554184f 0.001157904/ -0.098655692f 0.098246725f -0.000845340f
0.000422670f 0.000212592f -0.000094944f},
{0.001138296f 0.000042312f 0.099163540f -0.096536776f 0.00103641Of, -0.002165460f -0.000399384f -0.000488136f -0.098561794f
0.097472539f -0.000397580f -0.000177560fl,
};
staticfloat ind torque[3] = {0. Ot;
unsigned short thrusters; i/Fudgednumber
update num = 0;
thruster quant = 0;
thrusters = propGetCurrentThrustersO;
for (i=0;i<12;i++)
thruster quant = thruster quant + ((thrusters>>i)& OxOOO1);
}
for (i=0;i<12;i++)
if (thrusters& (1<<i))
142
Fk[i] = (F nom[i] - (thruster_quant- 1)*0 0106J);
else
Fk[i] = O.Of,
//To get L-cross_D, DO THIS://
/*
for (i=O;i<12;i++)
L_cross D[O][i] = L[fi] *D[2][i]- L[2][i]*D[1][i];
L_crossD[1][i]=L[2][i *D[O][i]-L[O[i]*D[2]i];
Lcross D[2][i] =L[O][i]*D[]][i]- L[ffi]*D[O][i];
} */
//OR USE CONST://
memse t(part index,0,sizeof(part index));
memset(omega_dotminus,O,sizeof(omega dot minus));
mathMatVecMult(ind_torque, (float **) Lcross_D, Fk, 3, 12);
//To get omega dot minus, DO THIS:!/
/*
for (i=0;i<3;i++)
for (=0;j<3j+ +)
Itemp[ijy] = Inom[ilj];
ludcmp((float **) I_temp, 3, (int *)partindex,&even_odd);
lubksb((float **) I_temp, 3, (int *)partindex, ind_torque);
143
for (i=0;i<3;i++)
{
omega_dot_minus[i] = ind_torque[i];
}*I
IIIIIIIIIIII
IIGR THIS.il
IIIIIIIIIIII
n1athMatVecMult(omega_dot_minus, (float **) I_inv, ind_torque, 3, 3);
for (i=O;i<3;i++)
{
for (j=0,j<3,j++)
{
}
P_inv_minus[i][i]
=
10. Of;
}
for (i=0;i<3;i+ +)
{
bias[i] = start_rate[i],'
}
}
void update_KF(float current_rate[3], float *omega_dot, float *omega_dot_minus, float
*R_acc)
{
IIUpdate KF Variables
static float H[3][3] = {O.Oj};
static float P_inv_temp[3][3] = {O. OJ};
static float Mat_Tempi [3][3] = {O. OJ};
static float Mat_ Temp2[3] [3] = {O. OJ};
static float K[3][3] = {O. OJ};
static float ang_vel[3] = {O. OJ};
static float Vec_Tempi[3] = {O.Oj};
static float Vec_Temp2[3] = {O.Oj};
static float Vec_ TeJnp3 [3] = {O. OJ};
static float Vec_Temp4[3] = {O.Oj};
IIProp Variables
float phi = i. Of;
144
update_num+ +;
for (i=O;i<3;i++)
{
for (j=0,j<3,j+ +)
{
H [i} [j] = o. Of;
}
H[i][i] = update_nunl*dt;
}
mathMatTransposeMatMult((float **)Mat_Tempi, (float**) H, (float **) R_inv,
3, 3, 3);
mathMatMatMult((float **) Mat_Temp2, (float **) Mat_Tempi, (float **) H, 3,
3,3);
mathMatAdd((float **) P_inv, (float **) P_inv_minus, (float **) Mat_Temp 2, 3,
3);
for (i=O;i<3;i+ +)
{
for (j=0,j<3,j+ +)
{
}
}
ludcmp((float**) P_inv_temp, 3,(int*) part_index, &even_odd);
for (i=O;i<3;i++)
{
for (j=0,j<3,j++)
h[j] = O.Oj;
h[i} = i.Of;
lubksb((float**) P_inv_temp, 3, (int*) part_index, h);
for (j=O;j<3,j+ +)
P[j][i] = h[j];
R acc[i] = P[i}[i};
}
mathMatMatMult((float **) K, (float **) P, (float **) Mat_Tempi, 3, 3, 3);
mathMatVecMult((float *) Vee_Tempi, (float **) H, (float *) amega_dot_minus,
3,3);
mathVecAdd((float *) Vec_Temp2, (float *) bias, (float *) Vec_Tempi, 3);
145
for (i=O;i<3;i+
+)
Temp2[i]j;
Vec_Temp2[i] = -Vec_
math VecAdd((float *) Vec Temp3, (float*) current rate, (float *) VecTemp2,
mathMatVecMult((float *) VecTemp4, (float **) K, (float *) VecTemp3, 3, 3);
mathVecAdd((float *) omega_dot, (float *) omega dot minus, (float *)
Vec_Temp4, 3);
//PropagateStep
for (i=O;i<3;i+
+)
for (j=O;j<3j++)
P inv_
minus[il[fl
=-P
inv[i][j];
for (i=O;i<3;i++)
omega dot minus[i] = phi *omega_dot[i];
void prop KF(float *omega dot,float *omegamdot minus)
//PropagateKF Variables
E.3
Mass ID - low level (mass_id.c)
#include <string.h>
#include "mass id.h"
#include "math matrix.h"
#include "mass id constants.h"
146
#include "math lubksb.h"
#include "math ludcmp.h"
#include "comm.h"
:#include "commands.h"
#include "spheresconstants.h"
#include "system.h"
#include "spheres types.h"
#include "util_serialprintfh"
dbg float packet DebugVecl, DebugVec2,DebugVec3;
float L[3][12] = {0.0Of;
float cm nom[3] = (0.O);
unsignedint count, index;
float Ck[6], Ak[6];
float C[3][3] = {0.O};
float A1[6][3] = (0. O},A2[6][6 = .Ojf;
float Bl[6] = (.Of}, B2[6] = {0.Of};
static int part indexl[3] = {0});
static int partindex2[6] = {0};
staticfloat even_odd;
staticfloat h1[3] = {0.O•;
staticfloat h2[6] = {0.OJ};
int m,n;
void mass ID InitO
{
for (m=0;m<3;m++)
for (n=O;n<12;n++)
{
L[m][n] = L_nom[m][n] - cm_nom[m];
}
memset(Al,0,sizeof(A1));
memset(A2, 0,
sizeof(A2));
}
mass ID output massID Batch(mass ID input batch_one, mass_ID input batch two)
f
float Al _top[3][3] = {0.0]), Al_bot[3][3] = {0.0/};
147
floatA2 top[3][6] = [0.O}, A2_bot[3][6] = [0.};
float Bl top[3] = [0.0f}, Bl_bot[3] = {0.0}, B2_top[3] = {0.0}, B2_bot[3] =
{o.o0.;
float MatTempl[3][3] = {0. Of, Mat_Temp2[3][3] = [0.0f);
float Mat Temp3[3][6] = [0. 03];
floatMat Temp4[6][6] = [0. 01f, Mat Temp5[6][6] = {0. Ofj;
float Mat_Temp6[6][6] = [0. 0O};
float R_batch[6][6] = 0.0f};
float Al_temp[3][6] = [0. O1;
mass_IDoutputbatchresult;
// Get LS Equations
get_LS_equations(baions(batch_one,(float **)Al_top, (float **) A2 _top, (float*)
Bl_top, (float *) B2_top);
get_LS_equationsations(batch_two,(float **)Al_bot, (float **)A2 bot, (float *)
Bl_bot, (float *) B2_bot);
// Combine Matrices
for (m=0;m<3;m++)
for (n=0;n<3;n++)
Al[m][n] = Al_top[m]n];
for (n=O;n<6;n++)
A2[m][n] = A 2_top[m][n];
Bl[m] = Bltop[m];
B2[m] = B2_top[m];
I
for (m=3;m<6;m++)
for (n=0;n<3;n++)
A l[m][n] = A l_bot[m-3][n];
for (n=0;n<6;n++)
A2[m][n] = A2_bot[m-3][n];
Bl[m] = Bl_bot[m-3];
B2[m] = B2_bot[m-3];
}
148
for (m=O;m<3;m++)
for (n=O;n<6;n++)
Al temp[m][n] = Al[n][m];
I
I
//Perform batch
memset(Mat_Templ, 0, sizeof(Mat Templ));
// CM offset
mathMatMatMult((float **)Mat_ Templ, (float **) Al_temp, (float **)Al, 3, 6,
3);
ludcmp((float**) Mat Templ, 3, (int*)part indexl, &evenodd);
for (m=0O;m<3;m++)
for (n=O;n<3;n++)
{
hl[n] = 0.Of,
h1/m] = .Of
lubksb((float* *)Mat_Templ,
for (n=0;n<3;n++)
3, (int*) part index], hl);
{
Mat_Temp2[n][m]
= hl[n];
mathMatMatTransposeMult((float**)Mat Temp3, (float **) Mat Temp2, (float
**)A, 3, 3, 6);
mathMatVecMult(batch result.CM offset, (float **)Mat Temp3, (float*) B, 3,
mathMatTransposeMatMult((float**)Mat Temp4, (float **) A2, (float **)A2, 6,
6, 6);
ludcmp((float**) Mat Temp4, 6, (int*)part_index2, &evenodd);
for (m=0;m<6;m++)
for (n=O;n<6;n++)
h2[n] = 0.Of
h2[m] = 1.Of0
149
lubksb((float**) MatTemp4, 6, (int*) part index2, h2);
for (n=O;n<6;n++)
Mat_Temp5[n][m] = h2[n];
mathMatMatTransposeMult((float**)MatTemp6, (float**) Mat Temp5, (float
*)A2, 6, 6, 6);
6);
mathMatVecMult(batch result.InvInertia,(float **)Mat Temp6, (float *) B2, 6,
//Covariances
memset(R batch, 0, sizeof(R batch));
for (m=0;m<3;m++)
I
Rbatch[m][m] = batch one.Racc[m];
Rbatch[m+3][m+3]= batch_two.R_acc[m];
ludcmp((float**)R batch, 6,(int*) part
_index2, &even odd);
for (m=0;m<6;m++)
for (n=O;n<6;n++)
h2[n] = o.
Of0
h2[m] = 1.Of0
lubksb((float**) R batch, 6, (int*) part index2, h2);
for (n=0;n<6;n++)
Mat Temp5[n][m] = h2[n];
mathMatTransposeMatMult((float**)Mat_Temp3, (float **) A , (float **
Mat Temp5, 6, 3, 6);
mathMatMatMult((float **) batch_result.CMoffsetcov, (float **)Mat_Temp3,
(float **)A1, 3, 6, 3);
mathMatTransposeMatMult((float**) Mat Temp4, (float**)A2, (float **)
Mat Temp5, 6, 6, 6);
mathMatMatMult((float **) batch_result.InvInertia_cov,(float**) Mat Temp4,
(float **)A2, 6, 6, 6);
150
return batch result;
}
massID output mass ID RLS(mass ID output minus, mass ID input recursive one)
float A lrls[3][3]= {O. OJ};
float A2rls[3][6] = 0. Of};
float B rls[3] = {O. Of};
float B2rls[3] = {0. O};
float Qcm[3][3] = o0.0 };
float Qcm_inv[3][3] = {0.0f3;
float Qcm_inv minus[3][3] = 0.0o};
float QI[6][6] = {0. 0]o;
float QI inv[6][6] = {0. Of;
float QIinv2[6][6] = {0.0]];
staticfloat QI inv3[6][6] = 0.0f} ;
float QInmv minus[6][6] = {0. o};
float R[3][3] = 0.Of};
float Mat
float Mat
float Mat
float Mat
float Mat
float Mat
Temp][3][3] = {0. Ofl;
Temp2[3][3] = {0. Of};
Temp3[3][3] = {0. Of;
Temp4[6][3] = {0. Of;
Temp5[6][6] = {0. O;
Temp6[6][3] = {0. OP;
float VecTempl[3] = {O.0Of;
float Vec Temp2[3] = {0. P0;
float VecTemp3[3] = {0. Oj;
float Vec Temp4[6] = 0. Of;
int s, t;
static mass ID outputpropagate;
get LSequations(recursive one, (float**) A rls, (float **) A2rls, (float *) Brls,
(float *) B2rls);
memset(R, 0, sizeof(R));
for (s=O;s<3;s++)
R[s][s] = recursive one.R acc[s];
ludcmp((float**) R, 3,(int*)part-indexl, &evenodd);
for (s=0;s<3;s++)
151
for (t=O;t<3;t++)
hl[t] = O.Of,
hl[s] = i.Of;
lubksb((float* *) R, 3, (int*) part index], hl);
for (t=0O;t<3;t++)
Mat Templ[t][s] = hl[t];
I
I
memcpy(Qcm_invy minus, minus.CMoffset_cov, sizeof(minus.CM offsetcov));
memcpy(QI inv_minus, minus.Inv_nertia_cov,sizeof(minus.Inv_Inertia_cov));
mathMatTransposeMatMult((float**)Mat_Temp2, (float **)A lrls, (float **)
Mat Templ, 3, 3, 3);
mathMatMatMult((float * *)MatTemp3, (float **)Mat Temp2, (float **)Alrls,
3, 3, 3);
mathMatAdd((float **) Qcm_inv, :(float **) Qcm invminus, (float **)
Mat Temp3, 3, 3);
for (s=0;s<3;s+
+)
for(t=O;t<3;t++)
{
propagate.CM offset cov[s][t] = Qcminv[s]ft];
ludcmp((float**) Qcm_inv, 3,f(int*) part index], &even_odd);
for (s=O;s<3;s++)
{
for (t=O;t<3;t++)
{
hl[t] = O.Of,
hi[s] = i.Of,
lubksb((float**) Qcm inv, 3, (int*)part indexl, hl);
for (t=O;t<3;t++)
Qcm[t][s] = hl[t];
152
mathMatTransposeMatMult((float**)Mat Temp4, (float **)A2rls, (float **
Mat Templ, 3, 6, 3);
mathMatMatMult((float **) Mat Temp5, (float **)Mat Temp4, (float **) A2rls,
6, 3, 6);
mathMatAdd((float * *)QIinv,(float **) QI_inv_minus, (float **)Mat Temp5,
6, 6);
for (s=O;s<6;s++)
{
for(t =0;t<6;t++)
{
propagate.Inv_lnertiacov[s][t] = QLinv[s][t];
QIinv2[s][t] = QLinv[s][t];
QIinv3[s][t] = Qlinv[s][t];
I
I
ludcmp((float* *) QI mv, 6, (int*)part index2, &even_odd);
for (s=O;s<6;s++)
for (t=O;t<6;t++)
h2[t] = O.Of,
I
h2[s] =1.Oflubksb((float**) QIinv, 6, (int*) part_index2, h2);
for (t=O;t<6;t++)
{
QI[t][s] = h2[t];
mathMatVecMult((float *) Vec Templ, (float **) Alrls, (float*)
minus.CM offset, 3, 3);
for (s=O;s<3;s++)
Vec_ Temp2[s] = Brls[s]- Vec_ Templ[s];
mathMatMatMult((float **)Mat Temp3, (float**) Qcm, (float **) Mat Temp2,
3, 3, 3);
mathMatVecMult((float *) Vec Temp3, (float **)MatTemp3, (float *)
Vec_Temp2, 3, 3);
mathVecAdd((float *)propagate.CMoffset, (float *) minus.CM_offset, (float *)
VecTemp3, 3);
153
mathMatVecMult((float *) Vec_ Templ, (float **)A2rls, (float *)
minus.Inv_Inertia, 3, 6);
for (s= 0;s<3;s++)
Vec_Temp2[s] = B2rls[s] - VecTempl[s];
mathMatMatMult((float **) Mat Temp6, (float **) QI, (float **) Mat Temp4, 6,
6, 3);
mathMatVecMult((float *) Vec
_Temp4, (float **)MatTemp6, (float *)
VecTemp2, 6, 3);
mathVecAdd((float *)propagate.InvyInertia,(float*) minus.Inv Inertia, (float *)
VecTemp4, 6);
returnpropagate;
I
void get LS equations(mass ID input measurement,float **al, float **a2, float *bl,
float *b2)
unsignedshort thruster_quant;
staticfloat Fk[12] = 0.O};
constfloat L_cross_D_Default[3][12]=
{
(0.002128790f -0.000079130f -0.001669776f -0.000188340f
0.098154965f -0.097065877f 0.000746910f -0.000912890f 0.000452016f
0.000226008f -0.099429926f 0.097972308f},
(0.097615516f -0.097638341f -0.003122740f 0.000352225f 0.000554184f 0.001157904f -0.098655692f 0.098246725f -0.000845340f
0.000422670f 0.000212592f -0.000094944f],
(0.001138296f 0.000042312f 0.099163540f -0.096536776f 0.001036410f -0.002165460f -0.000399384f -0.000488136f -0.098561794f
0.097472539f -0.000397580f -0.000177560fj,
,;
unsigned short thrusters;//Fudged number
constfloat I inv[3][3]
=
{43.6765f -0.1734f 0.5977j],
(-0.1734f 41.3231f 0.0577jf,
{0.5977f 0.0577f 46.7372f},
float alpha[3];
float VecTemp[3] = (0.Of};
thruster_quant = 0;
thrusters = measurement.thrust;
memcpy(alpha,measurement.omega_dot,sizeof(measurement.omega dot));
154
for (n=0;n<12;n++)
thrusterquant= thruster quant + ((thrusters>>n)& OxO001);
for (n=O;n<12;n++)
{
if (thrusters& (1<<n))
{
Fk[n] = (F mag Default[n] - (thrusterquant- 1) *0. 0106f); //
TEMP use ofFmag_Default
}
else
{
Fk[n] = 0. Of
}
}
// Create CM offset RLS equations
mathMatVecMult(Ck, (float **) D Default, (float *) Fk, 3, 12); // TEMP use of
D Default
mathMatVecMult(Ak, (float **) L_cross D Default, (float *)Fk, 3, 12); / TEMP
use ofLcross D Default...needto take Cg into account
mathSkewSymmetric (Ck, (float *) C);
mathMatMatatMult((float**) a, (float **) Iinv, (float**) C, 3, 3, 3);
mathMatVecMult((float *) VecTemp, (float **) I_inv, (float *)Ak, 3, 3); // TEMP
Ak uses L not Lnom
+)
for (n=0;n<3;n+
bl[n] = alpha[n]- VecTemp[n];
// Create Inverse InertiaRLS equations
((float *)a2)[0*6+0]= Ak[O];
((float *)a2)[1*6+1] = Ak[1];
((float *)a2)[2*6+2] = Ak[2];
((float *)a2)[0*6+3]= Ak[1];
((float *)a2)[0*6+4] = Ak[2];
((float *)a2)[1*6+ 3] = Ak[0];
((float *)a2)[1*6+5] = Ak[2];
((float *)a2)[2*6+4] = Ak[O];
((float *)a2)[2*6+5] = Ak[1];
0;n<3;n++)
for (n=O
b2[n] = alpha[n];
155