JLamparielloFinalProjectASEN5070

ASEN5070: Statistical Orbit Determination I
Final Project
15 December 2014
Jim Lampariello
“Do not say what is impossible, for the dreams of yesterday are the hopes of today
and the reality of tomorrow”
-Robert Goddard

Page | 1
Table of Contents
1. Introduction............................................................................................................................................ 3
1.1 Problem Overview ............................................................................................................................ 3
1.2 Constants and Initial Parameters ...................................................................................................... 3
2. General Approach ................................................................................................................................... 4
2.1 Governing Equations......................................................................................................................... 4
2.2 Linearization...................................................................................................................................... 4
2.3 Observations and Coordinate Systems ............................................................................................. 5
3. Filter Implementations............................................................................................................................ 5
3.1 Batch Processor ................................................................................................................................ 6
3.2 Sequential Algorithm ........................................................................................................................ 6
3.2.1 Conventional Kalman Filter ........................................................................................................ 6
3.2.2 Joseph Implementation.............................................................................................................. 6
4. Results..................................................................................................................................................... 7
4.1 Pre-fit and Post-fit Residuals............................................................................................................. 7
4.2 Covariance Matrix........................................................................................................................... 11
4.3 State Estimate................................................................................................................................. 12
4.4 Numerical Considerations............................................................................................................... 13
5. Observation Discussion......................................................................................................................... 14
6. Estimating All Three Station Locations.................................................................................................. 15
7. Additional Studies: Varying the A Priori Covariance Data ..................................................................... 17
Appendix: Code Used in Report................................................................................................................ 20

Page | 2
List of Figures
Figure 1. Pre-fit range residuals for the batch processor............................................................................ 7
Figure 2. Pre-fit range residuals for the sequential filters .......................................................................... 8
Figure 3. Pre-fit range rate residuals for the batch processor .................................................................... 8
Figure 4. Post-fit range rate residuals for the sequential filters ................................................................. 9
Figure 5. Post-fit range residuals .............................................................................................................. 10
Figure 6. Post-fit range rate residuals....................................................................................................... 10
Figure 7. Trace of the covariance for position .......................................................................................... 11
Figure 8. Trace of the covariance for velocity........................................................................................... 11
Figure 9. One sigma covariance ellipsoid for the batch processor (after three iterations)....................... 12
Figure 10. One sigma covariance ellipsoids for the sequential filters....................................................... 12
Figure 11. Negative values on the diagonal of the position covariance matrix......................................... 14
Figure 12. One sigma position covariance ellipsoid for the range only case............................................. 15
Figure 13. One sigma position covariance ellipsoid for the range rate only case ..................................... 15
Figure 14. One sigma covariance ellipsoid for the batch processor without fixed stations ...................... 16
Figure 15. One sigma covariance ellipsoids for the conventional Kalman filter without fixed stations.... 16
Figure 16. Position covariance ellipsoid for the batch processor when station 2 is fixed......................... 17
Figure 17. Position covariance ellipsoid for the batch processor when station 3 is fixed......................... 17
Figure 18. Trace of the position covariance, with a priori divided by 1000 .............................................. 18
Figure 19. Trace of the position covariance, with a priori divided by 100 000 ......................................... 18
Figure 20. Trace of the position covariance, with a priori divided by 1 000 000....................................... 19
List of Tables
Table 1. Constants Used in the Study ......................................................................................................... 3
Table 2. Initial State Guess ......................................................................................................................... 4
Table 3. RMS of the Pre-Fit Residuals......................................................................................................... 9
Table 4. RMS of the Post-Fit Residuals ..................................................................................................... 10
Table 5. Initial state estimate for each filter implementation .................................................................. 13

Page | 3
1. Introduction
1.1 Problem Overview
This study aims to determine the orbit of a satellite based on a series of ground based observations of
range and range rate from three tracking stations. In addition to understanding the motion of the
satellite, it is also desired to know statistical information about the solution that was reached. Several
assumptions are made in the analysis:
1. The satellite is under two body motion, perturbed only by the effects of drag and the J2
effects of an oblate gravity field.
2. Atmospheric density is assumed to follow an exponential model
3. The ration of the satellite’s mass to its cross sectional area is known
4. Measurements of range and range rate are gathered from one of three tracking stations
5. The position of one tracking station is known well, while the other two have uncertainties in
their positions.
6. Earth rotation is purely about its polar axis, which is fixed in inertial space (i.e. polar
“wobbles” are neglected)
Eighteen parameters will be estimated. These include: three components of satellite position, three
components of satellite velocity, the gravitational parameter of the earth, the J2 coefficient, the satellite
drag coefficient, and three position components for each tracking station.
1.2 Constants and Initial Parameters
Several constants, including Earth parameters, drag model parameters, and measurement uncertainty,
need to be defined in order to solve the problem. These parameters are listed in Table 1 along with
their respective values.
Table 1. Constants Used in the Study
Constant Value
Rotation Rate of the Earth 7.2921158553x10-5
rad/s
Time Epoch 3 Oct 1999 23:11:9.1814 UTC
Radius of the Earth 6378136.3 m
Reference Density 3.614x10-13
kg/m3
Reference Density Height 88667 m
Reference Density Radius 70000 m + Radius of the Earth
Satellite Mass 970 kg
Satellite Cross Sectional Area 3.0 m2
Range Data Standard Deviation 0.01 m
Range Rate Data Standard Deviation 0.001 m/s
As discussed below in Section 3, an initial guess for the eighteen desired quantities is necessary to start
the estimation process. This initial guess at the start epoch must be close enough to the true value such
that the solution converges. Values for the estimate at the starting epoch are given in Table 2 (positions
and velocities are given in the Earth-Centered-Inertial coordinate frame).

