Upload
others
View
10
Download
0
Embed Size (px)
Citation preview
UNSCENTED KALMAN FILTERING FOR RELATIVE
ATTITUDE AND POSITION ESTIMATION
by
Shu Ting Goh
Sept 1, 2007
A thesis submitted to
the Faculty of the Graduate School of
State University of New York at Buffalo
in partial fulfillment of the requirements for the degree of
Master of Science
Department of Mechanical & Aerospace Engineering
UMI Number: 1446289
14462892007
UMI MicroformCopyright
All rights reserved. This microform edition is protected against unauthorized copying under Title 17, United States Code.
ProQuest Information and Learning Company 300 North Zeeb Road
P.O. Box 1346 Ann Arbor, MI 48106-1346
by ProQuest Information and Learning Company.
To my family and friends
ii
Acknowledgment
First of all, I would like to thank my advisor, Prof. John L. Crassidis. His knowledge
and experience has greatly assisted me throughout my graduate research. And I am
greatly appreciative of his patience, support and guidance through my graduate studies.
I would like to thank my committee members, Prof. Joesph Mook and Prof.
Tarunraj Singh. Prof. Joseph Mook has let me to become interested in the control field
after attending his class in my undergraduate education. And I am very appreciative of
what I have learned in Prof. Tarunraj Singh’s classes.
Besides that, I would like to thank to the students in Furnas 1017, especially
Jemin George who has helped me greatly for the LaTex formating. The work would
have been much more difficult without their assistance.
Lastly, I would like to thank to my parents and my friends in Malaysia who have
supported my choice of major all of the time. I am grateful the opinions and insight they
gave when I was choosing aerospace engineering as my career. Without them, I would
not have the opportunity to get into this field.
iii
Contents
Preamble iii
Acknowledgment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iii
List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vii
List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ix
Abstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . x
1 Introduction 1
1.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Previous Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.3 Research Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2 Vision Based Navigation System 8
2.1 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
3 Relative Attitude and Position Kinematics 14
3.1 Attitude Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.2 Relative Attitude Kinematics . . . . . . . . . . . . . . . . . . . . . . . . 17
3.2.1 Chief and Deputy Gyro Bias Case . . . . . . . . . . . . . . . . . . 20
iv
Contents v
3.2.2 Relative and Deputy Gyro Bias Case . . . . . . . . . . . . . . . . 24
3.2.3 Relative and Chief Gyro Bias Case . . . . . . . . . . . . . . . . . 26
3.3 Relative Position Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.3.1 Relative Orbit Motion Equations . . . . . . . . . . . . . . . . . . 29
3.4 Relative attitude and position kinematics . . . . . . . . . . . . . . . . . . 37
3.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
4 State estimation 41
4.1 Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
4.2 Unscented Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.3 Algorithm of State Estimation for Relative Attitude and Position Estimation 55
4.3.1 Extended Kalman Filter for Relative Attitude and Position Esti-
mation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
4.3.2 Unscented Filter for Relative Attitude and Position Estimation . 59
4.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
5 Simulation and Results 69
5.1 Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
5.2 Simulation Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
5.2.1 Well-known initial condition . . . . . . . . . . . . . . . . . . . . . 74
5.2.2 4 deg relative attitude error and 4% relative position elements error
in initial condition . . . . . . . . . . . . . . . . . . . . . . . . . . 80
5.2.3 14 deg relative attitude error and 14% relative position elements
error in initial condition . . . . . . . . . . . . . . . . . . . . . . . 86
Contents vi
5.3 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92
6 Discussion, Conclusion and Future Works 94
6.1 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94
6.2 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96
6.3 Future Works . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 96
Bibliography 98
List of Figures
2.1 Vision Based Navigation System1 . . . . . . . . . . . . . . . . . . . . . . 8
2.2 General type of Spacecraft Formation with Relative Motion.1 . . . . . . . 10
5.1 Attitude Errors and 3σ Bounds with Initial States Well-Known . . . . . . 74
5.2 Estimated chief gyro bias Initial States Well-Known . . . . . . . . . . . . 75
5.3 Estimated deputy gyro bias Initial States Well-Known . . . . . . . . . . . 76
5.4 Position Errors and 3σ Bounds Initial States Well-Known . . . . . . . . . 77
5.5 Velocities Errors and 3σ Bounds Initial States Well-Known . . . . . . . . 78
5.6 Chief Orbit Elements Errors and 3σ Bounds Initial States Well-Known . 79
5.7 Attitude Errors and 3σ Bounds with Initial Error = 4 deg and 4% . . . . 80
5.8 Estimated chief gyro bias with Initial Error = 4 deg and 4% . . . . . . . 81
5.9 Estimated deputy gyro bias with Initial Error = 4 deg and 4% . . . . . . 82
5.10 Position Errors and 3σ Bounds with Initial Error = 4 deg and 4% . . . . 83
5.11 Velocities Errors and 3σ Bounds with Initial Error = 4 deg and 4% . . . 84
5.12 Chief Orbit Elements Errors and 3σ Bounds with Initial Error = 4 deg
and 4% . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85
5.13 Attitude Errors and 3σ Bounds with Initial Error = 14 deg and 14% . . . 86
vii
List of Figures viii
5.14 Estimated Chief gyro bias with Initial Error = 14 deg and 14% . . . . . . 87
5.15 Estimated Deputy gyro bias with Initial Error = 14 deg and 14% . . . . . 88
5.16 Position Errors and 3σ Bounds with Initial Error = 14 deg and 14% . . . 89
5.17 Velocities Errors and 3σ Bounds with Initial Error = 14 deg and 14% . . 90
5.18 Chief Orbit Elements Errors and 3σ Bounds with Initial Error = 14 deg
and 14% . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
List of Tables
4.1 Continuous-Discrete Extended Kalman Filter2 . . . . . . . . . . . . . . . 45
4.2 Discrete Extended Kalman Filter2 . . . . . . . . . . . . . . . . . . . . . . 49
4.3 Extended Kalman Filter for Relative Attitude Estimation1 . . . . . . . . 56
4.4 Extended Kalman Filter for Relative Attitude and Position Estimation . 57
ix
Abstract
In this thesis, the Unscented Filter is derived for the relative attitude and position esti-
mation. In the previous work, the Extended Kalman Filter had shown its success in the
estimation. For an inaccurate nonlinear model system, the Unscented Filter has proved
to converge faster than the standard Extended Kalman Filter. The relative attitude and
position estimation approach is based on the vision-based navigation (VISNAV) system.
The line-of-sight measurement with the gyro measurements are used by the Unscented
Filter to estimate the relative attitude, gyro biases and relative position.
The modified Rodrigues parameters are used to define the local quaternion error
for the sigma points transformation, while the quaternion is used for global attitude
propagation. The transformation between quaternion and modified Rodrigues parame-
ters is always guarantees quaternion normalization. Both spacecraft are assumed to have
an equal type of build so that their orbits are predictable. The simulation results show
that the standard Extended Kalman Filter is only able to provide converged results in
a very limited low error, while the Unscented Filter can provide converged results up to
a certain high error and with a faster convergence rate.
x
Chapter 1
Introduction
1.1 Overview
Spacecraft formation flying is an important technology since its technology has been
introduced. In early-day space missions, formation flying mainly involved docking prob-
lems where the relative distance between spacecraft decreased to zero in a very slow and
controlled manner. For example, in the apollo mission two spacecraft were assembled in
orbit. While today, modern formation flying focuses on maintain the distance between
the spacecraft in a formation throughout the mission.3 In the past few years, various
research on spacecraft formation flying navigation has been done. Most studies focus
into determining the relative position and attitude between the vehicles.4,5 A GPS-like
technology to a deep space has been proposed before,6,7 and this technology can not
only be applied to spacecraft formation flying, but also aircraft formation, ground vehi-
cle, etc. However, this technology requires extensive hardware development. Recently,
a novel approach has developed for relative position and attitude estimation based on a
vision-based navigation (VISNAV) system.1 The extended Kalman Filter (EKF) results
using the VISNAV system showed that the relative attitude and the relative position
1
Chapter 1. Introduction 2
deputy spacecraft to the chief spacecraft can be well estimated. In this thesis, the main
objective is to use the unscented filter (UF)8–10 for the relative attitude and position
estimation. In theory, the UF will have a better estimate result if there are large initial
errors.
Spacecraft attitude means how a spacecraft is orientated in space. Every space-
craft has its own instrument (or payload) that must be orientated or point into some
direction, regards of its own mission. Attitude determination is ability to know the
spacecraft’s attitude while the attitude control is the ability to command the spacecraft
to a desired attitude.11 There are many ways to represent the attitude of a vehicle, for
example, Euler angle, Euler axis/angle, quaternion,12 Rodrigues Parameters,13 Modified
Rodrigues Parameters,14,15 etc.16 Euler angles are commonly used for the flight dynam-
ics, especially for commercial aircraft. It is due to the three angles that are used by
Euler angles, roll, pitch and yaw that gives a clear physical meaning. However, Euler
angles have a disadvantage of singularity at every 90 degree of rotation, which limits
their usefulness. The Modified Rodrigues Parameters, which also use three dimensional
parameters for the attitude representation, allow the rotation of spacecraft up to 360
degrees before they meet the singularity.15
In all of the attitude representations, the quaternion is the most convenient
spacecraft attitude representation because there is no singularity issue. The quaternion
uses four parameters to describe the three-dimensional space, hence, the components
in quaternion cannot be independent to each other. Instead of taking a time deriva-
tive of each angle in the quaternion matrix, the quaternion can be propagate through a
known gyro rate and quaternion parameters. Also there is a closed-form solution for the
quaternion propagation, which leads to the ease of computational aspects for attitude
determination. The quaternion has several advantages over the rotation matrix and Eu-
ler angle attitude representations: 1) It has fewer elements (four compared to nine for
Chapter 1. Introduction 3
the rotation matrix) 2) fewer constraints (one instead of six), and 3) the composition is
simpler (16 multiplication instead of 27).11
For the relative attitude estimation involved in formation flying, the relative
quaternion estimation works as the same way as the usual quaternion estimation. How-
ever, the difference is that now the estimator will estimate the relative gyro rate of two
spacecraft for propagation, instead of only estimating a gyro rate from one spacecraft.
This means that there are at least two gyro rates that need to estimated, which de-
pends on the total number of spacecraft in the formation. Fortunately, there is also a
closed-form solution for the discrete-time propagation of the relative quaternion. The
sensor measurement that used in this thesis and previous research in Ref. 1 for this
relative attitude estimation are based on a vision-based navigation (VISNAV) system,
which has the following advantages: 1)a very small sensor size, 2) very wide sensor field-
of-view (FOV), 3) no complex/time consuming charge-coupling-device signal processing
or pattern recognition required, 4) excellent rejection of ambient light interference under
a wide variety of operating conditions, and 5) relatively simple electronic circuits with
modest digital signal processing micro-computer requirements. Detailed descriptions on
VISNAV can be found in Ref. 17 and 18.
In general, the position of the spacecraft can be well estimated from the moment
the spacecraft is launched. But due to modern formation flying applications, the chief
and deputy spacecraft are designed to only be separated after they getting into a specific
position. The accuracy of the relative position between the spacecraft is important
in formation flying navigation. In the Ref. 1 and this thesis, both chief and deputy
spacecraft are assumed to have equal type and build, which means they have the same
ballistic coefficient. Thus each orbit will decay at the same rate. Then, the relative orbit
motion equations in Ref. 3 is able to be applied for these cases.
In previous research, Ref. 1 used an EKF for the relative attitude and position
Chapter 1. Introduction 4
estimation simulation. The EKF has shown that for the condition of well-known initial
condition, the estimated relative attitude errors are within 0.01 degrees while the position
errors are within 0.01 meters. The EKF has been widely used in the spacecraft orbit
determination and attitude determination due to its robustness for nonlinear estimation.
The EKF has worked with the condition that the estimate states are closed to true states.
A filter has been developed by Julier, Uhlmann and Durrant-Whyte which is an
alternative to the EKF, called the Unscented Filter (UF). The UF use the same kind
of update as the standard Kalman filter, however, it uses a different way to propagate
the states. A matrix called the sigma points matrix will be formed from the measure-
ment and process noise matrix, where the states will projected into those sigma points
respectively to propagate into next time steps, and so on each sigma points of states
will be used to estimate the measurement outputs. Then the propagated covariance ma-
trices will also obtained from these sigma points. There are several papers on attitude
determination using the UF.19,20 The results showed that the UF converges faster than
the standard EKF when large initial errors are present. The differences are caused by
how the covariance of each filter reacting when the direction of the error distribution
is changing. In the EKF case, the direction always point in one direction, while the
covariance in UF will make its turn into the appropriate direction when needed. Besides
that, the UF has severals advantages over the EKF: 1) the expected error is lower than
the EKF, 2) the UF can be applied to non-differentiable functions, 3) the UF avoids
the derivation of Jacobian matrices, and 4) the UF is valid to higher-order expansions
than the standard EKF.2 Even though the UF has more advantages than the standard
EKF, there still some limitations that prevent the UF to be widely used in the nonlinear
estimation. The major problem is that the UF needs a significant amount of compu-
tation, where the optimal value for unscented parameters are required to be known to
let the UF produce the best results. Different combinations of the Unscented filter pa-
rameters either produce a fast convergence, or cause the covariance matrix to become a
Chapter 1. Introduction 5
nonpositive, semidefinite matrix.
1.2 Previous Work
In Ref. 1 a novel approach based on VISNAV system has been developed. The purpose
of this work is to show that it is possible to estimate the relative attitude and posi-
tion between two spacecraft without using any external measurements. There are three
different formulations for relative attitude estimation that can be presented. The first
formulation estimates the relative attitude and the relative chief and gyro bias. The
second formulation estimates the relative attitude, the relative rate bias and the deputy
gyro bias. The last formulation estimates the relative attitude, the relative rate bias and
the chief gyro bias.
The derivation of the nonlinear relative orbit motion of equations, can be found
either in Ref. 3 and will be reviewed in a further section in this thesis. It is assumed
that only acceleration disturbances exist, and a bounded relative orbit is used where the
cartesian initial conditions’ constraint3 must be satisfy. The work in Ref. 1 shows that
not only estimation of the relative position of the deputy spacecraft is possible, but also
estimation of the chief radius, true anomaly and anomaly rate is possible.
The EKF formulation has been presented to estimate the relative attitude and
position estimation. The Hubble Space Telescope and its parameters has been selected
for the simulation. All the initial estimated values are assumed to be well known in
the simulation. The simulation results showed that the combined relative attitude and
position estimation with an EKF is able to achieve accurate results.
Chapter 1. Introduction 6
1.3 Research Work
In this thesis, an Unscented filter (UF) will be used for relative attitude and position
estimation. The purpose of this research is to use the UF to handle large initial errors for
the relative attitude and position estimation. However, unlike the work in the Ref. 1, this
thesis will mainly focus on the first formulation, which is to estimate the relative attitude,
chef and deputy gyro bias and the relative position will be used in the simulation. But
all of the three formulation will be reviewed.
The application of the UF for attitude determination is not new. In Ref. 19, it
has been shown the UF performs much better than standard EKF for the large initial
attitude-error situation. A three-component attitude vector generalized Rodrigues pa-
rameters will be use instead of the usual quaternion. This is due to the linearized model
that used in Ref. 1 does not take full advantage of the UF capabilities. Also, the singular-
ity for the vector will never encountered because of only three-component representation
are used for attitude errors. During the propagation stage, the three-component attitude
vectors will be converted into quaternions with respect to each sigma points. Then the
propagated quaternions will be converted back into the three-component attitude vectors
again for the purpose of covariance propagation and to obtain the mean for update.
The simulations in the thesis will be as follows, first, the UF will be used for
relative estimation where the initial conditions are well-known. The results from the UF
should be identical to the EKF results. Then, the initial condition error will be increased
in a fixed rate, while both the UF and EKF will run together to compare the results
from both filters. Several UFs will run at the same time with different combination of
Unscented parameters to obtain optimal estimation. In this case, it is to be expected
that only a few of the combinations of the Unscented parameters will either able to
produce results or produce the best results. Theoretically, the UF should able to work
no matter how large the initial error while EKF is limited up to certain amount of error.
Chapter 1. Introduction 7
But this isn’t true for the UF for relative attitude and position estimation case, which
will explained further in the future chapters.
The organization of the thesis processes as follows. First the VISNAV system will
be reviewed. After that, the relative attitude and position kinematics will be reviewed,
followed by a review on the derivation of the EKF for relative attitude and position
estimation. Then the UF will derived for this estimation. Lastly, the simulation results
will be shown.
Chapter 2
Vision Based Navigation System
Figure 2.1: Vision Based Navigation System1
The VISNAV system comprises and optical sensor of a new kind combined with
specific light sources (beacons), to achieve a selective of “intelligent” vision. The sensor
is made up of a Position Sensing Diode (PSD) placed in the focal plane of a wide angle
8
Chapter 2. Vision Based Navigation System 9
lens. When the rectangular silicon area of the PSD is illuminated by energy from a
beacon focuses by the lens, it generates electrical currents in four directions that can
be processed with appropriate electronic equipment to estimate the energy centroid of
the image. The current imbalances generated are almost linearly proportional to the
location of the centroid of the light beam onto the PSD area. While the individual
currents depend on the intensity of the light, their imbalance is weakly dependent on
the light intensity. The idea behind this concept (“intelligent” vision) is that the PSD
can used to see only specific light sources, thanks to frequency domain structuring of
the target lights and some relatively simple analog signal processing (demodulation).The
light is produced by LEDs (beacons) modulated at an arbitrary known frequency while
the currents generated are driven through an active filter set on the same frequency.
Calculating the current imbalances then yields two analog signals directly related to the
coordinates locating the centroid of that beacon’s energy distribution on the PSD, in a
quasi-linear fashion, and therefore to the incident direction of this light on the wide-angle
lens, which gives a line of sight (LOS) vector.
The technique of measuring objects (2D or 3D) that are used by the VISNAV
system in this thesis research is a close range photogrammetry-type application. The
photogrammetry technique determine the object space locations from the photographic
images or LOS measurements. In general, photogrammetry can be divided into two cat-
egories: 1) far range photogrammetry with camera settings to infinity (commonly used
in star tracker cameras21), and 2) close range photogrammetry with camera distance set-
tings to finite values. The difference between far range and close range photogrammetry
is that close range photogrammetry can be used to determine both attitude and position
of an object, while far range photogrammetry can only used to determine the attitude
of the object.
Figure 2.1 shows a schematic of the typical quantities involved in basic photogram-
Chapter 2. Vision Based Navigation System 10
Figure 2.2: General type of Spacecraft Formation with Relative Motion.1
metry from LOS measurements, derived from light beacons in this case. It is assumed
that the location of the sensor focal plane is known within the deputy spacecraft coordi-
nate system, which is usually obtained through calibration. The location of light sources
beacons are known and placed on the chief spacecraft. Without loss in generality, we
assume that the chief spacecraft frame coincides with the frame describe in Figure 2.2.
If we choose the z-axis of the sensor coordinate system to be directed outward along the
boresight, then given object space and image space coordinate frames (see Figure 2.1),
the ideal object to image space projective transformation (noiseless) can be written as
follows:22
χi = −f A11(Xi − x) + A12(Yi − y) + A13(Zi − z)
A31(Xi − x) + A32(Yi − y) + A33(Zi − z), i = 1, 2, . . . , N (2.1a)
γi = −f A21(Xi − x) + A22(Yi − y) + A23(Zi − z)
A31(Xi − x) + A32(Yi − y) + A33(Zi − z), i = 1, 2, . . . , N (2.1b)
where N is the total number of observations (or output measurements in this case),
Chapter 2. Vision Based Navigation System 11
(χi, γi) are the image space observations for the ith LOS, (Xi, Yi, Zi) are the known object
space locations of the ith beacon attached on the chief spacecraft. Also (x, z, y) are the
unknown relative position of the deputy spacecraft to the chief spacecraft. The variable
f is the known focal length, and Ajk are the unknown coefficients of the attitude matrix,
A. This attitude matrix is associated to the orientation from the chief spacecraft to
the deputy spacecraft. The goal of the inverse problem is given the observations (χi, γi)
with known object space locations (Xi, Yi, Zi), determine the attitude matrix (A) and the
relative position of the deputy spacecraft (x, y, z). The observation can be reconstructed
in unit vector form as
bi = Ari, i = 1, 2, . . . , N (2.2)
where
bi ≡1√
f 2 + χ2i + γ2
i
−χi
−γi
f
(2.3a)
ri ≡1√
(Xi − x)2 + (Yi − y)2 + (Zi − z)2
Xi − x
Yi − y
Zi − z
(2.3b)
Shuster23 has shown that nearly all the probability of the errors that are caused by
the measurement noise is concentrated on a very small area about the direction of Ari.
Therefore, the sphere containing that point can be approximated by a tangent plane,
characterized by
Chapter 2. Vision Based Navigation System 12
bi = Ari + υi, υiAri = 0 (2.4)
where bi denotes the ith sensor measurement, and υi denotes the ith sensor’s error, which
is approximately Gaussian, and satisfies
Eυi = 0 (2.5a)
E[υiυTi = σ2
i [I3×3 − (Ari)(Ari)T ] (2.5b)
where E denotes the expectation and I3×3 denotes a 3 by 3 identity matrix. Equation
(2.5b) makes the small field-of-view (FOV) assumption of Ref. 23. However, for a large
FOV with significant radial distortion, this covariance model should be modified appro-
priately.24 The advantage of using the model in equation (2.5) is that the measurement
covariance in the EKF (and so for the UF) formulation can effectively be replaced by a
nonsingular matrix given by σ2i I3×3 (more details in Ref. 25). Hence, the measurement
covariance matrix used in the EKF(and the UF) from all available LOS vector is given
by
Rk = diag
[σ2
1 σ22 . . . σ2
N
] I3× (2.6)
where “diag” denotes a diagonal matrix and denotes the Kronecker tensor product.
2.1 Summary
The technique of measuring object used by the VISNAV system in this thesis research is
close range photogrammetry, which can be used to determine the relative attitude and
Chapter 2. Vision Based Navigation System 13
position between two spacecraft. The observation can be reconstructed into a linear form
of unit vector as shown in equation (2.2), where can be used to estimate the attitude
matrix and the position of a spacecraft. It assume that the measurement noise is approx-
imately Gaussian, and it is linearly proportional to the measurement equation, which
is shown in equations (2.4) and (2.5). Thus from the equations (2.5), the measurement
covariance matrix can be obtained to use in the EKF and UF.
Chapter 3
Relative Attitude and Position
Kinematics
3.1 Attitude Kinematics
This section reviews the basic attitude kinematics equation of motions by using the
quaternion, and then the three different relative attitude formulations will be reviewed.
At the end of the section, the sensitivity matrix for the relative attitude kinematics will
be given.
The quaternion matrix is defined by q ≡ [ %T q4 ]T with % ≡ [ q1 q2 q3 ] =
e sin(ϑ/2), and q4 = cos(ϑ/2), where e is the axis of rotation and ϑ is the angle of
rotation. Since a four-dimensional vector is used to describe the three dimensions, the
quaternion components cannot be independent of each other. The quaternion satisfies a
single constraint given by qTq = 1.The attitude matrix is related to the quaternion by
A(q) = ΞT (q)Ψ(q) (3.1)
14
Chapter 3. Relative Attitude and Position Kinematics 15
with
Ξ ≡
q4I3×3 + [%×]
−%T
(3.2a)
Ψ ≡
q4I3×3 − [%×]
−%T
(3.2b)
where [%×] is the cross product matrix, which is defined as
[a×] ≡
0 −a3 a2
a3 0 −a1
−a2 a1 0
(3.3)
One of advantage of using quaternion is that successive rotations can be accomplished
by using quaternion multiplication. Here we adopt the convention of Lefferts, Markley
and Shuster26 who multiply the quaternions in the same order as the attitude matrix
multiplication, which can be written as:
A(q′)A(q) = A(q′ ⊗ q)
The composition of the quaternions is bilinear with,
q′ ⊗ q =
[Ψ(q′) q′
]q =
[Ξ(q) q
]q′ (3.4)
The inverse of the quaternion is define as q−1 =
[−% q4
]T. Note that the identity
quaternion is equivalent to
[0 0 0 1
], which can be obtain through q⊗ q−1.
Chapter 3. Relative Attitude and Position Kinematics 16
The quaternion kinematics equation is given by
q =1
2Ξ(q)ω =
1
2Ω(ω)q (3.5)
where
Ω(ω) ≡
−[ω×] ω
−ωT 0
(3.6)
Some useful identities are given by
ΞT (q)Ξ(q) = ΨT (q)Ψ(q) = I3×3 (3.7a)
Ξ(q)ΞT (q) = Ψ(q)ΨT (q) = I4×4 − qqT (3.7b)
ΞT (q)q = ΨT (q)q = 03×1 (3.7c)ω0
⊗ q = Ω(ω)q (3.7d)
q⊗
ω0
= Γ(ω)q (3.7e)
Ψ(q)ω = Γ(ω)q (3.7f)
where
Γ(ω) =
[ω×] ω
−ωT 0
(3.8)
Chapter 3. Relative Attitude and Position Kinematics 17
3.2 Relative Attitude Kinematics
In this section, the derivation of the closed-form solution from continuous to the time-
discrete propagation of the relative quaternion will be reviewed. All of the three formu-
lations are previously derived for EKF filter in Ref. 1. In the UF case, the formulations
is the same as the EKF case, although there is a slight difference of the end result of
obtaining the time-discrete process noise covariance, however the steps of taking the
linearization process is exactly the same as the EKF.
The relative attitude, denoted by the quaternion q, which is used to map vectors
in the chief frame to vectors in the deputy frame is expressed by
q = qd ⊗ q−1c (3.9)
where qc is the attitude of the chief spacecraft, while qd is the attitude of the deputy
spacecraft. From the observation, equation (3.9) looks similar to the error quaternion
that is used in Kalman filtering. Then, by following the Ref. 26, the relative quaternion
kinematics can be shown as follows:
q = −
[ωc×]%
0
+1
2
(ωd − ωc)
0
⊗ q (3.10)
where ωc is the angular velocity of the chief and ωd is the angular velocity of the deputy
spacecraft. Again, equation (3.10) is similar to the kinematics shown in Ref. 27, which
can be rewritten as:
q1
2Ξ(q)ωdc (3.11)
where ωdc is the relative angular velocity,
Chapter 3. Relative Attitude and Position Kinematics 18
ωdc ≡ ωd − A(q)ωc (3.12)
By defining Θ(ωd,ωc) ≡ Ω(ωd)−Γ(ωc), and by substituting equation (3.1) into equation
(3.12), and using the identities in equations (3.5), (3.7b), (3.7c), and (3.7f), equation
(3.11) can be simplified as
q =1
2Θ(ωd,ωc)q (3.13)
Now, the closed-form solution for the state transition matrix of 12Θ(ωd,ωc) can be shown.
As side note the eigenvalues of this matrix are given by ±(‖ωd‖+ ωc‖)j and ±(‖ωd‖ −
‖ωc‖)j. Since the matrices Ω(ωd) and Γ(ωc) commute, we can write
exp
[1
2Θ(ωd,ωc)t
]= exp
[1
2Ω(ωd)t
]exp
[−1
2Γ(ωc)t
](3.14)
The closed-form solution for the matrix exponential of 12Ω(ωd)t is well documented
(see Ref. 28). By applying a similar derivation in the matrix −12Γ(ωc)t, we obtain
exp
[−1
2Γ(ωc)t
]= I4×4 cos
(1
2‖ωc‖t
)− Γ(ωc)
sin(
12‖ωc‖t
)‖ωc‖
(3.15)
Therefore, the discrete-time propagation of the relative quaternion equation is given by
qk+1 = Ω(ωdk)Γ(ωck)qk (3.16)
with
Chapter 3. Relative Attitude and Position Kinematics 19
Ω(ωdk) ≡
cos(
12‖ωdk
‖∆t)I3×3 − [ψk×] ψk
−ψTk cos
(12‖ωdk
‖∆t) (3.17a)
Γ(ωck) ≡
cos(
12‖ωck‖∆t
)I3×3 − [ζk×] −ζk
ζTk cos(
12‖ωck‖∆t
) (3.17b)
where
ψk ≡sin(
12‖ωdk
‖∆t)ωdk
‖ωdk‖
(3.18a)
ζk ≡sin(
12‖ωck‖∆t
)ωck
‖ωck‖(3.18b)
where ∆t is the sampling interval. Note that the matrices Ω(ωdk) and Γ(ωck) also
commute.
Next, the necessary equations for the relative attitude kinematics between two
spacecraft will be reviewed. Three different kinds of formulations will be presented.
However this thesis will focus on the first formulation, which is estimate the relative
attitude, and the chief and deputy gyro bias. The second formulation will estimate
the relative attitude, and the relative rate bias and the deputy gyro bias. The third
formulation is to estimate the relative attitude and the relative rate bias and the chief
gyro bias.
Chapter 3. Relative Attitude and Position Kinematics 20
3.2.1 Chief and Deputy Gyro Bias Case
In this section, a formulation for relative attitude, chief and deputy gyro bias estimation
will be reviewed, which is also the main formulation that this thesis will be use on. The
truth equations are given by
q =1
2Ξ(q)ωdc (3.19a)
ωdc = ωd − A(q)ωc (3.19b)
βc = ηcu (3.19c)
βd = ηdu (3.19d)
ωc = ωc − βc − ηcv (3.19e)
ωd = ωd − βd − ηdv (3.19f)
The estimates are given by
˙q =1
2Ξ(q)ωdc (3.20a)
ωdc = ωd − A(q)ωc (3.20b)
˙βc = 0 (3.20c)
˙βd = 0 (3.20d)
ωc = ωc − βc (3.20e)
ωd = ωd − βd (3.20f)
The above equations show that the quaternion kinematics involves the attitude matrix.
Note that the error quaternion cannot obtain thru a straight difference of q−q, because
Chapter 3. Relative Attitude and Position Kinematics 21
it will not return a unit vector. The error quaternion and its derivative are given by
δq = q⊗ q−1 (3.21a)
δq = q⊗ q−1 + q⊗ ˙q−1 (3.21b)
By taking the time derivative of q⊗ q−1 =
[0 0 0 1
]T, gives
q−1 = −1
2q−1 ⊗
ωdc0
(3.22)
Then substituting equations (3.19a) and (3.22) into equation (3.21b), leads to
δq =1
2
ωdc0
⊗ δq− 1
2δq⊗
ωdc0
(3.23)
Next, define the following error variables: δωd ≡ ωd − ωd and δωc ≡ ωc − ωc, and use
these definitions in ωdc,
ωdc = ωd − A(q)ωc + δωd − A(q)δωc (3.24)
Lets δα be defined as a small angle-error correction. The linearization process makes
the following assumptions that are valid within first-order:26
δq =
12δα
1
(3.25a)
A(q) = I3×3 − [δα×]A(q) (3.25b)
Chapter 3. Relative Attitude and Position Kinematics 22
By substituting equation (3.25b) into equation (3.24) and neglecting the second-order
effects, leads to
ωdc = ωdc − [A(q)ωc×]δα+ δωd − A(q)δωc (3.26)
Substituting equations (3.25a) and (3.26) into equation (3.23), and again neglecting the
second-order effects leads to
δα = −[ωd×]δα+ δωd − A(q)δωc (3.27)
The derivative of the fourth error quaternion component is zero. Next, using δωd =
−(∆βd + ηdv) and ωc = −(∆βc + ηcv) , where ∆βd ≡ βd − βd and ∆βc ≡ βc − βc, in
equation (3.27) leads to
δα = −[ωd×]δα−∆βd + A(q)∆βc + A(q)ηcv − ηdv (3.28)
The error-state dynamics now can be shown in linearized matrix form of
∆x = F∆x +Gw (3.29)
with
∆x ≡[δαT ∆βTc ∆βTd
]T(3.30a)
w ≡[ηTcv ηTdv ηTcu ηTdu
]T(3.30b)
Chapter 3. Relative Attitude and Position Kinematics 23
F =
−[ωd×] A(q) −I3×3
03×3 03×3 03×3
03×3 03×3 03×3
(3.31a)
G =
A(q) −I3×3 03×3 03×3
03×3 03×3 I3×3 03×3
03×3 03×3 03×3 I3×3
(3.31b)
The spectral density matrix of the process noise w is given by
Q =
σ2cvI3×3 03×3 03×3 03×3
03×3 σ2dvI3×3 03×3 03×3
03×3 03×3 σ2cuI3×3 03×3
03×3 03×3 03×3 σ2duI3×3
(3.32)
Solutions for the state transition matrix of F and discrete-time process noise
covariance are intractable due to the dependence of both on the attitude matrix. In the
Ref. 29, a numerical solution can be obtain for a fixed parameter system, which includes
constant sampling interval and time invariant state and covariance matrices. This is
shown below as
A =
−F GQGT
0 F T
∆t (3.33)
B = eA ≡
B11 B12
0 B22
=
B11 Φ1Q
0 ΦT
(3.34)
Chapter 3. Relative Attitude and Position Kinematics 24
where Φ is the state transition matrix and the Q is the discrete-time process noise
covariance matrix. Then we have
Φ = BT22 (3.35a)
Q = ΦB12 (3.35b)
For the case of a “small” enough sampling interval, i.e. within Nyquist’s limit, then
Q = ∆tGQGT is a good approximation for the solution given by equation (3.35a).
3.2.2 Relative and Deputy Gyro Bias Case
In this section, the formulation to estimate the relative attitude, relative and deputy
gyro bias will be reviewed. The advantage of estimating the relative bias directly is that
the EKF gives its covariance directly in this case. Now the linearized equations will
involve βd and βdc instead of βc. Define ∆βdc ≡ βdc− βdc, and using βdc = βd−A(q)βc,
βc = βc + ∆βc and equations (3.25b), leads to
∆βdc = [(βdc − βd)×]δα+ ∆βd − A(q)∆βc (3.36)
Again, the second-order effect are ignored. Solving equation (3.36) for A(q)∆βc −∆βd
and substituting the results into equation (3.28) leads to
δα = −[(ωd − βdc)×]δα−∆βdc + A(q)ηcv − ηdv (3.37)
Next, we determine a dynamics model for βdc = βd−A(q)βc by taking the time derivative
of this equation, and using equations (3.19a) to (3.19f). The result is
Chapter 3. Relative Attitude and Position Kinematics 25
βdc = −[(ωd − A(q)ωc − βd − ηdv + A(q)ηcv)×]βdc
+ [(ωd − A(q)ωc − ηdv + A(q)ηcv)×]βd + ηdu − A(q)ηcu
(3.38)
Then, the estimate equation can be written as
˙βdc = −[(ωd − A(q)ωc − βd)×]βdc + [(ωd − A(q)ωc)×]βd (3.39)
The linear dynamics of ∆˙βdc can be derived in a similar fashion as the other linearized
equations shown to this point. The error-state dynamics are given by
∆x = F∆x +Gw (3.40)
with
∆x ≡[δαT ∆βTc ∆βTd
]T(3.41a)
w ≡[ηTcv ηTdv ηTcu ηTdu
]T(3.41b)
Chapter 3. Relative Attitude and Position Kinematics 26
F =
−[(ωd − βdc)×] −I3×3 03×3
F12 F22 F23
03×3 03×3 03×3
(3.42a)
F21 = [(βd − βdc)×][A(q)ωc×] (3.42b)
F22 = −[(ωd − A(q)ωc − βd)×] (3.42c)
F23 = [(ωd − A(q)ωc − βdc)×] (3.42d)
G =
A(q) −I3×3 03×3 03×3
−[(βd − βdc)×]A(q) [(βd − βdc)×] −A(q) I3×3
03×3 03×3 03×3 I3×3
(3.42e)
The spectral density matrix of the process noise w is given by equation (3.31b). The
filter equations can now be employed using the state matrices in equation (3.40) in the
covariance propagation as well as equations (3.20a), (3.20d) and (3.39) for the state
propagation. Note that it is difficult to obtain a closed-form solution for the state
propagation equation due to the appearance of the attitude matrix in equation (3.39) in
this case. However, if the sampling interval is “small” enough, or within the Nyquist’s
limit, then a pure discrete-time propagation can be employed by holding q constant
throughout the interval, while using the discrete-time quaternion propagation in equation
(3.16).
3.2.3 Relative and Chief Gyro Bias Case
In this section, the necessary equations to estimate the relative attitude, and the relative
and chief gyro biases will be reviewed. For brevity these equations are shown without
derivation. The dynamic equation for the relative bias estimate is given by
Chapter 3. Relative Attitude and Position Kinematics 27
˙βdc = [A(q)βc×]βdc = [A(q)βc×](ωd − A(q)ωc) (3.43)
The error-state dynamics are given by
∆x = F∆x +Gw (3.44)
with
∆x ≡[δαT ∆βTc ∆βTd
]T(3.45a)
w ≡[ηTcv ηTdv ηTcu ηTdu
]T(3.45b)
F =
−[(ωd − βdc)×] −I3×3 03×3
F21 F22 F23
03×3 03×3 03×3
(3.46a)
F21 = [A(q)βc×][A(q)ωc×] + [(ωd − A(q)ωc − βdc)×][A(q)βc×] (3.46b)
F22 = [A(q)βc×] (3.46c)
F23 = [(ωd − A(q)ωc − βdc)×]A(q) (3.46d)
G =
A(q) −I3×3 03×3 03×3
−[A(q)βc×]A(q) [A(q)βc×] −A(q) I3×3
03×3 03×3 03×3 I3×3
(3.46e)
Next, assume the relative position for the spacecraft are well-known. Thus, the
Chapter 3. Relative Attitude and Position Kinematics 28
sensitivity matrix will only involve of quaternion, chief and deputy gyro bias. Again, the
truth and estimated sensor are
b = A(q)r (3.47a)
b− = A(q−)r (3.47b)
Now we use the identity of the cross product matrix
b ≡[b1 b2 b3
]T(3.48a)
a ≡[a1 a2 a3
]T(3.48b)
[a×]b = −[b×]a (3.48c)
By using the identities defined in equation (3.48), substituting A(q) = A(δq)A(q−) and
A(δq) ≈ I3×3 − [δα×] into equations (3.47) and (3.47a), it leads to
∆b = [A(q−r×]δα (3.49)
where ∆b ≡ b− b−. Then, the sensitivity matrix for the sensor measurement of relative
attitude kinematics, with condition of relative position of spacecraft are well-known, is
given by
Chapter 3. Relative Attitude and Position Kinematics 29
Hk(x−k ) =
[A(q−)r1×] 03×3 03×3
[A(q−)r1×] 03×3 03×3
......
...
[A(q−)r1×] 03×3 03×3
∣∣∣∣∣∣∣∣∣∣∣∣∣tk
(3.50)
where r−i is given by equation (2.3b). The number of rows of the sensitivity matrix
depends on the total number of sensor, N , on deputy spacecraft.
3.3 Relative Position Kinematics
In this section, the derivations of relative orbit motion of equations in Ref. 3 will be
reviewed. This is followed by the review for the derivation of the linearization of the
relative orbit equations. Both the chief and deputy spacecraft are assumed to have equal
type and build. If each spacecraft has different type and build from the other, each of the
spacecraft will have different decay rate, ballistic coefficient which cause an uncontrolled
orbit (i.e., they do not repeat each orbit). Thus, for the assumption case, it is possible
to obtain the closed relative orbits through analytical solution.
3.3.1 Relative Orbit Motion Equations
Figure 2.2 in chapter 2 shows the orientation of the chief and deputy spacecraft around
a planet (or earth in this case). Assume that the inertia frame (N frame) is rotating
together with the earth, while the O reference frame is rotating together with the chief
spacecraft respect to the inertia frame. The O reference frame has frame or, oθ, oh.
Vector or is the chief orbit radius direction, and oh is parallel to the orbit momentum
vector in the orbit normal direction. The vector oθ can be simply obtained through
the right-hand coordination between oh and or. The O reference frame vectors can be
Chapter 3. Relative Attitude and Position Kinematics 30
expressed as below:
or =rcrc
(3.51a)
oθ = oh × or (3.51b)
o + h =h
h(3.51c)
with h = rc × rc. The relative orbit position vector, ρ is expressed by ρ = (x, y, z)T in
the O reference frame, which can be rewritten as ρ = xrr + yrθ + zrh. Then, the deputy
spacecraft position vector, with respect to the inertia frame is
rd = rc + ρ = (rc + x)or + yoθ + zoh (3.52)
The angular velocity of the O reference frame with respect to the inertia frame is
ωo/N = f oh (3.53)
where f is the chief true anomaly. Thus, by taking the time derivative of deputy space-
craft for twice, with respect to the inertia frame, the deputy spacecraft acceleration is
given by
rd = (rc + x− 2yf − fy − f 2(rc + x))or
+ (y + 2f(rc + x) + f(rc + x)− f 2y)oθ + zoh
(3.54)
Given the chief orbit angular momentum magnitude, h = r2c f , from the Keplerian mo-
tion, the angular momentum h is constant, which leads to the time derivative of h as
Chapter 3. Relative Attitude and Position Kinematics 31
h = 2rcrcf + r2c f = 0 (3.55)
The true anomaly acceleration can be obtained as:
f = −2rcrcf (3.56)
From figure 2.2, the chief spacecraft position can be express as rc = rcor. Again, the
time derivative for chief spacecraft acceleration with respect to the inertia frame is given
by
rc = (rc − rcf 2)or (3.57)
By using the orbit equation of motion rc = − µr3c
rc,and substituting into equation (3.57),
the chief spacecraft acceleration can be express as
rc = − µr2c
= rcf2
(1− rc
p
)(3.58a)
rc = rcf2 − mu
r2c
= rcf2
(1− rc
p
)(3.58b)
By substituting equations (3.58b) and (3.56) into equation (3.54), the deputy spacecraft
acceleration expression is updated by
rd =
(x− 2f
(y − y rc
rc
)− xf 2 − µ
r2c
)or
+
(y + 2f
(x− xrc
rc
)− yf 2
)oθ + zoh
(3.59)
Chapter 3. Relative Attitude and Position Kinematics 32
Now, let the orbit equations of motion for deputy spacecraft be given by rd = −mur3d
rd.
Substituting equation (3.59) into the deputy spacecraft orbital equations of motion leads
to
rd = −mur3d
Orc + x
y
z
(3.60)
Where rd =√
(rc + x)2 + y2 + z2. Then, by equating equations (3.59) and (3.60) for
each unit vector, the nonlinear relative equations of motion can be expressed as
x− 2f
(y − y rc
rc
)− xf 2 − mu
r2c
= −mur3d
(rc + x) (3.61a)
y + 2f
(x− xrc
rc
)− yf 2 = −mu
r3d
y (3.61b)
z = −mur3d
z (3.61c)
The assumption for the relative equations of motion is that there is no disturbance acting
on the spacecraft. Then equation (3.61) is valid for arbitrarily large relative orbits, and
the chief orbit can be eccentric. For the case where the relative orbit coordinate (x, y, z)
is small compared to the chief orbit radius, then equation (3.61) can be simplified. Thus,
the deputy spacecraft orbit radius rd can be approximated as
rd = rc
√1 + 2
x
rc+x2 + y2 + z2
r2c
≈ rc
√1 + 2
x
rc(3.62)
which leads to
Chapter 3. Relative Attitude and Position Kinematics 33
mu
r3d
≈ mu
r3c
(1− 3
3
rc
)(3.63)
The termmu
r3c
can also be expressed in the following terms:
mu
r3c
=rcpf 2 =
f 2
1 + e cos f(3.64)
where p is the semilatus rectum of the chief. The orbit elements in equation (3.64) are
chief orbit elements. By using the expression in equation (3.64), and neglecting the
higher order terms, equation (3.60) can be simplified to
−nur3d
Orc + x
y
z
≈ −mur3c
(1− 3
x
rc
)Orc + x
y
z
≈ −mur3c
Orc − 2x
y
z
(3.65)
Then, by substituting equation (3.65) into equation (3.61), with the assumption of the
relative orbit coordinate (x, y, z) is small compared to the chief orbit, the relative orbit
equations of motion can be simplified to
x− xf 2
(1 + 2
rcp
)− 2f
(y − y rc
rc
)= 0 (3.66a)
y + 2f
(x− xrc
rc
)− yf 2
(1− rc
p
)= 0 (3.66b)
z +rcpf 2z = 0 (3.66c)
Next, let the true latitude be θ = ω+ f , and let the orbit plane orientation be inertially
Chapter 3. Relative Attitude and Position Kinematics 34
fixed, with ω = 0. Therefore θ = f , which is equal to the true anomaly rate. Then, as-
sume that there are acceleration disturbances which are modelled as zero-mean Gaussian
white-noise processes. The relative orbit equations of motion can be written
x− xθ2
(1 + 2
rcp
)− 2θ
(y − y rc
rc
)= wx (3.67a)
y = 2θ
(x− xrc
rc
)− yθ2
(1− rc
p
)= wy (3.67b)
z +rcpθ2z = wz (3.67c)
The true anomaly acceleration and the chief orbit radius acceleration are given by
θ = −2rcrcθ (3.68a)
rc = rcθ2
(1− rc
p
)(3.68b)
For the case chief spacecraft orbit is assumed to be a circular orbit, where rc = 0
and p = rc, the relative equations of motion can be simplified into the form known as
Clohessy-Wiltshire (CW) equations ,30,31 which is given below (with disturbances added
here)
x− 2ny − 2n3x = wx (3.69a)
y + 2nx = wy (3.69b)
z + n2z = wz (3.69c)
Chapter 3. Relative Attitude and Position Kinematics 35
where n = θ is the mean motion.
Next, we define the state space model for the nonlinear general relative orbit
equations motion:
X =
[x y z x y z rc rc θ θ
]T≡[x1 x2 x3 x4 x5 x6 x7 x8 x9 x10
]T (3.70)
X = f(X) ≡
x4
x5
x6
x1x10(1 + 2x7/p) + 2x10(x5 − x2x8/x7)
−2x10(x4 − x1x8/x7) + x2x210(1− x7/p)
−x7x210x3/[
x8
x7x210(1− x7/p)
x10
−2x8x10/x7
(3.71)
The chief radius and its true anomaly as well as the derivative are estimated. If the
relative attitude parameters are all well known in this case, then the error-state vector
for the relative position is given by
∆x ≡[∆ρT ∆ρT ∆rc ∆rc ∆θ ∆θ
]T(3.72)
where ρ ≡[x y z
]T, ρ ≡
[x y z
]Tand ∆x = F∆x + Gw, where the F matrix is
a straightforward partial matrix ∂f(X)/∂X.
Chapter 3. Relative Attitude and Position Kinematics 36
J =∂f(X)
∂X
∣∣∣x=x
=
0 0 0 1 0 0 0 0 0 0
0 0 0 0 1 0 0 0 0 0
0 0 0 0 0 1 0 0 0 0
J1 −2x8
x7x10 0 0 2x10 0 J2 −2x2
x7x10 0 J3
2x8
x7x10 J4 0 −2x10 0 0 J5 2x1
x7x10 0 J6
0 0 −x7
px2
10 0 0 0 −1px2
10x3 0 0 J7
0 0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 J8 0 0 J9
0 0 0 0 0 0 0 0 0 1
0 0 0 0 0 0 2x8
x7x10 − 2
x7x10 0 −2x8
x7
(3.73)
J1 = x10(1 + 2x7
p)
J2 = 2x1x10
p+ 2x10x2
x8
x7
J3 = x1(1 + 2x7
p) + 2x5 − 2x2
x8
x7
J4 = x210(1− 2x7
p)
J5 = −2x10x1x8
x27− x2
x102
p
J6 = −2x4 + 2x1x8
x7+ 2x2x10(1− x7
p)
J7 = −2x7
px10x3
J8 = x21−(1− x7
p)− x7
px2
10
J9 = 2x7x10(1− x7
p)
(3.74)
The process noise vector for the relative position is defined as w ≡[wx wy wz
]T.
Chapter 3. Relative Attitude and Position Kinematics 37
Then the G matrix for the error-state vector for relative position is given by
G =
[03×3 I3×3 02×3 02×3
]T(3.75)
3.4 Relative attitude and position kinematics
In the previous sections, three different linearized error-state vector for relative attitude
kinematics (equations (3.31), (3.42), and (3.46)), and one for relative position (equation
(3.73)) have been shown. In this section, the error-state vector matrices for combination
of relative attitude and relative position kinematics will be shown. Instead of using all
three kinds of relative attitude cases, only one case, the chief and deputy gyro bias case
will be used in this section. Then, the sensitivity matrix for combination of relative
attitude and relative position kinematics will be reviewed.
The new error-state vector, through appending equation (3.72) into equation (3.30a), is
given by
∆x =
[δαT ∆βTc ∆βTd ∆ρT ∆ρT ∆rc ∆rc ∆θ ∆θ
]T(3.76)
Thus, the new matrices F and G for the covariance propagation are given by
Chapter 3. Relative Attitude and Position Kinematics 38
F =
−[ωd×] A(q) −I3×3 03×10
03×3 03×3 03×3 03×10
03×3 03×3 03×3 03×10
010×3 010×3 010×3∂f(X)∂X
∣∣x=x
(3.77a)
G =
A(q) −I3×3 03×3 03×3 03×3
03×3 03×3 I3×3 03×3 03×3
03×3 03×3 03×3 I3×3 03×3
03×3 03×3 03×3 03×3 03×3
03×3 03×3 03×3 03×3 I3×3
02×3 02×3 02×3 02×3 02×3
02×3 02×3 02×3 02×3 02×3
(3.77b)
Then, the new process noise vector is defined as w ≡[ηTcv ηTdv ηTcu ηTdu wx wy wz
]T.
The new process noise covariance matrix Q is given by
Q =
σ2cvI3×3 03×3 03×3 03×3 03×1 03×1 03×1
03×3 σ2dvI3×3 03×3 03×3 03×1 03×1 03×1
03×3 03×3 σ2cuI3×3 03×3 03×1 03×1 03×1
03×3 03×3 03×3 σ2duI3×3 03×1 03×1 03×1
01×3 01×3 01×3 01×3 w2x 0 0
01×3 01×3 01×3 01×3 0 w2y 0
01×3 01×3 01×3 01×3 0 0 w2z
(3.78)
Next, the sensitivity matrix given by equation (3.50) is modified to become
Chapter 3. Relative Attitude and Position Kinematics 39
Hk(q−k , ρ
−k ) =
[A(q)r21×] 03×3 03×3
∂b−1∂ρ−
03×7
[A(q)r22×] 03×3 03×3
∂b−2∂ρ−
03×7
......
......
...
[A(q)r2N×] 03×3 03×3
∂b−N∂ρ−
03×7
∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣tk
(3.79)
and again, r−i is given by equation (2.3b), and evaluated at ρ−i ≡[x− y− z−
]T. The
partial matrix ∂b−i /∂ρ− is given by
∂b−i∂ρ−
= A(q−)∂r−i∂ρ−
(3.80)
with
∂r−i∂ρ−
=1
s−i
[(Yi − y−)2 + (Zi − z−)2] (Xi − x−)(Yi − y−) (Xi − x−)(Zi − z−)
(Xi − x−)(Yi − y−) [(Xi − x−)2 + (Zi − z−)2] (Yi − y−)(Zi − z−)
(Xi − x−)(Zi − z−) (Yi − y−)(Zi − z−) [(Xi − x−)2 + (Yi − y−)2]
(3.81)
where s−i ≡ [(Xi − x−)2 + (Yi − y−)2 + (Zi − z−)2]3/2. Then, this error-state matrix and
sensitivity matrix now are able to be used by the filters for relative attitude and position
estimation.
3.5 Summary
The relative attitude and position kinematics have been reviewed in this chapter. First,
equation (3.16) shows a discrete-time closed-form solution for the relative attitude quater-
nion propagation. Then the error-state matrix for three different cases of the relative
Chapter 3. Relative Attitude and Position Kinematics 40
attitude kinematics has been shown, which are chief and deputy gyro bias case, relative
gyro and deputy gyro bias case and relative gyro and chief gyro bias case. The sensitivity
matrix in equation (3.50) for the relative attitude alone makes the assumption that the
relative position is well-known. Then, the EKF or UF will able to estimate both the
gyro bias and the relative attitude information with the process noise covariance.
Next, a detailed derivation of the relative orbital equation of motion, equation
(3.67), between the chief and deputy spacecraft has been shown. It is assumed that the
only disturbance that exists is in the acceleration. Then, the linearized error-state matrix
of the state-space model of relative position is appended into the error-state matrix of
the relative attitude kinematics, which is shown in equations (3.77a) and (3.77b). With
the new process noise covariance matrix, equation (3.78) and the sensitivity matrix,
equation (3.79), can now be used in the EKF and UF to estimate the relative attitude,
chief gyro bias, deputy gyro bias and the relative position of the spacecraft.
Chapter 4
State estimation
In this section, the derivation of EKF will be reviewed, followed by the review of the
derivation of the UF. In 1960, R.E. Kalman published a paper, Ref. 32, describing a
recursive solution to the discrete-data linear filtering solution. Since that time, due in
large part to advances in digital computing, the Kalman filter has been the subject of
extensive research and application.33 It is often that the sensor does not either measure
all the states or measure them directly. Therefore, it is important for the estimator
to obtain the state information to control the application. The Kalman filter has the
advantage on estimating all the states for this kind of case since it supports the estimation
of past, present and even predicts for the future, either with or without knowing well
about the model system. Besides that, the sensor never outputs the exact measurement
data, except with some error that we call “measurement noise”. So, the estimator needs
to filter or minimize the measurement noise, which the Kalman filter has been shown to
handle very well.
However, most of the dynamic systems in the world are essentially nonlinear in
nature, including the attitude and orbital dynamics. Therefore, a nonlinear version of the
Kalman filter (or EKF) has been developed shortly after the introduction of the original
41
Chapter 4. State estimation 42
Kalman filter.1 The EKF has been successfully applied to many nonlinear systems for
many years, with the appropriate assumption that the estimated states are close to the
true states. Since there is a limitation on the EKF, another approach of Kalman Filter
has been developed by Julier, Uhlmann and Durrant-Whyte to overcome the problem,
which is the Unscented Filter (UF). The UF works the same way as the standard Kalman
Filter does, but it allows higher errors in the initial estimation value. The UF uses the
sigma points transformation from covariance matrix for the propagation stage, and the
linearization for nonlinear model during the estimation process is not necessary.
4.1 Extended Kalman Filter
The standard Kalman Filter consists of a few steps - initialize, gain, update and propa-
gation. First getting the initial estimated states, then using the covariance, sensitivity
matrix, to obtain the gain for updating the state estimation and thus propagate them
into next time step, and repeat again. Given the truth model
x(t) = f(x(t),u(t), t) +G(t)w(t) (4.1a)
y(t) = h(x(t)) + v(t) (4.1b)
where f(x(t),u(t), t) and h(x(t)) function vectors that assumed to be continuously dif-
ferentiable. The vector w denotes the process noise, and v denotes the measurement
noise. Given the process noise defined as
Chapter 4. State estimation 43
Ew(t) = 0 Ew(τ)w(t)T = Q(t)δ(t− τ)
Evk = 0 EvjvTk = δjkRk
(4.2)
where Q(t) is the power spectral density matrix of the process noise and δ(t− τ) is the
Dirac delta function with
δjk =
0, when j/ 6= k
1, when j = k
(4.3)
The problem with the nonlinear model is that unlike the linear case, a Gaussian in-
put does not necessarily produce a Gaussian output. The EKF uses a Taylor’s series
expansion to linearize the nonlinear model, which is given by
f(x(t),u(t), t) = f(x(t),u(t), t) +∂f
∂x
∣∣∣x=x
+ h.o.t (4.4a)
hk(xk) = hk(x−k ) +
∂hk(x)
∂x
∣∣∣x=x−k
(xk − x−k ) + h.o.t (4.4b)
where h.o.t abbreviates “higher order terms”. If we assume that the estimated value is
close to the truth, with the error, δx < 1, then the higher order terms can be neglected.
Since both f(x(t),u(t), t) and hk(xk) are assumed to be continuously differentiable, we
can define the partial derivatives as below,
F (x(t),u(t), t) ≡ ∂f
∂x
∣∣∣x=x
Hk(x−k ) ≡ ∂hk(x)
∂x
∣∣∣x=x−
(4.5)
Note that F (x(t),u(t), t) is expanded about the current estimate (conditional mean)
Chapter 4. State estimation 44
while Hk(x−k , t) is expanded about the propagated states. By substituting equation
(4.4b) into equation (4.3), and neglecting the h.o.t leads to
f(x(t),u(t), t) ' f(x(t),u(t), t) + F (x(t),u(t), t)(x− x) (4.6a)
hk(xk) ' hk(x−k ) +Hk(x
−k )(xk − x−k ) (4.6b)
Taking the expectation of equation (4.6) yields
Ef(x(t),u(t), t) = f(x(t),u(t), t) = f(x(t),u(t), t) (4.7a)
Ehk(xk) = hk(xk) = hk(xk) (4.7b)
By taking the reference of equation (4.7), the expectation of equation (4.4a) can be
shown as equation (4.7a) below:
˙x(t) = f(x(t),u(t), t) (4.8)
The propagation equation of the covariance, given in continuous time is
P (t) = F (x(t),u(t), t)P (t) + P (t)F T (x(t),u(t), t) +G(t)Q(t)GT (t) (4.9)
Next, the update equation for the estimated states can be shown below:
Chapter 4. State estimation 45
Table 4.1: Continuous-Discrete Extended Kalman Filter2
Modelx(t) = f(x(t),u(t), t) +G(t)w(t)
yk = hk(xk) + vk
Initializex(t0) = x0
P0 = Ex0xT0
GainKk = P−k H
Tk (x−k )[Hk(x
−k )P−k H
Tk (x−k ) +Rk]
−1
Hk(x−k ) ≡ ∂hk(x)
∂x
∣∣x=x
Updatesx+k = x−k +Kk[yk − hk(x
−k )]
P+k = [I −KkHk(x
−k )]P−k
Propagation
˙x(t) = f(x(t),u(t), t)
P (t) = F (x(t),u(t), t)P (t) + P (t)F T (x(t),u(t), t) +G(t)Q(t)GT (t)
f(x(t),u(t), t) ≡ ∂f∂x
∣∣xx
x+k = K ′kx
−k +Kkyk (4.10)
where x−k denotes the pre-update estimated states, and x+k denotes the post-update
estimated states. Define the error-states of equation as
x−k ≡ x−k − xk (4.11a)
x+k ≡ x+
k − xk (4.11b)
Substituting equation (4.1b) and (4.11a) into equation (4.10) leads to
x+k = K ′kx
−k +Kkhk(xk) +Kkvk + x−k − x−k (4.12)
Chapter 4. State estimation 46
By taking the expectation of equation (4.12), with reference of equation (4.2) and Ex =
0 yields
K ′kx−k = x−k −Kkhk(xk) (4.13)
Then substituting equation (4.13) into equation (4.10) leads to
x+k = x−k +Kk[yk − hk(xk)] (4.14)
Substituting equation (4.13) into equation (4.12) and using the approximation of equa-
tion (4.6b) yields
x+k = x−k +Kk[hk(xk)− hk(xk)] +Kkvk
= x−k +Kk[hk(x−k ) +Hk(x
−k )(xk − xk)− h−k (xk)] +Kkvk
= x−k +Kk[yk − hk(x−k )]
(4.15)
To obtain the Kalman gain Kk, the mean-square-error (MSE) is minimized through
minimizing the trace of the error covariance matrix. The definition of error covariance
is given by
P−k ≡ Ex−k x−Tk (4.16a)
P+k ≡ Ex+
k x+Tk (4.16b)
Using equation (4.15) in equation (4.16b) with the assumption that P+k is not correlated
to yk leads to
Chapter 4. State estimation 47
P+k = P−k +KkE[hk(xk)− hk(xk)]hk(xk)− hk(xk)]
TKTk
+ Ex−k [hk(xk)− hk(xk)]T+KkEhk(xk)
− hk(xk)]x−Tk +KkRkK
Tk
(4.17)
The cost function is to minimize the trace of P+k :
J(Kk) = Tr[P+k ] (4.18)
Taking the partial derivative of equation (4.18) with respect to Kk gives
∂
∂Kk
J(Kk) = 0 (4.19)
and using the approximation of equation (4.6b) to solve for Kk gives
Kk = −Ex−K [hk(xk)− hk(x)k)]T
× [E[hk(xk)− hk(xk)][hk(xk)− hk(xk)]T+Rk]
−1
= P−k HTk (x−k )[Hk(x
−k )P−k H
Tk (x−k ) +Rk]
−1
(4.20)
Substituting equation (4.20) into equation (4.17) with the approximation of equation
(4.6b) leads to
P+k = P−k +KkE[hk(xk)− hk(xk)]x
Tk
= [I −KkHk(x−k )]P−k
(4.21)
Chapter 4. State estimation 48
The covariance propagation in discrete time is given by
P−k+1 = ΦkP+k ΦT
k +GkQkGTk (4.22)
For most of the cases, the process noise covariance Q is given as spectral density matrix,
Q(t). By multiplying Q(t) by a delta function, it can be converted into a covariance
matrix. Since the discrete EKF requires the process noise covariance to be expressed
in discrete time, the state transition is required to be done. Again, the state transition
matrix can be shown by reviewing equations (3.33), (3.34), (3.35a) and (3.35b). Given
A =
−F GQGT
0 F T
∆t (4.23)
where F can be obtained in equation (4.5), ∆t is a constant sampling interval, and Q is
the constant continuous-time process noise covariance.
B = eA ≡
B11 B12
0 B22
=
B11 Φ1Q
0 ΦT
(4.24)
where Φ is the state transition matrix of F and the Q = GQGT is the discrete-time
process noise covariance matrix, which can be rewritten and reformulated as below:
Φ = BT22 (4.25a)
Q = ΦB12 (4.25b)
Table 4.1 shows a summary of the Continuous-Discrete EKF, while the Discrete
EKF is given in Table 4.2. One needs to be extremely careful when applying the Dis-
Chapter 4. State estimation 49
Table 4.2: Discrete Extended Kalman Filter2
Modelxk+1 = f(xk,uk, k) +Gkwk
yk = hk(xk) + vk
Initializex(t0) = x0
P0 = Ex0xT0
GainKk = P−k H
Tk (x−k )[Hk(x
−k )P−k H
Tk (x−k ) +Rk]
−1
Hk(x−k ) ≡ ∂hk(x)
∂x
∣∣x=x
Updatesx+k = x−k +Kk[yk − hk(x
−k )]
P+k = [I −Kkhk(x
−k )]P−k
Propagationxk+1 = f(xk,uk, k)
P−k+1 = ΦkP+k ΦT
k +GkQkkGTk
crete EKF since that the filter requires the true states to be “close” to the estimated
states. Unlike the linear Kalman filter, the stability of EKF is not guaranteed due to the
condition above. However, it still being widely used in practice since it is often robust
to initial condition errors.
4.2 Unscented Kalman Filter
The derivation of Unscented Filter (UF) will be reviewed in this section. Given the
nonlinear model system
xk+1 = f(xk,uk, k) +Gkwk (4.26a)
yk = h(xk, k) + vk (4.26b)
Again, assume the measurement noise vk and the process noise wk are zero mean Gaus-
Chapter 4. State estimation 50
sian noise process, with the covariances given by Rk and Qk respectively. The UF
update equations, which is similar to the standard Kalman filter update equations can
be rewritten as
x+k = x−k +Kk[yk − hk(x
−k , k)] (4.27a)
P+k = P−k −KkP
vvk KT
k (4.27b)
where the gain Kk is computed by
Kk = P xyk (P vv
k )−1 (4.28)
and P υυk denotes the covariance of vk, where vk ≡ yk − h(x−k , k), and P xy
k is the cross-
correlation matrix between x−k and y−k .
The UF uses a different way of propagation than the standard EKF. Given a
n×n covariance matrix, P , a set of 2n sigma points can be generated from the columns
(or rows) of the matrices ±√
(n+ λ)P ak . Where
√M is shorthand notation for a matrix
Z such that ZZT = M . The set of points is zero-mean, but if the distribution has
mean µ, then simply adding µ to each of the points yields a symmetric set of 2n points
having the desired mean and covariance.9 Due to the symmetric nature of this set, its
odd central moments are zero, so its first three moments are the same as the original
Gaussian distribution. The covariance matrix for the sigma points is defined by
P ak =
P+k P xw
k P xvk
(P xwk )T Qk Pwv
k
(P xvk )T (Pwv
k )T Rk
(4.29)
Chapter 4. State estimation 51
where P xwk is the correlation between the state error and process noise, P xv
k is the cor-
relation between state error and measurement noise, and Pwvk is the correlation between
process noise and the measurement noise. In most system, they are all zero. Augmenting
the covariance requires the computation of 2(q + l) additional sigma points (where q is
the dimension of wk and l is the dimension of vk, the dimension of vk does not necessar-
ily have to be same with the dimension of output, m), but the effects of the process and
measurement noise in terms of the impact on the mean and covariance are introduced
with the same order of accuracy as the uncertainty in state.
The general formulation for the propagation equations is given as follows. First,
compute the following set of sigma points :
σk ← 2n columns from ±√
(n+ λ)P ak (4.30a)
χk(0) = xak (4.30b)
χk(i) = σk(i) + xak i = 1, 2, . . . , 2n (4.30c)
where the augmented state, xak is define by
xak =
xk
wk
vk
, xak =
xK
(0)q×1
0l×1
(4.31)
and the λ is a composite scaling parameter, which is given by
λ = α2(n+ κ)− n (4.32)
The α are constant value, which determines the spread of the sigma points. Usually the
Chapter 4. State estimation 52
α is set to a small positive value (e.g., 1× 10−1 ≤ α ≤ 1). The scalar κ is a convenient
parameter for exploiting knowledge (if available) about the higher moments of the given
distribution. In scalar systems (i.e., for n = 1), a value of κ (with the α assumed to be
1) leads to errors in the mean and variance that are sixth order. For higher-dimensional
system, choosing κ = 3 − n minimizes the mean-squared-error up to the fourth order.
However, caution should be exercised when κ is negative since a possibility exists that
the predicted covariance can become non-positive semi-definite matrix. For this case,
another approach can be used, which the “covariance” is evaluated about χ(0)k+1 and
guarantees a positive semi-definite covariance matrix.8 Next, the transformed set of
sigma points are evaluated for each of the points by
χk+1(i) = f(χxk(i),χwk (i),uk, k), i = 0, 1, 2, . . . , 2n (4.33)
where both χxk(i) and χwk (i) are evaluated in the matrix χak(i) which is shown below
χak(i) =
χxk(i)
χwk (i)
χvk(i)
(4.34)
where χvk(i) in the χak(i) will be used to compute the output covariance, where the output
for each sigma points can be defined as
γk+1(i) = h(χxk+1,uk+1,χvk+1, k + 1), i = 0, 1, 2, . . . , 2n (4.35)
Now, define the following weights:
Chapter 4. State estimation 53
Wmean0 =
λ
n+ λ(4.36a)
W cov0 =
λ
n+ λ+ (1− α2 + β) (4.36b)
Wmeani = W cov
i =1
2(n+ λ), i = 1, 2, . . . , 2n (4.36c)
where β is used to incorporate prior knowledge of the distribution. For ease of use, β
can be set to be zero for most of time. Another good starting guess is β = 2. The
weights that are defined in equation (4.36) will be used to calculate the predicted mean,
covariance and the observations. The predicted mean is given by
x−k+1 =2n∑i=0
Wmeani χxk+1(i) (4.37)
The predicted covariance is given by
P−k+1 =2n∑i=0
W covi [χxk+1(i)− x−k+1][χ
xk+1(i)− x−k+1]
T (4.38)
The mean observation is given by
y−k+1 =2n∑i=0
Wmeani γk+1(i) (4.39)
The output covariance is given by
P yyk+1 =
2n∑i=0
W covi [γk+1(i)− y−k+1][γk+1(i)− y−k+1]
T (4.40)
Then the innovation covariance is simply given by
Chapter 4. State estimation 54
P vvk+1 = P yy
k+1 (4.41)
Finally, the cross-correlation matrix is determined by
P xyk+1 =
2n∑i=0
W covi [χxk+1(i)− x−k+1][γk+1(i)− y−k+1]
T (4.42)
The filter gain will be computed by using equation (4.28), and the state vector can now
be updated using equation (4.27a). Even though there are total of 2n+ 1 propagations
required for the UF, the computations may be comparable to the EKF, especially if the
continuous-time covariance equation needs to be integrated and a numerical Jacobian
matrix is evaluated. For the ideal case, the UF is ideally suited for parallel computation
architectures, since the propagation can be performed in parallel. Also, if the measure-
ment noise vk appears linearly with the output, l = m, then the augmented state can
be reduced. This is because the system state does not need to be augmented with the
measurement noise. In this case, the augmented state will be redefined as
xak =
xk
wk
, xak =
xk
0q×1
(4.43)
The covariance measurement noise will be added into the innovation covariance, which
is given by
P vvk+1 = P yy
k+1 +Rk+1 (4.44)
The derivation of the UF has been reviewed. However, there are some elements that need
to be modified for both of the quaternion vector and the UF before it can be used for
the relative attitude estimation, which will be shown in a further section in this chapter.
Chapter 4. State estimation 55
There is a square root version of UF that has been developed in Ref. 34, which improves
the numerical properties. Besides that, Ref. 35 also presents an Unscented Particle
Filter which makes no assumption on the form of probability densities. However, both
of them will not be considered in this thesis research.
4.3 Algorithm of State Estimation for Relative At-
titude and Position Estimation
The derivations of both the EKF and the UF have been reviewed in the previous sec-
tion. However, due to the characteristic of the quaternion vector, the straightforward
addition of state updates for quaternion vector will not work. Besides that, for the same
reason, a conversion of quaternion-error into Rodrigues parameters will be required for
UF estimation before sigma points projection.
4.3.1 Extended Kalman Filter for Relative Attitude and Posi-
tion Estimation
A summary of discrete EKF has been shown in Table 4.2. In this section, the algorithm
for the EKF will be slightly modified so that the EKF can be optimized for this estima-
tion. Since the straight-forward addition between the quaternion and its error will not
produce a unit vector, the quaternion will update through the quaternion multiplication.
First, the algorithm for EKF estimation for relative attitude, chief and deputy gyro bias,
with the assumption that the relative position is well-known, will be shown. This will
be followed by the algorithm for EKF estimation for relative attitude, chief and deputy
gyro, and the relative position.
Chapter 4. State estimation 56
Table 4.3: Extended Kalman Filter for Relative Attitude Estimation1
Initializeq(t0) = q0, βc(t0) = βc0 βd(t0) = βd0
P0 = P0
Gain
P+k = ΦkP
−k ΦT
K +QKk = P+
k HTk (x−k )[Hk(x
−k )P+
k HTk (x−k ) +Rk]
−1
Hk(q−k ) =
[A(q−)r1×] 03×3 03×3...
......
[A(q−)rN×] 03×3 03×3
∣∣∣∣∣∣∣tk
Updates
∆x+k = Kk[yk − hk(q
−k )]
∆x+k ≡
[δα+T
k ∆β+Tck
∆β+Tdk
]Thk(q
−k ) =
A(q−)r1
A(q−)r2...
A(q−)rN
∣∣∣∣∣∣∣∣∣tk
q+k = q−k +
1
2Ξ(q−k )δα+
k
β+ck
= β−ck + ∆β+ck
β+dk
= β−dk+ ∆β+
dk
Propagation
ω+ck
= ωck − β+ck
ω+dk
= ωdk− β+
dk
q−k+1 = Ω(ω+dk
)Γ(ω+ck
)q+k
P−k+1 = [I −KkHk(q−k )]P+
k
Table 4.3 shows the summary of the discrete EKF estimation for relative attitude,
chief and deputy gyro bias. The covariance matrix is updated before the gain being cal-
culated. From Ref. 1 the simulation showed that both ways for updating the covariance
matrix, either pre-gain or post-gain are able to produce good results. However, the co-
variance matrix and the state-errors converge better if the covariance matrix updates
before the gain is calculated.
Due to the complication of the quaternion updates, first, we assume the quater-
Chapter 4. State estimation 57
Table 4.4: Extended Kalman Filter for Relative Attitude and Position Estimation
Initializeq(t0) = q0, βc(t0) = βc0 βd(t0) = βd0
Xpos(t0) = Xpos0
P0 = P0
Gain
P+k = ΦkP
−k ΦT
K +QKk = P+
k HTk (x−k )[Hk(x
−k )P+
k HTk (x−k ) +Rk]
−1
Hk(q−k ) =
[A(q−)r1×] 03×3 03×3
∂b−1∂ρ−
03×7
......
......
...
[A(q−)rN×] 03×3 03×3∂b−N∂ρ−
03×7
∣∣∣∣∣∣∣∣∣∣∣tk
Updates
∆x+k = Kk[yk − hk(q
−k )]
∆x+k ≡
[δα+T
k ∆β+Tck
∆β+Tdk
∆X+posk
]T∆X+
posk≡[∆ρTk ∆ρTk ∆rck ∆ ˙rck ∆θk ∆θk
]Thk(q
−k ) =
A(q−)r1
A(q−)r2...
A(q−)rN
∣∣∣∣∣∣∣∣∣tk
q+k = q−k +
1
2Ξ(q−k )δα+
k
β+ck
= β−ck + ∆β+ck
β+dk
= β−dk+ ∆β+
dk
X+posk
= X−posk+ ∆X+
posk
Propagation
ω+ck
= ωck − β+ck
ω+dk
= ωdk− β+
dk
q−k+1 = Ω(ω+dk
)Γ(ω+dk
)q+k
˙X−pos = f(X+
posk)
P−k+1 = [I −KkHk(q−k )]P+
k
nion error, δα is small enough, which leads to
Chapter 4. State estimation 58
δq =
1
2δα
1
(4.45)
Thus, to within first-order the quaternion update is given by
q+k =
1
2δα+
k
1
⊗ q−k (4.46)
By using the quaternion multiplication rule in equation (3.4), equation (4.46) can be
rewritten as
q+k = q−k +
1
2Ξ(q−k )δα+
k (4.47)
Table 4.4 shows the summary of EKF estimation for the relative attitude, chief and
deputy gyro bias, and the relative position between two spacecrafts. The subscript “pos”
of Xpos denotes the relative position vector. The propagation equation of the relative
position is shown in continuous-time, X form. However since it does not have the same
complexity as the quaternion vector, the discrete-time of the relative position can be
easily determined by using a fourth-order Runge-Kutta method. In general, Table 4.4
is similar to the Table 4.3 with the exception that there are more states that need to
be estimated and updated. Again, the covariance matrix is updated before the gain is
calculated, which is due to the reason that was mentioned before.
Chapter 4. State estimation 59
4.3.2 Unscented Filter for Relative Attitude and Position Es-
timation
In this section, a modified UF will be derived for the relative attitude and position
estimation. First, an approach based on a trapezoidal approximation will be shown in
this section. In this approach, the way of sigma points being formed is different than
the general UF, which is shown below:
σk ← 2n columns from ±√
(n+ λ)[P+k + Qk] (4.48)
and the predicted covariance is modified to
P−k+1 =2n∑i=0
W covi [χxk+1(i)− x−k+1][χ
xk+1(i)− x−k+1]
T + Qk (4.49)
where Q is the total desired process noise, which can be obtained through a Lyapunov
function (or known as Sylvester equation36) that is given by
ΦkQkΦTk + Q = GkQkGT
k (4.50)
where Φk denotes the discrete-time state model, andQk denotes the discrete-time process
noise matrix, where both can be obtained from equation (4.25). The advantage of this
approach is that there is less augmented states that need to be computed. By adding the
Qk at the beginning and end of the propagation it might yields better results, although
Ref. 19 has shown that it had not much affect on the results.
Since the predicted quaternion mean is derived using an averaged sum of quater-
nions, no guarantees can be made that the resulting quaternion will have unit norm.
Therefore, if using the quaternion kinematics directly, which is equation (3.16), this ap-
Chapter 4. State estimation 60
proach has a clear deficiency. This makes the straightforward implementation of the UF
with quaternions undesirable.19 A better way is using an unconstrained three-component
vector to represent an attitude error quaternion, which is the modified Rodrigues pa-
rameters. The modified Rodrigues parameters works very well in this case since the
vector can be used for straightforward summation, mean, and also able to covert back
into quaternion vector without losing any information. First, the state vector is defined
by:
χk(0) = x+k =
δp+k
β+ck
β+dk
X+posk
(4.51)
where δp is the modified Rodrigues parameters of the local quaternion error, denoted
by δq ≡[δαT δq4
]T. The modified Rodrigues parameters is given by
δp ≡ fδα
a+ δq4(4.52)
where a is a parameter from 0 to 1, and f is a scale factor. Note that when a = 0 and
f = 1, equation (4.52) gives the Gibbs vector, and when a = f = 1 equation (4.52)
gives the standard vector of MRPs. For small errors the attitude part of the covariance
is closely related to the attitude estimation errors for any rotation sequence, given by a
simple factor.26 For example, the Gibbs vector linearize to half angles, and the vector of
MRPs linearize to quarter angles. In our case, we will choose f = 2(a+ 1) so that ‖δp‖
is equal to ϑ for small errors. The vector χk(i) in equation (4.31) is partitioned into
Chapter 4. State estimation 61
χk(i) ≡
χδpk (i)
χβc
k (i)
χβd
k (i)
χposk (i)
, i = 0, 1, . . . , 38 (4.53)
where χδpk is from the attitude-error part, χβc
k and χβd
k are from chief and deputy gyro
bias part, and χposk is from the relative position part respectively. To describe χδpk we
first define a new quaternion generated by multiplying an error quaternion by the current
estimate. Using the notation in equation (4.31), we assume
q+k (0) = q+
k (4.54a)
q+k (i) = δq+
k (i)⊗ q+k , i = 1, 2, . . . , 38 (4.54b)
where δqk(i) ≡[δα+T
k δq+4l
(i)
]Tis given by
δq+4K
(i) =−a‖χδpk (i)‖2 + f
√f 2 + (1− a2)‖χδpk (i)‖2
f 2 + ‖χδpk (i)‖2(4.55a)
δα+k (i) = f−1[a+ δq+
4k(i)]χδpk (i), i = 1, 2, . . . , 38 (4.55b)
Equation (4.54a) clearly requires that χδpk (0) to be zero. This is due to the reset of
the attitude error to zero after the previous update, which is used to move information
from one part of estimate to another part.37 Although, there is no new information
added, we might expect the covariance to be rotated since the reset rotates the reference
frame for the covariance. But the covariance depends on the assumed statistics of the
measurements, not on the actual measurements. Therefore, since the update is zero-
Chapter 4. State estimation 62
mean, the mean rotation caused by the reset is actually zero, so the covariance is in fact
not affected by the reset. Next, the updated quaternions are propagated forward using
equation (3.16) with
q−k+1(i) = Ω(ω+dk
(i))Γ(ω+dk
(i))q+k (i), i = 0, 1, . . . , 38 (4.56)
and the estimated angular velocity for chief and deputy gyro are given by
ω+ck
(i) = ωck − χβc
k (i) (4.57a)
ω+dk
(i) = ωdk− χβd
k (i), i = 0, 1, . . . , 38 (4.57b)
Note that ω+k = ωk − χβk(0), where χβk(0) is the zeroth-bias sigma points given by the
current estimate, i.e., χβk(0) = β+k . It will not be zero for both chief and deputy gyro
bias at that sigma points for most of the time. Then, the propagated error quaternions
are computed by using
δq−k+1(i) = q−k+1(i)⊗ [q−k+1(0)]−1, i = 0, 1, . . . , 38 (4.58)
Note that δq−k+1(0) is the identity quaternion, which denotes δq−k+1(0) =
[0 0 0 1
]T.
The propagated sigma points can be computed again using equation (4.52)
χδpk+1(0) = 0 (4.59a)
χδpk+1(i) = fδα−k+1(i)
a+ δq−4k+1(i), i = 1, 2, . . . , 38 (4.59b)
with
Chapter 4. State estimation 63
δq−k+1(i) =
[δα−Tk+1(i) δq−4k+1
]T(4.60)
The propagated chief and deputy gyro bias are assumed to be same as the previous
updated bias, which are
χβc
k+1(i) = χβc
k (i) (4.61a)
χβd
k+1(i) = χβd
k (i), i = 0, 1, . . . , 38 (4.61b)
Furthermore, the propagation of relative position is given by
χk+1(i)− = f(χ+
k )(i), i = 0, 1, . . . , 38 (4.62)
And again, the discrete-time of equation (4.62) can be obtained through fourth-order
Runge-Kutta method. Then the predicted mean and covariance can now be computed
by using equations (4.37) and (4.49).
A brief procedure of UF algorithm for relative attitude and position will be shown
in the summary section. The UF has been slightly modified so that it can be implemented
into the relative attitude and position estimation. By using the modified Rodrigues
parameters from the quaternion errors, it can be guaranteed that the averaged sum of
the augmented states for the error quaternions will have a unit norm. However, the UF
algorithm requires more computational cost and runs slower than the EKF. One way of
reducing the computational load is using the lower-triangular Cholesky decomposition
for the sigma points computations. The result shows that there is about a 15% saving
in the computational load.19
Chapter 4. State estimation 64
4.4 Summary
The derivations of both EKF and UF have been shown in this chapter. Due to the
complication of the characteristic of quaternion, both the EKF and UF need to be
modified so that the filters can be implemented into the quaternion updates. Table
4.4 showed the summary of the EKF estimation for the relative attitude and position
estimation. The assumption of the quaternion error δα is small enough leads to the
equation (4.47). Then, the state updates for quaternion part can now be computed.
Besides the EKF, the modification of UF also has been shown in this chapter.
This summary section will show the brief procedure of the UF algorithm that will be
used for the relative attitude and position estimation.
Initialization:
q(t0) = q0, βc(t0) = βc0 βd(t0) = βd0
δp = 03×1
Xpos(t0) = Xpos0
Chapter 4. State estimation 65
Sigma Points:
ΦkQkΦTk + Q = GkQkGT
k
σk ←2n columns from ±√
(n+ λ)P+k + Q
χk(0) = x+k =
δp+k
β+ck
β+dk
X+posk
χk(i) = χk(0) + σk(i), i = 1, 2, . . . , 38
Propagation:
q+k (0) = q+
k
q+k (i) = δq+
k (i)⊗ q+k
δqk(i) ≡[δα+T
k δq+4l
(i)
]T
δq+4K
(i) =−a‖χδpk (i)‖2 + f
√f 2 + (1− a2)‖χδpk (i)‖2
f 2 + ‖χδpk (i)‖2
δα+k (i) = f−1[a+ δq+
4k(i)]χδpk (i), i = 1, 2, . . . , 38
Chapter 4. State estimation 66
ω+ck
(i) = ωck − χβc
k (i)
ω+dk
(i) = ωdk− χβd
k (i)
q−k+1(i) = Ω(ω+dk
(i))Γ(ω+dk
(i))q+k (i)
δq−k+1(i) = q−k+1(i)⊗ [q−k+1(0)]−1, i = 0, 1, . . . , 38
χδpk+1(0) = 0
χδpk+1(i) = fδα−k+1(i)
a+ δq−4k+1(i), i = 1, 2, . . . , 38
χβc
k+1(i) = χβc
k (i)
χβd
k+1(i) = χβd
k (i)
χposk+1(i) = f(χposk (i)) i = 0, 1, . . . , 38
Weights:
Wmean0 =
λ
n+ λ
W cov0 =
λ
n+ λ+ (1− α2 + β)
Wmeani = W cov
i =1
2(n+ λ), i = 1, 2, . . . , 2n
Predicted Mean:
x−k+1 =2n∑i=0
Wmeani χk+1(i)
Chapter 4. State estimation 67
Predicted Covariance:
P−k+1 =2n∑i=0
W covi [χk+1(i)− x−k+1][χk+1(i)− x−k+1]
T + Q
Mean Observation:
γk+1(i) =
A[q−(i)]r1
A[q−(i)]rN...
A[q−(i)]rN
k+1
, i = 0, 1, . . . , 38
y−k+1 =2n∑i=0
Wmeani γk+1(i)
Output Covariance:
P yyk+1 =
2n∑i=0
W covi [γk+1(i)− y−k+1][γk+1(i)− y−k+1]
T
Innovation Covariance
P vvk+1 = P yy
k+1 +Rk
Cross-correlation matrix:
P xyk+1 =
2n∑i=0
W covi [χk+1(i)− x−k+1][γk+1(i)− y−k+1]
T
Gain:
Chapter 4. State estimation 68
Kk = P xyk (P vv
k )−1
Update:
x+k+1 = x−k+1 +Kkvk
P+k+1 = P−k+1 −KkP
vvk KT
k
vk ≡ yk+1 − y−k+1
Reset:
δp+k+1 = 03×1
Chapter 5
Simulation and Results
In this section, the necessary data for the simulation will be provided. First, a simulation
will run both the EKF and UF with the condition that the initial states are well-known
and the results from both filters will be compared. Then the simulation will be repeated
by increasing the initial estimated states’ error gradually until both filters are not able to
produce any result. By theory, the UF will able to provide a converged result regardless
how bad the initial estimated states are.
5.1 Simulation
The parameters from Hubble Space Telescope are selected for the chief spacecraft in
this simulation. We assume that the deputy spacecraft has the same exact properties
as the chief spacecraft. The angular velocities for chief spacecraft are given by ωc =[0 0.0011 −0.0011
]Tand the angular velocities for deputy spacecraft are given by
ωd =
[−0.002 0 0.0011
]T. The gyro noise parameters for both chief and deputy gyro
are given by σcu = σdu =√
10× 10−10rad/sec3/2 and σcv = σdv =√
1× 10−5rad/sec1/2,
with the chief and deputy gyro have the initial biases of 1 deg/hr for each axis. Given
69
Chapter 5. Simulation and Results 70
that the process noise components for the relative position, which are the acceleration
disturbances wx, wy and wz are each√
10× 10−11m/(s√s). Assume that there are total
of 6 beacons exist on the chief:
X1 = 0.5m, Y1 = 0.5m, Z1 = 0.0m (5.1a)
X2 = −0.5m, Y2 = −0.5m, Z2 = 0.0m (5.1b)
X3 = −0.5m, Y3 = 0.5m, Z3 = 0.0m (5.1c)
X4 = 0.5m, Y4 = −0.5m, Z4 = 0.0m (5.1d)
X5 = 0.2m, Y5 = 0.5m, Z5 = 0.1m (5.1e)
X6 = 0.0m, Y6 = 0.2m, Z6 = −0.1m (5.1f)
All of the beacons are assumed to be visible throughout the entire simulation run. The
VISNAV measurements are generated using equation (2.4) with a measurement standard
deviation given by 0.0005 degrees for each axis. The semimajor axis of the chief spacecraft
is a = 6, 998, 455 meters and the eccentricity is e = 0.00172. The orbit period of deputy
spacecraft is calculated to be 5,820 seconds. This period makes the deputy move about
6 times around the chief during the simulation run.1 A bounded relative orbit is used,
where the constraint is valid for eccentric and circular chief orbit. The constraint required
to satisfy the Cartesian initial conditions3 is:
y(t0)
x(t0)=
−n(2 + e)√(1 + e)(1− e)3
(5.2)
Note that t0 is defined at the perigee point, although both the EKF and UF can be
initiated at any part of the orbit. The initial chief orbit radius is rc(t0) = a(1− e), and
its true anomaly rate is θ(t0) =√µ/p(1 + e)/rc, where µ = 3.986008× 1013m3/s2 and p
Chapter 5. Simulation and Results 71
is semi latus rectum p = a(1− e2).
First, the simulation will be run with the assumption that the true initial states
are well-known. The initial quaternion vector is given by q(t0) =
[√
2/2 0 0√
2/2
]T,
and the initial condition for the relative position vector, in units of meters, meters per
second, radian and radian per second is given by
X(t0) =
[200 200 100 0.01 −0.4325 0.01 rc(t0) 0 0 θ(t0)
]T(5.3)
The initial estimate bias for both chief and deputy gyro are given by βc(t0) = βd(t0) =[0 0 0
]T. Each individual covariance sub-matrix for attitude, gyro bias, position and
velocity is assumed to be isotropic, i.e. a diagonal matrix with equal elements. The
covariance matrices for relative attitude, chief and deputy gyro bias are given by
Patt =
[1 1 1
]deg2 (5.4a)
Pchief = Pdeputy =
[4 4 4
]deg2/sec2 (5.4b)
The initial position covariance is set to 5I3×3m2 and the initial velocity covariance is
set to 0.02I3×3(m/s)2. The initial variance for the chief position is set to 1,000 m2
and the variance for chief radius rate is set to 0.01 (m/s)2. The initial variance for
the true anomaly is set to 1 × 10−4rad2 and the true anomaly rate variance is set to
1× 10−4(rad/sec)2.The total covariance matrix is defined by
P ≡ diag
[Patt Pchief Pdeputy Ppos
](5.5)
Chapter 5. Simulation and Results 72
where Ppos is the covariance matrix for the relative position elements. The unscented
parameters, α, β and κ are required to be determined before running the UF. For the
simplicity, α = 1 and β = 0 will be used for the entire simulation run, and a set of values,
κ =
[1 2 3
]will be used for this simulation. Furthermore, for the MRP conversion,
amrp = 1 with f = 4 has been chosen, which gives four times the vector of MRPs for the
errors representation.19 Note that amrp denotes the parameters for MRP, and a denotes
the semimajor axis for the chief spacecraft.
Let l to be the lth step of the error increment with each step of error increment
consisting of 2 deg for the attitude part and 2% for the relative position part. The new
initial estimated states matrices are given by
q(t0) =
[Ψ(δq) δq
]⊗ qtrue(t0) (5.6a)
δq =
[± sin(l × 2 deg) ± sin(l × 2 deg) ± sin(l × 2 deg) δq4
]T(5.6b)
Xpos(t0) = Xpostrue(t0) + 0.02× l × As •Xpos
true(t0) (5.6c)
where As denotes a 10×1 matrix such as its elements are either +1 or−1, and a•b denotes
the entry-by-entry product of matrices a and b with the condition of both a and bmatrices
must have same dimension. Note that the Cartesian initial condition requirement in
equation (5.2) has been ignored. However, we still assume that the simulation start at
the perigee point, where the chief radius velocity and the true anomaly at t0 remains to
be zero. The initial gyro bias for the chief and deputy spacecraft, and their covariance
matrices remain the same. Then, the new covariance matrices for each relative attitude
and the relative position are given by
Chapter 5. Simulation and Results 73
Patt =
[(l × 2)2 (l × 2)2 (l × 2)2
](5.7a)
Ppos = l × 0.02× |X|2 (5.7b)
The initial variance for the chief radius rate and true anomaly still the same, which are
given by
Ppos(8) = 0.01, Ppos(9) = 10−4 (5.8)
Again, the α = 1 and β = 0 will to be used for the entire simulation run. Since the
initial estimated states are different than the true states, the effect of κ on the results
will be more critical. But we don’t know what are the values of κ that give better results.
Therefore, let κ be a vector of κ =
[−18 −17 −16 . . . 3
]for the entire simulation
run. Then, a = 1 with f = 4 will be used again since it gives four times the vector of
MRPS for the error representation.
Chapter 5. Simulation and Results 74
5.2 Simulation Figures
5.2.1 Well-known initial condition
(a) EKF result (b) UF result with κ = 1
(c) UF result with κ = 2 (d) UF result with κ = 3
Figure 5.1: Attitude Errors and 3σ Bounds with Initial States Well-Known
Chapter 5. Simulation and Results 75
(a) EKF result (b) UF result with κ = 1
(c) UF result with κ = 2 (d) UF result with κ = 3
Figure 5.2: Estimated chief gyro bias Initial States Well-Known
Chapter 5. Simulation and Results 76
(a) EKF result (b) UF result with κ = 1
(c) UF result with κ = 2 (d) UF result with κ = 3
Figure 5.3: Estimated deputy gyro bias Initial States Well-Known
Chapter 5. Simulation and Results 77
(a) EKF result (b) UF result with κ = 1
(c) UF result with κ = 2 (d) UF result with κ = 3
Figure 5.4: Position Errors and 3σ Bounds Initial States Well-Known
Chapter 5. Simulation and Results 78
(a) EKF result (b) UF result with κ = 1
(c) UF result with κ = 2 (d) UF result with κ = 3
Figure 5.5: Velocities Errors and 3σ Bounds Initial States Well-Known
Chapter 5. Simulation and Results 79
(a) EKF result (b) UF result with κ = 1
(c) UF result with κ = 2 (d) UF result with κ = 3
Figure 5.6: Chief Orbit Elements Errors and 3σ Bounds Initial States Well-Known
Chapter 5. Simulation and Results 80
5.2.2 4 deg relative attitude error and 4% relative position ele-
ments error in initial condition
(a) EKF result (b) UF result with κ = 1
(c) UF result with κ = −10 (d) UF result with κ = 2
Figure 5.7: Attitude Errors and 3σ Bounds with Initial Error = 4 deg and 4%
Chapter 5. Simulation and Results 81
(a) EKF result (b) UF result with κ = −16
(c) UF result with κ = −10 (d) UF result with κ = 2
Figure 5.8: Estimated chief gyro bias with Initial Error = 4 deg and 4%
Chapter 5. Simulation and Results 82
(a) EKF result (b) UF result with κ = −16
(c) UF result with κ = −10 (d) UF result with κ = 2
Figure 5.9: Estimated deputy gyro bias with Initial Error = 4 deg and 4%
Chapter 5. Simulation and Results 83
(a) EKF result (b) UF result with κ = −16
(c) UF result with κ = −10 (d) UF result with κ = 2
Figure 5.10: Position Errors and 3σ Bounds with Initial Error = 4 deg and 4%
Chapter 5. Simulation and Results 84
(a) EKF result (b) UF result with κ = −16
(c) UF result with κ = −10 (d) UF result with κ = 2
Figure 5.11: Velocities Errors and 3σ Bounds with Initial Error = 4 deg and 4%
Chapter 5. Simulation and Results 85
(a) EKF result (b) UF result with κ = −16
(c) UF result with κ = −10 (d) UF result with κ = 2
Figure 5.12: Chief Orbit Elements Errors and 3σ Bounds with Initial Error = 4 deg and4%
Chapter 5. Simulation and Results 86
5.2.3 14 deg relative attitude error and 14% relative position
elements error in initial condition
(a) UF result with κ = −10 (b) UF result with κ = −5
(c) UF result with κ = 3
Figure 5.13: Attitude Errors and 3σ Bounds with Initial Error = 14 deg and 14%
Chapter 5. Simulation and Results 87
(a) UF result with κ = −10 (b) UF result with κ = −5
(c) UF result with κ = 3
Figure 5.14: Estimated Chief gyro bias with Initial Error = 14 deg and 14%
Chapter 5. Simulation and Results 88
(a) UF result with κ = −10 (b) UF result with κ = −5
(c) UF result with κ = 3
Figure 5.15: Estimated Deputy gyro bias with Initial Error = 14 deg and 14%
Chapter 5. Simulation and Results 89
(a) UF result with κ = −10 (b) UF result with κ = −5
(c) UF result with κ = 3
Figure 5.16: Position Errors and 3σ Bounds with Initial Error = 14 deg and 14%
Chapter 5. Simulation and Results 90
(a) UF result with κ = −10 (b) UF result with κ = −5
(c) UF result with κ = 3
Figure 5.17: Velocities Errors and 3σ Bounds with Initial Error = 14 deg and 14%
Chapter 5. Simulation and Results 91
(a) UF result with κ = −10 (b) UF result with κ = −5
(c) UF result with κ = 3
Figure 5.18: Chief Orbit Elements Errors and 3σ Bounds with Initial Error = 14 degand 14%
Chapter 5. Simulation and Results 92
5.3 Results
Figure 5.1 to 5.6 show the results of both the EKF and UF estimation for relative
attitude and position between two spacecraft. The initial conditions are assumed to be
well-known. The attitude errors plot, figure 5.1 shows that both EKF and UF results
are identical. Figures 5.2 and 5.3 show the chief and deputy gyro bias results. Both
the EKF and UF able to converge in under 30minutes. The 3σ bounds position errors
of UF in figure 5.4, and the 3σ bounds velocities error of UF in figure 5.5, are slightly
different than the EKF. The UF has a slightly large mean covariance than the EKF’s
mean covariance. Figure 5.6 shows the 3σ bounds of chief orbit elements for both the
EKF and UF. It shows that the UF has larger 3σ boundary of chief radius and chief
radius rate, and a slower decay rate than the EKF. This is due to the initial covariance
for chief radius is very large compared to other elements in the covariance matrix, while
the state estimation in the UF is heavily dependent on the covariance. Therefore, this is
the reason why the UF results in figures 5.5 and 5.6 are slightly different than the EKF
results.
Figures 5.7 to 5.12 show the results of both EKF and UF estimation with the
initial estimated values are assumed to have 4 deg error in relative attitude and 4% error
in the relative position elements. Figures 5.7, 5.8 and 5.9 clearly show that the EKF
took about 2 hours before the relative attitude, chief and gyro bias converge. The UF
only needs about 30 minutes to obtain converged result. However, from figures 5.10,
5.11 and 5.12, the EKF is unable to produce any converged results for the relative
position elements estimation, but the UF is able to converge within the first 2 hours.
The characteristic of high nonlinearity for the relative orbit equations of motion with a
small error in the initial estimation will lead to a large error during state transition and
propagation, especially if the error δx > 1 where the higher order terms in the Taylor’s
series expansion will dominate the first order. But the UF allows higher initial errors as
Chapter 5. Simulation and Results 93
its nature, therefore there are no effects on the UF.
Figures 5.13 to 5.18 show the UF estimation results with the initial estimated
values assumed to have 14 deg error in relative attitude and 14% error in relative position
elements. For the EKF estimation, the EKF either will only be able to produce a
converged result in chief and deputy gyro bias or will be unable to produce any results
at all with an initial error past the 4 deg and 4% point. Therefore at this point, there
are no results that are available for the EKF estimation. Figures 5.13, 5.14 and 5.15
show that the UF results converge within 1 hour to 2 hours. Figures 5.16, 5.17 and
5.18 show the different effects of the κ value on the UF results. All of the κ results,
κ = −10,−5 and 3 converged under 100 minutes. However, at this point, it clearly
shows that different values of κ will affect the convergence quality. The κ = −5 values
shows that it has the lowest mean errors in relative position elements, which we can say
is the optimal parameter among those three values. But other values of κ are applicable
as long as they produce a converging result. The chief orbit radius and its radius rate
took a longer time to converge due to that they have much larger value of error even
though it is only 14% error of their truth states. The errors produced by the chief orbit
radius is one of the main factors that causes the UF results inability to converge as we
increase the initial error to 15 deg and 15% or more.
Chapter 6
Discussion, Conclusion and Future
Works
6.1 Discussion
In the previous chapter, the simulation results showed that the EKF estimation is able to
work up until the error was not more than 4 deg and 4%. This result has been expected
since the fundamental concept of the EKF is required for the initial estimated value to
be close to the truth. For the UF estimator, it shows that the UF is able to handle much
higher initial error than the EKF, up to 14 deg and 14%. However, there after are few
factors that cause the UF’s inability to produce any results beyond that point.
One of the factors is that the relative attitude quaternion propagation consists of
three components, which are current quaternion state, chief and deputy gyro rate. Com-
pared to standard quaternion propagation that only consist of current quaternion state
and a gyro rate, the relative attitude propagation is more highly nonlinear than the stan-
dard attitude propagation. Therefore, the errors combined by these three components
increase dramatically compared to the standard attitude estimation.
94
Chapter 6. Discussion, Conclusion and Future Works 95
Another main factor that causes the problem is that as the error increases in the
initial states in the relative position elements, the higher-order terms in the Taylor’s
series expansion begin to dominate the first-order term due to the error, δx > 1. This
domination becomes very significant even for only a small amount of error that is being
increased. As we can see, the UF estimator will also be affected by the higher-order term
domination problem, where it is only able to filter the error up to 14 deg and 14%.
In the simulation, the unscented parameters, α has been set to 1 and β has been
set to zero for all of the time. The UF estimation is heavily dependent on how the sigma
point is being computed, which means that the (n+λ) term is also very important. Since
λ = α2(n+κ)−n, and λ = 3 or λ = 3−n is a good value for the UF estimation for most
of the cases. Therefore, if α is small, κ will need to be tuned to a very high value. Also, it
is needed to prevent for the case that is either λ = n or λ close to n, since this will cause
a singularity or nonpositive-semidefinite matrix during the sigma point transformation.
Therefore, good values for κ are usually those that let λ fall between −16 to 3 in this
simulation. However it needs to be cautioned that the response of λ also depends on
the output measurement. During the entire simulation run, especially when the initial
errors are high, not all of λ within this range are guaranteed to give convergence results
for all of the time. The β term has been “ignored” for the entire simulation due to the
fact that changing of β does not really help the results. Increasing β does not guarantee
the accuracy of the estimation, i.e. β = 1 might give a better convergence but β = 2
might not able to produce any results. Therefore, more pre-estimate computation with
respect to the change of β will need to be done if the effect of β on the estimation needs
to be observed.
Chapter 6. Discussion, Conclusion and Future Works 96
6.2 Conclusion
The UF approach for the relative attitude and position estimation has been proposed.
By comparing the results between the EKF and the UF estimation, the UF shows that it
has met the expectation. The simulation showed that the errors in the relative position
elements, especially the chief orbit radius error, is the critical part for the results. The
unscented parameters α, β and κ show that not all of the combination will guaranteed
converged results if the initial estimated states are very bad. Therefore, the UF needs a
lot of pre-computation before applied on any application. But this situation can be solved
since that the computing technology nowadays is more focused on parallel processing.
6.3 Future Works
This thesis shows that the possibility of implementing the UF for relative attitude and
position estimation. But the UF is not as perfect as expected. Research can be done to
expand or improve this study:
• Study more depth the correlation between the unscented parameters α, β with the
weights and covariance, and the effects on the estimation results.
• A new approach to deal with the high order Taylor’s series expansion’s domination.
• Using the square root UF implementation or Unscented Particle filter for the es-
timation. Most of the time the combination of the unscented parameters failed to
converge due to the covariance matrix becoming non-positive semidefinite matrix.
• Since most of the formation flying will not only involve two spacecraft, it is neces-
sary to use EKF and UF to deal with the relative attitude and position estimation
within multiple clusters.
Chapter 6. Discussion, Conclusion and Future Works 97
• Study the possibility of relative attitude and position estimation between two
spacecraft that have different type of build than each other. In this case, the
relative orbit equations of motion will become very hard to predict.
Bibliography
[1] Kim, S.-G., Crassidis, J. L., Cheng, Y., Fosbury, A. M., and Junkins, J. L., “Kalman
Filtering for Relative Spacecraft Attitude and Position Estimation,” AIAA Guid-
ance, Navigation, and Control Conference, Aug. 2005, AIAA Paper #2005-6087.
[2] Crassidis, J. L. and Junkins, J. L., Optimal Estimation of Dynamic Systems , Chap-
man & Hall/CRC, Boca Raton, FL, 2004.
[3] Schaub, H. and Junkins, J. L., Analytical Mechanics of Aerospace Systems , Ameri-
can Institute of Aeronautics and Astronautics, Inc., New York, NY, 2003.
[4] Robertson, A., Corazzini, T., LeMaster, F., and How, J. P., “Formation Sensing
and Control Technologies for a Separated Sapcecraft Interferometer,” Proceedings
of the 1998 American Control Conference, June 1998, pp. 1574–1580.
[5] Carpenter, J., Leitner, J., Folta, D., and Burns, R., “Benchmark Problems for
Spacecraft Formation Flying Missions,” AIAA Guidance, Navigation, and Control
Conference, Aug. 2003, AIAA 2003-5364.
[6] Purcell, G., Kuang, D., Lichten, S., Wu, S. C., , and Young, L., “Autonomous For-
mation Flyer (AFF) Sensor Technology Development,” AAS Guidance and Control
Conference, Feb. 1998, AAS 98-062.
[7] Corazzini, T., Robertson, A., Adams, J. C., Hassibi, A., and How, J. P., “GPS
Sensing for Spacecraft Formation Flying,” ION-GPS-97 Conference, Sept. 1997.
98
Bibliography 99
[8] Julier, S. J. and Uhlmann, J. K., “A New Extension of the Kalman Filter to Non-
linear Systems,” Int. Symp. Aerospace/Defense Sensing, Simul. and Controls, Or-
lando, FL, 1997.
[9] Julier, S. J., Uhlmann, J. K., and Durrant-Whyte, H. F., “A New Approach for Fil-
tering Nonlinear systems,” Proceedings of the American Control Conference, Vol. 3,
June 1995, pp. 1628–1632.
[10] Julier, S. J., Uhlmann, J. K., and Durrant-Whyte, H. F., “A New Method for the
Nonlinear Transformation of Means and Covariances In Filters and Estimators,”
IEEE Transactions on Automatic Control , Vol. AC-45, No. 3, Mar. 2000, pp. 477–
482.
[11] Pisacane, V. L. and Moore, R. C., editors, Fundamentals of Space Systems , Oxford
University Press, 1st ed., 1994.
[12] Hamilton, W. R., Elements of Quaternions , Longmans, Green and Co., London,
England, 1866.
[13] Schaub, H. and Junkins, J. L., “Stereographic Orientation Parameters for Attitude
Dynamics: A Generalization of the Rodrigues Parameters,” Journal of the Astro-
nautical Science, Vol. 44, No. 1, Jan.-March 1996, pp. 1–20.
[14] Marandi, S. R. and Modi, V. J., “A Preferred Coordinate System and Associ-
ated Orientation Representation in Attitude Dynamics,” Acta Astronautica, Vol. 15,
No. 11, Nov. 1987, pp. 833–843.
[15] Crassidis, J. L. and Markley, F. L., “Attitude Estimation Using Modified Rodrigues
Parameters,” Proceedings of the Flight Mechanics/Estimation Theory Symposium,
May 1996, pp. 71–83.
Bibliography 100
[16] Shuster, M. D., “A Survey of Attitude Representations,” Journal of the Astronau-
tical Sciences , Vol. 41, No. 4, Oct.-Dec. 1993, pp. 439–517.
[17] Junkins, J. L., Hughes, D. C., Wazni, K. P., and Pariyapong, V., “Vision-Based
Navigation for Rendezvous, Docking and Proximity Operation,” 22nd Annual AAS
Guidance and Control Conference, Feb. 1999, AAS 99-021.
[18] Alonso, R., Crassidis, J. L., and Junkins, J. L., “Vision-Based Relative Naviga-
tion for Formation Flying of Spacecraft,” AIAA Guidance, Navigation, and Control
Conference, Aug. 2000, AIAA Paper #00-4439.
[19] Crassidis, J. L. and Markley, F. L., “Unscented Filtering for Spacecraft Attitude
Estimation,” AIAA Journal of Guidance, Control, and Dynamics , Vol. 26, No. 4,
July-Aug. 2003, pp. 536–542.
[20] Ma, G.-F. and Jiang, X.-Y., “Unscented Kalman Filter for Spacecraft Attitude Es-
timation and Calibration Using Magnetometer Measurements,” Machine Learning
and Cybernetics, 2005. ws of 2005 International Conference on, Vol. 1, 18-21 Aug.
2005, pp. 506–511.
[21] Ju, G., Pollack, T., and Junkins, J. L., “DIGISTAR II Micro-Star Tracker: Au-
tonomous On-Orbit Calibration and Attitude Estimation,” AAS/AIAA Astrody-
namics Specialist Conference, Aug. 1999, AAS 99-431.
[22] Light, D. L., “Satellite Photogrammetry,” Manual of Photogrammetry , edited by
C. Slama, chap. 17, American Society of Photogrammetry, Falls Church, VA, 4th
ed., 1980.
[23] Shuster, M. D. and Oh, S. D., “Three-Axis Attitude Determination from Vector
observations,” Journal of Guidance and Control , Vol. 4, No. 1, Jan.-Feb. 1981,
pp. 70–77.
Bibliography 101
[24] Cheng, Y., Crassidis, J. L., and Markley, F. L., “Attitude Estimation for Large
Field-of-View Sensors,” AAS Malcolm D. Shuster Astronautics Symposium, June
2005, AAS-05-462.
[25] Shuster, G., “Kalman Filtering of Spacecraft Attitude and the QUEST Model,”
The Journal of the Astronautical Sciences , Vol. 38, No. 3, July-September 1990,
pp. 377–393.
[26] Lefferts, E. J., Markley, F. L., and Shuster, M. D., “Kalman Filtering for Spacecraft
Attitude Estimation,” Journal of Guidance, Control and Dynamics , Vol. 5, No. 5,
Sept.-Oct. 1982, pp. 417–429.
[27] Xing, G. Q. and Parvez, S. A., “Nonlinear Attitude State Tracking Control for
Spacecraft,” Journal of Guidance, Control, and Dynamics , Vol. 24, No. 3, May-
June 2001, pp. 624–626.
[28] Markley, F. L., “Matrix and Vector Algebra,” Spacecraft Attitude Determination and
Control , edited by J. R. Wertz, chap. Appendix C, Kluwer Academic Publishers,
The Netherlands, 1978, p. 755.
[29] van Loan, C. F., “Computing Integrals Involving the Matrix Exponential,” IEEE
Transactions on Automatic Control , Vol. AC-23, No. 3, June 1978, pp. 396–404.
[30] Hill, G., “Researches in the Lunar Theory,” American Journal of Mathematics ,
Vol. 1, 1878, pp. 5–26.
[31] Clohessy, W. H. and Wiltshire, R. S., “Terminal Guidance System for Satellite
Rendezvous,” journal of the Aerospace Sciences , Vol. 27, No. 4, 1960, pp. 604–610.
[32] Kalman, R. E., “A New Approach to Linear Filtering and Prediction Problems,”
Transactions of the ASME–Journal of Basic Engineering , Vol. 82, No. Series D,
1960, pp. 35–45.
Bibliography 102
[33] Welch, G. and Bishop, G., “An Introduction to the Kalman Filter,” TR 95-041.
[34] Wan, E. A. and van der Merwe, R., “Square-Root Unscented Kalman Fiilter for
State and Parameter-Estimation,” Proceedings of the IEEE International Confer-
ence on Acoustics Speech, and Signal Processing , May 2001, pp. 3461–3464.
[35] Wan, E. and van der Merwe, R., “The Unscented Kalman Filter,” Kalman Filtering
and Neural Networks , edited by S. Haykin, chap. 7, John Wiley & Sons, New York,
NY, 2001.
[36] Maciejowski, J. M., Multivariable Feedback Design, Addison-Wesley Publishing
Company, Wokingham, England, 1989.
[37] Markley, F. L., “Attitude Representations for Kalman Filtering,” AAS/AIAA As-
trodynamics Specialist Conference, Aug. 2001, AAS 01-309.