Real-time Mass Property Estimation
by
Andrew M. Wright
Submitted to the Department of Mechanical Engineering
in partial fulfillment of the requirements for the degree of
Master of Science in Mechanical Engineering
at the
MASSACHUSETTS INSTITUTE OF TECHNOLOGY
May 2000
@Andrew Wright, 2000. The author hereby grants to M.I.T. and to
Lawrence Livermore National Laboratory permission to reproduce
and to distribute copies of this thesis document in whole or in part.
MASSACHUSETTS INSTITUTE
OF TECHNOLOGY
SEP 2 0 2000
Author .....
......................
..........
.L1BRARIF.,..
Department of Mechanical Engineering
May 18, 2000
Certified by...
Kamal Youcef-Toumi
Professor of Mechanical Engineering
Thesis Supervisor
Certified by...
I
U
Dr. Lawrence C. Ng
LLNL supervisor
Thesis Supervisor
-----..-----.
Ain A. Sonin
Chairman, Department Committee on Graduate Students
Accepted by ......
Real-time Mass Property Estimation
by
Andrew M. Wright
Submitted to the Department of Mechanical Engineering
on May 18, 2000, in partial fulfillment of the
requirements for the degree of
Master of Science in Mechanical Engineering
Abstract
In this thesis, we designed and implemented a mass property estimation algorithm to
be executed autonomously by space-borne satellites. Implemented solely in code, the
estimation algorithm uses recursive least squares with a forgetting factor to converge
running mass property estimates to the true values for the satellite alone, or in the
case of a docking mission, the rigidly connected body. A six degree-of-freedom dynamic simulation of the satellite was written to test the algorithm, which was then
corroborated experimentally on a five degree-of-freedom microsatellite test bed. A
method for estimating the mass properties of the docked object alone was demonstrated.
Thesis Supervisor: Kamal Youcef-Toumi
Title: Professor of Mechanical Engineering
Thesis Supervisor: Dr. Lawrence C. Ng
Title: LLNL supervisor
3
4
Acknowledgments
First, I would like to extend my thanks and gratitude to Professor Kamal YoucefToumi for overseeing and guiding my research. His suggestions helped me academically and personally through this thesis. Also, I am indebted to Dr. Larry Ng for his
mentorship and guidance during my internship at the Lawrence Livermore National
Laboratories, without whom this unique research opportunity would not be possible.
I would also like to thank everyone on the Microsat team at LLNL for their
technical, moral, and financial support. Arno Ledebuhr, Microsat program leader,
Joe Kordas, deputy program leader, and Gloria Purpura, administrative specialist
have been instrumental in assuring all the details for allowing me to intern in the
Microsat program were taken care of. I am especially grateful to Eric Breitfeller, for
sharing his expertise and friendship with me. I could not imagine how lost I would be
without his assistance. Special thanks go to those who spent countless hours helping
me with the experimental test runs: Bruce Wilson, the Microsat software guru, Jeff
Robinson, and Don Antelman, the hardware and manufacturing crew, you have my
deepest respect. Their willingness to devote their personal time after-hours allowed a
most critical step in this research to be accomplished. I am grateful and proud to have
worked with all of the extraordinary people on the LLNL Microsat team. Also, special
thanks go to Bob Langland and Ronna Oelrich who oversaw each of my internships
at LLNL and guided me safely through the many inevitable administrative hurdles.
Finally, I would like to thank Fermin Garcia for his assistance in the early phase of
my work with the Microsat group and in the writing of this thesis. Considering his
technical skill and enthusiasm, it is inconceivable that his future could be other than
bright.
I can not thank my parents and sister, Robert, Sanae, and Sharon Wright, enough
for their ceaseless and caring support. Though miles away, they were always ready
to lend an attentive ear, a warm laugh, and a helping hand.
At a time when academics threatened to overwhelm me, she was there to assure
me that life is indeed richer than my rigorous, and engrossing curriculum at MIT.
5
She fed me, helped me with my studies, and nursed me when sick while at the same
time tackling her own studies and inspiring me with her fresh outlook. Emily, I'm all
yours.
6
Contents
1
2
1.1
B ackground . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
19
1.2
O verview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
20
Dynamic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
23
Overview of Inertia . . . . . . . . . . . . . . . . . . . . . . . .
24
Essential Microsat Hardware . . . . . . . . . . . . . . . . . . . . . . .
27
Overview of Pulse Width Modulation . . . . . . . . . . . . . .
28
Space-based Operational Procedure . . . . . . . . . . . . . . . . . . .
30
2.1.1
2.2
2.2.1
2.3
31
Mass Property Estimation Method
3.1
Recursive Least Squares
. . . . . . . . . . . . . . . . . . . . . . . . .
31
3.2
Estimate Convergence and Observability . . . . . . . . . . . . . . . .
33
. . . . . . . . . . . . . . . . . . . . .
33
3.2.1
4
23
Description of the Microsat
2.1
3
19
Introduction
Observability Condition
37
Single Body Scenario
4.1
Algorithm Structure
. . . . . .
. . . . . . . . . . . . . . . . . . . .
37
4.2
Single Body Simulations . . . .
. . . . . . . . . . . . . . . . . . . .
41
4.3
4.2.1
Simulation Description
.... .... ..... ... ....
41
4.2.2
Preliminary Simulation
... ..... .... .... ....
41
4.2.3
Refined Simulation . . .
.. ..... ..... .... ... .
52
. . . . . . . . . . . . . . . . . . . . 60
Experimental Setup . . . . . . .
7
4.4
Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . .
63
4.5
Comparison of Simulation and Experiment . . . . . . . . . . . . . . .
72
5 Multiple Body Scenario
73
5.1
Algorithm Structure
. . . . . . . . . . . . . . . . . . . . . . . .
73
5.2
Multiple Body Simulation . . . . . . . . . . . . . . . . . . . . .
74
5.2.1
Simulation Description . . . . . . . . . . . . . . . . . . .
74
5.2.2
Multiple Body Simulation Results . . . . . . . . . . . . .
77
Experimental Proposal . . . . . . . . . . . . . . . . . . . . . . .
89
5.3
6 Conclusions and Recommendations for Future Work
91
6.1
C onclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
91
6.2
Recommendations for Future Work . . . . . . . . . . . . . . . .
92
A Real-time Estimation Code Integration and Raw Trial Data
95
B Initial Covariance Matrix Determination
99
B.1
Covariance Matrices Used in Simulations
. . . . . . . . . . . . . .
99
B.2 Effects from Variation in Initial P . . . . . . . . . . . . . . . . . . . .
103
B.3 Effects from Variation in Initial
Q
. . . . . . . . . . . . . . . . . . . .
106
B.4 Effects from Variation in Initial R . . . . . . . . . . . . . . . . . . . . 109
C Simulation Code
C.1 Matlab Docking Simulation
113
. . . . . . . . . . . . . . . . . . . . . . .
113
C.1.1
Initialization and Support Functions
. . . . . . . . . . . . . .
113
C.1.2
Docking Code . . . . . . . . . . . . . . . . . . . . . . . . . . .
119
C.1.3
Matlab Plotting Functions . . . . . . . . . . . . . . . . . . . .
128
8
List of Figures
2-1
Diagram illustrating the orientation of the body-fixed axes with respect
to the m icrosat. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2-2
Diagram and equation of combined inertia as a function of the inertia
of the respective objects [7]. . . . . . . . . . . . . . . . . . . . . . . .
2-3
24
26
CAD model of prototype LLNL microsat showing essential hardware
components: Inertial Measurement Unit, Central Processing Unit, and
Propulsion system . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2-4
27
Approximation of desired force constrained to actual jet thrust value,
. . . . . .
29
4-1
Timeline of the mission showing all control actions. . . . . . . . . . .
42
4-2
Simulation 1: Control torques on each axis resulting from execution
Fjet,
with variable pulse duration, p, over each duty cycle.
of the mission plan. Though all the torque commands are perfectly
balanced with opposite commands during the open loop excitations,
large control torques are evident when attempting to return to initial
orientation, suggesting large deviations in orientation due to inter-axis
torque disturbances. The oscillations at 2, 6, and 10 seconds show the
open loop control roll, pitch, and yaw, respectively to induce parameter
convergence........................................
9
46
4-3
Simulation 1: Three axis moment of inertia (MOI) estimate plot showing the convergence from initial moment estimates of zero to the true
moment represented by the horizontal line. The "precision" boundary
for each plot is the 'true' value ± Pkk with k = 1 for X-axis MOI,
k = 4 for Y-axis MOI, k = 6 for Z-axis MOI. The "precision" remains nearly constant from 12 to 20 seconds because there is no direct
excitation of these modes during that period . . . . . . . . . . . . . .
4-4
47
Simulation 1: Three inter-axis product of inertia (POI) estimate plot
showing the convergence from zero initial values to the true product
represented by the horizontal line. The "precision" boundary for each
plot is the 'true' value ±Pkk with k = 2 for X-Y POI, k = 3 for
X-Z P01, k = 5 for Y-Z POI. Though tapering significantly at the
beginning of the simulation, the "precision" envelopes remain wide at
the end of the simulation from lack of direct excitation, which indicates
the current residual error is more significant than previous residual
error when calculating the current estimate.
4-5
. . . . . . . . . . . . . .
48
Diagram showing uncertainty in CG location based on diverts only in
Y direction. There is no difference in dynamics as the CG location
varies along the Y-axis. . . . . . . . . . . . . . . . . . . . . . . . . . .
4-6
49
Simulation 1: Center of gravity (CG) location estimate with respect
to the initial CG location corresponding to the focus of the divert
thrusters on the microsat. The true CG deviation from the focus in
Simulation 1 is 0.1 m in X, Y, and Z directions. The CG location in the
Y direction is unobservable, and thus, there is no change from initial
values. (see Figure 4-5). The "precision" boundary for each plot is the
'true' value ±Pkk with k = 7 for CG along X-axis, k
=
8 for CG
along Y-axis, k = 9 for CG along Z-axis. . . . . . . . . . . . . . . . .
10
50
4-7
Simulation 1: Commanded force plot showing the initial divert away
from the bay, and the divert oscillations at 16 seconds. Simulation
1 contained diverts in the Y (lateral) direction only. Plot (b) shows
the estimated total mass for the microsat in Simulation 1 revealing
correlation between mass convergence and command force excitations.
Since the mass estimate is filtered separately from the other estimates,
the "precision" boundary is only the 'true' value ±/f2. . . . . . . . .
4-8
51
Simulation 2: Open loop control torques about three axes. Periods
of open loop excitation of roll, pitch, and yaw axes start at 2, 6, 10
seconds, respectively. The remaining time was devoted to returning the
microsat to the initial orientation, then holding attitude using closed
loop control. The isolated spikes in each plot are the small torques to
return the microsat to initial orientation which are executed when each
closed loop control period begins. . . . . . . . . . . . . . . . . . . . .
4-9
55
Simulation 2: Time plots of the moment of inertia parameter estimation about the three body-fixed axes. The inertia estimates are initialized at zero, then converge on the true value shown as the horizontal
line. The "precision" boundary for each plot is the 'true' value ±
Pkk
with k = 1 for X-axis MOI, k = 4 for Y-axis MOI, k = 6 for Z-axis MOI. 56
4-10 Simulation 2: Time plots of the product of inertia parameters. The
product of inertia estimates are initialized at zero, then converge on
the true value shown as the horizontal line. The "precision" boundary
for each plot is the 'true' value ±Pkk with k = 2 for X-Y POI, k = 3
for X-Z POI, k = 5 for Y-Z POI.
. . . . . . . . . . . . . . . . . . . .
11
57
4-11 Simulation 2: Time plot of center of gravity location estimation in
the X, Y, Z body-fixed inicrosat axes. The "precision" boundary for
each plot is the 'true' value ±
Pkk with k = 7 for CG along X-axis,
k = 8 for CG along Y-axis, k = 9 for CO along Z-axis. The Y-axis CG
location is unobservable as shown in Figure (4-5), thus the "precision"
bounds do not converge. The "precision" bounds on the X and Z axis
CG location estimates taper initially due to the initial divert away from
the docking bay, then after a period of inactivity in divert excitations,
the bounds taper again at 16 seconds due to the open loop excitations.
58
4-12 Simulation 2: Time plot of open loop Y-axis divert thrusts and time
plot of total mass estimation. The convergence in the first two seconds
and at 16 seconds corresponds to the open loop Y-axis divert thrusts.
The lack of divert thrusts between 2 and 16 seconds lowers the signal
to noise ratio, thereby causing the significant divergence in the mass
estimate. Since the mass estimate is filtered separately from the other
estimates, the "precision" boundary is only the 'true' value ± P2 . . .
59
4-13 Microsat experimental test bed at Lawrence Livermore National Laboratories. The microsat shown above is moving freely in five degrees of
freedom. The spherical air bearing at the top of the support structure
allows all three rotational degrees of freedom and the support structure
itself slides freely in two degrees of freedom on the plane of the glass
tab le. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
60
4-14 Combined Linear and Spherical Air Bearing. . . . . . . . . . . . . . .
61
4-15 Spherical Air Bearing location at CG of vehicle. . . . . . . . . . . . .
61
4-16 Timing Schematic for Microsat Prototype.
63
4-17 Experimental Trial:
. . . . . . . . . . . . . . .
Control torques requested by the microsat con-
troller. As in the simulations, the isolated spikes are caused by small
attitude corrections when each closed loop attitude control period begins. 66
12
4-18 Experimental Trial: Moment of inertia estimates: the "precision" boundary for each plot is the current estimate ± Pkkwith k = 1 for X-axis
MOI, k = 4 for Y-axis MOI, k = 6 for Z-axis MOI.
4-19 Experimental Trial: Product of inertia estimates:
. . . . . . . . . .
67
The "precision"
boundary for each plot is the current estimate ± Pkk with k = 2
for X-Y POI, k = 3 for X-Z POI, k = 5 for Y-Z POI. . . . . . . . . .
68
4-20 Experimental Trial: Center of gravity deviation estimates: The "precision" boundary for each plot is the current estimate t
Pkkwith k = 7
for CG along X-axis, k = 8 for CG along Y-axis, k = 9 for CG along
Z -ax is. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
69
4-21 Experimental Trial: Command force plot showing the initial divert
away from the docking station and the divert excitations at 16 seconds. The second plot shows the corresponding mass estimate. The
mass estimate is filtered separately from the other estimates, thus, the
"precision" boundary is only the 'true' value ±fI.
. . . . . . . . . .
70
4-22 Experimental Trial: Observed Accelerations by the IMU on-board the
Microsat. The acceleration due to gravity is filtered from the Z acceleration plot, as are the centripetal, and Euler accelerations because
the IMU is located at a fixed distance from the axes of rotation. . . .
5-1
71
Diagram illustrating the three main control phases during the docking
mission. First, the Microsat executes a series of open loop rotations
and diverts (a), then docks with the space-borne object (b) and again,
executes a series of open loop rotations and diverts (c) to identify the
combined body mass parameters.
5-2
. . . . . . . . . . . . . . . . . . . .
75
Timeline of the multiple body docking mission detailing all control
actions.
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
13
76
5-3
Docking Simulation: Control torques on each axis resulting from execution of the docking mission plan. The closed loop control torques
to return the microsat to initial attitude occur at 8 seconds.
At 9
seconds, the mass properties of the microsat immediately change to
the combined body mass properties (docking), inducing large control
torque com m ands.
5-4
. . . . . . . . . . . . . . . . . . . . . . . . . . . .
78
Docking Simulation (0-9 Seconds): Three axis moment of inertia estimate plot showing the convergence from initial moment estimates of
zero to the true moment represented by the horizontal line. The "precision" boundary for each plot is the 'true' value ± Pkkwith k =1
for X-axis MOI, k = 4 for Y-axis MOI, k
5-5
=
6 for Z-axis MOI ... . . .
80
Docking Simulation (0-9 Seconds): Three inter-axis product of inertia estimate plot showing the convergence from zero initial values to
the true product represented by the horizontal line. The "precision"
boundary for each plot is the 'true' value ± Pkkwith k = 2 for X-Y
POI, k = 3 for X-Z POI, k = 5 for Y-Z POI. . . . . . . . . . . . . . .
5-6
81
Docking Simulation (0-9 Seconds): Center of gravity (CG) location
estimate with respect to the initial CG location corresponding to the
focus of the divert thrusters on the microsat. The "precision" boundary
for each plot is the 'true' value ± Pkkwith k = 7 for CG along X-axis,
k = 8 for CG along Y-axis, k = 9 for CO along Z-axis.
5-7
. . . . . . . .
82
Docking Simulation (0-9 Seconds): Commanded force plot showing no
activity until the divert oscillations at 6 seconds. The docking simulation contains diverts only in the Y (lateral) direction. The mass plot
shows the estimated total mass for the microsat alone before docking. Interestingly, the precision envelope begins to converge before
any divert activity due to noisy inputs from the accelerometer. Since
the mass estimate is filtered separately from the other estimates, the
"precision" boundary is only the 'true' value ±/I2. . . . . . . . . . .
14
83
5-8
Docking Simulation (9-36 Seconds): Three axis moment of inertia estimate plot showing the convergence from initial moment estimates of
zero to the true moment represented by the horizontal line. The Y
and Z axis MOI estimates are not within the precision bounds for two
reasons: the covariance matrix was not reinitialized at a high enough
value to account for the large estimate error, and until directly excited,
the low signal to noise ratio prevented the estimates from converging.
However, once excited, both the Y and Z axis MOI estimates converged
near the true values.
5-9
. . . . . . . . . . . . . . . . . . . . . . . . . . .
85
Docking Simulation (9-36 Seconds): Three inter-axis product of inertia
estimate plot showing the convergence from zero initial values to the
true product represented by the horizontal line.
. . . . . . . . . . . .
86
5-10 Docking Simulation (9-36 Seconds): Center of gravity location estimate
with respect to the initial CO location corresponding to the focus of
the divert thrusters on the microsat.
At 9 seconds, the overall CO
shifted along the X-axis toward the docked body. At 12 seconds, the
estimation precision was reinitialized and does not reconverge for the X
and Z CO location until the open loop Y-axis diverts for the combined
body at 24 seconds; the Y CO location precision never converges (See
F igure 4-5). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
87
5-11 Docking Simulation (9-36 Seconds): Commanded force plot showing
the Y-axis divert oscillations at 24 seconds. Plot (b) shows the estimated total mass for the combined body. The mass estimate actually
diverges initially since there are no excitations other than accelerometer
noise (which actually converge the precision envelope). At 24 seconds,
the divert oscillations induce good convergence of the mass estimate.
15
88
5-12 Proposed experimental setup: the CG location is external to both
bodies, thus allowing the ball which is fixed to the rigid connecting
bracket to be located between the two bodies. This experimental setup
allows full rotational freedom and translational freedom in the plane
of the glass table (5 degrees of freedom). . . . . . . . . . . . . . . . .
90
A-i Real-time moment of inertia estimate plots for previously untested microsat. ........
...................................
96
A-2 Real-time attitude plot for previously untested microsat. The three
plots, qPitch, qRoll, and qYaw are the first three terms of the microsat
quaternion which represents the axis of rotation at that time . . . . .
97
B-i Simulation 1: Three axis moment of inertia estimate plot showing good
convergence from initial moment estimates of zero to the true moment
represented by the horizontal line. The "precision" boundary for each
plot is the 'true' value ±Pkk with k = 1 for X-axis MOI, k = 4 for
Y-axis MOI, k = 6 for Z-axis MOI. The "precision" remains nearly
constant from 12 to 20 seconds because there is no direct excitation of
these modes during that period. . . . . . . . . . . . . . . . . . . . . .
102
B-2 Simulation 1: Three axis moment of inertia estimate plot with lower
initial P values (diagonal of 0.01's). . . . . . . . . . . . . . . . . . . .
104
B-3 Simulation 1: Three axis moment of inertia estimate plot with higher
initial P values (diagonal of 1000's). . . . . . . . . . . . . . . . . . . .
105
B-4 Simulation 1: Three axis moment of inertia estimate plot with lower
initial
Q values
(zero matrix).
. . . . . . . . . . . . . . . . . . . . . .
107
B-5 Simulation 1: Three axis moment of inertia estimate plot with higher
initial
Q values
(diagonal of I X 10-2S).
. . . . . . . . . . . . . . . .
108
B-6 Simulation 1: Three axis moment of inertia estimate plot with lower
initial R values (diagonal of 5 x 10- 6's).
. . . . . . . . . . . . . . . .
B-7 Simulation 1: Three axis moment of inertia estimate plot with higher
. . . . . . . . . . . . . . . . . . . .
initial R values (diagonal of 5's).
16
110
List of Tables
4.1
Microsat parameters for first simulation showing reasonable mass properties including cross coupling inertia terms, center of gravity offset in
all axes and no noise. . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.2
Microsat parameters for second simulation showing the approximate
mass properties and sensor noise values of the experimental vehicle.
5.1
43
.
53
Non-mass related microsat parameters for the multiple body docking
simulation. These properties remain the same after docking with the
............................
space-borne object. ........
5.2
Overall 'true' and estimated mass parameters for the microsat alone
before docking at 9 seconds.
5.3
. . . . . . . . . . . . . . . . . . . . . . .
79
Overall 'true' and estimated mass parameters for the multiple body
docking simulation. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.4
77
84
Mass properties of docked object calculated at 36 seconds and the 'true'
docked object mass properties showing reasonable estimation. ....
17
89
18
Chapter 1
Introduction
Space missions in the future will require autonomous manipulation and control of
space-borne objects. To this end, Lawrence Livermore National Laboratories has
developed technology for a new generation of small, highly maneuverable satellites:
Microsats. Potential missions for the microsat include performing inspection maneuvers, and docking with other satellites. Attaining accurate knowledge of the mass
properties of the microsat and the docked objects is highly desirable not only for
precise motion control, but also as an in depth inspection of the docked object. Many
accurate methods for determining the mass properties exist; however, they often require a ground based test facility, or provide only the initial mass properties calculated
from the CAD model. In reality, the microsat will experience change in mass and
mass distribution due to fuel consumption and docking operations. Ideally, satellites
would autonomously determine the current mass properties in real-time during a mission. In this thesis, we develop an algorithm that a satellite such as the microsat can
use during a mission to calculate the complete set of mass parameters to enhance the
microsat operation during pre and post docking precision maneuvers.
1.1
Background
The field of system identification with respect to control of space objects is historically
rich; indeed, some of the first practical Kalman filter applications were developed
19
for orbital guidance and navigation problems in the Apollo space program
[4].
In
particular, there exists a wealth of information on estimation schemes used for attitude
prediction and control of satellites
[6].
Generally, satellites are intended to hold
attitude and orbit and thus are equipped with minimal propulsion capability. As a
result, most satellites do not experience large changes in mass properties; little or no
fuel is consumed. In this case, the mass properties calculated through ground based
testing remain suitable estimations for the operational life of the satellite. The extent
of mission based estimation of satellite mass properties consists of applying constant
torques or accelerations then calculating the mass or moment of inertia about one
axis from the resulting accelerations. These mass properties could then be used in
any number of simple control techniques designed for known mass parameters. Cristi
proposed an indirect adaptive control algorithm to estimate the inertia matrix for the
sole purpose of attitude control [1]. The estimation scheme presented in this paper
also produces the mass properties directly; however, it represents a deviation from the
traditional motivation for system identification. In the field of system identification,
complex physical phenomena often forces designers to construct models based on
data taken from experiments which are directly conducted to excite the plant and
measure its response. The final objective in system identification is to stabilize the
plant dynamics and give good performance which usually results in the identification
of a scrambled combination of system parameters, thus obscuring the physical nature
of the system in order to better represent the system in a control sense [2]. In this
thesis, we seek to aid in the control of the microsat while preserving the physical
representation of the system as an investigative tool.
1.2
Overview
Methods to recursively determine the inertia parameters of a satellite during a space
mission exist, at least in theory [1]. In this report we extend the estimated state to
include the ten essential mass parameters of a satellite: six inertia matrix parameters,
three center of gravity location parameters, and the total mass. Furthermore, given
20
accurate estimation of the ten mass parameters before and after rigidly docking with
space-borne objects, we demonstrate a method of determining mass properties of
the docked object and the combined body. We then present computer simulation
and experimental verification of the proposed estimation scheme on the Lawrence
Livermore National Laboratory's ground-based Microsat testbed.
21
22
Chapter 2
Description of the Microsat
2.1
Dynamic Model
The microsat is a compact, roughly cylindrical object capable of controlling attitude
and position via jet propulsion. In this study, we assume the satellite to have a rigid
body. For rigid bodies in general, the Newtonian equation:
F
(2.1)
P = mV
(2.2)
dt
where p is the momentum, F is force, m is mass, and v is velocity, describes the
motion of the center of mass, or translation of the satellite. The attitude dynamics
of a rigid spacecraft are given by Euler's equation:
dL
d
dt
=
mist
+ Tcontrol - W x L
(2.3)
L =W
10(2.4)
where L is the angular momentum vector and Tdist,
Tcontrol
are the disturbance and
control torques respectively. The second term in Equation (2.3), W x L, accounts for
the induced torque when the spacecraft angular velocity vector, W, is not aligned with
23
Yaw
Z An'is
Y Ais
Pitch
Roll
Figure 2-1: Diagram illustrating the orientation of the body-fixed axes with respect
to the microsat.
the angular momentum vector, L, or 10, where I is the inertia tensor.
Roll, pitch, and yaw are dynamic terms referring to rotations about the body-
fixed X, Y, and Z axes, respectively. Figure (2-1) illustrates the axis orientation with
respect to the microsat.
2.1.1
Overview of Inertia
A significant portion of this paper is devoted to the investigation of the inertia matrix parameters of the microsat and objects the microsat may encounter in space.
The lack of restoring forces in space prevents the use of the significantly simplified
planar dynamics often used to model ground based phenomena. Therefore, the iner-
24
tia representation in this paper must remain in three dimensional, symmetric matrix
form:
I
/Iz
IXY
Izz
IXY
IvY
IYZ
Izz
lyz
Izz
\
(2.5)
where the diagonal terms are the moments of inertia:
N
=
my
z)
(2.6)
i=1
N
I
=Zmi(Xi + zy)
(2.7)
i=1
N
izz =
mi(Xi + y)
(2.8)
i=1
and the off-diagonal terms are the products of inertia:
N
I
= -
>y mixzyi
(2.9)
N
Izz = - EMixizi
(2.10)
i=1
N
z = - >miyizi
(2.11)
Intuitively, the moments of inertia of an object, dependent on the amount and distance
of mass from the axis of rotation, describes the resistance the object has to torque
along each axis of a body-fixed frame. The products of inertia describe the inter-axis
correlation and indicate the degree of symmetry in the mass distribution of the object
with respect to the chosen body-fixed frame.
The inertia of two rigidly fixed bodies, such as a microsat docked with a spaceborne object, can be derived from the individual inertia matrices, center of gravity
locations, and masses of both the microsat and the object.
The combined inertia
of the two-body system, derived from the three dimensional parallel axis theorem is
presented in Figure (2-2):
25
CG2
Y~
z
b
a2
Z
0
G
x
1
0G 1
combined
S
(bO
Icombined =
ICGl +
M
1
I + M 2x
(MiX
+ c
-alb 1
-al cl
M1y1 + M2Y2
lvi combined
2
combined
-aibi
-aici
c2
a2 +
±
- b11
1
1
1i + b
MIz 1 + M 2 z 2
+ ICG2 + M2
combined
b2 + c2
-a 2 b2
-a2C2
)
-a2c2
a2 + c2
-b2C2
-
b2 C2
2i + b2
where:
X1, y1, zi = distance from origin, 0, to first object
Z2 = distance from origin, 0, to second object
is the inertia matrix with respect to the center of gravity of the first object and with frame orientation parallel
to the combined Inertia frame,
M 1 is the mass of the first object,
al, bi, ci are the x, y, z components of the distance from the combined CG to the CG of the first object
ICG2 is the inertia matrix with respect to the center of gravity of the second object and with frame orientation
parallel to the combined Inertia frame,
M 2 is the mass of the first object,
a2, b2 , C2 are the x, y, z components of the distance from the combined CG to the CG of the second object.
X2, Y2,
ICGI
Figure 2-2: Diagram and equation of combined inertia as a function of the inertia of
the respective objects [7].
26
Divert Jets (2)
Fuel Tanks (4)
CPU
Attitude Control Jets (16)
Inertial Measurement Unit (Internal)
Figure 2-3: CAD model of prototype LLNL microsat showing essential hardware
components: Inertial Measurement Unit, Central Processing Unit, and Propulsion
system.
2.2
Essential Microsat Hardware
Three essential hardware components that allow the microsat to recognize and respond to it's environment are the on-board sensor array, CPU, and propulsion system. The CAD drawing in Figure (2-3) shows these major hardware components on
a microsat prototype.
One of the key elements necessary for mass property estimation in the microsat
sensor array is the Inertial Measurement Unit (IMU). The IMU is a combined rate
integrating gyro and accelerometer often found on strap-down navigation systems. In
a strap-down navigation system, the sensors are physically attached to the satellite
thus giving the orientation and acceleration data with respect to the satellite axes.
The microsat prototype uses a Litton IMU which supplies the rate of change of
orientation of the microsat to well within t5 prad/s and the linear acceleration to
27
within ±3 cm/s 2. The gyro operates at 400Hz; however in order to synchronize with
the overall control loop rate of 20Hz, the IMU temporarily stores and sums every
batch of 20 samples.
The prototype Microsat contains a Motorola MV 1603 Power PC 603e microprocessor running the VxWorks 5.3.1 real-time operating system. The processor cycles
the overall control loop at 20Hz, during which it reads data from the sensor array,
interprets the data, implements the control law, and operates the solenoid valves for
the attitude control system jets and divert jets.
In this investigation, the microsat uses cold gas propulsion system, which means
the gas does not rely on chemical reaction to produce thrust.
In particular, the
microsat uses compressed nitrogen to produce attitude control system (ACS) thrusts
on the order of 0.25 lb and up to 5 lb divert thrusts. These thrust ratings are the
steady state values for the jets; however, there also exists transient rise and fall times
associated with jet actuation. The jet thrust on the Microsat is assumed to have a
3 ms rise and fall time due to the mechanical response time of the valves and the
propulsion dynamics of the gas. This is a conservative estimation given the solenoid
valve maximum rise and fall times of 1.5 ms.
2.2.1
Overview of Pulse Width Modulation
Pulse width modulation as applied to gas thrusters is a method of compensating for
the full on, full off nature of the thrusters. Though the thrust force is constrained
to either being on or off, the duration of the burn is constrained in the lower bound
only by the bandwidth of the solenoid gas valves and the rise and fall time of the gas
propulsion dynamics and in the upper bound by the width of the total duty cycle. In
other words, the gas thruster is capable of mimicking the desired force over a time
interval by matching the area under the desired force curve and the area under the
thruster force curve within a duty cycle. Thus, the gas thruster attempts to match
the average desired impulse over the duty cycle with a constant steady state thrust
28
F
Flet
(k-1
(k-1)T
(k+1)T
kT
O
(k+2)T
Desired force calculated for each time step
Desired force profile for each time step
Pulse width modulated jet approximation of desired force
Figure 2-4: Approximation of desired force constrained to actual jet thrust value,
Fet, with variable pulse duration, p, over each duty cycle.
for the appropriate duration, p:
I/(k+1)T
kT
Fdes(t)dt G- Fet -P
(2.12)
where the left hand side of the equation is the area under the desired force curve over
the interval, k, Fjet is the steady state thrust of the jet, and p is the duration the
thrust is held on. The diagram in Figure (2-4) illustrates the pulse width modulation
of the jet thrust which approximates the desired force calculated at each duty cycle.
Assuming the desired force over the duty cycle to be a constant, u, and that the
desired force is less than the steady state thrust, the following equation resolves the
proper duration, p, for an equivalent pulse:
uT
P
Fjet
29
(2.13)
2.3
Space-based Operational Procedure
Ideally, the microsat would be able to continuously update an estimate of the mass
properties based on information from sensors already in place.
During a mission,
the microsat compares the command thrust from pulse width modulated jets to the
motion information from the on-board control sensors. Using this approach requires
only extra computation and no additional hardware and thus introduces minimal
interference to the space mission.
30
Chapter 3
Mass Property Estimation Method
3.1
Recursive Least Squares
The well-known recursive least squares (RLS) approach was chosen because of its
effective and easily implemented estimation scheme. The following RLS equations
adapted from common discrete Kalman filter
[3]
and recursive least squares [1] equa-
tions are used in this study:
0
k+1 =
0
k
+ Pk+1H T (tk)Rk
Pk+1 = Pk - PkHT (tk)
1[U(tk) - H(tk)k]
[Rk + H(tk)PH T (tk)
1H(tk)Pk + Q
(3.1)
(3.2)
where 0 is the nx1 vector of unknown paramaters, P is the nxn error covariance
matrix, H is the lxn observation or regressor matrix, R is the lxi measurement noise
Q is
covariance matrix, ulx1 the input vector, and
the nxn model noise covariance
matrix.
The initial value of the covariance matrix, P, is given by the expectation of the
outer product of the parameter estimate error vector squared
PO
E[00 T ]
0
31
[31:
(3.3)
-(3.4)
where the error in the parameter estimates, 0, is the difference between the estimated
(0) and actual(0) values. Similarly, the measurement noise covariance matrix, R, is
given by the following equation:
Ro = E[vilT]
(3.5)
where v is the vector of measurement error or "noise" [3]. The addition of
Q in
the
update equation (3.2) yields a recursive structure equivalent to a discrete Kalman
filter with no state propagation i.e. the estimated state is assumed constant. A true
Kalman filter accounts for time-varying paramaters by "postulating that the true
parameter vector, [0], is not constant, but varies like a random walk" [5]:
k+1 - Ok + Wk
(3.6)
Qk= E[wkwi]
(3.7)
0
where w is white Gaussian. In essence, the added
Q term
acts as a 'forgetting ma-
trix' by preventing the covariance matrix, P, from tending to zero, thereby ensuring
effective parameter estimate corrections based on the residual error.
In reality, two events significantly change the mass properties over time. The first
is fuel consumption, and the second is docking with other space-borne objects. In
the former case, the change in mass properties is assumed to be much slower than
the convergence of the estimation to the true values. Thus, the filter is capable of
following the migrating mass parameters due to fuel consumption. On the other hand,
during the docking procedure the microsat undergoes a large, instantaneous change
in mass properties. In this case, the filter covariance matrix, P, is reset to initial
values and the mass property estimates are recalculated.
32
3.2
Estimate Convergence and Observability
Though the recursive least squares approach is widely used, the RLS estimates do
not necessarily converge on the true values. In order for the limit of the estimates,
lim
k-+oo
Ok
--
:)
(3.8)
to equal the true values, the measurements in the observation matrix, H, must be
persistently exciting; i.e.
N
H (t,)H T (tk)
lim Amin[
N -+oo
= O
(3.9)
k=O
where Amin is the minimum eigenvalue of the outer product of the regressor matrices,
H, and HT
[1].
Through simulation, Cristi shows that transient signals during regu-
lation provide enough excitation for convergence of only the inertia matrix estimates.
By providing open loop command thrusts to the microsat, we seek to provide
sufficiently exciting input to drive the inertia matrix as well as the center of gravity
location, and total mass estimates to their true values. Though all measurements
may be exciting, the system may remain unobservable if the observation matrix, H,
is sparse. The practical result, in this case, is the convergence of only some of the
estimates. Satisfying the following observability condition guarantees convergence of
all the parameter estimates.
3.2.1
Observability Condition
If we take the lx vector z which is linearly related to the estimated nxl state, 0, by
Hglxn),i
z = HO,
(3.10)
then assuming the measurements to be contained in the observation matrix, H, the
residual,
U(tk) - Z(tk),
33
(3.11)
is the error between the input, u, and the vector, z. In Equation (3.1) this error
is weighted by the sensor covariance matrix, R, re-transformed to the state, via the
observation matrix, then weighted once more by the system covariance matrix, P.
The resultant vector is added to the previous estimate of the state to produce the
new state estimate. In other words the comparison between the input vector, u, and
the vector, z, drives the current estimates toward the true state values.
A sparse observation matrix, H, produces a vector, z, which is not dependent on
all previous state estimates, thus, a sparse H indicates a lack of feedback in producing
the current state estimates.
Gelb defines the observability condition for a discrete, deterministic, constant
nth-order system
[31:
(3.12)
Xk+1 = (Xk
where x is the nxi state, and b is the nxn state transition matrix. In this system,
there are n scalar noise-free measurements taken over time, zk:
Zk = HXk,
k = 0, 1, 2, ... ,n - 1
(3.13)
so that H is a constant, n-dimensional row vector (lxn). We may write
zo=
HxO
Z
Hx1 = H?xo
=
Zn_1=
(3.14)
Hx,- 1
= H4n-IXO
Therefore,
zi
.
\ zn_1
HG
=
.
h Hn
xo =
TXO
(3.15)
mIxs
If xO is to be determined uniquely, the nxn matrix , E, must be nonsingular. Thus,
34
the observability condition is that the matrix
(HT
THT
...
( T)n-1HT)
(3.16)
be of rank n.
The extension of this observability condition to accommodate an 1 vector z, i.e.
1 measurements instead of a scalar z at each time step, k, produces a (l-n)xn matrix,
7T. Thus, the new observability condition is that an nxn window centered on the
current time, k, must be of rank n.
Because we assume the mass properties to remain constant, the state transition
matrix, <D, is the identity matrix, and drops from 7. Thus, with each time step, H is
appended by the current observation matrix,
7 = (HT
H[
HT:
... HT),
(3.17)
and the observability condition requires the latest nxn elements of 7 to be of rank,
n. Depending on the structure of H, there exists a chance the observability condition
may never be satisfied. This occurs when elements in H which must necessarily be
non-zero to prevent singularity are not dependent on any parameter estimates, and are
therefore constrained to be zero. In other words, no amount of commanded excitation
would produce a non-singular rxn window in E, thus, never satisfying the observability
condition. A method employed in this thesis to prevent this case is to split the single
large filter which estimates all the mass parameters into smaller simultaneous filters,
thereby reducing the number of elements in H which are constrained to zero. In
practice, each parameter might not be excited simultaneously, and thus, the rank
of E may not be full; however, if each parameter is sufficiently excited over a short
enough time period, one can be confident that all the parameters converged near the
true values by the end of the time period.
35
36
Chapter 4
Single Body Scenario
During a space mission, when accurate mass property estimates are especially necessary, the proposed LLNL Microsat executes a series of open loop thrust commands to
excite and estimate all the mass parameters. This section presents two simulations
and experimental results for the estimation of the mass properties of the microsat
alone.
The preliminary simulation establishes the viability of the estimation algorithm
given suitably large values to be estimated and no noise. The second, refined simulation closely matches the experimental setup in hopes of accurately emulating the true
dynamics of the experiment. In the experimental trial, a control plan identical to that
in the second simulation was executed. Data concerning the dynamics of the satellite
were recorded in real time, thus producing a synchronized set of control actions and
dynamic reactions. Once the experimental trial was recorded, the resulting data was
processed incrementally through the estimation filter.
4.1
Algorithm Structure
In order to prevent producing a sparse observation matrix, H, two simultaneous
Kalman filters are used. In the single body case, the mass parameter vectors to be
estimated apply to the microsat by itself:
37
01
=(1
112
'13
CGx
133
123
'22
CGY
CG2)T
(4.1)
(4.2)
02 = (m)
where Ijj is the corresponding inertia matrix value with respect to the center of gravity, CG
is the current three dimensional center of gravity location with respect to
the initial CG location, and m is the total mass of the object. The following equations relate the state vectors to the resultant torque and force through the observed
accelerations and velocities contained in the regressor matrices, Hi:
Ti
T2
=H1O1
(4.3)
=H2
(4.4)
T3
Fi
F2
F3
where -rj is the resultant torque component, and F is the resultant force component.
The regressor matrix, H 1 , is a combination of the dynamic torque equation (2.3)
added with the torque component from firing divert jets which do not pass through
the center of gravity. The torque equations are rewritten below for convenience:
Ttotal = idnamics
cdivert
-
Tdynamics =Iw±WXIW
Tdivert = rX
F
(4.5)
(4.6)
(4.7)
where W- is the angular acceleration vector in three dimensions:
W
W2J,
38
(4.8)
'
is the angular velocity vector:
(4.9)
L02
W3
r is the vector from the initial center of gravity location, on which the divert jets are
focused, to the current center of gravity:
CG\
CGY
(4.10)
,
CGz;
and F is the force vector from those divert jets:
Fcmd
F=
(4.11)
F2cmd
F3cmd /
where F cmd is the component commanded force. Expanding, then summing
and
7c'iert
'cynamics
in Equations (4.6) and (4.7), then collecting terms into an expression linear
in the mass parameters to be estimated (Equation 4.1) gives the explicit form of the
regressor matrix, H 1 :
LD1
1W3
W
I
I
W 1A 32
LD2 -Wl103
-W2WW3
~
)2
_2-
L;)3 - L)1 W2
W2W-W2
-U)2W3
WW
02
32
W
C;4 -W2W3
W102
22W
W 22-o
3-UW
2
-W1W3
W2W3
0
F3cmd
-W1W3
-F3cmd
0
Fcmd
-Flcmd
0
W3
F2cmd
-F2cmd
(4.12)
The second regressor matrix in Equation (4.4) multiplied by the state vector in Equation (4.2) is simply the Newtonian equation, (2.1), rewritten below:
F = md
(4.13)
where F is the applied divert force vector, m is the total mass of the satellite, and
39
a is the satellite acceleration vector. The explicit form of H 2 given in the following
equation:
a,
H2 =
(4.14)
a2
ka3
where aj is the observed component acceleration.
At each iteration, the covariance matrix, P is updated, and the elements of the
covariance matrix along the diagonal indicate the second moment of the error, 0, as
seen in Equation (4.15)
[3].
/
P = E[O]
E[02]
E[ 10 2]
SE[0 1 02]
E[02]
E[16n]
-
E[ 1On]
...
(4.15)
... E[6n]
The "precision" bounds in all the figures are the square roots of the covariance terms,
or the root-mean-squared (rms) values of the error, 0, found on the diagonal of the P
matrix. The off-diagonal terms indicate the cross-correlation between the elements in
0. In all simulations in this thesis, the initial error covariance matrix, P, measurement
noise matrix, R, and model noise covariance matrix,
Q,
are diagonal, meaning no
initial cross correlation exists between the parameter estimate errors/noise.
The
magnitude of each element is chosen based on a best educated guess; though defined
theoretically, the initial covariance matrices often need to be redefined to produce a
stable, and converging filter. Appendix B presents examples of the iteration process
required for the simulations in this thesis to find suitable covariance matrix values.
The resultant torque and force vector from the left sides of equations 4.3 and 4.4
subtracted from the commanded torque and force produces the residual error. This
error is then multiplied by the Kalman gain, then added to the current estimation as
seen in the update equation, 3.1. Assuming the inputs in H are persistently exciting,
the state estimate, 0 , will eventually converge on the true mass property values.
40
4.2
Single Body Simulations
This section presents six degree-of-freedom simulations of the microsat in space. The
objective of these simulations is to show the estimation algorithm capable of determining the correct mass properties of the microsat. In order to accurately simulate the
estimation algorithm, we reproduced both the microsat plant and controller dynamics
in a variable step differential equation solver.
4.2.1
Simulation Description
The six degree-of-freedom (DOF) simulation executed in Matlab takes advantage of
the pulse width modulated control action of the microsat. Depending on the desired
control action, the resultant jet thrust durations could vary between a minimum of 3
ms and a maximum equal to the duration of the control cycle, 50 ms. Each step of the
differential equation solver corresponds to the duration of constant thrust. Thus, this
six DOF simulation provides accurate dynamics data while minimizing computation.
(See Appendix C for Simulation code)
The controller parameters in the simulation match those downloaded to a physical
microsat; however, physical parameters must be specified within the simulation as
truth. These variables include the mass parameters to be estimated. Though specified
in the simulation, the inertia, CG location, and mass only manifest themselves in the
dynamic reactions to the control thrusts. Random processes normally encountered in
reality can also be reproduced within the simulation as sensor noise. The simulated
trials indicate the ability of the estimation algorithm to converge on the specified true
parameters.
4.2.2
Preliminary Simulation
The indirect adaptive control simulations presented by Cristi show a fast convergence
of the six 3x3 inertia matrix parameters. The algorithm presented in this paper
shows similar convergence for the center of gravity location and total mass as well
as the inertia matrix parameters. However, rather than immediately substituting the
41
Divert Away
From Initial
Docking Port
Open Loop
Roll Oscillations
0
2
4
Open Loop
Pitch Oscillations
6
Open Loop
Y-Axis Diverts
Open Loop
Yaw Oscillations
10
8
12
14
16
18
20
Time (s)
Indicates Control Action To
Return To Initial Attitude
And Hold
Figure 4-1: Timeline of the mission showing all control actions.
estimated mass properties into the controller, the controller executes a series of open
loop command torques and diverts to excite the estimated states. Specifically, each
moment of inertia is excited directly with alternating 100 ms pulses for approximately
two seconds. Each two second excitation is followed by the command to return to
the initial orientation and hold attitude for an additional two seconds. After the
roll, pitch, and yaw motions are executed, the microsat diverts along the Y axis with
alternating 200 ms pulses for two seconds. Figure (4-1) shows the overall mission
profile used in the following simulations.
The first simulation, demonstrates the ability of the estimation algorithm to converge on the true values with perfect information from the sensors. The true mass
properties correspond to a reasonable estimate of a physical microsat. (See Table 4.1)
In the first simulation trial, we show the convergence of reasonable mass parameter
estimates in a perfect world with no random variations. The truth parameters are
not perfect in a design sense; satellites are often physically symmetric and produced
at extremely tight tolerances which practically eliminates the center of gravity offset
from the divert thrusters. Because symmetric design represents a desire to produce
42
Microsat Mass Property
(Calculated at end of first trial)
'True' 'Estimated'
Value Value
Moment of Inertia Ixx [kgm2]
0.7
0.70
Moment of Inertia Iyy [kgm2]
4
4.00
Moment of Inertia Izz [kgm2]
4
4.00
Product of Inertia Ixy [kgm2]
0.1
0.10
Product of Inertia Ixz [kgm2]
0.1
0.09
Product of Inertia Iyz [kgm2]
0.1
0.10
0.1
0.10
0.1
0.00
through which all divert jets fire [m]
0.1
0.10
Total Mass [kg]
20
19.99
Moment arm about X-axis from zero deviation CG to ACS Jet [m]
0.1475
Not estimated
Moment arm about Y-axis from zero deviation CG to ACS Jet [m]
0.2575
Not estimated
Moment arm about Z-axis from zero deviation CG to ACS Jet [m]
0.2575
Not estimated
ACS jet thrust [lbf]
0.25
Not estimated
Divert jet thrust [lbf]
0.50
Not estimated
0
Not estimated
0
Not estimated
Center of Gravity X-axis deviation from point
through which all divert jets fire [m]
Center of Gravity Y-axis deviation from point
through which all divert jets fire [m]
Center of Gravity Z-axis deviation from point
Gyro noise 6
o- magnitude
Accelerometer noise 6 u- magnitude
Table 4.1: Microsat parameters for first simulation showing reasonable mass properties including cross coupling inertia terms, center of gravity offset in all axes and no
noise.
43
a dynamically intuitive vehicle, many of the mass property terms which are of vital
interest on a space mission because of their interference in control are assumed to
be zero or near zero. These parameters include the products of inertia, or inter-axis
coupling terms, and the center of gravity location. One can imagine that a deviation
in the center of gravity of a space vehicle could result in unpredicted torques caused by
divert jets that were assumed to have focused on the center of gravity. Therefore, in
order to predict these interferences, this simulation assumes reasonably large center
of gravity offsets from the divert thrusters as well as dynamically significant cross
coupling as seen in the product of inertia values in the true mass parameters in
Table (4.1). These mass parameter truth values may very well closely represent the
parameters encountered as a combined body of a microsat docked with another spaceborne object. We investigate this trial to assure the estimation algorithm does not
depend on the significantly simplified dynamics of a symmetric vehicle.
The control torques shown in Figure (4-2) indicate large control efforts to regain
the initial attitude after the excitations. Because this simulation reproduces a perfect
world with no random variations, these deviations are not a result of unaccounted
jet misalignments or sensor noise. Rather, the deviations are indications of the high
degree of cross correlation in this system. Though each roll, pitch, and yaw motion is
executed independently, rotation still occurs in the remaining two axes because of the
off-diagonal inertia matrix terms, or products of inertia. Also, the command torque
oscillations at 16 seconds represent the controller effort to reduce the additional torque
produced by the Y-divert oscillations through a point that is 0.1 m away from the
center of gravity.
The control actions in this trial were exciting enough in the first 2 seconds to significantly converge both the X-axis and Z-axis moments of inertia. As each rotation
axis is excited, the corresponding moment of inertia estimate shows further convergence on the true value (See Figure 4-3). This is also true of the "precision" of the
filtering algorithm; as a specific mode is excited, the corresponding covariance term
decreases. The inter-axis correlations induce compensation in the form of immediate
control torques in all axes. Thus all the moment estimations begin to show excitation
44
and convergence though not specifically excited. Even before any specific rotational
axis is excited, the roll moment of inertia estimate nearly converged. The product of
inertia estimates (See Figure 4-4) show convergence as a direct result of cross coupling of axis rotations. Despite initially diverging, the products of inertia estimates
eventually converge on the true values as indicated by the tapering precision envelope.
The center of gravity location estimations are calculated based on the comparison
between the control actions and the resultant angular velocities. The X and Z components of the CG location estimation converge quickly and in a similar fashion to the
inertia parameters; however, the Y component does not deviate from the initial value
of zero. Though the estimation algorithm attempts to converge on all ten parameters
simultaneously, observability of the parameters is not guaranteed. In this case, the
Y component of the CG location depends on linear excitation of the vehicle in any
direction other than along the body-fixed Y-axis. Intuitively, since the vehicle was
given only diverts along the Y-axis, no degree of deviation of the center of gravity
along this axis would induce a rotation on the vehicle (see Figure 4-5); thus, the Y
component can never converge to the true value of 0.1 m with forces applied only in
the Y-axis direction.
Analogous to the relationship between torque commands and inertia, CG estimation, the divert force commands contribute to the mass estimation. In this trial, the
microsat was excited only in the Y-axis as seen in Figure (4-7). The initial diverts
in this simulation were to simulate the deployment of the microsat away from the
docking bay and were extraneous to the purpose of parameter convergence. However,
they proved to converge the mass estimate to within 5% of the true value as seen in
Figure (4-7). The divert oscillation at 16 seconds provided little if any improvement
in mass estimation from the initial divert contribution.
45
Control Roll Torque N-m
0.4
0 .2 --
0
- -..
.
- 0 .2 -. ..
-0.4
. - .. -
. .. . . .
. -.
- . . .- . . .
- . .
. .
- . . . . .-.
0
-...-.
2
4
6
.-..
8
10
12
Control Pitch Torque N-m
.- \..
14
16
18
20
18
20
1
-
0.5 ......... ----
-
0
- -
- 0 .5 --- -------1
0
2
4
6
8
10
12
Control Yaw Torque N-m
14
16
1
---
0
.- . -..-..-.-.-.-..-.-.-.-.
. .. .. .. . . . . . . .
- 0 .5 -.. . .. . . -.
-1
0
2
4
6
8
10
time (s)
N
-.-.
12
14
16
18
20
Figure 4-2: Simulation 1: Control torques on each axis resulting from execution of the
mission plan. Though all the torque commands are perfectly balanced with opposite
commands during the open loop excitations, large control torques are evident when
attempting to return to initial orientation, suggesting large deviations in orientation
due to inter-axis torque disturbances. The oscillations at 2, 6, and 10 seconds show the
open loop control roll, pitch, and yaw, respectively to induce parameter convergence.
46
X axis moment of inertia kg-rn 2
0.8
.. .. . .
.
- - - -
--
_
-
0.6
. . .. . ....
*
..
-
.
0.4
0.2
Estimate
0
True value
--
-
S-
-Filter
"precision"
-0.2
)
2
4
6
8
10
12
14
Y axis moment of inertia kg-rn2
16
18
20
5
4
3
2
1
.
-- Estimate
_- True valueFilter "precision"
- -
0
C)
2
4
6
8
10
12
14
Z axis moment of inertia kg-m 2
16
18
20
5
------------ -
-
4
3
2
.......
. .
~ .. .~~.~
1
0
I
)
2
-Estimate
-..
...
T rue va lueFilter "precision"
.
........I
4
6
8
10
12
14
16
18
20
time (s)
Figure 4-3: Simulation 1: Three axis moment of inertia (MOI) estimate plot showing
the convergence from initial moment estimates of zero to the true moment represented
by the horizontal line. The "precision" boundary for each plot is the 'true' value
±/ Pkkwith k = 1 for X-axis MOI, k = 4 for Y-axis MOI, k = 6 for Z-axis MOI. The
"precision" remains nearly constant from 12 to 20 seconds because there is no direct
excitation of these modes during that period.
47
2
X-Y product of inertia kg-m
0.5
0
-
-0.5
)
2
4
6
-
14
12
10
8
X-Z product of inertia kg-m2
Estimate
True value
Filter "precision"
16
18
20
0.5
0
Estimate
--- True value
V7
-0.5
)
2
4
Filter "precision"
6
12
10
8
Y-Z product of inertia kg-m
14
16
18
20
2
0.5
0
-
-
Estimate
True value
-- Filter "precision"
J
0
2
4
6
8
10
12
14
16
18
20
time (s)
Figure 4-4: Simulation 1: Three inter-axis product of inertia (POI) estimate plot
showing the convergence from zero initial values to the true product represented by
the horizontal line. The "precision" boundary for each plot is the 'true' value ±Pkk
with k = 2 for X-Y POI, k = 3 for X-Z POI, k = 5 for Y-Z POI. Though tapering
significantly at the beginning of the simulation, the "precision" envelopes remain
wide at the end of the simulation from lack of direct excitation, which indicates the
current residual error is more significant than previous residual error when calculating
the current estimate.
48
Z Axis
Induced Yaw w CG x 0bservable
Potential CG
locations along
line
Fapplied,
y-direction
so Y Axis
No Induced Pitch
y
CGY NOT Observable
Induced Roll-
CGZ Observable
X Axis
Figure 4-5: Diagram showing uncertainty in CG location based on diverts only in
Y direction. There is no difference in dynamics as the CG location varies along the
Y-axis.
49
CG location along X-axis
0.1
0.08
0.06
ile "rciin
Estimate
-
0.04
-
-True
-.-.
0.02
0
0
2
4
12
10
8
6
value
Filter "precision"
14
2C
18
16
CG location along Y-axis
4
2
-
0
- - - - -
- --
- - --
-- --
- --
-- Estimate
-2 -.
.-
.
-
True value
Filter "precision"
- - - ---4 ... . .. . . .... . ... . . ..................................................
14
16
12
10
8
6
4
2
0
18
2(
CG location along Z-axis
0.1
0.08
0.06
0.04 0.02
0
. . . ... . .
.... . ..-.
0
2
..
4
6
E stim a te
--..
-- True value
-..-...
Filter "precision"
10
8
-..
12
14
16
18
.. .
20
time (s)
Figure 4-6: Simulation 1 Center of gravity (CG) location estimate with respect
to the initial CG location corresponding to the focus of the divert thrusters on the
microsat. The true CG deviation from the focus in Simulation 1 is 0.1 m in X, Y, and
Z directions. The CG location in the Y direction is unobservable, and thus, there is
no change from initial values. (see Figure 4-5). The "precision" boundary for each
plot is the 'true' value ± Pkkwith k = 7 for CG along X-axis, k = 8 for CG along
Y-axis, k = 9 for CG along Z-axis.
50
(a)
Force command (Y-Axis Divert) N
3
I
I
12
14
2
-.
1
0
-1
-2
-3
)
2
4
6
8
(b)
10
time (s)
16
18
20
Total Mass kg
22
20
18
16
Estimate
-
True value
-
-
14
12
)
..
2
..-
4
6
-
8
10
time (s)
12
14
Filter "precision"
16
18
20
Figure 4-7: Simulation 1: Commanded force plot showing the initial divert away from
the bay, and the divert oscillations at 16 seconds. Simulation 1 contained diverts in
the Y (lateral) direction only. Plot (b) shows the estimated total mass for the microsat
in Simulation 1 revealing correlation between mass convergence and command force
excitations. Since the mass estimate is filtered separately from the other estimates,
the "precision" boundary is only the 'true' value ± P2 .
51
4.2.3
Refined Simulation
The second single-body simulation closely approximates the proposed experimental
trial and uses nearly the same mission profile as the first simulation. The microsat
simulation parameters match the speculated mass parameters of the prototype, and
the sensor noise also matches the precision of the IMU data on the prototype microsat.
(See Table 4.2) However, one important detail from the experimental setup that is
not modeled in this simulation is the mass contribution from the dynamic air bearing.
The air bearing itself contains pressurized air tanks and the structure for supporting
the microsat and thus, has a significant mass. The mass contribution appears only
in the total mass parameter because the air bearing is constrained to translate with
the microsat, but not rotate. Therefore, the total mass value is set at 25 kg, 5 kg
higher than the previous simulation to account for the added mass of the air bearing.
The mission profile for the second simulation differs only in the frequency of divert
oscillation at 16 seconds. The divert pulses last for approximately 100 ms as opposed
to 250 ms in the first simulation.
As in the first simulation, all the open loop torque commands are balanced with
opposite commands. Barely noticeable attitude corrections between excitation periods indicate attitude deviations produced by the small products of inertia during the
oscillating rotations and sensor error (See Figure 4-8). Despite the sensor noise and
the effects from inter-axis torque coupling, the relatively inactive controller between
excitation periods indicates a well balanced system.
The inertia matrix does not converge as quickly in this trial. Rather the moments
and products of inertia converge more predictably as each mode is excited. Though
the estimates do not converge as rapidly between excitations, the minute control
actions taken to maintain attitude also contribute to the convergence of the moment
of inertia terms as seen in the initial steps in the Y-axis and Z-axis plots in Figure
(4-9). One interesting anomaly in the product of inertia estimates is the immediate
step following the excitation of the roll and pitch axis at 4 seconds. Though seemingly
large, the deviations from the true values remain within the precision envelope. At
52
Microsat Mass Property
(Calculated at end of second trial)
'True'
Value
'Estimated'
Value
Moment of Inertia Ixx [kgm2]
0.35
0.35
Moment of Inertia Iyy [kgm2]
2.5
2.49
Moment of Inertia Izz [kgm2]
2.0
2.00
Product of Inertia Ixy [kgm2]
-0.01
-0.01
Product of Inertia Ixz [kgm2]
-0.05
-0.05
Product of Inertia Iyz [kgm2]
0.06
0.06
0
0.00
0
0.00
through which all divert jets fire [m]
0
0.00
Total Mass [kg]
25
14.18
Moment arm about X-axis from zero deviation CG to ACS Jet [m]
0.1475
Not estimated
Moment arm about Y-axis from zero deviation CG to ACS Jet [m]
0.2575
Not estimated
Moment arm about Z-axis from zero deviation CG to ACS Jet [m]
0.2575
Not estimated
ACS jet thrust [lbf]
0.25
Not estimated
Divert jet thrust [lbf]
0.50
Not estimated
o magnitude
Accelerometer noise 6 o- magnitude
0.0001
Not estimated
0.02
Not estimated
Center of Gravity X-axis deviation from point
through which all divert jets fire [m]
Center of Gravity Y-axis deviation from point
through which all divert jets fire [m]
Center of Gravity Z-axis deviation from point
Gyro noise 6
Table 4.2: Microsat parameters for second simulation showing the approximate mass
properties and sensor noise values of the experimental vehicle.
53
the end of the second simulated trial, all products of inertia converged to the relatively
tiny true values.
Because both the true CG locations and the initial CG location are assumed to be
zero, the unobservability of the CG location along the Y-axis is not apparent except
for the unperturbed precision envelope. The envelope never converges on the true Y
location for the CG due to lack of excitations in that mode. The X and Y-axis CG
estimates remain near zero as indicated by the millimeter scaling in Figure (4-11).
The behavior of the mass estimate initially mimics that of the first trial. However, immediately after the initial divert excitations cease, the mass estimate slowly
diverges from the true value. In the presence of applied force, the noise contribution
from the accelerometer is small by comparison; however, when the force excitations
cease, the algorithm still observes acceleration from noise. Therefore, the mass estimates continue to change though the force input has stopped. When the Y-axis
divert oscillations return at 16 seconds, the mass estimate again begins to converge
with the true value.
The mass estimate behavior shown in Figure (4-12) reveals interesting assumptions within the filter. The precision envelope does not follow the mass estimate as
the noise drives the estimate toward zero. Instead, the filter believes the sensor noise
to be good data, thus decreasing the covariance on the assumption that the mass was
decreasing. This occurs because the 'ghost' accelerations from the noise multiplied
with the current estimated mass always produces an error when compared with the
zero applied diverts; the filter assumes the only mass that could account for an acceleration when no force is applied is zero mass. Thus, from 2 to 16 seconds when there
are no applied forces, the mass estimate tends to zero because of the noise.
54
Control Roll Torque N-m
0.4
0.2
0
-0.2
-0.4
)
2
4
6
8
10
12
14
Control Pitch Torque N-rn
16
18
2(
1
0.5 - .- .-.-.-.-.-
--
-. - .-- --.- -- - .- .-. -
0
.. . ..--.
.. . . .
-0.5 -.. .-.
-1
)
2
4
...-..
6
- --
-
8
10
12
Control Yaw Torque N-m
14
- ---
16
18
20
1
0.5
I
0
-.
- -
-
-- - --
---
I
W1
.........
-- - -
I
I
I
- - - - .........
- -.
.-
-.
-
- - - --.
-0.5
-1
0
2
4
6
8
10
time (s)
12
14
16
18
20
Figure 4-8: Simulation 2: Open loop control torques about three axes. Periods of
open loop excitation of roll, pitch, and yaw axes start at 2, 6, 10 seconds, respectively.
The remaining time was devoted to returning the microsat to the initial orientation,
then holding attitude using closed loop control. The isolated spikes in each plot are
the small torques to return the microsat to initial orientation which are executed
when each closed loop control period begins.
55
X axis moment of inertia kg-rn 2
0.4
0.2
. ....
....
.... .. .... .. .. .. .. .T
0
-Estimate
ru e v a lu e
-- Filter "precision"
.. .
-0.2
0
14
12
10
8
6
Y axis moment of inertia kg-m2
4
2
16
18
2(
3
2
-
1
-
Estimate
-True value
-- Filter "precision"
--
0
0
2
14
12
10
8
6
kg-m2
inertia
of
Z axis moment
4
16
18
2(
3
2
1
. . .
.
......
.. ..... . ......
. .....
0
0
.--
2
4
6
8
10
12
Estimate
-- True value
F ilte r "p recisio n '
14
16
18
20
time (s)
Figure 4-9: Simulation 2: Time plots of the moment of inertia parameter estimation
about the three body-fixed axes. The inertia estimates are initialized at zero, then
converge on the true value shown as the horizontal line. The "precision" boundary
for each plot is the 'true' value ± Pkk with k = 1 for X-axis MOI, k = 4 for Y-axis
MOI, k = 6 for Z-axis MOI.
56
X-Y product of inertia kg-rn 2
0.04
Estimate
True value
-
0.02 -
.....-
.
Filter "precision'
-
0
-0.02
-0.04
. . . . . . . ... . .
-0.06
)
2
4
6
. . .
.
.
.
8
.
.I
.
.
10
. .
.
.
12
X-Z product of inertia
.. .
.
.
14
. .
.
.
16
. .
.
.
.
2' 0
18
kg-rn2
0.2
-- Estimate
- -True value
-
0.1
Filter "precision"
--
-
0
-
-0.1
-0.2
J
2
4
6
8
10
12
Y-Z product of inertia
.. . .. . . .. . .... . . . .
.
.
14
16
18
20
kg-rn2
. .
.
.
. .
.
. .
. .
.
.
. .
.
.
. .
0.5
0
Estimate
.
.-
True value
-- - Filter "precision'
-0.5
0
2
4
6
8
10
12
14
16
18
20
time (s)
Figure 4-10: Simulation 2: Time plots of the product of inertia parameters. The
product of inertia estimates are initialized at zero, then converge on the true value
shown as the horizontal line. The "precision" boundary for each plot is the 'true'
value ±Pkk with k
=
2 for X-Y POI, k = 3 for X-Z POI, k = 5 for Y-Z POI.
57
CG location along X-axis
0.05
0
Estimate
--
-0.05
-
)
2
4
6
14
12
8
10
CG location along Y-axis
True value
Filter "precision"
16
18
20
4
2
0
- -Estimate
-
-4
)
2
4
6
8
10
14
12
True value
Filter "precision"
16
18
20
CG location along Z-axis
0.05
0
Estimate
True value
-
-0.05
Filter "precision'
0
2
4
6
8
10
time (s)
12
14
16
18
20
Figure 4-11: Simulation 2: Time plot of center of gravity location estimation in the
X, Y, Z body-fixed microsat axes. The "precision" boundary for each plot is the 'true'
value ± Pkk with k = 7 for CG along X-axis, k = 8 for CG along Y-axis, k = 9 for
CG along Z-axis. The Y-axis CG location is unobservable as shown in Figure (4-5),
thus the "precision" bounds do not converge. The "precision" bounds on the X and
Z axis CG location estimates taper initially due to the initial divert away from the
docking bay, then after a period of inactivity in divert excitations, the bounds taper
again at 16 seconds due to the open loop excitations.
58
Force command (Y-Axis Divert) N
3
. ...
..
..
.
.
.
2
4
6.
8
10
..... 12
2
4
6
8
10
12
.. . . . . . .
. . . . . . ..
.
1*
0
0
-3
0
14.
.
16.
18
2'
14
16
18
20
time (s)
Total Mass kg
30 I
25
20
15
10
5
-
.....
..
......
.....
...... ... . .-
0
-5
0
2
4
6
8
10
time (s)
... .
12
Estimate
-..
Tru e va lu e-- Filter "precision"
14
16
18
20
Figure 4-12: Simulation 2: Time plot of open loop Y-axis divert thrusts and time plot
of total mass estimation. The convergence in the first two seconds and at 16 seconds
corresponds to the open loop Y-axis divert thrusts. The lack of divert thrusts between
2 and 16 seconds lowers the signal to noise ratio, thereby causing the significant
divergence in the mass estimate. Since the mass estimate is filtered separately from
the other estimates, the "precision" boundary is only the 'true' value ±P 2 .
59
Figure 4-13: Microsat experimental test bed at Lawrence Livermore National Laboratories. The microsat shown above is moving freely in five degrees of freedom. The
spherical air bearing at the top of the support structure allows all three rotational
degrees of freedom and the support structure itself slides freely in two degrees of
freedom on the plane of the glass table.
4.3
Experimental Setup
One of the primary objectives of the microsat experimental test bed at Lawrence Livermore Laboratories (See Figure 4-13) is to test and validate proposed space missions.
In addition to housing the IMU, CPU, and propulsion system, the experimental vehicle is situated on both a linear and spherical air bearing to reproduce a dynamically
similar environment to space.
The experimental testbed achieves 5 degrees-of-freedom for the microsat with
the use of extremely low friction spherical and linear air bearings. The spherical
60
Figure 4-14: Combined Linear and Spherical Air Bearing.
Figure 4-15: Spherical Air Bearing location at CG of vehicle.
61
air bearing is at the center of gravity of the microsat (see Figure 4-15) and allows
free rotations in all coordinate axes (3 degrees of freedom).
Because the satellite
would normally rotate about the CG in space, placing the spherical bearing of the
microsat experimental setup at this rotation point allows space-like rotations on the
ground. Similarly, the spherical air bearing is placed on a linear air bearing which
allows the CG to translate without friction within a plane (2 degrees of freedom).
Figure (4-14) shows the air bearing setup. While the microsat experimental setup
perfectly recreates free rotations when the center of gravity of the microsat is located
at the spherical air bearing, the additional mass of the air bearing is attached to the
microsat, which, for the mission plan, is the dynamic equivalent of a point mass at
the spherical air bearing location.
One major difference between the simulation and experiment lies in the real-time
microsat controller. At the beginning of every 50 ms control cycle, the microsat
controller is allotted 10 ms to compute the desired thrust for the next cycle. During
closed loop control, the CPU calculates the desired thrust from IMU information from
the previous cycle. Thus, a two cycle delay always exists between the collection of
sensor data and thrust execution. The timing diagram in Figure (4-16) shows the
digital control process. While the implications for such a delay are significant in the
control of the microsat, the estimation algorithm is not affected except the command
thrusts must be saved then input to the estimation algorithm with the appropriate,
corresponding IMU data.
62
Control Cycle (k-3)
Control Cycle (k-2)
Control Cycle (k-1)
Execute Td (k-3)
Execute Td (k-2)
Execute Td (k-1)
Calculate
Td (k-2)
Calculate
Td (k-1)
Gather IMU Data
For Td(k-1)
Calculate
Td (k)
Gather IMU Data
For Td(k)
Gather IMU Data
For Td(k+l)
Control Cycle (k)
Execute Td (k)
Calculate
Td (k+1)
Gather IMU Data
For Td(k+2)
-10
ms
50 ms
Time
o
Figure 4-16: Timing Schematic for Microsat Prototype.
4.4
Experimental Results
The implementation of the estimation scheme and the mission plan for the experimental trial was virtually identical to the second simulation trial (See Figure 4-1).
The experimental run itself consisted of executing the mission plan and recording the
subsequent dynamics. Though the estimation algorithm was not running real-time
on the satellite, the data taken from this run could be input into the estimation algorithm sequentially as if running real-time. With each time step of the data, the
estimation algorithm iterates once. Thus, with one experimental run, many different
variations of estimation algorithms could be tested. Real-time trials of the estimation
algorithm proved to produce similar results (See Appendix A).
The time allotted between the open loop roll, pitch, and yaw oscillations proved
to be long enough for the microsat to regain the initial attitude as shown by the
minimal control action before each open loop oscillation. This indicates the microsat
to be well balanced in three respects. First, the Y-divert jet is well focused on the
CG of the satellite because the satellite would otherwise begin to rotate about the
roll or yaw axis which would then induce a roll or yaw torque in compensation. In
63
Figure (4-17), the compensating torques seem minute during the Y-diverts executed
at 0 and 16 seconds. The second aspect of the well balanced microsat is the CG of
the microsat seems to be centered on the spherical air bearing. If the CG were any
significant distance away from the air bearing, a parasitic torque would be created
due to the force of gravity acting at the CG and the restoring force acting at the
air bearing. As the distance in the X-Y plane between these forces increases, the
moment arm of the torque increases, resulting in a large control effort by the satellite
to maintain a level orientation. Figure (4-17) shows little if any control action to
maintain the satellite balance. The microsat is also well balanced in that there is
little cross correlation between the rotational axes. This can be seen again in Figure
(4.17) by the minimal control effort after each oscillation. This is an indication of
small products of inertia.
A major difference between the experimental trials and simulations is the actual
'true' mass parameters are unknown. Thus, all estimation plots pertaining to the
experimental trial do not show a 'true' value in order to gauge the estimate accuracy.
Furthermore, the precision envelope is no longer centered on the true value, but on
the estimated value. As in the refined simulation, with the exception of the total mass
parameter, one can be reasonably sure the true value of the estimated parameters to
be within the precision envelope.
In Figure (4-18), the moment of inertia estimates for the experimental trial closely
follow those of the second Simulation (See Figure 4-9). The convergence of the precision envelopes, and presumably the estimates for the moments of inertia for each axis
correspond to the open loop roll, pitch, and yaw oscillations at 2, 6, and 10 seconds,
respectively. In Figure (4-19), both the X-Y, and X-Z products of inertia are dependent on roll activity and thus begin to converge at 2 seconds, while the Y-Z product
of inertia converges at 6 seconds, corresponding to the beginning of pitch activity.
The center of gravity location estimates in Figure (4-20) predictably remain near
zero; however, unlike the refined simulation trial, the precision envelope for the CG
location along the Y-axis converges. This is because in the experimental setup, there
is an effective restoring force from the air bearing support in the positive Z direction at
64
the air bearing. This force is taken into account in the estimation algorithm because
there is a chance the CG will migrate from the air bearing location which would
result in a parasitic torque. This torque is accounted for in the experimental trial by
assuming a force equal to the current mass estimation multiplied by the acceleration
due to gravity acting at the air bearing in the global positive Z direction.
Again, the commanded Y-Axis diverts are identical to those in the second simulation and the resultant total mass estimation plot resembles the simulation estimation.
Most notable is the divergence of the mass estimation between the end of the initial
divert at 2 seconds and the divert oscillations at 16 seconds. This is the result of
sensor noise which is shown in Figure (4-22). In reality, there should be no X or Z accelerations; however, the relatively large noise spikes in the Z direction are remnants
of the acceleration due to gravity. Because these accelerations were measured from
on-board the microsat, the acceleration due to gravity does not necessarily remain in
the local Z direction. Thus, when filtering out the gravity component to the acceleration, slight truncation errors appear when transforming the direction of gravity to
the microsat local coordinate system. Also, the error introduced when filtering the
Euler and centripetal accelerations because the sensor is not located at the rotation
axes accounts for some anomalous accelerations.
65
Control Roll Torque N-rm
0.4
0 .2 - -
-.-.-.-
-
-
0
-0.24.
_V.
0
. .. . .. . . . .
2
4
6
8
10
12
Control Pitch Torque N-m
14
16
18
20
1
-
0
-0.5
0
2
4
6
8
10
12
Control Yaw Torque N-m
14
16
18
2)
1
..-.. ... ....... .... .
.
.. .
.-.
.
. . . ..-.
0 .5 -.. . ..-.
-
0
- 0 .5 -. . . . .. . . .
-1
0
2
4
. .. . . . .. . . .. . . . -..
. . . . .. . ..
6
8
10
12
14
16
18
20
time (s)
Figure 4-17: Experimental Trial: Control torques requested by the microsat controller. As in the simulations, the isolated spikes are caused by small attitude corrections when each closed loop attitude control period begins.
66
X axis moment of inertia kg-m
2
0.4
-
--
-
-
~
~
--
0.2
Estim ate-- Filter "precision" about estimate
..
.. . ..
.... ..... . .-..
0
-
-0.2
)
2
4
..
.
.
14
12
10
8
6
Y axis moment of inertia kg-m 2
16
18
2C
3
2
1
Estimate
-_
-
Filter "precision" about estimate
-
0
0
2
4
14
2
kg-m
inertia
of
Z axis moment
6
8
12
10
16
18
2(
3
2
1
-Estimate
0
-~~-.--.-.-
0
2
Filter "precision" about estimate-
4
6
8
10
12
14
16
18
20
time (s)
Figure 4-18: Experimental Trial: Moment of inertia estimates: the "precision" boundary for each plot is the current estimate ± Pkk with k = 1 for X-axis MOI, k = 4
for Y-axis MOI, k = 6 for Z-axis MOI.
67
X-Y product of inertia kg-m2
0.4
0.2
0
- - - - -- - - - - - - - - - - - ---Estimate
- Filter "precision" about estimate
-0.2
-0.4
0
2
4
6
8
10
12
X-Z product of inertia kg-m
14
16
18
2(
2
0.4
0.2
0
-0.2
-
-0.4
2
0
4
6
-
Estimate
Filter "precision" about estimate
8
10
12
Y-Z product of inertia kg-m
14
16
18
2C
2
0.5
0
-0.5 -
Estimate
.--.
.
i
0
2
4
6
8
-Filter
10
"precision" about estimate
12
14
16
18
20
time (s)
Figure 4-19: Experimental Trial: Product of inertia estimates: The "precision"
boundary for each plot is the current estimate ± Pkk with k = 2 for X-Y POI,
k = 3 for X-Z POI, k
=
5 for Y-Z POI.
68
CG location along X-axis m
0.05
- Etimate
-- Filter "precision" about estimate
0
-0.05
0
2
4
6
8
12
10
16
14
18
2(
CG location along Y-axis m
0.05
-
0
-0.05
-
0
2
4
Estimate
Filter "precision" about estimate
7
6
-
-
14
10
12
8
CG location along Z-axis m
16
18
2)
0.1
Estimate
Filter "precision" about estimate
0.05 -
-
0
.
. ... . . .. . ..... .-..
...-
...
..
-..
-0.051
-0.1
I
0
2
4
I
I
6
8
~
10
12
14
16
18
20
time (s)
Figure 4-20: Experimental Trial: Center of gravity deviation estimates: The "precision" boundary for each plot is the current estimate ±Pkk with k = 7 for CG along
X-axis, k = 8 for CG along Y-axis, k = 9 for CG along Z-axis.
69
Force command (Y-Axis Divert) N
1 --
-J-.-
0
-1-
--2-3
230,
0
4
6
8
1 Estimate
10
12
14
time (s)
16
18
20
Total Mass kg
-Estimate
25
--Filter "precision" about estimate
-.
20
. . .. . . .. . .
- . . .. . . ... . . ..
15
. . . . ... . ... . . .. . . .. . . .
. . . . . . . .. . . .. . . ... . . ..
-
.......
...... ...
....
.
10
-
....
. ........-
5
0
-5
0
2
4
6
8
10
time (s)
12
14
16
18
20
Figure 4-21: Experimental Trial: Command force plot showing the initial divert away
from the docking station and the divert excitations at 16 seconds. The second plot
shows the corresponding mass estimate. The mass estimate is filtered separately from
the other estimates, thus, the "precision" boundary is only the 'true' value t P2 .
70
2
X Acceleration m/s
0.15
0.05
--0.05
0
2
4
6
12
8
10
Y Acceleration m/s 2
14
16
18
20
2
4
6
12
8
10
Z Acceleration m/s 2
14
16
18
20
14
16
18
20
0.2
0
. .
-U.
D
0.4
0.2
0
.
-0.2
-0.4
0
.
2
4
6
.
10
8
12
time (s)
Figure 4-22: Experimental Trial: Observed Accelerations by the IMU on-board the
Microsat. The acceleration due to gravity is filtered from the Z acceleration plot,
as are the centripetal, and Euler accelerations because the IMU is located at a fixed
distance from the axes of rotation.
71
4.5
Comparison of Simulation and Experiment
The overwhelming similarities between the simulated and experimental results show
the simulations capable of accurately modeling the physical dynamics of the satellite
and the experimental setup. Furthermore, the viability of this RLS algorithm in
estimating the mass properties of a satellite is corroborated both by simulations and
experiments. The limitations of the RLS algorithm were revealed in the divergence
of the estimates in the presence of a low signal to noise ratio.
72
Chapter 5
Multiple Body Scenario
The key mission of the microsat is to be able to interact with other space-borne
objects. This includes not only rendezvous in space, but docking and manipulating
the space-borne object. An especially attractive capability the microsat could fulfill
would be docking with satellites that require orbit changes, then transporting them
to the desired orbit. Once docked, the microsat could perform a variety of diagnostic
tests as well as extract desirable mass property information about the docked object.
This chapter investigates a method for not only determining the combined body mass
properties, but also extracting the mass properties of the docked object alone.
5.1
Algorithm Structure
The algorithm developed in Chapter 4 for estimating the mass properties of the
microsat alone is well suited to investigating the rigidly connected multiple body
problem. The combined body merely becomes a massively perturbed single body
mass property estimation case. Thus, the estimation method developed in Chapter 3
can be used directly after docking to estimate the combined body mass properties.
Though the estimation method is identical to the single body case, the mass
property estimation algorithm can not run seamlessly during the docking maneuver.
Because the covariance terms in the P matrix in Equation (3.2) were updated for
the single body case, the filter would be unprepared for the instant errors caused
73
by the change in mass properties while docking. In other words, at each estimate
update, changes in the estimates are weighted based on the memory of previous
estimates. Once the microsat docks with a space-borne object, the weighting based
on the memory from the single body case becomes obsolete, and thus, the Covariance
matrix, P, in Equation (3.2) is reset to new initial values, effectively erasing the
memory. Because the initial combined body mass property errors are uncertain, the
initial covariance matrix values in this simulation are chosen to be arbitrarily large.
In practice, this would translate to an initial period of uncertainty until the filter
settles on the combined body mass properties.
5.2
Multiple Body Simulation
This section presents the six degree-of-freedom simulation of the docking scenario.
The objective of this simulation is to show how the estimation algorithm developed
in chapter 4 for the single body can be extended to find the combined mass properties
as well as the mass properties of the docked object.
5.2.1
Simulation Description
As in the previous simulations, the RLS algorithm requires excitations of all modes
for full observability. Thus, the mission plan for this docking scenario requires roll,
pitch, and yaw excitations as well as translational excitations before docking and
after rigidly docking with the space-borne object. Figures (5-1) and (5-2) illustrate
the mission plan for the docking scenario.
A major difference between this mission plan and the mission plan of the single
body simulations is the deletion of closed loop control periods. Because the simulations in Chapter 4 were designed to be executed experimentally on the microsat
testbed, periods of attitude reorientation were necessary between the open loop oscillations. This assured the microsat would not rotate beyond the range of the spherical
air bearing limits. However, this docking simulation is not constrained to the experimental setup and thus, need not re-acquire the initial attitude between open loop
74
Microsat
(a)
Space-borne
object
0 - 8 Seconds:
Microsat identifies mass
properties alone.
(b)
8 - 12 Seconds:
Microsat docks with
space-borne object
'Z
12 - 36 Seconds:
(c)
identifies mass
Microsat
properties of combined
body
Figure 5-1: Diagram illustrating the three main control phases during the docking
mission. First, the Microsat executes a series of open loop rotations and diverts (a),
then docks with the space-borne object (b) and again, executes a series of open loop
rotations and diverts (c) to identify the combined body mass parameters.
75
Open Loop
Ro 11 Oscillations
Open Loop
Roll Oscillations
Dock;
Open Loop
Pitch Oscillations
Combined body
mass properties
Open Loop
Yaw Oscillations
take effect
Open Loop
Y-Axis Diverts
0
2
4
6
Open Loop
Pitch Oscillations
8 9
Open Loop
Yaw Oscillations
Reset Filter
Covariance
matrix
Open Loop
Y-Axis Diverts
12
16
20
24
36
Time (s)
Indicates Control Action To
Return To Initial Attitude
And Hold
Figure 5-2: Timeline of the multiple body docking mission detailing all control actions.
excitations. By the same logic, the docking simulation does not require specific oscillation pulses that maintain the initial attitude as in the previous simulations. Thus,
pseudorandom pulses with a maximum duration of 0.08 seconds are used for the open
loop excitations. The duration of the open loop excitations after docking are extended to allow time for convergence of the significantly larger estimate deviations.
Another major difference between this simulation and the single body simulation is
the attitude control system (ACS) jets and Y-axis divert jets are much stronger. This
is to be expected for a microsat to be able to manipulate much larger satellites.
Table (5.1) shows the simulation parameters that remain unchanged throughout
the docking simulation. The initial 'true' and final estimated simulation properties
for the microsat alone, combined body, and docked object are shown in Tables (5.2)
and (5.3). For simplicity, in this simulation the microsat docks directly to the center
of the face of a 2x2x2 meter cube with a density of 3 kg/m 3 . Before docking, all the
mass properties are defined with respect to the original microsat CG location. The
76
Microsat Non-mass Property
Value
Moment arm about X-axis from zero deviation CG to ACS Jet [m]
Moment arm about Y-axis from zero deviation CG to ACS Jet [m]
Moment arm about Z-axis from zero deviation CG to ACS Jet [m]
ACS jet thrust [lbf]
0.1475
0.2575
0.2575
1.00
3.00
0.0001
0.02
Divert jet thrust [lbf]
Gyro noise 6 - magnitude
Accelerometer noise 6 - magnitude
Table 5.1: Non-mass related microsat parameters for the multiple body docking simulation. These properties remain the same after docking with the space-borne object.
inertia matrix in particular is also defined along the same axes as the microsat bodyfixed axes. After docking, however, the inertia matrix is defined at the combined
body CG location with axes still aligned with the original microsat body-fixed axes.
The combined CG location is defined in the microsat body-fixed axes with respect to
the original microsat CG location.
5.2.2
Multiple Body Simulation Results
As implied from the control plan in Figure (5-2), the pseudorandom torque oscillations
proceed directly from roll to pitch to yaw in the first 6 seconds. Closed loop attitude
control induces the large torques seen between 8 and 12 seconds. At 12 seconds, the
open loop torque sequences are repeated in the combined body configuration, but for
longer durations.
From 0 to 9 seconds, the docking mission proceeds very much like the previous
single body simulations. All the mass parameters are excited by open loop oscillations. Figures (5-4) through (5-7) show the expected estimations from the filter and
mimic earlier simulations. The CG estimation plot, Figure (5-6), shows the precision
envelope to be unperturbed for the CG location along the X and Z axes until 6 seconds when the Y-diverts occur. As in previous simulations, the estimation of the CG
location along the Y-axis remains unobservable. Table (5.2) shows good convergence
of the estimated mass properties to the 'true' mass properties at 8 seconds when the
77
Control Roll Torque N-m
2
.
.
.
0
-2
0
5
10
3
15
20
Control Pitch Torque N-m
~~1~~
Iv''
'HI
1 0
-2
)
5
10
25
30
35
I
..........
15
20
25
Control Yaw Torque N-rn
30
35
3
-
2
.
-
.
1
0
-1
-2
-3
0
5
10
15
20
time (s)
25
30
35
Figure 5-3: Docking Simulation: Control torques on each axis resulting from execution
of the docking mission plan. The closed loop control torques to return the microsat to
initial attitude occur at 8 seconds. At 9 seconds, the mass properties of the microsat
immediately change to the combined body mass properties (docking), inducing large
control torque commands.
78
Mass property of microsat alone
(calculated at 8 seconds)
'True'
Value
Estimated
Value
Moment of Inertia Ixx [kgm2]
0.35
0.35
Moment of Inertia Iyy [kgm2]
2.5
2.50
Moment of Inertia Izz [kgm2]
2
2.00
Product of Inertia Ixy [kgm2]
0
0.00
Product of Inertia Ixz [kgm2]
0
0.00
Product of Inertia Iyz [kgm2]
0
0.00
0
0.00
0
0.00
divert jets fire [m]
0
0.00
Total Mass [kg]
20
19.55
Center of Gravity X-axis deviation
from point through which all
divert jets fire [m]
Center of Gravity Y-axis deviation
from point through which all
divert jets fire [m]
Center of Gravity Z-axis deviation
from point through which all
Table 5.2: Overall 'true' and estimated mass parameters for the microsat alone before
docking at 9 seconds.
induced excitations for the single body are complete.
At 9 seconds, the single body mass properties immediately change to the new
combined mass properties. Though the error for the moment of inertia estimates are
high, there are no immediate jumps in the updated estimates. This is because, the filter covariance matrix is not reinitialized until second 12, when the torque oscillations
begin. When the covariance matrix was reinitialized at second 12 to an arbitrary
value based on the prediction of the new estimate error (See Appendix B and C), the
estimations immediately react to the new mass parameters. In some of the estimation
graphs, it seems the precision envelopes converge prematurely. This occurs because
the noise reduces the covariance terms with each iteration. This is especially apparent for the moment of inertia estimations in Figure (5-8).
The moment of inertia
estimates do not seem to follow the precision bounds for two reasons. First, the noise
reduces the precision envelope before intended excitations take place, thus reducing
the effectiveness of the open loop excitations. Second, the reinitialization of the co-
79
2
X axis moment of inertia kg-m
0.4
%:~..
.
. ...
-
-- - - - - ----
-
0.2
Estimate
0
-
... .
L
-0.2
)
.
1
2
.
True value
- -
Filter "precision"
.
.
3
4
5
6
Y axis moment of inertia kg-rn 2
.
7
.
.
8
3
2
1
-.
- .........................
0
()
1
2
-
3
4
5
6
Z axis moment of inertia kg-m2
-
Estimate
True value
Filter "precision"
7
8
9
3
2
. . . .. . .
1
.
. ..
. .
. .
.
.
. .
.
. .
.
.
. .
.
.
.
-
---
0
0
1
2
3
4
5
6
Estimate
--- True value
-- Filter "precision"
7
8
9
time (s)
Figure 5-4: Docking Simulation (0-9 Seconds): Three axis moment of inertia estimate
plot showing the convergence from initial moment estimates of zero to the true moment represented by the horizontal line. The "precision" boundary for each plot is
the 'true' value ±Pkk with k = 1 for X-axis MOI, k = 4 for Y-axis MOI, k = 6 for
Z-axis MOI.
80
X-Y product of inertia kg-m2
-True
-.
.
0.02 -
Estimate
-.
. ..
0.04
.
.
.
..
--
value
Filter "precision"
0
-0.02
-
-
.. .........
. .. . . .
-0.04
-0.06
1
0
5
6
3
4
2
kg-m
X-Z product of inertia
2
8
7
9
0.2
--
.Estimate
True value
Filter "precision"
..-...................
0.1
0
..
.
.
...
.
..
..
.
..
..
.
..
.
.
.
.
.
.
.
.
..
-0.1
-0.2
1
0
2
3
4
5
6
Y-Z product of inertia kg-m 2
-
0.5 -.
7
9
Estimate
-
True value
-\-
8
"precision"
-Filter
0
.
--..
-
--
.
-0.5
0
1
2
3
4
5
6
7
8
9
time (s)
Figure 5-5: Docking Simulation (0-9 Seconds): Three inter-axis product of inertia
estimate plot showing the convergence from zero initial values to the true product
represented by the horizontal line. The "precision" boundary for each plot is the
'true' value ± Pkkwith k = 2 for X-Y POI, k = 3 for X-Z POI, k = 5 for Y-Z POI.
81
CG location along X-axis
0.05
Estimate
True value
Filter "precision"
-
-
0
-0.05
0
1
2
3
7
4
5
6
CG location along Y-axis
8
4
2
-------
-
Estimate---------True value
-- Filter "precision"
--
0
-4
1
0
2
3
4
5
6
CG location along Z-axis
7
8
0.05
Estimate
True value
Filter"precision"
-
--
0
-0.05
-
0
1
2
3
4
5
6
7
~ -~-
8
9
time (s)
Figure 5-6: Docking Simulation (0-9 Seconds): Center of gravity (CG) location estimate with respect to the initial CG location corresponding to the focus of the divert
thrusters on the microsat. The "precision" boundary for each plot is the 'true' value
± Pkkwith k = 7 for CO along X-axis, k = 8 for CO along Y-axis, k = 9 for CO
along Z-axis.
82
Force command (Y-Axis Divert) N
15
-
10 k.
-
..
5
0
-5 F
. .
.
.
. .
.
.
.
. .
.
. ..
. .
.
.
.
.
.
-10
-15'
0
2
1
3
5
4
6
7
9
8
time (s)
Total Mass kg
30
I
- -
- - I
I
Estimate
25
True value
Filter "precision"
-
-
20 --
-.-.--
I-
-
.--
-
-
.
15 F
... . . . . . . . ... . . . . . . . . . . . . .
10
..................
5F
-.
. .
..
-.
..
. .. . .. ....... . . .-
... . . . . . . . . .
0
-5
t
0
i
1
2
3
4
5
6
7
8
9
time (s)
Figure 5-7: Docking Simulation (0-9 Seconds): Commanded force plot showing no
activity until the divert oscillations at 6 seconds. The docking simulation contains
diverts only in the Y (lateral) direction. The mass plot shows the estimated total mass
for the microsat alone before docking. Interestingly, the precision envelope begins to
converge before any divert activity due to noisy inputs from the accelerometer. Since
the mass estimate is filtered separately from the other estimates, the "precision"
boundary is only the 'true' value ±P 2 .
83
Mass property of combined body
(calculated at 36 seconds)
'True'
Value
Estimated
Value
Moment of Inertia Ixx [kgm2]
16.35
16.23
Moment of Inertia Iyy [kgm2]
62.13
59.69
Moment of Inertia Izz [kgm2]
61.63
59.17
Product of Inertia Ixy [kgm2]
0
-0.28
Product of Inertia Ixz [kgm2]
0
0.28
Product of Inertia Iyz [kgm2]
0
-0.04
1.09
1.05
0
0.00
divert jets fire [m]
0
0.00
Total Mass [kg]
44
42.68
Center of Gravity X-axis deviation
from point through which all
divert jets fire [m]
Center of Gravity Y-axis deviation
from point through which all
divert jets fire [m]
Center of Gravity Z-axis deviation
from point through which all
Table 5.3: Overall 'true' and estimated mass parameters for the multiple body docking
simulation.
variance matrix, and thus, the precision envelope takes place with no knowledge of the
new mass parameters; the correct initial covariance matrix can not be known. Figure
(5-8) shows reinitialization at 12 seconds which underestimated the error. Though
the covariance matrix was not initialized perfectly, the mass estimates still behaved
well, showing close convergence during the corresponding open loop excitations.
From the docking at second 9 until the end of the mission at second 36, the combined mass parameter estimates seem to converge well. Though both the estimates
for the single body and the combined body show good convergence, the calculations
for the mass properties of the docked object seem to be less accurate. This is because
the errors from both estimation trials are stacked in the calculation. Furthermore,
producing a single number from a convergence plot introduces error. In this simulation, the proposed values for the microsat alone and the combined body are calculated
at 8 and 36 seconds, respectively. These final values are based on the last estimate
which remained within a small tolerance for the duration of 2 seconds. Thus, there
84
X axis moment of inertia kg-m2
20
15
~-
-
---
- --
-
-
-
10
5
Estimate
--
True value
0
Filter "precision"
-
-5
10
15
20
25
30
35
2
Y axis moment of inertia kg-m
60
-~
.
.
.
.
.
.
.
.
.
...
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
..
.
.
.
.
.
.
.
40
-.
20
.
..-Estimate
--
True value
Filter "precision"
0
10
15
20
25
Z axis moment of inertia kg-m
30
35
2
60
40
20
-
0
10
15
-
-
25
20
--..Estimate
True value
- - Filter "precision"
30
35
time (s)
Figure 5-8: Docking Simulation (9-36 Seconds): Three axis moment of inertia estimate plot showing the convergence from initial moment estimates of zero to the
true moment represented by the horizontal line. The Y and Z axis MOI estimates
are not within the precision bounds for two reasons: the covariance matrix was not
reinitialized at a high enough value to account for the large estimate error, and until
directly excited, the low signal to noise ratio prevented the estimates from converging.
However, once excited, both the Y and Z axis MOI estimates converged near the true
values.
85
X-Y product of inertia kg-m2
1.5
Estim ate
-True value
-........
1
.....................................
0.5
-
Filter "precision"
0--
-1.5.
10
15
20
25
X-Z product of inertia kg-rm2
30
35
1.5
- --
True value
Filter precision
0.5----
- 0 .5
Estimate
..
.-.
0.5~N.
10
-.. . .
. . .. .
. . . .-..
-
.. .. . .- . . ... . .-.. .
Fle"pcion
30
35
.....................................--
15
20
25
Y-Z product of inertia
.1..........................................................
kg-m2
1.5
1 -1-1
0.5 -.
-1.5
. . .. . .
0
-. - -
10
..
represented
horizonl by
15
..
........................
............
. . .... . . . . .. . .
-nEstimate
- True value
Filter "precision"
. .~-. .
-
20
25
-
30
...
35
time (s)
Figure 5-9: Docking Simulation (9-36 Seconds): Three inter-axis product of inertia
estimate plot showing the convergence from zero initial values to the true product
represented by the horizontal line.
86
CG location along X-axis
-..
10
5
. .~..
.. . ..-.. . .
-
E stim a teTrue value
Filter "precision"
.......
.-........................
0
-5
-10
10
10
15
20
25
CG location along Y-axis
30
r
35
Estimate
- -. -True
5
--
--
value
Filter "precision"
0
-5
.................................................
.
-~~~7
-
--
-7--
-10
10
15
20
25
30
35
CG location along Z-axis
Estimate
10
5
-
*I
.... ................................
True value
ilter "precision'
0
-5
... ~.............
...............................................
..............
-10
10
15
20
25
30
35
time (s)
Figure 5-10: Docking Simulation (9-36 Seconds): Center of gravity location estimate with respect to the initial CG location corresponding to the focus of the divert
thrusters on the microsat. At 9 seconds, the overall CG shifted along the X-axis toward the docked body. At 12 seconds, the estimation precision was reinitialized and
does not reconverge for the X and Z CG location until the open loop Y-axis diverts
for the combined body at 24 seconds; the Y CG location precision never converges
(See Figure 4-5).
87
(a)
Force command (Y-Axis Divert) N
15
10
5
0
-5
-10
-15
10
15
20
25
30
35
time (s)
(b)
Total Mass kg
60
..
50
.
.
.-.-.-
. . . .- . .
.
40
-
30
-
- -
20
Estimate
10 -. .. . .. . . .
.
0
10
-
- -15
. ..
. .. . .
.
.
-..
-...
~.Tru e v a lu e
- - Filter "precision"
20
25
30
35
time (s)
Figure 5-11: Docking Simulation (9-36 Seconds): Commanded force plot showing the
Y-axis divert oscillations at 24 seconds. Plot (b) shows the estimated total mass
for the combined body. The mass estimate actually diverges initially since there are
no excitations other than accelerometer noise (which actually converge the precision
envelope). At 24 seconds, the divert oscillations induce good convergence of the mass
estimate.
88
Calculated mass property of docked body
(calculated at 36 seconds)
'True' Estimated
Value Value
Moment of Inertia Ixx [kgm2]
16
15.93
Moment of Inertia Iyy [kgm2]
16
17.73
Moment of Inertia Izz [kgm2]
16
17.70
Product of Inertia Ixy [kgm2]
0
-0.20
Product of Inertia Ixz [kgm2]
0
-0.28
Product of Inertia Iyz [kgm2]
0
-0.04
2
1.93
0
0.00
divert jets fire [m]
0
0.00
Total Mass [kg]
24
23.13
Center of Gravity X-axis deviation
from point through which all
divert jets fire [m]
Center of Gravity Y-axis deviation
from point through which all
divert jets fire [m]
Center of Gravity Z-axis deviation
from point through which all
Table 5.4: Mass properties of docked object calculated at 36 seconds and the 'true'
docked object mass properties showing reasonable estimation.
is an error with magnitude on the order of that tolerance associated with extracting
the asymptotic value from a convergence plot.
5.3
Experimental Proposal
In this section, we propose a method for experimentally verifying the mass property
estimation scheme for multiple bodies. This experimental setup takes full advantage
of the five degrees of freedom allowed with the spherical, linear air bearing combination. Instead of placing the microsat and the target object on separate air bearings,
which allows rotations only about the Z and X axis when rigidly connected, both the
microsat and the target object are fixed rigidly together then placed on a single air
bearing (See Figure 5-12). If the 'docked' object is fixed at an appropriate distance
from the microsat, the combined CG can be located outside the bodies of both the
microsat and the target object. Thus, an external ball can be mounted on the same
89
Rigid Connection
"Docked" Object
Microsat
GD
Combined C
location
Air Bearing / Support
Figure 5-12: Proposed experimental setup: the CG location is external to both bodies,
thus allowing the ball which is fixed to the rigid connecting bracket to be located
between the two bodies. This experimental setup allows full rotational freedom and
translational freedom in the plane of the glass table (5 degrees of freedom).
member which connects the microsat rigidly to the target object. Assuming the ball
is at the combined CG location, the combined body has five complete degrees of
freedom.
90
Chapter 6
Conclusions and Recommendations
for Future Work
6.1
Conclusions
A real-time mass property estimation algorithm, designed to be implemented on satellites, was developed in this thesis. After laying the theoretical basis for the physical
dynamics and computational algorithm, the design was then tested in simulation and
verified by experiment. A primary objective of this research is to provide a convenient,
readily executable method of determining the inertia matrix, center of gravity location
and total mass of an experimental vehicle on the microsat test bed at the Lawrence
Livermore National Laboratories. The advantage of this estimation scheme is the relatively low cost in terms of hardware and mass requirements; the algorithm requires
only computation time and real-time IMU and controller data. In autonomous space
applications such as satellite mass property estimation, the addition of no mass for
this capability is especially attractive.
As we begin placing more satellites into space, the autonomous inspection and
manipulation of satellites will become an increasingly desirable capability. Thus, mass
property estimation of combined docked bodies and the docked objects themselves
was explored as a potential application for this estimation algorithm. The generic
nature of this algorithm allowed easy adaptation to the multiple body scenario.
91
In both the single body and combined body cases, the estimation algorithm converged very close to the mass properties in simulation. The experimentally tested
single body case corroborated the simulation results.
The main limitation of this algorithm is the susceptibility to noise. The experimental test showed good convergence during planned excitations; however, during
periods of low signal to noise ratio, the estimates tended to diverge from the real
values. Thus, in order to converge to the correct mass parameter values, the vehicle
must execute the mass property estimation algorithm only when there is sufficient
vehicle motion to maintain persistency of excitation. In order to determine the mass
properties of the docked object, the estimation algorithm performs a calculation based
on previous estimates. As a result, the error is especially large in evaluating the mass
properties of the docked object as compared to the error in estimating the single and
combined body mass parameters.
6.2
Recommendations for Future Work
As suggested in Chapter 5, the solution to experimentally testing the multiple body
mass property estimation is easily implemented given the current experimental setup.
By using a single spherical air bearing at the combined CG for the rigidly connected
body, the problem of parasitic torques from a CG that has migrated from the air
bearing location is circumvented.
Progress can still be made in making this estimation algorithm more robust. As
explained in Chapter 5, the change in covariance caused by instantaneous changes in
mass properties, such as docking, can not be predicted. Thus, a method of finding
the optimal re-initialization of covariance values for an instantaneous mass property
change remains to be investigated.
The solution to this problem would also allow
the estimation algorithm to be transferred and executed on any vehicle. Once the
estimation scheme proves stable in these cases, the algorithm may perform a more
direct role in the control of the satellite in the form of an indirect adaptive controller
based on the current mass property estimations. Instead of running isolated and in the
92
background, the estimations could be used immediately as 'true' satellite parameters
in the controller.
93
94
Appendix A
Real-time Estimation Code
Integration and Raw Trial Data
The mass property estimation algorithm was simulated and post-filtered off-line in
MATLAB. In order to test the real-time implementation, the estimation code was
written in the C, the language for the actual microsat control code, then downloaded
to a previously untested microsat. The raw results in Figure A-1 show the moment of
inertia estimates to converge on reasonable mass property estimates for this microsat.
Figure A-2 shows the microsat rotations corresponding to the simulation time. By
visual inspection, the moments of inertia seemed to be about one half of that of the
microsat examined in the previous trials in this thesis, to which the estimates seem
to converge. The estimated values, 0.23, 0.63, 1.0, corresponding to the roll, pitch,
and yaw moments of inertia, respectively are reasonable by visual inspection.
The moment of inertia plot shows initial convergence before the open loop excitations because the divert away from the docking position produced large torques. The
ACS jets were not yet balanced and tested at the time of the trial, so the 'true' thrust
values were based on coarse estimations. The main purpose of this trial was to prove
the estimation code could be integrated with the flight code and run in real-time on
board the microsat CPU. This was achieved as well as producing reasonable mass
property estimations.
95
MOT FINAL Run Jan 7,0)
Y, 04
1Tp
1a
1o
4p
tt
( I,7fWAI
""NIX
,M
1
42
(1Ix00
x
)60%X E*0000
WO"
Figure A-1: Real-time moment of inertia estimate plots for previously untested microsat.
96
MOI FINAL Run Jan 7,00
Y x10
tI4 XAX)i
7t..XE
qYaw&O02Vztph
.....
.
-.
....
--
---
--
300OOI
I
01*00
4r
1450A)0SAX
1~
20AX1
ii
IfO)OI
-3f0*0
tl
x
Figure A-2: Real-time attitude plot for previously untested microsat. The three plots,
qPitch, qRoll, and qYaw are the first three terms of the microsat quaternion which
represents the axis of rotation at that time.
97
98
Appendix B
Initial Covariance Matrix
Determination
B.1
Covariance Matrices Used in Simulations
As mentioned in Chapter 4.1, the initial error covariance matrix, P, measurement
noise matrix, R, and model noise covariance matrix,
Q are
often tested by trial and
error through simulation to determine the best values. Instead of using a single ten
parameter filter, two separate, simultaneous filters were used to avoid producing an
unnecessarily sparse regressor matrix, H, and thereby, avoid unobservability (See
Chapter 3.2). Thus, the first filter estimated nine parameters: six inertia matrix parameters, and three center of gravity location parameters. The second filter estimated
only the total mass. This division makes intuitive sense because the inertia matrix
and CG location estimates depend only on controller and angular velocity data, and
the total mass estimate depends only on the controller and linear acceleration data.
99
The simulations in this thesis used the following initial covariance matrix values:
P1 =
10
0
0
0
0
0
0
0
0
0
10
0
0
0
0
0
0
0
0
0
10
0
0
0
0
0
0
0
0
0
10
0
0
0
0
0
0
0
0
0
10
0
0
0
0
0
0
0
0
0
10
0
0
0
0
0
0
0
0
0
10
0
0
0
0
0
0
0
0
0
10
0
0
0
0
0
0
0
0
0
10/
P2
5x100
0
Q1=
9
0
1 x 100
9
(B.1)
(50)
(B.2)
0
0
0
0
...
0
0
0
0
...
1x10-9
0
0
0
...
0
0
...
---
5 x 10-
9
0
0
0
0
0
0
0
1x10- 9
0
0
0
0
0
0
5x10-9
0
0
0
0
0
0
.-
0
0
0
0
0
0
...
0
0
0
0
0
0
...
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
I x 100
0
Q2
=(I1X 10-5 )
100
9
1 x 100
9
...
(B.3)
0
1 x 10
9
/
(B.4)
5x
R,
R2 =
0
10-2
0
5x
10-
0
2
0
0
0
5 x 10-2
5 x 10-2
0
0
0
5 x 10-2
0
0
0
5 x 10-2
(B.5)
(B.6)
Theoretically, the actual initial error is unknown and the high initial rms error
terms ensure the Kalman gain starts at a large enough value to be effective. Figure
(B-1) shows the moment of inertia estimate of the microsat using the above covariance
values (Same as Figure (4-3)).
The following variations in initial covariance matrices are taken with respect to
the values used in simulation. By varying only one covariance matrix at a time, the
specific effect of the variation is revealed.
For this purpose, the matrix values in
Equation (B.1) through Equation (B.6) are considered 'optimal'.
101
2
X axis moment of inertia kg-m
U
0.8
...........
0.6
0.4
0.2
-~... .. .. .. .
. ... .
. .
Estimate
. . .
-
0
-
-
True value
Filter "precision"
-0.2
0
2
4
6
8
10
14
12
16
18
20
2
Y axis moment of inertia kg-m
5
4
3
2
1
-True
0
-.-.-
)
2
4
-Estimate
value
Filter "precision"
8
10
12
14
6
Z axis moment of inertia kg-m2
16
18
2C
-
5
4
3
2
1
-
0
...
......
. . ... . . ... . . ... . . ...... ..
0
2
4
6
8
10
12
-Estimate
.True value
Filter "precision"
14
16
18
20
time (s)
Figure B-1: Simulation 1: Three axis moment of inertia estimate plot showing good
convergence from initial moment estimates of zero to the true moment represented
by the horizontal line. The "precision" boundary for each plot is the 'true' value
± Pkk with k = 1 for X-axis MOI, k = 4 for Y-axis MOI, k = 6 for Z-axis MOI. The
"precision" remains nearly constant from 12 to 20 seconds because there is no direct
excitation of these modes during that period.
102
B.2
Effects from Variation in Initial P
In general, the initial P matrix acts as the initial gain. As seen in Figure (B-2), when
the initial P matrix is four orders of magnitude lower than the 'optimal' values, the
gain on the filter is low resulting in sluggish convergence of the estimates. On the
other hand, an initial P matrix with elements two orders of magnitude higher than
'optimal' still gives reasonable estimates as seen in Figure (B-3).
The only major
difference is the higher volatility of the estimates as seen in the large spike near the
beginning of the Z axis moment of inertia estimate. This is expected because the
higher initial P matrix values result in a higher gain.
103
2
X axis moment of inertia kg-m
1
-Estimate
. ..
T ru e v a lu e
S
Filter "precision"
.....
....
...
..
......
.. .... ..
0
-0.5
-
-
-
0.5
)
2
12
14
6
8
10
Y axis moment of inertia kg-m 2
4
18
16
2(
5
4
-E........... .
3 -- .. ....
stim ate
2
True value
Filter "precision"
-. -
1
0
. .
)
2
4
.
.
.
.
.
.
. .
. .
. . .
.
I
.
.
6
8
10
12
14
Z axis moment of inertia kg-m2
.
. .
16
. .
.
. .
.
.
18
.
2(
5
4
.
3
..
...
--
2 --..............................
1
0
0
-
...
.......-.. . .
.. . .
-..
.
......
. .. . . .. . . .. . . .
2
4
6
8
10
Estimate
True value
Filter "precision"
. . .... .-..
.. . . .
.. .. . .. . ..... . ... .. .
12
14
16
18
20
time (s)
Figure B-2:
Simulation 1: Three axis moment of inertia estimate plot with lower
initial P values (diagonal of 0.01's).
104
X axis moment of inertia kg-rm2
1
0.5
-f
.
. .
0
. . . . . .. . . ....
.... . . . . . .
. . . . . . . .
. . . . . . ..
-
)
2
4
. . . . . . . .
-. Estimate
-. . ..
-
-0.5
.. . . . . . .
-
10
12
14
8
6
Y axis moment of inertia kg-m 2
True value
Filter "precision'
16
18
2C
5
4
~-
-
-
-
- - - - - - -- - -
3
2
1
-
0
)
2
4
-- Estimate
--- True valueFilter "precision"
-
6
8
10
12
14
Z axis moment of inertia kg-in 2
16
18
2(
5
4
-
3
-
2
-- Estimate
1 S.........
True value
-
-
0
I
0
2
Filter "precision"
I||
4
6
8
10
12
14
16
18
20
time (s)
Figure B-3: Simulation 1: Three axis moment of inertia estimate plot with higher
initial P values (diagonal of 1000's).
105
B.3
Effects from Variation in Initial
The Q matrix accounts for the process noise. Lowering the
Q
Q
matrix to zero does
not noticeably change the moment of inertia estimate plots (Figure (B-4)). The
Q
matrix prevents the P matrix from tending to zero as seen in Equation (3.2); because
a zero
Q matrix
does not affect the estimate, and the covariance does not converge
to zero, a logical conclusion is the second term in Equation (3.2) is also near zero
when the "precision" bounds are constant. This makes sense because the constant
"precision" bounds correspond to periods of no excitation of these modes, resulting
in zero elements in the H matrix in the second term of Equation (3.2). When the
Q
matrix is seven orders of magnitude higher than 'optimal' as in Figure (B-5),
the "precision" bounds tend to diverge during periods of inactivity.
sense because the
Q
The excessively high
This makes
matrix adds a constant with each update of Equation (3.2).
Q
matrix prevents the estimates shown in Figure (B-5) from
converging on the true values.
106
X axis moment of inertia kg--m2
1
-
0.5
-
0
-
Estimate
True value
_
-
-0.5
)
2
4
6
8
10
12
14
Y axis moment of inertia kg-in 2
Filter "precision"
16
18
2(
5
4
. ..
.. .
- - - - -. .
.
.
-- -
.
..
. .
.
. . .
- -
- - ---
.
..
. .
.
. .
.
- - -
3
2
~
1 - ...
---
. . . . .
-
Estimate
True value
Filter "precision"
0
2
0
4
6
8
10
12
14
Z axis moment of inertia kg-m 2
16
18
2(
5
4
3
2
.--
1
~
.-
0
2
0
-
..-
.
4
Estimate
True value
Filter "precision"
...
6
8
10
12
14
16
18
20
time (s)
Figure B-4: Simulation 1: Three axis moment of inertia estimate plot with lower
initial
Q
values (zero matrix).
107
X axis moment of inertia kg-rn2
3
Estimate
2 . ........
.......
..................
1
......-
.
.-
. ..8 .. .
10
. .
True value
YFilter "precision "
.-
Trueva.
-.
-1
0
2
0
4
.... 6
. .
Y axis moment of
14 . *
. ..12.. . ..
inertia kg-rn2
16 "precision"
18
..
Filter
20
Estimate
0- 6
-
4
-f
- -
---
-..-
True value
Filter "precision"
.- .. ..- . -..-..-..-.-..- ... .-
00
2
4
6
8
10
12
14
Z axis moment of inertia kg-
2
16
18
20
m
Estimate
6
~tl-lTrue
value
Filter precision"
00
2
4
6
8
10
time (s)
12
14
16
18
20
Figure B-5: Simulation 1: Three axis moment of inertia estimate plot with higher
X 10-2'S).
initial Q values (diagonal of I
108
B.4
Effects from Variation in Initial R
The R matrix accounts for the sensor noise. Though there is no noise in this simulation, the R matrix can not be reduced to zero. This would result in a singular
matrix and an infinite Kalman filter gain as seen in Equation (3.1).
Figure (B-6)
shows the effect of lowering the magnitude of the R matrix by four orders of magnitude. Intuitively, lowering the R matrix indicates a greater confidence in the observed
errors. Thus, the estimates as seen in Figure (B-6) are more volatile and subject to
anomalous errors. As expected, when the R matrix values are increased by 3 orders
of magnitude as in Figure (B-7), the estimates, as well as the "precision" bounds do
not exhibit good convergence.
109
X axis moment of inertia kg-m
2
0.8
0.7
0.6
ile "rciin
-
Estimate
0.5
True value
Filter "precision"
- 0
2
4
6
8
10
12
14
Y axis moment of inertia kg-m 2
16
18
2(
4.5
4
3.5
-- Estimate
T ru e va lu e
-- Filter "precision"
..
....
...... .. .... .. .... .-.
3
2.5
0
2
4
6
8
10
12
14
Z axis moment of inertia kg-m 2
16
18
I
I
I
4.24.1 -
4
........... .....
...
..
3.9-
-- True value
- Filter "precision"
3.8 - .. ...-...-..-..-..-..-..-..-
3.7-
0
2
Estim a te-
4
6
-'.-
8
10
time (s)
12
-
'
14
16
18
20
Figure B-6: Simulation 1: Three axis moment of inertia estimate plot with lower
initial R values (diagonal of 5 x 10- 6's).
110
X axis moment of inertia kg-in 2
1
0.5
0
Estimate
-True value
Filter "precision"
.--
-0.5
)
2
4
6
8
10
12
14
Y axis moment of inertia kg-m 2
16
-
20
18
5
4
3
2
1
-Estimate
- - True value
Filter "precision"
-_.
-
0
)
-
- -
2
-
-
--
4
.... ....
.........-..-
--
6
8
10
12
14
Z axis moment of inertia kg-m 2
16
18
2(
5
4
..I. . .. .
... . . . . . .
. . .I
.....I
..
.
.I . .. . .
I
. . . . .I
.. . . . .
3
2
1
...........-
0
0
... .
.....
. ..
...
. .... .
2
4
6
8
10
....
12
Estimate
.-- True value
Filter "precision"
-..
14
16
18
20
time (s)
Figure B-7: Simulation 1: Three axis moment of inertia estimate plot with higher
initial R values (diagonal of 5's).
111
112
Appendix C
Simulation Code
C.1
C.1.1
Matlab Docking Simulation
Initialization and Support Functions
Initialization Function
init .m
global KKVIXYZ WDOT;
% initialize states
KKVstate= zeros(1,15);
KKVstate(1) = 1; %n
KKVstate(2) = 0; %ql
KKVstate(3) = 0; %q2
KKVstate(4) = 0; %q3
KKVstate(5) = 0; %wl
KKVstate(6) = 0; %w2
KKVstate(7) = 0; Xw3
KKVstate(8) = 0; Xx
KKVstate(9) = 0; %y
KKVstate(10)= 0; %z
KKVstate(11)= 0; Xvx
KKVstate(12)= 0; %vy
KKVstate(13)= 0; %vz
KKVstate(14) = 20; XKKVTotalMass
KKVstate(15) = 2; XKKVJetWetMass
% Time management
SIMTIME= 0;
SIMTOTAL= 90.1;
Basetimestep= 0.0025;
113
% True Microsat Inertia
KKVIXYZ= [0.35 0 0; 0 2.5 0; 0 0 2.01;
% ACS Jet force
MomArmX= 0.1475; %meters
MomArmY= 0.2575; %meters
MomArmZ= 0.2575; %meters
ACSFjet= 1* 4.4482;
%
ACSJetDutyCycle= 0.05; %
DIVFjet= 3* 4.4482;
%
(theoretical)
(theoretical)
(theoretical)
Newtons (1 lb)
seconds (duty cycle)
Newtons (3 lb)
%Kalman Filter parameters
KKVIXYZest=diag([.15 1.3 1.31);
WDOT=[1e-9;1e-9;1e-9];
w1= KKVstate(5);
w2= KKVstate(6);
w3= KKVstate(7);
PHI = [ 0
wl*w3
-wi*w2
OMEGA
=
-wi*w3
w2*w3
wi^2-w2^2
wi*w2
w3^2-w1^2
-w2*w3
-w2*w3
0
wl*w2
w2^2-w3^2
-wl*w2
w1*w3
w2*w3;
-wi*w3;
0]';
-[ 0 -w3 w2;
w3 0 -wi;
-w2 wi 0];
% Initial desired torques and forces
U = [0;0;0];
F = [0;0;0];
Uave=U;
% Covariance matrices
KFP1=diag([10, 10, 10,10, 10, 10, 10, 10, 10]);
KFP2=[2500];
KFQ1=diag([5e-9, 1e-11, 1e-11, Se-9, 1e-11, 5e-9, le-9,
KFQ2=[4e-2];
KFR1=diag([5e-2 5e-2 5e-2]);
KFR2=diag([3e1 3e1 3e1]);
1e-9,
% Initial conditions for WDOT calc.
prevW=[0;0;0];
beenhere=0;
% For saving micosat and estimator state at each iteration
AvemACSJetKKV=[0;0;0];
WDOTsum=[0;0;0];
WDOTave=[0;0;0];
Wsum=[0;0;0];
Wave=[0;0;0];
Accelsum=[0;0;0];
KKVaccelave=[0;0;0];
KKVaccelprev=[0;0;0];
vDot=[0;0;0];
114
le-9]);
currMass= KKVstate(14)*0;
CGx=0;
CGy=0;
CGz=0;
AvefDIVJetKKV=[0;0;0];
PHIlsave=[];
PHI2save=[];
KFPlsave=[];
KFP2save=[];
Forcesave=[];
% SET CG LOCATION (offset from nominal location)
CG = [0 0 0];
% Noise
Parameters for simulation
Wavenoisemag= 0.0001;
Accelnoisemag= 0.02; %abs(Accelsum)*.05;
FYDivertDT=0;
% Mass property values after docking
M12=44;
CG12=[12/11 0 0];
I12=diag([16.35 62.1364 61.6364]);
% Final estimate variable initialization
finalthetal=[0 0 0 0 0 0 0 0 0];
finaltheta2=[0];
prevthetal=zeros(9,1);
prev_theta2=[0];
sustain=zeros(10,1);
% Filter
covariance reinitialization
values at docking
KFPlinit=diag([3600 3600 3600 3600 3600 3600 100 100 100]);
KFP2init=[2500];
%FOR PSEUDO RANDOM INPUTS
BEENHERE1=0;
BEENHERE2=0;
BEENHERE3=0;
BEENHERE4=0;
BEENHERE5=0;
BEENHERE6=0;
BEENHERE7=0;
BEENHERE8=0;
Quaternion
to Direction Cosine Matrix Function
q2c .m
function [DCM] = q2C(q1,q2,q3,q4);
% Quaternion to Direction Cosine Matrix transformation function
115
% q4=n
DCM= [1-2*(q2^2+q3^2) 2 *(ql*q2+q3*q4) 2*(ql*q3-q2*q4);
2*(q2*ql-q3*q4) 1-2*(q3^2+qi^2) 2*(q2*q3+ql*q4);
2*(q3*ql+q2*q4) 2*(q3*q2-ql*q4) 1-2*(q1^2+q2^2)];
Cross Product Function
x-prod. m
function [crossprod] = x-prod( a,b );
% Takes crossproduct of Avect and Bvect
crossprod= [a(2)*b(3)-a(3)*b(2);
a(3)*b(1)-a(1)*b(3);
a(1)*b(2)-a(2)*b(1)];
ODE Solver Function
SIMPLEode23.m
function [tout, yout] = kkvode23( tO, tfinal, KKVstate,fCGThrustECI,
mACSJetKKV, mCGThrustKKV );
global vDot;
% Initialization
pow = 1/3;
tol = le-6;
trace = 0;
% Initialization
t = to;
hmax = (tfinal - t)/4;
hmin = (tfinal - t)/20000;
h = (tfinal - t)/4;
y = KKVstate(:);
%State vector
tout = t;
yout = y.1;
tau = tol * max(norm(y, 'inf'), 1);
A The main loop
SLOPE
iSLOPE
=
=
0;
0;
trigger = 0;
while (t < tfinal) & (h >= hmin) & (trigger
116
==
0)
...
if t + h > tfinal, h = tfinal - t; end
% Compute the slopes
si = kkvl( t, y,fCGThrustECI, mACSJetKKV, mCGThrustKKV );
si = s1(:);
s2 = kkvl( t+h, y+h*sl,fCGThrustECI, mACSJetKKV, mCGThrustKKV );
s2 = s2(:);
s3 = kkvl( t+h/2, y+h*(sl+s2)/4,fCGThrustECI,
mACSJetKKV,
mCGThrustKKV );
s3 = s3(:);
% Estimate the error and the acceptable error
delta = norm(h*(sl - 2*s3 + s2)/3,'inf');
tau
=
tol*max(norm(y,'inf'),1.0);
% Update the solution only if the error is acceptable
if ( delta <= tau ) I ( h <= hmin )
t = t + h;
y = y + h*(sl + 4*s3 + s2)/6;
tout = [tout; t];
yout = [yout; y.'];
% Get wdot from slopesiSLOPE = iSLOPE + 1;
SLOPE=SLOPE + (sl+4*s3+s2)/6;
% Guidance/ACS algorithm to determine whether to break ode cycle
end
o
Update the step size
if delta -= 0.0
h = min(hmax, 0.9*h*(tau/delta)^pow);
end
end;
SLOPE = SLOPE / iSLOPE;
WDOT= SLOPE(5:7);
[m,n] = size(yout);
yout = yout(m,:);
tout = tout(m,:);
if (t < tfinal)
disp('SINGULARITY LIKELY')
t
end
State Derivative Function
kkvl.m
function KKVStateDot = KKV( t, KKVstate,fCGThrustECI, mACSJetKKV, mCGThrustKKV )
117
Code to take derivative of microsat state vector
adapted from code written by Eric Breitfeller.
%
%
%globevar;
global vDot KKVIXYZ;
% Assign names to the KKV states in the KKVstate vector
n = KKVstate(1);
q1 =
q2 =
q3 =
wi =
w2 =
KKVstate(2);
KKVstate(3);
KKVstate(4);
KKVstate(5);
KKVstate(6);
w3 = KKVstate(7);
x
y
z
= KKVstate(8);
= KKVstate(9);
= KKVstate(10);
vx = KKVstate(11);
vy = KKVstate(12);
vz = KKVstate(13);
KKVTotalMass = KKVstate(14);
KKVJetWetMass = KKVstate(15);
%Quaternion propagation equation
nDot = (
0 - wl*ql - w2*q2 - w3*q3
qxDot = (wl*n +
0
+ w3*q2 - w2*q3
+ wl*q3
qyDot = (w2*n - w3*ql + 0
qzDot = ( w3*n + w2*ql - wl*q2 +
0
)/2;
)/2;
)/2;
)/2;
%Vehicle rotational dynamics
w_x_Iw = x-prod( [wi w2 w3]', KKVIXYZ*[wl w2 w3]' );
%wDot
= inv( KKVIXYZ )*( mCGThrustKKV + mACSJetKKV + mDragKKV - w-xIw);
wDot
= inv( KKVIXYZ )*( mCGThrustKKV + mACSJetKKV - w-xIw);
XVehicle translational dynamics
xDot = vx;
yDot = vy;
zDot = vz;
vDot = ( fCGThrustECI )/KKVTotalMass;
%Fuel mass rate
KKVThrusterWetMassDot = 0;% -sum(abs(fCGThrustECI))/CGTHRUSTISP;
KKVJetWetMassDot = 0; -sum(abs(fACSJetECI))/ACSJETISP;
KKVTotalMassDot = 0;
% Form the derivative state vector
KKVStateDot = [ nDot qxDot qyDot qzDot wDot' xDot yDot zDot vDot'
KKVTotalMassDot
KKVJetWetMassDot]';
118
...
C.1.2
Docking Code
docksim.m
clear all;
global vDot;
% Initialize KKV
init.m;
% Miscellaneous parameters
DT
ii
0.05; %seconds
=
1;
iii=1;
tic
% Begin main loop
while( SIMTIME < SIMTOTAL
)
% Kalman Filter Inertia Tensor estimation updates every 50 ms no propagation
if beenhere == 1
kkv_C_eci = q2c(KKVstate(2),KKVstate(3),KKVstate( 4) ,KKVstate(1));
KKVaccel= kkv_C_eci*KKVAccelave;
PHI1=
[WDOTave(1) WDOTave(2)-Wave(1)*Wave(3)
WDOTave(3)+Wave(1)*Wave(2)
-Wave(2)*Wave(3)
Wave(2)^2-Wave(3)^2
Wave(2)*Wave(3) ...
0
+F(3)
-F(2); ...
Wave(1)*Wave(3)
WDOTave(1)+Wave(2)*Wave(3)
Wave(3)^2-Wave(l)^2 ...
WDOTave(3)-Wave(1)*Wave(2)
WDOTave(2)
-Wave(1)*Wave(3) ...
-F(3)
0
+F(1); ...
-Wave(1)*Wave(2)
Wave(1)^2-Wave(2)^2
WDOTave(1)-Wave(2)*Wave(3)
Wave (1) *Wave (2)
WDOTave(2)+Wave(1)*Wave(3)
WDOTave(3) ...
0];
-F(1)
+F(2)
PH12=[KKVaccel]';
THETA1= [KKVIXYZest(1,1);KKVIXYZest(1,2);KKVIXYZest(1,3);KKVIXYZest(2,2);
...
KKVIXYZest(2,3) ;KKVIXYZest(3,3);CGx;CGy;CGz];
THETA2= [currMass];
KFP1 = KFP1 - KFP1*PHI1*inv(KFR1 + PHI1'*KFP1*PHI1) * PHI1'*KFP1 + KFQ1;
KFP2 = KFP2 - KFP2*PHI2*inv(KFR2 + PH12'*KFP2*PHI2) * PHI2'*KFP2 + KFQ2;
THETA1= THETA1 + KFP1*PHI1*inv(KFR1)*([AvemACSJetKKV]-PHI1'*THETA1);
THETA2= THETA2 + KFP2*PHI2*inv(KFR2)*([AvefDIVJetKKV]-PHI2'*THETA2);
%CHECK FOR FINAL STATE VALUE
% getting final estimate (if constant for 100 iterations)
for jjj=1:6,
if abs(THETA1(jjj)-prev-thetal(jjj)) <= .05
sustain(jjj)=sustain(jjj)+1;
else
sustain(jjj)=0;
end
119
...
end
for jjj=7:9,
if abs(THETA1(jjj)-prev-thetal(jjj)) <= .05
sustain(jjj)=sustain(jjj)+1;
else
sustain(jjj)=0;
end
end
if abs(THETA2-prev-theta2) <= .1
sustain(10)=sustain(10)+1;
else
sustain(10)=0;
end
for jjj=1:9
if sustain(jjj) == 100
finalthetal(jjj)=THETA1(jjj);
end
end
if sustain(10) ==100
finaltheta2=THETA2;
end
prevythetal=THETA1;
prevtheta2=THETA2;
%SAVE CG ESTIMATE AND MASS ESTIMATE
StateEstimatesave(iii, :)=[THETA1' THETA2'];
PHI1save=[PHI1save;PHI1'];
PHI2save=[PHI2save;PHI2];
KFPlsave=[KFPlsave;diag(KFP1)'];
%
KKVIXYZest= [THETA1(1),THETA1(2),THETA1(3);
THETA1(2),THETA1(4),THETA1(5);
THETA1(3),THETA1(5),THETA1(6)1;
CGx=THETA1(7);
CGy=THETA1(8);
CGz=THETA1(9);
currMass=THETA2(1);
end
beenhere=1;
%
PD ACS control (minimize Az & El)
Kp = 7;
%proportional constant
Kd = 4;
%derivative constant
% PWM Control
% ACS jet control ton = (P+D)*T/mACSjet
=
desmom/capablemoment * dutycycle
% U= [PDroll; PDpitch; PDyaw];
U= [0 0 0]';
F= [0 0 0]';
% DOCKING MISSION PLAN
if iii <= 100
if BEENHERE1==0
Burnstart=iii;
120
Burntime=(rand*2-1)*4;
BEENHERE1=1;
end
if iii-Burnstart < abs(Burntime)
if Burntime <= 0
U=-[1.5 0 0]';
else
U=[1.5 0 0]';
end
else
U=[0 0 01';
end
if round(iii/5)-iii/5==0
BEENHERE1=0;
end
elseif (iii>100 & iii<=200)
if BEENHERE2==0
Burnstart=iii;
Burntime=(rand*2-1)*4;
BEENHERE2=1;
end
if iii-Burnstart < abs(Burntime)
if Burntime <= 0
U=-[0 1.5 0]';
else
U=[0 1.5 0]';
end
else
U=[0 0 0]';
end
if round(iii/5)-iii/5==0
BEENHERE2=0;
end
elseif (iii>200 & iii<=300)
if BEENHERE3==0
Burnstart=iii;
Burntime=(rand*2-1)*4;
BEENHERE3=1;
end
if iii-Burnstart < abs(Burntime)
if Burntime <= 0
U=-[0 0 1.5
';
else
U=[0 0 1.5]';
end
else
U=[0 0 0]';
end
if round(iii/5)-iii/5==0
BEENHERE3=0;
end
elseif (iii>300 & iii<=400)
if BEENHERE4==0
121
Burnstart=iii;
Burntime=(rand*2-1)*4;
BEENHERE4=1;
end
if iii-Burnstart < abs(Burntime)
if Burntime <= 0
F=-[0 DIVFjet 0]';
else
F=[0 DIVFjet 0]';
end
else
F=[0 0 0]';
end
if round(iii/5)-iii/5==0
BEENHERE4=0;
end
elseif iii>400 & iii<449
% PD ATTITUDE CONTROL PERIOD
U= -OMEGA*KKVIXYZest*[Wave(1),Wave(2),Wave(3)]'
-Kd*[Wave(1),Wave(2),Wave(3)]' ...
-Kp*[KKVstate(2),KKVstate(3),KKVstate(4)]';
elseif iii==449
% ARCHIVE ESTIMATED STATE FOR SINGLE BODY
archthetal=[final_thetai,final-theta2];
elseif iii==450
% 'TRUE' MASS PROPERTY CHANGE TO COMBINED BODY
KKVstate(14)=M12;
CG=CG12;
KKVIXYZ=I12;
elseif iii>450 & iii<600
U= -OMEGA*KKVIXYZest*[Wave(1),Wave(2),Wave(3)]'
-Kd*[Wave(1),Wave(2),Wave(3)]' ...
-Kp*[KKVstate(2),KKVstate(3),KKVstate(4)]';
elseif iii==600
KFPi=KFPlinit;
KFP2=KFP2init;
elseif (iii>600 & iii<=800)
if BEENHERE5==0
Burnstart=iii;
Burntime=(rand*2-1)*4;
BEENHERE5=1;
end
if iii-Burnstart < abs(Burntime)
if Burntime <= 0
U=-[ACSFjet 0 0]';
else
U=[ACSFjet 0 0]';
end
else
U=[0 0 0]';
end
if round(iii/5)-iii/5==0
BEENHERE5=0;
end
122
elseif (iii>800 & iii<=1000)
if BEENHERE6==O
Burnstart=iii;
Burntime=(rand*2-1)*4;
BEENHERE6=1;
end
if iii-Burnstart < abs(Burntime)
if Burntime <= 0
U=-[0 ACSFjet 0]';
else
U=[0 ACSFjet 0]';
end
else
U=[0 0 0]';
end
if round(iii/5)-iii/5==0
BEENHERE6=0;
end
elseif (iii>1000 & iii<=1200)
if BEENHERE7==0
Burnstart=iii;
Burntime=(rand*2-1)*4;
BEENHERE7=1;
end
if iii-Burnstart < abs(Burntime)
if Burntime <= 0
U=-[0 0 ACSFjet
1';
else
U=[0 0 ACSFjet]';
end
else
U=[0 0 0]';
end
if round(iii/5)-iii/5==0
BEENHERE7=0;
end
elseif (iii>1200 & iii<=1800)
if BEENHERE8==0
Burnstart=iii;
Burntime=(rand*2-1)*4;
BEENHERE8=1;
end
if iii-Burnstart < abs(Burntime)
if Burntime <= 0
F=-[0 DIVFjet 0]';
else
F=[0 DIVFjet 0]';
end
else
F=[0 0 0]';
end
if round(iii/5)-iii/5==0
BEENHERE8=0;
end
123
elseif iii==1801
% ARCHIVE STATE FOR COMBINED MASS, ...
A THEN CALCULATE MASS PROPERTIES OF DOCKED BODY
arch-theta2=[finalthetal,finaltheta2l;
M2= abs(arch-theta2(10)-arch-theta(10));
CG2= ([arch-theta2(7:9)1*arch theta2(10)[arch-thetal (7:9)] *arch thetal (10))
...
.
/M2;
vectl=[arch-thetal(7:9)]-[arch-theta2(7:9)1;
vect2=CG2-[archtheta2(7:9)1;
al=vectl(1);
bl=vectl(2);
ci=vectl(3);
a2=vect2(1);
b2=vect2(2);
c2=vect2(3);
mat1=[b1^2+c1^2 -al*bl -al*cl; -al*bl a1^2+c1^2 -bl*ci;
-al*cl
...
-bl*cl al^2+bl^2];
mat2=[b2^2+c2^2 -a2*b2 -a2*c2; -a2*b2 a2^2+c2^2 -b2*c2; ...
-a2*c2 -b2*c2 a2^2+b2^2];
Icomb=[archtheta2(1:3) ;archtheta2(2) arch_theta2(4:5);
archtheta2(3) arch_theta2(5:6)];
I1= [arch-thetal(1:3);archthetal(2) arch-thetal(4:5); ...
archthetal(3) archthetal(5:6)];
I2=Icomb-I1-archthetal(10)*matl-M2*mat2;
end
A ASSIGN FORCE VECTOR BASED ON 'ON' TIMES (FRACTION OF FULL CYCLE)
A F=[0,FYDivertDT/.05*DIVFjet,0]';
% CALCULATE DT's BASED ON DESIRED JET AND DIVERT THRUSTS
YawDT = U(3)*ACSJetDutyCycle/(2*ACSFjet*MomArmZ);
YawDTcount= abs(round(YawDT/.0025));
PitchDT= U(2)*ACSJetDutyCycle/(2*ACSFjet*MomArmY);
PitchDTcount= abs(round(PitchDT/.0025));
RollDT = U(1)*ACSJetDutyCycle/(2*ACSFjet*MomArmX);
RollDTcount= abs(round(RollDT/.0025));
XDivertDT= F(1)/DIVFjet * ACSJetDutyCycle;
XDivertDTcount= abs( round( XDivertDT / 0.0025 ));
YDivertDT= F(2)/DIVFjet * ACSJetDutyCycle;
YDivertDTcount= abs( round( YDivertDT / 0.0025 ));
ZDivertDT= F(3)/DIVFjet * ACSJetDutyCycle;
ZDivertDTcount= abs( round( ZDivertDT / 0.0025 ));
% Determine Jet on/off matrix for scheduler
% cols: 4 roll jets, 4 pitch jets, 4 yaw jets, ...
2 X-axis Thruster, 2 Y-axis Thruster, 2 Z-axis Thruster
%
% ACS jet order: clockwise from top left when looking into rotation axis
% Xaxis(Zaxis up), Yaxis(Zaxis up), Zaxis(Yaxis up)
124
% Thruster jet order: positive divert along axis then negative (xyz)
Schedule = zeros(20,18);
% each row is a 2.5 ms timestep, each column is a jet: 1 on, 0 off
Schedule(21,:) = 5;
if RollDTcount ~= 0
if RollDT > 0 % if roll is positive, jets 1,3 turn on
Schedule(1:min(RollDTcount,20) ,1) = 1;
Schedule(1:min(RollDTcount,20) ,3) = 1;
else A if roll is negative, jets 2,4 turn on
Schedule(1:min(RollDTcount,20),2) = 1;
Schedule(1:min(RollDTcount,20) ,4) = 1;
end
end
if PitchDTcount -= 0
if PitchDT > 0 % if Pitch is positive, jets 5,7 turn on
Schedule(1:min(PitchDTcount,20),5) = 1;
Schedule(1:min(PitchDTcount,20),7) = 1;
else A if Pitch is negative, jets 6,8 turn on
Schedule(1:min(PitchDTcount,20),6) = 1;
Schedule(1:min(PitchDTcount,20) ,8) = 1;
end
end
if YawDTcount -= 0
if YawDT > 0 % if yaw is positive, jets 9,11 turn on
Schedule(1:min(YawDTcount,20) ,9) = 1;
Schedule(1:min(YawDTcount,20),11) = 1;
else % if yaw is negative, jets 10,12 turn on
Schedule(1:min(YawDTcount,20) ,10) = 1;
Schedule(1:min(YawDTcount,20),12) = 1;
end
end
A SCHEDULE IN DIVERTS (different base dt??)
-= 0
if XDivertDT > 0 % if xdivert is positive, jet 13 turns on
Schedule(1:min(XDivertDTcount,20),13) = 1;
else % if xdivert is negative, jet 14 turns on
Schedule(1:min(XDivertDTcount,20) ,14) = 1;
end
end
if XDivertDTcount
if YDivertDTcount ~= 0
if YDivertDT > 0 % if ydivert is positive, jet 15 turns on
Schedule(1:min(YDivertDTcount,20),15) = 1;
else A if ydivert is negative, jet 16 turns on
Schedule(1:min(YDivertDTcount,20),16) = 1;
end
end
if ZDivertDTcount -= 0
if ZDivertDT > 0 % if zdivert is positive, jet 17 turns on
Schedule(1:min(ZDivertDTcount,20),17) = 1;
125
else A if zdivert is negative, jet 18 turns on
Schedule(1:min(ZDivertDTcount,20),18) = 1;
end
end
% 50 ms Duty cycle loop
rowcount = 1;
A rowcount is the step in the 18 jet scheduler (20 steps total within 0.05s
A duty cycle)
DTcount=1; DTcount is the number of unchanged rows in scheduler
%Initialize the values to be averaged over 50ms for estimator
AvemACSJetKKV=0;
Wsum=0;
AvefDIVJetKKV=O;
Accelsum=0;
IMatrixsum=0;
while rowcount <= 20
if rowcount ~= 20 & norm(Schedule(rowcount,:) - Schedule(rowcount+1,:)) ==0
X CAREFUL!! comparing rows to see if different by using norm
DTcount=DTcount+1;
rowcount = rowcount + 1;
else %meaning a jet turns on or off at this iteration (0.0025s)
DT= DTcount*Basetimestep;
%Simulation parameters
t0= SIMTIME;
tfinal= tO + DT;
SIMTIME= tfinal;
A Determine actual fCGThrustECI, mACSJetKKV produced by jets
% DETERMINE MOMENTS FROM ACS JETS
if Schedule(rowcount,1) == 1
XmACSJetKKV = 2*ACSFjet*MomArmX;
elseif Schedule(rowcount,2) == 1
XmACSJetKKV = -2*ACSFjet*MomArmX;
else
XmACSJetKKV = 0;
end
if Schedule(rowcount,5) == 1
YmACSJetKKV = 2*ACSFjet*MomArmY;
elseif Schedule(rowcount,6) == 1
YmACSJetKKV = -2*ACSFjet*MomArmY;
else
YmACSJetKKV = 0;
end
if Schedule(rowcount,9) == 1
ZmACSJetKKV = 2*ACSFjet*MomArmZ;
elseif Schedule(rowcount,10) == 1
ZmACSJetKKV = -2*ACSFjet*MomArmZ;
else
ZmACSJetKKV = 0;
end
126
% control torque vector output from jets
mACSJetKKV= [XmACSJetKKV;YmACSJetKKV;ZmACSJetKKV];
% weighted fraction of 50 ms at this torque (for averaging @ 0.05s)
moment increment= mACSJetKKV* DTcount/20;
% summing the weighted fractions to build average moment over 0.05s
AvemACSJetKKV= AvemACSJetKKV+momentincrement;
% DETERMINE FORCE ON BODY FROM DIVERT JETS
if Schedule(rowcount,13) ==
1
XfDIVThrustKKV = DIVFjet;
elseif Schedule(rowcount,14) == 1
XfDIVThrustKKV = -DIVFjet;
else
XfDIVThrustKKV = 0;
end
if Schedule(rowcount,15) ==
1
YfDIVThrustKKV = DIVFjet;
elseif Schedule(rowcount,16) == 1
YfDIVThrustKKV = -DIVFjet;
else
YfDIVThrustKKV = 0;
end
if Schedule(rowcount,17) ==
1
ZfDIVThrustKKV = DIVFjet;
elseif Schedule(rowcount,18) == 1
ZfDIVThrustKKV = -DIVFjet;
else
ZfDIVThrustKKV = 0;
end
% weighted fraction of 50 ms at this force (for averaging 0 0 .05s)
forceincrement=[XfDIVThrustKKV YfDIVThrustKKV ZfDIVThrustKKV]' * DTcount/20;
% summing the weighted fractions to build average force over 0.05s
AvefDIVJetKKV= AvefDIVJetKKV+forceincrement;
%CONVERT FROM KKV TO ECI FRAME
kkv_Ceci = q2c(KKVstate(2),KKVstate(3),KKVstate(4),KKVstate(1));
eci_C-kkv = kkv_C_eci';
fCGThrustECI = eci_C_kkv * [XfDIVThrustKKV YfDIVThrustKKV ZfDIVThrustKKV]';
% DETERMINE MOMENT ON BODY FROM DIVERT JETS (CG OFFSET)
% MOMENT= RADIUS CROSS FORCE
mCGThrustKKV= x-prod(-CG, [XfDIVThrustKKV YfDIVThrustKKV ZfDIVThrustKKV]);
%Integrate KKV states
[KKVtout, KKVstate]= SIMPLEode23( tO, tfinal, KKVstate, fCGThrustECI, ...
mACSJetKKV, mCGThrustKKV);
%Get weighted sum of body rates and accelerations for averaging (after 0.05s)
Wsum= Wsum + ([KKVstate(5:7)1'*DTcount/20);
IMatrixsum= IMatrixsum + (KKVIXYZest .* DTcount/20);
Accelsum = Accelsum + (vDot * DTcount/20);
%Save data for plotting (at each irregular interval)
KKVxs(ii,:)= KKVstate;
127
KKVt(ii,:)= KKVtout;
IMatrixirreg(ii,:,:)= KKVIXYZest;
Usave(ii,:) = U';
%Increment index
ii = ii + 1;
DTcount = 1;
rowcount = rowcount + 1;
end
end
Xoutside
of the .05 s loop (to get averages for filter)
%Get average inertia values for plotting
IMatrix(iii,:,:)=IMatrixsum;
%Get average of actual moment applied by jets (control torques) for estimator
momentavesave(iii,:)=AvemACSJetKKV';
%Get average body rate over 0.05s (w/ noise)
Wave=Wsum + randn(3,1).* Wavenoisemag;
Wavesave(iii,:)=Wave';
currW=Wave;
WDOTave=(currW-prevW)/ACSJetDutyCycle;
prevW=currW;
WDOTavesave(iii,:)=WDOTave';
%Get acceleration from accelerometer- over .05s
KKVAccelave= Accelsum + randn(3,1).* Accelnoisemag;
KKVAccelavesave(iii,:)= (kkv_C_eci*KKVAccelave)';
%Update OMEGA for estimator (averaged at 20 Hz)
OMEGA= -[ 0 -Wave(3) Wave(2);
Wave(3) 0 -Wave(1);
0];
-Wave(2)
Wave(1)
%test for optimal observability (KFP is error covariance)
KFPsave(iii,:)=[diag(KFP1)' KFP2];
Forcesave=[Forcesave;F'];
iii=iii+1;
end
toc
C.1.3
Matlab Plotting Functions
Control Torque Plot
plotfigi .m
figure('Position', [185 28 567 600]);
subplot (3,1,1);
128
plot((1:size(momentavesave,1))*0.02,momentavesave(:
title('Control Roll Torque N-m');
,1));grid;
subplot (3,1,2);
plot((1:size(momentavesave,1))*0.02,momentavesave(:
title('Control Pitch Torque N-m');
,2));grid;
subplot (3,1,3);
plot((1:size(momentavesave,1))*0.02,momentavesave(:
title('Control Yaw Torque N-m');
xlabel('time (s)');
,3));grid;
Moment of Inertia Plot
plotfig2.m
figure('Position',
[185 28 567 600]);
xlength=(1:size(IMatrix,1))*.02;
subplot (3,1,1);
plot((1:size(IMatrix,1))*0.02,IMatrix(: ,1,1),'-',
..
[0:20],KKVIXYZ(1,1)*ones(1,21),'-.',
xlength,KKVIXYZ(1,1)+(KFPsave(1:length(xlength) ,1)) .5,'--',
xlength,KKVIXYZ(1,1)-(KFPsave(1:length(xlength),1)).^.5 ,'--');grid;
title('X axis moment of inertia kg-m^2');
legend('Estimate' ,'True value' ,'Filter
"precision"');
subplot (3,1,2);
plot((1:size(IMatrix,1))*0.02,IMatrix(:,2,2),'-',
...
[0:20],KKVIXYZ(2,2)*ones(1,21),'-.',
...
xlength,KKVIXYZ(2,2)+(KFPsave(1:length(xlength) ,4)). .5
xlength,KKVIXYZ(2,2)-(KFPsave(1:length(xlength) ,4)). .5
title('Y axis moment of inertia kg-m^2');
legend('Estimate','True value','Filter "precision"');
'--');grid;
subplot (3,1,3);
plot((1:size(IMatrix,1))*0.02,IMatrix(:,3,3),
-',
...
[0:20],KKVIXYZ(3,3)*ones(1,21), -.', ...
xlength,KKVIXYZ(3,3)+(KFPsave(1:length(xlength) ,6)). .5
xlength,KKVIXYZ(3,3)-(KFPsave(1:length(xlength) ,6)). .5
title('Z axis moment of inertia kg-m^2');
xlabel('time (s)');
legend('Estimate' ,'True value' ,'Filter "precision"');
Product of Inertia Plot
plotf ig3.m
129
'--')
;grid;
figure('Position', [185 28 567 600]);
xlength=(1:size(IMatrix,1))*.02;
subplot (3,1,1);
plot((1:size(IMatrix,1))*0.02,IMatrix(: ,1,2),'b', .
[0:201,KKVIXYZ(1,2)*ones(1,21),'-.', ...
xlength,KKVIXYZ(1,2)+(KFPsave(1:length(xlength),2)).^.5
xlength,KKVIXYZ(1,2)-(KFPsave(1:length(xlength),2)).^.5
title('X-Y product of inertia kg-m^2');
legend('Estimate','True value' ,'Filter "precision"');
subplot (3,1,2);
plot((1:size(IMatrix,1))*0.02,IMatrix(:
'--',
,'--')
,1,3), 'b',
;grid
;grid;
...
[0:20],KKVIXYZ(1,3)*ones(1,21),'-.', ...
xlength,KKVIXYZ(1,3)+(KFPsave(1:length(xlength) ,3)). ^5
xlength,KKVIXYZ(1,3)-(KFPsave(1:length(xlength) ,3)). .5
title('X-Z product of inertia kg-m^2');
Xxlabel('time (s)');
axis([0 20 -.5 .5]);
legend('Estimate' ,'True value' ,'Filter "precision"');
'--')
;grid;
subplot (3,1,3);
plot((1:size(IMatrix,1))*0.02,IMatrix(:,2,3),'b',
[0:20],KKVIXYZ(2,3)*ones(1,21),'-.', ...
xlength,KKVIXYZ(2,3)+(KFPsave(1:length(xlength),5)).^.5
xlength,KKVIXYZ(2,3)-(KFPsave(1:length(xlength),5)).^.5
title('Y-Z product of inertia kg-m^2');
xlabel('time (s)');
axis([0 20 -.5 .5]);
legend('Estimate' ,'True value' ,'Filter "precision"');
Center of Gravity Location Plot
plotfig4 .m
figure('Position', [185 28 567 600]);
xlength=(1:size(IMatrix,1))*.02;
subplot (3,1,1);
plot((1:size(StateEstimatesave,1))*0.02,StateEstimatesave(:,7),'b',
.
[0:20],CG(1)*ones(1,21),'-.', ...
xlength,CG(1)+(KFPsave(1:length(xlength),7))..5
,'--',
,'--');grid;
xlength,CG(1)-(KFPsave(1:length(xlength),7))
title('CG location along X-axis');
legend('Estimate','True value','Filter "precision"');
.5
subplot (3,1,2);
plot((1:size(StateEstimatesave,1))*0.02,StateEstimatesave(:,8),'b',
[0:20],CG(2)*ones(1,21),'-.', ...
...
xlength,CG(2)+(KFPsave(1:length(xlength) ,8))..5 ,'--',
.^5
,'--');grid;
,8))
xlength,CG(2)-(KFPsave(1:length(xlength)
130
..
title('CG location along Y-axis');
legend('Estimate' ,'True value' ,'Filter "precision"');
subplot (3,1,3);
plot((1:size(StateEstimatesave,1))*0.02,StateEstimatesave(:
[0:20],CG(3)*ones(1,21),'-.',
xlength,CG(3)+(KFPsave(1:length(xlength),9)).^.5 ,
xlength, CG (3) -(KFPsave (1: length (xlength),9)) .^.5
,9),'b',
,'--');grid;
title('CG location along Z-axis');
xlabel('time (s)');
legend('Estimate','True value','Filter "precision"');
Force Command and Mass Estimation Plot
plotfig5.m
figure('Position', [185 28 567 600]);
xlength=(1:size(IMatrix,1))*.02;
subplot (2,1,1);
plot((1:size(Forcesave,1))*0.02,Forcesave(:,2));grid;
title('Force command (Y-Axis Divert) N');
xlabel('time (s)');
subplot (2,1,2);
plot((1:size(StateEstimatesave,1))*0.02,StateEstimatesave(: ,10)
[0:20],KKVstate(14)*ones(1,21),'-.',
...
xlength,KKVstate(14)+(KFPsave(1:length(xlength) ,10)) .^5,'--',
xlength,KKVstate(14)-(KFPsave(1:length(xlength) ,10)) .^5 ,'--');grid;
title('Total Mass kg');
xlabel ('time (s)');
legend('Estimate' ,'True value','Filter "precision"');
131
,
'b',
132
Bibliography
[1] R. Cristi. Adaptive quaternion feedback regulation for eigenaxis rotations. Journal
of Guidance Control & Dynamics, 1994.
[2] G. F. Franklin, J. D. Powell, and M. Workman.
Digital Control of Dynamic
Systems. Addison-Wesley, Menlo Park, California, 1998.
[3] Arthur Gelb. Applied Optimal Estimation. The M.I.T. Press, Cambridge, Massachusetts, 1974.
[4] E. J. Lefferts, F. L. Markley, and M. D. Schuster. Kalman filtering for spacecraft
attitude estimation. Journal of Guidance Control & Dynamics, 1982.
[5] L. Ljung. System Identification: Theory for the user. Prentice-Hall, Englewood
Cliffs, New Jersey, 1987.
[6] J. E. Wertz. Spacecraft Attitude Determination and Control. Kluwer Academic
Publishers, Dordrecht, Holland, 1997.
[7] J. H. Willams. Fundamentals of Applied Dynamics. J. Wiley, New York, 1996.
133