Page | 4
Table 2. Guess of Initial State at Start Epoch
State Element Value
Satellite Position [X,Y,Z] [757700.0 5222607.0 4851500.0] m
Satellite Velocity [X,Y,Z] [2213.21 4678.34 -5371.3] m/s
Gravitational Parameter 3.986004415x1014
km3
/m2
J2 Term 0.001082626925638815
Drag Coefficient 2.0
Tracking Station 1 Position [X,Y,Z] [-5127510.0 -3794160.0 0.0] m
Tracking Station 2 Position [X,Y,Z] [3860910.0 3238490.0 3898094.0] m
Tracking Station 3 Position [X,Y,Z] [549505.0 -1380872 6182197.0] m
2. General Approach
2.1 Governing Equations
One of the key assumptions of the study is that the satellite operates under a perturbed gravity model
including drag and oblateness effects. The potential function of motion in an oblate gravity field is
written in Equation 1. In the following equation, 𝜇 is the gravitational parameter, 𝑟 is the magnitude of
the satellite’s position vector, 𝑅 𝐸 is the radius of the Earth, and 𝜑 is the satellite’s latitude. The
gravitational force can be determined by taking the gradient of this potential function.
𝑈 =
𝜇
𝑟
[1 − 𝐽2 (
𝑅 𝐸
𝑟
)
2
(
3
2
𝑠𝑖𝑛2
𝜑 −
1
2
)]
(1)
Since acceleration due to drag is independent of the acceleration due to the gravity field, the drag force
model can be added component-wise to form the overall force model. The acceleration due to drag is
given in vector form Equation 2, where 𝑐 𝑑 is the drag coefficient, 𝜌 is the atmospheric density, and
𝐴
𝑚
is
the area to mass ratio.
𝑟̈ 𝑑𝑟𝑎𝑔 =
1
2
𝑐 𝑑 𝜌
𝐴
𝑚
[
𝑥̇ + 𝜃̇ 𝑦
𝑦̇ − 𝜃̇ 𝑧
𝑧̇
]
𝑇
[
𝑥̇ + 𝜃̇ 𝑦
𝑦̇ − 𝜃̇ 𝑧
𝑧̇
]
(2)
2.2 Linearization
The equations of motion presented in Section 2.1 are linearized about a reference trajectory, 𝑋∗
(𝑡). The
difference between the “true” trajectory, 𝑋(𝑡), and the reference trajectory is known as the state
deviation vector, as shown in Equation 3. The same relationship holds for the derivatives of the state.
𝑥(𝑡) = 𝑋(𝑡) − 𝑋∗
(𝑡) (3)
A similar procedure can be completed with the measurements 𝑌(𝑡) and their calculated equivalents,
𝑌∗
(𝑡). Equation 4 defines the observation deviation vector
𝑦(𝑡) = 𝑌(𝑡) − 𝑌∗
(𝑡) (4)

Page | 5
Combining the two concepts, a mapping matrix can be formed. The mapping matrix, 𝐻̃, is composed of
the partial derivatives of the measurements with respect to each state variable. This matrix can be used
as shown in Equation 5. The remaining term, 𝜀, is the error in the observations (the post-fit residuals).
𝑦 = 𝐻̃ 𝑥 + 𝜀 (5)
The final piece of the linearization puzzle comes in the form of the state transition matrix, denoted as 𝜙.
This matrix maps the state deviation vector from one time to another, as demonstrated in Equation 6.
Like the previous equations, the state transition matrix is the first term of a Taylor expansion of the state
vector about a reference time. The differential equation for the state transition matrix is given as
Equation 7, where A is a matrix of the partial derivatives of the derivative of the state with respect to
each state element.
𝑥(𝑡 𝑘) = 𝜙(𝑡 𝑘, 𝑡 𝑘−1)𝑥(𝑡 𝑘−1) (6)
𝜙̇ = 𝐴𝜙, 𝜙(𝑡 𝑘, 𝑡 𝑘) = 𝐼 (7)
In certain filter applications, the mapping matrix needs to be updated as well. The new matrix, 𝐻, is
formed by multiplying the mapping matrix by the state transition matrix.
2.3 Observations and Coordinate Systems
This study is performed in an Earth-centered inertial (ECI) frame, however station coordinates are given
in an Earth-centered, Earth-fixed (ECEF) frame. At the initial epoch, these two frames are coincident,
but they will eventually differ as the Earth rotates. Because “wobbles” in the Earth’s axis are neglected,
the evolution of tracking station coordinates is a simple rotation about the +Z ECI axis at the rate given
in Section 1.2. Station coordinates are transformed to the ECI frame before applying the range equation
shown below.
𝜌 = √(𝑥 − 𝑥 𝑠𝑖)2 + (𝑦 − 𝑦𝑠𝑖)2 + (𝑧 − 𝑧 𝑠𝑖)2 (8)
In Equation 8, 𝜌 is the range measurement, 𝑥 𝑦 𝑧 are satellite coordinates, and 𝑥 𝑠𝑖 𝑦𝑠𝑖 𝑧 𝑠𝑖 are the
position coordinates of the station which took the measurement. The equation for range rate is simply
the time derivative of the range measurement. The mapping matrix is formed by taking the partial
derivative of range and range rate with respect to each state variable.
3. Filter Implementations
The job of a filter is to determine the best estimate of the state of the system. In many cases, the “best”
solution is defined to be the one that solves the least-squares problem. That is, it minimizes the sum of
the squares of the errors from Equation 5.
It is useful to add a weighting matrix 𝑊, which allows one observation to be considered more
“meaningful” than another. The weighting matrix and the a priori (initial) state estimate 𝑥̅ can be
combined with Equation 5 to create the cost function shown in Equation 9.
𝐽(𝑥) =
1
2
(𝒚 − 𝐻𝒙) 𝑇
𝑊(𝒚 − 𝐻𝒙) +
1
2
(𝒙̅ − 𝒙) 𝑇
𝑊̅ (𝒙̅ − 𝒙)
(9)

Page | 6
Two methods of minimizing 𝐽(𝑥) are considered in this study: the batch processor and the sequential
algorithm. While mathematically equivalent, each one has a different numerical implementation.
3.1 Batch Processor
The minimum of Equation 9 can be found by taking the derivative of 𝐽(𝑥) with respect to 𝒙 and setting
the result equal to zero. (The Second Derivative Test can be used to verify that a minimum is found
rather than a maximum.) Equation 10 shows the final state deviation estimate after performing this
process.
𝒙̂ = (𝐻 𝑇
𝑊𝐻 + 𝑊̅ )−1(𝐻 𝑇
𝑊𝒚 + 𝑊̅ 𝒙̅) (10)
In the batch processor, each measurement is filtered at the same time. The mapping matrix, H, is
created by combining the mapping matrices from each time point before applying Equation 10. The
batch processor is useful for looking at large amounts of past data, as it can efficiently work through a
set of measurements to yield the best estimated initial state. The process is usually iterated to help
remove errors from the linearization process.
3.2 Sequential Algorithm
Rather than filter each measurement at the same time, the sequential algorithm processes each
measurement as it becomes available. Thus, the sequential algorithm has many applications where real-
time data evaluation is necessary.
3.2.1 Conventional Kalman Filter
Perhaps the most well-known of the data processing methods, the conventional Kalman filter (CKF) is
relatively straightforward. The key to the filter is using the previous best estimate as the current a priori
estimate. Therefore, it consists of two main steps: the time update and the measurement update.
First the state deviation vector and the covariance matrix, 𝑃, are mapped to the current epoch using the
state transition matrix (the step known as the time update). This yields the a priori information needed
for the calculation of the new state deviation vector and covariance matrix (the step known as the
measurement update). The necessary equations for the conventional Kalman filter are laid out in
Equation 11. The subscript k denotes calculations performed at the current timestep, and the subscript
k-1 denotes calculations from the previous timestep.
𝒙̅ = 𝜙(𝑡 𝑘, 𝑡 𝑘−1)𝒙̂ 𝑘−1
𝑃̅ 𝑘 = 𝜙(𝑡 𝑘, 𝑡 𝑘−1)𝑃𝑘−1 𝜙(𝑡 𝑘, 𝑡 𝑘−1) 𝑇
𝒚 = 𝒀 𝒌 − 𝒀 𝒄𝒂𝒍𝒄 (11)
𝐻̃ 𝑘 =
𝑑𝐺
𝑑𝑋∗
𝐾𝑘 = 𝑃̅ 𝑘 𝐻̃ 𝑘
𝑇
[𝐻̃ 𝑘 𝑃̅ 𝑘 𝐻̃ 𝑘
𝑇
]
−1
𝑃𝑘 = [𝐼 − 𝐾𝑘 𝐻̃ 𝑘]𝑃̅𝑘
3.2.2 Joseph Implementation
The conventional Kalman filter can be affected by numerical issues, especially when the covariance
matrix is updated. Namely, the covariance matrix can become non-symmetric and non-positive-definite.
To help avoid this problem, it can sometimes be beneficial to use the Joseph implementation of the

