Analysis of an Application where the Unscented Kalman Filter is Not
Appropriate
Except where reference is made to the work of others, the work described in this dissertation is
my own or was done in collaboration with my advisory committee. This dissertation does not
include proprietary or classi?ed information.
Abby Anderson
Certi?cate of Approval:
Robert Dean, Co-Chair
Assistant Professor
Electrical and Computer Engineering
A. Scottedward Hodel, Co-Chair
Associate Professor
Electrical and Computer Engineering
Stanley Reeves
Professor
Electrical and Computer Engineering
George Flowers
Professor
Mechanical Engineering
George Flowers
Dean
Graduate School
Analysis of an Application where the Unscented Kalman Filter is Not
Appropriate
Abby Anderson
A Dissertation
Submitted to
the Graduate Faculty of
Auburn University
in Partial Ful?llment of the
Requirements for the
Degree of
Doctor of Philosophy
Auburn, Alabama
December 18, 2009
Analysis of an Application where the Unscented Kalman Filter is Not
Appropriate
Abby Anderson
Permission is granted to Auburn University to make copies of this dissertation at its
discretion, upon the request of individuals or institutions and at
their expense. The author reserves all publication rights.
Signature of Author
Date of Graduation
iii
Dissertation Abstract
Analysis of an Application where the Unscented Kalman Filter is Not
Appropriate
Abby Anderson
Doctor of Philosophy, December 18, 2009
(M.S., Auburn University, 2006)
(B.E.E., Auburn University, 2003)
241 Typed Pages
Directed by A. Scottedward Hodel and Robert Dean
In this work a spin stabilized rocket with a ring of lateral pulse jets for attitude correction
and ?ns that open early in ?ight is simulated. The rocket is simulated with ?ve di?erent sensor
packages: rate gyros only, rate gyros and an ideal magnetometer, rate gyros and a magnetometer,
rate gyros and angle gyros, and rate gyros and angle gyros and a magnetometer. The gyros are
microelectromechanical systems (MEMS) devices. All control e?ort must be applied in the ?rst
seconds of ?ight because of the properties of the rocket. A comparison of the Extended Kalman
Filter (EKF) andtheUnscented Kalman Filter (UKF) foreach sensorsuiteis presentedtodetermine
the best approach to improve the circular probableerror (CEP) of the rocket. A solution to Wahba?s
problem, the EStimator of the Optimal Quaternion (ESOQ) algorithm, is used for state estimation
for certain sensor con?gurations. Wahba?s problem has traditionally been used for state estimation
of orbiting satellites.
iv
Acknowledgments
First, I would like to thank my family. Particularly my parents Je? and Rhonda Anderson.
Without their support none of this would have been possible.
I would also like to thank my committee: Dr. Hodel, Dr. Dean, Dr. Reeves, and Dr. Flowers.
I would especially like to thank Dr. Dean for stepping into a di?cult situation to allow me to
complete my work.
But the person who is most directly responsible for the completion of this Ph.D. is Dr. Hodel.
(Yes, Dr. Hodel, I realize I started the previous sentence with a conjunction, but note that there
are no dangling modi?ers.) Over the years that I have been his student, we have had conversations
ranging from work to our sometimes unorthodox religious views. No matter the topic, Babylon 5
always had a way of working itself into the discussion. As Delenn aptly said, ?The universe puts
us in places where we can learn. They are never easy places, but they are right. Wherever we
are, it?s the right place ... and the right time. Pain that sometimes comes is part of the process of
constantly begin born.? I miss our conversations. I have been blessed not only to have Dr. Hodel
as an adviser for both my graduate degrees but also to consider him my friend.
One day in one of my undergraduate classes that Dr. Hodel was teaching, he said, ?My
students? job is to make me look good.? I never quite let him forget that statement, but I hope
this work (for which he suggested the title ?The Unscented Kalman Filter Stinks?) makes you look
good, Dr. Hodel. It is dedicated to you.
?A student is not above his teacher, nor a servant above his master. It is enough for the
student to be like his teacher and the servant like his master ...? (Matthew 10:24-25, NIV).
?And so it begins.? - Kosh, Babylon 5
v
Style manual or journal used IEEE Transactions on Aerospace and Electronic Systems
(together with the style known as ?auphd?). Bibliography follows IEEE Transactions.
Computer software used The document preparation package TEX (speci?cally LATEX) together
with the departmental style-?le auphd.sty.
vi
Table of Contents
List of Figures x
1 Introduction 1
2 Wahba?s Problem 4
2.1 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
2.2 Problem Solutions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.3 Davenport?s q-Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.3.1 Rotation Vectors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.3.2 q-Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.4 QUEST Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
2.5 Kalman Filter Type Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.5.1 Wahba?s Problem as Maximum Likelihood Estimation . . . . . . . . . . . . . 22
2.5.2 Filter QUEST Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
2.6 REQUEST Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
2.6.1 Time-Invariant REQUEST . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
2.6.2 Time-Varying REQUEST . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
2.7 Extended QUEST Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
2.8 Energy Approach Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
2.9 Singular Value Decomposition Algorithm . . . . . . . . . . . . . . . . . . . . . . . . 43
2.10 Fast Optimal Attitude Matrix (FOAM) Algorithm . . . . . . . . . . . . . . . . . . . 47
2.11 Alternative Quaternion Attitude Estimation Algorithm . . . . . . . . . . . . . . . . . 49
2.12 Euler-q Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
2.13 ESOQ Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
2.14 ESOQ2 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
2.15 A Slightly Sub-Optimal Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
2.16 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
3 Magnetometer Navigation 65
3.1 Modeling Earth?s Magnetic Field . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
3.1.1 Equation Development . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
3.1.2 Model Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
3.2 Magnetometer Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
3.3 Magnetometer Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
3.3.1 Attitude Independent Bias Estimation . . . . . . . . . . . . . . . . . . . . . . 72
3.3.2 TWOSTEP Algorithm Calibration . . . . . . . . . . . . . . . . . . . . . . . . 77
3.3.3 Recursive Least Squares Method . . . . . . . . . . . . . . . . . . . . . . . . . 82
3.4 Angular Rate Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83
vii
3.5 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
4 Estimation And Control 88
4.1 Current Missile Control Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
4.2 Reaction Jet Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90
4.2.1 Variable-Structure Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90
4.2.2 Time-Optimal Control with A Single Reaction Jet . . . . . . . . . . . . . . . 92
4.2.3 Multiple-Reaction Jet Control . . . . . . . . . . . . . . . . . . . . . . . . . . . 96
4.2.4 Projectile Linear Theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
4.3 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
5 Rocket Dynamics and State Estimation 105
5.1 Rotational Rate Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105
5.2 Pitch Rate and Yaw Rate Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . 110
5.2.1 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111
5.3 State Estimation with a Direction Vector . . . . . . . . . . . . . . . . . . . . . . . . 111
5.3.1 Ideal Vector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121
5.3.2 Magnetometer Measurements . . . . . . . . . . . . . . . . . . . . . . . . . . . 122
5.4 Estimation with Rate and Angle Gyros . . . . . . . . . . . . . . . . . . . . . . . . . 127
5.4.1 EKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132
5.5 Estimation with Magnetometer, Rate Gyros, and Angle Gyros . . . . . . . . . . . . 135
5.5.1 EKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139
5.6 Wahba?s Problem Applied to a Rocket . . . . . . . . . . . . . . . . . . . . . . . . . . 140
5.6.1 Wahba?s Problem Solution as Input to an EKF and UKF . . . . . . . . . . . 141
5.6.2 Wahba?s Problem Solution Combined with an EKF and UKF . . . . . . . . . 143
6 Results 145
6.1 Rate Gyros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 146
6.2 Rate Gyros and an Ideal Vector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151
6.3 Rate Gyros and a Magnetometer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 157
6.4 Rate Gyros and Angle Gyros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162
6.5 Rate Gyros, Angle Gyros, and Magnetometer . . . . . . . . . . . . . . . . . . . . . . 167
6.5.1 ESOQ Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167
6.5.2 EKF and UKF with All Sensors . . . . . . . . . . . . . . . . . . . . . . . . . 175
6.5.3 ESOQ Algorithm Combined with Kalman Filters . . . . . . . . . . . . . . . . 175
6.6 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 195
7 Conclusions and Future Work 197
7.1 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 197
7.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 198
Bibliography 199
Appendices 205
viii
A Rotation Sequences 206
A.1 Euler Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 206
A.2 Quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 207
B Spherical Mathematics 210
B.1 Spherical Coordinate System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 210
B.2 Legendre Functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 212
B.3 Laplace?s Equation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 213
B.3.1 Solution Derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 213
B.3.2 Relationship with Legendre Functions and Spherical Harmonics . . . . . . . . 215
C Kalman Filtering 218
C.1 Linear Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 218
C.2 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 219
C.3 Unscented Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 220
D Latitude and Longitude 224
D.1 Cartesian Calculations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 224
D.2 Geodetic Coordinates . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 226
ix
List of Figures
2.1 Inertial and Body Reference Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
5.1 Relationship Among Rotation Quaternion Components . . . . . . . . . . . . . . . . . 116
6.1 Scaled MSE of Roll Rates of KF and UKF Estimates from Rate Gyros . . . . . . . . 147
6.2 Scaled MSE of Pitch Rates of KF and UKF Estimates from Rate Gyros . . . . . . . 148
6.3 Scaled MSE of Yaw Rates of KF and UKF Estimates from Rate Gyros . . . . . . . . 148
6.4 Scaled MSE of Roll Angles of KF and UKF Estimates from Rate Gyros . . . . . . . 149
6.5 Scaled MSE of Pitch Angles of KF and UKF Estimates from Rate Gyros . . . . . . 149
6.6 Scaled MSE of Yaw Angles of KF and UKF Estimates from Rate Gyros . . . . . . . 150
6.7 Normalized CEP for KF Estimates from Rate Gyros . . . . . . . . . . . . . . . . . . 150
6.8 Normalized CEP for UKF Estimates from Rate Gyros . . . . . . . . . . . . . . . . . 151
6.9 Scaled MSE of Roll Rates of EKF and UKF Estimates from Rate Gyros and an Ideal
Vector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153
6.10 Scaled MSE of Pitch Rates of EKF and UKF Estimates from Rate Gyros and an
Ideal Vector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153
6.11 Scaled MSE of Yaw Rates of EKF and UKF Estimates from Rate Gyros and an Ideal
Vector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154
6.12 Scaled MSE of Roll Angles of EKF and UKF Estimates from Rate Gyros and an
Ideal Vector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154
6.13 Scaled MSE of Pitch Angles of EKF and UKF Estimates from Rate Gyros and an
Ideal Vector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 155
6.14 Scaled MES of Yaw Angles of EKF and UKF Estimates from Rate Gyros and an
Ideal Vector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 155
6.15 Normalized CEP for EKF Estimates from Rate Gyros and an Ideal Vector . . . . . . 156
x
6.16 Normalized CEP for UKF Estimates from Rate Gyros and an Ideal Vector . . . . . . 156
6.17 Scaled MSE of Roll Rates of EKF and UKF Estimates from Rate Gyros and a
Magnetometer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158
6.18 Scaled MSE of Pitch Rates of EKF and UKF Estimates from Rate Gyros and a
Magnetometer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158
6.19 Scaled MSE of Yaw Rates of EKF and UKF Estimates from Rate Gyros and a
Magnetometer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 159
6.20 Scaled MSE of Roll Angles of EKF and UKF Estimates from Rate Gyros and a
Magnetometer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 159
6.21 Scaled MSE of Pitch Angles of EKF and UKF Estimates from Rate Gyros and a
Magnetometer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 160
6.22 Scaled MSE of Yaw Angles of EKF and UKF Estimates from Rate Gyros and a
Magnetometer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 160
6.23 Normalized CEP for EKF Estimates from Rate Gyros and a Magnetometer . . . . . 161
6.24 Normalized CEP for UKF Estimates from Rate Gyros and a Magnetometer . . . . . 161
6.25 Scaled MSE of Roll Rates of EKF and UKF Estimates from Rate Gyros and Angle
Gyros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163
6.26 Scaled MSE of Pitch Rates of EKF and UKF Estimates from Rate Gyros and Angle
Gyros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163
6.27 Scaled MSE of Yaw Rates of EKF and UKF Estimates from Rate Gyros and Angle
Gyros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 164
6.28 Scaled MSE of Roll Angles of EKF and UKF Estimates from Rate Gyros and Angle
Gyros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 164
6.29 Scaled MSE of Pitch Angles of EKF and UKF Estimates from Rate Gyros and Angle
Gyros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 165
6.30 Scaled MSE of Yaw Angles of EKF and UKF Estimates from Rate Gyros and Angle
Gyros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 165
6.31 Normalized CEP for EKF Estimates from Rate Gyros and Angle Gyros . . . . . . . 166
6.32 Normalized CEP for UKF Estimates from Rate Gyros and Angle Gyros . . . . . . . 166
6.33 CEP for ESOQ Controlled Rocket with Ideal Angle Gyros and Magnetometer . . . . 168
xi
6.34 CEP for ESOQ Controlled Rocket with Nonideal Angle Gyros and Magnetometer . . 168
6.35 Actual Roll Rates (Normalized) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 169
6.36 ESOQ Estimated Roll Rates with Nonideal Sensors (Normalized) . . . . . . . . . . . 169
6.37 Actual Roll Angles (Normalized) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 170
6.38 ESOQ Estimated Roll Angles with Nonideal Sensors (Normalized) . . . . . . . . . . 170
6.39 Actual Pitch Rates (Normalized) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 171
6.40 ESOQ Estimated Pitch Rates with Nonideal Sensors (Normalized) . . . . . . . . . . 171
6.41 Actual Pitch Angles (Normalized) . . . . . . . . . . . . . . . . . . . . . . . . . . . . 172
6.42 ESOQ Estimated Pitch Angles with Nonideal Sensors (Normalized) . . . . . . . . . . 172
6.43 Actual Yaw Rates (Normalized) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173
6.44 ESOQ Estimated Yaw Rates with Nonideal Sensors (Normalized) . . . . . . . . . . . 173
6.45 Actual Yaw Angles (Normalized) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 174
6.46 ESOQ Estimated Yaw Angles with Nonideal Sensors (Normalized) . . . . . . . . . . 174
6.47 CEP for EKF Controlled Rocket with Rate Gyros, Angle Gyros, and Magnetometer 175
6.48 CEP for UKF Controlled Rocket with Rate Gyros, Angle Gyros, and Magnetometer 176
6.49 CEP for Rocket Controlled by Estimates from an EKF with Inputs from the ESOQ
Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 177
6.50 CEP for EKF/ESOQ Hybrid Estimator Controlled Rocket . . . . . . . . . . . . . . . 178
6.51 CEP for UKF/ESOQ Hybrid Estimator Controlled Rocket . . . . . . . . . . . . . . 179
6.52 CEP for KF/ESOQ Hybrid Estimator Controlled Rocket . . . . . . . . . . . . . . . 179
6.53 Roll Rate MSE for ESOQ Estimates as Input to an EKF . . . . . . . . . . . . . . . 180
6.54 Pitch Rate MSE for ESOQ Estimates as Input to an EKF . . . . . . . . . . . . . . . 180
6.55 Yaw Rate MSE for ESOQ Estimates as Input to an EKF . . . . . . . . . . . . . . . 181
6.56 Roll Angle MSE for ESOQ Estimates as Input to an EKF . . . . . . . . . . . . . . . 181
6.57 Pitch Angle MES for ESOQ Estimates as Input to an EKF . . . . . . . . . . . . . . 182
xii
6.58 Yaw Angle MSE for ESOQ Estimates as Input to and EKF . . . . . . . . . . . . . . 182
6.59 Roll Rate MSE for EKF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . . 184
6.60 Pitch Rate MSE for EKF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . . 184
6.61 Yaw Rate MSE for EKF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . . 185
6.62 Roll Angle MSE for EKF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . . 185
6.63 Pitch Angle MSE for EKF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . 186
6.64 Yaw Angle MSE for EKF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . . 186
6.65 Roll Rate MSE for UKF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . . 187
6.66 Pitch Rate MSE for UKF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . . 188
6.67 Yaw Rate MSE for UKF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . . 188
6.68 Roll Angle MSE for UKF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . . 189
6.69 Pitch Angle MSE for UKF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . 189
6.70 Yaw Angle MSE for UKF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . . 190
6.71 Roll Rate MSE for KF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . . . 191
6.72 Pitch Rate MSE for KF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . . . 192
6.73 Yaw Rate MSE for KF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . . . 192
6.74 Roll Angle MSE for KF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . . . 193
6.75 Pitch Angle MSE for KF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . . 193
6.76 Yaw Angle MSE for KF/ESOQ Hybrid . . . . . . . . . . . . . . . . . . . . . . . . . . 194
B.1 The Spherical Coordinate System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 211
D.1 Derivation of Latitudes and Longitudes . . . . . . . . . . . . . . . . . . . . . . . . . 225
D.2 Geodetic Coordinate System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 227
xiii
Chapter 1
Introduction
Increasing the accuracy of munitions while reducing cost is of key interest to the military. One
method is to use onboard guidance systems that can steer a missile to within meters or less of its
target. However, this approach is expensive. A less costly but less accurate alternative is the spin
stabilization approach. These systems lack closed-loop ?ight control and many of the sophisticated
sensors found in onboard guidance systems. The accuracy of spin stabilized rockets is a?ected by
many factors including motor misalignments, tip-o? error, and wind.
One method to improve the accuracy of spin stabilized rockets is to include microelectrome-
chanical system (MEMS) sensors. MEMS sensors have the advantages of low cost since they are
batch producible using conventional IC technology and light weight. However, they are subject to
various errors including constant biases, walking biases, and additive noise.
In this work we compare state estimation techniques to improve the accuracy of a spin sta-
bilized rocket equipped with various sensor suites which include MEMS devices. We compare the
improvement in rocket accuracy when states are estimated by a Kalman Filter (KF), an Extended
Kalman Filter (EKF), and an Unscented Kalman Filter (UKF) for various sensor con?gurations.
We also use the EStimator of the Optimal Quaternion (ESOQ) algorithm, a solution to Wahba?s
problem, alone and in conjunction with the EKF and UKF to estimate states with certain sensor
suites. The ?rst sensor con?guration consists only of MEMS gyros to measure the rocket?s rota-
tional rates. The second sensor suite consists of MEMS gyros and an ideal sensor that measures
a known inertial frame vector in the body frame. The third sensor suite is comprised of MEMS
gyros and a tri-axial magnetometer. The next sensor suite consists of MEMS rate gyros and MEMS
gyros that directly measure rotational angles. The ?nal sensor suite combines the rate gyros, angle
gyros, and magnetometer into one package.
1
For this work we make several assumptions about the rocket. The rocket is launched from
a stationary ground launcher. A main thruster propels the rocket, and a ring of lateral thrusters
located at the rocket?s rear supply torque for control. Fins are located at the back of the rocket
that open shortly after launch. Due to aerodynamic forces, control of such a rocket is only feasible
during the ?rst seconds of ?ight so state estimation is con?ned to that time interval.
In this dissertation we present previous work pertaining to improving munition accuracy, de-
velop system models for the various sensor suites, and present the results of state estimation. In
Chapter 2 we present several solutions to Wahba?s problem, a method of determining a vehicle?s
attitude using pairs of sensors such as accelerometers and magnetometers. Next, in Chapter 3 we
review concepts associated with navigation via magnetometers. In Chapter 4 we present methods
for controlling rockets with a focus on rockets with lateral jets. We develop the system equations
for our various rocket models in Chapter 5. We present the results of simulating the systems from
Chapter 5 in Chapter 6 and draw some conclusions.
In this work we make the following contributions:
? We estimate the states of a rocket that is controlled solely with a ring of lateral thrusters.
? We concentrate on estimation of a rocket?s states during the ?rst seconds of ?ight. Other
work focuses on controlling munitions later in ?ight.
? We compare the e?ects that various sensor suites have on munition accuracy.
? We use a magnetometer to estimate the states of a rocket. Magnetometers are conventionally
used on satellites rather than rockets. Usually a satellite has a model of the magnetic ?eld
of the body it is orbiting on board. A rocket, however, does not have a magnetic ?eld model
available.
? We apply a solution of Wahba?s problem to estimate the states of a rocket. Wahba?s prob-
lem is traditionally applied to orbiting satellites since magnetic ?eld and gravitational ?eld
measurements can be found with magnetometers and accelerometers.
2
? We compare the performances of various state estimators including the KF, EKF, UKF, and
ESOQ algorithm to estimate the states of a rotating rocket and present conclusions about
which estimator performs best.
3
Chapter 2
Wahba?s Problem
Wahba?s problem is a minimization problem most often applied to ?nd the attitude of space-
craft with a ?xed attitude. Measurements are taken in the spacecraft?s body frame that give the
angles between known objects and the spacecraft. Often the known objects are stars and the sen-
sors are star trackers. The di?erence between the body frame measurements and known inertial
frame values are used to derive an attitude solution for the spacecraft. The relationship between
the body frame and the inertial frame is illustrated in Figure 2.1. The origin of the inertial frame
(with axes denoted by a subscript u1D456) is located at the center of the earth while the origin of the
body frame (with axes denoted by a subscript u1D44F) is located at the center of gravity of the rocket.
Various algorithms have been developed based on Wahba?s cost function. While the cost function
as originally posed is for static systems, researchers have extended it for use with dynamic systems.
2.1 Problem Statement
Grace Wahba ?rst presented what has become known as Wahba?s Problem in 1965 in SIAM Re-
view [1]. The problem is as follows: Given two vector sets {v1, v2, ... ,vu1D45B} and {v?1, v?2, ... ,v?u1D45B}
with u1D45B ? 2 entries, ?nd the rotation matrix u1D440 (an orthogonal matrix with determinant +1) that
minimizes
u1D45Buni2211.alt02
u1D457=1
?v?u1D457 ?u1D440vu1D457?2.
In other words, ?nd the rotation matrix u1D440 that minimizes the mean square error between the
two vector sets. A solution to this problem means that given at least two noncolinear, nonzero
measurements (such as magnetic ?eld and gravitational acceleration) in the body frame of a moving
vehicle and known values of the measured quantities in a stationary reference frame (i.e. an inertial
reference frame), the attitude of the vehicle can be found via the rotation matrix u1D440.
4
Figure 2.1: Inertial and Body Reference Frames
5
2.2 Problem Solutions
Various solutions to Wahba?s Problem were published in 1966 [2]. The solution given by Farrell
and Stuelpnagel, based on the polar decomposition, proceeds as follows. Let the column vectors
v1, ... ,vu1D45B, v?1, ... ,v?u1D45B have dimension u1D458, and let u1D449 and u1D449? denote the two u1D458?u1D45B matrices formed
from the two vector sets. Then the problem can be rewritten as
u1D444(u1D440) =
u1D45Buni2211.alt02
u1D457=1
?v?u1D457 ?u1D440vu1D457?2 = tr((u1D449? ?u1D440u1D449)u1D447(u1D449? ?u1D440u1D449)), (2.1)
where u1D444(u1D440) is de?ned as the sum of squares to be minimized.
Proof. Let a u1D458?u1D45B matrix u1D434 be de?ned as [a1 a2 ... au1D45B] where au1D457 are column vectors of length u1D458.
Then
u1D434u1D447u1D434=
?
??
??
??
??
au1D4471
au1D4472
...
au1D447u1D45B
?
??
??
??
??
uni005B.alt03
a1 a2 ... au1D45B
uni005D.alt03
=
?
??
??
??
??
au1D4471 a1 au1D4471 a2 ... au1D4471au1D45B
au1D4472 a1 au1D4472 a2 ... au1D4472au1D45B
... ... ... ...
au1D447u1D45Ba1 au1D447u1D45Ba2 ... au1D447u1D45Bau1D45B
?
??
??
??
??
tr{u1D434u1D447u1D434} = au1D4471 a1 +au1D4472 a2 +...+au1D447u1D45Bau1D45B =
u1D45Buni2211.alt02
u1D457=1
?au1D457?2
Expand expression (2.1) so that u1D444(u1D440) becomes
u1D444(u1D440) = tr((u1D449?u1D447 ?u1D449u1D447u1D440u1D447)(u1D449? ?u1D440u1D449))
= tr(u1D449?u1D447u1D449?)+tr(u1D449?u1D447u1D440u1D449)?tr(u1D449u1D447u1D440u1D447u1D449?) +tr(u1D449u1D447u1D440u1D447u1D440u1D449).
Since u1D440 is orthogonal (i.e. u1D440u1D447u1D440 = u1D440u1D440u1D447 = u1D43C) and tr(u1D434u1D435u1D436) = tr(u1D436u1D447u1D435u1D447u1D434u1D447), Q(M) can be
written as
u1D444(u1D440) = tr(u1D449?u1D447u1D449?) +tr(u1D449u1D447u1D449)?2tr(u1D449u1D447u1D440u1D447u1D449?). (2.2)
6
Since only the last term of Equation (2.2) is dependent on u1D440, u1D444(u1D440) is minimized when u1D439(u1D440) =
tr(u1D449u1D447u1D440u1D447u1D449?) is maximized. u1D439(u1D440) may be written as
u1D439(u1D440) = tr(u1D440u1D447u1D449?u1D449u1D447)
because tr(u1D434u1D435u1D436) = tr(u1D435u1D436u1D434) = tr(u1D436u1D434u1D435) when the matrices? dimensions are conformable. By the
polar decomposition, u1D449?u1D449u1D447 can be written as u1D448u1D443 where u1D448 is orthogonal (and unique if u1D449?u1D449u1D447 is
nonsingular) and u1D443 is symmetric and positive semide?nite. So
u1D439(u1D440) = tr(u1D440u1D447u1D448u1D443).
Because u1D443 is a real symmetric matrix, it has a spectral decomposition u1D443 = u1D441u1D437u1D441u1D447 where u1D441 is an
orthogonal matrix and u1D437 = diag(u1D4511,...,u1D451u1D45B) with u1D451u1D456 ?u1D451u1D456+1. De?ning u1D44B = u1D441u1D440u1D447u1D448u1D441u1D447 results in
u1D439(u1D440) = tr(u1D440u1D447u1D448u1D441u1D447u1D437u1D441) = tr(u1D441u1D440u1D447u1D448u1D441u1D447u1D437) = tr(u1D44Bu1D437) =
u1D458uni2211.alt02
u1D456=1
u1D451u1D456u1D465u1D456u1D456.
u1D439(u1D440) attains its maximum value when the elements u1D465u1D456u1D456 are all at their maximum value. Since
u1D44B is an orthogonal matrix, all of its elements lie between the values 1 and -1. This means the
diagonal elements are maximized when they are equal to 1, which means the u1D439(u1D440) is the identity
matrix.
Since u1D440 is a rotation matrix, its determinant is required to be 1. Thus the determinant of u1D44B
is
?u1D44B? = ?u1D441u1D440u1D447u1D448u1D441u1D447? = ?u1D441??u1D440u1D447??u1D448??u1D441u1D447? = ?u1D441?2?u1D440??u1D448? = ?u1D448?.
If det(u1D448) = 1, then u1D44B = u1D43C maximizes u1D439(u1D440). If det(u1D448) = ?1, then det(u1D44B) = ?1, and a solution is
u1D44B =
?
?? u1D43Cu1D458?1 0
0 ?1
?
??.
7
De?ne u1D44B0 as the matrix which maximizes u1D439(u1D440) according to the determinant of u1D448 so that u1D44B0 =
u1D441u1D440u1D4470 u1D448u1D441u1D447. Then the rotation matrix that minimizes the sum of squares of u1D444(u1D440) is
u1D4400 = u1D448u1D441u1D447u1D44Bu1D4470 u1D441.
The matrix is unique if u1D449?u1D449u1D447 is nonsingular.
Wessner o?ers another solution to Wahba?s problem [2]. Like Farrell and Stuelpnagel, Wessner
recasts the problem to maximize
u1D439(u1D440) = tr(u1D440u1D447u1D449?u1D449u1D447).
If u1D449?u1D449u1D447 is nonsingular, then u1D449?u1D449u1D447 has the polar decomposition
u1D449?u1D449u1D447 = u1D434 = u1D448u1D443.
De?ne u1D448 = (u1D434u1D447)?1(u1D434u1D447u1D434)1/2 and u1D443 = (u1D434u1D447u1D434)1/2 where (u1D434u1D447u1D434)1/2 is the symmetric square root
of u1D434u1D447u1D434 with positive eigenvalues. From Farrell and Stuelpnagel, the optimal solution is u1D440u1D45C =
u1D448u1D441u1D447u1D44Bu1D45Cu1D441 where u1D441 is orthogonal. Wessner assumes that the determinant of u1D434 is positive; thus,
u1D44Bu1D45C = u1D43C. Then u1D440u1D45C = u1D448u1D441u1D447u1D441 = u1D448, which results in
u1D440u1D45C = (u1D434u1D447)?1(u1D434u1D447u1D434)1/2
= (u1D449?u1D449u1D447)?1(u1D449?u1D449u1D447u1D449?u1D449u1D447)1/2
2.3 Davenport?s q-Method
Another method for the solution of Wahba?s problem is the u1D45E-method developed by Paul
Davenport in 1968 [3]. Davenport de?nes a rotation vector to solve the problem. We discuss the
q-method in two parts. We ?rst present an overview of rotation vectors, followed by a discussion of
Wahba?s problem. Davenport often uses the notation u1D44B2 to represent u1D44Bu1D447u1D44B where u1D44B is a vector.
Thus, in Davenport?s notation, u1D44B2 is a scalar value not to be confused with the vector value u1D44B.
For simplicity, we use Davenport?s notation in this discussion.
8
2.3.1 Rotation Vectors
A rotation of an angle 0 ? u1D703 ? u1D70B about a unit vector u1D44B =
uni005B.alt03
u1D4651 u1D4652 u1D4653
uni005D.alt03
? ?3 can be
represented as the 3?3 matrix operator
u1D445u1D465(u1D703) = cos(u1D703)
?
??
??
?
1 0 0
0 1 0
0 0 1
?
??
??
?
+(1?cos(u1D703))
?
??
??
?
u1D46521 u1D4651u1D4652 u1D4651u1D4653
u1D4651u1D4652 u1D46522 u1D4652u1D4653
u1D4651u1D4653 u1D4652u1D4653 u1D46523
?
??
??
?
+ sin(u1D703)
?
??
??
?
0 u1D4653 ?u1D4652
?u1D4653 0 u1D4651
u1D4652 ?u1D4651 0
?
??
??
?
(2.3)
and
u1D445 = (u1D435u1D447)?1u1D435 (2.4)
where
u1D435 =
?
??
??
?
1 u1D4653 tanuni0028.alt01u1D7032uni0029.alt01 ?u1D4652 tanuni0028.alt01u1D7032uni0029.alt01
?u1D4653 tanuni0028.alt01u1D7032uni0029.alt01 1 u1D4651 tanuni0028.alt01u1D7032uni0029.alt01
u1D4652 tanuni0028.alt01u1D7032uni0029.alt01 ?u1D4651 tanuni0028.alt01u1D7032uni0029.alt01 1
?
??
??
?
. (2.5)
In order to simplify notation, de?ne
u1D44C = tan
uni0028.alt03u1D703
2
uni0029.alt03
u1D44B and u1D44D = sin
uni0028.alt03u1D703
2
uni0029.alt03
u1D44B
from which we obtain sinuni0028.alt01u1D7032uni0029.alt01 =
?u1D44C2
?1+u1D44C2 =
?u1D44D2, cosuni0028.alt01u1D703
2
uni0029.alt01 = 1
?1+u1D44C2 =
?1?u1D44D2, sin(u1D703) = 2?u1D44C2
1+u1D44C2 =
2uni221A.alt01u1D44D2(1?u1D44D2), and cos(u1D703) = 1?u1D44C21+u1D44C2 = 1?2u1D44D2. Hence Equation (2.3) can be rewritten as
u1D445 = 11+u1D44C2
?
??
??
?
(1?u1D44C2)
?
??
??
?
1 0 0
0 1 0
0 0 1
?
??
??
?
+2
?
??
??
?
u1D46621 u1D4661u1D4662 u1D4661u1D4663
u1D4661u1D4662 u1D46622 u1D4662u1D4663
u1D4661u1D4663 u1D4662u1D4663 u1D46622
?
??
??
?
+2
?
??
??
?
0 u1D4663 ?u1D4662
?u1D4663 0 u1D4661
u1D4662 ?u1D4661 0
?
??
??
?
?
??
??
?
(2.6)
9
or
u1D445 = (1?2u1D44D2)
?
??
??
?
1 0 0
0 1 0
0 0 1
?
??
??
?
+2
?
??
??
?
u1D46721 u1D4671u1D4672 u1D4671u1D4673
u1D4671u1D4672 u1D46722 u1D4672u1D4673
u1D4671u1D4673 u1D4672u1D4673 u1D46723
?
??
??
?
+2
uni221A.alt01
1?u1D44D2
?
??
??
?
0 u1D4673 ?u1D4672
?u1D4673 0 u1D4671
u1D4672 ?u1D4671 0
?
??
??
?
. (2.7)
Alternatively, u1D435 may be written in terms of the elements of the vector u1D44C as
u1D435 =
?
??
??
?
1 u1D4663 ?u1D4662
?u1D4663 1 u1D4661
u1D4662 ?u1D4661 1
?
??
??
?
. (2.8)
Let u1D445 be a rotation matrix, and separate u1D445 into symmetric and skew symmetric parts
u1D445 = 12(u1D445+u1D445u1D447)+ 12(u1D445?u1D445u1D447) (2.9)
where u1D445+u1D445u1D447 is symmetric and u1D445?u1D445u1D447 is skew symmetric. Then a comparison of Equation (2.9)
to Equation (2.7) yields nine conditions for the vector u1D44D. Taking the trace u1D70E of Equation (2.7)
yields
u1D70E = 3?4u1D44D2 = 1+2cos(u1D703) (2.10)
and inspection of the skew symmetric portion of u1D445 yields
2
uni221A.alt01
1?u1D44D2u1D44D = 12
?
??
??
?
u1D45F23 ?u1D45F32
u1D45F31 ?u1D45F13
u1D45F12 ?u1D45F21
?
??
??
?
. (2.11)
Rearrange Equations (2.10) and (2.11) to obtain
u1D44D2 = 3?u1D70E4 and u1D44D = 12?1+u1D70E
?
??
??
?
u1D45F23 ?u1D45F32
u1D45F31 ?u1D45F13
u1D45F12 ?u1D45F21
?
??
??
?
.
10
We write u1D44D2 in terms of u1D703 which results in
u1D44D2 = 12 ? 12 cos(u1D703),
which means that u1D44D2 ? 1. Thus, the mapping de?ned by Equation (2.7) is a mapping of three-
dimensional vectors over the ?eld of real numbers whose Euclidean length is less than or equal to
one (denote this set of vectors byu1D701) onto the group of rotation matrices. The mapping is one-to-one
except when u1D44D2 = 1 (u1D70E = ?1).
Since u1D44C = u1D44Dcos(u1D703
2)
, u1D44C = 1?1?u1D44D2u1D44D. Then using the same methods as above
u1D44C = 11+u1D70E
?
??
??
?
u1D45F23 ?u1D45F32
u1D45F31 ?u1D45F13
u1D45F12 ?u1D45F21
?
??
??
?
.
u1D44C is unde?ned when u1D70E = ?1. To avoid this singularity, allow vectors of in?nite magnitude whose
direction is given by a unit vector u1D44B. When u1D70E = ?1, Equation (2.6) reduces to
u1D445 = ?u1D43C +2u1D44Bu1D44Bu1D447. (2.12)
Let u1D702 denote the set of all real three-dimensional vectors augmented by the vectors of in?nite
magnitude just discussed. Then Equations (2.6) and (2.12) de?ne a mapping from u1D702 onto the group
of rotation matrices. Thus, either u1D701 or u1D702 may be used to parametrize the group of rotations. To
distinguish between the new type of vector and ordinary vectors, Davenport de?nes the rotation
vector: Given a set u1D6FF of real three-dimensional vectors and a mappingu1D70F that maps u1D6FF onto the group
of rotation matrices, then two elements of u1D6FF are said to be equivalent if they map into the same
rotation matrix. Normal vector operations are applied to rotation vectors.
Rotation vectors can be combined to yield new rotation vectors. Given two rotation vectors u1D44C1
and u1D44C2, there are two associated rotation matrices u1D4451 and u1D4452. Then u1D445 = u1D4452u1D4451 is also a rotation
11
matrix, and there exists an associated rotation vector u1D44C. This vector u1D44C is given by
u1D44C = 11?u1D44C
1u1D44C2
(u1D44C1 +u1D44C2 +u1D44C1 ?u1D44C2). (2.13)
Similarly, for the two rotation vectors u1D44D1 and u1D44D2 which de?ne rotations u1D4451 and u1D4452, there exists a
u1D44D that gives u1D445 = u1D4452u1D4452. u1D44D is de?ned by
u1D44D = sgn
uni0028.alt03uni221A.alt02
1?u1D44D21
uni221A.alt02
1?u1D44D22 ?u1D44D1 ?u1D44D2
uni0029.alt03
u1D44D0 (2.14a)
u1D44D0 =
uni221A.alt02
1?u1D44D22u1D44D1 +
uni221A.alt02
1?u1D44D21u1D44D2 +u1D44D1 ?u1D44D2. (2.14b)
This leads to the de?nition of Davenport?s rotation product: Let u1D6FF be a set of vectors, let ? be a
binary operation on u1D6FF, and let u1D70F be a mapping of u1D6FF onto the group of rotation matrices. Then ? is
said to be a rotation product if it is preserved by u1D70F, i.e. if u1D70F(u1D449 ?u1D44A) = u1D70F(u1D449)u1D70F(u1D44A) for every u1D449 and
u1D44A in u1D6FF.
Davenport also develops other useful relationships between rotation matrices, vectors, and
rotation vectors. Assume a vector u1D449 is rotated by u1D445 to yield u1D449? = u1D445u1D449. If u1D44C and u1D44D are the rotation
vectors de?ning u1D445, then u1D449? is
u1D449? = u1D44Cu1D449
= 11+u1D44C2[(1?u1D44C2)u1D449 +2(u1D449 ?u1D44C)u1D44C +2u1D449 ?u1D44C], u1D44C2 <? (2.15a)
= ?u1D449 +2(u1D449 ?u1D44B)u1D44B, u1D44C2 = ? (2.15b)
u1D449? = u1D44Du1D449 = (1?2u1D44D2)u1D449 +2(u1D449 ?u1D44D)u1D44D +2
uni221A.alt01
1?u1D44D2u1D449 ?u1D44D. (2.16)
To determine u1D445 from u1D449? and u1D449 where u1D449? = u1D445u1D449
u1D44C = 11+u1D449 ?u1D449?u1D449? ?u1D449, u1D449 ?u1D449? ?= ?1 (2.17)
u1D44D = 1uni221A.alt012(1 +u1D449 ?u1D449?)u1D449? ?u1D449, u1D449 ?u1D449? ?= ?1 (2.18)
12
If u1D449 ?u1D449? = ?1, then u1D44D is any vector satisfying the two conditions u1D44D2 = 1 and u1D44D?u1D449 = 0 and Y has
in?nite magnitude with direction de?ned by u1D44D.
2.3.2 q-Method
Davenport poses Wahba?s problem in the following manner
u1D719(u1D445) =
u1D45Buni2211.alt02
u1D456=1
(u1D44Au1D456 ?u1D445u1D449u1D456)2
=
u1D45Buni2211.alt02
u1D456=1
[u1D44A2u1D456 +u1D4492u1D456 ?2u1D44Au1D456 ?(u1D445u1D449u1D456)]
where u1D449u1D456 is a vector in the inertial frame and u1D44Au1D456 is a vector in the body frame. Recasting the
problem in terms of the rotation vector u1D44C results in
u1D719(u1D44C) =
u1D45Buni2211.alt02
u1D456=1
(u1D4492u1D456 +u1D44A2u1D456 ? 21+u1D44C2[(1?u1D44C2)u1D449u1D456 ?u1D44Au1D456 +2(u1D449u1D456 ?u1D44C)(u1D44Au1D456 ?u1D44C)
+ 2(u1D44Au1D456 ?u1D449u1D456)?u1D44C]), u1D44C2 <? (2.19a)
=
u1D45Buni2211.alt02
u1D456=1
uni0028.alt01u1D4492
u1D456 +u1D44A
2
u1D456 ?2[?u1D449u1D456 ?u1D44Au1D456 +2(u1D449u1D456 ?u1D44B)(u1D44Au1D456 ?u1D44B)]
uni0029.alt01, u1D44C2 = ? (2.19b)
where u1D44B2 = 1. Take the derivative of Equation (2.19a) with respect to u1D466u1D457, where u1D457 refers to the
u1D457u1D461? component of a vector, to obtain
?u1D719
?u1D466u1D457 =
?4
(1+u1D44C2)2
u1D45Buni2211.alt02
u1D456=1
(2[(u1D449u1D456 ?u1D44Au1D456)?u1D44C ?(u1D449u1D456 ?u1D44C)(u1D44Au1D456 ?u1D44C)?u1D449u1D456u1D44Au1D456]u1D466u1D457
+(1+u1D44C2)[(u1D449u1D456 ?u1D44C)u1D464u1D456u1D457 +(u1D44Au1D456 ?u1D44C)u1D463u1D456u1D457 ?(u1D449u1D456 ?u1D44Au1D456)u1D457]).
Thus, for u1D719 to have a minimum, we must have
2
u1D45Buni2211.alt02
u1D456=1
[(u1D44Au1D456?u1D449u1D456)?u1D44C +(u1D449u1D456?u1D44C)(u1D44Au1D456?u1D44C)+u1D449u1D456?u1D44Au1D456]u1D44C = (1+u1D44C2)
u1D45Buni2211.alt02
u1D456=0
[(u1D449u1D456?u1D44C)u1D44Au1D456+(u1D44Au1D456?u1D44C)u1D449u1D456+u1D44Au1D456?u1D449u1D456]. (2.20)
13
Take the dot product with u1D44C of both sides and rearrange terms, which results in
2
u1D45Buni2211.alt02
u1D456=1
(u1D449u1D456 ?u1D44C)(u1D44Au1D456 ?u1D44C) =
u1D45Buni2211.alt02
u1D456=1
[(u1D44Au1D456 ?u1D449u1D456)?u1D44C +2u1D449u1D456 ?u1D44Au1D456]u1D44C ?
u1D45Buni2211.alt02
u1D456=1
(u1D44Au1D456 ?u1D449u1D456)?u1D44C (2.21)
Substituting Equation (2.21) into Equation (2.19a) results in the expression
u1D719(u1D445) =
u1D45Buni2211.alt02
u1D456=1
(u1D4492u1D456 +u1D44A2u1D456 ?2u1D449u1D456 ?u1D44Au1D456)?2u1D44C ?
u1D45Buni2211.alt02
u1D456=1
u1D44Au1D456 ?u1D449u1D456
which implies that u1D719 is minimized when 2u1D44C ?uni2211.alt01u1D45Bu1D456=1u1D44Au1D456 ?u1D449u1D456 is maximized. Substituting Equation
(2.21) into Equation (2.20) yields
u1D45Buni2211.alt02
u1D456=1
([(u1D44Au1D456 ?u1D449u1D456)?u1D44C +2u1D449u1D456 ?u1D44Au1D456]u1D44C ?[(u1D449u1D456 ?u1D44C)u1D44Au1D456 +(u1D44Au1D456 ?u1D44C)u1D449u1D456 +u1D44Au1D456 ?u1D449u1D456]) = 0
which can be written as
(u1D434u1D447u1D44Cu1D43C +u1D435)u1D44C = u1D434 (2.22)
where u1D43C is identity, u1D434 is the vector uni2211.alt01u1D45Bu1D456=1u1D44Au1D456 ?u1D449u1D456, and u1D435 is a symmetric matrix with elements
u1D44Fu1D457u1D458 = ?
u1D45Buni2211.alt02
u1D456=1
(u1D463u1D456u1D457u1D464u1D456u1D458 +u1D463u1D456u1D458u1D464u1D456u1D457), u1D457 ?= u1D458 (2.23)
u1D44Fu1D457u1D457 = 2
u1D45Buni2211.alt02
u1D456=1
(u1D449u1D456 ?u1D44Au1D456 ?u1D463u1D456u1D457u1D464u1D456u1D457). (2.24)
If u1D434 = 0, then u1D44C = 0 is the desired solution. For u1D434?= 0, multiply each side of Equation (2.22)
by the adjoint of (u1D434u1D447u1D44Cu1D43C +u1D435) to yield
det(u1D434u1D447u1D44Cu1D43C +u1D435)u1D44C = adj(u1D434u1D447u1D44Cu1D43C +u1D435)u1D434.
Then multiply the result by u1D434u1D447 to get
det(u1D434u1D447u1D44Cu1D43C +u1D435)u1D434u1D447u1D44C = u1D434u1D447 adj(u1D434u1D447u1D44Cu1D43C +u1D435)u1D434.
14
De?ne the scalar u1D434u1D447 = u1D706, det(u1D706u1D43C +u1D435) = u1D453(u1D706), and u1D434u1D447 adj(u1D706u1D43C +u1D435)u1D434 = u1D454(u1D706). Then
?(u1D706) = u1D706u1D453(u1D706)?u1D454(u1D706) = 0.
The maximum value of u1D44C ?uni2211.alt01u1D45Bu1D456=1(u1D44Au1D456 ?u1D449u1D456) (which minimizes u1D719(u1D44C)) is the largest zero of ?(u1D706), u1D7060,
where ?(u1D706) is a fourth-degree polynomial in u1D706 and ?u1D453(u1D706) is the characteristic polynomial of u1D435.
Since u1D719(u1D44C) is a nonnegative function,
u1D7060 ? 12
u1D45Buni2211.alt02
u1D456=1
(u1D449u1D456 ?u1D44Au1D456)2,
which gives an upper bound on u1D7060.
Since u1D435 is symmetric, an orthogonal matrix u1D443 exists such that u1D443?1u1D435u1D443 is diagonal. Let u1D44C?
and u1D448 be vectors such that u1D44C = u1D443u1D44C? and u1D434 = u1D443u1D448. Then
((u1D448u1D447u1D44C?)u1D43C +u1D435)u1D443u1D44C? = u1D443u1D448.
Multiply both sides by u1D443?1 to get
((u1D448u1D447u1D44C?)u1D43C +u1D437)u1D44C? = u1D448
whereu1D437 is a diagonal matrix with the eigenvalues of u1D435 arranged in increasing order (u1D7061 ?u1D7062 ?u1D7063)
as its entries. Multiply by adj((u1D448u1D447u1D44C?)u1D43C +u1D437) and then by u1D448u1D447 to get
det(u1D448u1D447u1D44C?u1D43C +u1D437)u1D448u1D447u1D44C? = u1D448u1D447 adj(u1D448u1D447u1D44C?u1D43C +u1D437)u1D448.
Since u1D448u1D447u1D44C? = u1D434u1D447u1D44C = u1D706 and u1D437 is diagonal,
u1D706(u1D706+u1D7061)(u1D706+u1D7062)(u1D706+u1D7063) = u1D46221(u1D706+u1D7062)(u1D706+u1D7063) +u1D46222(u1D706+u1D7061)(u1D706+u1D7063) +u1D46223(u1D706+u1D7061)(u1D706+u1D7062).
15
Some analysis on this equation yields a lower bound for u1D7060:
u1D7060 ? ?u1D7061
A numerical algorithm can then be used to ?nd u1D7060.
Since the above analysis is derived from Equation (2.19a), thesolution is only valid for rotations
of less than 180?. To obtain the minimum among all 180? rotations, a Lagrange multiplier term is
added to the derivative of Equation (2.19b) with respect to u1D465u1D457, which yields
?4
u1D45Buni2211.alt02
u1D456=1
(u1D449u1D456 ?u1D44B)u1D464u1D456u1D457 +(u1D44Au1D456 ?u1D44B)u1D463u1D456u1D457 +2u1D707u1D465u1D457 = 0, u1D457 = 1,2,3.
Writing the above equation in terms of a single vector equation results in
uni0028.alt04uni0028.alt04
u1D707
2 ?2
u1D45Buni2211.alt02
u1D456=1
u1D449u1D456 ?u1D44Au1D456
uni0029.alt04
u1D43C +u1D435
uni0029.alt04
u1D44B = 0 (2.25)
where the matrix u1D435 is as de?ned in Equations (2.23)-(2.24). The roots of Equation (2.25) are
u1D707u1D458
2 ?2
u1D45Buni2211.alt02
u1D456=1
u1D449u1D456 ?u1D44Au1D456 = u1D706u1D458, u1D458 = 1,2,3
where the u1D706u1D458?s are the eigenvalues of u1D435. The condition equations then become
(u1D435?u1D706u1D458u1D43C)u1D44B = 0, u1D44B2 = 1,
and the solutions are the unit eigenvectors of u1D435. Then the solution u1D719(u1D445) for rotations of 180? is
u1D719(u1D445) =
u1D45Buni2211.alt02
u1D456=1
(u1D449u1D456 ?u1D44Au1D456)2 +2u1D706u1D458
and
u1D719(u1D445) =
u1D45Buni2211.alt02
u1D456=1
(u1D449u1D456 ?u1D44Au1D456)2 ?2u1D7060
16
for rotations other than 180?. Thus, the rotation minimizing u1D719(u1D445) is
u1D44C = (u1D7060u1D43C +u1D435)?1u1D434
when u1D453(u1D7060) ?= 0 or
(u1D7060u1D43C +u1D435)u1D44B = 0 and u1D44B2 = 1
when u1D453(u1D7060) = 0, where u1D7060 is the largest zero of ?(u1D706).
For the case where u1D453(u1D7060) is near zero, a near-singular matrix occurs. This can be avoided by
using the u1D44D vector representation. This is given by
u1D44D = sgn(u1D453(u1D7060))uni221A.alt01u1D4532(u1D706
0)+u1D434u1D447(adj(u1D7060u1D43C +u1D435))2u1D434
adj(u1D7060u1D43C +u1D435)u1D434
when u1D453(u1D7060) ?= 0 and
(u1D7060u1D43C +u1D435)u1D44D = 0 and u1D44D2 = 1
when u1D453(u1D7060) = 0 where
adj(u1D706u1D43C +u1D435) = u1D7062u1D43C +u1D706(tr(u1D435)u1D43C ?u1D435)+adj(u1D435)
u1D453(u1D706) = u1D7063 +tr(u1D435)u1D7062 +tr(adj(u1D435))u1D706+det(u1D435)
u1D454(u1D706) = u1D434u1D447 adj(u1D706u1D43C +u1D435)u1D434.
For the case where u1D45B = 2, u1D44921 = u1D44922 , and u1D44A21 = u1D44A22, the least squares solution is found as
follows. De?ne
u1D4481 = u1D4491 ?u1D4492?u1D449
1 ?u1D4492?
u1D448?1 = u1D44A1 ?u1D44A2?u1D44A
1 ?u1D44A2?
u1D4482 = u1D4491 +u1D4492?u1D449
1 +u1D4492?
u1D448?2 = u1D44A1 +u1D44A2?u1D44A
1 +u1D44A2?
,
17
and use the rotation vector techniques to obtain the rotation that maps u1D448u1D456 to u1D448?u1D456 to yield the least
squares solution.
A concise summary of Davenport?s q-method is given in [4]. Wahba?s problem is presented as
u1D43D(u1D434) =
u1D45Buni2211.alt02
u1D456=1
u1D464u1D456??u1D462u1D456u1D435 ?u1D434?u1D462u1D456u1D445?2
where u1D464u1D456 is the weight of the u1D456th vector measurement, ?u1D462u1D456u1D435 are inertial frame vectors, ?u1D462u1D456u1D445 are the
corresponding vectors in the body frame, and u1D434 is a rotation matrix. The cost function may also
be written as
u1D43D(u1D434) = ?2
u1D45Buni2211.alt02
u1D456=1
u1D44Au1D456u1D434u1D449u1D456 +constant terms
where the unnormalized vectors u1D44Au1D456 and u1D449u1D456 are de?ned as
u1D44Au1D456 = ?u1D464u1D456?u1D462u1D456u1D435 u1D449u1D456 = ?u1D464u1D456?u1D462u1D456u1D445.
The loss function is a minimum when
u1D43D?(u1D434) =
u1D45Buni2211.alt02
u1D456=1
u1D44Au1D456u1D434u1D449u1D456 = tr(u1D44Au1D447u1D434u1D449)
u1D44A =
uni005B.alt03
u1D44A1 u1D44A2 ... u1D44Au1D45B
uni005D.alt03
u1D449 =
uni005B.alt03
u1D4491 u1D4492 ... u1D449u1D45B
uni005D.alt03
and is maximized.
To ?nd the attitude matrix u1D434, write u1D434 in terms of the quaternion u1D45E
u1D434(u1D45E) = (u1D45E24 ?q?q)u1D43C +2qqu1D447 ?2u1D45E4u1D444
18
where
u1D45E =
?
?? q
u1D45E4
?
??
u1D444 =
?
??
??
?
0 ?u1D45E3 u1D45E2
u1D45E3 0 ?u1D45E1
?u1D45E2 u1D45E1 0
?
??
??
?
.
Substitute u1D434(u1D45E) into the cost function to yield
u1D43D?(u1D45E) = u1D45Eu1D447u1D43Eu1D45E (2.26)
where
u1D43E =
?
?? u1D446?u1D43Cu1D70E u1D44D
u1D44Du1D447 u1D70E
?
??
u1D435 = u1D44Au1D449u1D447
u1D446 = u1D435u1D447 +u1D435
u1D44D =
uni005B.alt03
u1D43523 ?u1D43532 u1D43531 ?u1D43513 u1D43512 ?u1D43521
uni005D.alt03u1D447
u1D70E = tr(u1D435).
The extrema of u1D43D?, subject to the constraint u1D45Eu1D447u1D45E = 1, can be found using Lagrange multipliers.
De?ne
u1D454(u1D45E) = u1D45Eu1D447u1D43Eu1D45E?u1D706u1D45Eu1D447u1D45E
where u1D706 is the Lagrange multiplier. Di?erentiating the equation with respect to u1D45Eu1D447 and setting the
result equal to zero results in
u1D43Eu1D45E = u1D706u1D45E. (2.27)
19
Thus, the quaternion that gives the optimal attitude is an eigenvector of u1D43E. Substitute Equation
(2.27) into Equation (2.26) to get
u1D43D?(u1D45E) = u1D45Eu1D447u1D43Eu1D45E = u1D45Eu1D447u1D706u1D45E = u1D706.
Thus, u1D43D? is maximized when the eigenvector corresponding to the largest eigenvalue is chosen.
2.4 QUEST Algorithm
The quaternion estimation (QUEST) algorithm is a method to solve Wahba?s problem. The
algorithm is asingle-point algorithm soattitude estimates arebasedsolely oncurrentmeasurements.
The following is a summary of the QUEST algorithm as presented in [5].
Wahba?s problem can be posed in terms of quaternions as
u1D43D(u1D45E) = 12
u1D458uni2211.alt02
u1D456=1
u1D44Eu1D456?bu1D456 ?u1D434(u1D45E)ru1D456?2 (2.28)
where bu1D456 are measured body frame unit vectors, ru1D456 are the corresponding vectors in the inertial
frame, and u1D44Eu1D456 are positive weights. The goal is to ?nd the quaternion u1D45E that minimizes the cost
function u1D43D. The minimization of Equation (2.28) can be written as the maximization of the cost
function u1D454(u1D45E)
u1D454(u1D45E) = 1?u1D43D(u1D45E)/
uni0028.alt04
1
2
u1D458uni2211.alt02
u1D456=1
u1D44Eu1D456
uni0029.alt04
,
which can be written as
u1D454(u1D45E) = u1D45Eu1D447u1D43Eu1D45E. (2.29)
The matrix u1D43E is formed as follows. De?ne
u1D45Au1D458 =
u1D458uni2211.alt02
u1D456
u1D44Eu1D456 (2.30a)
u1D70E = 1u1D45A
u1D458
u1D458uni2211.alt02
u1D456=1
u1D44Eu1D456bu1D447u1D456 ru1D456 (2.30b)
20
u1D435 = 1u1D45A
u1D458
u1D458uni2211.alt02
u1D456=1
u1D44Eu1D456bu1D456ru1D447u1D456 (2.30c)
u1D446 = u1D435+u1D435u1D447 (2.30d)
z = 1u1D45A
u1D458
u1D458uni2211.alt02
u1D456=1
u1D44Eu1D456(bu1D456 ?ru1D456) (2.30e)
u1D43E =
?
?? u1D446?u1D70Eu1D43C z
zu1D447 u1D70E
?
??. (2.30f )
The optimal unit quaternion u1D45E? that maximizes u1D454(u1D45E) satis?es
u1D43Eu1D45E? = u1D706u1D45E?
where u1D706 is a Lagrange multiplier. Then u1D706 is an eigenvalue of u1D43E and u1D45E? is an eigenvector. Thus,
u1D454(u1D45E?) = u1D706. Because it is desired to maximize u1D454, choose u1D706 = u1D706u1D440u1D434u1D44B, the largest eigenvalue of u1D43E,
and u1D45E? as the corresponding eigenvector. Rodrigues parameters (or Gibbs vector) can be used to
calculate the eigenvector:
y? = [(u1D706u1D440u1D434u1D44B +u1D70E)u1D43C ?u1D446]?1z (2.31a)
u1D45E? = 1uni221A.alt011+?y??2
?
?? y?
1
?
??. (2.31b)
The maximum eigenvalue is exactly one when error free measurements are used; otherwise the
eigenvalue is close to one.
2.5 Kalman Filter Type Approach
In [6] and [7] Shuster develops the basis to cast the QUEST algorithm as a Kalman ?lter-
type problem. In [6] it is shown that Wahba?s problem can be equivalent to maximum likelihood
estimation through the appropriate choice of weights. In [7] Shuster extends the results of [6] to
21
form a ?lter QUEST algorithm that is comparable to a Kalman ?lter. [6] is summarized in ?2.5.1
and [7] in ?2.5.2.
2.5.1 Wahba?s Problem as Maximum Likelihood Estimation
Wahba?s loss function can be written as
u1D43F(u1D434) = 12
u1D45Buni2211.alt02
u1D458=1
u1D44Eu1D458? ?u1D44Au1D458 ?u1D434?u1D449u1D458?2
where ?u1D44Au1D458 are body-?xed unit vectors and ?u1D449u1D458 are the same unit vectors in an inertial frame. Assume
?u1D44Au1D458 is the measurement provided by sensor u1D458 and take the unit vector measurement to have the
probability density
u1D70C?u1D44Au1D458( ?u1D44A?u1D458,u1D434) = u1D4A9u1D458exp
uni0028.alt03
? 12u1D70E2
u1D458
? ?u1D44A?u1D458 ?u1D434?u1D449u1D458?2
uni0029.alt03
(2.32)
which is de?ned over the unit sphere
? ?u1D44A?u1D458? = 1. (2.33)
?u1D44A?u1D458 is the value of the random variable ?u1D44Au1D458 which satis?es Equation (2.33), and u1D4A9u1D458 is chosen to
ensure that the probability density function is valid:
u1D4A9?1u1D458 =
uni222B.alt02 2u1D70B
0
uni222B.alt02 u1D70B
0
exp
uni0028.alt03
? 12u1D70E2
u1D458
(1?cos(u1D703))
uni0029.alt03
sin(u1D703)u1D451u1D703u1D451u1D719
u1D4A9u1D458 = [2u1D70Bu1D70E2u1D458(1?u1D452?2/u1D70E2u1D458)]?1. (2.34)
The probability density function is about the direction u1D434?u1D449u1D458 and can be approximated by a
tangent plane
u1D44Au1D458 = u1D434?u1D449u1D458 +?u1D44Au1D458, ?u1D44Au1D458 ?u1D434?u1D449u1D458 = 0.
The sensor error ? ?u1D44Au1D458 is approximately Gaussian with
u1D438[?u1D44Au1D458] = 0, u1D438[?u1D44Au1D458?u1D44Au1D447u1D458 ] = u1D70E2u1D458[u1D43C ?(u1D434?u1D449u1D458)(u1D434?u1D449u1D458)u1D447]
22
It can be shown that for ?u1D44Au1D458
u1D438[? ?u1D44Au1D458] = ?u1D70C2u1D458u1D70Fu1D458u1D434?u1D449u1D458
u1D438[? ?u1D44Au1D458? ?u1D44Au1D458] = u1D70C2u1D458[u1D43C ?(3?2u1D70Fu1D458)(u1D434?u1D449u1D458)(u1D434?u1D449u1D458)u1D447]
u1D70C2u1D458 = 12u1D438[? ?u1D44Au1D458 ?(u1D434?u1D449u1D458)?2]
u1D70Fu1D458 = 1u1D70C2
u1D458
u1D438[1? ?u1D44Au1D458 ?u1D434?u1D449u1D458].
which means that
u1D70C2u1D458 = u1D70E2u1D458 ?u1D70E4u1D458 +u1D442(u1D452?2/u1D70E2u1D458)
u1D70Fu1D458 = 11?u1D70E2
u1D458
+u1D442(u1D452?2/u1D70E2u1D458).
De?ne u1D44D?u1D458 as a sequence of measurements and u1D70Cu1D4671,...,u1D467u1D45B(u1D44D?1,...u1D44D?u1D45B,u1D465) as a joint probability
distribution where u1D465 is a parameter vector. Then for the model given by Equation (2.32)
u1D70Cu1D4671,...,u1D467u1D45B(u1D44D?1,...,u1D44D?u1D45B,u1D434) =
u1D45Buni220F.alt02
u1D458=1
1
2u1D70Bu1D70E2u1D458u1D453u1D458 exp(??
?u1D44Au1D458 ?u1D434?u1D449u1D458?2/2u1D70E2u1D458)
u1D453u1D458 = 1?u1D452?2/u1D70E2u1D458.
De?ne the negative-log-likelihood function as
u1D43D(u1D465) = ?loguni0028.alt01u1D70Cu1D4671,...,u1D467u1D45B(u1D44D?1,...,u1D44D?u1D45B,u1D465)uni0029.alt01
so that
u1D43D(u1D434) =
u1D45Buni2211.alt02
u1D458=1
uni0028.alt03 1
2u1D70E2u1D458?
?u1D44A?u1D458 ?u1D434?u1D449u1D458?2 +logu1D70E2u1D458 +log2u1D70B+logu1D453u1D458
uni0029.alt03
. (2.35)
Allowing u1D44Eu1D458 = 1u1D70E2
u1D458
, the negative-log-likelihood function becomes equivalent to the Wahba loss func-
tion.
23
The QUEST algorithm gives an estimate of attitude. To get an estimate of the error covariance
matrix, the Fisher information matrix is used. The Fisher information matrix for u1D465 is de?ned by
u1D439u1D465u1D465 = u1D438
uni005B.alt03 ?2
?u1D465?u1D465u1D447u1D43D(u1D465)
uni005D.alt03
u1D465u1D461u1D45Fu1D462u1D452
.
As more data is gathered, the Fisher information matrix approaches the inverse of the error covari-
ance matrix
limu1D45B??u1D439u1D465u1D465 = u1D443?1u1D465u1D465 .
The Fisher information matrix is not de?ned in terms of the quaternion, but rather in terms of
incremental error angles u1D703 for which
u1D434= u1D452[[u1D703]]u1D434u1D461u1D45Fu1D462u1D452
where the notation [[x]] denotes the matrix
[[x]] =
?
??
??
?
0 u1D4653 ?u1D4652
?u1D4653 0 ?u1D4651
u1D4652 ?u1D4651 0
?
??
??
?
for some vector x. Then
u1D43D(u1D703) = u1D706(0)u1D440u1D434u1D44B ?tr(u1D452[[u1D703]]u1D434u1D461u1D45Fu1D462u1D452u1D435u1D447)
which leads to
u1D439u1D703u1D703 = tr(u1D434u1D461u1D45Fu1D462u1D452u1D435u1D447u1D461u1D45Fu1D462u1D452)u1D43C ?u1D434u1D461u1D45Fu1D462u1D452u1D435u1D447u1D461u1D45Fu1D462u1D452
=
u1D45Buni2211.alt02
u1D458=1
1
u1D70E2u1D458(u1D43C ?(
?u1D44Au1D458)u1D461u1D45Fu1D462u1D452( ?u1D44Au1D447u1D458 )u1D461u1D45Fu1D462u1D452)
where
( ?u1D44Au1D458)u1D461u1D45Fu1D462u1D452 = u1D434u1D461u1D45Fu1D462u1D452?u1D449u1D458.
24
Evaluated at the maximum likelihood estimate of the attitude, u1D439u1D703u1D703 is approximated as
u1D439u1D703u1D703 ? tr(u1D434??u1D440u1D43Fu1D435?u1D447)u1D43C ?u1D434??u1D440u1D43Fu1D435?u1D447.
Then
u1D435? =
uni005B.alt031
2 tr(u1D439u1D703u1D703)u1D43C ?u1D439u1D703u1D703
uni005D.alt03
u1D434??u1D440u1D43F.
2.5.2 Filter QUEST Algorithm
The attitude estimate u1D434?u1D458 based on the ?rst u1D458 estimates and the error covariance matrix u1D443u1D458?u1D458
are determined by
u1D435u1D458 =
u1D458uni2211.alt02
u1D459=1
u1D44Eu1D459 ?u1D44Au1D459?u1D449u1D447u1D459 .
For the reasons presented in ?2.5.1 Shuster chooses u1D44Eu1D459 = 1/u1D70E2u1D459 . Thus,
u1D435u1D458 = u1D435u1D458?1 + 1u1D70E2
u1D458
?u1D44Au1D458?u1D449u1D447u1D458 . (2.36)
When u1D434u1D458 is constant, Equation (2.36) is the ?lter.
For dynamic systems
u1D451
u1D451u1D461 = [[u1D714(u1D461)]]u1D434(u1D461).
Let u1D434(u1D461u1D458) satisfy u1D434u1D458+1 = u1D719u1D458u1D434u1D458 with known u1D719u1D458. De?ne
u1D451
u1D451u1D461
?u1D44Au1D458(u1D461) = [[u1D714(u1D461)]] ?u1D44Au1D458(u1D461)
with boundary condition ?u1D44Au1D458(u1D461) = ?u1D44Au1D458 where ?u1D44Au1D458 is the time u1D461u1D458 measurement. So
?u1D44Au1D459(u1D461u1D458+1) = u1D719u1D458 ?u1D44Au1D459(u1D461u1D458).
Then u1D435u1D458 at time u1D461u1D458 is
u1D435u1D458 =
u1D458uni2211.alt02
u1D459=1
1
u1D70E2u1D459 u1D719u1D458?1u1D719u1D458?2...u1D719u1D459
?u1D44Au1D459?u1D449u1D447u1D459
25
which satis?es
u1D435u1D458 = u1D719u1D458?1u1D435u1D458?1 + 1u1D70E2
u1D458
?u1D44Au1D458?u1D449u1D447u1D458 .
De?ne
u1D435u1D458?u1D458?1 = u1D435(u1D461u1D458) given ?u1D44A1,..., ?u1D44Au1D458?1
=
u1D458?1uni2211.alt02
u1D459=1
1
u1D70E2u1D459
?u1D44Au1D459(u1D461u1D458)?u1D449u1D447u1D459
u1D435u1D458?u1D458 = u1D435(u1D461u1D458) given ?u1D44A1,..., ?u1D44Au1D458
=
u1D458uni2211.alt02
u1D459=1
1
u1D70E2u1D459
?u1D44Au1D459(u1D461u1D458)?u1D449u1D447u1D459 .
Then the ?lter QUEST algorithm is
u1D435u1D458?u1D458 = u1D719u1D458?1u1D435u1D458?1?u1D458?1 (2.37a)
= u1D435u1D458?u1D458?1 + 1u1D70E2
u1D458
?u1D44Au1D458?u1D449u1D447u1D458 . (2.37b)
Information from the previous attitude u1D434?0?0 and corresponding estimate error covariance matrix
u1D4430?0 can be included
u1D4350?0 =
uni005B.alt031
2 tr(u1D443
?1
0?0)u1D43C ?u1D443
?1
0?0
uni005D.alt03
u1D434?0?0. (2.38)
Otherwise, if no a priori information is available
u1D4350?0 = 0. (2.39)
Direct incorporation of process noise into the QUEST algorithm problem statement results in
prohibitively expensive computational costs in the resulting algorithm. An alternative approach is
26
to use a fading memory approximation. With this method the ?lter QUEST formulation becomes
u1D4350?0 =
uni005B.alt031
2 tr(u1D443
?1
0?0 )u1D43C ?u1D443
?1
0?0
uni005D.alt03
u1D434?0?0 or 0 (2.40)
u1D435u1D458+1?u1D458 = u1D6FCu1D719u1D458u1D435u1D458?u1D458 (2.41)
u1D435u1D458?u1D458 = u1D435u1D458?u1D458?1 + 1u1D70E2
u1D458
?u1D44Au1D458?u1D449u1D447u1D458 . (2.42)
For u1D6FC = 1 the usual QUEST algorithm results and for u1D6FC = 0 only the current measurements
contribute to the estimate. Notice that unlike a traditional Kalman ?lter, the QUEST algorithm
computes only attitude. It does not include estimates of biases or misalignments.
2.6 REQUEST Algorithm
The REQUEST (recursive quaternion estimation) algorithm, developed by Bar-Itzhack [5], is
a recursive version of the QUEST algorithm. Rather than updating the attitude pro?le matrix u1D435
as Shuster does in the ?lter QUEST algorithm [7], the REQUEST algorithm recursively updates
the QUEST algorithm?s u1D43E matrix. An algorithm is developed for both the time-invariant case and
time-varying case.
2.6.1 Time-Invariant REQUEST
For the time-invariant case, the vehicle?s body frame axes are assumed to be nonrotating with
respect to the reference axes. The algorithm begins by ?rst processing u1D458 sets of vectors with the
QUEST algorithm. Let u1D43Eu1D458 denote the u1D43E matrix computed by the QUEST algorithm. It is desired
to update u1D43E with u1D457 new pairs of vectors. Then de?ne bu1D456 as measured body frame unit vectors, ru1D456
27
as the corresponding vectors in the inertial frame, and u1D44Eu1D456 as positive weights, and let
u1D6FFu1D45Au1D458+u1D457 =
u1D458+u1D457uni2211.alt02
u1D456=u1D458+1
u1D44Eu1D456 (2.43a)
u1D6FFu1D70Eu1D458+u1D457 =
u1D458+u1D457uni2211.alt02
u1D456=u1D458+1
u1D44Eu1D456bu1D447u1D456 ru1D456 (2.43b)
u1D6FFu1D435u1D458+u1D457 =
u1D458+u1D457uni2211.alt02
u1D456=u1D458+1
u1D44Eu1D456bu1D456ru1D447u1D456 (2.43c)
u1D6FFu1D446u1D458+u1D457 = u1D6FFu1D435u1D458+u1D457 +u1D6FFu1D435u1D447u1D458+u1D457 (2.43d)
u1D6FFzu1D458+u1D457 =
u1D458+u1D457uni2211.alt02
u1D456=u1D458+1
u1D44Eu1D456(bu1D456 ?ru1D456) (2.43e)
u1D6FFu1D43Eu1D458+u1D457 =
?
?? u1D6FFu1D446u1D458+u1D457 ?u1D6FFu1D70Eu1D458+u1D457u1D43C u1D6FFzu1D458+u1D457
u1D6FFzu1D447u1D458+u1D457 u1D6FFu1D70Eu1D458+u1D457
?
??. (2.43f )
Then the new algorithm is
u1D45Au1D458+u1D457 = u1D45Au1D458 +u1D6FFu1D45Au1D458+u1D457 (2.44a)
u1D43Eu1D458+u1D457 = (u1D45Au1D458/u1D45Au1D458+u1D457)u1D43Eu1D458 +(1/u1D45Au1D458+u1D457)u1D6FFu1D43Eu1D458+u1D457. (2.44b)
The maximum eigenvalue of u1D43Eu1D458+u1D457 is then calculated, and Equations (2.31a)-(2.31b) are used to
?nd the optimal attitude quaternion.
2.6.2 Time-Varying REQUEST
In the time-varying case, the body frame has rotation with respect to the reference frame.
Bar-Itzhack develops the REQUEST algorithm [5] both for the case of error-free measurements
(i.e. no process noise) and for the case of noisy measurements.
28
Error-Free Propagation
Assume that at timeu1D461u1D45B, u1D458 pairs of vectors bu1D456 and ru1D456 are processed using the QUEST algorithm.
Immediately afterward, the body frame axes rotate to a new orientation and at time u1D461u1D45B+1 u1D457 new
vector measurements are taken. It is desired to ?nd the attitude quaternion estimate u1D45E?u1D45B+1?u1D45B at time
u1D461u1D45B+1 based on the ?rst u1D458 measurements. Then the cost function u1D454(u1D45E) = u1D45Eu1D447u1D43Eu1D45E can be written as
u1D454(u1D45Eu1D45B?u1D45B) = u1D45Eu1D447u1D45B?u1D45Bu1D43Eu1D45B?u1D45Bu1D45Eu1D45B?u1D45B.
The dynamics of a rotation are described by the di?erential equation
?u1D45E = 12?u1D45E (2.45)
where ? is a skew symmetric matrix of angular velocities
? =
?
??
??
??
??
0 u1D714u1D467 ?u1D714u1D466 u1D714u1D465
?u1D714u1D467 0 u1D714u1D465 u1D714u1D466
u1D714u1D466 ?u1D714u1D465 0 u1D714u1D467
?u1D714u1D465 ?u1D714u1D466 ?u1D714u1D467 0
?
??
??
??
??
.
Then
u1D45E(u1D461u1D45B+1) = ?(u1D461u1D45B+1,u1D461u1D45B)u1D45E(u1D461u1D45B)
where ?(u1D461u1D45B+1,u1D461u1D45B) is the state transition matrix. Set u1D45E(u1D461u1D45B) = u1D45Eu1D45B?u1D45B, where u1D45Eu1D45B?u1D45B is the quaternion to
be transformed from time u1D461u1D45B to time u1D461u1D45B+1, and u1D45E(u1D461u1D45B+1) = u1D45Eu1D45B+1?u1D45B where u1D45Eu1D45B+1?u1D45B is the transformed
quaternion u1D45Eu1D45B?u1D45B to get
u1D45Eu1D45B+1?u1D45B = ?u1D45Eu1D45B?u1D45B.
Because ? is skew symmetric, ? is orthogonal so
u1D45Eu1D45B?u1D45B = ?u1D447u1D45Eu1D45B+1?u1D45B.
29
Then
u1D454(u1D45Eu1D45B?u1D45B) = u1D454?(u1D45Eu1D45B+1?u1D45B) = u1D45Eu1D447u1D45B+1?u1D45B?u1D43Eu1D45B?u1D45B?u1D447u1D45Eu1D45B+1?u1D45B.
Thus, the problem has been transformed into ?nding the quaternion estimate u1D45Eu1D45B+1?u1D45B that
maximizes u1D454?. Let
u1D43Eu1D45B+1?u1D45B = ?u1D43Eu1D45B?u1D45B?u1D447.
Since u1D43Eu1D45B+1?u1D45B is obtained through a similarity transform, it has the same eigenvalues as u1D43Eu1D45B?u1D45B. Then
u1D454?(u1D45Eu1D45B+1?u1D45B) = u1D45Eu1D447u1D45B+1?u1D45Bu1D43Eu1D45B+1?u1D45Bu1D45Eu1D45B+1?u1D45B.
Adding the constraint that ?u1D45Eu1D45B+1?u1D45B?2 = 1 by using the Lagrange multiplier u1D706u1D45B+1?u1D45B, u1D45E?u1D45B+1?u1D45B satis?es
the equation
u1D43Eu1D45B+1?u1D45Bu1D45E?u1D45B+1?u1D45B = u1D706u1D45B+1?u1D45Bu1D45E?u1D45B+1?u1D45B
and is the eigenvector ofu1D43Eu1D45B?u1D45B that corresponds to the largest eigenvalue. So the algorithm becomes
u1D43Eu1D45B+1?u1D45B = ?u1D43Eu1D45B?u1D45B?u1D447 (2.46a)
u1D43Eu1D45B+1?u1D45B+1 = (u1D45Au1D45B/u1D45Au1D45B+1)u1D43Eu1D45B+1?u1D45B +(1/u1D45Au1D45B+1)u1D6FFu1D43Eu1D45B+1 (2.46b)
where u1D45Au1D45B = u1D45Au1D458 and u1D45Au1D45B+1 and u1D6FFu1D43Eu1D45B+1 are calculated using Equations (2.44a) and (2.43f), respec-
tively.
Noisy Propagation
For thecase ofnoiseless propagation, it isassumedthattherotation ratesu1D714areknownperfectly.
That is, there is noprocessnoise, only noise fromthesensors. With noisypropagation, the measured
angular rate can be described as
u1D714u1D45A = u1D714+u1D716
where u1D716 is the error component. Bar-Itzhack assumes that the gyros are of good quality with
slowly varying biases. To handle noisy measurements, two di?erent possible modi?cations to the
30
REQUEST algorithm are proposed:
u1D43Eu1D45B+1?u1D45B+1 = u1D70Cu1D45B(u1D45Au1D45B/u1D45Au1D45B+1)(u1D43Eu1D45B+1?u1D45B +1/u1D45Au1D45B+1)u1D6FFu1D43Eu1D45B+1
and
u1D43Eu1D45B+1?u1D45B+1 = u1D70Cu1D45Bu1D45Au1D45Bu1D45D
u1D45Bu1D45Au1D45B +u1D6FFu1D45Au1D45B+1
u1D43Eu1D45B+1?u1D45B + 1u1D70C
u1D45Bu1D45Au1D45B +u1D6FFu1D45Au1D45B+1
u1D6FFu1D43Eu1D45B+1
where 0 <u1D70Cu1D45B ? 1 is a forgetting factor to determine how much weight is placed on past measure-
ments.
2.7 Extended QUEST Algorithm
The QUEST algorithm and the previously presented algorithms based on QUEST have the
disadvantage of being unable to estimate anything other than attitude. Other dynamic states -
for example, gyro bias - cannot be estimated. In [8] Psiaki presents an extension of the QUEST
algorithm called Extended QUEST that attempts to overcome these limitations.
Yet another way to write Wahba?s loss function as given by Equation (2.28) is
u1D43Du1D444u1D448u1D438u1D446u1D447[u1D434(u1D45E)] =
u1D45Auni2211.alt02
u1D456=1
1
u1D70E2u1D456 +
1
2u1D45E
u1D447u1D43Bu1D45Au1D452u1D44Eu1D460u1D45E (2.47a)
where the symmetric Hessian matrix u1D43Bu1D45Au1D452u1D44Eu1D460 is given by
u1D43Bu1D45Au1D452u1D44Eu1D460 =
u1D45Auni2211.alt02
u1D456=1
2
u1D70E2u1D456
?
?? [u1D43C(bu1D447u1D456 ru1D456)?ru1D456bu1D447u1D456 ?bu1D456ru1D447u1D456 ] ?(bu1D456 ?ru1D456)
?(bu1D456 ?ru1D456)u1D447 ?bu1D447u1D456 ru1D456
?
?? (2.47b)
and ru1D456 is a set of known unit vectors in the inertial frame, bu1D456 is the set ru1D456 measured in the body
frame, u1D45E is the quaternion that rotates from the inertial frame to the body frame, and u1D70Eu1D456 are
weights. Di?erentiating Equation (2.47a) with respect to u1D45E and adding the constraint u1D45Eu1D447u1D45E = 1 with
a Lagrange multiplier u1D706 results in
(u1D43Bu1D45Au1D452u1D44Eu1D460 +u1D706u1D43C)u1D45E = 0 or u1D43Bu1D45Au1D452u1D44Eu1D460u1D45E = ?u1D706u1D45E.
31
u1D45E is both a quaternion and a normalized eigenvector of u1D43Bu1D45Au1D452u1D44Eu1D460, and ?u1D706 is the corresponding eigen-
value. The optimal solution to the problem occurs when u1D45E corresponds to the most negative ?u1D706.
The extended QUEST ?lter solves a more general quadratic function of the form
minu1D45E u1D43D(u1D45E) = 1
2u1D45E
u1D447u1D43Bu1D45E+u1D454u1D447u1D45E (2.48a)
subject to u1D45Eu1D447u1D45E = 1 (2.48b)
where u1D43B is the cost function?s Hessian matrix and u1D454 is the cost function?s gradient vector at u1D45E = 0.
Solving the problem results in
u1D43D?(u1D45E) = 12u1D45Eu1D447u1D43Bu1D45E+u1D454u1D447u1D45E+ 12u1D706u1D45Eu1D447u1D45E
?u1D43D?(u1D45E)
?u1D45E = 0 = u1D43Bu1D45E+u1D454+u1D706u1D43Cu1D45E
0 = (u1D43B +u1D706u1D43C)u1D45E+u1D454.
Solving for u1D45E results in u1D45E = ?(u1D43B+u1D706u1D43C)?1u1D454, which can be substituted into Equation (2.48b) to yield
u1D454u1D447(u1D43B +u1D706u1D43C)?2u1D454 = 1. (2.49)
Multiplying both sides of Equation (2.49) by (det(u1D43B +u1D706u1D43C))2 results in an eighth-order polynomial
in u1D706 from which the optimal (largest) u1D706 can be found using numerical algorithms.
The problem statement for the extended QUEST algorithm is then
minu1D45E(u1D458),x(u1D458)u1D43D = 1
2
u1D45A(u1D458)uni2211.alt02
u1D456=1
1
u1D70E2u1D456(u1D458){bu1D456(u1D458)?u1D434[u1D45E(u1D458)]ru1D456(u1D458)}
u1D447{bu1D456(u1D458)?u1D434[u1D45E(u1D458)]ru1D456(u1D458)}
+ 12[u1D445u1D464u1D464(u1D458?1)w(u1D458?1)]u1D447[u1D445u1D464u1D464(u1D458?1)w(u1D458?1)]
+ 12{u1D445u1D45Eu1D45E(u1D458?1)[u1D45E(u1D458?1)? ?u1D45E(u1D458?1)]}u1D447{u1D445u1D45Eu1D45E(u1D458?1)[u1D45E(u1D458?1)? ?u1D45E(u1D458?1)]}
+ 12{u1D445u1D465u1D45E(u1D458?1)[u1D45E(u1D458?1)? ?u1D45E(u1D458?1)] +u1D445u1D465u1D465(u1D458?1)[x(u1D458?1)? ?x(u1D458?1)]}u1D447
? {u1D445u1D465u1D45E(u1D458?1)[u1D45E(u1D458?1)? ?u1D45E(u1D458?1)]
+ u1D445u1D465u1D465(u1D458?1)[x(u1D458?1)? ?x(u1D458?1)]} (2.50a)
32
subject to
u1D45E(u1D458) = ?[u1D461(u1D458),u1D461(u1D458?1);u1D45E(u1D458?1),x(u1D458?1),w(u1D458?1)]u1D45E(u1D458?1) (2.50b)
u1D465(u1D458) = u1D453u1D465[u1D461(u1D458),u1D461(u1D458?1);u1D45E(u1D458?1),x(u1D458?1),w(u1D458?1)] (2.50c)
u1D45Eu1D447(u1D458)u1D45E(u1D458) = 1 (2.50d)
whereu1D45E is the attitude quaternion, x is the vector of ?lter states, and w is the process noise vector.
The vectors ?u1D45E(u1D458?1) and ?x(u1D458?1) are the a posteriori estimates of u1D45E and x at sample time u1D461(u1D458?1).
The matrices u1D445u1D464u1D464(u1D458?1), u1D445u1D45Eu1D45E(u1D458?1), u1D445u1D465u1D45E(u1D458?1), and u1D445u1D465u1D465(u1D458?1) are penalizing weights. Equations
(2.50b) and (2.50c) are the ?lter?s dynamic model. ? is the state transition matrix associated with
the quaternion?s kinematic di?erential equation given by Equation (2.45).
The ?rst of the two parts of the extended QUEST algorithm is the propagation step. This
step begins with obtaining a priori estimates of u1D45E(u1D458) and x(u1D458)
?u1D45E(u1D458) = ?[u1D461(u1D458),u1D461(u1D458?1); ?u1D45E(u1D458?1),?x(u1D458?1),0]?u1D45E(u1D458?1) (2.51)
?x(u1D458) = u1D453u1D465[u1D461(u1D458),u1D461(u1D458?1); ?u1D45E(u1D458?1),?x(u1D458?1),0]. (2.52)
The next step in part one is to develop a linearized dynamic model from Equations (2.51)-(2.52)
?
?? ?u1D45E(u1D458)
?x(u1D458)
?
??=
?
?? ?u1D45Eu1D45E(u1D458?1) ?u1D45Eu1D465(u1D458?1)
?u1D465u1D45E(u1D458?1) ?u1D465u1D465(u1D458?1)
?
??
?
?? ?u1D45E(u1D458?1)
?x(u1D458?1)
?
??+
?
?? ?u1D45E(u1D458?1)
?u1D465(u1D458?1)
?
??u1D464(u1D458?1)
where
?q(u1D458) = u1D45E(u1D458)? ?u1D45E(u1D458)
?x(u1D458) = x(u1D458)? ?x(u1D458)
?q(u1D458?1) = u1D45E(u1D458?1)? ?u1D45E(u1D458?1)
?x(u1D458?1) = x(u1D458?1)? ?x(u1D458?1)
33
?u1D45Eu1D45E(u1D458?1) = ?+
uni005B.alt03??
?u1D45E
uni005D.alt03
?u1D45E(u1D458?1)
?u1D45Eu1D465(u1D458?1) =
uni005B.alt03??
?u1D465
uni005D.alt03
?u1D45E(u1D458?1)
?u1D45E(u1D458?1) =
uni005B.alt03??
?u1D464
uni005D.alt03
?u1D45E(u1D458?1)
?u1D465u1D45E(u1D458?1) = ?u1D453u1D465?u1D45E
?u1D465u1D465 = ?u1D453u1D465?u1D465
?u1D465(u1D458?1) = ?u1D453u1D465?u1D464
with all partial fractions evaluated at [u1D45E(u1D458?1),x(u1D458?1),w(u1D458?1)] = [?u1D45E(u1D458?1),?x(u1D458?1),0]. ?q(u1D458?1)
and ?q(u1D458) are not quaternions. The ?nal propagation step is to form an information matrix and
left QR-factorize
u1D444
?
??
??
?
?u1D445u1D45Eu1D45E(u1D458) 0 0
?u1D445u1D465u1D45E(u1D458) ?u1D445u1D465u1D465(u1D458) 0
?u1D445u1D464u1D45E(u1D458) ?u1D445u1D464u1D458(u1D458) ?u1D445u1D464u1D464(u1D458?1)
?
??
??
?
=
?
?? u1D4451 u1D4452 u1D4453
0 0 u1D445u1D464u1D464(u1D458?1)
?
??
where
u1D4451 =
?
?? u1D445u1D45Eu1D45E(u1D458?1) 0
u1D445u1D465u1D45E(u1D458?1) u1D445u1D465u1D465(u1D458?1)
?
??
u1D4452 =
?
?? ?u1D45Eu1D45E(u1D458?1) ?u1D45Eu1D465(u1D458?1)
?u1D465u1D45E(u1D458?1) ?u1D465u1D465(u1D458?1)
?
??
?1
u1D4453 =
?
?? u1D43C 0 ??u1D45E(u1D458?1)
0 u1D43C ??u1D465(u1D458?1)
?
??.
34
Thematrices ?u1D445u1D45Eu1D45E(u1D458), ?u1D445u1D465u1D465(u1D458), and ?u1D445u1D464u1D464(u1D458?1) aresquare, and ?u1D445u1D465u1D465(u1D458) and ?u1D445u1D464u1D464(u1D458?1) arenonsingular.
The propagation step results in the modi?ed cost function
u1D43D = 12
u1D45A(u1D458)uni2211.alt02
u1D456=1
1
u1D70E2u1D456(u1D458){bu1D456(u1D458)?u1D434[u1D45E(u1D458)]ru1D456(u1D458)}
u1D447{bu1D456(u1D458)?u1D434[u1D45E(u1D458)]ru1D456(u1D458)}
+ 12{?u1D445u1D45Eu1D45E(u1D458)[u1D45E(u1D458)? ?u1D45E(u1D458)]}u1D447{?u1D445u1D45Eu1D45E(u1D458)[u1D45E(u1D458)? ?u1D45E(u1D458)]}
? 12{?u1D445u1D465u1D45E(u1D458)[u1D45E(u1D458)? ?u1D45E(u1D458)] + ?u1D445u1D465u1D465(u1D458)[x(u1D458)? ?x(u1D458)]}u1D447
? {?u1D445u1D465u1D45E(u1D458)[u1D45E(u1D458) ? ?u1D45E(u1D458)] + ?u1D445u1D465u1D465(u1D458)[x(u1D458) ? ?x(u1D458)]}. (2.53)
It is assumed that w(u1D458?1) is set to
[w(u1D458?1)]u1D45Cu1D45Du1D461 = ??u1D445?1u1D464u1D464(u1D458?1){?u1D445u1D464u1D45E(u1D458)[u1D45E(u1D458)? ?u1D45E(u1D458)] + ?u1D445u1D464u1D465(u1D458)[x(u1D458)? ?x(u1D458)]}.
The second phase of the algorithm is a measurement update. Equations (2.47a) and (2.47b)
express the squared measurement error cost terms quadratically inu1D45E(u1D458). The resulting measurement
error Hessian matrix u1D43Bu1D45Au1D452u1D44Eu1D460(u1D458) is used to pose the measurement update problem as
minu1D45E(u1D458),x(u1D458)u1D43D = 1
2u1D45E
u1D447(u1D458)u1D43Bu1D45Au1D452u1D44Eu1D460(u1D458)u1D45E(u1D458) + 1
2{
?u1D445u1D45Eu1D45E(u1D458)[u1D45E(u1D458)? ?u1D45E(u1D458)]}u1D447{?u1D445u1D45Eu1D45E(u1D458)[u1D45E(u1D458)? ?u1D45E(u1D458)]}
? 12{?u1D445u1D465u1D45E(u1D458)[u1D45E(u1D458)? ?u1D45E(u1D458)] + ?u1D445u1D465u1D465(u1D458u1D458)[x(u1D458)? ?x(u1D458)]}u1D447
? {?u1D445u1D465u1D45E(u1D458)[u1D45E(u1D458)? ?u1D45E(u1D458)] + ?u1D445u1D465u1D465(u1D458)[x(u1D458) ? ?x(u1D458)]} (2.54a)
subject to
u1D45Eu1D447(u1D458)u1D45E(u1D458) = 1. (2.54b)
To ?nd the optimum ?lter state estimate x(u1D458), set the derivative of u1D43D with respect to u1D465 to zero to
get
[x(u1D458)]u1D45Cu1D45Du1D461 = ?x(u1D458)? ?u1D445?1u1D465u1D465(u1D458) ?u1D445u1D465u1D45E[u1D45E(u1D458)? ?u1D45E(u1D458)]. (2.55)
35
A least squares optimization problem results from substituting Equation (2.55) into the cost func-
tion
minu1D45E(u1D458)u1D43D = 1
2u1D45E
u1D447(u1D458)u1D43Bu1D45Au1D452u1D44Eu1D460(u1D458)u1D45E(u1D458) + 1
2{
?u1D445u1D45Eu1D45E[u1D45E(u1D458)? ?u1D45E(u1D458)]}u1D447
? {?u1D445u1D45Eu1D45E(u1D458)[u1D45E(u1D458)? ?u1D45E(u1D458)]} (2.56a)
subject to
u1D45Eu1D447(u1D458)u1D45E(u1D458) = 1, (2.56b)
which is equivalent to Equation (2.47a) as seen by letting
u1D43B = u1D43Bu1D45Au1D452u1D44Eu1D460 + ?u1D445u1D447u1D45Eu1D45E(u1D458) ?u1D445u1D45Eu1D45E(u1D458) (2.57)
and
u1D454 = ??u1D445u1D447u1D45Eu1D45E(u1D458) ?u1D445u1D45Eu1D45E(u1D458)?u1D45E(u1D458). (2.58)
The measurement update is then complete by solving Equation (2.47a) with Equations (2.57) and
(2.58). The solution is the a posteriori estimate of the attitude quaternion at sample u1D458, ?u1D45E(u1D458). ?u1D45E(u1D458)
is substituted into Equation (2.55) to compute ?x(u1D458).
To apply the algorithm recursively, it is necessary to express the cost function as
u1D43D = 12{u1D445u1D45Eu1D45E(u1D458)[u1D45E(u1D458)? ?u1D45E(u1D458)]}u1D447{u1D445u1D45Eu1D45E(u1D458)[u1D45E(u1D458)? ?u1D45E(u1D458)]}
+ 12{u1D445u1D465u1D45E(u1D458)[u1D45E(u1D458)? ?u1D45E(u1D458)] +u1D445u1D465u1D465[x(u1D458)? ?x(u1D458)]}u1D447
?{u1D445u1D465u1D45E(u1D458)[u1D45E(u1D458)? ?u1D45E(u1D458)] +u1D445u1D465u1D465[x(u1D458)? ?x(u1D458)]}
where u1D445u1D45Eu1D45E(u1D458) is a matrix square root u1D445u1D447u1D45Eu1D45E(u1D458)u1D445u1D45Eu1D45E(u1D458) = [u1D43Bu1D45Au1D452u1D44Eu1D460(u1D458)+ ?u1D445u1D447u1D45Eu1D45E(u1D458)?u1D445u1D45Eu1D45E(u1D458)+u1D706(u1D458)u1D43C], u1D445u1D465u1D45E(u1D458) =
?u1D445u1D465u1D45E(u1D458), andu1D445u1D465u1D465(u1D458) = ?u1D445u1D465u1D465(u1D458). Thematrix squareroot exists since [u1D43Bu1D45Au1D452u1D44Eu1D460(u1D458)+ ?u1D445u1D447u1D45Eu1D45E(u1D458) ?u1D445u1D45Eu1D45E(u1D458)+u1D706(u1D458)u1D43C]
is at least positive semide?nite at the global minimum.
36
2.8 Energy Approach Algorithm
In [9] Mortari develops several solutions to Wahba?s problem through a unique physical inter-
pretation. Mortari poses Wahba?s problem as
u1D70Eu1D464 = 12
u1D45Buni2211.alt02
u1D456=1
u1D6FCu1D456?su1D456 ?u1D434vu1D456?2 = 1?
uni2211.alt02
u1D456
u1D6FCu1D456su1D447u1D456 u1D434vu1D456 (2.59)
where uni2211.alt01u1D456u1D6FCu1D456 = 1 and su1D456 and vu1D456 are body frame and inertial frame unit vectors, respectively. The
weights u1D6FCu1D456 are derived from a measure of the sensors? accuracy. Let
u1D6FDu1D456 = 1E[cos?1(u1D446u1D447
u1D456 su1D456)]
whereu1D446u1D456 is aunit vector of true values andsu1D456 is a unit vector of observed values as de?ned previously.
Then u1D6FCu1D456 = u1D6FDu1D456/uni2211.alt01u1D458u1D6FDu1D458. It is assumed that errors in observed directions are less than 0.5? so that the
anglesu1D717u1D456 between su1D456 andu1D434vu1D456 (where su1D447u1D456 u1D434vu1D456 = cos(u1D717u1D456)) can be considered small (cos(u1D717u1D456) ? 1?u1D7172u1D456/2).
Then Equation (2.59) can be expressed as
u1D70Eu1D464 = 1?
uni2211.alt02
u1D456
u1D6FCu1D456su1D447u1D456 u1D434vu1D456 = 1?
uni2211.alt02
u1D456
u1D6FCu1D456cos(u1D717u1D456)
? 1?
uni2211.alt02
u1D456
u1D6FCu1D456(1?u1D7172u1D456/2) = 12
uni2211.alt02
u1D456
u1D6FCu1D456u1D7172u1D456. (2.60)
The expression in Equation (2.60) is the same as the expression for the energy in u1D45B springs
with displacements u1D717u1D456 and sti?nesses u1D6FCu1D456. Applying the rotation matrix u1D434 to the vu1D456 unit vectors
is like a rigid rotation with the rotation being optimal when the rotated unit vectors are as close
as possible to the sensed vectors as if they were attracted with a torque proportional to u1D6FCu1D456. The
more accurate the sensors are (the greater u1D6FCu1D456), the more strongly u1D434vu1D456 is attracted. Therefore, the
vu1D456 can be thought of as a rigid body free to rotate about the origin and constrained by u1D45B spherical
springs with sti?nesses u1D6FCu1D456 applied between the directions u1D434vu1D456 and su1D456. This formulation is identical
to Wahba?s problem.
37
The minimum value of stored spring energy E is de?ned from Equation (2.60)
E = 12
uni2211.alt02
u1D456
u1D6FCu1D456u1D7172u1D456.
Then
E ?u1D70Eu1D464 = 1?
uni2211.alt02
u1D456
u1D6FCu1D456u1D460u1D447u1D456 u1D434u1D463u1D456 = 1?u1D70Eu1D440.
E is minimized when u1D70Eu1D440 is maximized
u1D70Eu1D440 =
uni2211.alt02
u1D456
u1D6FCu1D456u1D460u1D447u1D456 u1D434u1D463u1D456 = tr(u1D434u1D435u1D447)
u1D435 =
uni2211.alt02
u1D456
u1D6FCu1D456u1D460u1D456u1D463u1D447u1D456 .
Sinceu1D434is a rotation matrix, the cost function must include the constraintu1D434u1D447u1D434= u1D43C where det(u1D434) =
1. The augmented cost function is then
u1D70E?u1D440 = tr(u1D434u1D435u1D447)?tr
uni0028.alt031
2u1D43F(u1D434
u1D447u1D434?u1D43C)
uni0029.alt03
= tr
uni0028.alt03
u1D434u1D435u1D447 ? 12u1D43F(u1D434u1D447u1D434?u1D43C)
uni0029.alt03
where u1D43F is a symmetric matrix of Lagrange multipliers. The maximization of u1D70E?u1D440 leads to one of
the following expressions (depending on the method used):
u1D43F =
?
??
??
u1D435u1D447u1D434
u1D435u1D434u1D447.
(2.61)
Since u1D43F is symmetric, u1D435u1D447u1D434= u1D434u1D447u1D435 and u1D435u1D434u1D447 = u1D434u1D435u1D447. Since u1D434u1D447 = u1D434?1, both expressions of u1D43F lead
to
u1D434u1D435u1D447u1D434 = u1D435, (2.62)
which Mortari calls the R-equation. All solutions of the R-equation, u1D434, are orthonormal.
38
One method to solve the R-equation is an eigenanalysis of the 6?6 matrix
u1D43B =
?
?? 0 u1D435
u1D435u1D447 0
?
??.
This is just one formulation of the H-matrix; other formulations give alternative solutions. Since it
is symmetric, u1D43B has only real eigenvalues. Also, if u1D706u1D456 is an eigenvalue of u1D43B, then so is ?u1D706u1D456. Let uu1D456
be the upper three elements of the u1D456u1D461? eigenvector of u1D43B and du1D456 be the lower three elements. Then
u1D706u1D456 has eigenvector
?
?? uu1D456
du1D456
?
??and ?u1D706
u1D456 has eigenvector
?
?? ?uu1D456
du1D456
?
??. It is assumed that 0 ?u1D706
1 ?u1D7062 ?u1D7063.
Let u1D448 =
uni005B.alt03
u1 u2 u3
uni005D.alt03
, u1D437 =
uni005B.alt03
d1 d2 d3
uni005D.alt03
, and ? = diag(u1D7061,u1D7062,u1D7063). Then
?
?? 0 u1D435
u1D435u1D447 0
?
??
?
?? u1D448
u1D437
?
??=
?
?? u1D448
u1D437
?
???
u1D435u1D437 = u1D448?
u1D435u1D447u1D448 = u1D437?
u1D448?1u1D435u1D437 = ?.
So
u1D435u1D447u1D448 = u1D437u1D448?1u1D435u1D437 ? (u1D448u1D437)?1u1D435u1D447(u1D448u1D437?1) = u1D435.
Thus, u1D434= u1D448u1D437?1 satis?es the R-equation.
It can be shown that 2u1D448u1D447u1D448 = u1D43C and 2u1D437u1D447u1D437 = u1D43C where multiplying by 2 ensures orthonormality.
Since u1D43B is symmetric, its eigenvectors wu1D456 = {uu1D447u1D456 du1D447u1D456 } are orthonormal. So
wu1D447u1D456 wu1D458 = uu1D447u1D456 uu1D458 +du1D447u1D456 du1D458 = u1D6FFu1D456u1D458 u1D6FFu1D456u1D458 =
?
??
??
1, u1D456 = u1D458
0, u1D456?= u1D458.
39
When u1D456 = u1D458, uu1D447u1D456 uu1D456 + du1D447u1D456 du1D456 = 1 for u1D706u1D456, and ?uu1D447u1D456 uu1D456 + du1D447u1D456 du1D456 = 0 for ?u1D706u1D456. Then if
uni005B.alt03
u1D462u1D447u1D456 u1D451u1D447u1D456
uni005D.alt03u1D447
is
an eigenvector, then
uni005B.alt03
u1D462u1D447u1D456 ?u1D451u1D447u1D456
uni005D.alt03u1D447
is an eigenvector as well. Further, these two eigenvectors are
orthogonal to each other. So du1D447u1D456 du1D456 = uu1D447u1D456 uu1D456, which means 2uu1D447u1D456 uu1D456 = 1 and 2du1D447u1D456 du1D456 = 1. So
uu1D447u1D456 uu1D456 = du1D447u1D456 du1D456 = 12.
The matrices u1D448 and u1D437 are orthogonal. Also
u1D435u1D447(u1D435u1D437) = u1D435u1D447(u1D448?) = u1D437?2
u1D435(u1D435u1D447u1D448) = u1D435(u1D437?) = u1D448?2,
which means
(u1D435u1D447u1D435)u1D437 = u1D437?2 (2.63a)
(u1D435u1D435u1D447)u1D448 = u1D448?2. (2.63b)
This implies that
2u1D448u1D447u1D448 = 2u1D437u1D447u1D437 = u1D43C. (2.64)
So
u1D434u1D43C = 2u1D448u1D437u1D447. (2.65)
Note that Equations (2.63a)-(2.63b) represent the singular value decomposition of the u1D435 matrix
(u1D435 = u1D448?u1D437u1D447).
The solution form given by Equation (2.65) has a singularity. It occurs if det(u1D435) = 0, which
always happens when u1D45B = 2 and in some cases when u1D45B > 2. When det(u1D435) = 0, u1D7061 = 0, and
the eigenvectors associated with the eigenvalues ?u1D7061 = 0 cannot be discriminated. This can be
overcome due to the orthogonality of u1D448 and u1D437. Once (u2,d2) and (u3,d3) are found
u1 = ?2u2 ?u3 d1 = ?2d2 ?d3.
40
An alternate solution form that does not have singularities can be found by performing an
eigenanalysis on Equations (2.63a)-(2.63b). The resulting u1D448 and u1D437 matrices are orthonormal, and
the conditions of Equation (2.64) do not hold. The ?rst alternative solution form is
u1D434u1D43Cu1D43C = u1D448u1D437?1 = u1D448u1D437u1D447.
The eigenvectors u1D448 =
uni005B.alt03
u1 u2 u3
uni005D.alt03
andu1D437 =
uni005B.alt03
d1 d2 d3
uni005D.alt03
must refer to the same eigenvalues
sequence 0 ?u1D7061 ?u1D7062 ?u1D7063, and the condition det(u1D448) = det(u1D437) must be satis?ed.
Since u1D435u1D435u1D447 and u1D435u1D447u1D435 have the same eigenvalues, let u1D440 = u1D435u1D435u1D447 (or u1D440 = u1D435u1D447u1D435). Then the
characteristic equation of u1D440 is
u1D7063 +u1D4502u1D7062 +u1D4501u1D706+u1D4500
where
u1D4502 = ?tr(u1D440) = ?u1D45A11 ?u1D45A22 ?u1D45A33
u1D4501 = tr(adj(u1D440)) = u1D45A11u1D45A22 +u1D45A11u1D45A33 +u1D45A22u1D45A33 ?u1D45A212 ?u1D45A213 ?u1D45A223
u1D4500 = ?det(u1D440) = u1D45A11u1D45A22u1D45A13 +2u1D45A12u1D45A13u1D45A23 ?u1D45A222u1D45A213 ?u1D45A11u1D45A223 ?u1D45A33u1D45A212.
u1D440 is symmetric so all of its eigenvalues are real. Setting
u1D45D =
uni221A.alt01
(u1D4502/3)2 ?u1D4501/3
u1D45E = [u1D4501/2?(u1D4502/3)2]u1D4502/3?u1D4500/2
u1D467 = 13 cos?1(u1D45E/u1D45D3),
then ?
??
??
?
u1D7061
u1D7062
u1D7063
?
??
??
?
= ?u1D45D
?
??
??
?
cos(u1D467) +?3sin(u1D467)
cos(u1D467)??3sin(u1D467)
?2cos(u1D467)
?
??
??
?
? u1D45023 .
41
When u1D45B = 2, u1D4500 = ?det(u1D440) = 0. Then (u1D440 ?u1D706u1D456u1D43C)fu1D456 = 0 where fu1D456 is uu1D456 or du1D456 depending on the
choice of u1D440. The row vectors of (u1D440 ?u1D706u1D456u1D43C) must lie on the same plane. The solution fu1D456 can be
computed by normalizing the cross product between two row vectors of the matrix (u1D440 ?u1D706u1D456u1D43C).
It is desired to compute fu1D456 from the cross product having the highest modulus. The 3 ? 3
symmetric matrix (u1D440 ?u1D706u1D456u1D43C) can be written as
u1D440 ?u1D706u1D456u1D43C =
?
??
??
?
mu1D4471
mu1D4472
mu1D4473
?
??
??
?
=
?
??
??
?
u1D45Au1D44E u1D45Au1D465 u1D45Au1D466
u1D45Au1D465 u1D45Au1D44F u1D45Au1D467
u1D45Au1D466 u1D45Au1D467 u1D45Au1D450
?
??
??
?
.
Then the eigenvector u1D453u1D456 is chosen from among
e1 = m2 ?m1 =
uni005B.alt03
u1D45Au1D44Fu1D45Au1D450 ?u1D45A2u1D467 u1D45Au1D466u1D45Au1D467 ?u1D45Au1D465u1D45Au1D450 u1D45Au1D465u1D45Au1D467 ?u1D45Au1D466u1D45Au1D44F
uni005D.alt03u1D447
e2 = m3 ?m1 =
uni005B.alt03
u1D45Au1D466u1D45Au1D467 ?u1D45Au1D465u1D45Au1D450 u1D45Au1D44Eu1D45Au1D450 ?u1D45A3u1D466 u1D45Au1D465u1D45Au1D466 ?u1D45Au1D467u1D45Au1D44E
uni005D.alt03u1D447
e3 = m1 ?m2 =
uni005B.alt03
u1D45Au1D465u1D45Au1D467 ?u1D45Au1D466u1D45Au1D44F u1D45Au1D465u1D45Au1D466 ?u1D45Au1D467u1D45Au1D44E u1D45Au1D44Eu1D45Au1D44F ?u1D45A2u1D465
uni005D.alt03u1D447
which are parallel. The eu1D456 with highest modulus has the maximum element u1D45Du1D458
u1D45D1 = (u1D45Au1D44Fu1D45Au1D450 ?u1D45A2u1D467)2
u1D45D2 = (u1D45Au1D44Eu1D45Au1D450 ?u1D45A2u1D466)2
u1D45D3 = (u1D45Au1D44Eu1D45Au1D44F ?u1D45A2u1D465)2.
Then
fu1D456 = eu1D458/
uni221A.alt02
eu1D447u1D458eu1D458.
By this method the evaluation of the eigenvectors f2 and f3 is su?cient. Then f1 = f2 ?f3.
Mortari gives several other possible solution forms. Other previously known solutions can be
derived from the R-equation as well. Appropriate manipulation of the R-equations gives the ?direct
42
solution? developed by Stuelpnagel as well as a solution based on the singular value decomposition
of u1D435.
Some algorithms become singular when the u1D435 matrix is singular (as is always the case when
u1D45B = 2). Mortari o?ers a general method to eliminate the singularity for the case u1D45B = 2. When
u1D45B = 2, it is possible to add the unit vector s3 = (s1 ? s2)/sin(u1D717u1D460) and the associated vector
v3 = (v1 ?v2)/sin(u1D717u1D463) to the data without a?ecting the computed optimal attitude. So s3 and
v3 can be added to the data to avoid a singularity. New weights u1D6FD1 and u1D6FD2 that replace u1D6FC1 and
u1D6FC2 are such that u1D6FD1/u1D6FD2 = u1D6FC1/u1D6FC2 and u1D6FD1 +u1D6FD2 +u1D6FD3 = 1. Mortari suggests the values u1D6FD1 = 2u1D6FC1/3,
u1D6FD2 = 2u1D6FC2/3, andu1D6FD3 = 1/3 because these values maximize the distance from the singularity provided
by the value of ?det(u1D435)?.
2.9 Singular Value Decomposition Algorithm
In [10] a singular value decomposition approach is used to solve Wahba?s problem by ?nding
the matrix u1D434u1D45Cu1D45Du1D461 that minimizes
u1D43F(u1D434) = 12
u1D45Buni2211.alt02
u1D456=1
u1D44Eu1D456?bu1D456 ?u1D434ru1D456?2 (2.66)
where ru1D456 are inertial frame unit vectors, bu1D456 are the corresponding unit vectors in the body frame,
u1D44Eu1D456 are weights, and u1D45B is the number of observations. The weights are normalized so that
u1D45Buni2211.alt02
u1D456=1
u1D44Eu1D456 = 1. (2.67)
43
Then
u1D43F(u1D434) = 12
u1D45Buni2211.alt02
u1D456=1
u1D44Eu1D456(bu1D456 ?u1D434ru1D456)u1D447(bu1D456 ?u1D434ru1D456)
= 12
u1D45Buni2211.alt02
u1D456=1
u1D44Eu1D456(bu1D447u1D456 ?ru1D447u1D456 u1D434u1D447)(bu1D456 ?u1D434ru1D456)
= 12
u1D45Buni2211.alt02
u1D456=1
u1D44Eu1D456[bu1D447u1D456 ?2bu1D447u1D456 u1D434ru1D456 +ru1D447u1D456 u1D434u1D447u1D434ru1D456]
= 12
u1D45Buni2211.alt02
u1D456=1
u1D44Eu1D456[2?2bu1D447u1D456 u1D434ru1D456]
=
u1D45Buni2211.alt02
u1D456=1
u1D44Eu1D456 ?
u1D45Buni2211.alt02
u1D456=1
bu1D447u1D456 u1D434ru1D456
= 1?
u1D45Buni2211.alt02
u1D456=1
bu1D447u1D456 u1D434ru1D456.
Then the cost function can then be written as
u1D43F(u1D434) = 1?tr(u1D434u1D435u1D447) (2.69)
where
u1D435 =
u1D45Buni2211.alt02
u1D456=1
u1D44Eu1D456bu1D456ru1D447u1D456 . (2.70)
The singular value decomposition of the matrix u1D435 is given by
u1D435 = u1D448u1D446u1D449u1D447
where u1D448 and u1D449 are orthogonal matrices and
u1D446 = diag(u1D4601,u1D4602,u1D4603) u1D4601 ?u1D4602 ?u1D4603 ? 0.
44
De?ne
u1D448+ = u1D448[diag(1,1,det(u1D448))]
u1D449+ = u1D449[diag(1,1,det(u1D449))]
and
u1D44A = u1D448u1D447+u1D434u1D449+ = cos(?)u1D43C +(1?cos(?))eeu1D447 ?sin(?)[u1D452?]
where
[u1D452?] =
?
??
??
?
0 ?u1D4523 u1D4522
u1D4523 0 ?u1D4521
?u1D4522 u1D4521 0
?
??
??
?
.
Thus u1D44A can be represented by a Euler axis/angle rotation with unit vector e and rotation angle
?. De?ne
u1D446? = diag(u1D4601,u1D4602,u1D451u1D4603) (2.71)
u1D451 = det(u1D448)det(u1D449) = ?1. (2.72)
Then u1D435 can be written as
u1D435 = u1D448+u1D446?u1D449u1D447+. (2.73)
Substitute Equation (2.73) into Equation (2.69) to get
u1D43F(u1D434) = 1?tr(u1D446?u1D44A)
= 1?cos(?)tr(u1D446?)?(1?cos(?))[u1D4601u1D45221 +u1D4602u1D45222 +u1D451u1D4603u1D45223]+tr(u1D446?)?tr(u1D446?)
= 1?tr(u1D446?) +(1?cos(?))[u1D4601 +u1D4602 +u1D451u1D4603 ?u1D4601u1D45221 ?u1D4602u1D45222 ?u1D451u1D4603u1D45223]
= 1?tr(u1D446?) +(1?cos(?))[u1D4601(u1D45221 +u1D45222 +u1D45233) +u1D4602 +u1D451u1D4603 ?u1D4601u1D45221 ?u1D4602u1D45222 ?u1D451u1D4603u1D45223]
= 1?tr(u1D446?) +(1+cos(?))[u1D4602 +u1D451u1D4603 +(u1D4601 ?u1D4602)u1D45222 +(u1D4601 ?u1D451u1D4603)u1D45223].
45
u1D43F(u1D434) is minimized when ? = 0, which gives u1D44A = u1D43C so
u1D43F(u1D434u1D45Cu1D45Du1D461) = 1?tr(u1D446?) = 1?u1D4601 ?u1D4602 ?u1D451u1D4603
and
u1D434u1D45Cu1D45Du1D461 = u1D448+u1D449u1D447+ = u1D448[diag(1,1,u1D451)]u1D449u1D447. (2.74)
The minimum is unique unless u1D4602 +u1D451u1D4603 = 0, in which case there is a family of minimizing u1D44A
matrices given by setting e2 = e3 = 0
u1D44A =
?
??
??
?
1 0 0
0 cos(?) sin(?)
0 ?sin(?) cos(?)
?
??
??
?
.
Equation (2.74) is a transformation from the inertial frame to the body frame via two transfor-
mations. The matrix u1D449u1D447+ transforms from the inertial frame to an intermediate from (S-frame),
and u1D448+ transforms from the S-frame to the body frame. The rank of u1D435 determines the solution?s
uniqueness. If u1D435 has rank less than two, the solution is not unique. The sensitivity in the attitude
as a function of the variations u1D6FFru1D456 and u1D6FFbu1D456 is given by
u1D467 =
u1D45Buni2211.alt02
u1D456=1
u1D44Eu1D456[(u1D448u1D447+u1D6FFbu1D456)?(u1D449u1D447+ ru1D456)+(u1D448u1D447+bu1D456)?(u1D449u1D447+u1D6FFru1D456)].
In summary the algorithm is as follows:
1. Compute u1D435 from Equation (2.70).
2. Find the singular value decomposition of u1D435.
3. Compute u1D451 from Equation (2.72).
4. Compute u1D434u1D45Cu1D45Du1D461 from Equation (2.74).
5. Compute u1D43F(u1D434u1D45Cu1D45Du1D461) and any desired statistics.
46
2.10 Fast Optimal Attitude Matrix (FOAM) Algorithm
In [11] Markley develops the FOAM algorithm to solve Wahba?s problem. Markley presents
Wahba?s loss function as detailed in ?2.9. The cost function can be rewritten as
u1D43F(u1D434) = u1D7060 ?tr(u1D434u1D435u1D447)
u1D7060 =
u1D45Buni2211.alt02
u1D456=1
u1D44Eu1D456
u1D435 =
u1D45Buni2211.alt02
u1D456=1
u1D44Eu1D456bu1D456ru1D447u1D456 .
The orthogonal matrix u1D434 that maximizes tr(u1D434u1D435u1D447) minimizes the expression
?u1D434?u1D435?2 = tr[(u1D434?u1D435)(u1D434?u1D435)u1D447] = tr(u1D43C)?2tr(u1D434u1D435u1D447) +?u1D435?2
soWahba?s problemis equivalent to ?ndingtheorthogonal matrixu1D434that isclosest tou1D435in Euclidean
norm.
The matrix u1D435 has the decomposition
u1D435 = u1D448+ diag[u1D4461,u1D4462,u1D4463]u1D449u1D447+
where u1D448+ and u1D449+ are orthogonal matrices required to have positive det
u1D4461 ?u1D4462 ? ?u1D4463?.
The optimal attitude estimate is then
u1D434u1D45Cu1D45Du1D461 = u1D448+u1D449u1D447+.
47
Matrix u1D435 has the properties
?u1D435?2u1D439 = u1D44621 +u1D44622 +u1D44623
det(u1D435) = det(u1D448+ diag[u1D4461,u1D4462,u1D4463]u1D449u1D447+ )
= det(u1D448+)det(diag[u1D4461,u1D4462,u1D4463])det(u1D449u1D447+ )
= (1)(u1D4461u1D4462u1D4463)(1)
= u1D4461u1D4462u1D4463
adj(u1D435u1D447) = u1D448+ diag[u1D4462u1D4463,u1D4463u1D4461,u1D4461u1D4462]u1D449u1D447+
u1D435u1D435u1D447u1D435 = u1D448+ diag[u1D44631,u1D44632,u1D44633]u1D449u1D447+
all of which can be evaluated without computing the SVD. They are used to compute u1D434u1D45Cu1D45Du1D461
u1D434u1D45Cu1D45Du1D461 = [(u1D705+?u1D435?2)u1D435+u1D706adj(u1D435u1D447)?u1D435u1D435u1D447u1D435]/u1D701 (2.75)
u1D705 = u1D4462u1D4463 +u1D4463u1D4461 +u1D4461u1D4462 (2.76)
u1D706 = u1D4461 +u1D4462 +u1D4463 (2.77)
u1D701 = (u1D4462 +u1D4463)(u1D4463 +u1D4461)(u1D4461 +u1D4462). (2.78)
The SVD is not necessary to calculate the coe?cients u1D705, u1D706, and u1D701. The coe?cients u1D705 and u1D701
can be written in terms of u1D706
u1D705 = 12(u1D7062 ??u1D435?2) (2.79)
u1D701 = u1D705u1D706?det(u1D435) (2.80)
so u1D434(u1D706) = u1D434u1D45Cu1D45Du1D461 when u1D706= u1D4461 +u1D4462 +u1D4463. u1D706 can be found with the quartic polynomial
0 = u1D45D(u1D706) = (u1D7062 ??u1D435?2)2 ?8u1D706det(u1D435)?4?adj(u1D435)?2.
48
The roots are all real, and the maximum root is required (it is unique unless u1D4462 +u1D4463 = 0 in which
case the attitude solution is not unique). u1D706 can be found by applying Newton?s method.
The covariance matrix u1D443 for u1D434u1D45Cu1D45Du1D461 can be given as
u1D443 = u1D7060u1D70E2u1D461u1D45Cu1D461(u1D705u1D43C +u1D435u1D435u1D447)/u1D701 (2.81)
where
u1D70E2u1D461u1D45Cu1D461 =
uni0028.alt04 u1D45Buni2211.alt02
u1D456=1
u1D70E?2u1D456
uni0029.alt04?1
. (2.82)
The u1D456u1D461? measurement error vector components are assumed to have a Gaussian distribution with
respect to the actual vector, and phase is assumed to have a uniform distribution with variance
u1D70E2u1D456 per axis. The two choices for u1D7060 are 1 and u1D70E?2u1D461u1D45Cu1D461. The normalized (u1D7060 = 1) form is useful with
?xed-point arithmetic or if measurement weights are arbitrarily assigned. The unnormalized form
(u1D7060 = u1D70E?2u1D461u1D45Cu1D461) is good if weights are computed with measurement variances.
2.11 Alternative Quaternion Attitude Estimation Algorithm
In [12] Markley presents another method for attitude estimation based on Wahba?s problem.
This method also uses SVD the formulation of Wahba?s problem presented in?2.9. The loss function
can be rewritten as
u1D43F(u1D434) = u1D7060 ?tr(u1D434u1D435u1D447) (2.83)
u1D7060 =
u1D45Buni2211.alt02
u1D456=1
u1D44Eu1D456 (2.84)
u1D435 =
u1D45Buni2211.alt02
u1D456=1
u1D44Eu1D456bu1D456ru1D447u1D456 . (2.85)
The assumed orthogonality of A gives
?u1D434?u1D435?2 = tr[(u1D434?u1D435)(u1D434?u1D435)u1D447] = 3?2tr(u1D434u1D435u1D447)+?u1D435?2.
This norm is minimized by the same matrix that maximizes tr(u1D434u1D435u1D447).
49
Let
u1D701(u1D706,u1D435) = 12u1D706(u1D7062 ??u1D435?2)?det(u1D435).
Then the optimal matrix u1D434u1D45Cu1D45Du1D461 is given by
u1D434(u1D706) =
uni005B.alt031
2(u1D706
2 +?u1D435?2)u1D435+u1D706adj(u1D435u1D447)?u1D435u1D435u1D447u1D435
uni005D.alt03
/u1D701(u1D706,u1D435) (2.86)
where u1D706 is the largest root of the quartic equation resulting from
u1D706 = tr[u1D434(u1D706)u1D435u1D447].
Several methods exist to solve for u1D434u1D45Cu1D45Du1D461 that involve the computation of u1D706. u1D706 should be close to the
value of u1D7060 from Equation (2.84) since
u1D43F(u1D434u1D45Cu1D45Du1D461) = u1D7060 ?u1D706? 0,
and with small measurement errors the loss function should be close to zero. The resulting attitude
estimate is
u1D4340 = u1D440/u1D701(u1D7060,u1D435) (2.87)
u1D440 = 12(u1D70620 +?u1D435?2)u1D435+u1D7060 adj(u1D435u1D447)?u1D435u1D435u1D447u1D435. (2.88)
The estimate u1D4340 is only approximately orthogonal.
A variant of Shepperds? algorithm can be used to obtain the normalized attitude quaternion u1D45E
from u1D4340 to construct an orthogonal attitude matrix u1D434u1D452u1D460u1D461
u1D434u1D452u1D460u1D461 =
?
??
??
?
u1D45E24 +u1D45E21 ?u1D45E22 ?u1D45E23 2(u1D45E1u1D45E2 +u1D45E3u1D45E4) 2(u1D45E1u1D45E3 ?u1D45E2u1D45E4)
2(u1D45E2u1D45E1 ?u1D45E3u1D45E4) u1D45E24 ?u1D45E21 +u1D45E22 ?u1D45E23 2(u1D45E2u1D45E3 +u1D45E1u1D45E4)
2(u1D45E3u1D45E1 +u1D45E2u1D45E4) 2(u1D45E3u1D45E2 ?u1D45E1u1D45E4) u1D45E24 ?u1D45E21 ?u1D45E22 +u1D45E23
?
??
??
?
. (2.89)
50
If u1D456, u1D457, u1D458 is a cyclic permutation of 1, 2, 3, the quaternion components obey the relations
4u1D701(u1D7060,u1D435)u1D45E2u1D456 ? u1D701(u1D7060,u1D435) +u1D440u1D456u1D456 ?u1D440u1D457u1D457 ?u1D440u1D458u1D458 = u1D463u1D456
4u1D701(u1D7060,u1D435)u1D45Eu1D456u1D45Eu1D457 ? u1D440u1D456u1D457 +u1D440u1D457u1D456 = u1D463u1D457
4u1D701(u1D7060,u1D435)u1D45Eu1D456u1D45Eu1D458 ? u1D440u1D456u1D458 +u1D440u1D458u1D456 = u1D463u1D458
4u1D701(u1D7060,u1D435)u1D45Eu1D456u1D45E4 ? u1D440u1D457u1D458 ?u1D440u1D458u1D457 = u1D4634 = u1D464u1D456
4u1D701(u1D7060,u1D435)u1D45Eu1D457u1D45E4 ? u1D440u1D458u1D456 ?u1D440u1D456u1D458 = u1D464u1D457
4u1D701(u1D7060,u1D435)u1D45Eu1D458u1D45E4 ? u1D440u1D456u1D457 ?u1D440u1D457u1D456 = u1D464u1D458
4u1D701(u1D7060,u1D435)u1D45E24 ? u1D701(u1D7060,u1D435) +u1D440u1D456u1D456 +u1D440u1D457u1D457 +u1D440u1D458u1D458 = u1D4644.
Let u1D456 correspond to the index of the largest diagonal element of u1D440, and de?ne the quaternion
components for u1D459 = 1,...,4 by
u1D45Eu1D459 = u1D463u1D459/?u1D463? for u1D440u1D457u1D457 +u1D440u1D458u1D458 < 0
or
u1D45Eu1D459 = u1D464u1D459/?u1D464? for u1D440u1D457u1D457 +u1D440u1D458u1D458 ? 0
where ?u1D463? and ?u1D464? are the Euclidean norms of the u1D463 and u1D464.
2.12 Euler-q Algorithm
In [13] Mortari develops the Euler-q algorithm. It is desired to ?nd the attitude matrix u1D434 that
rotates unit vectors vu1D456 from the inertial frame to corresponding unit vectors su1D456 in the body frame.
u1D434 can be expressed in terms of the Euler axis e, a unit 3-vector about which a rotation takes place,
and the the Euler angle u1D719, the angle of rotation about the Euler axis, as follows
u1D434 = u1D43Ccos(u1D719) +(1?cos(u1D719))eeu1D447 ? ?u1D452sin(u1D719)
51
where ?u1D452 is the 3?3 skew-symmetric, cross-product matrix
?u1D452 =
?
??
??
?
0 ?u1D4523 u1D4522
u1D4523 0 ?u1D4521
?u1D4522 u1D4521 0
?
??
??
?
.
Let u1D6FDu1D456 represent the accuracy of the u1D456u1D461? sensor. Then the sensor relative precision can be de?ned as
u1D6FCu1D456 = 1/
uni0028.alt04
u1D6FDu1D456
u1D45Buni2211.alt02
u1D458=1
(1/u1D6FDu1D458)
uni0029.alt04
.
Wahba?s problem is to ?nd the 3?3 matrix u1D434 that minimizes the loss function
u1D43Fu1D44A(u1D434) = 12
u1D45Buni2211.alt02
u1D456=1
u1D6FCu1D456?su1D456 ?u1D434vu1D456?2 = 1?
u1D45Buni2211.alt02
u1D456=1
u1D6FCu1D456su1D447u1D456 u1D434vu1D456
or equivalently maximizes the gain function
u1D43Au1D44A(u1D434) = 1?u1D43Fu1D44A(u1D434) =
u1D45Buni2211.alt02
u1D456=1
u1D6FCu1D456su1D447u1D456 u1D434vu1D456 = tr(u1D434u1D435u1D447)
u1D435 =
u1D45Buni2211.alt02
u1D456=1
u1D6FCu1D456su1D456vu1D447u1D456 .
The gain function can be rewritten as
u1D43Au1D44A(e,u1D719) = cos(u1D719)tr(u1D435) +(1?cos(u1D719))eu1D447u1D435e+sin(u1D719)zu1D447e
z =
u1D45Buni2211.alt02
u1D456=1
u1D6FCu1D456su1D456 ?vu1D456.
52
De?ne a loss function
u1D43Fu1D440(e) =
u1D45Buni2211.alt02
u1D456=1
u1D709u1D456u1D6FF2u1D456 =
u1D45Buni2211.alt02
u1D456=1
u1D709u1D456eu1D447du1D456du1D447u1D456 e = eu1D447u1D43Be
du1D456 = vu1D456 ?su1D456/?vu1D456 ?su1D456?
u1D6FFu1D456 = eu1D447du1D456 = du1D447u1D456 e
u1D43B = u1D43Bu1D447 =
u1D45Buni2211.alt02
u1D456=1
u1D709u1D456du1D456du1D447u1D456
where u1D709u1D456 are relative weights. The worst case for the du1D456 direction deviation vector occurs at the
angle u1D6FD?u1D456 when Su1D456 is displaced from the measured su1D456 by the angle u1D6FDu1D456 and the spherical triangle is
right. The value of u1D6FD?u1D456 is obtained from
sin(u1D714u1D456)sin(u1D6FD?u1D456) = sin(u1D6FDu1D456)
where u1D714u1D456 is the angle between the su1D456 and vu1D456 directions. The relative weights u1D709u1D456 are then derived
from u1D6FD?u1D456 in the same way that u1D6FCu1D456 are obtained from u1D6FDu1D456
u1D709u1D456 = 1/
uni0028.alt04
u1D6FD?u1D456
u1D45Buni2211.alt02
u1D458=1
(1/u1D6FD?u1D458)
uni0029.alt04
.
The augmented cost function is de?ned as
u1D43F?u1D440(e) = eu1D447u1D43Be?u1D706(eu1D447e?1)
which leads to
u1D43Be = u1D706e.
Thus, the Euler axis is the eigenvector of the u1D43B matrix associated with the eigenvalue u1D706 so
u1D43Fu1D440(e) = u1D706= eu1D447u1D43Be.
53
Since u1D43Fu1D440(e) has to be a minimum, the unknown eigenvalue has to be the smallest u1D706
u1D43Beu1D45Cu1D45Du1D461 = u1D706u1D45Au1D456u1D45Beu1D45Cu1D45Du1D461.
u1D706u1D45Au1D456u1D45B can be found using the characteristic equation of u1D43B
0 = u1D7063 +u1D44Eu1D7062 +u1D44Fu1D706+u1D450
u1D44E= ?tr(u1D43B)
u1D44F= tr(adj(u1D43B))
u1D450 = ?det(u1D43B).
Since u1D43B is symmetric positive semide?nite, its eigenvalues are real and nonnegative. Let 0 ?u1D7061 ?
u1D7062 ?u1D7063, and de?ne the variables
u1D45D2 = (u1D44E/3)2 ?(u1D44F/3)
u1D45E = [(u1D44F/2)?(u1D44E/3)2](u1D44E/3)?(u1D450/2)
u1D464 = 13 cos?1(u1D45E/u1D45D3)
which leads to
u1D7061 = ?u1D45D(?3sin(u1D464) +cos(u1D464))?u1D44E/3
u1D7062 = u1D45D(?3sin(u1D464)?cos(u1D464))?u1D44E/3
u1D7063 = 2u1D45Dcos(u1D464)?u1D44E/3.
The calculation of eu1D45Cu1D45Du1D461 proceeds as follows
(u1D43B ?u1D706u1D45Au1D456u1D45Bu1D43C)eu1D45Cu1D45Du1D461 = u1D440eu1D45Cu1D45Du1D461 =
?
??
??
?
u1D45Au1D4471
u1D45Au1D4472
u1D45Au1D4473
?
??
??
?
eu1D45Cu1D45Du1D461 =
?
??
??
?
u1D45Au1D44E u1D45Au1D465 u1D45Au1D466
u1D45Au1D465 u1D45Au1D44F u1D45Au1D467
u1D45Au1D466 u1D45Au1D467 u1D45Au1D450
?
??
??
?
eu1D45Cu1D45Du1D461 = 0.
54
The direction of the optimal Euler axis can be found from the cross product of any two row vectors
of u1D440
e1 = m2 ?m3
e2 = m3 ?m1
e3 = m1 ?m2.
The most accurate eu1D456 is the one with the greatest modulus, which is determined by ?nding the
greatest u1D45Du1D456
u1D45D1 = ?u1D45Au1D44Fu1D45Au1D450?u1D45A2u1D467?
u1D45D2 = ?u1D45Au1D44Eu1D45Au1D450 ?u1D45A2u1D466?
u1D45D3 = ?u1D45Au1D44Eu1D45Au1D44F ?u1D45A2u1D465?.
Let u1D45Du1D458 = max(u1D45D1,u1D45D2,u1D45D3). Then Euler axis with the greatest modulus is
eu1D45Cu1D45Du1D461 = eu1D458/?eu1D458?.
The optimal Euler angle can be derived from
sin(u1D719u1D45Cu1D45Du1D461) = (1/u1D437)zu1D447eu1D45Cu1D45Du1D461
cos(u1D719u1D45Cu1D45Du1D461) = (1/u1D437)(tr(u1D435)?eu1D447u1D45Cu1D45Du1D461u1D435eu1D45Cu1D45Du1D461)
u1D4372 = (zu1D447eu1D45Cu1D45Du1D461)2 +(tr(u1D435)?eu1D447u1D45Cu1D45Du1D461u1D435eu1D45Cu1D45Du1D461)2.
The matrix u1D440 is singular or ill-conditioned under one of three conditions: a) if the row vectors
ofu1D440 are parallel (colinear), b) the Euler axis e and all observed vectors are approximately coplanar
(one vector is nearly linearly dependent on the other two), or c) the row vectors of u1D440 become 0.
For case (a) there is no solution, but for cases (b) and (c) the method of sequential rotations (MSR)
can be applied. MSR states that if an optimal attitude matrix exists for the u1D45B unit vector pairs
55
(su1D456,vu1D456), then there exists u1D45B unit vector pairs (su1D456,wu1D456) that imply an optimal attitude matrix u1D439.
The vectors su1D456 and wu1D456 are related by any rotation matrix u1D445: wu1D456 = u1D445su1D456. u1D434 is related to u1D439 by
u1D439 =
uni005B.alt03
f1 f2 f3
uni005D.alt03
= u1D434u1D445u1D447. If the (su1D456,vu1D456) data set implies a singularity, the set (su1D456,wu1D456) would
not necessarily imply a singularity. So MSR evaluates the attitude of u1D439 by using the rotated unit
vector wu1D456 and then computes u1D434 as u1D434 = u1D439u1D445. By using one of the following rotation matrices
u1D4451 =
?
??
??
?
1 0 0
0 ?1 0
0 0 ?1
?
??
??
?
u1D4452 =
?
??
??
?
?1 0 0
0 1 0
0 0 ?1
?
??
??
?
u1D4453 =
?
??
??
?
?1 0 0
0 ?1 0
0 0 1
?
??
??
?
,
no new computation is required, only sign changes.
2.13 ESOQ Algorithm
In [14] Mortari presents the commonly used EStimator of the Optimal Quaternion (ESOQ)
algorithm. The ESOQ algorithm is a singularity-free algorithm that ?nds a closed form expression
for u1D706u1D45Au1D44Eu1D465 and u1D45Eu1D45Cu1D45Du1D461 of the q-method equation (Equation (2.94) below).
Let su1D456 be unit vectors measured in the body frame, vu1D456 be the actual vectors in the inertial
frame, and u1D434 be the matrix that rotates vu1D456 to the body frame. Let u1D6FDu1D456 be the precision of the u1D456u1D461?
sensor such that the angle between the true and observed direction is smaller than u1D6FDu1D456. The sensor
relative precision is de?ned as
u1D6FCu1D456 = 1/
uni0028.alt04
u1D6FDu1D456
u1D45Buni2211.alt02
u1D458=1
(1/u1D6FDu1D458)
uni0029.alt04
. (2.90)
56
Wahba?s problem can be solved by computation of the 3?3 rotation matrix u1D434 that minimizes
u1D43Fu1D44A(u1D434) = 12
u1D45Buni2211.alt02
u1D456=1
u1D6FCu1D456?su1D456 ?u1D434vu1D456?2 = 1?
u1D45Buni2211.alt02
u1D456=1
u1D6FCu1D456su1D447u1D456 u1D434vu1D456 (2.91)
or equivalently maximizes the gain function
u1D43Au1D44A(u1D434) = 1?u1D43Fu1D44A(u1D434) =
u1D45Buni2211.alt02
u1D456=1
u1D6FCu1D456su1D447u1D456 u1D434vu1D456 = tr(u1D434u1D435u1D447) (2.92)
u1D435 =
u1D45Buni2211.alt02
u1D456=1
u1D6FCu1D456su1D456vu1D447u1D456 . (2.93)
The q-method solution equation is
u1D43Eu1D45Eu1D45Cu1D45Du1D461 = u1D706u1D45Au1D44Eu1D465u1D45Eu1D45Cu1D45Du1D461 (2.94)
meaning u1D45Eu1D45Cu1D45Du1D461 is the eigenvector of the symmetric matrix u1D43E
u1D43E =
?
?? u1D446 z
zu1D447 u1D461
?
??=
?
?? u1D435+u1D435u1D447 ?u1D43Ctr(u1D435) z
zu1D447 tr(u1D435)
?
?? (2.95)
z =
u1D45Buni2211.alt02
u1D456=1
u1D6FCu1D456su1D456 ?vu1D456. (2.96)
associated with its maximum eigenvalue assuming that the vectors su1D456 and vu1D456 are normalized. The
characteristic equation of the matrix u1D43E can be written as
0 = u1D7064 +u1D44Eu1D7063 +u1D44Fu1D7062 +u1D450u1D706+u1D451 (2.97)
u1D44E = tr(u1D43E) = 0 (2.98)
u1D44F = ?2(tr(u1D435))2 +tr(adj(u1D435+u1D435u1D447))?zu1D447z (2.99)
u1D450 = ?tr(adj(u1D43E)) (2.100)
u1D451 = det(u1D43E). (2.101)
57
The third order auxiliary equation of the characteristic equation is
u1D4623 ?u1D44Fu1D4622 ?4u1D451u1D462+4u1D44Fu1D451?u1D4502 = 0 (2.102)
which has the solution
u1D4621 = 2?u1D45Dcos
uni0028.alt031
3 cos
?1(u1D45E/u1D45D3/2)
uni0029.alt03
+u1D44F/3 (2.103)
u1D45D = (u1D44F/3)2 +4u1D451/3 (2.104)
u1D45E = (u1D44F/3)3 ?4u1D451u1D44F/3 +u1D4502/2. (2.105)
Then
u1D7061 = 12
uni0028.alt02
?u1D4541 ?
uni221A.alt01
?u1D4621 ?u1D44F+u1D4542
uni0029.alt02
(2.106)
u1D7062 = 12
uni0028.alt02
?u1D4541 +
uni221A.alt01
?u1D4621 ?u1D44F+u1D4542
uni0029.alt02
(2.107)
u1D7063 = 12
uni0028.alt02
u1D4541 ?
uni221A.alt01
?u1D4621 ?u1D44F?u1D4542
uni0029.alt02
(2.108)
u1D7064 = 12
uni0028.alt02
u1D4541 +
uni221A.alt01
?u1D4621 ?u1D44F?u1D4542
uni0029.alt02
(2.109)
u1D4541 =
uni221A.alt01
u1D4621 ?u1D44F (2.110)
u1D4542 = 2
uni221A.alt02
u1D46221 ?4u1D451 (2.111)
where
?1 ?u1D7061 ?u1D7062 ?u1D7063 ?u1D7064 = u1D706u1D45Au1D44Eu1D465 ? 1. (2.112)
If u1D45B = 2
u1D7064 = ?u1D7061 = (u1D4543 +u1D4544)/2 (2.113)
u1D7063 = ?u1D7062 = (u1D4543 ?u1D4544)/2 (2.114)
u1D4543 =
uni221A.alt02
2
?
u1D451?u1D44F (2.115)
u1D4544 =
uni221A.alt02
?2
?
u1D451?u1D44F. (2.116)
58
The q-method solution equation is equivalent to
(u1D43E?u1D706u1D45Au1D44Eu1D465u1D43C)u1D45Eu1D45Cu1D45Du1D461 = u1D43Bu1D45Eu1D45Cu1D45Du1D461 = 0 (2.117)
which meansu1D45Eu1D45Cu1D45Du1D461 is perpendicular to each row vector hu1D456 of symmetricu1D43B. Four di?erent and parallel
cross-products u1D45Eu1D458 can be evaluated using the 4-D cross-product
u1D45Eu1D458(u1D456) = (?1)u1D458+u1D456det(u1D43Bu1D458u1D456)
where u1D456 = 1,...,4 and the 3?3 submatrices u1D43Bu1D458u1D456 are obtained from matrix u1D43B by deleting the u1D458u1D461?
row and the u1D456u1D461? column. The optimal u1D45E is then
u1D45Eu1D45Cu1D45Du1D461 = u1D45Eu1D43A/?u1D45Eu1D43A?
where u1D43A is the index associated with the element u1D45Eu1D458(u1D458) with the largest magnitude.
2.14 ESOQ2 Algorithm
In[15] Mortari presents theESOQ2algorithm, an Euler angle variation of the ESOQalgorithm.
The same procedure as outlined in ?2.13 in Equations (2.90)-(2.117) is used to develop the ESOQ2
algorithm.
The quaternion solution to the q-method equation (Equation (2.94)) can be expressed in terms
of an Euler axis and angle
u1D45E = {eu1D447 sin(u1D719/2) cos(u1D719/2)}u1D447.
It can be shown that the equation for determining the optimal Euler axis is
[(u1D461?u1D706u1D45Au1D44Eu1D465)(u1D446?u1D706u1D45Au1D44Eu1D465u1D43C)?zzu1D447]e = u1D440e = 0
59
where
u1D440 =
?
??
??
?
mu1D4471
mu1D4472
mu1D4473
?
??
??
?
=
?
??
??
?
u1D45Au1D44E u1D45Au1D465 u1D45Au1D466
u1D45Au1D465 u1D45Au1D44F u1D45Au1D467
u1D45Au1D466 u1D45Au1D467 u1D45Au1D450
?
??
??
?
.
All row vectors of u1D440 are perpendicular to e. The optimal Euler axis e can be found by taking the
cross-product of two rows of u1D440. This fails in two cases:
1. When the rows of u1D440 become zero which occurs if u1D719? 0 (the Euler angle singularity).
2. When the rows of u1D440 become parallel (an unresolvable case).
The three choices for e are
e1 = m2 ?m3
e2 = m3 ?m1
e3 = m1 ?m2
where the eu1D456?s di?er only in modulus. The most accurate eu1D456 is the one with the greatest modulus,
which is determined by ?nding the greatest u1D45Du1D456
u1D45D1 = ?u1D45Au1D44Fu1D45Au1D450?u1D45A2u1D467?
u1D45D2 = ?u1D45Au1D44Eu1D45Au1D450 ?u1D45A2u1D466?
u1D45D3 = ?u1D45Au1D44Eu1D45Au1D44F ?u1D45A2u1D465?.
Let u1D45Du1D458 have the largest value. Then the most reliable Euler axis is
eu1D45Cu1D45Du1D461 = eu1D458/?eu1D458?.
60
The optimal Euler angle is then derived from
u1D44B(u1D458) = ?sin(u1D719/2) (2.118)
u1D44C(u1D458) = ?cos(u1D719/2) (2.119)
where ? is a proportional constant and u1D458 identi?es the element of vector u1D44B with the greatest
modulus. The optimal quaternion can then be computed from e and u1D719. To avoid the singularity
that occurs when u1D719? 0, the MSR can be applied.
2.15 A Slightly Sub-Optimal Algorithm
In [16] an algorithm for the solution of Wahba?s problem that requires no iterations and little
computation but results in a slightly nonorthogonal matrix solution is presented. Wahba?s loss
function can be written as
u1D43F(u1D434) = 12
u1D45Buni2211.alt02
u1D457=1
?vu1D457 ?u1D434uu1D457?2
where uu1D457 are unit vectors in the inertial frame of u1D45B observations and vu1D457 are the corresponding unit
vectors in the body frame. The loss function can be generalized by using an u1D45B ?u1D45B symmetric
positive-de?nite weight matrix u1D44A
u1D43F(u1D434) = 12 tr(u1D44A(u1D434u1D448 ?u1D449)u1D447(u1D434u1D448 ?u1D449))
u1D448 = [u1,u2,...,uu1D45B]
u1D449 = [v1,u1D4632,...,vu1D45B].
The loss function can be written as
u1D43F(u1D434) = 12 tr(u1D44A(u1D448u1D447u1D448 +u1D449u1D447u1D449))?tr(u1D434u1D435u1D447)
u1D435 = u1D449u1D44Au1D448u1D447.
61
The loss function is minimized when tr(u1D434u1D435u1D447) is maximized. This occurs when u1D434 is the orthogonal
matrix that is closest to u1D435 in the Euclidean (or Frobenius) norm. The loss function can be further
generalized to
u1D43F(u1D434) = 12 tr(u1D44A(u1D434u1D448 ?u1D449)u1D447u1D44D(u1D434u1D448 ?u1D449))
where u1D44D is a 3?3 symmetric positive-de?nite weight matrix.
Let u1D4340 be an extremum of the loss function and u1D716 an arbitrary variation in the direction of an
arbitrary non-zero matrix u1D43B. Then
u1D43F(u1D4340 +u1D716u1D43B) = 12?u1D44D1/2(u1D4340u1D448 ?u1D449)u1D44A1/2?2u1D439 +u1D716tr(u1D43Bu1D447u1D44D(u1D4340u1D448 ?u1D449)u1D44Au1D448u1D447] (2.120)
+ 12u1D7162?u1D44D1/2u1D43Bu1D448u1D44A1/2?2u1D439. (2.121)
In order for Equation (2.121) have a solution, the following condition must hold
(u1D4340u1D448 ?u1D449)u1D44Au1D448u1D447 = 0.
If u1D448 is full rank
u1D4340 = u1D449u1D44Au1D448u1D447(u1D448u1D44Au1D448u1D447)?1 = u1D435(u1D448u1D44Au1D448u1D447)?1.
The solution provides an unbiased estimate of u1D434 under some assumptions:
1. Error-free reference vectors
2. Additive random measurement errors
vu1D457 = u1D434u1D461u1D45Fu1D462u1D452uu1D457 +nu1D457 ?u1D457
u1D449 = u1D434u1D461u1D45Fu1D462u1D452u1D448 +u1D441
3. E[N] = 0.
62
The attitude estimate is given by
u1D4340 = (u1D434u1D461u1D45Fu1D462u1D452u1D448 +u1D441)u1D44Au1D448u1D447(u1D448u1D44Au1D448u1D447)?1
= u1D434u1D461u1D45Fu1D462u1D452 +u1D6FFu1D434
u1D6FFu1D434 = u1D441u1D44Au1D448u1D447(u1D448u1D44Au1D448u1D447)?1.
An estimate of the deviation of u1D4340 from u1D434u1D461u1D45Fu1D462u1D452 is
u1D443 = u1D438[(u1D6FFu1D434)u1D447u1D6FFu1D434]
= (u1D448u1D44Au1D448u1D447)?1u1D448u1D44Au1D445u1D44Au1D448u1D447(u1D448u1D44Au1D448u1D447)?1
u1D445 = u1D438[u1D441u1D447u1D441].
If we choose u1D44A = u1D445?1, then
u1D443 = (u1D448u1D44Au1D448u1D447)?1 = (u1D448u1D445?1u1D448u1D447)?1.
If {nu1D457} is a white sequence, then u1D445 and u1D44A are diagonal matrices.
This method requires a minimum of three observations (u1D45B = 3). In the unconstrained problem,
a third observation can be added (the cross-product v1?v2 as a measurement of u1?uu1D460) to make
u1D448 rank 3.
2.16 Conclusions
Many algorithms have been developed to ?nd solutions to Wahba?s problem. While many
mathematical solutions are o?ered to solve the problem, Davenport is the ?rst to present a solution
in the form of a practical algorithm. Davenport?s algorithm develops what has become known as
Davenport?s equation but does not provide a quick way to solve the equation. However, Davenport?s
equation is the foundation of many other algorithms. The QUEST algorithm is one such algorithm.
Faster than Davenport?s method, the QUEST algorithm has the major disadvantage of using only
single measurements. Past information is not taken into account so anomalous data can cause
63
bad estimates. To overcome these limitations, variations of the QUEST algorithm have been
developed to make the algorithm more like Kalman ?ltering. These algorithms cannot compute
sensor biases and accounting for process noise proves to be too computationally expensive. The
REQUEST algorithm makes the QUEST algorithm recursive, but REQUEST only works if high
quality sensors are used to get attitude measurements and does not estimate biases. The extended
QUEST algorithm is developed to estimate biases but is slow. The energy approach algorithm
o?ers a new formulation of Wahba?s problem that is analogous to a physical system; therefore, it
is easy to understand. The algorithm has the drawbacks of assuming small errors in measurements
and of often encountering singularities when ?nding a solution. The SVD algorithm, which requires
taking a computationally expensive SVD, provides the basis for the FOAM algorithm, which does
not require computing an SVD. The FOAM algorithm and most algorithms developed after the
FOAM algorithm provide the same solution, but the speeds at which the solution is derived di?er.
The ESOQ algorithm provides the same optimal solution as the FOAM algorithm but is faster
and guaranteed singularity free. The ESOQ2 algorithm di?ers from the ESOQ algorithm in that it
provides a solution in terms of Euler angles rather than a quaternion. The sub-optimal algorithm
is computationally inexpensive and the fastest of all the algorithms presented; however, it has the
drawback of computing a rotation matrix that is nonorthogonal.
The ESOQ algorithm is best suited for estimating the states of a ballistic rocket in the early
stages of ?ight. This algorithm is fast while providing an orthogonal rotation matrix estimate. As
much accuracy in an attitude estimate as possible as quickly as possible is necessary for the system
presented in this work. The ESOQ algorithm also accounts for past measurements rather than
being a single-point algorithm like other Wahba?s problem algorithms. No assumptions are made
about the quality of sensors used, which is essential since MEMS sensors are not as accurate as
more conventional sensors.
64
Chapter 3
Magnetometer Navigation
Magnetometers are a key component of solving Wahba?s problem as discussed in Chapter 2.
In this chapter we present information relevant to the use of a magnetometer for state estimation
of a vehicle. In ?3.1 we develop equations to model the earth?s magnetic ?eld and present a
model that implements those equations. Next, in ?3.2 we present a model for a magnetometer, a
device that measures magnetic ?elds. We also present several possible error sources that corrupt
magnetometer measurements. In ?3.3 we present methods to calibrate a magnetometer in order to
minimize measurement errors. In ?3.4 we present an algorithm that estimates angular rates based
on magnetometer measurements. Finally, in ?3.5 we evaluate the presented algorithms as they
pertain to the problem in this work.
3.1 Modeling Earth?s Magnetic Field
To simulate a magnetometer?s behavior, a model of the earth?s magnetic ?eld is necessary. In
[17] Roithmayr develops equations that are necessary to construct such a model.
3.1.1 Equation Development
An in?nite series of spherical harmonics can be used to describe magnetic ?elds in general.
Then at a point u1D444 above the earth?s surface, the magnetic ?eld vector B is given by
Bu1D45B,u1D45A = u1D43Eu1D45B,u1D45Au1D44E
u1D45B+2
u1D445u1D45B+u1D45A+1
uni007B.alt02u1D454u1D45B,u1D45Au1D436u1D45A +?u1D45B,u1D45Au1D446u1D45A
u1D445 [(u1D460u1D706u1D434u1D45B,u1D45A+1 +(u1D45B+u1D45A+1)u1D434u1D45B,u1D45A)?r?u1D434u1D45B,u1D45A+1?e3]
? u1D45Au1D434u1D45B,u1D45A[(u1D454u1D45B,u1D45Au1D436u1D45A?1 +?u1D45B,u1D45Au1D446u1D45A?1)?e1 +(?u1D45B,u1D45Au1D436u1D45A?1 ?u1D454u1D45B,u1D45Au1D446u1D45A?1)?e2]
uni007D.alt02
(3.1)
where
B =
?uni2211.alt02
u1D45B=1
u1D45Buni2211.alt02
u1D45A=0
Bu1D45B,u1D45A. (3.2)
65
In Equation (3.1) u1D44E is the earth?s average radius (6371 km), u1D445 is the magnitude of the position
vector R from the earth?s center to point u1D444, ?r is a unit vector in the direction of R, and u1D454u1D45B,u1D45A and
?u1D45B,u1D45A are degree u1D45B, order u1D45A Gauss coe?cients. ?e1, ?e2, and ?e3 are unit vectors of an Earth-?xed
coordinate system. The coe?cients u1D43Eu1D45B,u1D45A are determined recursively from either
u1D43Eu1D45B,u1D45A =
uni0028.alt03u1D45B?u1D45A
u1D45B+u1D45A
uni0029.alt031/2
u1D43Eu1D45B?1,u1D45A u1D45A = 1,...,?, u1D45B?u1D45A+1
or
u1D43Eu1D45B,u1D45A = [(u1D45B+u1D45A)(u1D45B?u1D45A+1)]?1/2u1D43Eu1D45B,u1D45A?1 u1D45A = 2,...,?, u1D45B?u1D45A
whereu1D43Eu1D45B,0 = 1 wheneveru1D45A = 0 andu1D43E1,1 = 1. u1D434u1D45B,u1D45A andu1D434u1D45B,u1D45A+1 are degreeu1D45BLegendre polynomials
(see ?B.2) with orders u1D45A and u1D45A+1, respectively. When u1D45B = u1D45A, the polynomial is given by
u1D434u1D45B,u1D45B = (1)(3)(5)...(2u1D45B?1) u1D45B = 1,...,?
= (2u1D45B?1)u1D434u1D45B?1,u1D45B?1 u1D45B = 2,...,?
and u1D4340,0 = 1. In general,
u1D434u1D45B,u1D45A(u1D462) = 1u1D45B?u1D45A[(2u1D45B?1)u1D462u1D434u1D45B?1,u1D45A(u1D462)?(u1D45B+u1D45A?1)u1D434u1D45B?2,u1D45A(u1D462)] u1D45A = 0,...,?, u1D45B? (u1D45A+1).
Arguments of the Legendre polynomials are u1D460u1D706 = sin(u1D706) = ?r??e3 where u1D706 is the geographic latitude.
The variables u1D446u1D45A and u1D436u1D45A are de?ned recursively as
u1D446u1D45A = u1D4461u1D436u1D45A?1 +u1D4361u1D446u1D45A?1
u1D436u1D45A = u1D4361u1D436u1D45A?1 ?u1D4461u1D446u1D45A?1
66
where
u1D4460 = 0
u1D4461 = R??e2
u1D4360 = 1
u1D4361 = R??e1.
the earth?s magnetic ?eld can be modeled as a tilted dipole as long as the point u1D444 is not near
the magnetic poles. For the dipole model
u1D4341,0(u1D460u1D706) = u1D460u1D706
u1D4341,1(u1D460u1D706) = 1
u1D4341,2(u1D460u1D706) = 0
u1D43E1,1 = u1D43E1,0 = 1.
Then
B1,0 =
uni0028.alt02u1D44E
u1D445
uni0029.alt023
u1D4541,0(3u1D460u1D706?r??e3)
B1,1 =
uni0028.alt02u1D44E
u1D445
uni0029.alt023
[3(u1D4541,1?r??e1 +?1,1?r??e2)?r?(u1D4541,1?e1 +?1,1?e2)].
So
B1,0 +B1,1 =
uni0028.alt02u1D44E
u1D445
uni0029.alt023
[3(?u1D45F?M)?u1D45F?M]
where the terrestrial dipole moment M is de?ned as
M = u1D4541,1?e1 +?1,1?e2 +u1D4541,0?e3.
67
3.1.2 Model Implementation
Two implementations of models of the earth?s magnetic ?eld are commonly used: the Inter-
national Geomagnetic Reference Field (IGRF) [18] and the World Magnetic Model (WMM) (an
eighth-order model) [19] which is used by the U.S. DoD and NATO. Due to the changes in the be-
havior of the earth?s magnetic ?eld such as from secular variation (slow changes in time of the main
magnetic ?eld), these models are updated every ?ve years (last updated in 2005). Our development
is based on the WMM, and further discussion is concentrated solely on the WMM.
the earth?s magnetic ?eld is generated mainly by three sources:
1. The main ?eld generated by the earth?s outer core (Bu1D45A).
2. The crustal ?eld generated by the earth?s crust and upper mantle (Bu1D450).
3. Electrical currents in the upper magnetosphere and atmosphere which induce electrical cur-
rents in the ground and oceans (Bu1D451).
The total magnetic ?eld can then be written as
B(r,u1D461) = Bu1D45A(r,u1D461) +Bu1D450(r) +Bu1D451(r,u1D461)
where r is a position vector and u1D461 is time. To model the earth?s magnetic ?eld the WMM uses data
gathered from the Danish ?rsted and German CHAMP satellites, which have good global coverage
and low noise, as well as data from ground observatories.
The WMM only takes into account the contributions of Bu1D45A. This introduces errors into the
model since Bu1D450 and Bu1D451 are ignored. The WMM also has other error sources. A magnetic sensor will
not match the WMM in all locations on the earth; it may observe spatial and temporal anomalies.
Spatial anomalies on land are caused by mountain ranges, ore deposits, geological faults, trains,
railroad tracks, power lines, and other such conditions. Disturbances in the atmosphere from ionic
activity from space will also cause variations in the magnetic ?eld.
Seven quantities describe the geomagnetic ?eld vector B. These quantities are northerly inten-
sity u1D44B, easterly intensity u1D44C, vertical intensity (positive downwards)u1D44D, total intensity u1D439, horizontal
68
intensity u1D43B, inclination (or dip) u1D43C which is the angle between the horizontal plane and the ?eld
vector (measured positive downwards), and declination (or magnetic variation) u1D437 which is the hor-
izontal angle between true north and the ?eld vector (measured positive eastwards). These values
are calculated as follows
u1D43B =
uni221A.alt01
u1D44B2 +u1D44C2
u1D439 =
uni221A.alt01
u1D43B2 +u1D44D2
u1D437 = arctan(u1D44C,u1D44B)
u1D43C = arctan(u1D44D,u1D43B)
u1D43Au1D449 = u1D437?u1D706 for u1D719> 55?
u1D43Au1D449 = u1D437+u1D706 for u1D719<?55?
where u1D43Au1D449 is grid variation, u1D706 is longitude, and u1D719 is latitude. The WMM algorithm is used to
compute the magnetic ?eld for a given location and time (?,u1D719,u1D706,u1D461) (see ?D.2 for a more complete
discussion of these variables), where ? is geodetic altitude, u1D719 and u1D706 are geodetic latitude and
longitude, and u1D461 is the time given in decimal year. The algorithm proceeds as follows:
1. Convert ellipsoidal geodetic coordinates (?,u1D719,u1D706) to spherical geocentric coordinates (u1D45F,u1D719?,u1D706).
2. Determine the Gauss coe?cients of degree u1D45B and order u1D45A for the desired time.
3. Compute the ?eld vector components in geocentric coordinates.
4. Convert the ?eld vector components to the geodetic reference frame.
5. Compute u1D43B, u1D439, u1D437, u1D43C, and u1D43Au1D449.
For a more complete discussion of the algorithm details, see [19].
69
3.2 Magnetometer Modeling
A magnetometer is a device that measures magnetic ?eld strength and direction and in nav-
igation can be used to obtain heading information. A summary of some basics concerning mag-
netometers as well as a new o?-line calibration technique are given by McClendon in his master?s
thesis [20]. A brief overview of some points in the thesis follows.
In order to properly use magnetometer data, relationships among reference frames must be
established. An inertial frame is an Earth-?xed set of axes with the u1D465u1D466-plane tangent to the earth?s
surface. The u1D465-axis points to the earth?s magnetic north. (Declination angles can be used to
reconcile magnetic north and true north.) The u1D466-axis points east, and the u1D467-axis points downward.
The heading frame is de?ned such that it shares its u1D467-axis with the inertial frame u1D467-axis. The
heading frame?s u1D465- and u1D466-axes are coplanar with the inertial frame?s u1D465- and u1D466-axes but may be
rotated by some angle u1D713, the heading. A body frame is de?ned such that the u1D465-axis points out
the front of the body, the u1D466-axis to the right, and the u1D467-axis down. This reference frame rotates
along with the body of the rocket or aircraft. One frame can be rotated into the other using the
appropriate set of Givens rotations (rotation matrices or quaternions).
Measurements of the earth?s magnetic ?eld can be used to determine vehicle heading
u1D713 = ?arctan
uni0028.alt04
u1D435?u1D466
u1D435?u1D465
uni0029.alt04
(3.3)
where u1D435?u1D466 and u1D435?u1D465 are the u1D466- and u1D465-axis components, respectively, of the magnetic ?eld measured
in the heading reference frame. Then body frame measurements of magnetic ?eld intensity ?Bu1D44F =
[ ?u1D435u1D44Fu1D465, ?u1D435u1D44Fu1D466, ?u1D435u1D44Fu1D467]u1D447 can be rotated to the heading frame via
B?u1D465u1D466u1D467 =
?
??
??
?
cos(u1D719) sin(u1D719)sin(u1D703) sin(u1D719)cos(u1D703)
0 cos(u1D703) ?sin(u1D703)
?sin(u1D719) sin(u1D703)cos(u1D719) cos(u1D703)cos(u1D719)
?
??
??
?
?Bu1D44F.
the earth?s magnetic ?eld has both vertical and horizontal components with magnitudes that vary
with latitude. The horizontal component, which points north, is parallel to the earth?s surface. The
70
vertical component does not a?ect heading calculations unless the magnetometer has a nonzero roll
or pitch angle, in which case coupling between the axes occurs.
Several error sources must beaccounted for in order to get an accurate measurement of heading.
The ?rst commonly encountered error source is soft iron error. This error occurs when materials
(typically ferromagnetic metals such as iron, nickel, or cobalt) near the magnetometer warp existing
magnetic ?elds. Soft ironerror isafunction of themagnetometer?s orientation. Anothererror source
is hard iron error. Hard iron error is interference from objects near the magnetometer that emit
their own magnetic ?eld. This error can be corrected through calibration. Yet another error source
is misalignment of the magnetometer?s measurement axes with body axes of the vehicle on which
the magnetometer is mounted. A ?nal error source is scale factor errors in the sensor. These errors
can be used to produce the following model for a magnetometer:
?Bu1D44F = u1D43Eu1D460u1D43Eu1D460u1D456u1D43Eu1D45A(Bu1D44F +u1D6FFBu1D44F). (3.4)
Bu1D44F is a 3?1 vector of the true magnetic ?eld in the body frame, u1D43Eu1D45A is a 3?3 matrix representing
measurement transformation from mounting error, u1D43Eu1D460u1D456 is a 3? 3 matrix that factors in soft iron
error, u1D43Eu1D460 is a 3?3 matrix of sensor scale factor errors, and u1D6FFBu1D44F is a vector of measurement biases
resulting from hard iron error sources. A more complete model includes the addition of u1D702u1D440, a
zero-mean Gaussian noise term with variance u1D70Eu1D440:
?Bu1D44F = u1D43Eu1D460u1D43Eu1D460u1D456u1D43Eu1D45A(Bu1D44F +u1D6FFBu1D44F)+u1D702u1D440. (3.5)
3.3 Magnetometer Calibration
In this section we present some methods for the calibration of a magnetometer. In ?3.3.1 we
review methods presented by Alonso and Shuster [21] to estimate magnetometer bias when attitude
is unknown. Next, we summarize the TWOSTEP algorithm [22] in ?3.3.2. Finally, in ?3.3.3 we
present a recursive least squares method developed by Hodgart and Tortora [23].
71
3.3.1 Attitude Independent Bias Estimation
In [21] Alonso and Shuster present several algorithms that estimate magnetometer bias without
knowledge of attitude. The magnetometer model used is
Bu1D458 = u1D434u1D458Hu1D458 +b+u1D716u1D458, u1D458 = 1,...,u1D441 (3.6)
(Bu1D458 is the magnetic ?eld measurement at time u1D461u1D458, Hu1D458 is value of the geomagnetic ?eld in an Earth-
?xed reference frame, u1D434u1D458 is the magnetometer?s attitude with respect to the earth-?xed frame, b is
the magnetometer?s constant bias, and u1D716u1D458 is measurement noise). Scalar measurements and scalar
noise values can be used to estimate bias. De?ne
u1D467u1D458 = ?Bu1D458?2 ??Hu1D458?2
u1D708u1D458 = 2(Bu1D458 ?b)?u1D716u1D458 ??u1D716u1D458?2.
Then u1D434u1D458Hu1D458 = Bu1D458 ?b?u1D716u1D43E. Since u1D434u1D458 is a rotation matrix, it does not change the magnitude of
Hu1D458. Thus,
?Hu1D458?2 = ?u1D434u1D458Hu1D458?2 = (Bu1D458 ?b?u1D716u1D458)u1D447(Bu1D458 ?b?u1D716u1D458)
= ?Bu1D458?2 ?2Bu1D458 ?b+?b?2 +?u1D716u1D458?2 ?2Bu1D458u1D716u1D458 +2bu1D716u1D458
= ?Bu1D458?2 ?2Bu1D458 ?b+?b?2 +?u1D716u1D458?2 ?2(Bu1D458 ?b)?u1D716u1D458
= ?Bu1D458?2 ?2Bu1D458 ?b+?b?2 ?u1D708u1D458.
So
u1D467u1D458 = ?Bu1D458?2 ??Bu1D458?2 +2Bu1D458 ?b??b?2 +u1D708u1D458
= 2Bu1D458 ?b??b?2 +u1D708u1D458, u1D458 = 1,...,u1D441. (3.7)
72
It is assumed that u1D716u1D458 ? u1D4A9(0,?u1D458). Then u1D708u1D458 ? u1D4A9(u1D707u1D458,u1D70E2u1D458) where
u1D707u1D458 = u1D438{u1D708u1D458} = ?tr(?u1D458)
u1D70E2u1D458 = u1D438{u1D7082u1D458}?u1D7072u1D458 = 4(Bu1D458 ?b)u1D447?u1D458(Bu1D458 ?b) +2tr(?2u1D458).
This model is the basis for the following bias estimators presented.
Scoring
The negative-log-likelihood function for magnetometer bias as modeled by Equation (3.6) is
u1D43D(b) = 12
u1D441uni2211.alt02
u1D458=1
uni005B.alt03 1
u1D70E2u1D458(u1D467u1D458 ?2Bu1D458 ?b+?b?
2 ?u1D707u1D458)2 +logu1D70E2
u1D458 +log2u1D70B
uni005D.alt03
,
which is quartic in b. The maximum-likelihood estimate b? for b satis?es
?u1D43D
?b
vextendsinglevextendsingle
vextendsinglevextendsingle
b?
= 0.
One solution procedure is to use scoring (taking the partial derivative a log-likelihood function with
respect to some parameter), which for this case is the Newton-Raphson approximation
bu1D441u1D4450 = 0
bu1D441u1D445u1D456+1 = bu1D441u1D445u1D456 ?
uni005B.alt03 ?2u1D43D
?b?bu1D447 (b
u1D441u1D445
u1D456 )
uni005D.alt03?1 ?u1D43D
?b(b
u1D441u1D445
u1D456 )
where
?2u1D43D
?b?bu1D447 =
u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D458
uni005B.alt014(B
u1D458 ?b)(Bu1D458 ?b)u1D447 +2(u1D467u1D458 ?2Bu1D458 ?b+?b?2 ?u1D707u1D458)u1D43C3?3
uni005D.alt01
?u1D43D
?b = ?
u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D458(u1D467u1D458 ?2Bu1D458 ?b+?b?
2 ?u1D707u1D458)2(Bu1D458 ?b).
73
Estimates of bias by scoring can also be obtained with the Gauss-Newton algorithm. The
algorithm proceeds as follows
bu1D43Au1D4410 = 0
bu1D43Au1D441u1D456+1 = bu1D43Au1D441u1D456 ?u1D439?1u1D44Fu1D44F ?u1D43D?buni0028.alt01bu1D43Au1D441u1D456 uni0029.alt01
where
u1D439u1D44Fu1D44F =
u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D4584(Bu1D458 ?b)(Bu1D458 ?b)
u1D447.
The estimate error covariance is then given byu1D443u1D44Fu1D44F = u1D439?1u1D44Fu1D44F . For quadratic functions both theNewton-
Raphson and Gauss-Newton methods tend to converge rapidly once the estimate is su?ciently close
to the minima. However, for quartic functions the possibility exists to become stuck in local minima
rather than converging to the global minimum.
Fixed-Point Method
The ?xed-point method proceeds as follows. First, de?ne
u1D43A =
u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D458[4Bu1D458B
u1D447
u1D458 +2(u1D467u1D458 ?u1D707u1D458)u1D43C3?3]
a =
u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D458(u1D467u1D458 ?u1D707u1D458)2Bu1D458
f(b) =
u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D458[4(Bu1D458 ?b)b+2?b?
2(Bu1D458 ?b)].
Then the optimal solution is given by
b? = u1D43A?1[a+f(b?)].
74
This can be solved iteratively via
bu1D439u1D4430 = 0
bu1D439u1D443u1D456+1 = u1D43A?1[a+f(bu1D439u1D443u1D456 )].
Convergence is usually poor.
Davenport?s Approximation
Davenport?s method gives an estimate of the bias, but the estimate is inconsistent. That is, the
mean-squared error of the estimates does not tend to zero as the number of observations increases.
The method begins with the approximate cost function
u1D43Du1D437(b) = 12
u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D458(u1D467u1D458 ?2Bu1D458 ?b+u1D706
2 ?u1D707u1D458)2
where u1D706 is a constant. Then
b?u1D437 = U+u1D7062V
U =
uni005B.alt04 u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D4584Bu1D458B
u1D447
u1D458
uni005D.alt04?1uni005B.alt04 u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D458(u1D467u1D458 ?u1D707u1D458)2Bu1D458
uni005D.alt04
V =
uni005B.alt04 u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D4584Bu1D458B
u1D447
u1D458
uni005D.alt04?1uni005B.alt04 u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D4582Bu1D458
uni005D.alt04
.
Then choose u1D706:
u1D7062 = ?u1D44F?
?u1D44F2 ?4u1D44Eu1D450
2u1D44E
u1D44E = ?V?2
u1D44F = 2U?V?1
u1D450 = ?U?2.
Usually, the ? is chosen to be minus.
75
Acu?na?s Algorithm
Acu?na?s algorithm does not rely on a ?eld model. The derived measurement u1D467u1D458,u1D459 and e?ective
measurement error ?u1D458,u1D459 are de?ned as
u1D467u1D458,u1D459 = ?Bu1D458?2 ??Bu1D459?2
= 2(Bu1D458 ?Bu1D459)?b+?u1D458,u1D459
?u1D458,u1D459 = ?Hu1D458?2 ??Hu1D459?2 +u1D708u1D458 ?u1D708u1D459
where u1D458 and u1D459 are two di?erent time indices. The cost function is then de?ned as
u1D43D(b) = 12
?uni2211.alt02
u1D458,u1D459
[u1D467u1D458,u1D459 ?2(Bu1D458 ?Bu1D459)?b]2
where the prime indicates that no index occurs more than once. In other words, u1D45B individual
magnetometer measurements can yield no more thanu1D45B/2 e?ective measurements. Then the optimal
solution is
b?u1D434 =
?
?
?uni2211.alt02
u1D458,u1D459
4(Bu1D458 ?Bu1D459)(Bu1D458 ?Bu1D459)u1D447
?
?
?1 ?uni2211.alt02
u1D458,u1D459
2(Bu1D458 ?Bu1D459)u1D467u1D458,u1D459
= b+?bu1D434
where
?bu1D434 =
?
?
?uni2211.alt02
u1D458,u1D459
4(Bu1D458 ?Bu1D459)(Bu1D458 ?Bu1D459)u1D447
?
?
?1 ?uni2211.alt02
u1D458,u1D459
2(Bu1D458 ?Bu1D459)?u1D458,u1D459.
The estimation error contains both random and systematic terms. The dominate error source
determines how to best construct u1D467u1D458,u1D459. For a more detailed discussion of Acu?na?s algorithm, see
[21].
76
3.3.2 TWOSTEP Algorithm Calibration
The TWOSTEP algorithm is a robust algorithm that is commonly used to estimate magne-
tometer bias. Full calibration had not been achieved with the TWOSTEP algorithm until recently.
In [22] Alonso and Shuster outline how the TWOSTEP algorithm estimates a magnetometer?s
biases and extend the algorithm for full magnetometer calibration.
Bias Estimate
It is desired to ?nd an estimate of b where the measurement model is given by Equation (3.6).
Then b can be estimated in the maximum likelihood sense by minimizing the cost function
u1D43D(b) = 12
u1D441uni2211.alt02
u1D458=1
uni005B.alt03 1
u1D70E2u1D458(u1D467u1D458 ?2Bu1D458 ?b+?b?
2 ?u1D707u1D458)2 +log(u1D70E2
u1D458) +log(2u1D70B)
uni005D.alt03
.
where u1D467u1D458 is de?ned by Equation (3.7), and the Gaussian noise u1D708u1D458 ?u1D4A9(u1D707u1D458,u1D70E2u1D458) is
u1D708u1D458 = 2(Bu1D458 ?b?u1D716u1D458 ??u1D716u1D458?2).
The minimization involves a quartic (or fourth degree polynomial) in b so multiple minima and
maxima exist. To ?nd the global minimum reliably, a good starting estimate of b is needed.
The centering approximation gives a reliable starting estimate of b. For a sequence of variables
u1D44Bu1D458, u1D458 = 1,...,u1D441, center values are de?ned as
u1D44B =u1D70E2
u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D458u1D44Bu1D458
1
u1D70E2 =
u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D458,
and centered values are de?ned as
?u1D44Bu1D458 = u1D44Bu1D458 ?u1D44B, u1D458 = 1,...,u1D441.
77
Then the cost function can be written in terms of centered and center values as
u1D43D(b) = ?u1D43D(b) +u1D43D(b)
where
?u1D43D(b) = 1
2
u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D458(?u1D467u1D458 ?2
?Bu1D458 ?b? ?u1D707u1D458)2 +terms independent of b
and
u1D43D(b) = 12 1u1D70E2(u1D467?2B?b+?b?2 ?u1D707)2 +terms independent of b.
The TWOSTEP algorithm is then used on the cost function to estimate b. First, the b that
minimizes ?u1D43D(b) is found. The minimum, guaranteed since the function is quadratic in b, is equal
to
?b? = ?u1D443u1D44Fu1D44F
u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D458(?u1D467u1D458 ? ?u1D707u1D458)2
?Bu1D458
where
?u1D443u1D44Fu1D44F = ?u1D439?1u1D44Fu1D44F =
uni005B.alt04 u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D4584
?Bu1D458?Bu1D447u1D458
uni005D.alt04?1
.
The algorithm then uses ?b? as an initial guess for the Gauss-Newton algorithm, which is applied
to the full negative-log-likelihood cost function
u1D43D(b) = 12(b? ?b?)u1D447 ?u1D443?1u1D44Fu1D44F (b? ?b?) + 12u1D70E2(u1D467?2B?b+?b?2 ?u1D707)2 +terms independent of b,
to ?nd the ?nal estimate of b. Then the iteration to ?nd the bias estimate is
bu1D43Au1D4411 = ?b?
bu1D43Au1D441u1D456+1 = bu1D43Au1D441u1D456 ?
uni005B.alt03
?u1D443?1u1D44Fu1D44F + 4
u1D70E2(B?b
u1D43Au1D441
u1D456 )(B?b
u1D43Au1D441
u1D456 )
u1D447
uni005D.alt03?1 ?u1D43D
?b(b
u1D43Au1D441
u1D456 ).
78
Full Calibration
Alonso and Shuster [22] give a more general model for the magnetic ?eld measured by a
magnetometer that includes scale factors and nonorthogonality of measurement axes
Bu1D458 = u1D447?1[u1D434u1D458Hu1D458 +b? +u1D716?u1D458] u1D458 = 1,...,u1D441.
Through a polar decomposition u1D447 can be written as u1D447 = u1D444(u1D43C + u1D437) where u1D444 is an orthogonal
matrix and u1D437 is symmetric. Then b = u1D444u1D447b? and u1D716u1D458 = u1D444u1D447u1D716?u1D458. When u1D434u1D458 is unknown, u1D444 cannot be
estimated, and ?u1D43Bu1D458?2 becomes
?u1D43Bu1D458?2 = ?(u1D43C +u1D437)Bu1D458 ?b?u1D716u1D458?2.
Therefore, only b and u1D437 can be estimated.
Rewriting the magnetometer model as
Bu1D458 = (u1D43C +u1D437)?1(u1D444u1D447u1D434u1D458Hu1D458 +b+u1D716u1D458) u1D458 = 1,...,u1D441
yields
u1D467u1D458 = ?Bu1D458?2 ??Hu1D458?2
= ?Bu1D447u1D458(2u1D437+u1D4372)Bu1D458 +2Bu1D447u1D458(u1D43C +u1D437)b??b?2 +u1D708u1D458
u1D708u1D458 = 2[(u1D43C +u1D437)Bu1D458 ?b]?u1D716u1D458 ??u1D716u1D458?2.
To estimate u1D437 and b de?ne
u1D438 = 2u1D437+u1D4372
c = (u1D43C +u1D437)b.
79
Then
u1D467u1D458 = ?Bu1D458u1D438Bu1D458 +2Bu1D447u1D458c??b(c,E)?2 +u1D708u1D458
where
E =
uni005B.alt03
u1D43811 u1D43822 u1D43833 u1D43812 u1D43813 u1D43823
uni005D.alt03
.
After writing Bu1D447u1D458u1D438Bu1D458 = u1D43Eu1D458E where
u1D43Eu1D458 =
uni005B.alt03
u1D43521,u1D458 u1D43522,u1D458 u1D43523,u1D458 2u1D4351,u1D458u1D4352,u1D458 2u1D4351,u1D458u1D4353,u1D458 2u1D4352,u1D458u1D4353,u1D458
uni005D.alt03
,
u1D467u1D458 can be written as
u1D467u1D458 = ?u1D43Eu1D458E+2Bu1D447u1D458c??b(c,E)?2 +u1D708u1D458
= u1D43Fu1D458?? ??b(??)?2 +u1D708u1D458
where
u1D43Fu1D458 =
uni005B.alt03
2Bu1D447u1D458 ... ?u1D43Eu1D458
uni005D.alt03
?? =
?
?? c
E
?
??.
Then the center and centered values can be calculated
u1D43Fu1D458 = u1D70E2
u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D458u1D43Fu1D458,
?u1D43Fu1D458 = u1D43Fu1D458 ?u1D43F.
The center and centered measurements are
?u1D467u1D458 = ?u1D43Fu1D458 ??? + ?u1D708u1D458, u1D458 = 1,...,u1D441
u1D467u1D458 = u1D43F?? ??b(??)?2 +u1D708,
and
?b(??)?2 = cu1D447(u1D43C +u1D437)?2c = cu1D447(u1D43C +u1D438)?1c.
80
The cost function is then
?u1D43D(??) = 1
2
u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D458(?u1D467u1D458 ?
?u1D43Fu1D458?? ? ?u1D707u1D458)2 +terms independent of ??
where
???? = ?u1D443?,?
u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D458(?u1D467u1D458 ? ?u1D707u1D458)
?u1D43Fu1D447u1D458
?u1D443?1?,? =
u1D441uni2211.alt02
u1D458=1
1
u1D70E2u1D458
?u1D43Fu1D458?u1D43Fu1D447u1D458.
The center cost function is
u1D43D(??) = 12u1D70E2(u1D467?u1D43F?? +?b(??)?2 ?u1D707)2,
and the complete cost function is
u1D43D(??) = 12(?? ????)u1D447 ?u1D443?,?(?? ????)+u1D43D(??).
The Gauss-Newton algorithm is applied to ?nd c? and E? where
?
?u1D450u1D45A?b(?
?)?2 = 2((u1D43C +u1D438)?1c)u1D45A
?
?u1D438u1D45A,u1D45B?b(?
?)?2 = ?(2?u1D6FFu1D45A,u1D45B)((u1D43C +u1D438)?1c)u1D45A((u1D43C +u1D438)?1c)u1D45B.
((u1D43C+u1D438)?1c)u1D45A denotes the u1D45Au1D461? element of ((u1D43C+u1D438)?1c), and u1D6FFu1D45A,u1D45B is the Kronecker delta (u1D6FFu1D45A,u1D45B = 1
when u1D45A = u1D45B and 0 otherwise). Then u1D438? may be written as u1D438? = u1D448u1D446u1D448u1D447 where u1D448 is an orthogonal
matrix and u1D446 is diagonal. Then de?ne a matrix u1D44A such that u1D446 = 2u1D44A +u1D44A2 where u1D44A is diagonal
so that
u1D464u1D457 = ?1+uni221A.alt011+u1D460u1D457 u1D457 = 1,2,3.
81
In general, u1D460u1D457 is much less than one so a solution exists. Then the desired parameters u1D437 and b may
be estimated as
u1D437? = u1D448u1D44Au1D448u1D447
b? = (u1D43C +u1D437?)?1c?.
3.3.3 Recursive Least Squares Method
In [23] Hodgart and Tortora develop a recursive least squares method to calibrate a magne-
tometer that is onboard a satellite orbiting Earth. The method makes some assumptions. First,
the satellite points roughly in a zenith-nadir direction. Second, the satellite slowly spins about the
zenith-nadir axis about which it is symmetric.
The magnetometer measurement is modeled as
?
??
??
?
u1D435u1D465
u1D435u1D466
u1D435u1D467
?
??
??
?
=
?
??
??
?
u1D45011 u1D45012 u1D45013 u1D45014
u1D45021 u1D45022 u1D45023 u1D45024
u1D45031 u1D45032 u1D45033 u1D45034
?
??
??
?
?
??
??
??
??
u1D435u1D465u1D45B
u1D435u1D466u1D45B
u1D435u1D467u1D45B
u1D435u1D45Cu1D45B
?
??
??
??
??
or
B = u1D436Bu1D45B
where u1D435 is an optimal estimate of the magnetic ?eld, Bu1D45B =
uni005B.alt03
u1D435u1D465u1D45B u1D435u1D466u1D45B u1D435u1D467u1D45B
uni005D.alt03u1D447
is a vector of
nominal values obtained a priori through on-ground calibration, and u1D435u1D45Cu1D45B is an arbitrary o?set.
Coe?cients u1D45011, u1D45022, u1D45033, u1D45014, u1D45024, and u1D45034 are magnitude dependent, and the authors cite batch
methods to ?nd these values. The remaining coe?cients are direction dependent, and a recursive
least squares algorithm is used to obtain those values.
Before the algorithm can be performed, several preliminary steps must be taken. Data must
be collected to obtain a ?le of magnetic data for each axis of the magnetometer. That data is then
converted to rough nominal values u1D435(u1D458)u1D465u1D45B, u1D435(u1D458)u1D466u1D45B , andu1D435(u1D458)u1D467u1D45B for 0 ?u1D458 ?u1D4580 by some nominal calibration.
Next, the latitude and longitude of each sample must be calculated to obtain a reference ?eld u1D435(u1D458)u1D4650 ,
82
u1D435(u1D458)u1D4660 , and u1D435(u1D458)u1D4670 . Finally, a reference magnetic angle u1D6FCu1D458 must be computed at each reference point
where
u1D6FCu1D458 = 4arctan
uni0028.alt03
u1D435u1D458u1D4670,
uni221A.alt02
(u1D435(u1D458)u1D4650 )2 +(u1D435(u1D458)u1D4660 )2
uni0029.alt03
.
Once the preliminary steps have been performed, the algorithm to ?nd the direction-dependent
coe?cients can be used. It is assumed that the magnitude-dependent coe?cients have already been
calculated. The method proceeds as described in Algorithm 3.3.1.
Initialize ?x0 =
uni005B.alt02
u1D450(0)12 u1D450(0)13 u1D450(0)21 u1D450(0)23 u1D450(0)31 u1D450032
uni005D.alt02u1D447
;
Initialize u1D4430 = diag(uni005B.alt01 u1D44312 u1D44313 u1D44321 u1D44323 u1D44331 u1D44332 uni005D.alt01) and u1D445 which are found empirically;
for u1D458 = 1,...,u1D4580 do
Form u1D436(u1D458?1)2 =
?
?? 1 u1D450
(u1D458?1)
12 u1D450
(u1D458?1)
13 0
u1D450(u1D458?1)21 1 u1D450(u1D458?1)23 0
u1D450(u1D458?1)31 u1D450(u1D458?1)32 1 0
?
??;
B(u1D458)2 = u1D436(u1D458?1)2 B(u1D458)1 where B(u1D458)1 is nominal calibration data;
u1D6FD(u1D458) = 4arctan(u1D435(u1D458)u1D4672 ,
uni221A.alt02
(u1D435(u1D458)u1D4652 )2 +(u1D435(u1D458)u1D4662 )2);
gu1D447u1D458 =
uni005B.alt02 u1D435
u1D4652u1D435u1D4662u1D435u1D4672
u1D435u1D4612?u1D4352?2
u1D435u1D4652u1D435u1D4671u1D435u1D4672
u1D435u1D4612?u1D4352?2
u1D435u1D4651u1D435u1D4662u1D435u1D4672
u1D435u1D4612?u1D4352?2
u1D435u1D4662u1D435u1D4671u1D435u1D4672
u1D435u1D4612?u1D4352?2 ?
u1D435u1D4612u1D435u1D4651
?u1D4352?2 ?
u1D435u1D4612u1D435u1D4661
?u1D4352?2
uni005D.alt02
where
u1D435(u1D458)u1D4612 =
uni221A.alt02
(u1D435(u1D458)u1D4652 )2 +(u1D435(u1D458)u1D4662 )2 and gu1D458 is a linearized observation vector.;
u1D43Eu1D458 = u1D443u1D458?1gu1D458(gu1D447
u1D458u1D443u1D458?1gu1D458+u1D4642u1D445)
where 0 <u1D464 ? 1 is a forgetting factor.;
?xu1D458 = u1D43Eu1D458(u1D6FCu1D458 ?u1D6FDu1D458) + ?xu1D458?1 where u1D6FCu1D458 is known before hand.;
u1D443u1D458 = (u1D43C ?u1D43Eu1D458u1D454u1D447u1D458 )u1D443u1D458?1/u1D4642;
end
Algorithm 3.3.1: RLS Magnetometer Calibration
3.4 Angular Rate Estimation
In [24] the authors propose an Extended Kalman Filter with time propagation based on the
solution of Jacobian elliptic functions to estimate a satellite?s angular rates from magnetometer
measurements. No modeling of the earth?s magnetic ?eld is necessary. Further, no knowledge of
the satellite?s attitude is needed. The satellite is assumed to be in a tumbling mode, which means
that the only applied torques are stochastic torques (i.e. there is no input).
83
The satellite?s dynamics are modeled by
?u1D714 = u1D43D?1(?u1D714?u1D43Du1D714) +u1D709
or
?x = u1D453(u1D465,u1D43D) +u1D709 (3.8)
where u1D714 is a vector of angular rates, u1D43D is the inertia matrix, and u1D709 ? u1D4A9(0,u1D444u1D450) is Gaussian noise.
The observation model is based on
u1D451?b
u1D451u1D461 =
??b
?u1D461 +u1D714?
?b (3.9)
where ?b is the earth magnetic ?eld vector, u1D451?bu1D451u1D461 is the derivative of the magnetic ?eld vector in an
inertial reference frame, and ??b?u1D461 is the derivative of the magnetic ?eld in the body reference frame.
For most orbits u1D451?bu1D451u1D461 ? 0 so
??b
?u1D461 ? ?u1D714?
?b = [?u1D44F?]u1D714
where
[?u1D44F?] =
?
??
??
?
0 ??u1D44Fu1D465 ?u1D44Fu1D466
?u1D44Fu1D467 0 ??u1D44Fu1D465
??u1D44Fu1D466 ?u1D44Fu1D465 0
?
??
??
?
.
A continuous model must be discretized since sensor measurements are not continuous. The
nonlinear state equation is discretized to
xu1D458+1 = u1D719u1D458xu1D458 +uu1D458
where u1D719u1D458 is the linearized state transition matrix, and uu1D458 ? u1D4A9(0,u1D444) where u1D444 = u1D444u1D450?u1D461. Instead of
conventional integration methods at the time propagation step of the EKF, the authors propose
using the analytical solution for rigid body motion in terms of the Jacobian elliptic function. They
assert that this greatly reduces the number of FLOPs required in computation. The linearized
state transition matrix is used to propagate the error covariance matrix u1D443u1D458. The transition matrix
84
is approximated as
u1D719u1D458 ?u1D43C +u1D439u1D458?u1D461
where
u1D439u1D458 = ?u1D453?u1D465
vextendsinglevextendsingle
vextendsinglevextendsingle
u1D465=?u1D465
=
?
??
??
?
0 (u1D43Du1D466u1D466?u1D43Du1D467u1D467)?u1D4653u1D43Du1D465u1D465 (u1D43Du1D466u1D466?u1D43Du1D467u1D467)?u1D4652u1D43Du1D465u1D465
(u1D43Du1D467u1D467?u1D43Du1D465u1D465)?u1D4653
u1D43Du1D466u1D466 0
(u1D43Du1D467u1D467?u1D43Du1D465u1D465)?u1D4651
u1D43Du1D466u1D466
(u1D43Du1D465u1D465?u1D43Du1D466u1D466)?u1D4652
u1D43Du1D467u1D467
(u1D43Du1D465u1D465?u1D43Du1D466u1D466)?u1D4651
u1D43Du1D467u1D467 0
?
??
??
?
,
and u1D43D is assumed to be diagonal.
The magnetometer measurement model used is
?bu1D458 = ?bu1D458 +u1D708u1D458
where u1D708u1D458 ? u1D4A9(0,u1D445u1D447u1D434u1D440) is the stationary measurement noise. The body-referenced temporal
derivative is approximated with a backwards di?erence. Then the observation equation is
zu1D458 = u1D43Bu1D458xu1D458 +nu1D458
where u1D43Bu1D458 = [?u1D44Fu1D458?1?]?u1D461, zu1D458 = ?bu1D458? ?bu1D458?1 is the e?ective measurement vector, and nu1D458 = u1D708u1D458?u1D708u1D458?1 is
the e?ective measurement noise, which is colored.
The EKF can be augmented to deal with the colored noise. u1D45Bu1D458 can be modeled as a ?rst-order
Markov process
nu1D458 = u1D719u1D450u1D458nu1D458?1 +wu1D458?1
where wu1D458 ? u1D4A9(0,u1D445u1D464u1D458 ) and u1D719u1D450u1D458 is related to the process autocorrelation. This yields
u1D719u1D450u1D458 = ?0.5u1D43C = u1D719u1D450
u1D445u1D464u1D458 = 1.5u1D445u1D447u1D434u1D440 = u1D445u1D464.
85
The original EKF state vector is augmented with xu1D458 =
uni005B.alt03
xu1D447u1D458 nu1D447u1D458
uni005D.alt03u1D447
. The new state transition
matrix, observation matrix, and u1D444 matrix are
u1D719u1D458 =
?
?? u1D719u1D458 0
0 u1D719u1D450
?
??
u1D43Bu1D458 =
uni005B.alt03
u1D43Bu1D458 u1D43C3?3
uni005D.alt03
u1D444 =
?
?? u1D444 0
0 u1D445u1D464
?
??.
The entries in u1D444 are found through a trial-and-error tuning process. Augmenting the system
results in a singular measurement model. This di?culty can be overcome by replacing the singular
measurement noise by a small positive-de?nite matrix in the Kalman ?lter measurement update
equation or implementing a reduced-order ?lter.
3.5 Conclusions
Several algorithms have been presented in this chapter to estimate the constant bias of a
magnetometer. In order for an algorithm to be suitable for estimating the bias of a magnetometer
on a rocket, the algorithm must be fast since all bias estimation must occur while the rocket is
still in the launch mechanism. The scoring method has the bene?t of being recursive but requires
taking partial derivatives, which is time consuming. This method can also become stuck in local
minima. The ?xed point method also has the bene?t of being iterative, but it requires taking the
inverse of a matrix, a costly operation. This algorithm also exhibits poor convergence, which makes
it unsuitable for our rocket problem. Davenport?s method of estimating bias has the disadvantage
of producing inconsistent bias estimates, and this algorithm requires taking the inverse of a matrix,
which makes it too slow for our problem. The TWOSTEP algorithm requires a good starting
point for estimates of bias, which is not available in the rocket problem so this method of bias
estimation is unsuitable. The recursive least squares method has the advantage of being a quick
and simple method to estimate biases. However, this algorithm is not suitable since the rocket does
86
has neither the orientation required by the algorithm nor is it spinning slowly as required by the
algorithm. Acu?na?s algorithm is the best algorithm for calibrating a magnetometer on a rocket in
a launch mechanism. This algorithm does not require a model and is simple to implement. It does
require taking the inverse of a matrix, but this computational cost can be minimized by using as
few measurements as possible in constructing the matrix to be inverted.
We have also presented a Kalman ?lter based algorithm for angular rate estimation using only
a magnetometer. This algorithm has the bene?ts of not requiring a magnetic ?eld model of the
earth and no knowledge of the attitude of the vehicle is needed. However, this algorithm assumes
the vehicle is tumbling and no input torques are applied making it unsuitable for use on a rocket.
The use of magnetometers primarily on satellites has resulted in many algorithms that are useful
for orbiting bodies but few that can be used on ballistic projectiles.
87
Chapter 4
Estimation And Control
Many methods have been developed to control missiles. In this chapter we present some of
these control methods with emphasis on reaction jet control. The various sensor suites assumed to
be present on the missiles are also considered. In ?4.1 we present several works that address control
laws for various kinds of missiles including skid-to-turn, air-to-air, surface-to-air, and surface-to-
surface. Next, in ?4.2 we present control methods for missiles with reaction jets. First, we present a
variable-structure (sliding-mode) control method. A presentation of a time-optimal control method
for missiles with a single reaction jet follows. In ?4.2 we present control methods for missiles with
multiple reaction jets. In ?4.2.4 we conclude the section with a presentation of a control method
that uses projectile linear theory. We draw some conclusions in ?4.3.
4.1 Current Missile Control Methods
In the literature to date, little if any work has been presented on controlling a projectile solely
during the ?rst few seconds of ?ight. Most work focuses on maneuverable projectiles such as skid-
to-turn (STT) missiles [25], [26], projectiles with thrust vectoring [27], and projectiles with other
forms of control [28], [29]. Moreover, the projectiles presented in the literature have various sensor
suites that allow them to perform tasks such as target tracking to aid in guidance and navigation
[30], [31], [32]. The projectiles in the literature that do have thrusters or reaction jets also have
other control mechanisms [33], [34], [35], [36], [37], [38].
Various control methods have been investigated to increase the accuracy and performance of
missiles (air-to-air, surface-to-air, surface-to-surface). One of the simplest control techniques is
linear control. In [35] time-varying pole placement is used to develop a controller while [39] use
linear-parameter-varying control. Due to the complex dynamics of missiles and the uncertainties
in system models, nonlinear control techniques are often applied to the problem. Backstepping is
88
a relatively simple nonlinear control technique [40]. Sliding-mode control is a common technique
applied to missiles [33], [41], [42]. In [26] a combination of sliding-mode control and adaptive
feedback linearization is used. Dynamic inversion [43], [44] or input/output linearization [25] are
also possible methods of deriving a solution. u1D43B? control is commonly used to obtain an optimal
solution [45], [46], [31]. The use of fuzzy logic and neural networks is also common [47], [48], [32],
[36], [49].
In [50] Kuhn addresses the issue of navigation of gun-launched munitions; however, a variety of
sensor readings are assumed to be available. Kuhn proposes an Extended Kalman Filter (EKF) to
calibrate gyros in-?ight and suggests that better results can be obtained by including accelerometer
and GPS data to improve estimates of aerodynamic drag. Kuhn notes that the use of low-cost
sensors can result in uncompensated sensor errors such as random walk and nonlinearity. In other
words, the use of inexpensive gyros alone is unreliable for navigation. Kuhn proposes the use of a
magnetometer to improve state estimates.
An estimator to predict vehicle attitude and gyro bias is presented in [51]. Gyros are modeled
to have random walk bias and additive noise. Estimates of attitude and gyro bias are obtained with
the proposed state observer, which uses quaternions to represent attitude estimations obtained from
the vehicle?s kinematic equations. The gyro bias estimates are shown to converge exponentially to
a root mean square (RMS) bound. However, for the bias estimates to converge, both the noise
driving the random walk bias and the additive noise must be bounded.
In [52] Gebre-Egziabher et al. present a gyro-free method of determining attitude using only
low-cost accelerometers and magnetometers. The work is based on Wahba?s problem, which shows
that attitude can be determined from two noncolinear vectors that are known in one frame and
measured in another frame [1] (see Chapter 2). In the proposed estimator, the accelerometers are
used to measure gravity in the body frame, and the magnetometers are used to measure the Earth?s
magnetic ?eld in the body frame. Using the solution to Wahba?s problem, the rotation is found
that puts the body frame into an Earth-?xed frame.
Although much literature exists for navigational solutions for maneuverable projectiles with
various control mechanisms and various sensor suites, little has been written about projectiles
89
controlled solely by reaction jets. An overview of the existing literature is presented in ?4.2. Even
less has been written about controlling such projectiles with only gyros and accelerometers. The
relatively recent rise in popularity of MEMS technology has yielded little work in controlling a
projectile using only MEMS inertial sensors.
4.2 Reaction Jet Control
While much work has been developed for controlling missiles through aerosurfaces, relatively
little attention has been given to control of missiles with on/o? reaction jets. In this section we ?rst
present an overview of work on variable-structure control, which has been developed for missiles
with both aerosurfaces and reaction jets. We then present an overview of a method to control a
missile with only a single reaction jet. We conclude the section with a presentation of a control
method for the case of a missile with multiple reaction jets.
4.2.1 Variable-Structure Control
Variable-structure control, also called sliding-mode control, has the de?ning characteristic that
system states are driven discontinuously toward a hyperplanein state space. The control is designed
so that all motion near the hypersurface is directed toward the hypersurface. The states? approach
to the hypersurface will be at least asymptotic. This is the type of control law developed for missiles
that are controlled with both aerosurfaces and reaction jets in [29], [37], and [53].
In [53] Weil and Wise develop an autopilot based on variable-structure control for a missile at
a trim ?ight condition. The nonlinear equations that describe the missile?s dynamics are linearized
about the trim conditions. The model has two sets of inputs: ?n de?ection and reaction jet
condition. The linearized equations are used to derive a linear switching surface for the reaction
jet control and a linear feedback controller for the ?ns. The problem is then split into two linear
quadratic regulator (LQR) problems, which are solved to ?nd the optimal balance between ?n
actuator rates and reaction jet output.
Thukral and Innocenti use variable-structure control systems to develop an autopilot so that
a high-performance missile with both on/o? reaction jets and control ?ns can perform a 180?
90
maneuver to reverse heading [29], [37]. This maneuver requires the missile to go through three
phases of ?ight: pitching to the point of stall, turning while stalled, and recovery from the stall
in the desired attitude and direction. Of particular interest is the second phase in which only
the reaction jets control the missile?s performance. In the ?rst and third stages both the ?ns and
reaction jets are used. Thukral and Innocenti develop a model for the missile?s behavior during
each phase of the maneuver, since the aerodynamic properties vary widely from stage to stage. In
each model the missile is treated as a rigid body with a ?rst bending-mode natural frequency of
about 30 Hz [29]. Aerodynamic data for the modeled missile is not available for the second stage
of the maneuver, so Thukral and Innocenti base their model on a worst-case scenario with attitude
described by
u1D43Cu1D466 ?u1D45E = u1D444u1D446u1D436u1D441u1D43Fu1D450u1D45D +u1D43Fu1D445u1D436u1D446u1D447u1D445u1D436u1D446u1D462u1D447 (4.1)
where u1D43Cu1D466 is the pitch axis moment of inertia, ?u1D45E is pitch angular acceleration, u1D444 is dynamic pressure,
u1D446 is cross sectional area, u1D436u1D441 is the normal force coe?cient, u1D43Fu1D450u1D45D is the distance between the center
of pressure and the center of mass, u1D43Fu1D445u1D436u1D446 is the jets? moment arm, u1D447u1D445u1D436u1D446 is the reaction jet thrust,
and u1D462u1D447 is the throttle variable for the reaction jets.
Variable-structure control is applied to the second-stage dynamics by choosing a desired model
for the missile?s pitch rotational dynamics from which error dynamics are de?ned. The desired
model has the form
?xu1D45A = u1D434u1D45Axu1D45A +u1D435u1D45Auu1D45A, (4.2)
and the actual system dynamics are
?x = u1D434x+u1D435uT. (4.3)
The error dynamics are modeled by
?e = u1D434u1D45Ae+[u1D434u1D45A ?u1D434]x+u1D435u1D45Auu1D45A ?u1D437?u1D435uu1D447 (4.4)
91
where u1D437 is a disturbance vector, e = xu1D45A?x. The control law implementation (nonlinear) is given
by
uu1D447 = sgn(u1D460) (4.5)
u1D460 = u1D43Ae (4.6)
where u1D43A is chosen to ensure a desirable response during sliding [29].
4.2.2 Time-Optimal Control with A Single Reaction Jet
In [54], [55], [56], and [57] Jahangir and Howe develop methods to control the attitude of a
missile with a single thruster located at right angles to the missile?s spin axis. The ?nal desired
attitude is known, and the goal is to drive the missile to the desired attitude in the minimum
amount of time. The missile, which is assumed to have a large roll rate, is controlled by turning
on the thruster for a short period of time during each roll cycle. In [56] Jahangir and Howe
develop a method to generate thruster ?ring times that does not require the solution of a Two-
Point Boundary-Value Problem (TPBVP). In [54] and [55] Jahangir and Howe develop a method
of control for the case when two pulses are su?cient to drive the missiles?s attitude to the desired
state. The remainder of this section summarizes [57], in which the Jahangir and Howe develop a
method of attitude control for the case where two thruster pulses are insu?cient to achieve the
desired ?nal attitude.
Jahangir and Howe develop a state vector of dimensionless variables and then transform this
vector so that boundary-condition points with known thruster ?ring times can be generated without
92
the need to solve a TPBVP. To this end the following variables are de?ned
?u1D466 = u1D714u1D466u1D714
u1D465
(4.7)
?u1D467 = u1D714u1D467u1D714
u1D465
(4.8)
u1D706u1D466 = u1D440u1D466u1D43C
u1D466u1D7142u1D465
(4.9)
u1D434 = 1? u1D43Cu1D465u1D43C
u1D466
= 1? u1D43Cu1D465u1D43C
u1D467
(4.10)
u1D447 = u1D714u1D465u1D461 (4.11)
where u1D714u1D465, u1D714u1D466, and u1D714u1D467 are rotational rates, u1D43Cu1D465, u1D43Cu1D466, and u1D43Cu1D467 are the missile?s moments of inertia, u1D440u1D466
is the moment applied about the missile?s u1D466-axis in the body frame, and u1D461 is time. Then the state
equations are given by
??u1D466 = u1D434?u1D467 +u1D706u1D466 (4.12)
??u1D467 = ?u1D434?u1D466 (4.13)
?u1D713 = (?u1D466sin(u1D719) +?u1D467cos(u1D719))sec(u1D703) (4.14)
?u1D703 = ?u1D466cos(u1D719)??u1D467sin(u1D719) (4.15)
?u1D719 = 1+(?u1D466 sin(u1D719) +?u1D467 cos(u1D719))tan(u1D703) (4.16)
where all derivatives are taken with respect to u1D447. Then the state space representation is given by
?x =
?
??
??
??
??
??
?
u1D434u1D4652
?u1D434u1D4651
(u1D4651 sin(u1D4655)+u1D4652 cos(u1D4655))sec(u1D4654)
u1D4651 cos(u1D4655)?u1D4652 sin(u1D4655)
1+(u1D4651 sin(u1D4655)+u1D4652 cos(u1D4655))tan(u1D4654)
?
??
??
??
??
??
?
+
?
??
??
??
??
??
?
1
0
0
0
0
?
??
??
??
??
??
?
u1D706u1D466 (4.17)
?x = u1D453(x)+gu1D462 (4.18)
93
where x =
uni005B.alt03
?u1D466 ?u1D467 u1D713 u1D703 u1D719
uni005D.alt03u1D447
and the control input is u1D462= u1D706u1D466. The initial state vector is
x0 =
uni005B.alt03
u1D4651,0 u1D4652,0 0 0 0
uni005D.alt03u1D447
, (4.19)
and the desired ?nal state vector is
uni005B.alt03
0 0 u1D4653,u1D451 u1D4654,u1D451 free
uni005D.alt03u1D447
(4.20)
where the ?fth term can have any value since roll angle does not matter. The control input has a
maximum u1D462u1D45Au1D44Eu1D465 so the bounds on the control are given by
0 ?u1D462?u1D462u1D45Au1D44Eu1D465.
The solution of the developed system involves a TPBVP (an iterative problem) so thruster
?ring times for the desired boundary conditions are found o?-line and stored in a look-up table.
The control is real-time implementable by table look-up and interpolation. Another approach is
to use a transformation that allows the boundary condition points for which thruster ?ring times
are known to be generated without the requirement of an iterative solution of a TPBVP. By using
a transformation of the state vector, time-optimal trajectories can be computed by integrating
backwards from the desired ?nal condition.
The transformation begins by de?ning a new reference frame. For this new frame, the axes
are ?xed in the target with the u1D465-axis pointing in the desired direction of the missile?s body frame
u1D465-axis. The Euler angles that convert from the missile?s body frame to the new frame are u1D4663, u1D4664,
and u1D4665 corresponding to yaw, pitch, and roll, respectively. De?ne u1D4661 = u1D4651 and u1D4662 = u1D4652. Then the
new state vector is
y =
uni005B.alt03
u1D4661 u1D4662 u1D4663 u1D4664 u1D4665
uni005D.alt03u1D447
, (4.21)
and the transformed system is
?y = u1D453(y) +gu1D462. (4.22)
94
The initial and ?nal conditions are given by
y(u1D4470) = y0 =
uni005B.alt03
u1D4661,0 u1D4662,0 u1D4663,0 u1D4664,0 u1D4665,0
uni005D.alt03u1D447
(4.23)
y(u1D447u1D453) = yu1D453 =
uni005B.alt03
0 0 0 0 free
uni005D.alt03u1D447
, (4.24)
respectively. The transformation from the new frame to the body frame is given by
u1D436(u1D4653,u1D451,u1D4654,u1D451,u1D4655,u1D451) = ?u1D436(u1D4663,u1D4664,u1D4665)u1D447 (4.25)
where
u1D436(u1D713,u1D703,u1D719) =
?
??
??
?
u1D450u1D713u1D450u1D703 u1D460u1D713u1D450u1D703 ?u1D460u1D703
?u1D460u1D713u1D450u1D719 +u1D450u1D713u1D460u1D703u1D460u1D719 u1D450u1D713u1D450u1D719 +u1D460u1D713u1D460u1D703u1D460u1D719 u1D450u1D703u1D460u1D719
u1D460u1D713u1D460u1D719 +u1D450u1D713u1D460u1D703u1D450u1D713 ?u1D450u1D713u1D460u1D719 +u1D460u1D713u1D460u1D703u1D450u1D719 u1D450u1D703u1D450u1D719
?
??
??
?
(4.26)
? =
?
??
??
?
1 0 0
0 cos(u1D4665,u1D453) sin(u1D4665,u1D453)
0 ?sin(u1D4665,u1D453) cos(u1D4665,u1D453)
?
??
??
?
. (4.27)
u1D450u1D456 and u1D460u1D456 are cos(u1D456) and sin(u1D456), respectively.
With the transformation de?ned, a time-optimal control law can be found. De?ne the cost
function as
u1D43D =
uni222B.alt02 u1D447u1D453
u1D4470
1u1D451u1D461. (4.28)
Necessary conditions for optimality are obtained from the Hamiltonian
u1D43B = qu1D447 ?y?1 (4.29)
95
where q is the costate vector. This leads to an optimal control u1D462? from the conditions
?y? = ?u1D43B?q = u1D453(y?) +gu1D462? (4.30)
?q = ??u1D43B?y = u1D43B(y?)q? (4.31)
u1D462? =
?
??
??
u1D462u1D45Au1D44Eu1D465 if u1D45E?1 > 0
0 if u1D45E?u1D45E < 0
(4.32)
where u1D43B(y) = ??u1D453/?y. The boundary conditions of the Hamiltonian and costate vector are
u1D43B(u1D447u1D453) = 0 (4.33)
q(u1D4470) = q0 =
uni005B.alt03
free free free free free
uni005D.alt03u1D447
(4.34)
q(u1D447u1D453) = qu1D453 =
uni005B.alt03
free free free free 0
uni005D.alt03u1D447
. (4.35)
A control history can be generated with the following steps:
1. Initialize yu1D453 and qu1D453.
2. Integrate the ?y and ?q equations backward in time, and at each u1D447?u1D45B = u1D45B?u1D447 obtain y(u1D447?u1D45B) where
?u1D447 is the time interval chosen to give desired data point spacing between y(u1D447?u1D45B) and y(u1D447?u1D45B+1)
and u1D45B is a positive integer.
3. Transform y(u1D447?u1D45B) to obtain the boundary conditions in the original form u1D4651,0, u1D4652,0, u1D4653,u1D451, and
u1D4654,u1D451. Store the thruster switching times as functions of these four variables.
4.2.3 Multiple-Reaction Jet Control
Costello et al. [58], [59], [60] are the only authors that investigate control of a projectile solely
through multiple reaction jets. In [58] a rocket is simulated that has a main thruster that has a
limited burn time and is stabilized by three pop-out ?ns. Lateral pulse jets, each of which can only
be ?red once, are located toward the front of the rocket. Lateral pulse jet control is investigated to
improve the dispersion pattern of direct-?re atmospheric rockets. This method of control falls under
96
the category of impulse control, unlike other common rocket control methods such as proportional
navigation guidance (PNG), which is continuous. In [58] Jitpraphai et al. investigate PNG,
parabolic and proportional navigation guidance (PAPNG), and trajectory tracking (TT) applied
to a direct-?re atmospheric rocket equipped with a lateral pulse jet control mechanism. All three
control laws use the same control logic to ?re the lateral pulse jets.
Proportional Navigation Guidance (PNG)
The guidance law for PNG is
u1D434u1D436 = u1D441u1D449u1D450?u1D706 (4.36)
where u1D441 is the proportional navigation constant (typically valued between 3 and 5), u1D449u1D436 is rocket
closing velocity, and ?u1D706 is line-of-sight (LOS) angular rate. PNG has horizontal plane and vertical
plane control law components and three reference frames are used: the LOS frame, the target
frame, and the inertial frame. For the following equations, the target is assumed to be stationary.
All axes in [58] are labeled (u1D43C,u1D43D,u1D43E) with a subscript to indicate the reference frame. The u1D43C-axis of
the LOS frame points from the rocket to the target.
The horizontal component of u1D706 is then
u1D706u1D43B = tan?1
uni0028.alt03u1D466
u1D447 ?u1D466
u1D465u1D447 ?u1D465
uni0029.alt03
(4.37)
and
?u1D706u1D43B = ??u1D466(u1D465u1D447 ?u1D465) + ?u1D465(u1D466u1D447 ?u1D466)
(u1D465u1D447 ?u1D465)2 +(u1D466u1D447 ?u1D466)2 . (4.38)
Then the horizontal acceleration command is
?u1D434u1D44Cu1D436 = u1D441u1D43B ?u1D706u1D43Bu1D462u1D43B (4.39)
u1D462u1D43B = cos(u1D706u1D43B)?u1D465+sin(u1D706u1D43B)?u1D466 (4.40)
97
where the ? indicates the LOS frame. The vertical plane component is given as
u1D706u1D449 = tan?1
uni0028.alt03u1D467
u1D447 ?u1D467
u1D465u1D447 ?u1D465
uni0029.alt03
(4.41)
?u1D706u1D449 = ??u1D467(u1D465u1D447 ?u1D465)+ ?u1D465(u1D467u1D447 ?u1D467)
(u1D465u1D447 ?u1D465)2 +(u1D467u1D447 ?u1D467)2 (4.42)
?u1D434u1D44Du1D436 = u1D441u1D449 ?u1D706u1D449u1D462u1D449 (4.43)
u1D462u1D449 = cos(u1D706u1D449)?u1D465+sin(u1D706u1D44C)?u1D467 (4.44)
The total acceleration command is then
?u1D434u1D436 = ?u1D434u1D44Cu1D436u1D43Du1D43F + ?u1D434u1D44Du1D436u1D43Eu1D43F = u1D434u1D44Bu1D436u1D43Cu1D435 +u1D434u1D44Cu1D436u1D43Du1D435 +u1D434u1D44Du1D436u1D43Eu1D435 (4.45)
where (u1D43C,u1D43D,u1D43E) are unit vectors. The input to the lateral pulse ring ?ring logic is the magnitude of
the o?-axis command acceleration. This magnitude is given by
? =
uni221A.alt02
u1D4342u1D44Cu1D436 +u1D4342u1D44Du1D436 (4.46)
and, the error signal phase is given by
u1D6FE = tan?1(u1D434u1D44Du1D436/u1D434u1D44Cu1D436). (4.47)
Parabolic and Proportional Navigation Guidance (PAPNG)
PAPNG uses the same horizontal control law as PNG. In the vertical plane, a desired parabolic
trajectory is described as
?u1D467u1D443 = ?u1D467u1D447 +u1D43E1?u1D465u1D45D +u1D43E2?u1D4652u1D443 (4.48)
where the ?u1D465u1D443 and ?u1D467u1D443 are components of the rocket position in the target frame, ?u1D465u1D447 and ?u1D467u1D447 are
components of the target position in the target frame, and u1D43E1 and u1D43E2 are de?ned by Equations
(4.49)-(4.50). The desired angle at which the rocket passes through the target is de?ned as u1D6FDu1D453.
98
Then
u1D43E1 = tan(u1D6FDu1D439) (4.49)
u1D43E2 = ?(u1D43E1
uni221A.alt01(u1D465
u1D447 ?u1D465)2 +(u1D466u1D447 ?u1D466)2 + ?u1D467u1D447 ? ?u1D467u1D443)
(u1D465u1D447 ?u1D465)2 +(u1D466u1D447 ?u1D466)2 . (4.50)
The command acceleration in the vertical LOS plane is
?u1D434u1D44Du1D436 = u1D449u1D43F(u1D6FDu1D437 ?u1D6FD)
u1D70F (4.51)
where u1D449u1D43F is the rocket velocity magnitude in the LOS frame, u1D6FD is the ?ight path angle in the LOS
frame, u1D70F is the acceleration command time constant, and u1D6FDu1D437 is the rocket?s desired ?ight path
angle. The acceleration command is then given by
?u1D434u1D436 = ? ?u1D434u1D44Du1D436 sin(u1D6FD)u1D43Cu1D43F + ?u1D434u1D44Cu1D436u1D43Du1D43F ? ?u1D434u1D44Du1D436 cos(u1D6FD)u1D43Eu1D43F (4.52)
= u1D434u1D44Bu1D436u1D43Cu1D435 +u1D434u1D44Cu1D436u1D43Du1D435 +u1D434u1D44Du1D436u1D43Eu1D435. (4.53)
Trajectory Tracking (TT)
In this method a command trajectory is assumed to be known prior to launch. From this infor-
mation the error between the desired path and the actual path can be computed. The magnitude
of this error is used as control input to the thruster ?ring logic. The magnitude of the error and
the error phase angle are
? =
uni221A.alt02
u1D4522u1D44C +u1D4522u1D44D (4.54)
u1D6FE = tan?1(u1D452u1D467/u1D452u1D44C). (4.55)
Lateral Pulse Jet Firing Logic
In order for a jet to ?re, four requirements must be met. The following two requirements apply
to all lateral jets.
99
? The error magnitude must be greater than some tolerance.
? The time between thruster ?rings must be greater than some threshold.
The next two requirements are for a speci?c lateral jet.
? The di?erence between the error phase angle and the individual pulse jet force must be less
than some threshold angle.
? The pulse jet has not been previously ?red.
The last condition enforces the requirement that each jet can only be ?red once.
In [59] the path control system is designed to track a speci?c trajectory. To do this the rocket?s
measured position is compared to the commanded position to create an error signal, which in the
rocket body frame is given by
?
??
??
?
u1D452u1D44B
u1D452u1D44C
u1D452u1D44D
?
??
??
?
=
?
??
??
?
u1D450u1D703u1D450u1D713 u1D450u1D703u1D460u1D713 ?u1D460u1D703
u1D460u1D719u1D460u1D703u1D450u1D713 ?u1D450u1D719u1D460u1D713 u1D460u1D719u1D460u1D703u1D460u1D719 +u1D450u1D719u1D450u1D713 u1D460u1D719u1D450u1D703
u1D450u1D719u1D460u1D703u1D450u1D719 +u1D460u1D719u1D460u1D713 u1D450u1D719u1D460u1D703u1D460u1D713 ?u1D460u1D719u1D450u1D713 u1D450u1D719u1D450u1D703
?
??
??
?
?
??
??
?
u1D465u1D436 ?u1D465
u1D466u1D436 ?u1D466
u1D467u1D436 ?u1D467
?
??
??
?
(4.56)
where u1D719, u1D703, and u1D713 are the roll, pitch, and yaw Euler angles. Magnitude and phase error are de?ned
as
? =
uni221A.alt02
u1D4522u1D44C +u1D4522u1D44D (4.57)
u1D6FE = tan?1(u1D452u1D467/u1D452u1D44C). (4.58)
With this information available at each computation cycle, four criteria are checked to see if an
individual thruster should be ?red (lateral jet thrust is modeled as a constant of a known duration).
The four conditions are
1. The tracking error magnitude must be greater than some threshold
? >u1D452u1D461?u1D45Fu1D452u1D460. (4.59)
100
2. A certain amount of time must have passed since the last jet ?ring
u1D461?u1D461? > ?u1D461u1D461?u1D45Fu1D452u1D460 (4.60)
where u1D461? is the time of the most recent pulse jet ?ring.
3. The angle between the tracking error and the individual pulse jet under consideration must
be less than a threshold
?u1D703u1D456 ?u1D45Du1D456?u1D6FE? ?u1D6FE(?u1D443u1D43D/2)?<u1D6FFu1D461?u1D45Fu1D452u1D460 (4.61)
whereu1D703u1D456 is the angle between the body frameu1D466-axis and the iu1D461? pulse jet and ?u1D443u1D43D is the pulse
jet ?ring duration.
4. The pulse jet under consideration must not have been previously ?red.
The purpose of ?u1D461u1D461?u1D45Fu1D452u1D460 is to ensure that the rocket has enough time to respond to a command to a
particular pulse ?ring as well as to ensure that many pulse ?rings at once do not overcompensate
for trajectory errors.
4.2.4 Projectile Linear Theory
In [60] Costello et al. utilize projectile linear theory to develop a control law. Dynamic
equations have been developed to allow the closed-form solution of a missile under restricted ?ight
conditions. The equations and the solutions to the equations have become known as projectile
linear theory. Projectile linear theory uses a reference frame that is attached to the projectile but
does not roll. A dimensionless variable arc length u1D460 is used rather than time, and a change of
variables from the velocity along the projectile?s axis of symmetry to total velocity u1D449 is used. The
model also assumes that Euler pitch and yaw angles and the aerodynamic angle of attack are small.
101
Costello et. al. convert their model of a projectile according to this theory to yield
u1D466(u1D460)
u1D437 =
u1D4660
u1D437 +(u1D7130 +u1D4630/u1D4490)u1D460+
uni0028.alt02u1D70Cu1D446u1D437
2u1D45A
uni0029.alt02(u1D436u1D44B0 ?u1D436u1D441u1D434)
u1D4490
uni007B.alt02?u1D463u1D453
u1D7192u1D453 [exp(u1D706
?
u1D453u1D460)sin(u1D719u1D453u1D460+u1D703u1D463u1D453 ?u1D70B)
? sin(u1D703u1D463u1D453 ?u1D70B)?u1D719u1D453 cos(u1D703u1D463u1D453 ?u1D70B)u1D460]
uni007D.alt02
+
uni0028.alt02u1D70Cu1D446u1D437
2u1D45A
uni0029.alt02(u1D436u1D44B0 ?u1D436u1D441u1D434)
u1D4490
uni007B.alt02?u1D463u1D460
u1D7192u1D460 [exp(u1D706
?
u1D460u1D460)sin(u1D719u1D460u1D460+u1D703u1D463u1D460 ?u1D70B)
? sin(u1D703u1D463u1D460 ?u1D70B)?u1D719u1D460cos(u1D703u1D463u1D460 ?u1D70B)u1D460]
uni007D.alt02
(4.62)
u1D467(u1D460)
u1D437 =
u1D4670
u1D437 +(?u1D7030 +u1D4640/u1D4490)u1D460+u1D454u1D437
uni005B.alt02 u1D45A
u1D70Cu1D446u1D437u1D4490u1D436u1D44B0
uni005D.alt022
?
uni007B.alt02
exp
uni0028.alt02u1D70Cu1D446u1D437u1D436u1D44B0u1D460
u1D45A
uni0029.alt02
? u1D70Cu1D446u1D437u1D436u1D44B0u1D460u1D45A ?1
uni007D.alt02uni0028.alt02u1D70Cu1D446u1D437
2u1D45A
uni0029.alt02u1D436u1D44B0 ?u1D436u1D441u1D434
u1D4490
uni007B.alt02?u1D464u1D453
u1D7192u1D453 [exp(u1D706
?
u1D453u1D460)sin(u1D719u1D453u1D460+u1D703u1D464u1D453 ?u1D70B)
? sin(u1D703u1D464u1D453 ?u1D70B)?u1D719u1D453 cos(u1D703u1D464u1D453 ?u1D70B)u1D460]
uni007D.alt02
+
uni0028.alt02u1D70Cu1D446u1D437
2u1D45A
uni0029.alt02(u1D436u1D44B0 ?u1D436u1D441u1D434)
u1D4490
uni007B.alt02?u1D464u1D460
u1D7192u1D460 [exp(u1D706
?
u1D460u1D460)sin(u1D719u1D460u1D460+u1D703u1D464u1D460?u1D70B)?sin(u1D703u1D464u1D460 ?u1D70B)
? u1D719u1D460cos(u1D703u1D464u1D460 ?u1D70B)u1D460]
uni007D.alt02
. (4.63)
u1D437 is projectile characteristic length, (u1D462,u1D463,u1D464) are the translational velocity components of the
projectile center of mass resolved in the ?xed plane reference frame, u1D449 is the magnitude of the mass
center velocity, u1D70C is air density, u1D45A is projectile mass, u1D436u1D441u1D434 and u1D436u1D44B0 are aerodynamic coe?cients,
and (u1D465,u1D466,u1D467) are position vector components of the projectile mass center expressed in the inertial
reference frame. ?u1D463u1D453, ?u1D463u1D460, u1D706?u1D453, u1D706?u1D460, u1D719u1D453, u1D719u1D460, u1D703u1D463u1D453, u1D703u1D463u1D460, ?u1D464u1D453, ?u1D464u1D460, u1D703u1D464u1D453, and u1D703u1D464u1D460 are all constants. These
equations are used to predict the impact point of the projectile given the current state of the system
which is obtained from an IMU.
Thus, the problem is recast to control the impact point of the projectile in the target plane
rather than controlling the trajectory. Control input is based on the di?erence between the actual
target location and the predicted impact point. Costello et. al. make two key assumptions: the
control law has full state feedback (u1D465, u1D466, u1D467, u1D713, u1D703, u1D719, u1D462, u1D463, u1D464, u1D45D, u1D45E, and u1D45F) and the projectile has been
provided with the target?s inertial coordinates. The distance by which the projectile is predicted
to miss the target is calculated as
u1D716 =
uni221A.alt01
(u1D466u1D447 ?u1D466u1D43C)2 +(u1D467u1D447 ?u1D467u1D43C)2 (4.64)
102
where the subscript u1D447 indicates the target frame and the subscript u1D43C indicates the inertial frame.
The miss distance phase angle is de?ned as
u1D6FE = ?tan?1
uni0028.alt02u1D466u1D461?u1D466u1D43C
u1D467u1D447 ?u1D467u1D43C
uni0029.alt02
(4.65)
A radius u1D446u1D44A de?nes a circle around the target in which it is desired for the missile to impact. As
the missile gets closer to the target, u1D446u1D44A gets smaller. If the computed miss distance is greater than
u1D446u1D44A, a thruster is ?red. Linear impact theory is used to determine if ?ring a speci?c thruster will
result in the missile hitting the target area.
Simple logic is used to decide if a pulse jet should be ?red. A line segment from the target to
the predicted impact point is computed via
u1D446u1D45D = (u1D466u1D43C ?u1D466u1D447)/(u1D467u1D43C ?u1D467u1D447). (4.66)
Then if the two inequalities
u1D466u1D43C ? [(u1D466u1D447 ?1)/u1D446u1D45D](u1D467u1D43C ?u1D467u1D447) (4.67)
u1D466?u1D43C ? [(u1D466u1D447 ?1)/u1D446u1D45D](u1D467?u1D43C ?u1D467u1D447) (4.68)
are both true or both false (meaning both points are on the same side of a curve that de?nes an
allowable overshoot region) a jet is ?red. The asterisk denotes controlled impact point coordinates.
Next, u1D716 is calculated. If u1D716 is smaller than the allowable overshoot, a thruster is ?red regardless of
the results of the two inequalities.
4.3 Conclusions
We have presented several control methods in this chapter. Those methods that apply to
missiles with control actuators other than reaction jets cannot be applied to our problem since
the only control method available is reaction jets. Innocenti and Thukral present a sliding mode
control law for a missile during a ?ight phase in which only reaction jets are available. This ?ight
103
phase is well after the missile has been launched; therefore, the missile is not subjected to the
disturbances of initial ?ight while only reaction jet control is used. The sliding mode control law
presented does not address the challenges of ?ight immediately after launch to which our rocket
is subjected. Further, sliding mode control is subject to chattering, oscillation about the sliding
surface. Chattering is not acceptable in a problem in which a rocket must be controlled accurately
within seconds during large disturbances. Jahangir and Howe?s control method is for a rocket
controlled solely with a reaction jet, but their rocket has a single reaction jet that can be ?red
multiple times. This method will not work for the rocket in this work because the rocket of concern
has multiple, single-?re reaction jets. Jahangir and Howe?s method also optimizes control of the
rocket over the entire ?ight whereas the rocket of concern can only be controlled for a few seconds.
Costello develops a few algorithms for a rocket controlled with multiple, one-shot reaction jets.
The PNG algorithm controls attitude in the horizontal and vertical planes, but requires multiple
reference frames including a target reference frame. For our problem a target reference frame is
unavailable. The PAPNG method uses a desired parabolic path in the vertical frame to derive
control commands, but this method also requires the use of a target reference frame. The TT
method uses the error between a desired path and the actual path of the rocket. This method is
best suited for controlling the rocket of this work since the TT method assumes multiple, one-shot
reaction jets and needs no information about the target, only a desired trajectory to the target.
This method can be used for the ?rst few seconds of ?ight as well as over the entire ?ight. The
control law based on linear projectile theory proposed by Costello is not suitable for our problem
since this method assumes full state feedback, which is unavailable to the rocket in this work.
104
Chapter 5
Rocket Dynamics and State Estimation
In this chapter the dynamics for a ballistic projectile are developed as well as methods for
estimating the projectile?s attitude. As shown in Figure 2.1, the body frame axes are located at
the rocket?s center of gravity with the u1D465-axis passing through the nose. The rocket is symmetric
about the u1D465-axis, which means that the moment of inertia in the y-direction is equal to the moment
of inertia in the z-direction. The rocket has a main thruster that propels it and a ring of lateral
thrusters located at the back of the rocket to provide control. Fins, which pop open shortly after
the rocket clears the launch mechanism, are also located at the rocket?s rear. As for the notation in
this chapter, all zero-mean white noise values given by the variable u1D702 are independent. All constant
biases are assumed to be estimated before the rocket leaves the launch platform; therefore, they do
not appear in the estimation schemes.
5.1 Rotational Rate Modeling
The rotational dynamics of a rigid body are given by
u1D451u1D714
u1D451u1D461 = u1D43D
?1(u1D70F ?u1D714?(u1D43Du1D714)) (5.1)
where u1D714 =
uni005B.alt03
u1D45D u1D45E u1D45F
uni005D.alt03u1D447
is a vector of rotational rates, u1D43D is the moment of inertia matrix, and
u1D70F is a vector of torques (moments) applied to the body frame. For a rotating rocket, the applied
moments are
u1D70F = u1D70Fu1D44Eu1D452u1D45Fu1D45C +u1D70Fu1D45Du1D45Fu1D45Cu1D45D +u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459
where u1D70Fu1D44Eu1D452u1D45Fu1D45C, u1D70Fu1D45Du1D45Fu1D45Cu1D45D, and u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459 respectively refer to the moments introduced by the aerodynamic,
propulsion, and attitude control systems. Similarly, the forces that a?ect the rocket?s body frame
105
are given by
F = Fu1D454u1D45Fu1D44Eu1D463 +Fu1D44Eu1D452u1D45Fu1D45C +Fu1D45Du1D45Fu1D45Cu1D45D +Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459
where the subscripts u1D454u1D45Fu1D44Eu1D463, u1D44Eu1D452u1D45Fu1D45C, u1D45Du1D45Fu1D45Cu1D45D, and u1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459 refer to the gravitational, aerodynamic, propul-
sion, and control forces, respectively.
Wind a?ects the rocket?s angle of attack u1D6FCu1D461u1D45Cu1D461. In the body frame x-direction, the wind is
assumed to be zero. Angle of attack is computed from the relative velocity
vu1D45F =
?
??
??
?
u1D463u1D45F,u1D465
u1D463u1D45F,u1D466
u1D463u1D45F,u1D467
?
??
??
?
as
tanu1D6FCu1D461u1D45Cu1D461 =
vextenddoublevextenddouble
vextenddoublevextenddouble
vextenddoublevextenddouble
vextenddouble
?
?? u1D463u1D45F,u1D466
u1D463u1D45F,u1D467
?
??
vextenddoublevextenddouble
vextenddoublevextenddouble
vextenddoublevextenddouble
vextenddouble2
u1D463u1D45F,u1D465 .
The wind azimuth angle is then
tanu1D719u1D44F = u1D463u1D45F,u1D466u1D463
u1D45F,u1D465
.
The aerodynamic moments are given by
u1D70Fu1D44Eu1D452u1D45Fu1D45C = u1D45Eu1D446u1D45Fu1D452u1D453u1D437u1D45Fu1D452u1D453
?
??
??
?
u1D45Fu1D45Fu1D452u1D453
?
??
??
?
u1D450u1D43Fu1D45D(u1D461) 0 0
0 u1D450u1D45Au1D45Eu1D45F(u1D461) 0
0 0 u1D450u1D45Au1D45Eu1D45F(u1D461)
?
??
??
?
?
??
??
?
u1D45D
u1D45E
u1D45F
?
??
??
?
+
?
??
??
?
u1D450u1D43F(u1D461)
u1D450u1D440(u1D461)
u1D450u1D441(u1D461)
?
??
??
?
?
??
??
?
(5.2)
where u1D45E is dynamic pressure, u1D446u1D45Fu1D452u1D453, u1D437u1D45Fu1D452u1D453, and u1D45Fu1D45Fu1D452u1D453 are constants, u1D450u1D43Fu1D45D(u1D461) and u1D450u1D45Au1D45Eu1D45F(u1D461) are dynamic
derivatives that are a function of Mach number, and u1D450u1D43F(u1D461), u1D450u1D440(u1D461), and u1D450u1D441(u1D461) are base coe?cients.
The roll base coe?cient u1D450u1D43F(u1D461) is given by
u1D450u1D43F(u1D461) = u1D719u1D453u1D456u1D45Bu1D450u1D43Fu1D437(u1D461)+u1D450u1D43F0(u1D461)
106
where u1D719u1D453u1D456u1D45B is the ?n cant. u1D450u1D43Fu1D437(u1D461) and u1D450u1D43F0(u1D461) are functions of mach number when the ?ns are
deployed and equal to zero otherwise. Then u1D450u1D43F(u1D461) can be written as
u1D450u1D43F(u1D461) = u1D452u1D453(u1D719u1D453u1D456u1D45Bu1D450u1D43Fu1D437(u1D461) +u1D450u1D43F0(u1D461))
where
u1D452u1D453 =
?
??
??
0 if ?ns are closed
1 if ?ns are open
.
The pitch and yaw base coe?cients are given by
?
?? u1D450u1D440(u1D461)
u1D450u1D441(u1D461)
?
??=
?
?? cos(u1D719u1D44F)
?sin(u1D719u1D44F)
?
??u1D450?
u1D45A(u1D461)
where
u1D450?u1D440(u1D461) = u1D450u1D464u1D440(u1D461)+u1D450u1D464u1D467 (u1D461)u1D450u1D454u1D465.
u1D450u1D464u1D440(u1D461) is the wind frame pitch moment, u1D450u1D464u1D467 (u1D461) is the u1D467-axis force coe?cient, and u1D450u1D454u1D465 is the x-
coordinate center of gravity. Both u1D450u1D464u1D440(u1D461) and u1D450u1D464u1D467 (u1D461) depend on angle of attack. Thus,
u1D70Fu1D44Eu1D452u1D45Fu1D45C = u1D45Eu1D446u1D45Fu1D452u1D453u1D437u1D45Fu1D452u1D453
?
??
??
?
u1D45Fu1D45Fu1D452u1D453
?
??
??
?
u1D450u1D43Fu1D45D(u1D461)u1D45D
u1D450u1D45Au1D45Eu1D45F(u1D461)u1D45E
u1D450u1D45Au1D45Eu1D45F(u1D461)u1D45F
?
??
??
?
+
?
??
??
?
u1D452u1D453(u1D719u1D453u1D456u1D45Bu1D450u1D43Fu1D437(u1D461) +u1D450u1D43F0(u1D461))
cos(u1D719u1D44F)u1D450?u1D45A(u1D461)
?sin(u1D719u1D44F)u1D450?u1D45A(u1D461)
?
??
??
?
?
??
??
?
.
The propulsion torque is given by
u1D70Fu1D45Du1D45Fu1D45Cu1D45D = ru1D45Du1D45Fu1D45Cu1D45D ?fu1D45Du1D45Fu1D45Cu1D45D +
?
??
??
?
u1D70Fu1D45Au1D45Cu1D461u1D45Cu1D45F
0
0
?
??
??
?
107
where
ru1D45Du1D45Fu1D45Cu1D45D =
?
??
??
?
u1D45Fu1D465
0
0
?
??
??
?
u1D437u1D45Fu1D452u1D453
is the motor lever arm with u1D45Fu1D465 being a constant. The propulsion force is given by
fu1D45Du1D45Fu1D45Cu1D45D = u1D447(u1D461)
?
??
??
?
cos(u1D452u1D713)cos(u1D452u1D703)
sin(u1D452u1D713)cos(u1D452u1D703)
?sin(u1D452u1D703)
?
??
??
?
where u1D447(u1D461) is the main motor thrust and u1D452u1D713 and u1D452u1D703 are motor misalignment angles. When u1D70Fu1D45Au1D45Cu1D461u1D45Cu1D45F
is assumed to be zero,
u1D70Fu1D45Du1D45Fu1D45Cu1D45D =
?
??
??
?
0
u1D45Fu1D465u1D437u1D45Fu1D452u1D453 sin(u1D452u1D703)u1D447(u1D461)
u1D45Fu1D465u1D437u1D45Fu1D452u1D453 sin(u1D452u1D713)cos(u1D452u1D703)u1D447(u1D461)
?
??
??
?
. (5.3)
Then from Equation (5.1), the rocket?s rotational dynamics are
?u1D714 =
?
??
??
?
1
u1D43Cu1D465u1D465(u1D461) 0 0
0 1u1D43Cu1D466u1D466(u1D461) 0
0 0 1u1D43Cu1D466u1D466(u1D461)
?
??
??
?
?
??
??
?
u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459 +u1D45Eu1D446u1D45Fu1D452u1D453u1D437u1D45Fu1D452u1D453
?
??
??
?
u1D45Fu1D45Fu1D452u1D453
?
??
??
?
u1D450u1D43Fu1D45D(u1D461) 0 0
0 u1D450u1D45Au1D45Eu1D45F(u1D461) 0
0 0 u1D450u1D45Au1D45Eu1D45F(u1D461)
?
??
??
?
?
??
??
?
u1D45D
u1D45E
u1D45F
?
??
??
?
+
?
??
??
?
u1D452u1D453(u1D719u1D453u1D456u1D45Bu1D450u1D43Fu1D437(u1D461) +u1D450u1D43F0(u1D461))
u1D450u1D45Cu1D460(u1D719u1D44F)u1D450?u1D45A(u1D461)
?sin(u1D719u1D44F)u1D450?u1D45A(u1D461)
?
??
??
?
?
??
??
?
+
?
??
??
?
0
u1D45Fu1D465u1D437u1D45Fu1D452u1D453 sin(u1D452u1D703)u1D447(u1D461)
u1D45Fu1D465u1D437u1D45Fu1D452u1D453 sin(u1D452u1D713)cos(u1D452u1D703)u1D447(u1D461)
?
??
??
?
?
?
??
??
?
0
(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D45F
?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D45E
?
??
??
?
?
??
??
?
.
108
The model then becomes
?u1D714 =
?
??
??
?
u1D6FC1(u1D461)
u1D6FC2(u1D461)
u1D6FC3(u1D461)
?
??
??
?
+
?
??
??
?
1
u1D43Cu1D465u1D465(u1D461) 0 0
0 1u1D43Cu1D466u1D466(u1D461) 0
0 0 1u1D43Cu1D466u1D466(u1D461)
?
??
??
?
u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459. (5.4)
where
u1D6FC1(u1D461) = (u1D45Eu1D446u1D45Fu1D452u1D453u1D437u1D45Fu1D452u1D453u1D45Fu1D45Fu1D452u1D453u1D450u1D43Fu1D45D(u1D461)u1D45D+u1D45Eu1D446u1D45Fu1D452u1D453u1D437u1D45Fu1D452u1D453u1D452u1D453(u1D719u1D453u1D456u1D45Bu1D450u1D43Fu1D437(u1D461) +
u1D450u1D43F0(u1D461)))/u1D43Cu1D465u1D465(u1D461)
u1D6FC2(u1D461) = (u1D45Eu1D446u1D45Fu1D452u1D453u1D437u1D45Fu1D452u1D453(u1D45Fu1D45Fu1D452u1D453u1D450u1D45Au1D45Eu1D45F(u1D461)u1D45E+u1D450u1D45Cu1D460(u1D719u1D44F)u1D450?u1D45A)+u1D45Fu1D465u1D437u1D45Fu1D452u1D453 sin(u1D452u1D703)u1D447(u1D461)?
(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D45F)/u1D43Cu1D466u1D466(u1D461)
u1D6FC3(u1D461) = (u1D45Eu1D446u1D45Fu1D452u1D453u1D437u1D45Fu1D452u1D453(u1D45Fu1D45Fu1D452u1D453u1D450u1D45Au1D45Eu1D45F(u1D461)u1D45F?sin(u1D719u1D44F)u1D450?u1D45A) +u1D45Fu1D465u1D437u1D45Fu1D452u1D453 sin(u1D452u1D713)cos(u1D452u1D703)u1D447(u1D461) +
(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D45E)/u1D43Cu1D466u1D466(u1D461)
We make the following assumptions for the purposes of state estimation. First, due to the
available sensors used for this problem, the angle of attack u1D6FCcannot be determined. It is reasonable
to assume that the angle of attack is small, so it is assumed to bezero. Second, the motor is assumed
to be aligned properly so that u1D452u1D713 and u1D452u1D703 are both zero. When the ?ns are closed, u1D452u1D453 = 0, and the
model becomes
?u1D714 =
?
??
??
?
u1D45Eu1D446u1D45Fu1D452u1D453u1D437u1D45Fu1D452u1D453u1D45Fu1D45Fu1D452u1D453u1D450u1D43Fu1D45D(u1D461)u1D45D/u1D43Cu1D465u1D465(u1D461)
(u1D45Eu1D446u1D45Fu1D452u1D453u1D437u1D45Fu1D452u1D453u1D45Fu1D45Fu1D452u1D453u1D450u1D45Au1D45Eu1D45F(u1D461)u1D45E?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D45F)/u1D43Cu1D466u1D466(u1D461)
(u1D45Eu1D446u1D45Fu1D452u1D453u1D437u1D45Fu1D452u1D453u1D45Fu1D45Fu1D452u1D453u1D450u1D45Au1D45Eu1D45F(u1D461)u1D45F+(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D45E)/u1D43Cu1D466u1D466(u1D461)
?
??
??
?
+
?
??
??
?
1
u1D43Cu1D465u1D465(u1D461) 0 0
0 1u1D43Cu1D466u1D466(u1D461) 0
0 0 1u1D43Cu1D466u1D466(u1D461)
?
??
??
?
u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459. (5.5)
109
5.2 Pitch Rate and Yaw Rate Estimation
In this section the estimation of angular rates using only gyroscopes is considered. The output
from the gyros is modeled as
u1D714u1D460 = u1D714+u1D464u1D456 +u1D44Fu1D456 +u1D702u1D456 (5.6)
where u1D714u1D460 is the sensed angular rate, u1D714 is the actual angular rate, u1D464u1D456 is a walking bias, u1D44Fu1D456 is a
constant bias, and u1D456 = {u1D465,u1D466,u1D467}. The walking bias dynamics are modeled as a ?rst-order Markov
process
?u1D464u1D456 = u1D450u1D456u1D464u1D456 +u1D702u1D456
where u1D450u1D456 is a constant and u1D702u1D456 is zero-mean Gaussian noise. De?ne u1D70F as the time constant of the
Markov process. Then u1D450u1D456 = ?1u1D70F. With the assumption that the roll rate is a known function, the
system model from Equation (5.5) becomes a linear time-varying (LTV) system given by
?x =
?
??
??
??
??
u1D44E11(u1D461) u1D44E12(u1D461) 0 0
?u1D44E12(u1D461) u1D44E22(u1D461) 0 0
0 0 u1D450u1D466 0
0 0 0 u1D450u1D467
?
??
??
??
??
x+
?
??
??
??
??
1
u1D43Cu1D466u1D466(u1D461) 0
0 1u1D43Cu1D466u1D466(u1D461)
0 0
0 0
?
??
??
??
??
u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459(u1D461) +w(u1D461) (5.7)
y =
?
?? 1 0 1 0
0 1 0 1
?
??x+v(u1D461) (5.8)
where w(u1D461) ? u1D4A9(0,?u1D464) is process noise, x =
uni005B.alt03
u1D45E u1D45F u1D464u1D466 u1D464u1D467
uni005D.alt03u1D447
, y =
uni005B.alt03
u1D714u1D460u1D466 u1D714u1D460u1D467
uni005D.alt03u1D447
, v(u1D461) ?
u1D4A9(0,?u1D463) is sensor noise, and
u1D44E11(u1D461) = u1D45Eu1D446u1D45Fu1D452u1D453u1D437u1D45Fu1D452u1D453u1D45Fu1D45Fu1D452u1D453u1D450u1D45Au1D45Eu1D45F(u1D461)u1D45E/u1D43Cu1D466u1D466(u1D461)
u1D44E12(u1D461) = ?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D45F/u1D43Cu1D466u1D466(u1D461)
u1D44E22(u1D461) = u1D45Eu1D446u1D45Fu1D452u1D453u1D437u1D45Fu1D452u1D453u1D45Fu1D45Fu1D452u1D453u1D450u1D45Au1D45Eu1D45F(u1D461)u1D45F/u1D43Cu1D466u1D466(u1D461).
110
5.2.1 Kalman Filter
Let Equation (5.7) be given by
?u1D465= u1D434(u1D461)u1D465(u1D461) +u1D435(u1D461)u1D462(u1D461) +u1D464(u1D461). (5.9)
Since the system is LTV, the states can be estimated with a time-varying Kalman ?lter [61]. The
Kalman ?lter is run in discrete time so the matrices u1D434 and u1D435 must be converted to discrete time
via the matrix exponential [62]. To generate time-varying model lookup tables we compute the
matrix exponential o?-line ?
?? u1D439 u1D43A
0 u1D43C
?
??= exp
?
??
?
?? u1D434 u1D435
0 0
?
??u1D447
u1D460
?
?? (5.10)
where u1D447u1D460 is sampling time. The Kalman ?lter then has the form
u1D465[u1D458?u1D458] = u1D465[u1D458?u1D458?1] +u1D43F(u1D458)(u1D466(u1D458)?u1D436u1D465[u1D458?u1D458?1]) (5.11)
u1D465[u1D458+1?u1D458] = u1D439(u1D458)u1D465[u1D458?u1D458] +u1D43A(u1D458)(u1D462(u1D458) +u1D464(u1D458)) (5.12)
whereu1D439(u1D458) ? ?4?4, u1D43A(u1D458),u1D43F(u1D458) ? ?4?2 are gains calculated o?-line, and u1D462(u1D458) is the thrust provided
by the control thrusters. The vector u1D464(u1D458) represents the moment contribution due to aerodynamic
forces that arise due to angle of attack and is set to zero. The o?-line design procedure schedules
the gains every 0.01 s for the ?rst two seconds of ?ight, after which the system becomes inactive.
There are a total of 32 lookup tables involved in these coe?cients plus one more for estimation of
pyrotechnic thruster moments.
5.3 State Estimation with a Direction Vector
In this section we expand the system model in order to permit the estimation of angular
position in addition to roll, pitch, and yaw rates. The angular position is determined from a sensor
that measures a vector in the body frame that is ?xed in the inertial frame.
111
Angular position can be represented with either Euler angles or quaternions. Euler angle
dynamics are given by ?
??
??
?
?u1D719
?u1D703
?u1D713
?
??
??
?
=
?
??
??
?
1 u1D460u1D719u1D460u1D703/u1D450u1D703 u1D450u1D719u1D460u1D703/u1D450u1D703
0 u1D450u1D719 ?u1D460u1D719
0 u1D460u1D719/u1D450u1D703 u1D450u1D719/u1D450u1D703
?
??
??
?
u1D714
where u1D460u1D456 and u1D450u1D456 are sin(u1D456) and cos(u1D456), respectively. Quaternion dynamics are given by
u1D451u1D45E
u1D451u1D461 =
1
2
?
??
??
??
??
0 u1D45F ?u1D45E u1D45D
?u1D45F 0 u1D45D u1D45E
u1D45E ?u1D45D 0 u1D45F
?u1D45D ?u1D45E ?u1D45F 0
?
??
??
??
??
?
??
??
??
??
u1D45Eu1D456
u1D45Eu1D457
u1D45Eu1D458
u1D45Eu1D45F
?
??
??
??
??
.
An attitude quaternion that rotates from a ?xed frame to a rotating frame can be converted to
corresponding Euler angles with the following equations
?
??
??
?
u1D719
u1D703
u1D713
?
??
??
?
=
?
??
??
?
arctan
uni0028.alt022(u1D45E
u1D45Fu1D45Eu1D456+u1D45Eu1D457u1D45Eu1D458)
2(u1D45E2u1D45F+u1D45E2u1D458)?1
uni0029.alt02
arcsin(2(u1D45Eu1D45Fu1D45Eu1D457 ?u1D45Eu1D456u1D45Eu1D458))
arctan
uni0028.alt022(u1D45E
u1D45Fu1D45Eu1D458+u1D45Eu1D456u1D45Eu1D457)
2(u1D45E2u1D45F+u1D45E2u1D456 )?1
uni0029.alt02
?
??
??
?
.
It is no longer possible to cast the system as linear when angular position is included in the
model; it is a nonlinear, time-varying system.
112
?xu1D438 =
?
??
??
??
??
??
??
??
??
??
??
??
??
?
u1D450u1D450u1D43F(u1D461)
u1D43Cu1D465u1D465(u1D461) 0 0 0 0 0 0 0 0
0 u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) ?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0
0 (u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0
1 u1D460u1D719u1D460u1D703/u1D450u1D703 u1D450u1D719u1D460u1D703/u1D460u1D703 0 0 0 0 0 0
0 u1D450u1D719 ?u1D460u1D719 0 0 0 0 0 0
0 u1D460u1D719/u1D450u1D703 u1D450u1D719/u1D450u1D703 0 0 0 0 0 0
0 0 0 0 0 0 u1D450u1D465 0 0
0 0 0 0 0 0 0 u1D450u1D466 0
0 0 0 0 0 0 0 0 u1D450u1D467
?
??
??
??
??
??
??
??
??
??
??
??
??
?
xu1D438 +
?
??
??
??
??
??
??
??
??
??
??
??
??
?
0 0
1
u1D43Cu1D466u1D466(u1D461) 0
0 1u1D43Cu1D466u1D466(u1D461)
0 0
0 0
0 0
0 0
0 0
0 0
?
??
??
??
??
??
??
??
??
??
??
??
??
?
u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459 +w(u1D461) (5.13)
yu1D438,u1D444 =
?
??
??
??
??
??
??
??
?
u1D45D+u1D464u1D465
u1D45E+u1D464u1D466
u1D45F+u1D464u1D467
u1D450u1D713u1D450u1D703u1D43Bu1D43Cu1D465 +u1D460u1D713u1D450u1D703u1D43Bu1D43Cu1D466 ?u1D460u1D703u1D43Bu1D43Cu1D467
(u1D450u1D713u1D460u1D703u1D460u1D719?u1D460u1D713u1D450u1D719)u1D43Bu1D43Cu1D465 +(u1D460u1D713u1D460u1D703u1D460u1D719 +u1D450u1D713u1D450u1D719)u1D43Bu1D43Cu1D466 +u1D450u1D703u1D460u1D719u1D43Bu1D43Cu1D467
(u1D450u1D713u1D460u1D703u1D450u1D719 +u1D460u1D713u1D460u1D719)u1D43Bu1D43Cu1D465 +(u1D460u1D713u1D460u1D703u1D450u1D719 ?u1D450u1D713u1D460u1D719)u1D43Bu1D43Cu1D466 +u1D450u1D703u1D460u1D719u1D43Bu1D43Cu1D467
?
??
??
??
??
??
??
??
?
+v(u1D461) (5.14)
11
3
or
?xu1D438 = u1D434(x,u1D461)x+u1D435(u1D461)u+w(u1D461)
yu1D438,u1D444 = ?(x,u1D461) +v(u1D461)
where u1D450= u1D45Eu1D446u1D45Fu1D452u1D453u1D437u1D45Fu1D452u1D453u1D45Fu1D45Fu1D452u1D453,
xu1D438 =
uni005B.alt03
u1D45D u1D45E u1D45F u1D719 u1D703 u1D713 u1D464u1D465 u1D464u1D466 u1D464u1D467
uni005D.alt03u1D447
,
and
yu1D438,u1D444 =
uni005B.alt03
u1D714u1D460u1D465 u1D714u1D460u1D466 u1D714u1D460u1D467 u1D43Bu1D435u1D465 u1D43Bu1D435u1D466 u1D43Bu1D435u1D467
uni005D.alt03u1D447
+v(u1D461).
u1D43Bu1D435u1D456 is the measured direction vector in the body frame, and u1D43Bu1D43Cu1D456 is the corresponding vector in the
inertial frame. Likewise, the quaternion formulation of the system is
114
?xu1D444 =
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
u1D450u1D450u1D43Fu1D45D(u1D461)
u1D43Cu1D465u1D465(u1D461) 0 0 0 0 0 0 0 0 0
0 u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) ?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0
0 (u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0
0 0 0 0 u1D45F2 ?u1D45E2 u1D45D2 0 0 0
0 0 0 ?u1D45F2 0 u1D45D2 u1D45E2 0 0 0
0 0 0 u1D45E2 ?u1D45D2 0 u1D45F2 0 0 0
0 0 0 ?u1D45D2 ?u1D45E2 ?u1D45F2 0 0 0 0
0 0 0 0 0 0 0 u1D450u1D465 0 0
0 0 0 0 0 0 0 0 u1D450u1D466 0
0 0 0 0 0 0 0 0 0 u1D450u1D467
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
xu1D444 +
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
0 0
1
u1D43Cu1D466u1D466(u1D461) 0
0 1u1D43Cu1D466u1D466(u1D461)
0 0
0 0
0 0
0 0
0 0
0 0
0 0
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459 +w(u1D461) (5.15)
u1D466u1D438,u1D444 =
?
??
??
??
??
??
??
??
?
1 0 0 0 0 0 0 1 0 0
0 1 0 0 0 0 0 0 1 0
0 0 1 0 0 0 0 0 0 1
0 0 0 u1D6FD u1D6FEu1D43Bu1D43Cu1D467 ?u1D6FEu1D43Bu1D43Cu1D466 0 0 0 0
0 0 0 ?u1D6FEu1D43Bu1D43Cu1D467 u1D6FD u1D6FEu1D43Bu1D43Cu1D465 0 0 0 0
0 0 0 u1D6FEu1D43Bu1D43Cu1D466 ?u1D6FEu1D43Bu1D43Cu1D465 u1D6FD 0 0 0 0
?
??
??
??
??
??
??
??
?
xu1D444 +cos(2cos?1(u1D45Eu1D44Fu1D456,u1D45F))
?
??
??
??
??
??
??
??
?
0
0
0
u1D43Bu1D43Cu1D465
u1D43Bu1D43Cu1D466
u1D43Bu1D43Cu1D467
?
??
??
??
??
??
??
??
?
+v(u1D461) (5.16)
11
5
where
u1D6FD = ? 1sin2(cos?1(u1D45E
u1D44Fu1D456,u1D45F))
(u1D43Bu1D43Cu1D465u1D45Eu1D44Fu1D456,u1D456 +u1D43Bu1D43Cu1D466u1D45Eu1D44Fu1D456,u1D457 +u1D43Bu1D43Cu1D467u1D45Eu1D44Fu1D456,u1D458)(1?cos(2cos?1(u1D45Eu1D44Fu1D456,u1D45F)))
u1D6FE = 1sin(cos?1(u1D45Eu1D44Fu1D456,u1D45F))sin(2cos?1(u1D45Eu1D44Fu1D456,u1D45F))
and xu1D444 =
uni005B.alt03
u1D45D u1D45E u1D45F u1D45Eu1D456u1D44F,u1D456 u1D45Eu1D456u1D44F,u1D457 u1D45Eu1D456u1D44F,u1D458 u1D45Eu1D456u1D44F,u1D45F u1D464u1D465 u1D464u1D466 u1D464u1D467
uni005D.alt03u1D447
. The quaternion subscripts u1D44Fu1D456 and
u1D456u1D44F denote rotations from the body to inertial frame and inertial to body frame, respectively. These
two quaternions are related by u1D45Eu1D456u1D44F = u1D45E?u1D44Fu1D456 (see ?A.2).
By using the Pythagorean theorem and some trigonometric identities, the quaternion equations
can be reformulated to remove all of the trigonometric functions. Since the quaternion used in the
system model is an attitude quaternion, it has a magnitude of unity or
u1D45E2u1D456u1D44F,u1D456 +u1D45E2u1D456u1D44F,u1D457 +u1D45E2u1D456u1D44F,u1D458 +u1D45E2u1D456u1D44F,u1D45F = u1D45E2u1D44Fu1D456,u1D456 +u1D45Eu1D44Fu1D456,u1D4572 +u1D45E2u1D44Fu1D456,u1D458 +u1D45E2u1D44Fu1D456,u1D45F = 1. (5.17)
This relationship is shown in Figure 5.1 where u1D44E =
uni221A.alt02
u1D45E2u1D456u1D44F,u1D456 +u1D45E2u1D456u1D44F,u1D457 +u1D45E2u1D456u1D44F,u1D458 and u1D44F = u1D45Eu1D456u1D44F,u1D45F. The results
are the same for u1D45Eu1D44Fu1D456. By the Pythagorean theorem u1D717 = cos?1(u1D44F) = sin?1(u1D44E) or u1D717 = cos?1(u1D45Eu1D456u1D44F,u1D45F) =
Figure 5.1: Relationship Among Rotation Quaternion Components
116
sin?1
uni0028.alt02uni221A.alt02
u1D45E2u1D456u1D44F,u1D456 +u1D45E2u1D456u1D44F,u1D457 +u1D45E2u1D456u1D44F,u1D458
uni0029.alt02
. Then using the identities
sin2(u1D717) = 12 ? 12 cos(2u1D717)
sin(2u1D717) = 2sin(u1D717)cos(u1D717)
cos(2u1D717) = 1?2sin2(u1D717),
u1D43Bu1D435u1D465 = 1?cos(2u1D717sin2(u1D717) (u1D45Eu1D456u1D44F,u1D456u1D43Bu1D43Cu1D465 +u1D45Eu1D456u1D44F,u1D457u1D43Bu1D43Cu1D466 +u1D45Eu1D456u1D44F,u1D458u1D43Bu1D43Cu1D467)u1D45Eu1D456u1D44F,u1D456 + sin(2u1D717)sin(u1D717) u1D43Bu1D43Cu1D467u1D45Eu1D456u1D44F,u1D457 ?
sin(2u1D717)
sin(u1D717) u1D43B
u1D43C
u1D466u1D45Eu1D456u1D44F,u1D458 +(2cos
2(u1D717)?1)u1D43Bu1D43C
u1D466
u1D43Bu1D435u1D466 = ?sin(2u1D717)sin(u1D717) u1D43Bu1D43Cu1D467u1D45Eu1D456u1D44F,u1D456 + 1?cos(2u1D717sin2(u1D717) (u1D45Eu1D456u1D44F,u1D456u1D43Bu1D43Cu1D465 +u1D45Eu1D456u1D44F,u1D457u1D43Bu1D43Cu1D466 +u1D45Eu1D456u1D44F,u1D458u1D43Bu1D43Cu1D467)u1D45Eu1D456u1D44F,u1D457 +
sin(2u1D717)
sin(u1D717) u1D43B
u1D43C
u1D465u1D45Eu1D456u1D44F,u1D458 +(2cos
2(u1D717)?1)u1D43Bu1D43C
u1D466
u1D43Bu1D435u1D467 = sin(2u1D717)sin(u1D717) u1D43Bu1D43Cu1D466u1D45Eu1D456u1D44F,u1D456? sin(2u1D717)sin(u1D717) u1D43Bu1D43Cu1D465u1D45Eu1D456u1D44F,u1D457 +
1?cos(2u1D717
sin2(u1D717) (u1D45Eu1D456u1D44F,u1D456u1D43B
u1D43C
u1D465 +u1D45Eu1D456u1D44F,u1D457u1D43B
u1D43C
u1D466 +u1D45Eu1D456u1D44F,u1D458u1D43B
u1D43C
u1D467)u1D45Eu1D456u1D44F,u1D458 +(2cos
2(u1D717)?1)u1D43Bu1D43C
u1D467
Then using the fact that sin(u1D703) =
uni221A.alt02
u1D45E2u1D456u1D44F,u1D456 +u1D45E2u1D456u1D44F,u1D457 +u1D45E2u1D456u1D44F,u1D458 and cos(u1D703) = u1D45Eu1D456u1D44F,u1D45F,
u1D43Bu1D435u1D465 = 2(u1D45Eu1D456u1D44F,u1D456u1D43Bu1D43Cu1D465 +u1D45Eu1D456u1D44F,u1D457u1D43Bu1D43Cu1D466 +u1D45Eu1D456u1D44F,u1D458u1D43Bu1D43Cu1D467)u1D45Eu1D456u1D44F,u1D456 +2u1D45Eu1D456u1D44F,u1D45Fu1D43Bu1D43Cu1D467u1D45Eu1D456u1D44F,u1D457 +2u1D45Eu1D456u1D44F,u1D45Fu1D43Bu1D43Cu1D466u1D45Eu1D456u1D44F,u1D458 ?
(2u1D45E2u1D456u1D44F,u1D45F ?1)u1D43Bu1D43Cu1D465 (5.18a)
u1D43Bu1D435u1D466 = ?2u1D45Eu1D456u1D44F,u1D45Fu1D43Bu1D43Cu1D467u1D45Eu1D456u1D44F,u1D456 +2(u1D45Eu1D456u1D44F,u1D456u1D43Bu1D43Cu1D465 +u1D45Eu1D456u1D44F,u1D457u1D43Bu1D43Cu1D466 +u1D45Eu1D456u1D44F,u1D458u1D43Bu1D43Cu1D467)u1D45Eu1D456u1D44F,u1D457 +2u1D45Eu1D456u1D44F,u1D45Fu1D43Bu1D43Cu1D465u1D45Eu1D456u1D44F,u1D458 +
(2u1D45E2u1D456u1D44F,u1D45F ?1)u1D43Bu1D43Cu1D466 (5.18b)
u1D43Bu1D435u1D467 = 2u1D45Eu1D456u1D44F,u1D45Fu1D43Bu1D43Cu1D466u1D45Eu1D456u1D44F,u1D456 ?2u1D45Eu1D456u1D44F,u1D45Fu1D43Bu1D43Cu1D465u1D45Eu1D456u1D44F,u1D457 +2(u1D45Eu1D456u1D44F,u1D456u1D43Bu1D43Cu1D465 +u1D45Eu1D456u1D44F,u1D457u1D43Bu1D43Cu1D466 +u1D45Eu1D456u1D44F,u1D458u1D43Bu1D43Cu1D467)u1D45Eu1D456u1D44F,u1D458 +
(2u1D45E2u1D456u1D44F,u1D45F ?1)u1D43Bu1D43Cu1D467 (5.18c)
117
Then
u1D6FE = 2u1D45Eu1D456u1D44F,u1D45F (5.19a)
u1D6FD = 2(u1D45Eu1D456u1D44F,u1D456u1D43Bu1D43Cu1D465 +u1D45Eu1D456u1D44F,u1D457u1D43Bu1D43Cu1D466 +u1D45Eu1D456u1D44F,u1D458u1D43Bu1D43Cu1D467). (5.19b)
The output equation is thus
yu1D438,u1D444 =
?
??
??
??
??
??
??
??
?
1 0 0 0 0 0 0 1 0 0
0 1 0 0 0 0 0 0 1 0
0 0 1 0 0 0 0 0 0 1
0 0 0 u1D6FD u1D6FEu1D43Bu1D43Cu1D467 ?u1D6FEu1D43Bu1D43Cu1D466 0 0 0 0
0 0 0 ?u1D6FEu1D43Bu1D43Cu1D467 u1D6FD u1D6FEu1D43Bu1D43Cu1D465 0 0 0 0
0 0 0 u1D6FEu1D43Bu1D43Cu1D466 ?u1D6FEu1D43Bu1D43Cu1D465 u1D6FD 0 0 0 0
?
??
??
??
??
??
??
??
?
xu1D444+(2u1D45E2u1D456u1D44F,u1D45F?1)
?
??
??
??
??
??
??
??
?
0
0
0
u1D43Bu1D43Cu1D465
u1D43Bu1D43Cu1D466
u1D43Bu1D43Cu1D467
?
??
??
??
??
??
??
??
?
+v(u1D461). (5.20)
Extended Kalman Filter (EKF)
Implementation of an EKF requires the availability of a linearized model of the nonlinear
systems given by Equations (5.13)-(5.14) and Equations (5.15)-(5.16). To linearize the output
equations, partial derivatives of Equations (5.18a)-(5.18c) are necessary
?u1D43Bu1D435u1D465
?u1D45Eu1D456u1D44F,u1D456 = 2u1D45Eu1D456u1D44F,u1D456u1D43B
u1D43C
u1D465 +2(u1D45Eu1D456u1D44F,u1D456u1D43B
u1D43C
u1D465 +u1D45Eu1D456u1D44F,u1D457u1D43B
u1D43C
u1D466 +u1D45Eu1D456u1D44F,u1D458u1D43B
u1D43C
u1D467) (5.21)
?u1D43Bu1D435u1D465
?u1D45Eu1D456u1D44F,u1D457 = 2u1D45Eu1D456u1D44F,u1D456u1D43B
u1D43C
u1D466 +2u1D45Eu1D456u1D44F,u1D45Fu1D43B
u1D43C
u1D467 (5.22)
?u1D43Bu1D435u1D465
?u1D45Eu1D456u1D44F,u1D458 = 2u1D45Eu1D456u1D44F,u1D456u1D43B
u1D43C
u1D467 ?2u1D45Eu1D456u1D44F,u1D45Fu1D43B
u1D43C
u1D466 (5.23)
?u1D43Bu1D435u1D465
?u1D45Eu1D456u1D44F,u1D45F = 2u1D45Eu1D456u1D44F,u1D457u1D43B
u1D43C
u1D467 ?2u1D45Eu1D456u1D44F,u1D458u1D43B
u1D43C
u1D466 +4u1D45Eu1D456u1D44F,u1D45Fu1D43B
u1D43C
u1D465 (5.24)
118
?u1D43Bu1D435u1D466
?u1D45Eu1D456u1D44F,u1D456 = ?2u1D45Eu1D456u1D44F,u1D45Fu1D43B
u1D43C
u1D467 +2u1D45Eu1D456u1D44F,u1D457u1D43B
u1D43C
u1D465 (5.25)
?u1D43Bu1D435u1D466
?u1D45Eu1D456u1D44F,u1D457 = 2u1D45Eu1D456u1D44F,u1D457u1D43B
u1D43C
u1D466 +2(u1D45Eu1D456u1D44F,u1D456u1D43B
u1D43C
u1D465 +u1D45Eu1D456u1D44F,u1D457u1D43B
u1D43C
u1D466 +u1D45Eu1D456u1D44F,u1D458u1D43B
u1D43C
u1D467) (5.26)
?u1D43Bu1D435u1D466
?u1D45Eu1D456u1D44F,u1D458 = 2u1D45Eu1D456u1D44F,u1D457u1D43B
u1D43C
u1D467 +2u1D45Eu1D456u1D44F,u1D45Fu1D43B
u1D43C
u1D465 (5.27)
?u1D43Bu1D435u1D466
?u1D45Eu1D456u1D44F,u1D45F = ?2u1D45Eu1D456u1D44F,u1D456u1D43B
u1D43C
u1D467 +2u1D45Eu1D456u1D44F,u1D458u1D43B
u1D43C
u1D465 +4u1D45Eu1D456u1D44F,u1D45Fu1D43B
u1D43C
u1D466 (5.28)
?u1D43Bu1D435u1D467
?u1D45Eu1D456u1D44F,u1D456 = 2u1D45Eu1D456u1D44F,u1D45Fu1D43B
u1D43C
u1D466 +2u1D45Eu1D456u1D44F,u1D458u1D43B
u1D43C
u1D465 (5.29)
?u1D43Bu1D435u1D467
?u1D45Eu1D456u1D44F,u1D457 = ?2u1D45Eu1D456u1D44F,u1D45Fu1D43B
u1D43C
u1D465 +2u1D45Eu1D456u1D44F,u1D458u1D43B
u1D43C
u1D466 (5.30)
?u1D43Bu1D435u1D467
?u1D45Eu1D456u1D44F,u1D458 = 2u1D45Eu1D456u1D44F,u1D458u1D43B
u1D43C
u1D467 +2(u1D45Eu1D456u1D44F,u1D456u1D43B
u1D43C
u1D465 +u1D45Eu1D456u1D44F,u1D457u1D43B
u1D43C
u1D466 +u1D45Eu1D456u1D44F,u1D458u1D43B
u1D43C
u1D467) (5.31)
?u1D43Bu1D435u1D467
?u1D45Eu1D456u1D44F,u1D45F = 2u1D45Eu1D456u1D44F,u1D456u1D43B
u1D43C
u1D466 ?2u1D45Eu1D456u1D44F,u1D457u1D43B
u1D43C
u1D465 +4u1D45Eu1D456u1D44F,u1D45Fu1D43B
u1D43C
u1D467. (5.32)
Equation (5.5) can be written as
?u1D714 =
?
??
??
?
u1D4531(u1D465,u1D461)
u1D4532(u1D465,u1D461)
u1D4533(u1D465,u1D461)
?
??
??
?
+u1D435(u1D461)u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459. (5.33)
u1D435(u1D461) is already linear so no action is necessary. The remainder of the equation is linearized as
follows
?u1D4531
?u1D45D = u1D450u1D450u1D43Fu1D45D(u1D461)/u1D43Cu1D465u1D465(u1D461)
?u1D4531
?u1D45E = 0
?u1D4531
?u1D45F = 0
?u1D4532
?u1D45D = ?
(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45F
u1D43Cu1D466u1D466(u1D461)
?u1D4532
?u1D45E = u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)/u1D43Cu1D466u1D466(u1D461)
?u1D4532
?u1D45F = ?
(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45D
u1D43Cu1D466u1D466(u1D461)
?u1D4533
?u1D45D = ?
(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45E
u1D43Cu1D466u1D466(u1D461)
?u1D4533
?u1D45E = ?
(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45D
u1D43Cu1D466u1D466(u1D461)
?u1D4533
?u1D45F = u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)/u1D43Cu1D466u1D466(u1D461).
Then the system model becomes
119
?xu1D444 =
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
u1D450u1D450u1D43Fu1D45D(u1D461)
u1D43Cu1D465u1D465(u1D461) 0 0 0 0 0 0 0 0 0
?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D43Cu1D466u1D466(u1D461) u1D45F u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) ?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D43Cu1D466u1D466(u1D461) u1D45D 0 0 0 0 0 0 0
?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D43Cu1D466u1D466(u1D461) u1D45E ?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D43Cu1D466u1D466(u1D461) u1D45F u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0
0 0 0 0 u1D45F2 ?u1D45E2 u1D45D2 0 0 0
0 0 0 ?u1D45F2 0 u1D45D2 u1D45E2 0 0 0
0 0 0 u1D45E2 ?u1D45D2 0 u1D45F2 0 0 0
0 0 0 ?u1D45D2 ?u1D45E2 ?u1D45F2 0 0 0 0
0 0 0 0 0 0 0 u1D450u1D465 0 0
0 0 0 0 0 0 0 0 u1D450u1D466 0
0 0 0 0 0 0 0 0 0 u1D450u1D467
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
xu1D444 +
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
0 0
1
u1D43Cu1D466u1D466(u1D461) 0
0 1u1D43Cu1D466u1D466(u1D461)
0 0
0 0
0 0
0 0
0 0
0 0
0 0
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459
+w(u1D461)
yu1D438,u1D444 =
?
??
??
??
??
??
??
??
?
1 0 0 0 0 0 0 1 0 0
0 1 0 0 0 0 0 0 1 0
0 0 1 0 0 0 0 0 0 1
0 0 0 ?u1D43Bu1D435u1D465?u1D45Eu1D456u1D44F,u1D456 ?u1D43Bu1D435u1D465?u1D45Eu1D456u1D44F,u1D457 ?u1D43Bu1D435u1D465?u1D45Eu1D456u1D44F,u1D458 ?u1D43Bu1D435u1D465?u1D45Eu1D456u1D44F,u1D45F 0 0 0
0 0 0 ?u1D43Bu1D435u1D466?u1D45Eu1D456u1D44F,u1D456 ?u1D43Bu1D435u1D466?u1D45Eu1D456u1D44F,u1D457 ?u1D43Bu1D435u1D466?u1D45Eu1D456u1D44F,u1D458 ?u1D43Bu1D435u1D466?u1D45Eu1D456u1D44F,u1D45F 0 0 0
0 0 0 ?u1D43Bu1D435u1D467?u1D45Eu1D456u1D44F,u1D456 ?u1D43Bu1D435u1D467?u1D45Eu1D456u1D44F,u1D457 ?u1D43Bu1D435u1D467?u1D45Eu1D456u1D44F,u1D458 ?u1D43Bu1D435u1D467?u1D45Eu1D456u1D44F,u1D45F 0 0 0
?
??
??
??
??
??
??
??
?
xu1D444 +v(u1D461)
12
0
or
?xu1D444 = u1D434u1D438xu1D444 +u1D435u+w
yu1D438,u1D444 = u1D436u1D438xu1D444 +v
where v is zeros for the elements corresponding to the ideal vector output. The matrices u1D434u1D438, u1D435,
and u1D436u1D438 are formed by evaluating each entry at a speci?c time value and the state estimate at the
previous time step.
5.3.1 Ideal Vector
One method to estimate the rocket?s attitude is to have a sensor that measures a vector in the
body frame that is known in the inertial frame. The measurement taken in the body frame can be
rotated to match the the known vector in the inertial frame to determine the rocket?s attitude. In
this section the ideal case where a known unit vector u1D43Bu1D43C =
uni005B.alt03
1 0 0
uni005D.alt03u1D447
pointing in the x-direction
in the inertial frame is considered. Although any known vector in the inertial frame is valid, the
x-direction unit vector is used for simplicity.
With u1D43Bu1D43Cu1D465 = 1 and u1D43Bu1D43Cu1D466 = u1D43Bu1D43Cu1D467 = 0, Equations (5.18a)-(5.18c) become
u1D43Bu1D435u1D465 = 2u1D45E2u1D456u1D44F,u1D456 +2u1D45E2u1D456u1D44F,u1D45F ?1
u1D43Bu1D435u1D466 = 2u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D457 +2u1D45Eu1D456u1D44F,u1D458u1D45Eu1D456u1D44F,u1D45F
u1D43Bu1D435u1D467 = 2u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458 ?2u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457.
Then the coe?cients u1D6FE and u1D6FD of the system output u1D466u1D438,u1D444 (Equation (5.20)) are
u1D6FE = 2u1D45Eu1D456u1D44F,u1D45F
u1D6FD = 2u1D45Eu1D456u1D44F,u1D456.
121
The output equation u1D466u1D438,u1D444 is then
u1D466u1D438,u1D444 =
?
??
??
??
??
??
??
??
?
1 0 0 0 0 0 0 1 0 0
0 1 0 0 0 0 0 0 1 0
0 0 1 0 0 0 0 0 0 1
0 0 0 u1D6FD 0 0 u1D6FE 0 0 0
0 0 0 0 u1D6FD u1D6FE 0 0 0 0
0 0 0 0 ?u1D6FE u1D6FD 0 0 0 0
?
??
??
??
??
??
??
??
?
u1D465u1D444 ?
?
??
??
??
??
??
??
??
?
0
0
0
u1D43Bu1D43Cu1D465
u1D43Bu1D43Cu1D466
u1D43Bu1D43Cu1D467
?
??
??
??
??
??
??
??
?
.
The linearized equations used for an EKF are then
?u1D43Bu1D435u1D465
?u1D45Eu1D456u1D44F,u1D456 = 4u1D45Eu1D456u1D44F,u1D456
?u1D43Bu1D435u1D465
?u1D45Eu1D456u1D44F,u1D457 = 0
?u1D43Bu1D435u1D465
?u1D45Eu1D456u1D44F,u1D458 = 0
?u1D43Bu1D435u1D465
?u1D45Eu1D456u1D44F,u1D45F = 4u1D45Eu1D456u1D44F,u1D45F
?u1D43Bu1D435u1D466
?u1D45Eu1D456u1D44F,u1D456 = 2u1D45Eu1D456u1D44F,u1D457
?u1D43Bu1D435u1D466
?u1D45Eu1D456u1D44F,u1D457 = 2u1D45Eu1D456u1D44F,u1D456
?u1D43Bu1D435u1D466
?u1D45Eu1D456u1D44F,u1D458 = 2u1D45Eu1D456u1D44F,u1D45F
?u1D43Bu1D435u1D466
?u1D45Eu1D456u1D44F,u1D45F = 2u1D45Eu1D456u1D44F,u1D458
?u1D43Bu1D435u1D467
?u1D45Eu1D456u1D44F,u1D456 = ?2u1D45Eu1D456u1D44F,u1D458
?u1D43Bu1D435u1D467
?u1D45Eu1D456u1D44F,u1D457 = 2u1D45Eu1D456u1D44F,u1D45F
?u1D43Bu1D435u1D467
?u1D45Eu1D456u1D44F,u1D458 = ?2u1D45Eu1D456u1D44F,u1D456
?u1D43Bu1D435u1D467
?u1D45Eu1D456u1D44F,u1D45F = 2u1D45Eu1D456u1D44F,u1D457.
5.3.2 Magnetometer Measurements
The ideal vector situation represents the best possible scenario, but a magnetometer gives
realistic performance. The Earth?s magnetic ?eld is ?xed in the inertial frame, so a magnetometer
measuring the ?eld in the body frame can be used to determine attitude. However, the Earth?s
magnetic ?eld is not constant, as discussed in Chapter 3. In this section, for the short time intervals
of the estimation process, the magnetic ?eld can be approximated as being constant. The magnetic
?eld is determined by taking measurements of the magnetic ?eld with a magnetometer attached to
the rocket prior to the launch.
The magnetometer sensor readings are modeled as
u1D43Bu1D435u1D460u1D456 = u1D43Bu1D435u1D456 +u1D464u1D45Au1D456 +u1D44Fu1D45Au1D456 +u1D702u1D45Au1D456 (5.34)
122
where u1D456 = {u1D465,u1D466,u1D467}, u1D702u1D456 is zero-mean white noise, and u1D44Fu1D456 is a constant bias. u1D464u1D45Au1D456 is a walking bias
modeled as a ?rst order Markov process with dynamics
?u1D464u1D45Au1D456 = u1D450u1D45Au1D456u1D464u1D45Au1D456 +u1D702u1D456. (5.35)
Then with a magnetometer and rate gyros, the system model is
123
?xu1D438 =
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
?
u1D450u1D450u1D43F(u1D461)
u1D43Cu1D465u1D465(u1D461) 0 0 0 0 0 0 0 0 0 0 0
0 u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) ?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0
0 (u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0
1 u1D460u1D719u1D460u1D703/u1D450u1D703 u1D450u1D719u1D460u1D703/u1D460u1D703 0 0 0 0 0 0 0 0 0
0 u1D450u1D719 ?u1D460u1D719 0 0 0 0 0 0 0 0 0
0 u1D460u1D719/u1D450u1D703 u1D450u1D719/u1D450u1D703 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 u1D450u1D465 0 0 0 0 0
0 0 0 0 0 0 0 u1D450u1D466 0 0 0 0
0 0 0 0 0 0 0 0 u1D450u1D467 0 0 0
0 0 0 0 0 0 0 0 0 u1D450u1D45Au1D465 0 0
0 0 0 0 0 0 0 0 0 0 u1D450u1D45Au1D466 0
0 0 0 0 0 0 0 0 0 0 0 u1D450u1D45Au1D467
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
?
xu1D438 +
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
?
0 0
1
u1D43Cu1D466u1D466(u1D461) 0
0 1u1D43Cu1D466u1D466(u1D461)
0 0
0 0
0 0
0 0
0 0
0 0
0 0
0 0
0 0
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
?
u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459
+ w(u1D461)
12
4
yu1D438,u1D444 =
?
??
??
??
??
??
??
??
?
u1D45D+u1D464u1D465
u1D45E+u1D464u1D466
u1D45F+u1D464u1D467
u1D450u1D713u1D450u1D703u1D43Bu1D43Cu1D465 +u1D460u1D713u1D450u1D703u1D43Bu1D43Cu1D466 ?u1D460u1D703u1D43Bu1D43Cu1D467 +u1D464u1D45Au1D465
(u1D450u1D713u1D460u1D703u1D460u1D719 ?u1D460u1D713u1D450u1D719)u1D43Bu1D43Cu1D465 +(u1D460u1D713u1D460u1D703u1D460u1D719 +u1D450u1D713u1D450u1D719)u1D43Bu1D43Cu1D466 +u1D450u1D703u1D460u1D719u1D43Bu1D43Cu1D467 +u1D464u1D45Au1D466
(u1D450u1D713u1D460u1D703u1D450u1D719 +u1D460u1D713u1D460u1D719)u1D43Bu1D43Cu1D465 +(u1D460u1D713u1D460u1D703u1D450u1D719 ?u1D450u1D713u1D460u1D719)u1D43Bu1D43Cu1D466 +u1D450u1D703u1D460u1D719u1D43Bu1D43Cu1D467 +u1D464u1D45Au1D467
?
??
??
??
??
??
??
??
?
u1D447
+v(u1D461)
where
xu1D438 =
uni005B.alt03
u1D45D u1D45E u1D45F u1D719 u1D703 u1D713 u1D464u1D465 u1D464u1D466 u1D464u1D467 u1D464u1D45Au1D465 u1D464u1D45Au1D466 u1D464u1D45Au1D467
uni005D.alt03u1D447
and
yu1D438,u1D444 =
uni005B.alt03
u1D714u1D460u1D465 u1D714u1D460u1D466 u1D714u1D460u1D467 u1D43Bu1D435u1D460u1D465 u1D43Bu1D435u1D460u1D466 u1D43Bu1D435u1D460u1D467
uni005D.alt03u1D447
+v(u1D461)
for the Euler angle formulation. For the quaternion formulation the system is
125
?xu1D444 =
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
u1D450u1D450u1D43Fu1D45D(u1D461)
u1D43Cu1D465u1D465(u1D461) 0 0 0 0 0 0 0 0 0 0 0 0
0 u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) ?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0
0 (u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0
0 0 0 0 u1D45F2 ?u1D45E2 u1D45D2 0 0 0 0 0 0
0 0 0 ?u1D45F2 0 u1D45D2 u1D45E2 0 0 0 0 0 0
0 0 0 u1D45E2 ?u1D45D2 0 u1D45F2 0 0 0 0 0 0
0 0 0 ?u1D45D2 ?u1D45E2 ?u1D45F2 0 0 0 0 0 0 0
0 0 0 0 0 0 0 u1D450u1D465 0 0 0 0 0
0 0 0 0 0 0 0 0 u1D450u1D466 0 0 0 0
0 0 0 0 0 0 0 0 0 u1D450u1D467 0 0 0
0 0 0 0 0 0 0 0 0 0 u1D450u1D45Au1D465 0 0
0 0 0 0 0 0 0 0 0 0 0 u1D450u1D45Au1D466 0
0 0 0 0 0 0 0 0 0 0 0 0 u1D450u1D45Au1D467
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
xu1D444 + (5.36)
?
?? 0 1u1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0
0 0 1u1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0
?
??
u1D447
u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459 +w(u1D461) (5.37)
12
6
yu1D438,u1D444 =
?
??
??
??
??
??
??
??
?
1 0 0 0 0 0 0 1 0 0 0 0 0
0 1 0 0 0 0 0 0 1 0 0 0 0
0 0 1 0 0 0 0 0 0 1 0 0 0
0 0 0 u1D6FD u1D6FEu1D43Bu1D43Cu1D467 ?u1D6FEu1D43Bu1D43Cu1D466 0 0 0 0 1 0 0
0 0 0 ?u1D6FEu1D43Bu1D43Cu1D467 u1D6FD u1D6FEu1D43Bu1D43Cu1D465 0 0 0 0 0 1 0
0 0 0 u1D6FEu1D43Bu1D43Cu1D466 ?u1D6FEu1D43Bu1D43Cu1D465 u1D6FD 0 0 0 0 0 0 1
?
??
??
??
??
??
??
??
?
xu1D444 +
(2u1D45E2u1D456u1D44F,u1D45F ?1)
uni005B.alt03
0 0 0 u1D43Bu1D43Cu1D465 u1D43Bu1D43Cu1D466 u1D43Bu1D43Cu1D467
uni005D.alt03u1D447
+v(u1D461) (5.38)
where xu1D444 =
uni005B.alt03
u1D45D u1D45E u1D45F u1D45Eu1D456u1D44F,u1D456 u1D45Eu1D456u1D44F,u1D457 u1D45Eu1D456u1D44F,u1D458 u1D45Eu1D456u1D44F,u1D45F u1D464u1D465 u1D464u1D466 u1D464u1D467 u1D464u1D45Au1D465 u1D464u1D45Au1D466 u1D464u1D45Au1D467
uni005D.alt03u1D447
and u1D6FE and u1D6FD
are given by Equations (5.19a) and (5.19b).
5.4 Estimation with Rate and Angle Gyros
This section develops estimators for the rocket when the onboard sensors are rate gyros and
gyros that sense angular position directly. A rate gyro can be used to estimate angular position
by integrating the sensor output. But due to noise on the sensor output, integration causes the
angular position estimates to become increasingly inaccurate in a matter of milliseconds. Shkel and
Painter [63], [64], [65] have proposed a type of MEMS gyroscope (referred to in this paper as an
angle gyro) that directly measures rotational angles, thus eliminating the problem of integrating
noise associated with a typical MEMS gyro (referred to hence forward as a rate gyro). The angle
gyro is still in the developmental stages; however, it is still a MEMS device, and its output can be
modeled in the same manner as a rate gyro. A gyro that senses angular position directly avoids
the problem of integrating noise, thereby producing more accurate readings. Such gyro readings
127
are modeled as
u1D719u1D460 = u1D719+u1D464u1D719 +u1D44Fu1D719 +u1D702u1D719 (5.39)
u1D703u1D460 = u1D703+u1D464u1D703 +u1D44Fu1D703 +u1D702u1D703 (5.40)
u1D713u1D460 = u1D713+u1D464u1D713 +u1D44Fu1D713 +u1D702u1D713 (5.41)
where u1D719, u1D703, and u1D713 are Euler angles, u1D44Fu1D456 are constant biases, and u1D702u1D456 are zero-mean Gaussian noise.
u1D464u1D456 is a walking bias modeled as a ?rst-order Markov process with dynamics
?u1D464u1D456 = u1D450u1D456u1D464u1D456 +u1D702u1D456. (5.42)
The system can be modeled in terms of Euler angles as follows. The state vector is given by
xu1D438 =
uni005B.alt03
u1D45D u1D45E u1D45F u1D719 u1D703 u1D713 u1D464u1D465 u1D464u1D466 u1D464u1D467 u1D464u1D719 u1D464u1D703 u1D464u1D713
uni005D.alt03u1D447
,
and the output vector is given by
yu1D438,u1D444 =
uni005B.alt03
u1D714u1D460u1D465 u1D714u1D460u1D466 u1D714u1D460u1D467 u1D719u1D460 u1D703u1D460 u1D713u1D460
uni005D.alt03u1D447
+v(u1D461).
Then the system model is
128
?xu1D438 =
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
?
u1D450u1D450u1D43Fu1D45D(u1D461)
u1D43Cu1D465u1D465(u1D461) 0 0 0 0 0 0 0 0 0 0 0
0 u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) ?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0
0 u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) (u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0
1 u1D460u1D719u1D460u1D703u1D450u1D703 u1D450u1D719u1D460u1D703u1D450u1D703 0 0 0 0 0 0 0 0 0
0 u1D450u1D719 ?u1D460u1D719 0 0 0 0 0 0 0 0 0
0 u1D460u1D719u1D450u1D703 u1D450u1D719u1D450u1D703 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 u1D450u1D465 0 0 0 0 0
0 0 0 0 0 0 0 u1D450u1D466 0 0 0 0
0 0 0 0 0 0 0 0 u1D450u1D467 0 0 0
0 0 0 0 0 0 0 0 0 u1D450u1D719 0 0
0 0 0 0 0 0 0 0 0 0 u1D450u1D703 0
0 0 0 0 0 0 0 0 0 0 0 u1D450u1D713
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
?
xu1D438 +
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
?
0 0
1
u1D43Cu1D466u1D466(u1D461) 0
0 1u1D43Cu1D466u1D466(u1D461)
0 0
0 0
0 0
0 0
0 0
0 0
0 0
0 0
0 0
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
?
u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459 +w(u1D461) (5.43)
12
9
yu1D438,u1D444 =
uni005B.alt03
u1D43C6?6 u1D43C6?6
uni005D.alt03
xu1D438 +v(u1D461). (5.44)
The system dynamics can be formulated in terms of quaternions as follows. The state vector
is given by
u1D465u1D444 =
uni005B.alt03
u1D45D u1D45E u1D45F u1D45Eu1D456u1D44F,u1D456 u1D45Eu1D456u1D44F,u1D457 u1D45Eu1D456u1D44F,u1D458 u1D45Eu1D456u1D44F,u1D45F u1D464u1D465 u1D464u1D466 u1D464u1D467 u1D464u1D719 u1D464u1D703 u1D464u1D713
uni005D.alt03u1D447
,
and the output vector is the same as for the Euler formulation. The system model is then
130
?xu1D444 =
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
u1D450u1D450u1D43Fu1D45D(u1D461)
u1D43Cu1D465u1D465(u1D461) 0 0 0 0 0 0 0 0 0 0 0 0
0 u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) ?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0
0 u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) (u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0
0 0 0 0 u1D45F2 ?u1D45E2 u1D45D2 0 0 0 0 0 0
0 0 0 ?u1D45F2 0 u1D45D2 u1D45E2 0 0 0 0 0 0
0 0 0 u1D45E2 ?u1D45D2 0 u1D45F2 0 0 0 0 0 0
0 0 0 ?u1D45D2 ?u1D45E2 ?u1D45F2 0 0 0 0 0 0 0
0 0 0 0 0 0 0 u1D450u1D465 0 0 0 0 0
0 0 0 0 0 0 0 0 u1D450u1D466 0 0 0 0
0 0 0 0 0 0 0 0 0 u1D450u1D467 0 0 0
0 0 0 0 0 0 0 0 0 0 u1D450u1D719 0 0
0 0 0 0 0 0 0 0 0 0 0 u1D450u1D703 0
0 0 0 0 0 0 0 0 0 0 0 0 u1D450u1D713
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
xu1D444 +
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
0 0
1
u1D43Cu1D466u1D466(u1D461) 0
0 1u1D43Cu1D466u1D466(u1D461)
0 0
0 0
0 0
0 0
0 0
0 0
0 0
0 0
0 0
0 0
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459
+w(u1D461) (5.45)
13
1
yu1D438,u1D444 =
?
??
??
??
??
??
??
??
??
u1D45D+u1D464u1D465
u1D45E+u1D464u1D466
u1D45F+u1D464u1D467
tan?1
uni0028.alt03
2(u1D45Eu1D456u1D44F,u1D457u1D45Eu1D456u1D44F,u1D458+u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D45F)
2(u1D45E2u1D456u1D44F,u1D45F+u1D45E2u1D456u1D44F,u1D458)?1
uni0029.alt03
sin?1(2(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458))
tan?1
uni0028.alt03
2(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D458+u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D457)
2(u1D45E2u1D456u1D44F,u1D45F+u1D45E2u1D456u1D44F,u1D456)?1
uni0029.alt03
?
??
??
??
??
??
??
??
??
+v(u1D461). (5.46)
5.4.1 EKF
Implementation of an EKF requires the linearization of the system model given Equations
(5.43) and (5.45). The Euler angle dynamic equations are
?u1D719 = u1D45D+ sin(u1D719)sin(u1D703)
cos(u1D703) u1D45E+
cos(u1D719)sin(u1D703)
cos(u1D703) u1D45F
?u1D703 = cos(u1D703)u1D45E?sin(u1D719)u1D45F
?u1D713 = sin(u1D719)
cos(u1D703)u1D45E+
cos(u1D719)
cos(u1D703)u1D45F.
The associated partial derivatives are
? ?u1D719
?u1D45D = 1
? ?u1D719
?u1D45E =
sin(u1D719)sin(u1D703)
cos(u1D703)
? ?u1D719
?u1D45F =
cos(u1D719)sin(u1D703)
cos(u1D703)
? ?u1D719
?u1D719 =
cos(u1D719)sin(u1D703)
cos(u1D703) u1D45E?
sin(u1D719)sin(u1D703)
cos(u1D703) u1D45F
? ?u1D719
?u1D703 =
sin(u1D719)
cos2(u1D703)u1D45E+
cos(u1D719)
cos2(u1D703)u1D45F
? ?u1D719
?u1D713 = 0
??u1D703
?u1D45D = 0
??u1D703
?u1D45E = cos(u1D703)
??u1D703
?u1D45F = ?sin(u1D719)
??u1D703
?u1D719 = ?cos(u1D719)u1D45F
??u1D703
?u1D703 = ?sin(u1D703)u1D45E
??u1D703
?u1D713 = 0
? ?u1D713
?u1D45D = 0
? ?u1D713
?u1D45E =
sin(u1D719)
cos(u1D703)
? ?u1D713
?u1D45F =
cos(u1D719)
cos(u1D703)
? ?u1D713
?u1D719 =
cos(u1D719)
cos(u1D703)u1D45E?
sin(u1D719)
cos(u1D703)u1D45F
? ?u1D713
?u1D703 =
sin(u1D719)sin(u1D703)
cos2(u1D703) u1D45E+
cos(u1D719)sin(u1D703)
cos2(u1D703) u1D45F
? ?u1D713
?u1D713 = 0
132
For the quaternion formulation, u1D4531, u1D4532, and u1D4533 of Equation (5.33) are
u1D4531 = tan?1
uni0028.alt04
2(u1D45Eu1D456u1D44F,u1D457u1D45Eu1D456u1D44F,u1D458 +u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D45F)
2(u1D45E2u1D456u1D44F,u1D45F +u1D45E2u1D456u1D44F,u1D458)?1
uni0029.alt04
(5.47)
u1D4532 = sin?1(2(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458)) (5.48)
u1D4533 = tan?1
uni0028.alt04
2(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D458 +u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D457)
2(u1D45E2u1D456u1D44F,u1D45F +u1D45E2u1D456u1D44F,u1D456)?1
uni0029.alt04
. (5.49)
Since
u1D451tan?1(u1D465)
u1D451u1D465 =
1
1+u1D4652
and
u1D451sin?1(u1D465)
u1D451u1D465 =
1?
1?u1D4652,
the partial derivatives of Equations (5.47)-(5.49)are developed as follows. The partial derivative of
u1D4531 with respect to u1D45Eu1D456u1D44F,u1D45F is
?u1D4531
?u1D45Eu1D456u1D44F,u1D45F =
uni0028.alt02
2u1D45Eu1D456u1D44F,u1D456(2(u1D45E2u1D456u1D44F,u1D45F +u1D45E2u1D456u1D44F,u1D458 ?1))?4u1D45Eu1D456u1D44F,u1D45F(?u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D456 +u1D45Eu1D456u1D44F,u1D457u1D45Eu1D456u1D44F,u1D458)
uni0029.alt02
uni0028.alt03
1+ 4(?u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D456+u1D45Eu1D456u1D44F,u1D457u1D45Eu1D456u1D44F,u1D458)2(2(u1D45E2
u1D456u1D44F,u1D45F+u1D45E
2
u1D456u1D44F,u1D458?1)2
uni0029.alt03
(2(u1D45E2u1D456u1D44F,u1D45F +u1D45E2u1D456u1D44F,u1D458)?1)2
= 2u1D45Eu1D456u1D44F,u1D456(2(u1D45E
2
u1D456u1D44F,u1D45F +u1D45E
2
u1D456u1D44F,u1D458)?1)?8u1D45Eu1D456u1D44F,u1D45F(u1D45Eu1D456u1D44F,u1D457u1D45Eu1D456u1D44F,u1D458 +u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D456)
(2(u1D45E2u1D456u1D44F,u1D45F +u1D45E2u1D456u1D44F,u1D458)?1)2 +4(u1D45Eu1D456u1D44F,u1D457u1D45Eu1D456u1D44F,u1D458 +u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D456)2 .
The denominator of ?u1D4531?u1D45Eu1D456u1D44F,u1D45F can be rewritten as
(2(u1D45E2u1D456u1D44F,u1D45F +u1D45E2u1D456u1D44F,u1D458)?1)2 +4(u1D45Eu1D456u1D44F,u1D457u1D45Eu1D456u1D44F,u1D458 ?u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D456)2
= 4(u1D45E2u1D456u1D44F,u1D45F +u1D45E2u1D456u1D44F,u1D458)(u1D45E2u1D456u1D44F,u1D45F +u1D45E2u1D456u1D44F,u1D458)?4(u1D45E2u1D456u1D44F,u1D45F +u1D45E2u1D456u1D44F,u1D458)+1+4u1D45E2u1D456u1D44F,u1D457u1D45E2u1D456u1D44F,u1D458 +
8u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D457u1D45Eu1D456u1D44F,u1D458u1D45Eu1D456u1D44F,u1D45F +4u1D45E2u1D44Fu1D456,u1D45Fu1D45E2u1D44Fu1D456,u1D456
= 4u1D45E4u1D456u1D44F,u1D45F +4u1D45E4u1D456u1D44F,u1D458 +8u1D45E2u1D456u1D44F,u1D45Fu1D45E2u1D456u1D44F,u1D458 ?4u1D45E2u1D456u1D44F,u1D45F ?4u1D45E2u1D456u1D44F,u1D458 +1+4u1D45E2u1D456u1D44F,u1D457u1D45E2u1D456u1D44F,u1D458 +
8u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D457u1D45Eu1D456u1D44F,u1D458u1D45Eu1D456u1D44F,u1D45F +4u1D45E2u1D456u1D44F,u1D45Fu1D45E2u1D456u1D44F,u1D456
= 1?4(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458)2.
133
Then
?u1D4531
?u1D45Eu1D456u1D44F,u1D45F =
4u1D45Eu1D456u1D44F,u1D456(u1D45E2u1D456u1D44F,u1D458 ?u1D45E2u1D456u1D44F,u1D45F)?8u1D45Eu1D456u1D44F,u1D457u1D45Eu1D456u1D44F,u1D458u1D45Eu1D456u1D44F,u1D45F ?2u1D45Eu1D456u1D44F,u1D456
1?4(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458)2 . (5.50)
The remaining derivatives are
?u1D4531
?u1D45Eu1D456u1D44F,u1D456 =
4u1D45Eu1D456u1D44F,u1D45F(u1D45E2u1D456u1D44F,u1D45F +u1D45E2u1D456u1D44F,u1D458)?2u1D45Eu1D456u1D44F,u1D45F
1?4(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458)2 (5.51)
?u1D4531
?u1D45Eu1D456u1D44F,u1D457 =
?4u1D45Eu1D456u1D44F,u1D458(u1D45E2u1D456u1D44F,u1D45F +u1D45E2u1D456u1D44F,u1D458)?2u1D45Eu1D456u1D44F,u1D458
1?4(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458)2 (5.52)
?u1D4531
?u1D45Eu1D456u1D44F,u1D458 =
4u1D45Eu1D456u1D44F,u1D457(u1D45E2u1D456u1D44F,u1D45F ?u1D45E2u1D456u1D44F,u1D458)?8u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458u1D45Eu1D456u1D44F,u1D45F ?2u1D45Eu1D456u1D44F,u1D456
1?4(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458)2 (5.53)
?u1D4532
?u1D45Eu1D456u1D44F,u1D45F =
2u1D45Eu1D456u1D44F,u1D457uni221A.alt01
1?4(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458)2 (5.54)
?u1D4532
?u1D45Eu1D456u1D44F,u1D456 =
?2u1D45Eu1D456u1D44F,u1D458uni221A.alt01
1?4(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458)2 (5.55)
?u1D4532
?u1D45Eu1D456u1D44F,u1D457 =
2u1D45Eu1D456u1D44F,u1D45Funi221A.alt01
1?4(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458)2 (5.56)
?u1D4532
?u1D45Eu1D456u1D44F,u1D458 =
?2u1D45Eu1D456u1D44F,u1D456uni221A.alt01
1?4(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458)2. (5.57)
With respect to u1D45Eu1D456u1D44F,u1D45F the partial derivative of u1D4533 is
?u1D4533
?u1D45Eu1D456u1D44F,u1D45F =
4u1D45Eu1D456u1D44F,u1D458(u1D45E2u1D456u1D44F,u1D45F +u1D45E2u1D456u1D44F,u1D456)?2u1D45Eu1D456u1D44F,u1D458 ?8u1D45Eu1D456u1D44F,u1D45F(u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D458)
(2(u1D45E2u1D456u1D44F,u1D45F +u1D45E2u1D456u1D44F,u1D456)?1)2 +4(u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D458)2 .
By the same method used to calculate the denominator of ?u1D4531?u1D45Eu1D456u1D44F,u1D45F, it can be shown that
(2(u1D45E2u1D456u1D44F,u1D45F +u1D45E2u1D456u1D44F,u1D456)?1)2 +4(u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D458)2 = 1?4(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458)2.
134
Then
?u1D4533
?u1D45Eu1D456u1D44F,u1D45F =
4u1D45Eu1D456u1D44F,u1D458(u1D45E2u1D456u1D44F,u1D45F ?u1D45E2u1D456u1D44F,u1D456)?8u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D457 ?2u1D45Eu1D456u1D44F,u1D458
1?4(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458)2 (5.58)
?u1D4533
?u1D45Eu1D456u1D44F,u1D456 =
4u1D45Eu1D456u1D44F,u1D457(u1D45E2u1D456u1D44F,u1D45F ?u1D45E2u1D456u1D44F,u1D456)?8u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458 ?2u1D45Eu1D456u1D44F,u1D457
1?4(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458)2 (5.59)
?u1D4533
?u1D45Eu1D456u1D44F,u1D457 =
4u1D45Eu1D456u1D44F,u1D456(u1D45E2u1D456u1D44F,u1D45F +u1D45E2u1D456u1D44F,u1D456)?2u1D45Eu1D456u1D44F,u1D456
1?4(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458)2 (5.60)
?u1D4533
?u1D45Eu1D456u1D44F,u1D458 =
4u1D45Eu1D456u1D44F,u1D45F(u1D45E2u1D456u1D44F,u1D45F +u1D45E2u1D456u1D44F,u1D456)?2u1D45Eu1D456u1D44F,u1D45F
1?4(u1D45Eu1D456u1D44F,u1D45Fu1D45Eu1D456u1D44F,u1D457 ?u1D45Eu1D456u1D44F,u1D456u1D45Eu1D456u1D44F,u1D458)2 (5.61)
5.5 Estimation with Magnetometer, Rate Gyros, and Angle Gyros
In this section estimators are developed for the rocket when the sensors used are a magnetome-
ter, rate gyros, and angle gyros. The sensor models are the same as used in the previous sections.
Linearizations derived in previous sections are also used in developing the EKF in this section.
By using both angle sensing gyros and a magnetometer, two sensors are being used to determine
angular position.
For the Euler formulation the state vector is
xu1D438 =
uni005B.alt03
u1D45D u1D45E u1D45F u1D719 u1D703 u1D713 u1D464u1D465 u1D464u1D466 u1D464u1D467 u1D464u1D719 u1D464u1D703 u1D464u1D713 u1D464u1D45Au1D465 u1D464u1D45Au1D466 u1D464u1D45Au1D467
uni005D.alt03u1D447
.
The output vector is
yu1D438,u1D444 =
uni005B.alt03
u1D714u1D460u1D465 u1D714u1D460u1D466 u1D714u1D460u1D467 u1D719u1D460 u1D703u1D460 u1D713u1D460 u1D43Bu1D435u1D465 u1D43Bu1D435u1D466 u1D43Bu1D435u1D467
uni005D.alt03u1D447
+v(u1D461).
The system is then given by
135
?xu1D438 =
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
?
u1D450u1D450u1D43Fu1D45D(u1D461)
u1D43Cu1D465u1D465(u1D461) 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) ?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0 0 0
0 u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) (u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0 0 0
1 u1D460u1D719u1D460u1D703u1D450u1D703 u1D450u1D719u1D460u1D703u1D450u1D703 0 0 0 0 0 0 0 0 0 0 0 0
0 u1D450u1D719 ?u1D460u1D719 0 0 0 0 0 0 0 0 0 0 0 0
0 u1D460u1D719u1D450u1D703 u1D450u1D719u1D450u1D703 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 u1D450u1D465 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 u1D450u1D466 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 u1D450u1D467 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 u1D450u1D719 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 u1D450u1D703 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 u1D450u1D713 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 u1D450u1D45Au1D465 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 u1D450u1D45Au1D466 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 u1D450u1D45Au1D467
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
?
xu1D438 +
?
?? 0 1u1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 1u1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0 0 0
?
??
u1D447
u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459 +w(u1D461)
13
6
yu1D438,u1D444 =
uni005B.alt03
u1D45D+u1D464u1D465 u1D45E+u1D464u1D466 u1D45F+u1D464u1D467 u1D719+u1D464u1D719 u1D703+u1D464u1D703 u1D713+u1D464u1D713 u1D4667 u1D4668 u1D4669
uni005D.alt03u1D447
+v(u1D461)
where
u1D4667 = cos(u1D713)cos(u1D703)u1D43Bu1D43Cu1D465 +sin(u1D713)cos(u1D703)u1D43Bu1D43Cu1D466 ?sin(u1D703)u1D43Bu1D43Cu1D465
u1D4668 = (cos(u1D713)sin(u1D703)sin(u1D713)?sin(u1D713)cos(u1D719))u1D43Bu1D43Cu1D465 +(sin(u1D713)sin(u1D703)sin(u1D719) +
cos(u1D713)cos(u1D719))u1D43Bu1D43Cu1D466 +cos(u1D703)sin(u1D719)u1D43Bu1D43Cu1D467
u1D4669 = (cos(u1D713)sin(u1D703)cos(u1D719) +sin(u1D713)sin(u1D719))u1D43Bu1D43Cu1D465 +(sin(u1D713)sin(u1D703)cos(u1D719)?
cos(u1D713)sin(u1D719))u1D43Bu1D43Cu1D466 +cos(u1D703)cos(u1D719)u1D43Bu1D43Cu1D467
The quaternion formulation state vector is
xu1D444 =
uni005B.alt03
u1D45D u1D45E u1D45F u1D45Eu1D456u1D44F,u1D456 u1D45Eu1D456u1D44F,u1D457 u1D45Eu1D456u1D44F,u1D458 u1D45Eu1D456u1D44F,u1D45F u1D464u1D465 u1D464u1D466 u1D464u1D467 u1D464u1D719 u1D464u1D703 u1D464u1D713 u1D464u1D45Au1D465 u1D464u1D45Au1D466 u1D464u1D45Au1D467
uni005D.alt03
.
The output vector is
yu1D438,u1D444 =
uni005B.alt03
u1D714u1D460u1D465 u1D714u1D465u1D466 u1D714u1D460u1D467 u1D719u1D460 u1D703u1D460 u1D713u1D460 u1D43Bu1D43Cu1D465 u1D43Bu1D43Cu1D466 u1D43Bu1D43Cu1D467
uni005D.alt03
+v(u1D461).
The system dynamics are then
137
?xu1D444 =
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
u1D450u1D450u1D43Fu1D45D(u1D461)
u1D43Cu1D465u1D465(u1D461) 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) ?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0 0 0 0
0 (u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 u1D45F2 ?u1D45E2 u1D45D2 0 0 0 0 0 0 0 0 0
0 0 0 ?u1D45F2 0 u1D45D2 u1D45E2 0 0 0 0 0 0 0 0 0
0 0 0 u1D45E2 ?u1D45D2 0 u1D45F2 0 0 0 0 0 0 0 0 0
0 0 0 ?u1D45D2 ?u1D45E2 ?u1D45F2 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 u1D450u1D465 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 u1D450u1D466 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 u1D450u1D467 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 u1D450u1D719 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 u1D450u1D703 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 u1D450u1D713 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 u1D450u1D45Au1D465 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 u1D450u1D45Au1D466 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 u1D450u1D45Au1D467
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
xu1D444 +
?
?? 0 1u1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 1u1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0 0 0 0
?
??
u1D447
u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459 +w(u1D461) (5.62)
13
8
y =
uni005B.alt03
u1D45D+u1D464u1D465 u1D45E+u1D464u1D466 u1D45F+u1D464u1D467 u1D4531 +u1D464u1D719 u1D4532 +u1D464u1D703 u1D4533 +u1D464u1D713 u1D43Bu1D435u1D465 +u1D464u1D45Au1D465
u1D43Bu1D435u1D466 +u1D464u1D45Au1D466 u1D43Bu1D435u1D467 +u1D464u1D45Au1D467
uni005D.alt03
+v(u1D461) (5.63)
where u1D43Bu1D435u1D465 , u1D43Bu1D435u1D466 , and u1D43Bu1D435u1D467 are given by Equations (5.18a)-(5.18c) and u1D4531, u1D4532, and u1D4533 are given by
Equations (5.47)-(5.49).
5.5.1 EKF
For the Euler formation these additional linearizations are necessary for an EKF:
?u1D4667
?u1D719 = 0
?u1D4667
?u1D703 = ?sin(u1D703)(cos(u1D713)u1D43B
u1D43C
u1D465 +sin(u1D713)u1D43B
u1D43C
u1D466 +u1D43B
u1D43C
u1D467)
?u1D4667
?u1D713 = cos(u1D703)(?sin(u1D713)u1D43B
u1D43C
u1D465 +cos(u1D713)u1D43B
u1D43C
u1D466)
?u1D4668
?u1D719 = (cos(u1D713)sin(u1D703)cos(u1D719) +sin(u1D713)sin(u1D719))u1D43B
u1D43C
u1D465 +(sin(u1D713)sin(u1D703)cos(u1D713)?
cos(u1D713)sin(u1D719))u1D43Bu1D43Cu1D466 +cos(u1D703)cos(u1D719)u1D43Bu1D43Cu1D467
?u1D4668
?u1D703 = cos(u1D713)cos(u1D703)sin(u1D719)u1D43B
u1D43C
u1D465 +sin(u1D713)cos(u1D703)sin(u1D719)u1D43B
u1D43C
u1D466 ?sin(u1D703)sin(u1D719)u1D43B
u1D43C
u1D467
?u1D4668
?u1D713 = (?sin(u1D713)sin(u1D703)sin(u1D719)?cos(u1D713)cos(u1D719))u1D43B
u1D43C
u1D465 +(cos(u1D713)sin(u1D703)sin(u1D719)?
sin(u1D713)cos(u1D719))u1D43Bu1D43Cu1D466
?u1D4669
?u1D719 = (?cos(u1D713)sin(u1D703)sin(u1D719) +sin(u1D713)cos(u1D719))u1D43B
u1D43C
u1D465 +(?sin(u1D713)sin(u1D703)sin(u1D719)?
cos(u1D713)cos(u1D719))u1D43Bu1D43Cu1D466 ?cos(u1D703)sin(u1D719)u1D43Bu1D43Cu1D467
?u1D4669
?u1D703 = cos(u1D713)cos(u1D703)cos(u1D719)u1D43B
u1D43C
u1D465 ?sin(u1D713)cos(u1D703)cos(u1D719)u1D43B
u1D43C
u1D466 ?sin(u1D703)cos(u1D719)u1D43B
u1D43C
u1D467
?u1D4669
?u1D713 = (?sin(u1D713)sin(u1D703)cos(u1D719) +cos(u1D713)sin(u1D719))u1D43B
u1D43C
u1D465 +(cos(u1D713)sin(u1D703)cos(u1D719) +
sin(u1D713)sin(u1D719))u1D43Bu1D43Cu1D466.
139
For the quaternion formulation, the necessary linearizations for the fourth through sixth terms of
u1D466 are given by Equations (5.50)-(5.61) and for u1D43Bu1D435u1D465 , u1D43Bu1D435u1D466 , and u1D43Bu1D435u1D467 by Equations (5.21)-(5.32).
5.6 Wahba?s Problem Applied to a Rocket
In this section we develop the equations necessary to use a Wahba?s problem solution for state
estimation of a rotating rocket. As presented in Chapter 2, it is possible to get an estimate of
attitude if two noncolinear vectors that are measured in the rocket?s body frame are known in
the inertial frame. For satellites these two measured vectors are usually a magnetic ?eld vector
(measured with a magnetometer) and a gravitational ?eld vector (measured with an accelerometer).
However, an accelerometer cannot be used to measure the gravitational ?eld vector of a rocket in
?ight.
To get the two needed vector sets to solve Wahba?s problem, we use a magnetometer and
angle gyros. The angle gyros measure the rocket?s rotational angles, which can then be used to
rotate a vector known in the inertial frame to the rocket?s body frame. We rotate the inertial
vector 1?3
uni005B.alt03
1 1 1
uni005D.alt03u1D447
into the body frame. With two sets of vectors from the magnetometer
measurements and angle gyro readings, we apply the ESOQ algorithm (see ?2.13) to estimate the
rocket?s attitude quaternion. The ESOQ algorithm is chosen because it provides a closed-form
solution in the form of a quaternion to the attitude estimation problem at a low computational
cost. Estimates of the rotational rates can be found via Euler di?erentiation by ?rst converting
the quaternion to a set of Euler angles. Then subtract the Euler angles of the current time step?s
Euler angles from the Euler angles of the previous time step, and divide by the time step.
The remainder of this section is organized as follows. In ?5.6.1 we develop a model for using
the ESOQ algorithm in conjunction with an EKF and a UKF. The ESOQ algorithm provides
an estimate of the attitude quaternion from magnetometer and angle gyro measurements. The
quaternion is then provided as a sensor reading to the system model. Since the ESOQ algorithm
provides a quaternion solution, Euler angle formations for the systems are omitted in?5.6.1. Finally,
in ?5.6.2, we present a model using the ESOQ algorithm to estimate angular position and Kalman
?ltering to estimate angular rates.
140
5.6.1 Wahba?s Problem Solution as Input to an EKF and UKF
In this section we develop the system model when the solution to Wahba?s problem (found via
the ESOQ algorithm) is modeled as a sensor measurement for an EKF and a UKF. The rocket is
assumed to be equipped with a magnetometer, rate gyros, and angle gyros as in ?5.5. The rate gyro
and magnetometer measurements are inputs to the ESOQ algorithm, which estimates the rocket?s
attitude quaternion. The estimated quaternion along with measurements from the rate gyros are
used as sensor readings for the EKF and UKF.
The state vector u1D465u1D444 for the system is
u1D465u1D444 =
uni005B.alt03
u1D45D u1D45E u1D45F u1D45Eu1D456u1D44F,u1D456 u1D45Eu1D456u1D44F,u1D457 u1D45Eu1D456u1D44F,u1D458 u1D45Eu1D456u1D44F,u1D45F u1D464u1D465 u1D464u1D466 u1D464u1D467 u1D464u1D456 u1D464u1D457 u1D464u1D458 u1D464u1D45F
uni005D.alt03u1D447
where u1D464u1D456, u1D464u1D457, u1D464u1D458, and u1D464u1D45F are walking biases associated with the quaternion estimate from the
ESOQ solution with dynamics
?u1D464u1D45B = u1D450u1D45Bu1D464u1D45B +u1D702u1D45B u1D45B = {u1D456,u1D457,u1D458,u1D45F}.
The system dynamics are then
141
?xu1D444 =
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
u1D450u1D450u1D43Fu1D45D(u1D461)
u1D43Cu1D465u1D465(u1D461) 0 0 0 0 0 0 0 0 0 0 0 0 0
0 u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) ?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0 0
0 (u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 u1D45F2 ?u1D45E2 u1D45D2 0 0 0 0 0 0 0
0 0 0 ?u1D45F2 0 u1D45D2 u1D45E2 0 0 0 0 0 0 0
0 0 0 u1D45E2 ?u1D45D2 0 u1D45F2 0 0 0 0 0 0 0
0 0 0 ?u1D45D2 ?u1D45E2 ?u1D45F2 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 u1D450u1D465 0 0 0 0 0 0
0 0 0 0 0 0 0 0 u1D450u1D466 0 0 0 0 0
0 0 0 0 0 0 0 0 0 u1D450u1D467 0 0 0 0
0 0 0 0 0 0 0 0 0 0 u1D450u1D456 0 0 0
0 0 0 0 0 0 0 0 0 0 0 u1D450u1D457 0 0
0 0 0 0 0 0 0 0 0 0 0 0 u1D450u1D458 0
0 0 0 0 0 0 0 0 0 0 0 0 0 u1D450u1D45F
?
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
??
xu1D444
+
?
?? 0 1u1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0 0 0
0 0 1u1D43Cu1D466u1D466(u1D461) 0 0 0 0 0 0 0 0 0 0 0
?
??
u1D447
u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459 +w(u1D461) (5.64)
.
14
2
The output equation u1D466 is
u1D466 =
uni005B.alt03
u1D45D+u1D464u1D465 u1D45E+u1D464u1D466 u1D45F+u1D464u1D467 u1D45Eu1D456u1D44F,u1D456 +u1D464u1D456 u1D45Eu1D456u1D44F,u1D457 +u1D464u1D457 u1D45Eu1D456u1D44F,u1D458 +u1D464u1D458 u1D45Eu1D456u1D44F,u1D45F +u1D464u1D45F
uni005D.alt03u1D447
+ v(u1D461) (5.65)
=
uni005B.alt03
u1D43C7?7 u1D43C7?7
uni005D.alt03
u1D465u1D444 +v(u1D461).
5.6.2 Wahba?s Problem Solution Combined with an EKF and UKF
In this section we develop the system model when the ESOQ algorithm and Kalman ?ltering
are used independently. The rocket?s attitude is estimated using the ESOQ algorithm, and an EKF
or UKF is used to estimate angular rates. The state vector u1D465u1D444 is
u1D465u1D444 =
uni005B.alt03
u1D45D u1D45E u1D45F u1D464u1D465 u1D464u1D466 u1D464u1D467
uni005D.alt03u1D447
.
The system dynamics are then
?xu1D444 =
?
??
??
??
??
??
??
??
?
u1D450u1D450u1D43Fu1D45D(u1D461)
u1D43Cu1D465u1D465(u1D461) 0 0 0 0 0
0 u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) ?(u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) 0 0 0
0 (u1D43Cu1D465u1D465(u1D461)?u1D43Cu1D466u1D466(u1D461))u1D45Du1D43Cu1D466u1D466(u1D461) u1D450u1D450u1D45Au1D45Eu1D45F(u1D461)u1D43Cu1D466u1D466(u1D461) 0 0 0
0 0 0 u1D450u1D465 0 0
0 0 0 0 u1D450u1D466 0
0 0 0 0 0 u1D450u1D467
?
??
??
??
??
??
??
??
?
xu1D444 +
?
?? 0 1u1D43Cu1D466u1D466(u1D461) 0 0 0 0
0 0 1u1D43Cu1D466u1D466(u1D461) 0 0 0
?
??
u1D447
u1D70Fu1D450u1D45Cu1D45Bu1D461u1D45Fu1D45Cu1D459 +w(u1D461). (5.66)
143
The output equation u1D466 is given by
u1D466 =
uni005B.alt03
u1D45D+u1D464u1D465 u1D45E+u1D464u1D466 u1D45F+u1D464u1D467
uni005D.alt03u1D447
+v(u1D461) (5.67)
=
?
??
??
?
1 0 0 1 0 0
0 1 0 0 1 0
0 0 1 0 0 1
?
??
??
?
u1D465u1D444 +v(u1D461). (5.68)
144
Chapter 6
Results
In this chapter we present the results of simulating the ?ight of a rotating rocket as modeled
in Chapter 5 when the missile?s control law is provided state estimates from a KF, an EKF, a UKF
[66] (see Appendix C.3 for a discussion of the UKF), and the ESOQ algorithm. We simulate the
rocket with various sensor suites and compare the performances of the various estimators. In ?6.1
we examine estimator performance when the rocket is equipped only with rate gyros. Next, in ?6.2
we simulate both rate gyros and an ideal sensor that perfectly measures a known inertial frame
vector in the body frame, and we compare the EKF and UKF performances. In ?6.3 we replace the
ideal sensor of ?6.2 with a magnetometer and analyze estimator performance. In ?6.4 we compare
the performance of the EKF and the UKF when the available sensors are rate gyros and angle
gyros, which directly measure the rocket?s attitude. Next in ?6.5 we compare the performance of
the ESOQ algorithm to estimators based on Kalman ?ltering. Finally in ?6.6, we summarize our
?ndings and draw some conclusions.
Monte-Carlo simulations of a spinning rocket are used to compare the performance of the EKF,
UKF, and ESOQ for estimation of angular rates and position. 250 simulations are run with varied
parameters, which include rocket mass properties, main motor misalignment, wind disturbances,
the time at which the tail ?ns deploy, and tip-o? error (angular rates experienced immediately
after exit from the launcher). The various estimators are run in tandem for each simulation. The
method we use to evaluate system performance is circular error probability (CEP), the radius of
the circle in which the rocket impacts 50% of the time. To calculate CEP exactly is di?cult, so
several approximations have been developed. The third method proposed by Taub and Thomas
145
[67] is used in this work:
u1D436u1D438u1D443 = uni0028.alt012u1D7122u1D708,0.5/u1D708uni0029.alt011/2
uni0028.alt04
u1D70E2u1D465 +u1D70E2u1D466
2
uni0029.alt041/2
(6.1a)
u1D708 =
uni0028.alt01u1D70E2
u1D465 +u1D70E2u1D466
uni0029.alt012
u1D70E4u1D465 +u1D70E4u1D466 (6.1b)
whereu1D70E2u1D465 is cross range variance,u1D70E2u1D466 is down range variance, andu1D7122u1D708,0.5 is thechi-squared distribution
withu1D708 degrees of freedom. We also use the average of the mean squared error (MSE) over all Monte
Carlo runs of rocket angular rates and position to evaluate performance. For the CEP all values
are normalized, and for MSE all values are scaled by dividing by a maximum value.
6.1 Rate Gyros
In this section we compare the KF (implemented for the system model given by Equations
(5.7)-(5.8) with the method of ?5.2.1) and UKF (implemented with Algorithm C.3.1 for the system
model given by Equation (5.4)) performances when rate gyros are the only sensors used for state
estimation. In Figures 6.1-6.3 and 6.4-6.6 we show the average MSE of the KF and UKF estimates
of rotational rates and angular position. The KF average MSEs are plotted in blue, and the UKF
average MSEs are plotted in green. As we show in Figure 6.1, the KF estimates of roll rate u1D45D
have an MSE around 0 while the UKF estimates have an MSE of almost 1. However, we see from
Figures 6.2 and 6.3 that the KF and UKF have comparable average MSEs for estimates of pitch
rate u1D45E and yaw rate u1D45F. The UKF average MSE spikes at approximately 1 s while the KF estimates
remain fairly smooth.
In Figures 6.4-6.6 we see the MSEs of the KF and UKF estimates of roll u1D719, pitch u1D703, and yaw u1D713
angles. The MSE of the roll angle oscillates for both the KF and the UKF with the UKF settling to
a smaller MSE than the KF after 1.5 s. The KF and UKF have almost identical MSEs for pitch as
shown in Figure 6.5. The KF MSE of yaw angle is almost 0 while the UKF MSE oscillates between
0 and 1.
We show normalized CEP plotted versus time in Figures 6.7 and 6.8. The baseline (uncon-
trolled) CEP is shown with a blue line, the controlled CEP is shown with a green line, and the
146
di?erence between the uncontrolled and controlled CEP is shown with a red line. In Figure 6.7 we
show the CEP when the KF provides estimates to the rocket?s control law, and in Figure 6.8 we
show the CEP when the UKF provides estimates to the rocket?s control law. Controlling the rocket
with the KF as its estimator lowers the CEP by more than 9 units while the UKF only lowers CEP
by a little less than 4 units.
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
p
EKF
UKF
Figure 6.1: Scaled MSE of Roll Rates of KF and UKF Estimates from Rate Gyros
147
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
q
KF
UKF
Figure 6.2: Scaled MSE of Pitch Rates of KF and UKF Estimates from Rate Gyros
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
r
KF
UKF
Figure 6.3: Scaled MSE of Yaw Rates of KF and UKF Estimates from Rate Gyros
148
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Roll
KF
UKF
Figure 6.4: Scaled MSE of Roll Angles of KF and UKF Estimates from Rate Gyros
 0.94
 0.95
 0.96
 0.97
 0.98
 0.99
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Pitch
EKF
UKF
Figure 6.5: Scaled MSE of Pitch Angles of KF and UKF Estimates from Rate Gyros
149
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Yaw
KF
UKF
Figure 6.6: Scaled MSE of Yaw Angles of KF and UKF Estimates from Rate Gyros
 0
 2
 4
 6
 8
 10
 12
 14
 16
 0  10  20  30  40  50
CEP (normalized)
time (s)
Baseline
Controlled
Difference
Figure 6.7: Normalized CEP for KF Estimates from Rate Gyros
150
 0
 2
 4
 6
 8
 10
 12
 14
 16
 0  10  20  30  40  50
CEP (normalized)
time (s)
Baseline
Controlled
Difference
Figure 6.8: Normalized CEP for UKF Estimates from Rate Gyros
6.2 Rate Gyros and an Ideal Vector
In this section we compare the EKF (implemented with Algorithm C.2.1 for the system model
given by Equations (5.15) and (5.20) using the equations developed in ?5.3) and UKF (implemented
with Algorithm C.3.1 for the system model given by Equations (5.15) and (5.20)) performances
when rate gyros and an ideal vector sensor are used for state estimation (for details of model
development see Chapter 5 and details of algorithm implementation see C). In Figures 6.9-6.11
and 6.12-6.14 we show the average MSEs of the EKF and UKF estimates of rotational rates and
angular position. The EKF average MSEs are plotted in blue, and the UKF average MSEs are
plotted in green. As we see from Figure 6.9, the UKF roll rate MSE starts at 0, increases to almost
0.6, and approaches 0 within the ?rst half second. The MSE jumps to 1 and then steadily decreases.
The EKF roll rate MSE increases quickly from 0 to more than 0.2 and then more slowly increases
to 0.3 until 1 s at which point it remains constant. The EKF MSE is consistently smaller than
151
the UKF MSE after 0.5 s. In Figures 6.10 and 6.11 we see that the EKF provides better initial
estimates of the pitch and yaw rate, and then the UKF average MSE approaches that of the EKF.
As shown in Figure 6.12, the EKF MSE for roll angle oscillates between 0 and 1 with an
average value of around 0.5. The UKF oscillates about 0.5 between 0.3 and 1 and settles to 0.6.
The UKF MSE for pitch angle as shown in 6.13 oscillates until reaching steady state at around
1.75 s. The EKF MSE exhibits the same behavior, but while the EKF MSE oscillations have the
same phase as the UKF MSE, the magnitude is larger. In Figure 6.14 it is shown that both the
EKF and UKF MSE oscillate, but the EKF MSE oscillates between 0.5 and 1 for the ?rst second
while the UKF MSE oscillates between 0 and 1. The UKF MSE settles to just under 0.6 at about
1.75 s while the EKF MSE does not settle within 2 s.
In Figure 6.15 we show the CEP resulting from the EKF, and in Figure 6.16 we show the CEP
resulting from the UKF. The baseline case is shown in blue, the controlled case is shown in green,
and the di?erence between the baseline and controlled cases is shown in red. The EKF reduces the
base line CEP from about 15 units to almost 6 units for a di?erence of about 9 units. The UKF
only reduces the baseline CEP by a little over 1 unit.
152
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
p
EKF
UKF
Figure 6.9: Scaled MSE of Roll Rates of EKF and UKF Estimates from Rate Gyros and an Ideal
Vector
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
q
EKF
UKF
Figure 6.10: Scaled MSE of Pitch Rates of EKF and UKF Estimates from Rate Gyros and an Ideal
Vector
153
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
r
EKF
UKF
Figure 6.11: Scaled MSE of Yaw Rates of EKF and UKF Estimates from Rate Gyros and an Ideal
Vector
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Roll
EKF
UKF
Figure 6.12: Scaled MSE of Roll Angles of EKF and UKF Estimates from Rate Gyros and an Ideal
Vector
154
 0.1
 0.2
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Pitch
EKF
UKF
Figure 6.13: Scaled MSE of Pitch Angles of EKF and UKF Estimates from Rate Gyros and an
Ideal Vector
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Yaw
EKF
UKF
Figure 6.14: Scaled MES of Yaw Angles of EKF and UKF Estimates from Rate Gyros and an Ideal
Vector
155
 0
 2
 4
 6
 8
 10
 12
 14
 16
 0  10  20  30  40  50
CEP (normalized)
time (s)
Baseline
Controlled
Difference
Figure 6.15: Normalized CEP for EKF Estimates from Rate Gyros and an Ideal Vector
 0
 2
 4
 6
 8
 10
 12
 14
 16
 0  10  20  30  40  50
CEP (normalized)
time (s)
Baseline
Controlled
Difference
Figure 6.16: Normalized CEP for UKF Estimates from Rate Gyros and an Ideal Vector
156
6.3 Rate Gyros and a Magnetometer
In this section we compare the EKF (implemented with Algorithm C.2.1 for the system model
given by Equations (5.37) and (5.38) using the equations developed in ?5.3) and UKF (implemented
with Algorithm C.3.1 for the system model given by Equations (5.37) and (5.38)) performances
when rate gyros and a magnetometer are used for state estimation. In Figures 6.17-6.19 and 6.20-
6.22, we show the average MSE of EKF and UKF estimates of rotational rates and angular position,
respectively. EKF average MSEs are plotted in blue, and UKF average MSEs are plotted in green.
As we see in Figure 6.17, the EKF provides estimates of roll rate with the same average MSE as
the UKF. For pitch and yaw rate, the EKF and UKF MSEs are almost the same with no signi?cant
di?erences, as shown in Figures 6.18 and 6.19.
In Figures 6.20-6.22 we show the average MSE of the roll u1D719, pitch u1D703, and yaw u1D713 angles,
respectively. The roll angle MSE for the EKF and UKF both oscillate between 0 and 0.8 but have
opposing phases. The UKF pitch angle MSE holds almost constant at a little over 0.3 units while
the EKF MSE oscillates approximately about 0.4 before it settles to about that value. The UKF
MSE for yaw angle oscillates between the 0 and 1. The EKF MSE oscillates about 0.6 faster than
the UKF MSE before reaching steady state.
The MSE plots of pitch rate and yaw rate estimates in Figures 6.10, 6.18, 6.11, and 6.19 explain
why the ideal vector case improves the CEP less than the magnetometer case for UKF estimates.
The MSE of the UKF estimates spikes almost immediately for the ideal vector case while for the
magnetometer case the MSE does not spike until 1 s. The ideal vector case estimates become much
better than the magnetometer case estimates after 1 s. Since control e?ort is most e?ective before
the ?ns open at 1 s, the inaccuracy of the early estimates in the ideal case detrimentally a?ects the
CEP.
We show in Figures 6.23 and 6.24 the normalized CEP resulting from the EKF and UKF
estimates, respectively. The blue lines depict the baseline case, the green lines depict the controlled
case, and the red lines depict the di?erences between the baseline and controlled cases. The EKF
estimates improve the CEP from the baseline case by almost 9 units while the UKF only improves
the baseline case by not quite 4 units.
157
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
p
EKF
UKF
Figure 6.17: Scaled MSE of Roll Rates of EKF and UKF Estimates from Rate Gyros and a
Magnetometer
 0.1
 0.2
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
q
EKF
UKF
Figure 6.18: Scaled MSE of Pitch Rates of EKF and UKF Estimates from Rate Gyros and a
Magnetometer
158
 0.1
 0.2
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
r
EKF
UKF
Figure 6.19: Scaled MSE of Yaw Rates of EKF and UKF Estimates from Rate Gyros and a
Magnetometer
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Roll
EKF
UKF
Figure 6.20: Scaled MSE of Roll Angles of EKF and UKF Estimates from Rate Gyros and a
Magnetometer
159
 0.1
 0.2
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Pitch
EKF
UKF
Figure 6.21: Scaled MSE of Pitch Angles of EKF and UKF Estimates from Rate Gyros and a
Magnetometer
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Yaw
EKF
UKF
Figure 6.22: Scaled MSE of Yaw Angles of EKF and UKF Estimates from Rate Gyros and a
Magnetometer
160
 0
 2
 4
 6
 8
 10
 12
 14
 16
 0  10  20  30  40  50
CEP (normalized)
time (s)
Baseline
Controlled
Difference
Figure 6.23: Normalized CEP for EKF Estimates from Rate Gyros and a Magnetometer
 0
 2
 4
 6
 8
 10
 12
 14
 16
 0  10  20  30  40  50
CEP (normalized)
time (s)
Baseline
Controlled
Difference
Figure 6.24: Normalized CEP for UKF Estimates from Rate Gyros and a Magnetometer
161
6.4 Rate Gyros and Angle Gyros
In this section we compare the EKF (implemented with Algorithm C.2.1 for the system model
given by Equations (5.45) and (5.46) using the equations developed in ?5.4.1) and UKF (imple-
mented with Algorithm C.3.1 for the system model given by Equations (5.45) and (5.46)) perfor-
mances with a sensor suite of both rate gyros and angle gyros. In Figures 6.25-6.27 and 6.28-6.30,
we show the average MSE of EKF and UKF estimates of rotational rates and angular position,
respectively. The EKF average MSEs are plotted in blue, and the UKF average MSEs are plotted
in green. As we see from Figure 6.25, the EKF MSE is slightly better than the UKF MSE until
shortly after 1.5 s, when the UKF MSE peaks while the EKF MSE begins to decrease. In Figures
6.26 and 6.27 we see that the EKF and UKF MSEs are almost identical, with the EKF MSE being
only slightly better. Both UKF pitch rate and yaw rate MSEs spike to 1 shortly after launch while
the EKF MSEs remain constant.
In Figures 6.28, 6.29, and 6.30 we show the average MSE of the EKF and UKF estimates
for the roll angle u1D719, pitch angle u1D703, and yaw angle u1D713, respectively. The EKF average MSE for roll
angle oscillates about 0.4. The UKF average MSE oscillates about 0.5, and the error spikes to
values greater than or equivalent to that of the EKF error. For pitch angle the UKF average MSE
remains constant at a value slightly greater than 0.2. The UKF average MSE remains smaller than
the EKF average MSE, which increases to 1 before 0.25 s and then settles to 0.8. For yaw angle
the EKF average MSE settles to slightly greater than 0.4 after spiking to 0.7 before 0.25 s. The
UKF average MSE oscillates between 0 and 1. After 1 s the magnitude of the oscillations begin to
decrease.
In Figures 6.31 and 6.32 we show the normalized CEP. We show the CEP for EKF estimates
in Figure 6.31 and the CEP for UKF estimates in Figure 6.32. The blue lines depict the baseline
case, the green lines depict the controlled case, and the red lines depict the di?erences between the
baseline and controlled cases. The EKF estimates reduce the controlled CEP from the baseline
CEP by approximately 9 units while the UKF only reduces the CEP between the controlled and
baseline cases by about 3 units.
162
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
p
EKF
UKF
Figure 6.25: Scaled MSE of Roll Rates of EKF and UKF Estimates from Rate Gyros and Angle
Gyros
 0.1
 0.2
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
q
EKF
UKF
Figure 6.26: Scaled MSE of Pitch Rates of EKF and UKF Estimates from Rate Gyros and Angle
Gyros
163
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
r
EKF
UKF
Figure 6.27: Scaled MSE of Yaw Rates of EKF and UKF Estimates from Rate Gyros and Angle
Gyros
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Roll
EKF
UKF
Figure 6.28: Scaled MSE of Roll Angles of EKF and UKF Estimates from Rate Gyros and Angle
Gyros
164
 0.1
 0.2
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Pitch
EKF
UKF
Figure 6.29: Scaled MSE of Pitch Angles of EKF and UKF Estimates from Rate Gyros and Angle
Gyros
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Yaw
EKF
UKF
Figure 6.30: Scaled MSE of Yaw Angles of EKF and UKF Estimates from Rate Gyros and Angle
Gyros
165
 0
 2
 4
 6
 8
 10
 12
 14
 16
 0  10  20  30  40  50
CEP (normalized)
time (s)
Baseline
Controlled
Difference
Figure 6.31: Normalized CEP for EKF Estimates from Rate Gyros and Angle Gyros
 0
 2
 4
 6
 8
 10
 12
 14
 16
 0  10  20  30  40  50
CEP (normalized)
time (s)
Baseline
Controlled
Difference
Figure 6.32: Normalized CEP for UKF Estimates from Rate Gyros and Angle Gyros
166
6.5 Rate Gyros, Angle Gyros, and Magnetometer
In this section we focus on the results of using the ESOQ algorithm to estimate the rocket?s
states. In ?6.5.1 we present the results of ESOQ state estimations with ideal sensors and nonideal
sensors. Next, in ?6.5.2 we present the results of using the EKF and UKF to estimate states when
rate gyro, angle gyro, and magnetometer measurements are all available in order to compare the
results to ESOQ-based estimators in following sections. Finally, in ?6.5.3 we present the results of
combining the ESOQ algorithm with the KF, EKF, and UKF.
6.5.1 ESOQ Algorithm
In Figures 6.33 and 6.34 we show the results of the rocket being controlled with estimates
from the ESOQ algorithm. The only sensors used for these simulations are angle gyros and a
magnetometer. In Figure 6.33 the results of the ESOQ algorithm are shown when ideal sensors
are used. The top line indicates the baseline CEP when no control is applied to the rocket, the
middle line is the rocket?s controlled CEP, and the bottom line is the di?erence between the two
cases. The ESOQ estimator with ideal sensor readings alone only improves the the CEP by about
1.5 units. The CEP resulting from nonideal sensor readings is shown in Figure 6.34. The top line
is the controlled CEP, the middle line is the baseline CEP, and the bottom line is the di?erence
between the two CEPs. The controlled CEP is worse than the baseline CEP when nonideal sensor
readings are provided to the ESOQ algorithm. One reason for these results is that the control law
relies heavily on rotational rate estimates. The ESOQ algorithm only provides angular position
estimates so rotational rates are estimated by Euler di?erentiation. We show normalized roll rates
for all 250 dispersion runs in Figure 6.35, and we show the normalized roll rates as estimated by
the ESOQ algorithm with nonideal sensors in Figure 6.36. In Figure 6.37 we show the normalized
actual roll angles, and in Figure 6.38 we show the normalized roll angles as estimated by the ESOQ
algorithm with nonideal sensors. While the ESOQ algorithm predicts roll angle well, the Euler
di?erentiation yields poor estimates of roll rate. The ESOQ algorithm yields similar results for
pitch angles and rates as shown in Figures 6.39-6.42. As we see from Figures 6.43-6.46, the ESOQ
algorithm poorly estimates yaw angles and therefore yaw rates.
167
 0
 2
 4
 6
 8
 10
 12
 14
 16
 0  10  20  30  40  50
CEP (normalized)
time (s)
Baseline
Controlled
Difference
Figure 6.33: CEP for ESOQ Controlled Rocket with Ideal Angle Gyros and Magnetometer
 0
 5
 10
 15
 0  10  20  30  40  50
CEP (normalized)
time (s)
Baseline
Controlled
Difference
Figure 6.34: CEP for ESOQ Controlled Rocket with Nonideal Angle Gyros and Magnetometer
168
-1
-0.8
-0.6
-0.4
-0.2
 0
 0  0.5  1  1.5  2
p (Scaled)
Time (s)
Actual
Figure 6.35: Actual Roll Rates (Normalized)
 0
 10
 20
 30
 40
 50
 60
 70
 80
 90
 0  0.5  1  1.5  2
p (Scaled)
Time (s)
ESOQ
Figure 6.36: ESOQ Estimated Roll Rates with Nonideal Sensors (Normalized)
169
-1
-0.5
 0
 0.5
 1
 0  0.5  1  1.5  2
Roll (Scaled)
Time (s)
Actual
Figure 6.37: Actual Roll Angles (Normalized)
-1
-0.5
 0
 0.5
 1
 0  0.5  1  1.5  2
Roll (Scaled)
Time (s)
ESOQ
Figure 6.38: ESOQ Estimated Roll Angles with Nonideal Sensors (Normalized)
170
-1
-0.5
 0
 0.5
 1
 0  0.5  1  1.5  2
q (Scaled)
Time (s)
Actual
Figure 6.39: Actual Pitch Rates (Normalized)
-4500
-4000
-3500
-3000
-2500
-2000
-1500
-1000
-500
 0
 0  0.5  1  1.5  2
q (Scaled)
Time (s)
ESOQ
Figure 6.40: ESOQ Estimated Pitch Rates with Nonideal Sensors (Normalized)
171
 0.84
 0.86
 0.88
 0.9
 0.92
 0.94
 0.96
 0.98
 1
 0  0.5  1  1.5  2
Pitch (Scaled)
Time (s)
Actual
Figure 6.41: Actual Pitch Angles (Normalized)
-0.9
-0.85
-0.8
-0.75
-0.7
-0.65
-0.6
 0  0.5  1  1.5  2
Pitch (Scaled)
Time (s)
ESOQ
Figure 6.42: ESOQ Estimated Pitch Angles with Nonideal Sensors (Normalized)
172
-1
-0.5
 0
 0.5
 1
 0  0.5  1  1.5  2
r (Scaled)
Time (s)
Actual
Figure 6.43: Actual Yaw Rates (Normalized)
-5000
-4000
-3000
-2000
-1000
 0
 0  0.5  1  1.5  2
r (Scaled)
Time (s)
ESOQ
Figure 6.44: ESOQ Estimated Yaw Rates with Nonideal Sensors (Normalized)
173
-1
-0.5
 0
 0.5
 1
 0  0.5  1  1.5  2
Yaw (Scaled)
Time (s)
Actual
Figure 6.45: Actual Yaw Angles (Normalized)
-70
-60
-50
-40
-30
-20
-10
 0  0.5  1  1.5  2
Yaw (Scaled)
Time (s)
ESOQ
Figure 6.46: ESOQ Estimated Yaw Angles with Nonideal Sensors (Normalized)
174
6.5.2 EKF and UKF with All Sensors
We show the CEPs resulting from EKF and UKF estimates with rate gyros, angle gyros, and
a magnetometer as described by Equations (5.62) and (5.63) in Figures 6.47 and 6.48, respectively.
In Figure 6.47 the top line is the baseline CEP, the bottom line is the EKF-controlled CEP, and
the middle line is the di?erence between the two. The EKF performs well reducing the CEP by
about 9 units. The UKF, however, performs poorly as we show in Figure 6.48, where the top line is
the UKF controlled CEP, the middle line is the baseline CEP, and the bottom line is the di?erence
between the baseline and controlled cases. The CEP is actually increased over the baseline case by
about 1 unit.
 0
 2
 4
 6
 8
 10
 12
 14
 16
 0  10  20  30  40  50
CEP (normalized)
time (s)
Baseline
Controlled
Difference
Figure 6.47: CEP for EKF Controlled Rocket with Rate Gyros, Angle Gyros, and Magnetometer
6.5.3 ESOQ Algorithm Combined with Kalman Filters
In Figures 6.49-6.52 we show the CEPs resulting from combining estimates from the ESOQ
algorithm with Kalman ?lter based estimators. We show the results of attitude estimates from the
ESOQ algorithm being used as inputs to an EKF as described by Equations (5.64) and (5.65) in
175
 0
 5
 10
 15
 0  10  20  30  40  50
CEP (normalized)
time (s)
Baseline
Controlled
Difference
Figure 6.48: CEP for UKF Controlled Rocket with Rate Gyros, Angle Gyros, and Magnetometer
Figure 6.49. For this case the ESOQ algorithm provides an estimate of the rocket?s attitude in the
form of a quaternion. This attitude estimate is then treated as a sensor reading which is provided
to the EKF. The EKF then provides rotational rate and attitude estimates based on readings from
the rate gyros and the ESOQ algorithm estimates. In Figure 6.49 the top line is the baseline CEP,
the bottom line is the controlled CEP, and the middle line is the di?erence between the two cases.
The CEP is improved by about 9 units. In Figure 6.50 the results of controlling the rocket with
estimates from a EKF/ESOQ hybrid are shown. The EKF is used to estimate angular rates, and
the ESOQ algorithm is used to estimate angular position. The top line in the ?gure is the baseline
CEP, the bottom line is the controlled CEP, and the middle line is the di?erence between them.
The CEP is improved by about 9 units. We show the results for a hybrid UKF/ESOQ estimator in
Figure 6.51. The top line in Figure 6.51 is the baseline CEP, the middle line is the controlled CEP,
and the bottom line is the di?erence between the baseline and controlled cases. The UKF/ESOQ
hybrid estimator improves the CEP by just under 4 units. In Figure 6.52 is shown the results of
the rocket controlled with a hybrid KF/ESOQ estimator, where the KF estimates rotational rates,
and the ESOQ algorithm estimates attitude. The top line in the ?gure is the baseline CEP, the
176
bottom line is the controlled CEP, and the middle line is their di?erence. The KF/ESOQ estimator
improves the CEP by slightly more than 9 units.
 0
 2
 4
 6
 8
 10
 12
 14
 16
 0  10  20  30  40  50
CEP (normalized)
time (s)
Baseline
Controlled
Difference
Figure 6.49: CEP for Rocket Controlled by Estimates from an EKF with Inputs from the ESOQ
Algorithm
In Figures 6.53-6.58 we show the MSE of the state estimates of the EKF, UKF, and the EKF
that has ESOQ estimates as inputs. Roll rate, pitch rate, and yaw rate MSEs are shown in Figures
6.53, 6.54, and 6.55, respectively. In these three ?gures the EKF MSE and the EKF/ESOQ MSE
are identical as shown by the blue EKF line being directly under the red EKF/ESOQ line. The
EKF estimators have better MSEs than the UKF roll rate until the ?ns open at 1 s. At this point
the UKF MSE steadily decreases. The UKF MSE for pitch rate is much smoother than the MSEs
for the EKF estimator. The UKF MSE spikes shortly after the rocket?s launch and then falls below
the EKF MSEs after 0.2 s. The MSEs of the EKF estimator begin to spike when the rocket?s ?ns
open. For yaw rate the UKF MSE peaks shortly after 0.1 s at which point it slowly decreases, not
falling below the MSEs of the EKF estimator until shortly before 1 s. As in the pitch rate case,
the EKF estimators? MSEs spike when the rocket?s ?ns open. In Figures 6.56, 6.57, and 6.58 we
show the roll angle, pitch angle, and yaw angle MSEs, respectively. The roll angle MSEs for all
177
 0
 2
 4
 6
 8
 10
 12
 14
 16
 0  10  20  30  40  50
CEP (normalized)
time (s)
Baseline
Controlled
Difference
Figure 6.50: CEP for EKF/ESOQ Hybrid Estimator Controlled Rocket
three estimators oscillate with there being little di?erence between the EKF and UKF estimators?
MSEs. The EKF/ESOQ estimator?s MSE stays less than the MSEs of the other estimator after
1.5 s. For pitch angle the UKF yields the worst MSE, which consistently stays near 0.8 units. The
MSEs of the EKF and EKF/ESOQ estimators oscillate with the EKF settling to a slightly higher
value of 0.6 units than the EKF/ESOQ estimator, which settles to around 0.55 units. The yaw
angle MSEs behave similarly to the pitch angle MSEs. The UKF MSE stays consistently around
0.65 units, while the MSEs of the other estimators oscillate. Unlike the pitch angle MSE, the EKF
MSE settles to a lower value of 0.6 while the EKF/ESOQ MSE reaches about 0.7 units.
178
 0
 2
 4
 6
 8
 10
 12
 14
 16
 0  10  20  30  40  50
CEP (normalized)
time (s)
Baseline
Controlled
Difference
Figure 6.51: CEP for UKF/ESOQ Hybrid Estimator Controlled Rocket
 0
 2
 4
 6
 8
 10
 12
 14
 16
 0  10  20  30  40  50
CEP (normalized)
time (s)
Baseline
Controlled
Difference
Figure 6.52: CEP for KF/ESOQ Hybrid Estimator Controlled Rocket
179
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Roll Rate
EKF
UKF
ESOQ/EKF
Figure 6.53: Roll Rate MSE for ESOQ Estimates as Input to an EKF
 0.1
 0.2
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Pitch Rate
EKF
UKF
EKF/ESOQ
Figure 6.54: Pitch Rate MSE for ESOQ Estimates as Input to an EKF
180
 0.1
 0.2
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Yaw Rate
EKF
UKF
EKF/ESOQ
Figure 6.55: Yaw Rate MSE for ESOQ Estimates as Input to an EKF
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Roll
EKF
UKF
EKF/ESOQ
Figure 6.56: Roll Angle MSE for ESOQ Estimates as Input to an EKF
181
 0.1
 0.2
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Pitch
EKF
UKF
EKF/ESOQ
Figure 6.57: Pitch Angle MES for ESOQ Estimates as Input to an EKF
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Yaw
EKF
UKF
EKF/ESOQ
Figure 6.58: Yaw Angle MSE for ESOQ Estimates as Input to and EKF
182
In Figures 6.59-6.64 we show the MSEs for the UKF and EKF estimators as well as the
EKF/ESOQ hybrid estimator, where the EKF is used to estimate angular rates, and the ESOQ
algorithm is used to estimate angular position. As shown in Figures 6.59, 6.60, and 6.61, the roll,
pitch, and yaw rate MSEs behave identically to those found in Figures 6.53-6.55. This is to be
expected, since the EKF and UKF estimators are identical in both cases and the EKF/ESOQ
hybrid is yielding purely EKF estimates for rate estimates. From this point forward UKF and pure
EKF estimator MSEs will not be discussed, since they will be identical to those of Figures 6.53-6.58
for all remaining plots in which they appear. They appear in the plots to more easily compare their
performances to the performance of the ESOQ estimators. In Figure 6.62 we show the roll angle
MSEs. The EKF/ESOQ hybrid estimator has a fairly steady MSE at less than 0.1 units with an
occasional spike for slightly more than 1 s. After 1 s the MSE begins to increase and settles to
around 0.2 units while the EKF and UKF MSEs are greater than 0.55 units. The EKF/ESOQ
hybrid MSE is worse than both the EKF and UKF MSEs for pitch angle as shown in Figure 6.63,
holding steady at 1 unit. The yaw angle MSE is shown in Figure 6.64. The EKF/ESOQ hybrid
oscillates between 0 and 1 units until the rocket?s ?ns open at 1 s. The magnitude of the oscillations
then begins to decrease until the MSE settles to 0.6 units.
183
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Roll Rate
EKF
UKF
ESOQ/EKF
Figure 6.59: Roll Rate MSE for EKF/ESOQ Hybrid
 0.1
 0.2
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Pitch Rate
EKF
UKF
EKF/ESOQ
Figure 6.60: Pitch Rate MSE for EKF/ESOQ Hybrid
184
 0.1
 0.2
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Yaw Rate
EKF
UKF
EKF/ESOQ
Figure 6.61: Yaw Rate MSE for EKF/ESOQ Hybrid
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Roll
EKF
UKF
EKF/ESOQ
Figure 6.62: Roll Angle MSE for EKF/ESOQ Hybrid
185
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Pitch
EKF
UKF
EKF/ESOQ
Figure 6.63: Pitch Angle MSE for EKF/ESOQ Hybrid
 0.1
 0.2
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Yaw
EKF
UKF
EKF/ESOQ
Figure 6.64: Yaw Angle MSE for EKF/ESOQ Hybrid
186
In Figures 6.65-6.70 we show the MSEs for the EKF, UKF, and UKF/ESOQ hybrid estimators.
The UKF/ESOQ hybrid estimator uses the UKF to provide angular rate estimates and the ESOQ
algorithm to provide angular position estimates. The MSE results of the UKF/ESOQ hybrid
estimator are identical to those of the EKF/ESOQ hybrid.
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Roll Rate
EKF
UKF
ESOQ/UKF
Figure 6.65: Roll Rate MSE for UKF/ESOQ Hybrid
187
 0.1
 0.2
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Pitch Rate
EKF
UKF
ESOQ/UKF
Figure 6.66: Pitch Rate MSE for UKF/ESOQ Hybrid
 0.1
 0.2
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Yaw Rate
EKF
UKF
ESOQ/UKF
Figure 6.67: Yaw Rate MSE for UKF/ESOQ Hybrid
188
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Roll
EKF
UKF
ESOQ/UKF
Figure 6.68: Roll Angle MSE for UKF/ESOQ Hybrid
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Pitch
EKF
UKF
ESOQ/UKF
Figure 6.69: Pitch Angle MSE for UKF/ESOQ Hybrid
189
 0.1
 0.2
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Yaw
EKF
UKF
ESOQ/UKF
Figure 6.70: Yaw Angle MSE for UKF/ESOQ Hybrid
190
In Figures 6.71-6.76 we show the MSEs for the EKF, UKF, and KF/ESOQ hybrid estimators.
The KF/ESOQ hybrid estimator uses the KF to provide angular rate estimates and the ESOQ al-
gorithm to provide angular position estimates. We present roll rate MSEs in Figure 6.71. Although
the roll rate is modeled as a known function, we show the roll rate MSE to validate our model. The
MSE spikes when the rocket launches but falls shortly thereafter to remain just above 0 until the
?ns open. Once the ?ns open, the MSE slightly increases but is still under 0.1 units. As shown in
Figure 6.72, the EKF/ESOQ hybrid has a worse MSE than both the EKF and UKF for pitch rate.
While the MSE is initially near 0, it jumps to more than 0.4 units and remains near 0.5 units until
the ?ns open at 1 s. At this point the MSE spikes to 1 unit and oscillates about 0.6 until ?nally
falling to about 0.3 units. Yaw rate MSE for the KF/ESOQ hybrid is almost identical to the pitch
rate MSE as shown in Figure 6.73. As we see in Figures 6.74, 6.75, and 6.76, the roll angle, pitch
angle, and yaw angle MSEs, respectively, are identical to those of Figures 6.68-6.70 since the same
estimator is used.
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Roll Rate
EKF
UKF
ESOQ/KF
Figure 6.71: Roll Rate MSE for KF/ESOQ Hybrid
191
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Pitch Rate
EKF
UKF
ESOQ/KF
Figure 6.72: Pitch Rate MSE for KF/ESOQ Hybrid
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Yaw Rate
EKF
UKF
ESOQ/KF
Figure 6.73: Yaw Rate MSE for KF/ESOQ Hybrid
192
 0
 0.2
 0.4
 0.6
 0.8
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Roll
EKF
UKF
ESOQ/KF
Figure 6.74: Roll Angle MSE for KF/ESOQ Hybrid
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Pitch
EKF
UKF
ESOQ/KF
Figure 6.75: Pitch Angle MSE for KF/ESOQ Hybrid
193
 0.1
 0.2
 0.3
 0.4
 0.5
 0.6
 0.7
 0.8
 0.9
 1
 0  0.5  1  1.5  2
MSE (Scaled)
Time (s)
Yaw
EKF
UKF
ESOQ/KF
Figure 6.76: Yaw Angle MSE for KF/ESOQ Hybrid
194
6.6 Conclusions
We have presented the results of simulating a rocket that is controlled with estimates from a
variety of estimators. The best performing estimators are the KF-based estimators, followed by the
EKF-based estimators. The UKF-based estimators perform poorly, and the pure ESOQ estimators
perform the worst of all. These results directly related to the ability of each estimator to accurately
estimate roll rate. The control law uses the estimate of roll rate to determine if a lateral thruster
will be in the correct position to improve the rocket?s attitude when ?red. As the roll rate estimate
becomes more accurate, the control law is better able to yield the desired attitude. The KF-based
estimators do not estimate roll rate; rather, they use a model of roll rate. Thus, the KF estimators
yield the best results. The EKF- and UKF-based estimators estimate roll rate based on dynamic
equations rather than using a model. The estimated roll rates are not as accurate as the modeled
roll rates so the EKF and UKF do not perform as well as the KF. The worst-performing estimator,
the ESOQ algorithm, does not provide a roll rate estimate at all. The ESOQ algorithm estimates
roll angle, and Euler di?erentiation is used to estimate roll rate. The Euler di?erentiation poorly
approximates roll rate so the ESOQ algorithm has the worst performance.
The importance of roll rate estimation explains why the KF with only rate gyros has better
performance than other estimators with more sensors. The addition of angle gyros and magnetome-
ters does nothing to improve estimates of roll rate. Angle gyros directly measure angular position,
and magnetometers indirectly measure angular position. While the additional sensors do improve
the performance of the EKF and UKF estimators, none of them approach the performance of the
KF estimators.
Aside from the poor roll rate estimation, another factor a?ects the performance of the UKF
estimator. A quaternion formulation of the system is used. The UKF algorithm is performed on
vector space, which is closed under addition; however, rotation quaternions are not a member of
vector space and are not closed under addition. Thus, the method the UKF uses to create sigma
points does not guarantee that a valid attitude quaternion will result from the operation. To ensure
that the quaternion component of the state vector is a valid rotation quaternion, it is normalized
as necessary in the algorithm. This repeated normalization causes the mean and covariance of the
195
random variable not to be preserved. A method to avoid this linearization is proposed by Cheon
and Kim in [68] but was not used in this work.
The best improvement in controlled CEP over the baseline CEP is yielded by the Kalman
?lter with rate gyros and the Kalman ?lter in conjunction with the ESOQ algorithm. The Kalman
?lter with rate gyros, however, is a better choice than the KF/ESOQ hybrid, since the hybrid ?lter
requires rate gyros, angle gyros, and a magnetometer. The KF/ESOQ hybrid is also more compu-
tationally expensive than the KF. The EKF performs essentially the same for each sensor package,
reducing the baseline CEP almost as much as the KF. However, the EKF is computationally more
expensive than the KF. The UKF performs poorly for each case as compared to the performance of
the KF. The UKF?s best performances are for the rate-gyro-only case and the rate-gyro and magne-
tometer case. The UKF?s reduction in CEP is actually better for the rate gyro and magnetometer
package than for the rate gyro and ideal vector package. This is because of the normalization of the
quaternion component during estimation. The noise present on the magnetometer helps counteract
the multiple normalizations. The UKF provides a worse CEP than the baseline CEP for the rate
gyro, angle gyro, and magnetometer sensor suite. Both the angle gyros and magnetometer output
models are nonlinear functions of the attitude quaternion, which exacerbates the e?ects of nor-
malizing the quaternion. Thus, poor estimates result. The best-performing UKF based estimator
is the UKF/ESOQ hybrid. Since the ESOQ algorithm is used to estimate attitude instead of the
UKF, no quaternion normalization is needed. However, the results are still poor compared to the
KF results.
196
Chapter 7
Conclusions and Future Work
In this chapter we summarize the work presented. In ?7.1 we present the contributions this
work has made to the ?eld of state estimation. We conclude the chapter with ?7.2 in which we
pose questions that directly follow from the work presented.
7.1 Contributions
In this work, we have made several contributions. We began by presenting various algorithms
that ?nd solutions to Wahba?s problem. We evaluated these algorithms, presenting their advantages
and disadvantages, to choose the appropriate algorithm for the ballistic rocket problem. Next, we
presented several algorithms that were designed to estimate the constant biases of a magnetometer
on a satellite. After reviewing each algorithm, we selected the algorithm best suited for estimating
the constant biases of a magnetometer on a rocket in a launch mechanism. These algorithms had
never previously been applied to a ballistic projectile since magnetometers are usually found on
orbiting satellites. In chapter 4, we presented several algorithms for controlling a rocket using
reaction jets. After evaluating their merits, we selected the algorithm most appropriate for a rocket
controlled solely by reaction jets for the ?rst few seconds of ?ight.
This work has also made several contributions to the ?eld of state estimation of a ballistic
rocket. We have addressed the issue of state estimation of a rocket that is controlled solely with
a ring of lateral jets. While the control of such a rocket has been previously addressed, state
estimation has not. We also focused on state estimation during the ?rst few seconds of ?ight
in a harsh environment. During this time the rocket is subject to many error sources including
blow-back and tip-o? errors. While previous work has addressed the issue of state estimation of
projectiles during later stages of ?ight, the ?rst few seconds of ?ight had been ignored. We also
compared the e?ect that various MEMS sensor suites had on the state estimation problem. We
197
modeled an angle gyro and implemented it in our simulations. Angle gyros, new developmental
sensors, have never previously been simulated in such a manner. A magnetometer also augmented
some sensor suites. The addition of a magnetometer to a rocket equipped only with inertial sensors
had never previously been done.
We used various state estimators in new ways in this work. We compared the performance
of the KF, EKF, UKF, and ESOQ algorithms for estimating the states of a rotating rocket and
evaluated which estimator performed best. The ESOQ algorithm, which provides a solution to
Wahba?s problem, had never previously been applied to a vehicle other than an orbiting satellite.
The use of the ESOQ algorithm was made possible by adding a magnetometer and angle gyros to
the rocket. We also used a solution to Wahba?s problem (the ESOQ algorithm) in conjunction with
Kalman ?lter based estimators, the EKF and UKF. No previous work has combined any solution
of Wahba?s problem with Kalman ?ltering for estimating the states of a rocket.
7.2 Future Work
Some questions concerning this work have yet to be answered. One area worth exploring is the
addition of more reaction jets to the rocket. What would be the a?ect on accuracy if more reaction
jets were available to apply torques? It is also of interest to study the e?ects of multiple reaction
jets that can be ?red multiple times. If the rocket was equipped with more memory capability,
more possibilities would be available. A magnetic ?eld model of the earth could be stored on the
rocket, which would impact the accuracy of a magnetometer on a rocket. Also, better and more
complex sensor calibration algorithms could be implemented. More memory would also allow a
more accurate model of the rocket to be stored onboard. This leads to the question of how would a
more accurate model a?ect the performance of the various state estimators presented in this work.
198
Bibliography
[1] G. Wahba, ?A least squares estimate of spacecraft attitude,? SIAM Review, vol. 7, no. 3, p.
409, Jul 1965.
[2] J. L. Farrell, J. C. Stuelpnagel, R. H. Wessner, J. Velman, and J. E. Brock, ?Problem 65-1, a
least squares estimate of satellite attitude,? SIAM Review, vol. 8, no. 3, pp. 384?386, Jul 1966.
[3] P. B. Davenport, ?A vector approach to the algebra of rotations with applications,? NASA,
Goddard Space Flight Center Greenbelt, MD, Tech. Rep. TN D-4696, Aug 1968.
[4] J. R. Wertz, Ed., Spacecraft Attitude Determination and Control. D. Reidel Publishing
Company, 1978.
[5] I. Y. Bar-Itzhack, ?REQUEST: A recursive QUEST algorithm for sequential attitude deter-
mination,? Journal of Guidance, Control, and Dynamics, vol. 19, no. 5, pp. 1034?1038, 1996.
[6] M. D. Shuster, ?Maximum likelihood estimation of spacecraft attitude,? Journal of Astronau-
tical Sciences, vol. 37, no. 1, pp. 79?88, Jan-Mar 1989.
[7] ??, ?A simple Kalman ?lter and smoother for spacecraft attitude,? Journal of the Astro-
nautical Sciences, vol. 37, no. 1, pp. 89?106, 1989.
[8] M. L. Psiaki, ?Attitude determination ?ltering via extended quaternion estimation,? Journal
of Guidance, Control, and Dynamics, vol. 23, no. 2, pp. 206?214, 2000.
[9] D. Mortari, ?Energy approach algorithm for attitude determination from vector observations,?
Journal of the Astronautical Sciences, vol. 45, no. 1, pp. 41?55, 1997.
[10] F. L. Markley, ?Attitude determination using vector observations and the singular value de-
composition,? The Jounal of the Astronautical Sciences, vol. 36, no. 3, pp. 245?258, Jul-Sep
1988.
[11] ??, ?Attitude determination using vector observations: A fast optimal matrix algorithm,?
The Journal of Astronautical Sciences, vol. 41, no. 2, pp. 261?280, Apr-Jun 1993.
[12] ??, ?New quaternion attitude estimation method,? Journal of Guidance, Control, and Dy-
namics, vol. 17, no. 2, pp. 407?409, Mar-Apr 1994.
[13] D. Mortari, ?Euler-q algorithm for attitude determination from vector observations,? Journal
of Guidance, Control, and Dynamics, vol. 21, no. 3, pp. 328?334, 1998.
[14] ??, ?ESOQ: A closed-form solution to the Wahba problem,? Journal of the Astronautical
Sciences, vol. 45, no. 2, pp. 195?204, Apr-Jun 1997.
199
[15] ??, ?ESOQ2 single-point algorithm for fast optimal spacecraft attitude determination,? Ad-
vance in the Astronautical Sciences, vol. 95, no. II, pp. 817?826, 1997.
[16] F. L. Markley and I. Y. Bar-Itzhack, ?Unconstrained optimal transformation matrix,? IEEE
Transactions of Aerospace and Electronic Systems, vol. 34, no. 1, pp. 338?340, Jan 1998.
[17] C. M. Roithmayr, ?Contributions of spherical harmonics to magnetic and gravitational
?elds,? NASA, Tech. Rep. TM-2004-213007, Mar 2004, accessed Dec. 12, 2006. [Online].
Available: http://hdl.handle.net/2002/13272
[18] S. McLean, ?IAGA division V-Mod geomagnetic ?eld modeling,? International Association of
Geomagnetism & Aeronomy and the International Union of Geodesy & Geophysics, accessed
Dec 18, 2006. [Online]. Available: http://www.ngdc.noaa.gov/IAGA/vmod/
[19] S. McLean, S. Maus, D. Dater, S. Macmillan, V. Lesur, and A. Thomson, ?The US/UK
world magnetic model for 2005-2010,? NOAA, Tech. Rep. NESDIS/NGDC-1, 2004, accessed
Dec 18, 2006. [Online]. Available: http://www.ngdc.noaa.gov/seg/WMM/DoDWMM.shtml
[20] J. A. McClendon, Jr., ?Full three axis calibration technique,? Master?s thesis, Auburn Univer-
sity, 2002.
[21] R. Alonso and M. D. Shuster, ?Attitude-independent magnetometer-bias determination: A
survey,? Journal of the Astronautical Sciences, vol. 50, no. 4, pp. 453?475, Oct-Dec 2002.
[22] ??, ?Complete linear attitude independent magnetometer calibration,? The Journal of the
Astronautical Sciences, vol. 50, no. 4, pp. 477?490, Oct-Dec 2002.
[23] M. S. Hodgart and P. Tortora, ?A recursive optimised algorithm for in-?ight magnetometer
calibration without knowledge of attitude,? in Proceedings of the 4u1D461? ESA International Con-
ference on Spacecraft Guidance, Navigation and Control Systems, Oct 18-21 1999, pp. 167?172.
[24] P. Tortora, Y. Oshman, and F. Santoni, ?Attitude independent estimation of spacecraft an-
gular rate using geomagnetic ?eld observations,? in Proceedings of the 2003 IEEE Aerospace
Conference, vol. 6, Mar 8-15 2003, pp. 6 2636?6 2645.
[25] J.-I. Lee and I.-J. Ha, ?Autopilot design for highly maneuvering STT missiles via singular
perturbation-like technique,? IEEE Transactions on Control Systems Technology, vol. 7, no. 5,
pp. 527?541, Sep 1999.
[26] J. Y. Choi and D. K. Chwa, ?Adaptive control based on parametric a?ne model for tail-
controlled missiles,? in Proceedings of the 39u1D461? IEEE Conference on Decision and Control,
vol. 2. IEEE, Dec 12-15 2000, pp. 1471?1476.
[27] F.-K. Yeh, K.-Y. Cheng, and L.-C. Fu, ?Variable structure-based nonlinear missile guid-
ance/autopilot design with highly maneuverable actuators,? IEEE Transactions on Control
Systems Technology, vol. 12, no. 6, pp. 944?949, Nov 2004.
[28] S. E. Lyshevski, ?Space transformation method in control of agile interceptors and missiles
with advanced microelectromechanical actuators,? in Proceedings of the 42u1D45Bu1D451 IEEE Conference
on Decision and Control, vol. 5. IEEE, Dec 9-12 2003, pp. 5432?5437.
200
[29] A. Thukral and M. Innocenti, ?A sliding mode missile pitch autopilot synthesis for high angle
of attack maneuvering,? IEEE Transactions on control Systems Technology, vol. 6, no. 3, pp.
359?371, May 1998.
[30] T. L. Song and S. J. Shin, ?Time-optimal impact angle control for vertical plane engagements,?
IEEE Transactions on Aerospace and Electronic Systems, vol. 35, no. 2, pp. 738?742, Apr 1999.
[31] A. V. Savkin, P. N. Pathirana, and F. A. Faruqi, ?The problem of precision missile guidance:
LQR and u1D43B? control frameworks,? in Proceedings of the 40u1D461? IEEE Conference on Decision
and Control, vol. 2. IEEE, Dec 4-7 2001, pp. 1535?1540.
[32] C. Kwan, R. Xu, W. Liu, R. Tan, and L. Haynes, ?Nonlinear control of missile dynamics,? in
Proceedings of the 37u1D461? IEEE Conference on Decision and Control, vol. 4. IEEE, Dec 16-18
1998, pp. 4685?4690.
[33] L.-C. Fu, C.-W. Tsai, and F.-K. Yeh, ?A nonlinear missile guidance controller with pulse type
input devices,? in Proceedings of the 1999 American Control Conference, vol. 6. IEEE, Jun
2-4 1999, pp. 3753?3757.
[34] Y. S. Choi, H. C. Lee, and J. W. Choi, ?Autopilot design for agile missile with aerodynamic
?n and side thruster,? in SICE 2003 Annual Conference, vol. 2. IEEE, Aug 4-6 2003, pp.
1476?1481.
[35] H. C. Lee, J. W. Choi, T. L. Song, and C. H. Song, ?Agile missile autopilot design via
time-varying eigenvalue assignment,? in ICARCV 2004 8u1D461? Control, Automation, Robotics
and Vision Conference, vol. 3, Dec 6-9 2004, pp. 1832?1837.
[36] M. B. McFarland and A. J. Calise, ?Adaptive nonlinear control of agile antiair missiles using
neural networks,? IEEE Transactions on Control Systems Technology, vol. 8, no. 5, pp. 749?
756, Sep 2000.
[37] M. Innocenti and A. Thukral, ?Robustness of a variable structure control system for maneuver-
able ?ight vehicles,? Journal of Guidance, Control, and Dynamics, vol. 12, no. 2, pp. 377?383,
Mar-Apr 1997.
[38] A. Thukral and M. Innocenti, ?Variable structure autopilot for high angle of attack maneuvers
using on-o? thrusters,? in Proceedings of the 33u1D45Fu1D451 IEEE Conference on Decision and Control,
vol. 4. IEEE, Dec 14-16 1994, pp. 3850?3851.
[39] W. Tan, A. K. Packard, and G. J. Balas, ?Quasi-LPV modeling and LPV control of a generic
missile,? in Proceedings of the American Control Conference, vol. 5, Jun 28-30 2000, pp. 3692?
3696.
[40] H. S. Nam, S.-H. Kima, C. Song, and J. Lyou, ?A robust nonlinear control approach to missile
autopilot design,? in Proceedings of the 40u1D461? SICE Annual Conference. International Session
Papers. IEEE, Jul 25-27 2001, pp. 310?314.
[41] J.-Y. Yu, Y.-A. Zhang, and W.-J. Gu, ?An approach to integrated guidance/autopilot design
for missiles based on terminal sliding mode control,? in Proceedings of 2004 International
Confernce on Machine Learning and Cybernetics, vol. 1, Aug 26-29 2004, pp. 610?615.
201
[42] H. Zhao, W. Gu, Y. Hu, and C. Pan, ?Second-order sliding mode control for aerodynamic
missiles using backstepping design,? in Proceedings of the 5u1D461? World Congress on Intelligent
Control and Automation, vol. 6, Jun 15-19 2004, pp. 5471?5474.
[43] R. Hindman and W. M. Shell, ?Missile autopilot design using adaptive nonlinear dynamic
inversion,? in Proceedings of the 2005 American Control Conference, vol. 6, Jun 8-10 2005, pp.
3918?3919.
[44] D. Malloy and B. C. Chang, ?Regulator design for linear parameter varying systems using
dynamic inversion,? in Proceedings of the 35u1D461? Conference on Decision and Control, vol. 2.
IEEE, Dec 11-13 1996, pp. 2271?2276.
[45] Y.-Y. Chen, Bor-SenChen, and C.-S. Tseng, ?Adaptive fuzzy mixed u1D43B2/u1D43B? lateral control
of nonlinear missile systems,? in Proceedings of the 126u1D461? IEEE International Conference on
Fuzzy Systems, vol. 1. IEEE, May 25-28 2003, pp. 512?516.
[46] D. Dumlu and K. ?Ozcaldiran, ?Design of a ?xed u1D43B? controller for a missile,? in Proceedings
of the 1998 IEEE International Conference on Control Applications, vol. 2. IEEE, Sep 1-4
1998, pp. 985?989.
[47] I. Astrov, A. Pedai, and E. R?ustern, ?Simulation of two-rate adaptive hybrid control with
neural and neuro-fuzzy networks for stochastic model of missile autopilot,? in Proceedings of
the 5u1D461? World Congress on Intelligent Control and Automation. IEEE, Jun 15-19 2004, pp.
2603?2607.
[48] A. Elsakann, A. Bahnasawi, and M. Elamir, ?High angle of attack maneuvering via intelligent
approaches,? in Proceedings of the 46u1D461? IEEE International Midwest Symposium on Circuits
and Systems, vol. 3. IEEE, Dec 27-30 2003, pp. 1039?1042.
[49] Y. Yuan, Y. Feng, and W. Gu, ?Fuzzy model reference learning control for aircraft pitch au-
topilot design,? in ICARCV 2004 8u1D461? Control, Automation, Robotics, and Vision Conference,
vol. 3, Dec 6-9 2004, pp. 1923?1927.
[50] T. Kuhn, ?Aspects of pure and satellite-aided inertial navigation for gun-launched munitions,?
in IEEE PLANS, Position Location and Navigation Symposium, 2004, pp. 327?336.
[51] J. Thienel and R. M. Sanner, ?A coupled nonlinear spacecraft attitude controller and ob-
server with an unknown constant gyro bias and gyro noise,? IEEE Transactions on Automatic
Control, vol. 48, no. 11, pp. 2011?2015, Nov 2003.
[52] D. Gebre-Egziabher, G. H.Elkaim, J. D. Powell, andB. W. Parkinson, ?A gyro-free quaternion-
based attitude determination system suitable for implementation using low cost sensors,? in
Position Location and Navigation Symposium, IEEE 2000, Mar 13-16 2000, pp. 185?192.
[53] R. D. Weil and K. A. Wise, ?Blended aero & reaction jet missile autopilot design using VSS
techniques,? in Proceedings of the 30u1D461? Conference on Decision and Control, vol. 3. IEEE,
Dec 11-13 1991, pp. 2828?2829.
[54] E. Jahangir, ?Time-optimal attitude control of a spinning missile,? Ph.D. dissertation, Uni-
versity of Michigan, 1990.
202
[55] E. Jahangir and R. M. Howe, ?A two-pulse scheme for the time-optimal attitude control of
a spinning missile,? in AIAA Guidance, Navigation and Control Conference, Aug 20-22 1990,
pp. 550?560.
[56] ??, ?A scheme to generate thruster ?ring times for the time-optimal reorientation of a
spinning missile,? in 29u1D461? Aerospace Sciences Meeting, Jan 7-10 1991, pp. 1?11.
[57] ??, ?Time-optimal attitude control scheme for a spinning missile,? Journal of Guidance,
Control, and Dynamics, vol. 16, no. 2, pp. 346?353, Mar-Apr 1993.
[58] T. Jitpraphai, B. Burchett, and M. Costello, ?A comparsion of di?erent guidance schemes for a
direct ?re rocket with a pulse jet control mechanism,? U.S. Army Research Laboratory, Tech.
Rep. ARL-CR-493, Apr 2002.
[59] T. Jitpraphai and M. Costello, ?Dispersion reduction of a direct ?re rocket using lateral pulse
jets,? Journal of Spacecraft and Rockets, vol. 38, no. 6, pp. 929?936, Nov-Dec 2001.
[60] B. Burchett and M. Costello, ?Model predictive lateral pulse jet control of an atmospheric
rocket,? Journal of Guidance, Control, and Dynamics, vol. 25, no. 5, pp. 860?867, Sep-Oct
2002.
[61] A. Anderson, D. Bittle, R. Dean, G. Flowers, J. Hester, and A. Hodel, ?Investigation of mal-
launch correction in spin-stabilized rockets,? in Proceedings of IEEE SoutheastCon, Mar 2007,
pp. 267?272.
[62] G. H. Golub and C. F. V. Loan, Matrix Computations, 3rd ed. 2715 North Charles Street,
Baltimore, MD 21218-4363: The John Hopkins University Press, 1996.
[63] A. M. Shkel, C. Akar, and C. Painter, ?Two types of micromachined vibratory gyrscopes,? in
Proceedings of Fourth IEEE Conference on Sensors, Oct-Nov 2005, pp. 531?536.
[64] C. Painter and A. Shkel, ?Active structural error suppression in MEMS vibratory gyroscopes,?
in Proceedings of the First IEEE International Conference on Sensors, vol. 1, no. 2, Jun 12-14
2002, pp. 1089?1094.
[65] C. C. Painter, ?Micromachined vibratory gyroscopes with imperfections,? Ph.D. dissertation,
University of California, Irvine, 2005.
[66] S. Julier and J. Uhlmann, ?A new extension of the Kalman ?lter to nonlinear systems,? in
Int. Symp. Aerospace/Defense Sensing, Simul. and Controls, Orlando, FL, 1997. [Online].
Available: citeseer.ist.psu.edu/julier97new.html
[67] A. E. Taub and M. A. Thomas, ?Con?cence intervals for CEP when the errors are elliptical
normal,? Naval Surface Weapons Center, Tech. Rep. NSWC TR 86-205, Nov 1983.
[68] Y.-J. Cheon and J.-H. Kim, ?Unscented ?ltering in a unit quaternion space for spacecraft
attitude estimation,? in IEEE International Symposium on Industrial Electronics, Jun 4-7
2007, pp. 66?71.
[69] J. B. Kuipers, Quaternions and Rotation Sequences: A Primer with Applications to Orbits,
Aerospace, and Virtual Reality. Princeton, New Jersey: Princeton University Press, 1999.
203
[70] W. F. Phillips, C. E. Hailey, and G. A. Gebert, ?Review of attitude representations used for
aircraft kinematics,? Journal of Aircraft, pp. 718?737, Jul/Aug 2001.
[71] W. J. Thompson, Atlas for Computing Mathematical Functions. New York: John Wiley &
Sons, INC., 1997.
[72] W. D. Parkinson, Introduction to Geomagnetism. Edinburgh, Scottland: Scottish Academic
Press, Ltd., 1984.
[73] R. F. Stengel, Optimal Control and Estimation. New York: Dover Publications, Inc., 1994.
[74] S. Julier and J. K. Uhlmann, ?A general method for approximating nonlinear transformations
of probability distributions,? Department of EngineeringScience, University of Oxford, Oxford,
Tech. Rep. OX1 3PI UK, Nov 1996.
[75] E. A. Wan and R. van der Merwe, ?The unscented Kalman ?lter for nonlinear estimation,? in
Adaptive Systems for Signal Processing, Communications, and Control Symposium 2000, Oct
1-4 2000, pp. 153?158.
[76] R. van der Merwe and E. A. Wan, ?The square-root unscented Kalman ?lter for state and
parameter-estimation,? in 2001 IEEE International Conference on Acoustics, Speech, and Sig-
nal Processing, vol. 6, May 7-11 2001, pp. 3461?3464.
[77] D. Rick, ?Deriving the haversine formula,? The Math Forum, April 1999, accessed Dec 18,
2006. [Online]. Available: http://mathforum.org/library/drmath/view/51897.html
[78] E. D. Kaplan, Ed., Understanding GPS Principles and Applications. Boston: Artech House,
1996.
204
Appendices
205
Appendix A
Rotation Sequences
A common problem that arises in modeling the dynamics of a moving body is changing from
one reference frame to another. Many reference frames exist, with the two most common being
the inertial reference frame and the body-?xed reference frame. An inertial reference frame has its
axes ?xed relative to the motion of the body. Its positive u1D465-axis points north, its positive u1D466-axis
points east, and its positive u1D467-axis points down. A body-?xed reference frame usually has its origin
located at the body?s center of gravity. Its positive u1D465-axis points toward the front of the vehicle,
such as toward the nose of an airplane. The positive u1D466-axis points to the vehicle?s right, and the
positiveu1D467-axis points down. A representation of these two coordinate frames is shown in Figure 2.1.
Converting between the inertial and body frames or any other reference frame involves sequences of
rotations. The most common way to represent these rotation sequences is either via Euler angles or
quaternions. References [69] and [70] give a detailed treatment of the mathematics and applications
of quaternions as well as Euler angles.
A.1 Euler Angles
The group SO(3) is comprised of all 3 ? 3 orthogonal matrices with a determinant of +1.
Applying a matrix in SO(3) to a vector is equivalent to rotating that vector by some angle around
a ?xed axis. One way of describing the rotation is with Euler angles and Euler axes. An Euler angle
is de?ned as the angle of rotation about a coordinate axis. By applying these angles of rotation in
a speci?c order, one reference frame can be rotated into another. Euler?s theorem states: ?Any two
independent orthonormal coordinate frames can be related by a sequence of rotations (not more
than three) about coordinate axes, where no two successive rotations may be about the same axis?
[69]. While there exist twelve di?erent sequences of rotations, the sequence z-y-x is commonly used
in aerospace applications.
206
In Figure 2.1 the Euler angles are labeled. A rotation about the z-axis is called yaw (u1D713), a
rotation about the y-axis is pitch (u1D703), and a rotation about the x-axis is roll (u1D719). Given the angles
u1D719, u1D703, and u1D713, any vector in one coordinate frame can be rotated into another coordinate frame. To
rotate from the inertial frame to the body frame
xu1D435 =
?
??
??
?
u1D450u1D713u1D450u1D703 u1D460u1D713u1D450u1D703 ?u1D460u1D703
u1D450u1D713u1D460u1D703u1D460u1D719 ?u1D460u1D713u1D450u1D719 u1D460u1D713u1D460u1D703u1D460u1D719 +u1D450u1D713u1D450u1D719 u1D450u1D703u1D460u1D719
u1D450u1D713u1D460u1D703u1D450u1D719 +u1D460u1D713u1D460u1D719 u1D460u1D713u1D460u1D703u1D450u1D719 ?u1D450u1D713u1D460u1D719 u1D450u1D703u1D450u1D719
?
??
??
?
xu1D43C
xu1D435 = u1D445u1D43Cu1D435xu1D43C
whereu1D450u1D465 = cos(u1D465) andu1D460u1D465 = sin(u1D465). u1D445u1D43Cu1D435 is a rotation matrix, so it is orthogonal, and its inverse is its
transpose. Therefore, to rotate from the body frame to the inertial frame xu1D43C = u1D445u1D447u1D43Cu1D435xu1D435 = u1D445u1D435u1D43Cxu1D435.
A di?culty arises with using Euler angles to represent rotations. No matter what rotation order is
used, there is always a singularity at some angle.
A.2 Quaternions
A quaternion can be thought of as the composition of a scalar value and a three-vector:
u1D45E = u1D45E0 +q = u1D45E0 +iu1D45E1 +ju1D45E2 +ku1D45E3.
Two quaternions u1D45D and u1D45E are equal if u1D45Du1D456 =u1D45Eu1D456 for u1D456 = 0,1,2,3. The sum of two quaternions is de?ned
as
u1D45D+u1D45E = (u1D45D0 +u1D45E0) +i(u1D45D1 +u1D45E1) +j(u1D45D2 +u1D45E2) +k(u1D45D3 +u1D45E3).
The product (which is not commutative) of two quaternions is
u1D45Du1D45E = u1D45D0u1D45E0 ?q?q+u1D45D0q+u1D45E0p+p?q.
207
The complex conjugate of a quaternion u1D45E = u1D45E0 +q is u1D45E? = u1D45E0 ?q, and the norm of a quaternion is
?u1D45E? = ?u1D45E?u1D45E = ?u1D45Eu1D45E?. A quaternion?s inverse is
u1D45E?1 = u1D45E
?
?u1D45E?2.
The derivative of a quaternion is another quaternion. Suppose u1D714 is the angular rate at which
a rotation is occurring about some unit vector ? = iu1D7141 +ju1D7142 +ku1D7143. Then
?u1D45E = u1D714u1D45E?
and
?u1D45E? = ?u1D714?u1D45E?.
These can be written in matrix form as
?u1D45E = u1D714
?
??
??
??
??
0 ?u1D7141 ?u1D7142 ?u1D7143
u1D7141 0 u1D7143 ?u1D7142
u1D7142 ?u1D7143 0 u1D7141
u1D7143 u1D7142 ?u1D7141 0
?
??
??
??
??
u1D45E
and
?u1D45E? = u1D714
?
??
??
??
??
0 u1D7141 u1D7142 u1D7143
?u1D7141 0 u1D7143 ?u1D7142
?u1D7142 ?u1D7143 0 u1D7141
?u1D7143 u1D7142 ?u1D7141 0
?
??
??
??
??
u1D45E?.
Quaternions are a way to represent rotations without singularities. To rotate a vector in ?3,
view the vector as a quaternion with its real part equal to zero (a pure quaternion). Then the
rotation of a vector v via a quaternion u1D45E is de?ned as
w = u1D45Evu1D45E?,
208
which is a linear operation. A series of rotations on vector v by quaternion u1D45D then u1D45E is de?ned as
w = u1D45Eu1D45Dvu1D45D?u1D45E?.
A unit quaternion may be written as
u1D45E = u1D45E0 +q = cos(u1D703)+usin(u1D703). (A.1)
The rotation of vector v by a unit quaternion u1D45E as de?ned in Equation (A.1) may be interpreted
as a rotation of v through an angle of 2u1D703 about the axis q. So to rotate some vector an angle of u1D6FC
about a vector q, apply a rotation with the following quaternion
u1D45E = u1D45E0 +q = cos(u1D6FC2) +usin(u1D6FC2).
209
Appendix B
Spherical Mathematics
Since the earth is often modeled as a sphere, we present math associated with spheres that
is used in modeling the earth?s magnetic ?eld. We begin by presenting spherical coordinates and
a method to convert between Cartesian and spherical coordinates. Next, we present Legendre
functions. We conclude the chapter by linking Legendre functions with Laplace?s equation to yield
the equations used to model the earth?s magnetic ?eld.
B.1 Spherical Coordinate System
A common coordinate system used in situations where points are located on a sphere is the
spherical coordinate system. Points in the spherical coordinate system are represented by three
values: a magnitude u1D45F, an azimuth angle u1D719, and an elevation angle u1D703. A point u1D443 in spherical
coordinates in reference to Cartesian axes is shown in Figure B.1. Let u1D443 be located at (u1D45F,u1D703,u1D719) in
spherical points. The variable u1D45F ? 0 describes u1D443?s distance from the origin. The variable u1D703 varies
between 0 and u1D70B and describes the angle between the u1D467-axis and some vector from the origin to
u1D443. u1D719 varies between 0 and 2u1D70B. This variable describes the angle between the u1D465-axis and the vector
from the origin to the projection of point u1D443 in the xy plane.
Now let the same point u1D443 be located at (u1D465,u1D466,u1D467) in Cartesian coordinates. Equations to
convert between the spherical and Cartesian coordinates can now be developed. Since u1D45F is the
distance between the origin and point u1D443,
u1D45F =
uni221A.alt01
u1D4652 +u1D4662 +u1D4672. (B.1)
Then from trigonometry
u1D703 = arccos
uni0028.alt02u1D467
u1D45F
uni0029.alt02
, (B.2)
210
Figure B.1: The Spherical Coordinate System
and
u1D719= arctan
uni0028.alt02u1D466
u1D465
uni0029.alt02
. (B.3)
By rearranging Equation (B.2)
u1D467 = u1D45Fcos(u1D703). (B.4)
From simple trigonometry the length of the projection of pointu1D443 onto the xy plane isu1D45Fcos(u1D70B2 ?u1D703) =
u1D45Fsin(u1D703). Then
u1D465= u1D45Fsin(u1D703)cos(u1D719), (B.5)
and
u1D466 = u1D45Fsin(u1D703)sin(u1D719). (B.6)
211
B.2 Legendre Functions
Legendre functions are de?ned in terms of solutions to Legendre?s di?erential equation
(1?u1D4672)u1D451
2u1D464
u1D451u1D4672 ?2u1D467
u1D451u1D464
u1D451u1D467 +[u1D708(u1D708 +1)?
u1D7072
1?u1D4672]u1D464 = 0
where u1D467, u1D708, and u1D707 are complex variables [71]. The variables u1D708 and u1D707 are said to be the degree and
order, respectively, of the system. Because the di?erential equation is second order, two linearly
independent solutions exist for each u1D467,u1D708, andu1D707. The regular solution atu1D467 = ?1 is called a Legendre
function of the ?rst kind and shall be labeled u1D443u1D707u1D708 (u1D467). Likewise, the irregular solution at u1D467 = ?1
is called a Legendre function of the second kind and shall be labeled as u1D444u1D707u1D708(u1D467). The solutions to
Legendre?s di?erential equation can be written equivalently in several forms.
It is common to use Legendre functions in the spherical coordinate system. Legendre functions
of the ?rst kind with integer degree and order have symmetry in spherical coordinates. De?ne a
modi?ed Legendre function as
u1D446u1D45Au1D45B (u1D465) =
uni221A.alt04
(u1D45B?u1D45A)!
(u1D45B+u1D45A)!u1D443
u1D45A
u1D45B (u1D465) u1D45B? 0, u1D45B?u1D45A.
Then
u1D443u1D45A?u1D45B(u1D465) = u1D443u1D45Au1D45B?1(u1D465),
u1D443?u1D45Au1D45B (u1D465) = (u1D45B?u1D45A)!(u1D45B+u1D45A)!u1D443u1D45Au1D45B (u1D465) u1D45B? 0,
u1D446?u1D45Au1D45B (u1D465) = u1D446u1D45Au1D45B (u1D465) u1D45B? 0,
and
u1D443u1D45Au1D45B (?u1D465) = (?1)u1D45B?u1D45Au1D443u1D45Au1D45B (u1D465).
212
For a ?xed u1D45B, the Legendre functions can be de?ned recursively:
u1D443u1D45Au1D45B (u1D465) = [(2u1D45B?1)u1D465u1D443u1D45Au1D45B?1(u1D465)?(u1D45B+u1D45A?1)u1D443u1D45Au1D45B?2(u1D465)]/(u1D45B?u1D45A)
u1D446u1D45Au1D45B (u1D465) = [(2u1D45B?1)u1D465u1D446u1D45Au1D45B?1(u1D465)?
uni221A.alt01
(u1D45B?1)2 ?u1D45A2u1D446u1D45Au1D45B?2(u1D465)]/
uni221A.alt01
u1D45B2 ?u1D45A2.
The recursion begins with
u1D443u1D45Au1D45A?1(u1D465) = 0
u1D443u1D45Au1D45A(u1D465) = (2u1D45A)!(1?u1D465
2)u1D45A/2
2u1D45Au1D45A!
u1D446u1D45Au1D45A?1(u1D465) = 0
u1D446u1D45Au1D45A(u1D465) =
uni221A.alt01(2u1D45A)!(1?u1D4652)u1D45A/2
2u1D45Au1D45A! .
B.3 Laplace?s Equation
Laplace?s equation is often encountered when studying and modeling the earth?s magnetic ?eld.
In this section the solution of Laplace?s equation in spherical coordinates is derived. The solution
is then related to Legendre functions. The proceeding analysis is taken from [72].
B.3.1 Solution Derivation
In the spherical coordinate system, Laplace?s equation is
?
?u1D45F
uni0028.alt03
u1D45F2?u1D449?u1D45F
uni0029.alt03
+ 1sin(u1D703) ??u1D703
uni0028.alt03
sin(u1D703)?u1D449?u1D703
uni0029.alt03
+ 1sin2(u1D703)
uni0028.alt03?2u1D449
?u1D7192
uni0029.alt03
= 0. (B.7)
Assume that the solution to Laplace?s equation is variable-separable such that
u1D449(u1D45F,u1D703,u1D719) = u1D445(u1D45F)u1D446(u1D703u1D719).
213
Then substituting the solution form into Equation (B.7)
0 = ??u1D45F
uni0028.alt03
u1D45F2 ??u1D45F(u1D445u1D446)
uni0029.alt03
+ 1sin(u1D703) ??u1D703
uni0028.alt03
sin(u1D703) ??u1D703(u1D445u1D446)
uni0029.alt03
+ 1sin2(u1D703)
uni0028.alt03 ?2
?u1D7192(u1D445u1D446)
uni0029.alt03
= ??u1D45F
uni0028.alt03
u1D45F2
uni005B.alt03
u1D445?0+u1D446?u1D445?u1D45F
uni005D.alt03uni0029.alt03
+ 1sin(u1D703) ??u1D703
uni0028.alt03
sin(u1D703)
uni005B.alt03
u1D445?u1D446?u1D703 +u1D446?0
uni005D.alt03uni0029.alt03
+
1
sin2(u1D703)
?
?u1D719
uni005B.alt03
u1D445?u1D446?u1D719 +u1D446?0
uni005D.alt03
= ??u1D45F
uni0028.alt03
u1D446u1D45F2?u1D445?u1D45F
uni0029.alt03
+ 1sin(u1D703) ??u1D703
uni0028.alt03
u1D445sin(u1D703)?u1D446?u1D703
uni0029.alt03
+ 1sin2(u1D703) ??u1D719
uni0028.alt03
u1D445?u1D446?u1D719
uni0029.alt03
= u1D446 ??u1D45F
uni0028.alt03
u1D45F2?u1D445?u1D45F +0?u1D45F2?u1D445?u1D45F
uni0029.alt03
+ 1sin(u1D703)
uni005B.alt03
u1D445 ??u1D703
uni0028.alt03
sin(u1D703)?u1D446?u1D703
uni0029.alt03
+0?sin(u1D703)?u1D446?u1D703
uni005D.alt03
+
1
sin2(u1D703)
uni005B.alt03
u1D445?
2u1D446
?u1D7192 +0?
?u1D446
?u1D719
uni005D.alt03
= u1D446 ??u1D45F
uni0028.alt03
u1D45F2?u1D445?u1D45F
uni0029.alt03
+u1D445 1sin(u1D703) ??u1D703
uni0028.alt03
sin(u1D703)?u1D446?u1D703
uni0029.alt03
+u1D445 1sin2(u1D703)?
2u1D446
?u1D7192.
Divide by u1D445u1D446 to yield
0 = 1u1D445 ??u1D45F
uni0028.alt03
u1D45F2?u1D445?u1D45F
uni0029.alt03
+ 1u1D446sin(u1D703) ??(u1D703)
uni0028.alt03
sin(u1D703)?u1D446?u1D703
uni0029.alt03
+ 1u1D446sin2(u1D703)?
2u1D446
?u1D7192,
and rearrange
1
u1D445
u1D451
u1D451u1D45F
uni0028.alt03
u1D45F2u1D451u1D445u1D451u1D45F
uni0029.alt03
= ?(u1D446sin(u1D703))?1 ??u1D703
uni0028.alt03
sin(u1D703)?u1D446?u1D703
uni0029.alt03
?(u1D446sin2(u1D703))?1?
2u1D446
?u1D7192. (B.8)
The left-hand side of Equation (B.8) depends only onu1D45F, and the right-hand side of the equation
depends only on u1D703 and u1D719. Both sides are then equal to some constant, assigned value u1D45B(u1D45B+1), since
the left and right sides must be equal for all values of u1D45F, u1D703, and u1D719. The left-hand side of Equation
(B.8) can be expressed as
u1D451
u1D451u1D45F
uni0028.alt03
u1D45F2u1D451u1D445u1D451u1D45F
uni0029.alt03
?u1D445u1D45B(u1D45B+1) = 0,
which has solutions u1D445 = u1D45Fu1D45B and u1D445 =u1D45F?u1D45B?1. The right-hand side is written as
1
sin(u1D703)
?
?u1D703
uni0028.alt03
sin(u1D703)?u1D446?u1D703
uni0029.alt03
+sin2(u1D703)?
2u1D446
?u1D7192 +u1D446u1D45B(u1D45B+1) = 0.
214
Assume now that the solution u1D446 is variable separable such that
u1D446(u1D703,u1D719) = u1D447(u1D703)u1D439(u1D719).
Then
sin(u1D703)
u1D447
u1D451
u1D451u1D703
uni0028.alt03
sin(u1D703)u1D451u1D447u1D451u1D703
uni0029.alt03
+sin2(u1D703)u1D45B(u1D45B+1) = ? 1u1D439 u1D451
2u1D439
u1D451u1D7192 .
By the same logic as above, both sides of the equation are equal to some constant, u1D45A2. Then
u1D451u1D439
u1D451u1D719 +u1D439u1D45A
2 = 0
has solutions u1D439 = cos(u1D45Au1D719) and u1D439 = sin(u1D45Au1D719). The right-hand side is written as
u1D451
u1D451u1D703
uni0028.alt03
sin(u1D703)u1D451u1D447u1D451u1D703
uni0029.alt03
+u1D447
uni005B.alt03
u1D45B(u1D45B+1)sin(u1D703)? u1D45A
2
sin(u1D703)
uni005D.alt03
= 0. (B.9)
B.3.2 Relationship with Legendre Functions and Spherical Harmonics
If in Equation (B.9) u1D45A = 0, the equation becomes Legendre?s equation
u1D451
u1D451u1D467
uni005B.alt03
(1?u1D4672)u1D451u1D447u1D451u1D467
uni005D.alt03
+u1D45B(u1D45B+1)u1D447 = 0 (B.10)
where u1D467 = cosu1D703. Assume that the solution u1D447 is in power series form to yield
u1D447 = u1D443u1D45B(u1D467) =
u1D458uni2211.alt02
u1D457=0
(?1)u1D457 (2u1D45B?2u1D457)!2u1D45Bu1D457!(u1D45B?u1D457)!(u1D45B?2u1D457)!u1D467u1D45B?2u1D457 (B.11)
215
where u1D458 is an integer equal either to 12u1D45B or 12(u1D45B?1). u1D443u1D45B(u1D467) is known as a Legendre polynomial or
a Legendre function of the ?rst kind. The ?rst few Legendre polynomials are
u1D4430(u1D467) = 1
u1D4431(u1D467) = u1D467
u1D4432(u1D467) = 12(3u1D4672 ?1)
u1D4433(u1D467) = 12(5u1D4673 ?3u1D467)
Solutions of Equation (B.9) are also called associated Legendre functions u1D443u1D45Au1D45B (cos(u1D703)) since they
can be derived by di?erentiating Equation (B.10) u1D45A times
u1D443u1D45Au1D45B (cos(u1D703)) = u1D441sinu1D45A(u1D703) u1D451
u1D45A
u1D451(cos(u1D703))u1D45Au1D443u1D45Bcos(u1D703).
u1D441 is the Schmidt normalizing factor
u1D4412 = 2(u1D45B?u1D45A)!(u1D45B+u1D45A)!,
which makes the average value of (u1D443u1D45Au1D45B (cosu1D703))2 constant over the surface of a sphere for all u1D45A. The
?rst few functions u1D443u1D45Au1D45B (cos(u1D703)) are
unnormalized Schmidt normalized
u1D44311 (cos(u1D703)) sin(u1D703) sin(u1D703)
u1D44312 (cos(u1D703)) 3sin(u1D703)cos(u1D703) 3?1/2 sin(u1D703)cos(u1D703)
u1D44322 (cos(u1D703)) 3sin2(u1D703) 12?1/2 sin2(u1D703)
A solution of Laplace?s equation can then be written as
u1D449 = (u1D434u1D45Fu1D45B +u1D435u1D45F?u1D45B?1)(u1D434? cos(u1D45Au1D719) +u1D435?sin(u1D45Au1D719))u1D443u1D45Au1D45B (cos(u1D703)) u1D45A?u1D45B (B.12)
216
where u1D45B and u1D45A are integers. The last two factors of Equation (B.12) can be combined for all
possible values of u1D45A to give the surface spherical harmonic
u1D446u1D45B(u1D703,u1D719) = u1D44E0u1D443u1D45B(cos(u1D703))+
u1D45Buni2211.alt02
u1D45A=1
(u1D44Eu1D45Acos(u1D45Au1D719) +u1D44Fu1D45Asin(u1D45Au1D719))u1D443u1D45Au1D45B (cos(u1D703)).
Surface spherical harmonics (derived from the Fourier series) are of the form
u1D44C(u1D703,u1D719) =
?uni2211.alt02
u1D45B=0
u1D45Buni2211.alt02
u1D45A=0
u1D443u1D45Au1D45B (cos(u1D703))(u1D44Eu1D45Au1D45B cos(u1D45Au1D719)+u1D44Fu1D45Au1D45B sin(u1D45Au1D719))
where u1D44Eu1D45Au1D45B and u1D44Fu1D45Au1D45B are coe?cients that can be used to de?ne an arbitrary function (meeting some
continuity conditions) on the surface of a sphere.
217
Appendix C
Kalman Filtering
Assume it is desired to know the states of the system
?x = u1D434x+u1D435u+w (C.1)
y = u1D436x+v (C.2)
where w is zero mean Gaussian process noise and v is zero mean Gaussian measurement noise. Let
u1D444u1D450 = u1D438[wwu1D447], and u1D445 = u1D438[vvu1D447]. w and v are also assumed to be uncorrelated. Further assume
measurements of some of the states are unavailable. The Kalman ?lter is an optimal means of
estimating states in the sense that it produces unbiased estimates (u1D438[e] = 0 where e = x? ?x) and
minimizes the error covariance matrix (u1D443 = u1D438[eeu1D461]).
C.1 Linear Systems
Since sensor measurements usually occur at regular intervals, the Kalman ?lter is most often
performed on discrete systems. The system of Equations (C.1)-(C.2) discretized for some interval
?u1D461 is
xu1D458+1 = u1D434u1D451xu1D458 +u1D435u1D451uu1D458 +wu1D458 (C.3)
yu1D458 = u1D436u1D451xu1D458 +vu1D458 (C.4)
where u1D444u1D451 = u1D438[wu1D458wu1D447u1D458], and u1D445u1D451 = u1D438[vu1D458vu1D447u1D458]. The Kalman ?lter consists of two stages, the time
update stage and the measurement update stage, which are performed in a continuous cycle. Time
218
update can be considered a prediction stage that predicts states before measurements are consid-
ered. Measurement update can be considered a correction stage in which the predicted state from
the time step are corrected with actual measurements.
The Kalman ?lter for linear systems is given in Algorithm C.1.1. A derivation of the equations
can be found in [73]. A superscript minus sign in the algorithm denotes a time step variable while a
superscript plus sign denotes a measurement update variable. If care is taken, the error covariance
matrix u1D443 can be viewed as a measure of how good the ?lter?s estimate is. As a word of caution
when using the Kalman ?lter: In order for the Kalman ?lter?s estimates to be reliable, the system
must be observable.
Data: ?x0 and u1D4430
Result: State estimate ?x and error covariance matrix u1D443
while Not done do
Time Update;
?x?u1D458+1 = u1D434u1D451?x+u1D458 +u1D435u1D451uu1D458;
u1D443?u1D458+1 = u1D434u1D451u1D443+u1D458 u1D434u1D447u1D451 +u1D444u1D451;
Gain calculation: u1D43Fu1D458+1 = u1D443?u1D458+1u1D436u1D447u1D451 [u1D436u1D451u1D443?u1D458+1u1D436u1D447u1D451 +u1D445u1D451]?1;
Measurement Update;
?x+u1D458+1 = ?x?u1D458+1 +u1D43Fu1D458+1(u1D466?u1D436u1D451?x?u1D458+1);
u1D443+u1D458+1 = (u1D43C ?u1D43Fu1D451u1D436u1D451)u1D443?u1D458+1;
end
Algorithm C.1.1: Kalman Filter
C.2 Extended Kalman Filter
Suppose a nonlinear system is de?ned by
?x(u1D461) = u1D453(x,u1D461)+u1D454(x,u1D461) +w(u1D461)
y(u1D461) = ?(x,u1D461) +u1D708(u1D461)
where w and u1D708 are zero-mean Gaussian noise with u1D438[wwu1D447] = u1D444 and u1D438[u1D708u1D708u1D447] = u1D445. The Kalman
?lter is applied only to linear systems. The Extended Kalman Filter (EKF) is the estimator used
for nonlinear systems. The EKF involves using the nonlinear equations in the prediction step and
219
a linearized version of the system in the correction step. Before the EKF algorithm is begun, the
Jacobian of u1D453(x,u1D461) and ?(x,u1D461) are found: u1D439 = ?u1D453(x,u1D461)?x and u1D43B = ??(x,u1D461)?x . The discrete matrices u1D439u1D451
and u1D43Bu1D451 can be found by applying the matrix exponential: u1D439u1D451 = u1D452u1D439?u1D461 and u1D43Bu1D451 = u1D452u1D43B?u1D461.
Once the preliminary steps are completed, the EKF algorithm can be performed. A time
update is performed by using the nonlinear equations to predict the next state via numerical
integration. An error covariance matrix is then computed from discrete matrices u1D439u1D451 and u1D444u1D451. The
next step is the measurement update. A gain matrixu1D43Fu1D458 is calculated, and a corrected state estimate
is produced. The complete method is given in Algorithm C.2.1.
Data: ?x0 and u1D4430
Result: State estimate ?x and error covariance matrix u1D443
while Not done do
Time Update;
?x?u1D458+1 = ?x+u1D458 +uni222B.alt01u1D461u1D458+1u1D461
u1D458 u1D453(?u1D465(u1D70F),u1D70F)u1D451u1D70F using a numerical integration technique such asRunge-Kutta or Euler;
u1D443?u1D458+1 = u1D439u1D451u1D443+u1D458 u1D439u1D451 +u1D444u1D451;
Gain Calculation: u1D43Fu1D458+1 = u1D443?u1D458+1u1D43Bu1D447u1D451 [u1D43Bu1D451u1D443?u1D458+1u1D43Bu1D447u1D451 +u1D445u1D458]?1;
Measurement Update;
?x+u1D458+1 = ?x?u1D458+1 +u1D43Fu1D458+1(u1D466??(?x?u1D458+1,u1D461));
u1D443+u1D458+1 = (u1D43C ?u1D43Fu1D458+1u1D43Bu1D451)u1D443?u1D458+1;
end
Algorithm C.2.1: Extended Kalman Filter
C.3 Unscented Kalman Filter
Due to errors from linearization, the EKF sometimes fails to converge. The unscented Kalman
?lter (UKF), based on the unscented transform developed by Julier and Uhlmann [74], does not
rely on linearization or partial derivatives [66]. Rather it constructs a set of points (sigma points)
that have the same sample mean and covariance as the distribution of the random variable of
interest. Each sigma point has an associated weight. Once the sigma points are constructed, they
are propagated through the nonlinear transform to yield a set of transformed samples. These
samples are then multiplied by their associated weights to yield a predicted mean, which in turn
is used to compute a predicted covariance. The UKF is accurate to the third order for Gaussian
distributions and requires the same order of computation as the EKF.
220
Let a nonlinear system be modeled as
xu1D458+1 = u1D439(uu1D458,uu1D458) +vu1D458
yu1D458 = u1D43B(xu1D458) +nu1D458
where x is of order u1D43F, vu1D458 is process noise with variance u1D445u1D463, and nu1D458 is sensor noise with variance
u1D445u1D45B. To completely capture the variable?s statistics, 2u1D43F+ 1 sigma points are needed. The sigma
points and associated weights as described in [75] are constructed as follows:
u1D7120 = x
u1D712u1D456 = x+(
uni221A.alt01
(u1D43F+u1D706)u1D443u1D465)u1D456 u1D456= 1,...,u1D43F
u1D712u1D456 = x?(
uni221A.alt01
(u1D43F+u1D706)u1D443u1D465)u1D456?u1D43F u1D456= u1D43F?1,...,2u1D43F
u1D44A(u1D45A)0 = u1D706/(u1D43F+u1D706)
u1D44A(u1D450)u1D45C = u1D706/(u1D43F+u1D706)+(1?u1D6FC2 +u1D6FD)
u1D44A(u1D45A)u1D456 = u1D44A(u1D450)u1D456 = 1/{2(u1D43F+u1D706)} u1D456 = 1,...,2u1D43F,
whereu1D706= u1D6FC2(u1D43F+u1D705)?u1D43Fis ascaling parameter,u1D6FCdetermines the spreadof the sigma points aroundx
(usually around 1e-3), u1D705is a scaling parameter usually set to zero, andu1D6FD is used to incorporate prior
knowledge of the distribution of x (for Gaussian distributions u1D6FD = 2). The notation (uni221A.alt01(u1D43F+u1D706)u1D443u1D465)u1D456
refers to the u1D456u1D461? row of the matrix square root. The UKF for state estimation as presented in [76]
is shown in Algorithm C.3.1.
Van der Merwe and Wan developed a more computationally e?cient implementation of the
UKF called the square-root UKF (SRUKF)[76]. The SRUKF uses the QR decomposition, Cholesky
factor updating, and e?cient least-squares algorithms. The algorithm is presented in Algorithm
C.3.2.
221
Initialize: ?x0 = u1D438[u1D4650], u1D4430 = u1D438[(x0 ? ?x0)(x0 ? ?x0)u1D447];
for u1D458 = 1,...,? do
Calculate Sigma Points:;
u1D712u1D458?1 =uni005B.alt01 ?xu1D458?1 ?xu1D458?1 +u1D702uni221A.alt01u1D443u1D458?1 ?xu1D458?1 ?u1D702uni221A.alt01u1D443u1D458?1 uni005D.alt01;
Time Update;
u1D712u1D458?u1D458?1 = u1D439[u1D712u1D458?1,uu1D458?1];
?x?u1D458 =uni2211.alt012u1D43Fu1D456=0u1D44A(u1D45A)u1D456 u1D712u1D456,u1D458?u1D458?1;
u1D443?u1D458 =uni2211.alt012u1D43Fu1D456=0u1D44A(u1D450)u1D456 [u1D712u1D456,u1D458?u1D458?1 ? ?x?u1D458 ][u1D712u1D456,u1D458?u1D458?1 ? ?x?u1D458 ]u1D447 +u1D445u1D463;
u1D713u1D458?u1D458?1 = u1D43B[u1D712u1D458?u1D458?1];
?y?u1D458 =uni2211.alt012u1D43Fu1D456=0u1D44A(u1D45A)u1D456 u1D713u1D456,u1D458?u1D458?1;
Measurement Update;
u1D443u1D466u1D458u1D466u1D458 =uni2211.alt012u1D43Fu1D456=0u1D44A(u1D450)u1D456 [u1D713u1D456,u1D458?u1D458?1 ? ?y?u1D458 ][u1D713u1D456,u1D458?u1D458?1 ? ?y?u1D458 ]u1D447 +u1D445u1D45B;
u1D443xu1D458yu1D458 =uni2211.alt012u1D43Fu1D456=0u1D44A(u1D450)u1D456 [u1D712u1D456,u1D458?u1D458?1 ? ?x?u1D458 ][u1D713u1D456,u1D458?u1D458?1 ? ?y?u1D458 ]u1D447;
u1D43E = u1D443xu1D458yu1D458u1D443?1yu1D458yu1D458;
?xu1D458 = ?x?u1D458 +u1D43E(yu1D458 ? ?y?u1D458 );
u1D443u1D458 = u1D443?u1D458 ?u1D43Eu1D458u1D443xu1D458yu1D458u1D43Eu1D447u1D458 ;
end
where u1D702 =uni221A.alt01(u1D43F+u1D706).;
Algorithm C.3.1: UKF for State Estimation
222
Initialize: ?x0 = u1D438[u1D4650], u1D4460 = chol{u1D438[(x0 ? ?x0)(x0 ? ?x0)u1D447]};
for u1D458 = 1,...,? do
Calculate Sigma Points:;
u1D712u1D458?1 =uni005B.alt01 ?xu1D458?1 ?xu1D458?1 +u1D702u1D446u1D458 ?xu1D458?1 ?u1D702u1D446u1D458 uni005D.alt01;
Time Update;
u1D712u1D458?u1D458?1 = u1D439[u1D712u1D458?1,uu1D458?1];
?x?u1D458 =uni2211.alt012u1D43Fu1D456=0u1D44A(u1D45A)u1D456 u1D712u1D456,u1D458?u1D458?1;
u1D446?u1D458 = qr{
uni005B.alt02 uni221A.alt02
u1D44A(u1D450)1 (u1D7121:2u1D43F,u1D458?u1D458?1 ? ?x?u1D458 ) ?u1D445u1D463
uni005D.alt02
};
u1D446?u1D458 = cholupdate{u1D446?u1D458 ,u1D7120,u1D458 ? ?x?u1D458,u1D44A(u1D450)0 } u1D713u1D458?u1D458?1 = u1D43B[u1D712u1D458?u1D458?1];
?y?u1D458 =uni2211.alt012u1D43Fu1D456=0u1D44A(u1D45A)u1D456 u1D713u1D456,u1D458?u1D458?1;
Measurement Update;
u1D446?y
u1D458
= qr{
uni005B.alt02 uni221A.alt02
u1D44A(u1D450)1 (u1D7131:2u1D43F,u1D458?u1D458?1 ? ?yu1D458) uni221A.alt01u1D445u1D45Bu1D458
uni005D.alt02
};
u1D446?y
u1D458
= cholupdate{u1D446?y
u1D458
,u1D7130,u1D458 ? ?yu1D458,u1D44A(u1D450)0 }
u1D443xu1D458yu1D458 =uni2211.alt012u1D43Fu1D456=0u1D44A(u1D450)u1D456 [u1D712u1D456,u1D458?u1D458?1 ? ?x?u1D458 ][u1D713u1D456,u1D458?u1D458?1 ? ?y?u1D458 ]u1D447;
u1D43Eu1D458 = (u1D443xu1D458yu1D458/u1D446u1D447yu1D458)/u1D446yu1D458;
?xu1D458 = ?x?u1D458 +u1D43E(yu1D458 ? ?y?u1D458 );
u1D448 = u1D43Eu1D458u1D446yu1D458;
u1D446u1D458 = cholupdate{u1D446?u1D458 ,u1D448,?1};
end
where u1D702 =uni221A.alt01(u1D43F+u1D706).;
Algorithm C.3.2: Square Root Implementation of the UKF
223
Appendix D
Latitude and Longitude
Latitude and longitude are a convenient way to describe position on a spherical surface such as
the Earth. Latitude and longitude are typically divided into units of degrees, minutes, and seconds
where a degree contains 60 minutes, and a minute contains 60 seconds. Lines of latitude run from
+90? (or 90? N) at the North Pole to ?90? (or 90? S) at the South Pole with the equator being at
0?. Lines of longitude vary from 180? (or 90? E) to ?180? (or 180? W) with 0? being the Prime
Meridian which runs through Greenwich, England.
D.1 Cartesian Calculations
In this section we summarize a method found in [77] to calculate latitude and longitude in
spherical geocentric coordinates. To use the WMM, it is necessary to have a latitude and a longi-
tude. So given an initial latitude and longitude as well as a distance traveled in the northerly and
easterly directions, it is necessary to calculate the new latitude and longitude. Spherical geometry
can be used to do this if the Earth is modeled as a sphere. The Earth is not actually spherical as
it bulges at the equator.
Several points are labeled in Figure D.1 that aid in ?nding the new latitude and longitude. Let
point A be the starting point with a known initial latitude u1D459u1D44Eu1D4611 and longitude u1D459u1D45Cu1D45B2, and let point
B be the ending point located at latitude u1D459u1D44Eu1D4612 and longitude u1D459u1D45Cu1D45B2 after traveling a distance of u1D44B in
the easterly direction and u1D44C in the northerly direction. Point D is located at the same latitude as
A and the same longitude as point B. Likewise, point C is located at the same longitude as A and
the same latitude as B. Point E is the point where A?s longitude intersects the equator, and point
F is where B?s longitude intersects the equator. Point O is located at the Earth?s center.
The derivation of u1D459u1D45Cu1D45B2 is straightforward. To ?nd u1D459u1D45Cu1D45B2 ?rst ?nd the angle ?u1D434u1D442u1D438 which is
equal to u1D459u1D44Eu1D4611 by the de?nition of latitude. Next, the radius of the circle that passes through points
224
Figure D.1: Derivation of Latitudes and Longitudes
A and D at u1D459u1D44Eu1D4611 is needed. This is found by ?rst dropping a perpendicular from A to the line
segment OE. The perpendicular intersects OE at point G. Then the length of segment OG is equal
to the desired radius. From trigonometry the length of OG is
u1D442u1D43A = u1D445cos(u1D459u1D44Eu1D4611)
where u1D445 is the Earth?s radius. Let u1D442? be the center of the circle that forms u1D459u1D44Eu1D4611. Then the angle
?u1D434u1D442?u1D437 is given by
?u1D434u1D442?u1D437 = u1D44Cu1D445cos(u1D459u1D44Eu1D4611)
225
since u1D44C is the arc length from A to D and u1D445cos(u1D459u1D44Eu1D4611) is the circle?s radius. Then u1D459u1D45Cu1D45B2 is given by
u1D459u1D45Cu1D45B2 = u1D459u1D45Cu1D45B1+?u1D434u1D442?u1D437
= u1D459u1D45Cu1D45B1+ u1D44Cu1D445cos(u1D459u1D44Eu1D4611). (D.1)
The derivation for u1D459u1D44Eu1D4612 is also relatively simple. First note that the angle ?u1D434u1D442u1D436 is
?u1D434u1D442u1D436 = u1D459u1D44Eu1D4612?u1D459u1D44Eu1D4611
by the de?nition of latitude. Then the arc length u1D44B between points A and C is
u1D44B = u1D445?u1D434u1D442u1D436
= u1D445(u1D459u1D44Eu1D4612?u1D459u1D44Eu1D4611).
Then u1D459u1D44Eu1D4612 is given by
u1D459u1D44Eu1D4612 = u1D459u1D44Eu1D4611+ u1D44Bu1D445. (D.2)
D.2 Geodetic Coordinates
In this section we summarize the geodetic reference frame as presented in [78]. For geodetic
coordinates the Earth is modeled as an ellipsoid as shown in Figure D.2. The u1D465-axis is in the
equatorial plane and ?xed with the Earth?s rotation such that it passes through the Greenwich
meridian. The u1D467-axis is parallel to the Earth?s rotation axis, and the u1D466-axis is perpendicular to the
u1D465- and u1D467-axes according to the right-hand rule. A cross-section is taken such that the major axis of
an ellipse corresponds to the equator. The center of the Earth is at point u1D442. Then the semimajor
axis u1D44Ehas a value equal to the Earth?s mean radius, and the semiminor axisu1D44Fis equal to the Earth?s
polar diameter. With values u1D44E and u1D44F determined by which physical model of the Earth is used, the
226
Figure D.2: Geodetic Coordinate System
eccentricity u1D452 and the ?attening u1D453 of the ellipsoid are
u1D452 =
uni221A.alt03
1? u1D44F
2
u1D44E2
u1D453 = 1? u1D44Fu1D44E.
Geodetic latitude, longitude, and height are de?ned with respect to the reference ellipsoid. In
Figure D.2 the vehicle?s position is given by point u1D446, which is de?ned by vector u = (u1D465u1D462,u1D466u1D462,u1D467u1D462)
in geocentric coordinates. Then the geodetic longitude u1D706, de?ned as the angle between the vehicle
and the u1D465-axis, measured in the u1D465u1D466-plane is
u1D706=
??
????
???
??
tan?1
uni0028.alt02
u1D466u1D462
u1D465u1D462
uni0029.alt02
, u1D465u1D462 ? 0
u1D70B
2 +tan
?1
uni0028.alt02
u1D466u1D462
u1D465u1D462
uni0029.alt02
, u1D465u1D462 < 0,u1D466u1D462 ? 0
?u1D70B2 +tan?1
uni0028.alt02
u1D466u1D462
u1D465u1D462
uni0029.alt02
, u1D465u1D462 < 0,u1D466u1D462 < 0.
227
Geodetic latitude u1D719 is the angle between the ellipsoid normal vector n and the projection of n onto
theu1D465u1D466-plane. Geocentric latitude isu1D719?. Geodetic height ?is the minimum distance from the vehicle
to the ellipsoid. To convert from geodetic to geocentric coordinates use
u1D465u1D462 = u1D44Ecos(u1D706)uni221A.alt011+(1?u1D4522)tan2(u1D719) +?cos(u1D706)cos(u1D719)
u1D466u1D462 = u1D44Esin(u1D706)uni221A.alt011+(1?u1D4522)tan2(u1D719) +?sin(u1D706)cos(u1D719)
u1D467u1D462 = u1D44E(1?u1D452
2)sin(u1D719)
uni221A.alt01
1?u1D4522 sin2(u1D719) +?sin(u1D719).
228