Embed Size (px)
Citation preview
System Characterization and Online Mass Property Identification of the SPHERES Formation Flight Testbed
Dustin Berkovitz, Prof. David Miller
09/07 SSL# 26-07
1
System Characterization and Online Mass Property Identification of the SPHERES Formation Flight Testbed
Dustin Berkovitz, Prof. David Miller
09/07 SSL# 26-07 This work is based on the unaltered text of the thesis by Dustin Berkovitz submitted to the Department of Aeronautics and Astronautics in partial fullfilment of the requirements for the degree of Master of Science in Aeronautics and Astronautics at the Massachusetts Institute of Technology.
2
3
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
4
Acknowledgments This work was performed primarily under contract NNA05AC52C with the NASA Ames Research Center; the contract officer technical representative (COTR) was Robert Mah. This contract was performed in conjunction with Payload Systems Inc. and Intellization Inc. as part of the 2005 NASA SBIR Phase II award. The author gratefully thanks the sponsors for their generous support that enabled this research. The author also thanks the United States Department of Defense Space Technologies Program Office for their support of the flight program of SPHERES and the NASA Reduced Gravity Office for their support of multiple SPHERES campaigns.
5
Contents
Abstract……………………………………………………………………………………3
List of Figures…...………………………………………………………………………...9
List of Tables…...………………………………………………………………………..12
Nomenclature…...……………………………………………………………………..…13
Chapter 1 Introduction………………………………………………………………15
1.1 SPHERES Background…...……………..……………………………………….15
1.2 SPHERES Hardware Overview…....…….………………………………………17
1.2.1 Inertial Sensors…...………………………………………………………17
1.2.2 Propulsion System…...………….……………………………………….18
1.2.3 Ultrasound-based Positioning System…..……………………………….18
1.3 Current Programs…...…………………………...……………………………….19
1.4 System Characterization Motivation…...………………………………………...21
Chapter 2 SPHERES Mass Properties………………………………………………24
2.1 Mass Properties Overview…...…………………………………………………..24
6
2.2 Measuring Center of Mass…...…………………………………………………..26
2.2.1 CoM Error Sources…...………………………………………………….27
2.3 Measuring Moment of Inertia…...……………………………………………….27
2.3.1 Torsional Pendulum Modeling…...……………………………………...28
2.3.2 Test Procedure…...………………………………………………………32
2.3.3 Products of Inertia…...…………………………………………………...32
2.3.4 Results…...……………………………………………………………….33
2.3.5 Alternate Method for Measuring Inertia ………………………………...34
2.3.6 Inertia Measurement Error Analysis……………………………………..35
2.3.7 Empirical Mass Properties Conclusion…………………………………..42
Chapter 3 Thruster Characterization………………………………………………...44
3.1 Propulsion System Overview…...………………………………………………..44
3.2 Propulsion Hardware…...………………………………………………………..45
3.3 Pulse-width Modulation Scheme…...……………………………………………47
3.4 Propulsion Limitations…...………………………………………………………51
3.5 Measuring Nominal Thrust Vector…...………………………………………….51
3.6 Thrust Latency…...………………………………………………………………53
3.7 Multiple Thruster Pressure Drop…...……………………………………………56
3.8 Sensor Characterization…...……………………………………………………..58
Chapter 4 Mass Property Identification……………………………………………..61
4.1 Online Mass Property Identification…...………………………………………...61
4.2 Theory and Equations…...……………………………………………………….63
4.3 KC-135 Micro-gravity Testing…...……………………………………………...65
7
4.4 Algorithm Development…...…………………………………………………….69
4.4.1 Gyro Filtering and Interpolation…...…………………………………….70
4.4.2 Angular Acceleration Estimation…...……………………………………71
4.4.3 Mass Property Identification…...………………………………………...76
4.4.4 C Code Implementation…...……………………………………………..79
Chapter 5 Conclusion……………………………………………………………….84
5.1 Thesis Summary…...……………………………………………………………..84
5.2 Conclusions…...………………………………………………………………….86
5.3 Future Work……………………………………………………………………...87
Bibliography……………………………………………………………………………..89
Appendix A Tuning Fork Gyroscopes…...…………………………………………….93
A.1 Background Physics…...………………………………………………………...93
A.2 Modeling Rate Sensing…...……………………………………………………..94
A.3 Application to SPHERES…...…………………………………………………..96
Appendix B Fuel Slosh…...…………………………………………………………...99
B.1 Propellant Properties….…………………………………………………………99
B.2 Fuel Slosh on SPHERES…...…………………………………………………..100
B.3 Modeling Fuel Slosh…...………………………………………………………101
B.4 Modeling Input…...…………………………………………………………….103
B.5 Results…...……………………………………………………………………..104
Appendix C SPHERES Satellite Properties…...……………………………………..107
C.1 CoM Offset…...………………………………………………………………...107
C.2 Moment of Inertia…...………………………………………………………….108
8
C.3 Thrust Strengths…...…………………………………………………………...108
C.4 Thrust Directions…...…………………………………………………………..109
Appendix D MATLAB Code…...……………………………………………………110
D.1 Mass Property ID Code…...……………………………………………………110
D.2 Interpolation Scheme…...……………………………………………………...113
D.3 Gyro Filter…...…………………………………………………………………114
D.4 Angular Acceleration Estimator…...…………………………………………...115
D.5 Batch Mass Property ID…...…………………………………………………...117
D.6 Recursive Mass Property ID…...………………………………………………119
Appendix E C Code…...……………………………………………………………..126
E.1 Mass Property ID Code…...……………………………………………………126
E.2 Angular Acceleration Kalman Filter…...……………………………………....141
E.3 Mass ID – low level…...………………………………………………………..146
9
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
10
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
11
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
12
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
13
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
CO2 Carbon Dioxide
ID Identification (as in “Mass Property Identification”)
14
(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
ς Damping ratio
κ Spring constant
b Damping coefficient
E Young’s modulus
σ Stress (Hooke’s Law); standard deviation
* Pseudo-inverse operator
, ,θ ω α&& & Angular acceleration
x Estimate of true value, x
15
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).
16
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.
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
Figure 1.1 SPHERES Flight Satellite
17
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
18
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 CO2 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
19
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 satellite-
to-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], space-
tethers [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.
20
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
21
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.
22
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.
23
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).
Plant Output
Sensor
+Controller
ActuatorMixerControl
Software 0x
Figure 1.3 Standard Control-Loop Representation
24
Chapter 2
SPHERES Mass Properties
2.1 Mass Properties Overview
Modern 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
25
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.
CM Offset
(from GC) X (mm) Y (mm) Z (mm) X (mm) Y (mm) Z (mm)
CM offset
(Dry) 0.49 -1.24 3.98 0.37 -1.52 3.32
CM offset
(Wet) 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 CO2 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).
CAD Model Measured
26
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 6-
DOF 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 3rd coordinate, the SPHERES satellite is rotated so it’s mounted on its side
and the moments are measured again.
27
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
28
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.
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
Figure 2.1 SPHERES Satellite Hanging in a Torsional
29
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 <20° 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:
cos 0
sin
F T mg
F mg m
φ
φ φ⊥
= − =
= =
∑
∑
&&l
(2.1)
mg
T
φ
m
l
x
y
Figure 2.2 Model of One String of the Torsional Pendulum
30
Solving for T: cosT mg φ= (assumes even distribution of weight between the three
strings). Since the string is attached to a plate with negligible vertical mobility, xF is
assumed to be the torque applied to the mounting plate.
sin cos sinxF T mgφ φ φ= =∑ (2.2)
From the rotational equations of motion: r F Iθ× = &&, 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 θ is the angle through which the mounting plate has turned. Substituting
for F and including a damping term gives:
cos sinmgr Iφ φ ςθ θ− − =& && (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 φ and θ are approximated as equal: rφ θ=l . This
approximation is shown in Figure 2.3:
θ
ϕ
r
l
These arc-lengths assumed equivalent
Figure 2.3 Equal Arc-lengths Assumption
31
This produces a non-linear damped harmonic oscillator:
cos sin 0r rI mgrθ ςθ θ θ + + =
&& &l l
(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 0θ from the pendulum’s equilibrium
point such that 0 cos tθ θ ω= . Substituting for θ , and solving for I:
( ) ( ) ( )0 020
cos cos sin coscosmgr r rI t t
tθ ω θ ω
θ ω ω = l l
(2.5)
As was mentioned earlier, this test is designed to run for small values of 0 cos tθ θ ω= .
Using small angle approximations for sine and cosine ( 1tω << and 0 1rθ <<l
), Equation
(2.5) becomes:
2
02
mgrI Iω
= −l
(2.6)
For this particular setup, the values are:
2
20
7.1729.8 /
0.1461~ 2.47
0.0448
m mass kgg m sr radius ml length mI baseInertia kgm
frequencyω
− =−− =−
− =−
32
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 off-
axis 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).
2 2sin cos sin 2BYZ zz yy YZI I B I B P B= + − (2.7)
33
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.
34
Inertia
(wrt GC)
Wet (kg m2)
x 10-2
Dry (kg m2)
x 10-2
Wet (kg m2)
x 10-2
Dry (kg m2)
x 10-2
%
Difference
(Wet)
Inertia(xx) 2.30 2.19 2.57 2.45 10.5%
Inertia(yy) 2.42 2.31 2.25 2.15 7.6%
Inertia(zz) 2.14 2.13 2.03 2.03 5.4%
Inertia(xy) 0.01 0.01 n/a n/a n/a
Inertia(xz) -0.03 -0.03 n/a n/a n/a
Inertia(yz) ~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 κ .
( )
2
sinIA t
I
τ θ κθθ ω
κω
= ==
= −
&&
(2.8)
The procedure in this case is to measure κ by testing an object of known inertia and use
that κ value for all future tests. It turns out however that this is an over-simplification of
CAD Model Measured
35
the torsional spring since, when compared to Equation (2.6), κ depends on m and ml
which is not constant:
2mgrκ =
l (2.9)
If the spring were linear, it would exhibit strain displacement proportional to applied
stress and κ would be constant. Table 2.3 shows data supporting this nonlinear
behavior. 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).
Inertia I (kg-m2)
Angular Rate ω (rad/sec)
Spring Constant κ
0.0448 2.286 0.2341 0.0674 2.933 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.
36
Mathematical Approximation (equal arc lengths, small angle, no damping)
To properly reduce Equation (2.4), the arc lengths φl and rθ are assumed equal. This is
a slightly less “risky” assumption than using small angle approximations for θ and φ .
Since θ is the larger angle, it has the highest potential for error under the small angle
assumption. Showing that the small angle assumption for θ is negligible will then prove
the equal arc-lengths assumption negligible as well. As part of the test procedure, the
maximum value of θ is 20 degrees (0.349 rad). The length of the arc defined by θ is
0.025499r mθ = . The length of the chord that approximates this arc is
2 sin 0.0254672
r mθ =
, 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.
01 ln2 n
aan
ςπ
=
(2.10)
The percent error in measured inertia is equal to 2100ς [3]. The data in Figure 2.4 was
taken for the X-axis inertia of SPHERE SN 2.
37
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:
( )
0
4
2 6
133062991 306ln 2.83329926
100 8.0267 %
n
naa
e
error e
ςπ
ς
−
−
===
= =
= =
(2.11)
There would need to be much more severe damping to have a noticeable impact on
inertia measurement.
38
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:
2B A ABI I mr= + (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 corner 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 2md (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 20.000717I kg mδ = ⋅ (1.025%).
39
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
( 2
17.2 9.83
mkgs
× × , assuming equal load on all three strings). Using a diameter of 2mm,
the cross-sectional area of the string is 6 23.14e m− . From Hooke’s law:
0.002496Eσε = = (2.13)
According to Equation (2.6), inertia varies with string length as 1l
. In the range of values
of l used for these tests, the relationship is close to linear (~1% change in l leads to
~1% change in I). For example, using a nominal string length of 2.6m, 0.0065mδ =l .
This translates into a change of I equal to 20.000175I kg mδ = ⋅ , 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
40
measured time on the stopwatch (T) is related to ω by the number of oscillations. For a
test over 20 oscillations:
2 40
20T T
π πω = = (2.14)
2
21600mgrI T
π=
l (2.15)
Using nominal values for m and l , I changes by 20.0007696I kg mδ = ⋅ (1.194%) with a
measurement error of 0.25T sδ = . 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
41
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 θ=20°)
B) a mass m on a string of length l that swings at very small amplitude (0.01m)
causing unwanted side-to-side motion
The total energy of a simple harmonic oscillator is:
212
E Aκ= (2.16)
where κ is the spring constant and A is the oscillation amplitude.
In Case A, 212A A AE Aκ= where
2
A A
A
IAκ ω
θ==
(2.17)
For a circular disk, 2I mr= . From Equation (2.6), 2
2 mgrI
ω =l
. Substituting into
Equation (2.16) yields:
2
212A
mgrE θ=l
(2.18)
In Case B, 212B B BE Aκ= where
42
2
0.01B B
B
mA meterκ ω=
= (2.19)
For a simple pendulum, 2B
gω =l
. Substituting into Equation (2.16) yields:
( )21 0.012B
mgE =l
(2.20)
Assuming a translational amplitude of 1cm ( 0.01BA = ) and a rotational amplitude of 20°
( 0.349AA = rad),
2
0.0370050.001423
0.002799
A
B
E JE J
I kg mδ
==
= ⋅
(2.21)
This constitutes an inertia error of 3.845%. Alternatively, if 0.005BA = 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 in-
depth characterization of the CO2 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
43
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.
44
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 (CO2) and is stored in mixed liquid/gaseous phase in a tank at the
center of the satellite. Since it is multiphase CO2, 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 CO2 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.
45
The CO2 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 CO2 remaining), the tank pressure
begins to drop as more CO2 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 CO2 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 CO2 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 micro-
gravity. Also, there is a time constant associated with the regulator’s ability to react to
changes in demand for CO2, 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. The capacitor helps
compensate for this delay since it acts a small reservoir of fuel.
46
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.
Dependency Comments System Pressure
Always set to 35 psi (regulator modified with slip ring to “bottom out” at 35 psi)
Nozzle Area Measured with an in-line contour projector Solenoid Mechanics
Solenoid variation negligible.
# Open Thrusters
Thrust varies linearly with # of open thrusters (R2=0.9955)
Temperature Negligible Firing Duration
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:
47
Thruster Locations (cm) X Y Z Thruster 1 -5.16 0.00 9.65 Thruster 2 -5.16 0.00 -9.65 Thruster 3 9.65 -5.16 0.00 Thruster 4 -9.65 -5.16 0.00 Thruster 5 0.00 9.65 -5.16 Thruster 6 0.00 -9.65 -5.16 Thruster 7 5.16 0.00 9.65 Thruster 8 5.16 0.00 -9.65 Thruster 9 9.65 5.16 0.00 Thruster 10 -9.65 5.16 0.00 Thruster 11 0.00 9.65 5.16 Thruster 12 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 Pulse-
Width 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.
48
Figure 3.2 PWM Mixer Code Block Diagram
Controller Command
Mixing Matrix ( ) 1* T TM M MM
−=
Resolve Opposing Thrust Resolve Negative Values
Scale by Maximum
cT×
Pulse Centering
Multiple Thruster Correction
Check 0ON
OFF c
TT T
≥≤
Schedule Propulsion Interrupt
PWM Mixer Transforms F and τ from controller to body axes. Outputs force commands normalized by thruster strength. Firing in opposite directions is inefficient and negative commands are equal to positive commands to the opposite thruster. Force commands are now normalized by max command as well as by thruster strength. PWM Assumption: higher force is proportional to higher % of control period. Mechanical limitations of solenoid valve define minimum impulse bit of 10ms. Total thruster on-times converted to centered ON/OFF times to preserve net thrust direction. First, minimum command is stretched by an amount relative to # of other active thrusters on. Next, second smallest command is stretched, ignoring change to minimum command, relative to (N-2). Continues through the maximum command. Checks that manipulation of thruster ON/OFF times hasn’t violated PWM constraints. Sends ON/OFF times to propulsion interrupt, which controls the actuation signals of the solenoid valves.
49
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.
1 2 12
1 2 12
1 2 12
1 1 2 2 12 12[ ] [ ] [ ]
X X X
Y Y Y
Z Z Z
T T T
F F FF F F
M F F F
F r F r F r
= × × ×
L
L
Lr r rr r r
L (2.1)
Next, the pseudo inverse of this matrix is obtained: ( ) 1* T TM M MM−
= . When a command
vector is multiplied by the matrix *M , 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
50
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 F1 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.
Uncorrected Thrust
Corrected Thrust
Thrust command is pulse width modulated – shorten pulse to account for higher thrust and multiple thruster pressure drop
1F
F
t
2F
T1F
F
t
2F
T
Figure 3.3 Thrust Correction for Simultaneous Firings
51
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 test-
stand 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.
52
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 propulsion 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.
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
- X
+ Z- Y
-X
Figure 3.4 SPHERES Body Axes
53
numbers are reasonably correlated with nozzle area (R2 = 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.
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
Thrust Strength Direction
Thruster avg. Stdev Dx Dy Dz
1 0.134 0.004 0.998 -0.022 0.025
2 0.130 0.002 0.999 -0.001 -0.025
3 0.122 0.006 0.059 0.996 0.032
4 0.130 0.004 -0.018 0.999 0.004
5 0.114 0.005 0.011 0.034 0.999
6 0.124 0.006 -0.022 -0.012 0.999
7 0.134 0.004 -0.999 -0.008 0.044
8 0.130 0.003 -0.998 -0.009 -0.037
9 0.128 0.006 0.042 -0.999 0.009
10 0.132 0.005 -0.019 -1.000 0.004
11 0.126 0.006 0.004 0.060 -0.998
12 0.110 0.005 -0.002 -0.030 -0.999
Table 3.3 Thruster Strength and Direction for SPHERE SN 2
54
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.
t
F
nomF
0t
T10 ms
V
0t 1t
T
20V
5V
t
Appliedforce
Signal tosolenoid
55
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.
660 670 680 690 700 710
400
600
800
1000
1200
1400
time (ms)
coun
ts
Thruster Delay
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, bω drops:
b nk
mω ω≈ = (2.2)
Figure 3.5 Thrust Latency in Solenoid Valve
Figure 3.6 Applied Impulse From Difference in Thruster Latency
56
The stiffness of the load cell to forces in the X and Y directions is 318 10κ = × 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].
57
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.
Figure 3.9 Pressure Change Propagation Data
P1
P4
P2
P3
Thruster 1
58
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 1000 Hz
Range ±25.6 mg
Resolution 12.5 µg/count
Bandwidth 300 Hz
Noise (0 to 10 Hz) < 7 µg rms
Noise (10 to 500 Hz) < 70 µg rms
Max Update Rate 1000 Hz
Range ±83 deg/s
Resolution 0.0407 deg/s/count
Bandwidth 300 Hz
Noise (0 to 100 Hz) < 0.05 deg/s/(Hz)1/2
Figure 3.10 Inertial Measurement Unit Specifications
59
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 (Coriolis-
induced 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.
1400 1450 1500 1550 1600 1650 1700 1750 18002400
2420
2440
2460
2480
2500
2520
time (ms)
coun
ts
Filtered Gyro Data
Figure 3.11 Filtered 338 Hz Gyro Noise
60
A couple key characteristics of these sensors are the biases and scale factors used to
convert from counts to rad/sec and m/s2. 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.
0 50 100 150 200 250 300 350 400 450 5000
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
0.45
0.5
frequency [Hz]
mag
nitu
de
of f
ft
FFT of signal with Ts = 0.001. DC component (0.00038837) is not shown.
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.
61
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
62
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 large-
scale 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.
63
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 T is a binary on/off value for each thruster.
1( )I Iω τ ω ω−= −& % (3.1)
( )thrusters disturb k disturbL D Fτ τ τ τ= + = × + (3.2)
( ),k nom bias random k kF B F F F T= + + (3.3)
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:
( ) ( ) ( )( )1,nom bias random k k disturbI L D B F F F T Iω τ ω ω−= × + + + − ×& (3.4)
F
mome
τ
True
moment
τ
True Cg
moment force Iτ α= × = Iτ α=
Figure 4.1 Ideal Maneuvers for Inertia and CoM Identification
64
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 ∆ is the CoM offset.
nomC C= + ∆ (3.5) [ ]1 . . . 1nomL L= − ∆ (3.6)
Refer to [Wilson, Lages, Mah] [21] for more detailed derivations. The final least squares
form is:
k nom kc DF T≡ (3.7) 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:
k kb ω= & (3.8)
( )( ) ( )1 1k nom nom kb I I I L D F Tω ω ω− −= + × − ×&
3 21
3 1
2 1
00
0k
k
c cA I c c
c c
−
− = − −
1
2
3
x∆ = ∆ ∆
Center of Mass LS Formula
1 2 3
2 1 3
3 1 2
0 0 00 0 00 0 0
k
k
a a aA a a a
a a a
=
111
122
133
112
113
123
III
xIII
−
−
−
−
−
−
= ( ) ( )k nom ka L D F T Iω ω≡ × − × Inertia LS Formula
65
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, ( ) 1T Tx A A A b−
=) . 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:
11 1
1 1 1 1T
k k k k kQ Q A R A−− −
+ + + + = + (3.9)
[ ]11 1 1 1 1 1ˆ ˆ ˆT
k k k k k k k kx x Q A R b A x−+ + + + + += + − (3.10)
4.3 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
66
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 Iyz. 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.
67
0 500 1000 1500 2000 2500 3000 3500 4000 4500−800
−700
−600
−500
−400
−300
−200
−100
0
100
200
time (ms)
coun
ts
Z−Axis Rotation Maneuver
0 20 40 60 80 100 120 140−150
−100
−50
0
50
100
150
200
250
300
time (ms)
coun
ts
Y−Axis Translation Manuever
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.0208 -0.000221 0.000149 -1.87
0.0229 -0.0000082 0.771
0.0260 3.517 Sphere S/N 3
0.0204 0.0000123 0.000157 0.737
0.0238 0.000108 -0.23 0.02795 -0.33 Sphere S/N 2
0.0236 -0.00029 0.000014 0.55
0.0268 -0.0000837 -2.68
0.0284 0.967 Sphere S/N 1
Inertia Matrix (kg m2) CM offset (mm)
Table 4.1 RGA Mass Property Results
68
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.
1.8 2 2.2 2.4 2.6 2.8 3 3.2 3.4
x 104
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
time (ms)
ang
acce
l (ra
d/s2 )
Angular Acceleration Estimate
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
69
determine if the mass property identification algorithm was accurate in measuring
changes in these parameters. Around the fiftieth parabola, a near empty tank of CO2 was
used to replace the current tank. This tank had been depleted to the point that all the
liquid CO2 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.
0 10 20 30 40 50 60 70 80
-1
-0.5
0
0.5
1
1.5
2
2.5
x 10-3 ID'ed Inertia
time [seconds]
Iner
tia
[kg-m
2 ]
IxxIyyIzzIxyIxzIyz
between parabolasproof mass added28g tank replacedproof mass removed
0 10 20 30 40 50 60 70 80
-4
-3.5
-3
-2.5
-2
-1.5
-1
-0.5
0
0.5
1
ID'ed deltaC
time [seconds]
CM
offse
t [m
m]
deltaCxdeltaCydeltaCz
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
70
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 ω& , offline C code that used mock data, and finally an online real-
time 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 ω& 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 of 1t∆
(1,000 at 1 kHz). This is why careful attention was paid to both
the noise of ω and noise created by the process of estimating ω& .
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.
71
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 0R , the standard deviation of the gyro signals.
0
9.5661 7 0 00 2.7365 7 0 /0 0 2.3044 7
eR e rad s
e
− = − −
Figure 4.5 Filtered Gyro Data Standard Deviation
4.4.2 Angular Acceleration Estimation A number of different candidate filters were considered for estimating ω& . Finite
differences are very light on computation, but produce unacceptable noise levels in the
72
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 ω& , which is equal to the filter order for
discrete filters. Two different line-fits are compared in Table 4.2, a non-overlapping line-
fit and an overlapping (“sliding”) line-fit, for varying window sizes. Both of these line-
fits are “centered” (the ω& estimate is centered around an odd number of ω 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.
Variance (rad/s^4) Window Size Non-OverlappingOverlapping
3 0.048096876 0.0048927235 0.034451072 0.0045782827 0.02625048 0.0042075989 0.02116443 0.00380269611 0.017619908 0.00338642513 0.015165923 0.00297875815 0.0132204 0.00259468
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 ω& is a discrete Kalman Filter.
The propagation and measurement update equations are derived from:
1ˆ ˆk k k k
k k k k
x x wz H x
φυ
+ = += +
(3.11)
73
In Equation (4.11), x is the estimate of the true value x , kw is the process noise from
propagating x , φ is the propagation matrix, z is the measurement, υ 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.
1 1 1( ) Tk k k k kP P H R H− − − −= +
1Tk k k kK P H R−=
ˆ ˆ ˆ( )k k k k k kx x K z H x− −= + −
Measurement update
1ˆ ˆk k kx xφ−+ =
1T
k k k k kP P Qφ φ−+ = +
Propagation
k
−
PHR
xz
φ
Q
K
- 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
74
Assuming constant thrust and no energy loss due to drag, ω& 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:
1
0Tk k i
Tk k k
k k
Q E w w
R E
H t
φ
υ υ
=
= = = =
(3.12)
This Kalman Filter has been shown to converge well for initial conditions 0ˆ 0x− = and
( ) 1
0 0P−− = 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. Whenever the firing
configuration changes, the Kalman Filter resets. 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.
75
The gyro data in Figure 4.5 was sent through the ω& Kalman Filter as well as line-fits of
varying order for comparison. Figure 4.7 shows plotted angular acceleration for a non-
overlapping line-fit and a sliding line-fit (both of order 15) and the Kalman Filter.
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 45.4508e rad s− . 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.
76
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 ω& estimate with low covariance has been
calculated. The MATLAB code performs a batch LS over all the data using
( ) 1
1ˆ T Tkx A 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
77
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, 1yyI − , 1
zzI − , and 1yzI − 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, 1xxI − . If those inertias are assumed 0, Equation (4.8)
reduces to:
11 2 3 1
11 2
11 3
0 00 0
xx
xy
xz
a a a IAx a I b
a I
ωωω
−
−
−
= = =
&
&
&
(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
78
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 xxI convergence.
The green and red lines are 1σ boundaries based on the estimate error covariance. The
regions where the σ envelope closes rapidly are during rotations induced about the X-
axis, when xxI 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
0 1000 2000 3000 4000 5000 6000 7000 8000 9000 10000
0
0.02
0.04
0.06
0.08
0.1
update #
iner
tia (
kgm
2 )
Ixx Recursive Estimate
Figure 4.9 Sample Ixx Convergence
79
orthogonal axes, which provides very little information that can be used to improve
confidence in the xxI 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 non-
trivial. 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.
80
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.
1.9 1.95 2 2.05 2.1
x 104
0.4
0.42
0.44
0.46
0.48
0.5
0.52
0.54
0.56
time (ms)
ang
acce
l (ra
d/s2 )
Angular Acceleration Estimate
MATLAB Visual C
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
81
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.
0 10 20 30 40 50 60 70 80 900
5
10
15
20
25
30
update #
(kgm
2 )- 1
Invers Inertia (Izz) - Visual Studio
0 10 20 30 40 50 60 70 80 900
5
10
15
20
25
30
update #
(kgm
2 )- 1
Invers Inertia (Izz) - Sphere Online
Figure 4.11 Visual C vs. Online Embedded C Mass 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 zzI followed by four pure translations designed to identify CMX .
82
4.6 4.8 5 5.2 5.4 5.6 5.8 6 6.2 6.4
x 104
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
time (ms)
Ang
ular
Rat
e (r
ad/s
ec)
Z-Gyro Data
Figure 4.12 Z-Axis Gyro Data From Online Mass ID Test
The resulting RLS estimates are plotted in Figures 4.13.
0 20 40 60 80 100 120 140 160 180-1.6
-1.4
-1.2
-1
-0.8
-0.6
-0.4
-0.2
0
update #
CM
off
set (
m)
X-axis CM offset
0 20 40 60 80 100 120 140 160 1800
0.01
0.02
0.03
0.04
0.05
0.06
0.07
update #
Iner
tia (
kgm
2 )
Izz estimate
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
83
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.
84
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.
85
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
86
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 real-
time 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.
87
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:
88
• 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.
89
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". SAWE,
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.
90
[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 (DART)”,
www.nasa.gov/centers/marshall/news/backgrounds/factsheets.htm, September 2004
[9] 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.” 2nd International Symposium on Formation Flying Missions &
Technologies, Washington, DC, September 2004
[13] Miller, David W., et al, SPHERES Critical Design Review, February 2002
91
[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,” 5th International ESA Conference on Guidance, Navigation and
Control Systems, 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," IEEE American Control Conference, June 2003
[20] Wilson, E., et al., “Motion-based System Identification and Fault Detection and
Isolation Technologies for Thruster Controlled Spacecraft,” JANNAF 3rd 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.” 45th
IEEE International Midwest Symposium on Circuits and Systems, Tulsa, Oklahoma,
Aug 2002.
[22] Wu, M., Lectures in “MEMS Physics and Design,” UCLA Electrical Engineering
course.
92
93
Appendix A
Tuning-Fork Gyroscopes
A.1 Background Physics
The angular rate sensors used in SPHERES are BEI GyroChip II vibrating quartz tuning-
fork 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.
2cF mv= × Ω (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.
94
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 with-
reference 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 xk
95
and yk respectively, and the damping coefficients are xb and yb . The simplified
equations of motions can be described as:
2
2
( 2 )
( 2 )x x x
y y y
Mx b x k x M x y y F
My b y k y M y x x F
= − − + Ω + Ω + Ω +
= − − + Ω − Ω − Ω +
&&& & &
&&& & & (A.2)
The Ω& 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. 0yF = since
the drive force is along the x-axis only.
2y yMy b y k y M x= − − − Ω&& & & (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):
22 2 0n ny y y xςω ω+ + + Ω =&& & & (A.4)
yx
Ω
Drive Axis
M
M
M
M
M
M
Sensing Axis
Figure A.2 Gyro Sensing Model 1
96
The position of the proof masses on the x-axis is assumed to be 0 sin( )dx a tω± + , where
0x is the rest position and dω is the drive frequency. The velocity of the masses along
the x-axis is:
cos( )d dx a tω ω=& (A.5)
Assuming that the drive frequency and natural frequency are equivalent, the equation of
motion along the y-axis becomes:
22 2 cos( ) 0n n n ny y y a tςω ω ω ω+ + + Ω =&& & (A.6)
The solution to this second order ordinary differential equation (ODE) is
sin( )nn
ay tωςωΩ= − . The constants a, nω , and ς are known or calibrated and y is sensed,
which allows solving for Ω . If the gyroscope measures just the amplitude of the y-
deflection, n ya
ςωΩ = .
A.3 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
97
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.
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.
98
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.
99
Appendix B
Fuel Slosh Analysis
B.1 Propellant Properties
The propellant used to maneuver the SPHERES satellites is gaseous CO2. When stored
in the onboard tank, it is in a mixed-phase state with liquid and gas at 860 psi (vapor
pressure of CO2) 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 CO2 is expelled from the
tank, some of the liquid vaporizes to reestablish phase equilibrium.
Nominal fuel mass for a “full” tank is 172g. The CO2 exists in dual-phase until
approximately 28g remains in the tank. After this point, the tank pressure starts to drop
100
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 CO2 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: rI Jω= . Hence, for this model we treat the applied rotational
impulse as the input and the resulting sinusoidal disturbance as the output.
101
Figure B.1 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.
M
k
b
k
b
Figure B.2 Linearized Fuel Slosh Model
102
Since the mirror image sides do not add any mathematical significance, the model will be
further simplified to:
The linear motion of this mass will represent the sloshing of liquid CO2, 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 kx x x u tM M
+ + =&& & (B.1)
where u(t) is the driving force. The states in this system are x
Xx
= &
, so
0 1 0
( )1
x xX u tk bx x
M M
= = + − −
&&
&& & (B.2)
M
k
b
x
Figure B.3 Simplified Fuel Slosh Model
103
[ ]1 0x
Yx
= &
(B.3)
This is a state-space representation with A, B, C, and D defined as:
[ ]
0 1
01
1 00
A k bM M
B
CD
= − −
=
==
(B.4)
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:
2
1( )0.02 6
H ss s
=+ +
(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).
104
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.
5 5
2
4.999 4.999( )1.999 0.9998e z eH z
z z
− −+=− +
(B.6)
B.5 Results
Figure B.5 shows the responses to input impulses of width 2 sec and 4 sec.
105
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 CO2, 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 CO2 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
106
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.
Axis
CoM Change (m)
Inertia Change (kg-m^2)
% of Nominal Inertia
Z 0.017 0.00117 4.5%(about X or Y)
X and Y 0.00234 0.0000468 0.2%(about Z)
Table B.1 Worst-case Effects of Fuel Slosh on Mass Properties
107
Appendix C
SPHERES Satellite Properties
C.1 CoM Offset
CoM offset in mm (from batch LS of mass property ID). σ is about 0.08 in X and Y, 0.4
in Z.
Dry Wet 0.5125 0.4549
-1.2502 -1.241 SPH1 2.9484 0.108 0.8691 0.8115
-1.2069 -1.2044 SPH2 2.7419 -0.0985 0.6343 0.5767
-1.6191 -1.6296 SPH3 3.5379 0.6975 0.7879 0.7303
-1.3827 -1.3824 SPH4 2.4472 -0.3932 0.4621 0.4045
-1.3613 -1.3294 SPH5 3.1517 0.3113
108
C.2 Moment of Inertia
Inertia in kg-m^2 (from batch LS of mass property ID). σ is about 0.002.
C.3 Thrust Strengths
SPH1 SPH2 SPH3 SPH4 SPH5
T stdev T stdev T stdev T stdev T stdev
1 0.1336 0.0044 0.1085 0.0030 0.1065 0.0050 0.1156 0.0044 0.1111 0.0053
2 0.1302 0.0023 0.1117 0.0018 0.1096 0.0031 0.1120 0.0023 0.1108 0.0032
3 0.1221 0.0063 0.1131 0.0090 0.1164 0.0097 0.1195 0.0098 0.1108 0.0080
4 0.1299 0.0042 0.1120 0.0070 0.1097 0.0057 0.1149 0.0074 0.1142 0.0049
5 0.1139 0.0047 0.1120 0.0025 0.1060 0.0097 0.1224 0.0026 0.1123 0.0075
6 0.1244 0.0056 0.1116 0.0025 0.1048 0.0070 0.1204 0.0014 0.1105 0.0050
7 0.1335 0.0040 0.1117 0.0043 0.1051 0.0050 0.1228 0.0075 0.1160 0.0060
8 0.1302 0.0027 0.1151 0.0053 0.1128 0.0055 0.1191 0.0071 0.1130 0.0047
9 0.1284 0.0059 0.1170 0.0078 0.1184 0.0083 0.1220 0.0089 0.1135 0.0066
10 0.1316 0.0051 0.1137 0.0064 0.1160 0.0059 0.1194 0.0074 0.1138 0.0047
11 0.1256 0.0056 0.1058 0.0022 0.1089 0.0093 0.1225 0.0023 0.1068 0.0073
12 0.1095 0.0050 0.1113 0.0026 0.1041 0.0072 0.1127 0.0021 0.1079 0.0063
SPH 1 2 3 4 5Ixx 0.024517 0.0242 0.0275 0.023 0.0242Iyy 0.021529 0.0214 0.0235 0.0242 0.021529Izz 0.020309 0.0194 0.0212 0.0214 0.0212Ixy -8.4E-05 0.000108 -8.2E-06 0.0001 -0.00022Ixz 0.000014 0.000157 0.000149 -0.0003 0Iyz 0.00029 1.23E-05 -0.00022 0 0.0001
109
C.4 Thrust Directions
SPH1 1 2 3 4 5 6 7 8 9 10 11 12Dx 0.9982 0.9985 0.0589 -0.0176 0.0107 -0.0224 -0.9986 -0.9984 0.0418 -0.0194 0.0041 -0.0018stdev 0.0014 0.0017 0.0330 0.0128 0.0075 0.0101 0.0010 0.0014 0.0107 0.0124 0.0069 0.0070Dy -0.0221 -0.0008 0.9961 0.9994 0.0337 -0.0121 -0.0077 -0.0095 -0.9990 -0.9997 0.0603 -0.0297stdev 0.0471 0.0255 0.0044 0.0002 0.0224 0.0224 0.0249 0.0365 0.0005 0.0002 0.0070 0.0218Dz 0.0249 -0.0248 0.0324 0.0036 0.9991 0.9994 0.0445 -0.0367 0.0088 0.0044 -0.9981 -0.9994stdev 0.0214 0.0438 0.0506 0.0268 0.0010 0.0005 0.0179 0.0225 0.0060 0.0085 0.0004 0.0009SPH2 1 2 3 4 5 6 7 8 9 10 11 12Dx 0.9988 0.9995 0.0126 -0.0209 -0.0011 -0.0076 -0.9996 -0.9989 0.0391 -0.0292 0.0085 -0.0046stdev 0.0009 0.0005 0.0278 0.0126 0.0168 0.0067 0.0003 0.0005 0.0040 0.0055 0.0064 0.0052Dy -0.0061 -0.0086 0.9994 0.9995 0.0244 -0.0278 0.0162 0.0374 -0.9989 -0.9992 0.0598 0.0075stdev 0.0299 0.0293 0.0005 0.0002 0.0178 0.0087 0.0096 0.0059 0.0003 0.0004 0.0085 0.0090Dz 0.0356 -0.0018 0.0104 -0.0149 0.9994 0.9995 0.0137 -0.0166 0.0219 0.0239 -0.9981 -0.9999stdev 0.0156 0.0095 0.0106 0.0119 0.0009 0.0002 0.0153 0.0227 0.0102 0.0117 0.0005 0.0001SPH3 1 2 3 4 5 6 7 8 9 10 11 12Dx 0.9996 0.9986 0.0100 -0.0393 0.0084 -0.0166 -0.9988 -0.9983 0.0078 -0.0310 -0.0258 0.0083stdev 0.0004 0.0009 0.0252 0.0119 0.0243 0.0204 0.0010 0.0012 0.0411 0.0283 0.0132 0.0259Dy -0.0091 -0.0247 0.9995 0.9988 -0.0087 -0.0835 0.0134 0.0162 -0.9990 -0.9990 0.0426 -0.0462stdev 0.0164 0.0152 0.0006 0.0003 0.0320 0.0309 0.0116 0.0124 0.0009 0.0011 0.0743 0.0381Dz 0.0171 -0.0405 0.0009 0.0125 0.9992 0.9957 0.0067 -0.0353 -0.0158 -0.0003 -0.9961 -0.9979stdev 0.0166 0.0187 0.0189 0.0228 0.0009 0.0033 0.0463 0.0428 0.0086 0.0181 0.0092 0.0024SPH4 1 2 3 4 5 6 7 8 9 10 11 12Dx 0.9990 0.9983 0.0264 -0.0455 -0.0128 -0.0136 -0.9996 -0.9988 0.0023 -0.0448 -0.0279 0.0231stdev 0.0010 0.0005 0.0209 0.0184 0.0272 0.0176 0.0005 0.0009 0.0273 0.0233 0.0109 0.0090Dy -0.0331 -0.0351 0.9992 0.9985 0.0264 -0.0660 0.0020 0.0149 -0.9995 -0.9986 0.0159 -0.0335stdev 0.0227 0.0132 0.0012 0.0008 0.0156 0.0114 0.0093 0.0102 0.0005 0.0009 0.0236 0.0281Dz 0.0196 -0.0451 0.0085 -0.0212 0.9991 0.9975 0.0092 -0.0408 -0.0114 0.0079 -0.9992 -0.9988stdev 0.0123 0.0032 0.0244 0.0108 0.0009 0.0005 0.0264 0.0224 0.0098 0.0122 0.0007 0.0007SPH5 1 2 3 4 5 6 7 8 9 10 11 12Dx 0.9988 0.9987 0.0229 -0.0669 -0.0219 -0.0036 -0.9992 -0.9991 0.0090 -0.0450 -0.0108 0.0136stdev 0.0005 0.0003 0.0120 0.0051 0.0223 0.0136 0.0003 0.0007 0.0385 0.0281 0.0070 0.0103Dy -0.0108 -0.0276 0.9993 0.9975 0.0272 -0.0662 0.0101 0.0110 -0.9989 -0.9985 0.0352 -0.0334stdev 0.0100 0.0075 0.0003 0.0006 0.0086 0.0047 0.0258 0.0130 0.0010 0.0013 0.0265 0.0189Dz 0.0452 -0.0406 -0.0202 -0.0225 0.9991 0.9977 0.0003 -0.0255 -0.0250 -0.0089 -0.9990 -0.9991stdev 0.0136 0.0064 0.0214 0.0086 0.0007 0.0003 0.0303 0.0296 0.0087 0.0089 0.0011 0.0006
110
Appendix D
MATLAB Code
D.1 Mass Property ID code (massid_kc1.m)
This is the high-level function call for the mass property ID algorithm. function [cm1, I1] = massid_kc1(start, finish); load('kc_test.mat'); disp('Initializing...'); % SYSTEM PARAMETERS – Initialize system parameters depending on which SPHERE was used. % -------------------------------------------------------------------------------- if Sph == 1 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;...
111
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 %I_nom = 0.0335;% kg.m^2, for 2-D 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 = [2055; 2079; 2051]; elseif Sph == 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]; %Calibrated Pressure Gauge 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.m^2, 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]; elseif Sph == 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];
112
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.m^2, 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 = [2055; 2061; 2053]; end; % % TEST PARAMETERS % % -------------------------------------------------------------------------------- rate_raw = gyro; % Convert rate %conversion = 1713.14;% mV/(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 telemetry from dropped comm packets disp('Interpolate missing data....'); [time, rate, accel, thrusters] = int(time, rate, accel, thrusters); % Find beginnings/ends of firings 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 interpolated gyro signal disp('Filter gyro 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 associated w dot estimate) updateind = find(thrusters); L = Lnom - cm_nom*ones(1,12);
113
disp('Estimate Angular Acceleration....'); wdotkalman %%%%%%%%%%%%%%%%%%%%%% % Batch Least Squares % %%%%%%%%%%%%%%%%%%%%%% disp('Batch LS....'); batchLS; % Batch Results: cm1 = cm(:,end) I1 = I_matend %%%%%%%%%%%%%%%%%%%%%%%%%%% % Recursive Least Squares % %%%%%%%%%%%%%%%%%%%%%%%%%%% disp('Recursive LS....'); 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)))); for j = 1:length(b) index = length(b) - j + 1; time(b(index)+1) = []; gyro(:,b(index)+1) = []; accel(:,b(index)+1) = []; thrusters(b(index)+1) = [];
114
end; x = time; a = find(diff(time) - 1*dt); for i = 1:3 y = gyro(i,:); xi = []; for j = 1:length(a) xi = [xi time(a(j))+1*dt :dt: time(a(j)+1)-1]; end; yi = interp1(x, y, xi, 'linear'); A = [[x xi]' [y yi]']; B = sortrows(A,1); intgyro(i,:) = B(:,2)'; end; inttime = B(:,1)'; y = thrusters; yi = interp1(x, y, xi, 'nearest'); C = [[x xi]' [y yi]']; D = sortrows(C,1); intthrusters = D(:,2); intaccel = accel;
D.3 Gyro Filter (gyrofiltvar.m)
This file is called by massid_kc1.m to filter the gyros after missing data points are
interpolated.
function filtered_data = gyrofiltvar(gyro, time, freq, ord) freq = freq*2*pi;
115
s = tf('s'); filter_cont = freq^ord/(s+freq)^ord; dt = (time(2)-time(1))/1000; filter_disc = c2d(filter_cont, dt ,'tustin'); filter_struct = get(filter_disc); b = filter_struct.num1; a = filter_struct.den1; filtered_data(1,:) = filtfilt(b, a, gyro(1,:)); filtered_data(2,:) = filtfilt(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 Filter Initializations phi = 1; Fk = (Fnom-sum(T(updateind(1),:))*0.0106) .* T(updateind(1),:); x_minus1 = inv(I_nom)*cross(L,D)*Fk'; P_inv_minus1 = 2*eye(3,3); Q = zeros(3,3); H1 = diag(zeros(1,3)); R = 1e-7 * [9.5661 0 0; 0 2.7365 0; 0 0 2.3044]; bias1 = rate(:,updateind(1) - 1); if dt < 5/1000 updateind(1:10) = []; %bias1 = mean(rate(:,1:updateind(1)-10),2); end; % Start Kalman Filter j=1*dt; k=1; while k < length(updateind)
116
Hk = diag([j j j])*1; G = find(T(updateind(k),:)); P_invk = P_inv_minusk + Hk'*inv(R)*Hk; Pk = inv(P_invk); Kk = Pk*Hk'*inv(R); xk = x_minusk + Kk*(rate(:,updateind(k))-bias1 - Hk*x_minusk); x_minusk+1 = phi*xk; P_minusk+1 = phi*Pk*phi' + Q; P_inv_minusk+1 = inv(P_minusk+1); if ~(thrusters(updateind(k+1)) == thrusters(updateind(k))) Fk = (Fnom-sum(T(updateind(k+1),:))*0.0106) .* T(updateind(k+1),:); j = 0; x_minusk+1 = inv(I_nom)*cross(L,D)*Fk'; P_inv_minusk+1 = 2*eye(3,3); bias1 = rate(:, updateind(k+1) - 1); if dt < 5/1000 updateind(k+1:k+11) = []; end; end; j=j+1*dt; k=k+1; end; % Extract angular acceleration and associated standard deviation for i = 1:length(x) acc(:,i) = xi; acc_xvar(i) = Pi(1,1); acc_yvar(i) = Pi(2,2); acc_zvar(i) = Pi(3,3); acc_Sigmax(i) = sqrt(acc_xvar(i)); acc_Sigmay(i) = sqrt(acc_yvar(i)); acc_Sigmaz(i) = sqrt(acc_zvar(i)); R_acci = diag(diag(Pi)); end; anew = zeros(3, length(time)); for i = 1:length(x) anew(:,updateind(i)) = xi; end;
117
% Clear local variables since this is a script, not a function clear H G x acc_xvar acc_yvar acc_zvar acc_Sigmay acc_Sigmaz P_inv_minus P_inv 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 of LS equations if necessary for well-conditioned Q cc=1; estI = zeros(1,3); for bb=1:length(updateind) if thrusters(updateind(bb)) >0 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) | 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'; estIdec = bi2de(estI) ; %% All switch statements are uncommented if we are using estI to simplify equations. % switch estIdec % case bi2de([1 0 0]) % A1(cc:cc+2,:) = inv(I_nom)*[ 0 -Ck(3);... % Ck(3) 0 ;... % -Ck(2) Ck(1)]; % b1(cc:cc+2,:) = anew(:,updateind(bb)) - inv(I_nom)*cross(Lnom,D)*Fk';
118
% A2(cc:cc+2,:) = [Ak(1) Ak(2) Ak(3) ;... % 0 Ak(1) 0 ;... % 0 0 Ak(1)]; % b2(cc:cc+2,:) = anew(:,updateind(bb)); % case bi2de([0 1 0]) % A1(cc:cc+2,:) = inv(I_nom)*[-Ck(3) Ck(2);... % 0 -Ck(1);... % Ck(1) 0 ]; % b1(cc:cc+2,:) = anew(:,updateind(bb)) - inv(I_nom)*cross(Lnom,D)*Fk'; % A2(cc:cc+2,:) = [0 Ak(2) 0 ;... % Ak(2) Ak(1) Ak(3) ;... % 0 0 Ak(2)]; % b2(cc:cc+2,:) = anew(:,updateind(bb)); % case bi2de([0 0 1]) % A1(cc:cc+2,:) = inv(I_nom)*[ 0 Ck(2);... % Ck(3) -Ck(1);... % -Ck(2) 0 ]; % b1(cc:cc+2,:) = anew(:,updateind(bb)) - inv(I_nom)*cross(Lnom,D)*Fk'; % A2(cc:cc+2,:) = [0 Ak(3) 0 ;... % 0 0 Ak(3) ;... % Ak(3) Ak(1) Ak(2)]; % b2(cc:cc+2,:) = anew(:,updateind(bb)); % case bi2de([1 1 0]) % A1(cc:cc+2,:) = inv(I_nom)*[ 0 -Ck(3) Ck(2);... % Ck(3) 0 -Ck(1);... % -Ck(2) Ck(1) 0 ]; % b1(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 ;... % 0 Ak(2) Ak(1) 0 Ak(3) ;... % 0 0 0 Ak(1) Ak(2)]; % b2(cc:cc+2,:) = anew(:,updateind(bb)); % case bi2de([1 0 1]) % A1(cc:cc+2,:) = inv(I_nom)*[ 0 -Ck(3) Ck(2);... % Ck(3) 0 -Ck(1);... % -Ck(2) Ck(1) 0 ]; % b1(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 ;... % 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 1]) % A1(cc:cc+2,:) = inv(I_nom)*[ 0 -Ck(3) Ck(2);... % Ck(3) 0 -Ck(1);... % -Ck(2) Ck(1) 0 ]; % b1(cc:cc+2,:) = anew(:,updateind(bb)) - inv(I_nom)*cross(Lnom,D)*Fk'; % A2(cc:cc+2,:) = [0 0 Ak(2) Ak(3) 0 ;...
119
% 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([1 1 1]) % FULL ORDER CASE A1(cc:cc+2,:) = inv(I_nom)*[ 0 -Ck(3) Ck(2);... Ck(3) 0 -Ck(1);... -Ck(2) Ck(1) 0 ]; b1(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 ;... 0 Ak(2) 0 Ak(1) 0 Ak(3) ;... 0 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 Estimates indexed with thruster on time (*NO* gaps between thruster firings) if bb == length(updateind) % Only taking batch after all data is included cm(:,(cc-1)/3) = inv(A1'*A1)*A1'*b1; I_inv(:,(cc-1)/3) = inv(A2'*A2)*A2'*b2; I_inv_mat(cc-1)/3 = [I_inv(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) I_inv(6,(cc-1)/3);... I_inv(5,(cc-1)/3) I_inv(6,(cc-1)/3) I_inv(3,(cc-1)/3)]; I_mat(cc-1)/3 = inv(I_inv_mat(cc-1)/3); end; end; end;
D.6 Recursive Mass Property ID (recursiveLS.m)
This is the last script call executed from massid_kc.m and it is where the recursive
equations are exercised.
120
% Batch over first two firings cc=1; % used to index rows of A 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); R_batch = []; 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) | 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 thrusters open Ckrls = D*Fk'; Akrls = cross(L,D)*Fk'; estIdec = bi2de(estI); % switch estIdec % case bi2de([1 0 0]) % A1rls(cc:cc+2,:) = inv(I_nom)*[ 0 -Ckrls(3) ;... % Ckrls(3) 0 ;... % -Ckrls(2) Ckrls(1)]; % b1rls(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([0 1 0]) % A1rls(cc:cc+2,:) = inv(I_nom)*[-Ckrls(3) Ckrls(2);... % 0 -Ckrls(1);... % Ckrls(1) 0 ]; % b1rls(cc:cc+2,:) = anew(:,updateind(count)) - inv(I_nom)*cross(Lnom,D)*Fk'; % A2rls(cc:cc+2,:) = [0 Akrls(2) 0 ;... % Akrls(2) Akrls(1) Akrls(3) ;...
121
% 0 0 Akrls(2)]; % b2rls(cc:cc+2,:) = anew(:,updateind(count)); % case bi2de([0 0 1]) % A1rls(cc:cc+2,:) = inv(I_nom)*[ 0 Ckrls(2);... % Ckrls(3) -Ckrls(1);... % -Ckrls(2) 0 ]; % b1rls(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]) % A1rls(cc:cc+2,:) = inv(I_nom)*[ 0 -Ckrls(3) Ckrls(2);... % Ckrls(3) 0 -Ckrls(1);... % -Ckrls(2) Ckrls(1) 0 ]; % b1rls(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) ;... % 0 0 0 Akrls(1) Akrls(2)]; % b2rls(cc:cc+2,:) = anew(:,updateind(count)); % case bi2de([1 0 1]) % A1rls(cc:cc+2,:) = inv(I_nom)*[ 0 -Ckrls(3) Ckrls(2);... % Ckrls(3) 0 -Ckrls(1);... % -Ckrls(2) Ckrls(1) 0 ]; % b1rls(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 0 Akrls(1) 0 Akrls(3) ;... % 0 Akrls(3) 0 Akrls(1) Akrls(2)]; % b2rls(cc:cc+2,:) = anew(:,updateind(count)); % case bi2de([0 1 1]) % A1rls(cc:cc+2,:) = inv(I_nom)*[ 0 -Ckrls(3) Ckrls(2);... % Ckrls(3) 0 -Ckrls(1);... % -Ckrls(2) Ckrls(1) 0 ]; % b1rls(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(1) 0 Akrls(3) ;... % 0 Akrls(3) 0 Akrls(1) Akrls(2)]; % b2rls(cc:cc+2,:) = anew(:,updateind(count)); % case bi2de([1 1 1]) A1rls(cc:cc+2,:) = inv(I_nom)*[ 0 -Ckrls(3) Ckrls(2);... Ckrls(3) 0 -Ckrls(1);... -Ckrls(2) Ckrls(1) 0 ]; b1rls(cc:cc+2,:) = anew(:,updateind(count)) - inv(I_nom)*cross(Lnom,D)*Fk'; A2rls(cc:cc+2,:) = [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)];
122
b2rls(cc:cc+2,:) = anew(:,updateind(count)); % case bi2de([0 0 0]) % disp('Error') % keyboard; % end; cc=cc+3; R_batch = diag([diag(R_batch); diag(R_acccount)]); 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 % Initial state/covariance estimates: cm_rls(:,1) = inv(A1rls'*A1rls)*A1rls'*b1rls; Qcm_inv1 = A1rls' * (inv(R_batch)) * A1rls; I_inv_rls(:,1) = inv(A2rls'*A2rls)*A2rls'*b2rls; QI_inv1 = (A2rls' * (inv(R_batch)) * A2rls); clear A1rls A2rls b1rls b2rls; k=2; % k=1 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) | T(updateind(bb),10)) estI(1,3) = 1; end; Fk = (Fnom-sum(T(updateind(bb),:))*0.0106) .* T(updateind(bb),:); Ckrls = D*Fk'; Akrls = cross(L,D)*Fk'; estIdec = bi2de(estI); % switch estIdec % case bi2de([1 0 0]) % A1rls = inv(I_nom)*[ 0 -Ckrls(3) ;... % Ckrls(3) 0 ;... % -Ckrls(2) Ckrls(1)];
123
% b1rls = anew(:,updateind(bb)) - inv(I_nom)*cross(Lnom,D)*Fk'; % A2rls = [Akrls(1) Akrls(2) Akrls(3) ;... % 0 Akrls(1) 0 ;... % 0 0 Akrls(1)]; % b2rls = anew(:,updateind(bb)); % case bi2de([0 1 0]) % A1rls = inv(I_nom)*[-Ckrls(3) Ckrls(2);... % 0 -Ckrls(1);... % Ckrls(1) 0 ]; % b1rls = 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([0 0 1]) % A1rls = inv(I_nom)*[ 0 Ckrls(2);... % Ckrls(3) -Ckrls(1);... % -Ckrls(2) 0 ]; % b1rls = anew(:,updateind(bb)) - inv(I_nom)*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]) % A1rls = inv(I_nom)*[ 0 -Ckrls(3) Ckrls(2);... % Ckrls(3) 0 -Ckrls(1);... % -Ckrls(2) Ckrls(1) 0 ]; % b1rls = anew(:,updateind(bb)) - inv(I_nom)*cross(Lnom,D)*Fk'; % A2rls = [Akrls(1) 0 Akrls(2) Akrls(3) 0 ;... % 0 Akrls(2) Akrls(1) 0 Akrls(3) ;... % 0 0 0 Akrls(1) Akrls(2)]; % b2rls = anew(:,updateind(bb)); % case bi2de([1 0 1]) % A1rls = inv(I_nom)*[ 0 -Ckrls(3) Ckrls(2);... % Ckrls(3) 0 -Ckrls(1);... % -Ckrls(2) Ckrls(1) 0 ]; % b1rls = anew(:,updateind(bb)) - inv(I_nom)*cross(Lnom,D)*Fk'; % A2rls = [Akrls(1) 0 Akrls(2) Akrls(3) 0 ;... % 0 0 Akrls(1) 0 Akrls(3) ;... % 0 Akrls(3) 0 Akrls(1) Akrls(2)]; % b2rls = anew(:,updateind(bb)); % case bi2de([0 1 1]) % A1rls = inv(I_nom)*[ 0 -Ckrls(3) Ckrls(2);... % Ckrls(3) 0 -Ckrls(1);... % -Ckrls(2) Ckrls(1) 0 ]; % b1rls = anew(:,updateind(bb)) - inv(I_nom)*cross(Lnom,D)*Fk';
124
% A2rls = [0 0 Akrls(2) Akrls(3) 0 ;... % Akrls(2) 0 Akrls(1) 0 Akrls(3) ;... % 0 Akrls(3) 0 Akrls(1) Akrls(2)]; % b2rls = anew(:,updateind(bb)); % case bi2de([1 1 1]) A1rls = inv(I_nom)*[ 0 -Ckrls(3) Ckrls(2);... Ckrls(3) 0 -Ckrls(1);... -Ckrls(2) Ckrls(1) 0 ]; b1rls = anew(:,updateind(bb)) - inv(I_nom)*cross(Lnom,D)*Fk'; A2rls = [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)]; b2rls = anew(:,updateind(bb)); % case bi2de([0 0 0]) % disp('Error') % keyboard; % end; % State/Covariance update: Qcm_invk = Qcm_invk-1 + A1rls' * (inv(R_acctemp-2+k)) * A1rls; cm_rls(:,k) = cm_rls(:,k-1) + inv(Qcm_invk) * A1rls' * (inv(R_acctemp-2+k)) * (b1rls - A1rls * cm_rls(:,k-1)); QI_invk = QI_invk-1 + A2rls' * (inv(R_acctemp-2+k)) * A2rls; I_inv_rls(:,k) = I_inv_rls(:,k-1) + inv(QI_invk)* A2rls' * (inv(R_acctemp-2+k)) * (b2rls - A2rls * I_inv_rls(:,k-1)); k=k+1; end; % %Put inverse inertia estimate into matrix form and invert it: for i = 1:size(I_inv_rls,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_rlsi = inv(I_inv_mat_rlsi); end; % Get varaince/stdev of Ixx, Iyy, Izz over time for j = 1:length(QI_inv) QIxx(j) = 1/(QI_invj(1,1)); Sigmaxx(j) = sqrt(QIxx(j)); Ixx_rls(j) = I_mat_rlsj(1,1); QIyy(j) = 1/(QI_invj(2,2)); Sigmayy(j) = sqrt(QIyy(j)); Iyy_rls(j) = I_mat_rlsj(2,2);
125
QIzz(j) = 1/(QI_invj(3,3)); Sigmazz(j) = sqrt(QIzz(j)); Izz_rls(j) = I_mat_rlsj(3,3); end;
126
Appendix E
C Code
E.1 Mass Property ID code (mass_id/gsp.c)
This is the highest layer of the online mass ID code.
/* Template file of gsp.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 "spheres_constants.h" #include "system.h" #include "control.h"
127
#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 "util_serial_printf.h" //1 kHz sampling declarations #define SHORTS_PER_SAMPLE (8) #define FLOATS_PER_SAMPLE (4) #define SAMPLES_PER_SECOND (1000) #define TEST_DURATION (5) static float *sample_buffer=NULL; static float *current_buffer = NULL; static float *BufferPos = NULL; //static unsigned int sample_bufsize =sizeof(unsigned short) * SHORTS_PER_SAMPLE * SAMPLES_PER_SECOND * TEST_DURATION; static unsigned int sample_bufsize =sizeof(float) * FLOATS_PER_SAMPLE * SAMPLES_PER_SECOND * TEST_DURATION; static default_rfm_payload pkt; // omega_dot KF declarations static float start_gyro[3] = 0.0f; static float current_gyro[3] = 0.0f; static float gsp_omega_dot[3], gsp_omega_dot_minus[3], last_omega_dot[3], gsp_R_acc[3], last_R_acc[3]; int k, p; unsigned int test_started = 1; unsigned short past_thrust, new_thrust; int down_count = -1; // mass_id declarations unsigned int num_firings = 0; static mass_ID_input batch_one = 0.0f, batch_two = 0.0f, recursive_one = 0.0f; static mass_ID_output batch_result = 0.0f, propagate = 0.0f; int rls_init = 1;
128
// gyro filter declarations static float U_gyro[3] = 0.0f; //debug declarations unsigned int data_num; dbg_float_packet DebugVec; //dbg_ushort_packet DebugVec; // control declarations unsigned int current_cycle = 0; void gspPadsInertial(IMU_sample *accel, IMU_sample *gyro, unsigned int num_samples) //100 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 ((unsigned short *) pkt)[data_start] = sysGetSpheresTime() & 0xffff; for (i=0;i<3;i++) ((unsigned short *) pkt)[data_start + 1 + i] = accel[0][i]; for (i=0;i<3;i++) ((unsigned short *) pkt)[data_start + 4 +i] = gyro[0][i];
129
((unsigned short *) pkt)[data_start + 7] = propGetCurrentThrusters(); if (is_second_sample) commSendPacket(COMM_CHANNEL_STL,GROUND,sysIdentityGet(),COMM_CMD_IMU_DATA, pkt, 0); is_second_sample = 0; print_int(1); else is_second_sample = 1; */ void gspPadsGlobal(unsigned int beacon, beacon_measurement_matrix measurements) void gspIdentitySet() ////////////////////////////////////////////////// // The unique logical identity of this sphere: // SPHERE1, SPHERE2, SPHERE3, SPHERE4, or SPHERE5. ////////////////////////////////////////////////// sysIdentitySet(SPHERE1); void gspInitProgram() //////////////////////////////////////////////////////////////////////////////////// //Basic Initialization - These settings can be changed as needed for each experiment //////////////////////////////////////////////////////////////////////////////////// padsInitializeFPGA(5);
130
//Set up comm TDM frames -- These are sphere-specific //Set up comm TDM frames -- These are sphere-specific commTdmaStandardInit(COMM_CHANNEL_STS, sysIdentityGet(), 1); commTdmaStandardInit(COMM_CHANNEL_STL, sysIdentityGet(), 1); //Enable both communications channels commTdmaEnable(COMM_CHANNEL_STS); commTdmaEnable(COMM_CHANNEL_STL); //Allocate a single sample's worth of internal storage padsInertialAllocateBuffers(1); //Sample and process inertial sensors at 10Hz padsInertialPeriodSet(10,10); //////////////////////////////////////////////////////////////////////////////////// //Advanced Initialization - These settings should not need adjustment for early experiments //////////////////////////////////////////////////////////////////////////////////// //Turn on 1Hz background telemetry commBackgroundPointerDefault(); //Set up flow control for data transfers ctrlBypassEnable(1); datacommTokenSetup(0, 18,8); // 1kHz sampling init // sample_buffer = (unsigned short *) calloc(sample_bufsize,1); sample_buffer =(float *) calloc(sample_bufsize,1); void gspInitTest(unsigned int test_number) //Turn off spheres beacon padsBeaconNumberSet(0); test_started = 1; for (k=0;k<3;k++) gsp_omega_dot_minus[k] =0.0f; gsp_omega_dot[k] = 0.0f; start_gyro[k] = current_gyro[k]; past_thrust = propGetCurrentThrusters();
131
switch (test_number) case 1: // mass_id preset rotation case 2: // firing straight, correct for induced acceleration //First set the control frequency ctrlPeriodSet(1000); // setup the data stack pointer 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_offset_cov, 0, sizeof(propagate.CM_offset_cov)); memset(propagate.Inv_Inertia, 0, sizeof(propagate.Inv_Inertia)); memset(propagate.Inv_Inertia_cov, 0, sizeof(propagate.Inv_Inertia_cov)); memset(batch_result.CM_offset, 0, sizeof(batch_result.CM_offset)); memset(batch_result.CM_offset_cov, 0, sizeof(batch_result.CM_offset_cov)); memset(batch_result.Inv_Inertia, 0, sizeof(batch_result.Inv_Inertia)); memset(batch_result.Inv_Inertia_cov, 0, sizeof(batch_result.Inv_Inertia_cov)); memset(batch_one.omega, 0, sizeof(batch_one.omega)); memset(batch_one.omega_dot, 0, sizeof(batch_one.omega_dot)); memset(batch_one.R_acc, 0, sizeof(batch_one.R_acc)); batch_one.thrust = 0; memset(batch_two.omega, 0, sizeof(batch_two.omega)); memset(batch_two.omega_dot, 0, sizeof(batch_two.omega_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(recursive_one.omega_dot)); memset(recursive_one.R_acc, 0, sizeof(recursive_one.R_acc)); recursive_one.thrust = 0; mass_ID_Init(); break; case 3: // download 1 kHz data ctrlPeriodSet(50); break; default: current_buffer = NULL;
132
BufferPos = current_buffer; void gspInitTask() taskTriggerMaskSet(PADS_INERTIAL_TRIG); void gspTaskRun(unsigned int gsp_task_trigger, unsigned int extra_data) // 1 kHz sampling variables float cur_time; unsigned int test_num; // Filtering Variables float x_input; const float Acoeff_gyro[3] = 1.0f, 0.6498f, 0.1056f; const float Bcoeff_gyro[3] = 0.5975f, 0.6498f, 0.5081f; // Test 3 is data transfer test_num = ctrlTestNumGet(); if (test_num == 3) return; / // Start off test resetting KF if (test_started) reset_KF((float *) current_gyro, (float *) gsp_omega_dot_minus); test_started = 0; new_thrust = propGetCurrentThrusters(); x_input = current_gyro[2]; //TEMP!!, just filtering z gyro for now if (new_thrust != 0) //If thruster(s) is on if (new_thrust != past_thrust) //If thruster(s) just turned on down_count = 1; // =10 for 1 kHz
133
num_firings++; down_count--; if (down_count == 0) //If 10 ms has passed to avoid transient for (k=0;k<3;k++) start_gyro[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 (for all vector copies)? last_omega_dot[k] = gsp_omega_dot[k]; 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 *) current_gyro, (float *) gsp_omega_dot, (float *) gsp_omega_dot_minus, (float *) gsp_R_acc); if (num_firings >= 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
134
propagate = mass_ID_RLS(propagate, recursive_one); else //Thrusters are off memset(gsp_omega_dot,0,sizeof(gsp_omega_dot)); if (new_thrust != past_thrust) //Thruster(s) just turned off switch (num_firings) //Need data from two different firings to start of mass_id case 0: break; case 1: for (k=0;k<3;k++) batch_one.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=0;k<3;k++) batch_two.omega_dot[k] = last_omega_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;
135
past_thrust = new_thrust; //Range check. Make sure that we don't record too many samples. if (current_buffer) // if ((BufferPos - current_buffer) < (SHORTS_PER_SAMPLE * SAMPLES_PER_SECOND * TEST_DURATION * sizeof(short))) /* if ((BufferPos - current_buffer) < (sample_bufsize >> 2) ) //Fill in the values for this sample cur_time = (float) sysGetSpheresTime(); BufferPos[0] = 7.0f; BufferPos[1] = 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[0][0]; BufferPos[5] = gyro[0][1]; BufferPos[6] = gyro[0][2]; BufferPos[7] = propGetCurrentThrusters(); BufferPos += FLOATS_PER_SAMPLE; */ data_num++; if (data_num >= 20) data_num = 0; void gspControl(unsigned int test_number, unsigned int test_time, unsigned int maneuver_number, unsigned int maneuver_time) // FastIMU variables static unsigned int started_flag = 0; static float *buffer_pos; static unsigned int last_length = 0;
136
// Control variables for mixer const unsigned int min_pulse = 20; static float defaultState[13] = 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f; float command[6] = 0.0f, 0.2f, 0.0f, 0.0f, 0.0f, 0.0f; float correct_torque[3]; prop_time firing_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; last_length = (sample_bufsize >> 2); switch (maneuver_number) case 0: case 1: DoStandardManeuver(1,1000); ctrlManeuverNumSet(2); break; case 2: DoStandardManeuver(12,1000); ctrlManeuverNumSet(3); break; case 3: DoStandardManeuver(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:
137
DoStandardManeuver(1,1000); ctrlManeuverNumSet(8); break; case 8: DoStandardManeuver(12,1000); ctrlManeuverNumSet(9); break; case 9: DoStandardManeuver(1,1000); ctrlManeuverNumSet(10); break; case 10: DoStandardManeuver(4,1000); ctrlManeuverNumSet(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: DoStandardManeuver(5,1000); ctrlManeuverNumSet(15); break; case 15: DoStandardManeuver(1,1000); ctrlManeuverNumSet(16); break; case 16: DoStandardManeuver(4,1000); ctrlManeuverNumSet(17); break; case 17: DoStandardManeuver(1,1000); ctrlManeuverNumSet(22); break; case 18: DoStandardManeuver(12,1000); ctrlManeuverNumSet(19);
138
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:
139
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[2] = 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: started_flag = 0; last_length = (sample_bufsize >> 2); current_cycle++; if (current_cycle == 2) ctrlMixSimple(command, min_pulse, defaultState); if (current_cycle == 4)
140
mathMatVecMult((float *) correct_torque, (float **) I_nom, (float *) last_omega_dot, 3, 3); for (k=3;k<5;k++) command[k] = command[k] - correct_torque[k-3]; ctrlMixSimpleCorrect(command, min_pulse, defaultState); if (current_cycle > 5) ctrlTestTerminate(TEST_RESULT_NORMAL); break; case 3: //Send the data buffer over the comm if (!started_flag) started_flag = 1; buffer_pos = sample_buffer; else // copy the data from the sample buffer into a packet, two samples at a time memcpy(pkt,buffer_pos,sizeof(default_rfm_payload)); // increment memory pointer by two samples (16 shorts) buffer_pos += 8; // put the packet in the STL queue commSendPacket(COMM_CHANNEL_STS,GROUND,sysIdentityGet(),COMM_CMD_DBG_FLOAT,(unsigned char *) pkt,0); // once all data is sent, stop transmissions if ((buffer_pos-sample_buffer) >= last_length) started_flag = 0; ctrlTestTerminate(TEST_RESULT_NORMAL); break; default: break;
141
E.2 Angular Acceleration Kalman Filter (est_ang_acc.c)
#include "est_ang_acc.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 "pads.h" #include <string.h> #include "util_serial_printf.h" //#include "mass_id_constants.h" // Declarations const float dt = 0.01f; float I_nom[3][3] = 0.0229f, 0.0000965f, -0.000293f , 0.0000965f, 0.0242f, -0.0000311f, -0.000293f, -0.0000311f, 0.0214f , ; const float I_inv[3][3] = 43.6765f, -0.1734f, 0.5977f, -0.1734f, 41.3231f, 0.0577f, 0.5977f, 0.0577f, 46.7372f, ; const float F_nom[12] = 0.12579f, 0.13153f, 0.13530f, 0.13568f, 0.12788f, 0.12956f, 0.12096f, 0.13180f, 0.13593f, 0.14065f, 0.12774f, 0.12620f; static int part_index[3] = 0; static float even_odd; static float P_inv_minus[3][3] = 0.0f; const float R_inv[3][3] =
142
7.2307e4f, 0.0f, 0.0f , 0.0f, 1.5119e4f, 0.0f , 0.0f, 0.0f, 3.1776e4f, ; unsigned int update_num; static float bias[3] = 0.0f; static float h[3] = 0.0f; static float P[3][3] = 0.0f; static float P_inv[3][3] = 0.0f; int i, j; // Functions void reset_KF(float start_rate[3], float *omega_dot_minus) unsigned short thruster_quant; static float Fk[12] = 0.0f; const float 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.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.000177560f, ; static float ind_torque[3] = 0.0f; unsigned short thrusters; //Fudged number update_num = 0; thruster_quant = 0; thrusters = propGetCurrentThrusters(); for (i=0;i<12;i++) thruster_quant = thruster_quant + ((thrusters>>i) & 0x0001); for (i=0;i<12;i++) if (thrusters & (1<<i))
143
Fk[i] = (F_nom[i] - (thruster_quant - 1)*0.0106f); else Fk[i] = 0.0f; ////////////////////////////// //To get L_cross_D, DO THIS:// ////////////////////////////// /* for (i=0;i<12;i++) L_cross_D[0][i] = L[1][i]*D[2][i] - L[2][i]*D[1][i]; L_cross_D[1][i] = L[2][i]*D[0][i] - L[0][i]*D[2][i]; L_cross_D[2][i] = L[0][i]*D[1][i] - L[1][i]*D[0][i]; */ ///////////////// //OR USE CONST:// ///////////////// memset(part_index,0,sizeof(part_index)); memset(omega_dot_minus,0,sizeof(omega_dot_minus)); mathMatVecMult(ind_torque, (float **) L_cross_D, Fk, 3, 12); //////////////////////////////////// //To get omega_dot_minus, DO THIS:// //////////////////////////////////// /* for (i=0;i<3;i++) for (j=0;j<3;j++) I_temp[i][j] = I_nom[i][j]; ludcmp((float **) I_temp, 3, (int *) part_index, &even_odd); lubksb((float **) I_temp, 3, (int *) part_index, ind_torque);
144
for (i=0;i<3;i++) omega_dot_minus[i] = ind_torque[i]; */ //////////// //OR THIS:// //////////// mathMatVecMult(omega_dot_minus, (float **) I_inv, ind_torque, 3, 3); for (i=0;i<3;i++) for (j=0;j<3;j++) P_inv_minus[i][j] = 0.0f; P_inv_minus[i][i] = 10.0f; 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) //Update KF Variables static float H[3][3] = 0.0f; static float P_inv_temp[3][3] = 0.0f; static float Mat_Temp1[3][3] = 0.0f; static float Mat_Temp2[3][3] = 0.0f; static float K[3][3] = 0.0f; static float ang_vel[3] = 0.0f; static float Vec_Temp1[3] = 0.0f; static float Vec_Temp2[3] = 0.0f; static float Vec_Temp3[3] = 0.0f; static float Vec_Temp4[3] = 0.0f; //Prop Variables float phi = 1.0f;
145
update_num++; for (i=0;i<3;i++) for (j=0;j<3;j++) H[i][j] = 0.0f; H[i][i] = update_num*dt; mathMatTransposeMatMult((float **) Mat_Temp1, (float **) H, (float **) R_inv, 3, 3, 3); mathMatMatMult((float **) Mat_Temp2, (float **) Mat_Temp1, (float **) H, 3, 3, 3); mathMatAdd((float **) P_inv, (float **) P_inv_minus, (float **) Mat_Temp2, 3, 3); for (i=0;i<3;i++) for (j=0;j<3;j++) P_inv_temp[i][j] = P_inv[i][j]; ludcmp((float**) P_inv_temp, 3,(int*) part_index, &even_odd); for (i=0;i<3;i++) for (j=0;j<3;j++) h[j] = 0.0f; h[i] = 1.0f; lubksb((float**) P_inv_temp, 3, (int*) part_index, h); for (j=0;j<3;j++) P[j][i] = h[j]; R_acc[i] = P[i][i]; mathMatMatMult((float **) K, (float **) P, (float **) Mat_Temp1, 3, 3, 3); mathMatVecMult((float *) Vec_Temp1, (float **) H, (float *) omega_dot_minus, 3, 3); mathVecAdd((float *) Vec_Temp2, (float *) bias, (float *) Vec_Temp1, 3);
146
for (i=0;i<3;i++) Vec_Temp2[i] = -Vec_Temp2[i]; mathVecAdd((float *) Vec_Temp3, (float *) current_rate, (float *) Vec_Temp2, 3); mathMatVecMult((float *) Vec_Temp4, (float **) K, (float *) Vec_Temp3, 3, 3); mathVecAdd((float *) omega_dot, (float *) omega_dot_minus, (float *) Vec_Temp4, 3); //Propagate Step for (i=0;i<3;i++) for (j=0;j<3;j++) P_inv_minus[i][j] = P_inv[i][j]; for (i=0;i<3;i++) omega_dot_minus[i] = phi*omega_dot[i]; void prop_KF(float *omega_dot, float *omega_dot_minus) //Propagate KF 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"
147
#include "math_lubksb.h" #include "math_ludcmp.h" #include "comm.h" #include "commands.h" #include "spheres_constants.h" #include "system.h" #include "spheres_types.h" #include "util_serial_printf.h" dbg_float_packet DebugVec1, DebugVec2, DebugVec3; float L[3][12] = 0.0f; float cm_nom[3] = 0.0f; unsigned int count, index; float Ck[6], Ak[6]; float C[3][3] = 0.0f; float A1[6][3] = 0.0f, A2[6][6] = 0.0f; float B1[6] = 0.0f, B2[6] = 0.0f; static int part_index1[3] = 0; static int part_index2[6] = 0; static float even_odd; static float h1[3] = 0.0f; static float h2[6] = 0.0f; int m,n; void mass_ID_Init() for (m=0;m<3;m++) for (n=0;n<12;n++) L[m][n] = L_nom[m][n] - cm_nom[m]; memset(A1,0,sizeof(A1)); memset(A2,0,sizeof(A2)); mass_ID_output mass_ID_Batch(mass_ID_input batch_one, mass_ID_input batch_two) float A1_top[3][3] = 0.0f, A1_bot[3][3] = 0.0f;
148
float A2_top[3][6] = 0.0f, A2_bot[3][6] = 0.0f; float B1_top[3] = 0.0f, B1_bot[3] = 0.0f, B2_top[3] = 0.0f, B2_bot[3] = 0.0f; float Mat_Temp1[3][3] = 0.0f, Mat_Temp2[3][3] = 0.0f; float Mat_Temp3[3][6] = 0.0f; float Mat_Temp4[6][6] = 0.0f, Mat_Temp5[6][6] = 0.0f; float Mat_Temp6[6][6] = 0.0f; float R_batch[6][6] = 0.0f; float A1_temp[3][6] = 0.0f; mass_ID_output batch_result; // Get LS Equations get_LS_equations(batch_one, (float **) A1_top, (float **) A2_top, (float *) B1_top, (float *) B2_top); get_LS_equations(batch_two, (float **) A1_bot, (float **) A2_bot, (float *) B1_bot, (float *) B2_bot); // Combine Matrices for (m=0;m<3;m++) for (n=0;n<3;n++) A1[m][n] = A1_top[m][n]; for (n=0;n<6;n++) A2[m][n] = A2_top[m][n]; B1[m] = B1_top[m]; B2[m] = B2_top[m]; for (m=3;m<6;m++) for (n=0;n<3;n++) A1[m][n] = A1_bot[m-3][n]; for (n=0;n<6;n++) A2[m][n] = A2_bot[m-3][n]; B1[m] = B1_bot[m-3]; B2[m] = B2_bot[m-3];
149
for (m=0;m<3;m++) for (n=0;n<6;n++) A1_temp[m][n] = A1[n][m]; // Perform batch memset(Mat_Temp1, 0, sizeof(Mat_Temp1)); // CM offset mathMatMatMult((float **) Mat_Temp1, (float **) A1_temp, (float **) A1, 3, 6, 3); ludcmp((float**) Mat_Temp1, 3,(int*) part_index1, &even_odd); for (m=0;m<3;m++) for (n=0;n<3;n++) h1[n] = 0.0f; h1[m] = 1.0f; lubksb((float**) Mat_Temp1, 3, (int*) part_index1, h1); for (n=0;n<3;n++) Mat_Temp2[n][m] = h1[n]; mathMatMatTransposeMult((float **) Mat_Temp3, (float **) Mat_Temp2, (float **) A1, 3, 3, 6); mathMatVecMult(batch_result.CM_offset, (float **) Mat_Temp3, (float *) B1, 3, 6); mathMatTransposeMatMult((float **) Mat_Temp4, (float **) A2, (float **) A2, 6, 6, 6); ludcmp((float**) Mat_Temp4, 6,(int*) part_index2, &even_odd); for (m=0;m<6;m++) for (n=0;n<6;n++) h2[n] = 0.0f; h2[m] = 1.0f;
150
lubksb((float**) Mat_Temp4, 6, (int*) part_index2, h2); for (n=0;n<6;n++) Mat_Temp5[n][m] = h2[n]; mathMatMatTransposeMult((float **) Mat_Temp6, (float **) Mat_Temp5, (float **) A2, 6, 6, 6); mathMatVecMult(batch_result.Inv_Inertia, (float **) Mat_Temp6, (float *) B2, 6, 6); // Covariances memset(R_batch, 0, sizeof(R_batch)); for (m=0;m<3;m++) R_batch[m][m] = batch_one.R_acc[m]; R_batch[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=0;n<6;n++) h2[n] = 0.0f; h2[m] = 1.0f; 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 **) A1, (float **) Mat_Temp5, 6, 3, 6); mathMatMatMult((float **) batch_result.CM_offset_cov, (float **) Mat_Temp3, (float **) A1, 3, 6, 3); mathMatTransposeMatMult((float **) Mat_Temp4, (float **) A2, (float **) Mat_Temp5, 6, 6, 6); mathMatMatMult((float **) batch_result.Inv_Inertia_cov, (float **) Mat_Temp4, (float **) A2, 6, 6, 6);
151
return batch_result; mass_ID_output mass_ID_RLS(mass_ID_output minus, mass_ID_input recursive_one) float A1rls[3][3] = 0.0f; float A2rls[3][6] = 0.0f; float B1rls[3] = 0.0f; float B2rls[3] = 0.0f; float Qcm[3][3] = 0.0f; float Qcm_inv[3][3] = 0.0f; float Qcm_inv_minus[3][3] = 0.0f; float QI[6][6] = 0.0f; float QI_inv[6][6] = 0.0f; float QI_inv2[6][6] = 0.0f; static float QI_inv3[6][6] = 0.0f; float QI_inv_minus[6][6] = 0.0f; float R[3][3] = 0.0f; float Mat_Temp1[3][3] = 0.0f; float Mat_Temp2[3][3] = 0.0f; float Mat_Temp3[3][3] = 0.0f; float Mat_Temp4[6][3] = 0.0f; float Mat_Temp5[6][6] = 0.0f; float Mat_Temp6[6][3] = 0.0f; float Vec_Temp1[3] = 0.0f; float Vec_Temp2[3] = 0.0f; float Vec_Temp3[3] = 0.0f; float Vec_Temp4[6] = 0.0f; int s, t; static mass_ID_output propagate; get_LS_equations(recursive_one, (float **) A1rls, (float **) A2rls, (float *) B1rls, (float *) B2rls); memset(R, 0, sizeof(R)); for (s=0;s<3;s++) R[s][s] = recursive_one.R_acc[s]; ludcmp((float**) R, 3,(int*) part_index1, &even_odd); for (s=0;s<3;s++)
152
for (t=0;t<3;t++) h1[t] = 0.0f; h1[s] = 1.0f; lubksb((float**) R, 3, (int*) part_index1, h1); for (t=0;t<3;t++) Mat_Temp1[t][s] = h1[t]; memcpy(Qcm_inv_minus, minus.CM_offset_cov, sizeof(minus.CM_offset_cov)); memcpy(QI_inv_minus, minus.Inv_Inertia_cov, sizeof(minus.Inv_Inertia_cov)); mathMatTransposeMatMult((float **) Mat_Temp2, (float **) A1rls, (float **) Mat_Temp1, 3, 3, 3); mathMatMatMult((float **) Mat_Temp3, (float **) Mat_Temp2, (float **) A1rls, 3, 3, 3); mathMatAdd((float **) Qcm_inv, (float **) Qcm_inv_minus, (float **) Mat_Temp3, 3, 3); for (s=0;s<3;s++) for(t=0;t<3;t++) propagate.CM_offset_cov[s][t] = Qcm_inv[s][t]; ludcmp((float**) Qcm_inv, 3,(int*) part_index1, &even_odd); for (s=0;s<3;s++) for (t=0;t<3;t++) h1[t] = 0.0f; h1[s] = 1.0f; lubksb((float**) Qcm_inv, 3, (int*) part_index1, h1); for (t=0;t<3;t++) Qcm[t][s] = h1[t];
153
mathMatTransposeMatMult((float **) Mat_Temp4, (float **) A2rls, (float **) Mat_Temp1, 3, 6, 3); mathMatMatMult((float **) Mat_Temp5, (float **) Mat_Temp4, (float **) A2rls, 6, 3, 6); mathMatAdd((float **) QI_inv, (float **) QI_inv_minus, (float **) Mat_Temp5, 6, 6); for (s=0;s<6;s++) for(t=0;t<6;t++) propagate.Inv_Inertia_cov[s][t] = QI_inv[s][t]; QI_inv2[s][t] = QI_inv[s][t]; QI_inv3[s][t] = QI_inv[s][t]; ludcmp((float**) QI_inv, 6,(int*) part_index2, &even_odd); for (s=0;s<6;s++) for (t=0;t<6;t++) h2[t] = 0.0f; h2[s] = 1.0f; lubksb((float**) QI_inv, 6, (int*) part_index2, h2); for (t=0;t<6;t++) QI[t][s] = h2[t]; mathMatVecMult((float *) Vec_Temp1, (float **) A1rls, (float *) minus.CM_offset, 3, 3); for (s=0;s<3;s++) Vec_Temp2[s] = B1rls[s] - Vec_Temp1[s]; mathMatMatMult((float **) Mat_Temp3, (float **) Qcm, (float **) Mat_Temp2, 3, 3, 3); mathMatVecMult((float *) Vec_Temp3, (float **)Mat_Temp3, (float *) Vec_Temp2, 3, 3); mathVecAdd((float *) propagate.CM_offset, (float *) minus.CM_offset, (float *) Vec_Temp3, 3);
154
mathMatVecMult((float *) Vec_Temp1, (float **) A2rls, (float *) minus.Inv_Inertia, 3, 6); for (s=0;s<3;s++) Vec_Temp2[s] = B2rls[s] - Vec_Temp1[s]; mathMatMatMult((float **) Mat_Temp6, (float **) QI, (float **) Mat_Temp4, 6, 6, 3); mathMatVecMult((float *) Vec_Temp4, (float **)Mat_Temp6, (float *) Vec_Temp2, 6, 3); mathVecAdd((float *) propagate.Inv_Inertia, (float *) minus.Inv_Inertia, (float *) Vec_Temp4, 6); return propagate; void get_LS_equations(mass_ID_input measurement, float **a1, float **a2, float *b1, float *b2) unsigned short thruster_quant; static float Fk[12] = 0.0f; const float 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.000177560f, ; unsigned short thrusters; //Fudged number const float I_inv[3][3] = 43.6765f, -0.1734f, 0.5977f, -0.1734f, 41.3231f, 0.0577f, 0.5977f, 0.0577f, 46.7372f, ; float alpha[3]; float VecTemp[3] = 0.0f; thruster_quant = 0; thrusters = measurement.thrust; memcpy(alpha,measurement.omega_dot,sizeof(measurement.omega_dot));
155
for (n=0;n<12;n++) thruster_quant = thruster_quant + ((thrusters>>n) & 0x0001); for (n=0;n<12;n++) if (thrusters & (1<<n)) Fk[n] = (F_mag_Default[n] - (thruster_quant - 1)*0.0106f); // TEMP use of F_mag_Default else Fk[n] = 0.0f; // 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 of L_cross_D_Default...need to take Cg into account mathSkewSymmetric (Ck, (float *) C); mathMatMatMult((float **) a1, (float **) I_inv, (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++) b1[n] = alpha[n] - VecTemp[n]; // Create Inverse Inertia RLS equations ((float *)a2)[0*6+0] = Ak[0]; ((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[0]; ((float *)a2)[2*6+5] = Ak[1]; for (n=0;n<3;n++) b2[n] = alpha[n];