Page | 7
covariance calculation (given as Equation 12). The Joseph implementation will always yield a symmetric
covariance matrix, though it will not always be positive definite.
𝑃𝑘 = (𝐼 − 𝐾𝑘 𝐻̃ 𝑘)𝑃̅𝑘(𝐼 − 𝐾𝑘 𝐻̃ 𝑘)
𝑇
+ 𝐾𝑘 𝑅 𝑘 𝐾𝑘
𝑇 (12)
4. Results
When examining the performance of a particular filter, it is instructive to look at the residuals, the
covariance matrix, and the state estimate itself. Each of these results will be examined in turn.
4.1 Pre-fit and Post-fit Residuals
The pre-fit residuals, defined as the observation minus the calculated observation using the nominal
trajectory, are an indication of how well the filter is matching the incoming data. Ideally, the difference
would be both small and random. Figure 1 shows the pre-fit residuals for the range measurement of the
batch processor and Figure 2 shows the pre-fit residuals for the range measurement the sequential
filters. Figure 3 and Figure 4 do the same for the range rate.
Figure 1. Pre-fit range residuals for the batch processor

Page | 8
Figure 2. Pre-fit range residuals for the sequential filters
Figure 3. Pre-fit range rate residuals for the batch processor

Page | 9
Figure 4. Post-fit range rate residuals for the sequential filters
Because the implementations are mathematically identical between the batch processor and the
sequential filters, the single iteration pre-fit residuals match very closely. Additionally, as the batch
processor is iterated, the residuals become smaller and smaller. This indicates that the errors from
linearization are removed during the iteration process. A similar result would be found had the
sequential filters been iterated. Table 3 contains the root-mean-square (RMS) values of the pre-fit
residuals for comparison purposes.
Table 3. RMS of the Pre-Fit Residuals
Batch Processor
(After 1 Iteration)
Conventional Kalman Kalman w/Joseph
Range RMS (m) 732.74826 732.74830 732.74830
Range Rate RMS (m/s) 2.90017 2.90017 2.90017
The post-fit residual error, defined in Equation 5, is another indication of the filter’s performance. Once
again, it should be small and without defined structure (that is, random). Figure 5 shows the post-fit
residuals for the range measurement, and Figure 6 shows the post-fit residuals for the range rate
measurement from the sequential filters.

Page | 10
Figure 5. Post-fit range residuals
Figure 6. Post-fit range rate residuals
As expected, the post-fit residuals are significantly smaller than the pre-fit residuals. The filter has
successfully converged on a solution, as evidenced by the small values of residuals that lack any pattern
or structure. The RMS values are presented in Table 4.
Table 4. RMS of the Post-Fit Residuals
Conventional Kalman Kalman w/Joseph
Range RMS (m) 0.112724 0.114994
Range Rate RMS (m/s) 0.001402 0.001447

Page | 11
4.2 Covariance Matrix
The covariance matrix is a statistical representation of the confidence of the solution from the filter. As
more measurements are processed, the covariance should drop – the filter should be more “certain” of
its results. Figure 7 shows the trace of the covariance for position for the sequential filters, and Figure 8
shows the trace of the covariance for velocity for the sequential filters.
Figure 7. Trace of the covariance for position
Figure 8. Trace of the covariance for velocity
As expected, the covariance decreases with time in each case. Both sequential implementations show
the same pattern, with very close results. Additionally, some negative values are present in the
covariance matrix – a topic covered in Section 4.4.

Page | 12
In examining the covariance matrix, it can be seen that there are off-diagonal terms. This indicates a
relationship between the variances in each component of position and velocity. To provide a better
visualization of the coupling, the covariance matrix can be transformed using eigenvalue/eigenvector
decomposition. The newly formed “principal axes” can be used to create covariance ellipsoids. Figure 9
shows the one-sigma covariance ellipsoid for the batch processor at the final time, and Figure 10 shows
the same thing for the sequential processors.
Figure 9. One sigma covariance ellipsoid for the batch processor (after three iterations)
Figure 10. One sigma covariance ellipsoids for the sequential filters
4.3 State Estimate
Because they are mathematically the same, it is expected that the state estimate for each filter
implementation should be similar. Table 5 demonstrates that this is in fact the case.

Page | 13
Table 5. Initial state estimate for each filter implementation
Batch Processor Conventional Kalman Kalman w/Joseph
Satellite X Position (m) 757700.000000259 757699.709791728 757700.067856212
Satellite Y Position (m) 5222606.99999972 5222607.03601309 5222606.55126764
Satellite Z Position (m) 4851499.99999998 4851499.52616626 4851499.986047
Satellite X Velocity
(m/s)
2213.20999999969 2213.25107382074 2213.2508239512
Satellite Y Velocity
(m/s)
4678.34000000018 4678.37243905948 4678.37294119831
Satellite Z Velocity
(m/s)
-5371.29999999997 -5371.31495864333 -5371.31464899568
Gravitational
Parameter (m3
/s2
)
398600441499990 398600429030503 398600433870391
J2 Term 0.00108262692564958 0.00108196002376434 0.00108197266733222
Drag Coefficient 1.99999978220765 2.15130610346163 2.14940733598982
Tracking Station 1 X
Position (m)
-5127510.00000023 -5127509.99999788 -5127509.99999822
Tracking Station 1 Y
Position (m)
-3794160.00000017 -3794159.99999843 -3794159.99999869
Tracking Station 1 Z
Position (m)
3.18736456095079e-08 -1.306780938589e-07
-2.62450204716101e-
07
Position (m)
3860910.00000013 3860899.37540614 3860899.46228799
Position (m)
3238489.99999979 3238500.21020438 3238499.86386659
Position (m)
3898094.00000005 3898099.74980478 3898099.82981571
Position (m)
549504.99999999 549499.055344415 549499.301854072
Position (m)
-1380872.00000001 -1380869.3238551 -1380869.83846841
Position (m)
6182197.00000043 6182198.5318693 6182198.47284805
In general, the state estimate is very close to the initial guess. All three state estimates agree to less
than 0.1% of each other, except for the drag coefficient. Tracking Station 1, which was fixed using a
small a priori covariance, has only a negligible change (indicating the fixing was correctly implemented).
4.4 Numerical Considerations
Because of the relative size of several values in the study (for example, the a priori covariance matrix)
numeric issues crop up. More specifically, matrix inverses become computationally unstable due to
round off error.
Both of the sequential filter implementations fall victim to this due to the necessary inverses from
Equation 11. The covariance matrices become non-positive-definite when some of the diagonal values
become negative. In the case of the conventional Kalman filter, the matrix also becomes non-

Page | 14
symmetric. Figure 11 shows a graphical representation where negative values are present in the
covariance matrix. The absolute value is taken to demonstrate a sense of magnitude.
Figure 11. Negative values on the diagonal of the position covariance matrix
The numeric issues with the sequential processors are well understood, and several solutions are
available. These typically fall under the category of “square root methods”, so named because they
operate on the square root of a particular matrix. The condition number of the matrix is therefore much
more attractive, and the methods are less prone to finite-precision error. The Potter algorithm is one
such well-known alternative.
Due to the different formulation for the covariance matrix, the batch filter is more numerically robust
than the sequential methods presented here. Large a priori covariances, for example, are effectively
ignored by the batch. Better conditioned matrix inverses also make the batch more computationally
friendly.
The covariance matrix can also cause issues when its values become very small small. In this case, the
filter becomes “saturated” as it trusts the calculated state more than another measurement. Saturation
can lead to incorrect estimations. Adding noise to the system, such as in state noise compensation, can
help avoid the issues of saturation. Noise artificially boosts the covariance matrix to ensure that
measurements are given the correct priority compared to the estimated state.
5. Observation Discussion
For the main study, both range and range rate were used to develop an estimated state. It would be
instructive, however, to examine how each measurement contributed to the solution. In order to do
this, the batch processor algorithm was repeated two additional times: once using only range data and
once using only range rate data.
Figure 12 and Figure 13 show the position covariance ellipsoids for each of the one-measurement cases.
Comparing back to Figure 9, it is clear that the range data provides the bulk of the solution power. The
covariance ellipsoid for the range-only case is nearly identical to the one when both measurements
were used. The range rate-only case shows an ellipsoid about two to three orders of magnitude larger.

Page | 15
Given the option of choosing only one measurement, selecting range data would provide the estimate
with the most confidence.
Figure 12. One sigma position covariance ellipsoid for the range only case
Figure 13. One sigma position covariance ellipsoid for the range rate only case
6. Estimating All Three Station Locations
As stated in the study assumptions, one ground station location was known very well while the other
two had more uncertainty. This was accomplished by making the a priori covariance for the fixed
station extremely small (essentially “fooling” the filter into not updating that estimate).
The fixed location is necessary to ensure that the problem is observable. Range and range rate are
inherently relative measurements that show how the satellite moves with respect to the ground

Page | 16
stations. The measurements tell nothing, though, about how the ground stations are oriented relative
to the earth.
To show the performance of the filters in an unobservable realm, the a priori covariance for the formerly
fixed station was increased to the same as the other stations. Each processor was then run again.
Figure 14 shows the final covariance ellipsoid for the batch processor, and Figure 15 shows the final
covariance ellipsoid for the sequential filters.
Figure 14. One sigma covariance ellipsoid for the batch processor without fixed stations
Figure 15. One sigma covariance ellipsoids for the conventional Kalman filter without fixed stations
Clearly, each method fails. The covariances are on the order of hundreds of meters, rather than
centimeters as seen earlier. The filters simply cannot get a good match on the data without more
information.

Page | 17
The question then arises: “Does it matter which station is fixed?” Running the conventional Kalman filter
with a different station fixed answers the question with a resounding “no”. As shown in Figure 16 and
Figure 17, the covariance ellipsoid is the same order of magnitude no matter what station is assumed to
be known.
Figure 16. Position covariance ellipsoid for the batch processor when station 2 is fixed
Figure 17. Position covariance ellipsoid for the batch processor when station 3 is fixed
7. Additional Studies: Varying the A Priori Covariance Data
The study was initially done with a very large a priori covariance matrix – essentially assuming that the
measurements are much more reliable than the initial guess. In reality, however, there may be
additional data that provides a more certain guess (for example, if information was received from a
launch vehicle prior to satellite separation). Therefore, it is useful to look at the state estimate with
smaller covariance matrices.

Page | 18
Three different covariance matrices were used for this portion of the study. The original one was divided
by 1000, 100 000, and 1 000 000. Figure 18, Figure 19, and Figure 20 show the trace of the covariance of
position for each case, respectively.
Figure 18. Trace of the position covariance, with a priori divided by 1000
Figure 19. Trace of the position covariance, with a priori divided by 100 000

Page | 19
Figure 20. Trace of the position covariance, with a priori divided by 1 000 000
The general trend, especially compared with the original Figure 7, is that the trace of the covariance
decreases more rapidly as the a priori covariance gets smaller. In each case, however, the end
conditions are approximately the same – a trace in the neighborhood of 10-4
. This is likely due to the
large number of measurements that are processed. The filter has so much information that it will
eventually settle on a confident solution. If there were fewer measurements, in the realm of about 50,
it would then be preferable to use a smaller a priori guess.

Page | 20
Appendix: Code Used in Report
main.m
% asen5070proj_main
% Main function call
clear;
clc;
close all;
format compact;
warning('off', 'MATLAB:Axes:NegativeDataInLogAxis')
warning('off', 'MATLAB:nearlySingularMatrix')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialization Section
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initial guesses. These will be updated
pos = [757700.0; 5222607.0; 4851500.0]; % Satellite Position (m)
vel = [2213.21; 4678.34;-5371.3;]; % Satellite Velocity (m/s)
mu = 3.986004415e14; % Gravitational param (m^3/s^2)
J2 = 0.001082626925638815; % J2 Constant (nd)
cd = 2; % Drag coefficient (nd)
posS1 = [-5127510.0; -3794160.0;0.0]; % Station 1 coordinates (m)
posS2 = [3860910.0; 3238490.0; 3898094.0];% Station 2 coordinates (m)
posS3 = [549505.0; -1380872;6182197.0]; % Station 3 coordinates (m)
theta = 0; % Angle defining ECEF (rad) on
% 3 Oct 1999, 23:11:09.1814 UTC
% Combine everything to build the state vector
STM0 = reshape(eye(18), 18*18,1);
state0 = [pos; vel; mu; J2; cd; posS1; posS2; posS3; theta; STM0];
% Call Batch Processor
[xHatBatch, rhoResidB, rhoDotResidB] = asen5070proj_batch(state0);
% Call Conventional Kalman Filter
[xHat, STMtot, dXBar, PHold, rhoResid, rhoDotResid, postfitResiduals] =
asen5070proj_convKalman(state0);
% Call Joseph Kalman Filter
[xHatJ, STMtotJ, dXBarJ, PHoldJ, rhoResidJ, rhoDotResidJ,postfitResidualsJ] =
asen5070proj_convKalmanJoseph(state0);
% figure
% plot(squeeze(postfitResiduals(2,1,:)), 'LineWidth', 2)
% hold on
% grid on
% plot(squeeze(postfitResidualsJ(2,1,:)), 'r', 'LineWidth', 2)
% legend('CKF', 'CKF w/Joseph', 'Location', 'best')
% xlabel('Number of Measurements', 'Fontsize', 14)
% ylabel('Post-fit Residual (m/s)', 'Fontsize', 14)
% set(gca, 'Fontsize', 14)
%
% figure
% semilogy(squeeze(PHold(1,1,:)), 'LineWidth', 2)
% hold on
% semilogy(squeeze(PHold(2,2,:)), 'r', 'LineWidth', 2)
% semilogy(squeeze(PHold(3,3,:)), 'k', 'LineWidth', 2)
% semilogy(squeeze(PHoldJ(1,1,:)), '--','LineWidth', 2)
% hold on
% semilogy(squeeze(PHoldJ(2,2,:)), '--r', 'LineWidth', 2)

Page | 21
% semilogy(squeeze(PHoldJ(3,3,:)), '--k', 'LineWidth', 2)
% ylabel('Trace of Covariance (m^2)', 'Fontsize', 14)
% legend('CKF X Pos','CKF Y Pos', 'CKF Z Pos', 'CKF w/Joseph X Pos','CKF w/Joseph Y
Pos','CKF w/Joseph Z Pos', 'Location', 'best')
% grid on
% figure
% plot(rhoDotResid, 'LineWidth', 2)
% hold on
% plot(rhoDotResidJ, 'r', 'LineWidth', 2)
% ylabel('Pre-fit Range Rate Residual (m/s)', 'Fontsize', 14)
% legend('CKF', 'CKF w/Joseph', 'Location', 'best')
%
% figure
% subplot(3,1,1)
% plot(rhoDotResidB(1,:), 'LineWidth', 2)
% ylabel('Iteration 1', 'Fontsize', 14)
% title('Pre-fit Range Rate Residuals (m/s)')
% grid on
% subplot(3,1,2)
% grid on
% subplot(3,1,3)
% grid on
% fprintf('nBatch Processor Initial Staten')
% state0(1:18)+xHatBatch
% fprintf('nConventional Kalman Filter Initial State:n')
% state0(1:18)+STMtotxHat
% fprintf('nJoseph Kalman Filter Initial State:n')
% state0(1:18)+STMtotJxHatJ
batch.m
function [Xhat0,rhoResid, rhoDotResid] = asen5070proj_batch(state0)
% Constants
radEarth = 6378136.3; % Radius of the earth (m)
omegaEarth = 7.2921158553e-5; % Rotation of earth (rad/s)
dens0 = 3.614e-13; % Reference density (kg/m^3)
areaSat = 3.0; % Satellite area (m^2)
ref0 = 700000+radEarth; % Reference radius for dens (m)
refH = 88667.0; % Reference height for dens (m)
mass = 970; % Satellite mass (kg)
% Integrator Options
myoptions = odeset('RelTol',1e-11,'AbsTol',1e-11);

Page | 22
tVect = 0:20:18340;
% Load in observation information and variance/covariance data
obs = load('asen5070proj_observations.dat');
[P0inv, W] = asen5070proj_p0();
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Batch Processor Section
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
iterBatch = 3; % Number of iterations to do
numObs = length(obs); % Number of observations
dXbar0 = zeros(18,1); % Inital deviation guess
for jj =1:iterBatch
[t,state] = asen5070proj_integrator(state0, tVect, radEarth, omegaEarth, areaSat,
mass, dens0, refH, ref0, myoptions);
lambda = P0inv;
N = zeros(18,1);
for ii = 1:numObs
stateInd = obs(ii,1)/20+1; % Maps observation time to integrator time
STM = reshape(state(stateInd,20:end), 18,18); % Get STM at this time point
[hTilde, yStar] = asen5070proj_hTildeMat(state(stateInd,:), omegaEarth,
obs(ii,2));
H = hTilde*STM; % Update to current time
y = [obs(ii,3); obs(ii,4)]-yStar; % Observation deviation vector
rhoResid(jj,ii) = y(1); % Store rho residual
rhoDotResid(jj,ii) = y(2); % Store rhoDot residual
lambda = lambda+H'*W*H; % Information Matrix HtWH
yHold(:,:,ii) = y; % Store it for postfit residual
comparisons
N = N+H'*W*y; % Normal Matrix HtWy
hTildeHold(:,:,ii) = hTilde ; % Store it for comparison to online
solutions
end
Xhat0 = (lambda+P0inv)(N+P0inv*dXbar0); % state deviation estimate
dXbar0 = dXbar0-Xhat0; % Update initial deviation
state0(1:18) = state0(1:18)+Xhat0; % Calculate estimated initial state
% Calculate post fit residuals
rangeRMS(jj) = sqrt(sum(rhoResid(jj,:).^2)/numObs);
rangeRateRMS(jj) = sqrt(sum(rhoDotResid(jj,:).^2)/numObs);
end
P = lambdaeye(size(lambda));
% [vector, value] = eig(P(1:3,1:3));
% semi(1) = sqrt(value(1,1));
% figure
% plotEllipsoid(vector, semi);
% xlabel('X (m)', 'Fontsize', 14)
% ylabel('Y (m)', 'Fontsize', 14)
% zlabel('Z (m)', 'Fontsize', 14)
% title('Position Covariance Ellipsoid', 'Fontsize', 14)
%
% load('asen5070_batchPHIcheck.mat')
% STM = reshape(state(2,20:end), 18,18); % Get STM at this time point
%
% relDiff = abs((STM(1:6,1:9)-Phi_t20(1:6,1:9))./Phi_t20(1:6,1:9));

Page | 23
% hist(reshape(log10(abs(relDiff)),6*9,1))
convKalman.m
function [xHat, STMtot, xBar, PHold, rhoResid, rhoDotResid, postfitResiduals] =
asen5070proj_convKalman(state0)
% Constants
P0 = eye(18,18);
% Recreate the P0 matrix since it is loaded in inverted
for ii = 1:18
P0(ii,ii) = 1./P0inv(ii,ii);
end
R = inv(W);
% Initial state transition matrix
STMtot = eye(18);
STM = STMtot;
% Number of observations
numObs = length(obs);
% Initial deviation guess
xBar = zeros(18,1);
xHat = zeros(18,1);
P = P0;
time0 = 0;
for ii = 1:numObs
tVect = [time0 obs(ii,1)];
time0 = obs(ii,1);
if tVect(2)>tVect(1)
[~,state] = asen5070proj_integrator(state0, tVect, radEarth, omegaEarth,
areaSat, mass, dens0, refH, ref0, myoptions);
STM = reshape(state(end,20:end), 18,18); % Get STM at this time point
[hTilde, yStar] = asen5070proj_hTildeMat(state(end,:), omegaEarth, obs(ii,2));
state0 = state(end,:);
state0(20:end) = reshape(eye(18,18), 18*18,1); % Reset STM
else
[hTilde, yStar] = asen5070proj_hTildeMat(state0, omegaEarth, obs(ii,2));
end
xBar = STM*xHat; % Update deviation vector

Page | 24
pBar = STM*P*STM'; % Update pBar
rhoResid(ii) = y(1); % Store rho residual
rhoDotResid(ii) = y(2); % Store rhoDot residual
K = pBar*hTilde'/(hTilde*pBar*hTilde' + R); % Kalman gain
xHat = xBar+K*(y-hTilde*xBar); % New state deviation
P = (eye(18) - K*hTilde)*pBar; % New covariance
STMtot = STM*STMtot; % Compute total STM.
tracePos(ii) = trace(P(1:3, 1:3));
traceVel(ii) = trace(P(4:6, 4:6));
Khold(:,:,ii) = K;
xBarHold(:,ii) = xBar;
PHold(:,:,ii) = P;
postfitResiduals(:,:, ii) = y-hTilde*xHat;
end
% [vector, value] = eig(P(1:3,1:3));
% figure
% subplot(1,2,1)
% plotEllipsoid(vector, semi);
% xlabel('X (m)','Fontsize', 14)
% ylabel('Y (m)','Fontsize', 14)
% zlabel('Z (m)','Fontsize', 14)
% title('CKF Position Covariance Ellipsoid', 'Fontsize', 14)
convKalmanJoseph.m
function [xHat, STMtot, xBar, PHold, rhoResid, rhoDotResid, postfitResiduals] =
asen5070proj_convKalmanJoseph(state0)
% Constants
P0 = eye(18,18);
% Recreate the P0 matrix since it is loaded in inverted
for ii = 1:18
P0(ii,ii) = 1./P0inv(ii,ii);
end
R = inv(W);
% Initial state transition matrix
STMtot = eye(18);
STM = STMtot;
% Number of observations

Page | 25
numObs = length(obs);
% Initial deviation guess
xBar = zeros(18,1);
xHat = zeros(18,1);
P = P0;
time0 = 0;
for ii = 1:numObs
tVect = [time0 obs(ii,1)];
time0 = obs(ii,1);
if tVect(2)>tVect(1)
[~,state] = asen5070proj_integrator(state0, tVect, radEarth, omegaEarth,
areaSat, mass, dens0, refH, ref0, myoptions);
STM = reshape(state(end,20:end), 18,18); % Get STM at this time point
[hTilde, yStar] = asen5070proj_hTildeMat(state(end,:), omegaEarth, obs(ii,2));
state0 = state(end,:);
state0(20:end) = reshape(eye(18,18), 18*18,1); % Reset STM
else
[hTilde, yStar] = asen5070proj_hTildeMat(state0, omegaEarth, obs(ii,2));
end
xBar = STM*xHat; % Update deviation vector
pBar = STM*P*STM'; % Update pBar
rhoResid(ii) = y(1); % Store rho residual
rhoDotResid(ii) = y(2); % Store rhoDot residual
K = pBar*hTilde'/(hTilde*pBar*hTilde' + R); % Kalman gain
xHat = xBar+K*(y-hTilde*xBar); % New state deviation
P = (eye(18) - K*hTilde)*pBar*(eye(18) - K*hTilde)'+K*R*K'; % New
covariance
STMtot = STM*STMtot; % Compute total STM.
tracePos(ii) = trace(P(1:3, 1:3));
traceVel(ii) = trace(P(4:6, 4:6));
Khold(:,:,ii) = K;
xBarHold(:,ii) = xBar;
PHold(:,:,ii) = P;
postfitResiduals(:,:, ii) = y-hTilde*xHat;
end
hTildeMat.m
function [hTilde, yStar] = asen5070proj_hTildeMat(state, omegaEarth, site)
% hTilde = asen5070proj_hTildeMat(state)
% Break up state for easier reading
x = state(1);
y = state(2);
z = state(3);
xDot = state(4);
yDot = state(5);
zDot = state(6);
xs1 = state(10);
ys1 = state(11);
zs1 = state(12);
xs2 = state(13);
ys2 = state(14);

Page | 26
zs2 = state(15);
xs3 = state(16);
ys3 = state(17);
zs3 = state(18);
theta = state(19);
% Htilde will change depending on the site, so this check uses the correct
% station
if site ==101
xs = xs1;
ys = ys1;
zs = zs1;
elseif site ==337
xs = xs2;
ys = ys2;
zs = zs2;
else
xs = xs3;
ys = ys3;
zs = zs3;
end
% Calculate constants to save calculations later
cT = cos(theta);
sT = sin(theta);
% rho and rhoDot are equivalent to the G* matrix - the measurements
% evaluated on the reference trajectory.
rho = sqrt(x^2+y^2+z^2+xs^2+ys^2+zs^2-2*z*zs+ 2*(x*ys-y*xs)*sT-2*(x*xs+y*ys)*cT);
rhoDot = 1/rho*(x*xDot+y*yDot+z*zDot-
(xDot*xs+yDot*ys)*cT+omegaEarth*(x*xs+y*ys)*sT...
+(xDot*ys-yDot*xs)*sT+omegaEarth*(x*ys-y*xs)*cT-zDot*zs);
yStar = [rho; rhoDot];
% Calculate necessary partial derivatives
dRhoDx = (x-xs*cT+ys*sT)/rho;
dRhoDy = (y-ys*cT-xs*sT)/rho;
dRhoDz = (z-zs)/rho;
dRhoDxs = (xs-x*cT-y*sT)/rho;
dRhoDys = (ys-y*cT+x*sT)/rho;
dRhoDzs = (zs-z)/rho;
dRhoDotDx = (xDot+xs*omegaEarth*sT+ys*omegaEarth*cT)/rho - rhoDot*(x-
xs*cT+ys*sT)/(rho^2);
dRhoDotDy = (yDot+ys*omegaEarth*sT-xs*omegaEarth*cT)/rho - rhoDot*(y-ys*cT-
xs*sT)/(rho^2);
dRhoDotDz = zDot/rho + rhoDot*(zs-z)/(rho^2);
dRhoDotDxs = (-xDot*cT+omegaEarth*x*sT-yDot*sT-omegaEarth*y*cT)/rho - rhoDot*(xs-
x*cT-y*sT)/(rho^2);
dRhoDotDys = (-yDot*cT+omegaEarth*y*sT+xDot*sT+omegaEarth*x*cT)/rho - rhoDot*(ys-
y*cT+x*sT)/(rho^2);
dRhoDotDzs = (-zDot)/rho - rhoDot*(zs-z)/(rho^2);
dRhoDotDxDot = (x-xs*cT+ys*sT)/rho;
dRhoDotDyDot = (y-ys*cT-xs*sT)/rho;
dRhoDotDzDot = (z-zs)/rho;
% hTilde varies depending on site, so this section checks that
if site ==101
hTilde = [dRhoDx dRhoDy dRhoDz 0 0 0 0 0 0 dRhoDxs dRhoDys dRhoDzs 0 0 0 0 0 0;...
dRhoDotDx dRhoDotDy dRhoDotDz dRhoDotDxDot dRhoDotDyDot dRhoDotDzDot 0 0 0
dRhoDotDxs dRhoDotDys dRhoDotDzs 0 0 0 0 0 0];
elseif site ==337

Page | 27
hTilde = [dRhoDx dRhoDy dRhoDz 0 0 0 0 0 0 0 0 0 dRhoDxs dRhoDys dRhoDzs 0 0 0;...
dRhoDotDx dRhoDotDy dRhoDotDz dRhoDotDxDot dRhoDotDyDot dRhoDotDzDot 0 0 0 0 0
0 dRhoDotDxs dRhoDotDys dRhoDotDzs 0 0 0];
else
hTilde = [dRhoDx dRhoDy dRhoDz 0 0 0 0 0 0 0 0 0 0 0 0 dRhoDxs dRhoDys dRhoDzs;...
dRhoDotDx dRhoDotDy dRhoDotDz dRhoDotDxDot dRhoDotDyDot dRhoDotDzDot 0 0 0 0 0
0 0 0 0 dRhoDotDxs dRhoDotDys dRhoDotDzs];
end
end
integrator.m
function [tOut, xOut] = asen5070proj_integrator(state0, tVect, radEarth, omegaEarth,
areaSat, mass, dens0, H, ref0, myoptions)
[tOut,xOut] = ode45(@asen5070proj_motionEq,tVect, state0, myoptions, radEarth,
omegaEarth, areaSat, mass, dens0, H, ref0);
motionEq.m
function dx = asen5070proj_motionEq(t,state, radEarth, omegaEarth, areaSat, mass,
dens0, H, ref0 )
% dx = asen5070proj_motionEq(t,x, omegaEarth)
% Break up state for easier reading.
% The first 18 elements of the state vector are the actual state
% Element 19 is the angle rotated by the earth
% The last 18x18 elements are the STM
x = state(1);
y = state(2);
z = state(3);
xDot = state(4);
yDot = state(5);
zDot = state(6);
mu = state(7);
J2 = state(8);
cd = state(9);
xs1 = state(10);
ys1 = state(11);
zs1 = state(12);
xs2 = state(13);
ys2 = state(14);
zs2 = state(15);
xs3 = state(16);
ys3 = state(17);
zs3 = state(18);
theta = state(19);
% Initialize derivative variables
dx = zeros(1,19+18*18);
% Reshape STM, calculate A matrix, and solve for STM_dot
STM = reshape(state(20:end), 18,18);
A = asen5070proj_aMat(state, radEarth, omegaEarth, areaSat, mass, dens0, H, ref0);
STM_dot = A*STM;
STM_dot = reshape(STM_dot, 18*18,1);

Page | 28
% Calculate some constants to save time
r = sqrt(x^2+y^2+z^2); % Radius magnitude of satellite
densTerm = dens0*exp(-(r-ref0)/H); % Air density term
dragConst = -0.5*cd*areaSat/mass*densTerm; % Drag Constant term
intTerm = 3/2*J2*(radEarth/r)^2*(5*(z/r)^2-1);% Term that appears a lot
intTermZ = 3/2*J2*(radEarth/r)^2*(5*(z/r)^2-3);% Similar to above, but changes in z
direction
muTerm1 = -mu/r^3; % mu term that appears a lot
dragVelX = xDot+omegaEarth*y;
dragVelY = yDot-omegaEarth*x;
dragVelZ = zDot;
dragVel = sqrt((xDot+omegaEarth*y)^2+(yDot-omegaEarth*x)^2+zDot^2); % magnitude of
drag velocity
dx = [xDot;...
yDot;...
zDot;...
muTerm1*x*(1-intTerm)+dragConst*dragVel*dragVelX;...
muTerm1*y*(1-intTerm)+dragConst*dragVel*dragVelY;...
muTerm1*z*(1-intTermZ)+dragConst*dragVel*dragVelZ;...
0;0;0; 0;0;0; 0;0;0; 0;0;0;
omegaEarth;
STM_dot];
aMat.m
function A = asen5070proj_aMat(state, radEarth, omegaEarth, areaSat, mass, dens0, H,
ref0 )
% A = asen5070proj_aMat(state)
% Calculates the A matrix (partial derivatives) given a vector of
% states
% Break up state for easier reading
x = state(1);
y = state(2);
z = state(3);
xDot = state(4);
yDot = state(5);
zDot = state(6);
mu = state(7);
J2 = state(8);
cd = state(9);
% Save common values as constants
r = sqrt(x.^2+y.^2+z.^2);
muTerm1 = -mu/r(1)^3;
muTerm2 = 3*mu/r(1)^5;
radTerm = J2*(radEarth/r(1))^2;
zTerm = (z(1)/r(1))^2;
dragConst = -0.5*cd*areaSat/mass;
dragVel = sqrt((xDot(1)+omegaEarth*y(1))^2+(yDot(1)-omegaEarth*x(1))^2+zDot(1)^2);
densTerm = dens0*exp(-(r(1)-ref0)/H);
% Calculate Partial Derivatives
%dXddot/dx
partXddotX = muTerm1*(1-3/2*radTerm*(5*zTerm-1))+muTerm2*x(1)^2*(1-
5/2*radTerm*(7*zTerm-1))...

Page | 29
-dragConst*densTerm*omegaEarth*(xDot(1)+omegaEarth*y(1))*(yDot(1)-
omegaEarth*x(1))/dragVel...
-dragConst*densTerm*dragVel*x(1)*(xDot(1)+omegaEarth*y(1))/(H*r(1));
%dXddot/dy
partXddotY = muTerm2*x(1)*y(1)*(1-5/2*radTerm*(7*zTerm-1))...
+dragConst*omegaEarth*densTerm*dragVel...
+(dragConst*omegaEarth*densTerm*(xDot(1)+omegaEarth*(y(1)))^2)/dragVel...
-dragConst*y(1)*densTerm*(xDot(1)+omegaEarth*y(1))*dragVel/(H*r(1));
%dXddot/dz
partXddotZ = muTerm2*x(1)*z(1)*(1-5/2*radTerm*(7*zTerm-3))...
-dragConst*z(1)*densTerm*(xDot(1)+omegaEarth*y(1))*dragVel/(H*r(1));
%dXddot/dxdot
partXddotXdot = dragConst*densTerm*(xDot(1)+omegaEarth*y(1))^2/dragVel...
+dragConst*densTerm*dragVel;
%dXddot/dydot
partXddotYdot = dragConst*densTerm*(yDot(1)-
omegaEarth*x(1))*(xDot(1)+omegaEarth*y(1))/dragVel;
%dXddot/dzdot
partXddotZdot = dragConst*densTerm*zDot(1)*(xDot(1)+omegaEarth*y(1))/dragVel;
%dXddot/mu
partXddotMu = -x(1)/r(1)^3*(1-3/2*radTerm*(5*zTerm-1));
%dXddot/J2
partXddotJ2 = 3/2*mu*x(1)/r(1)^3*radTerm/J2*(5*zTerm-1);
%dXddot/Cd
partXddotCd = dragConst/cd*densTerm*dragVel*(xDot(1)+omegaEarth*y(1));
%dYddot/dx
partYddotX = muTerm2*x(1)*y(1)*(1-5/2*radTerm*(7*zTerm-1))...
-dragConst*omegaEarth*densTerm*dragVel...
-dragConst*omegaEarth*densTerm*(yDot(1)-omegaEarth*x(1))^2/dragVel...
-dragConst*x(1)*densTerm*(yDot(1)-omegaEarth*x(1))*dragVel/(H*r(1));
%dYddot/dy
partYddotY = muTerm1*(1-3/2*radTerm*(5*zTerm-1))+muTerm2*y(1)^2*(1-
+dragConst*omegaEarth*densTerm*(xDot(1)+omegaEarth*y(1))*(yDot(1)-
omegaEarth*x(1))/dragVel...
-dragConst*y(1)*densTerm*(yDot(1)-omegaEarth*x(1))*dragVel/(H*r(1));
%dYddot/dz
partYddotZ = muTerm2*y(1)*z(1)*(1-5/2*radTerm*(7*zTerm-3))...
-dragConst*z(1)*densTerm*(yDot(1)-omegaEarth*x(1))*dragVel/(H*r(1));
%dYddot/dxdot
partYddotXdot = dragConst*densTerm*(yDot(1)-
omegaEarth*x(1))*(xDot(1)+omegaEarth*y(1))/dragVel;
%dYddot/dydot
partYddotYdot = dragConst*densTerm*(yDot(1)-omegaEarth*x(1))^2/dragVel...
+dragConst*densTerm*dragVel;
%dYddot/dzdot
partYddotZdot = dragConst*densTerm*zDot(1)*(yDot(1)-omegaEarth*x(1))/dragVel;
%dYddot/mu
partYddotMu = -y(1)/r(1)^3*(1-3/2*radTerm*(5*zTerm-1));
%dYddot/J2
partYddotJ2 = 3/2*mu*y(1)/r(1)^3*radTerm/J2*(5*zTerm-1);
%dYddot/Cd
partYddotCd = dragConst/cd*densTerm*dragVel*(yDot(1)-omegaEarth*x(1));
%dZdot/dx
partZddotX = muTerm2*x(1)*z(1)*(1-5/2*radTerm*(7*zTerm-3))...
-dragConst*omegaEarth*zDot(1)*densTerm*(yDot(1)-omegaEarth*x(1))/dragVel...
-dragConst*x(1)*zDot(1)*densTerm*dragVel/(H*r(1));
%dZdot/dy
partZddotY = muTerm2*y(1)*z(1)*(1-5/2*radTerm*(7*zTerm-3))...
+dragConst*omegaEarth*zDot(1)*densTerm*(xDot(1)+omegaEarth*y(1))/dragVel...

Page | 30
-dragConst*y(1)*zDot(1)*densTerm*dragVel/(H*r(1));
%dZddot/dz
partZddotZ = muTerm1*(1-3/2*radTerm*(5*zTerm-3))+muTerm2*z(1)^2*(1-
-dragConst*z(1)*zDot(1)*densTerm*dragVel/(H*r(1));
%dZddot/dxdot
partZddotXdot = dragConst*densTerm*zDot(1)*(xDot(1)+omegaEarth*y(1))/dragVel;
%dZddot/dydot
partZddotYdot = dragConst*densTerm*zDot(1)*(yDot(1)-omegaEarth*x(1))/dragVel;
%dZddot/dzdot
partZddotZdot = dragConst*densTerm*zDot(1)^2/dragVel +dragConst*densTerm*dragVel;
%dZddot/mu
partZddotMu = -z(1)/r(1)^3*(1-3/2*radTerm*(5*zTerm-3));
%dZddot/J2
partZddotJ2 = 3/2*mu*z(1)/r(1)^3*radTerm/J2*(5*zTerm-3);
%dZddot/Cd
partZddotCd = dragConst/cd*densTerm*dragVel*(zDot(1));
% Build partial matrix
A = [0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0;...
0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0;...
0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0;...
partXddotX partXddotY partXddotZ partXddotXdot partXddotYdot partXddotZdot
partXddotMu partXddotJ2 partXddotCd 0 0 0 0 0 0 0 0 0;...
partYddotX partYddotY partYddotZ partYddotXdot partYddotYdot partYddotZdot
partYddotMu partYddotJ2 partYddotCd 0 0 0 0 0 0 0 0 0;...
partZddotX partZddotY partZddotZ partZddotXdot partZddotYdot partZddotZdot
partZddotMu partZddotJ2 partZddotCd 0 0 0 0 0 0 0 0 0;...
];
% Lower portion is just zeros
A(7:18,1:18) = 0;
end
p0.m
function [P0inv, W] = asen5070proj_p0()
P0inv = [1/1e6 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;...
0 1/1e6 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;...
0 0 1/1e6 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;...
0 0 0 1/1e6 0 0 0 0 0 0 0 0 0 0 0 0 0 0;...
0 0 0 0 1/1e6 0 0 0 0 0 0 0 0 0 0 0 0 0;...
0 0 0 0 0 1/1e6 0 0 0 0 0 0 0 0 0 0 0 0;...
0 0 0 0 0 0 1/1e20 0 0 0 0 0 0 0 0 0 0 0;...
0 0 0 0 0 0 0 1/1e6 0 0 0 0 0 0 0 0 0 0;...
0 0 0 0 0 0 0 0 1/1e6 0 0 0 0 0 0 0 0 0;...
0 0 0 0 0 0 0 0 0 1/1e-10 0 0 0 0 0 0 0 0;...
0 0 0 0 0 0 0 0 0 0 1/1e-10 0 0 0 0 0 0 0;...
0 0 0 0 0 0 0 0 0 0 0 1/1e-10 0 0 0 0 0 0;...
0 0 0 0 0 0 0 0 0 0 0 0 1/1e6 0 0 0 0 0;...
0 0 0 0 0 0 0 0 0 0 0 0 0 1/1e6 0 0 0 0;...
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1/1e6 0 0 0;...
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1/1e6 0 0;...
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1/1e6 0;...
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1/1e6;...
];
P0inv = P0inv*1000000;
W = [1/(0.01)^2 0; ...
0 1/(0.001)^2];

Page | 31
plotEllipsoid.m
% Plot an ellipsoid given an orthonormal, right handed
% transformation matrix, R and the semi - axis, semi
%
% For the Stat. O.D. project R is made up of the eigenvectors
% of the upper 3x3 portion of the covariance matrix. semi
% contains sigma_x, sigma_y, sigma_z in a column vector.
%
% Taken from the ASEN5070 website, used with permission
% of the instructor
function plotEllipsoid(R,semi)
[x,y,z] = sphere(20);
x = x * semi(1);
y = y * semi(2);
z = z * semi(3);
[mm,nn] = size(x);
C = (R * [x(:) y(:) z(:)]')';
x = reshape(C(:,1),mm,nn);
y = reshape(C(:,2),mm,nn);
z = reshape(C(:,3),mm,nn);
surf(x,y,z)
axis equal

JLamparielloFinalProjectASEN5070

Recommended

Recommended

More Related Content

Viewers also liked

Viewers also liked (15)

Similar to JLamparielloFinalProjectASEN5070

Similar to JLamparielloFinalProjectASEN5070 (20)

JLamparielloFinalProjectASEN5070