MODELING INERTIAL MEASUREMENT UNITS AND ANALYZING THE
EFFECT OF THEIR ERRORS IN NAVIGATION APPLICATIONS
Except where reference is made to work of others, the word described in this thesis is my
own or was done in collaboration with my advisory committee. This thesis does not
include proprietary or classified information
Warren S. Flenniken, IV
Certificate of Approval:
A. Scottedward Hodel
Associate Professor
Electrical and Computer Engineering
David M. Bevly, Chair
Assistant Professor
Mechanical Engineering
George T. Flowers
Professor
Mechanical Engineering
Stephen L. McFarland
Dean
Graduate School
MODELING INERTIAL MEASUREMENT UNITS AND ANALYZING THE
EFFECT OF THEIR ERRORS IN NAVIGATION APPLICATIONS
Warren S. Flenniken, IV
A Thesis
Submitted to
the Graduate Faculty of
Auburn University
in Partial Fulfillment of the
Requirements for the
Degree of
Masters of Science
Auburn, Alabama
December 16, 2005
MODELING INERTIAL MEASUREMENT UNITS AND ANALYZING THE
EFFECT OF THEIR ERRORS IN NAVIGATION APPLICATIONS
Warren S. Flenniken, IV
Permission is granted to Auburn University to make copies of this thesis at
its discretion, upon the request of individuals or institutions and
at their expense. The author reserves all publication rights.
Signature of the Author
Date of Graduation
iii
THESIS ABSTRACT
MODELING INERTIAL MEASUREMENT UNITS AND ANALYZING THE
EFFECT OF THEIR ERRORS IN NAVIGATION APPLICATIONS
Warren S. Flenniken, IV
Master of Science, December 16, 2005
(B.A., Auburn University, 2005)
(B.M.E., Auburn University, 2000)
(B.A., Auburn University, 1998)
165 Typed Pages
Directed by David M. Bevly
In this thesis, a simple model that statistically represents an Inertial Measurement
Unit (IMU) output has been studied. This model is then expanded to produce a model
that incorporates terms that account for errors associated with high dynamics. Several
techniques are used to determine the error statistics of the IMU model and categorize the
performance of the IMU. These techniques include Allan variance charts, Monte Carlo
simulations and autocorrelation functions. Equations for the position, velocity and
heading error in a two and six degree of freedom system are developed. These analytical
equations for the error growth are then verified using Monte Carlo simulations. Monte
Carlo simulations are also used to compare the error bounds using both the simple model
and high dynamic model. A novel Kalman filter is developed to couple GPS with an
inertial measurement unit in order to bound the error growth. The Kalman filter
estimates both a constant and drifting bias. The error bounds on the position, velocity
and heading state estimations are compared to the error bounds in the simple model?s
uncoupled case. The framework formulated from these models is intended as an aid for
understanding the effects that the different error sources have on position, velocity, and
iv
heading calculations. With this information it is possible to determine the correct inertial
measurement unit with identified error characteristics for a specific application.
v
ACKNOWLEGEMENTS
I would first like to thank my advisor, Dr. David M. Bevly, for his guidance and
knowledge over the last two years. I would also like to thank everyone in my lab for their
assistance.
I would like to thank my wife, Shellie M. Flenniken, for giving me two wonderful
sons, Warren S Flenniken V and Mitchell Scott Flenniken, during my graduate school
tour. You are a wonderful and patient wife, and I will always love you for that.
For their financial and emotional support throughout my life, I would like to thank
my parents, Warren S. Flenniken, III and Ellen Crawford Flenniken. Without you, I
would have never been able to pursue this goal.
I would like to thank my brother, Jason Crawford Flenniken, for his
companionship during the retreats to Spur Ranch. They were much needed and
appreciated.
I would like to thank my in-laws, S. Felton Mitchell and L. Ann Mitchell for their
financial and emotional support, as well. Both of you have been great and I will always
remember it.
Finally I am dedicating this work to my grandfather, Warren S Flenniken, II, who
passed away September 20, 2003. He was a great man and wonderful role model. He is
missed by all that knew him.
vi
Style of Journal Used:
ASME Journal of Dynamic Systems, Measurement, and Control
Computer Software Used:
Microsoft Word 2002
vii
TABLE OF CONTENTS
LIST OF FIGURES...????????????????????????? xi
LIST OF TABLES.?????????????????????????? xiv
1. INTRODUCTION
1.1 Motivation????????????????????????.. 1
1.2 Background and Literature Review..??...??.????????..... 2
1.3 Purpose?????...???????????????????? 3
1.4 Outline?????????????????????????? 4
2. SIMPLE SENSOR MODELING
2.1 Introduction?...??.???????????????????? 6
2.2 Sensor Model???????????????????????. 7
2.3 Random Walk Evaluation.??????????????????. 9
2.4 Walking Bias Evaluation....?????????????.????.. 14
2.5 Constant Bias Evaluation..??????????????????. 16
2.6 Rate Gyro Model Validation??????????????..??... 16
2.7 Accelerometer Model Validation???????????????.. 21
2.8 Sensor Categorization???????????????????? 24
2.9 Summary and Conclusion??????????????????.. 30
3. INTEGRATION ERRORS OF A TWO DEGREE OF FREEDOM IMU
3.1 Introduction????????.....??...??.?????????... 31
3.2 IMU Integration Error???????????????????? 32
viii
3.3 Monte Carlo Validation???????????????????. 40
3.4 Longitudinal Velocity Error Equations???????????...?.. 43
3.5 Longitudinal Position Error Equations?????????????.. 45
3.6 Lateral Velocity Error Equations???????????????... 46
3.7 Lateral Position Error Equations...??????????????? 48
3.8 Bias Contribution in the IMU Integration Errors?????????.. 50
3.9 Gravitational Effects????????????????????.. 55
3.10 Summary and Conclusion??????????????????.. 60
4. INTEGRATION ERROR CORRECTIONS FOR A TWO DOF IMU
4.1 Introduction????????.....??...??.?????????... 61
4.2 Heading Error Reduction with a Two State Kalman Filter?????... 61
4.3 Heading Error during a GPS Outage??????????..???... 67
4.4 Heading Error Reduction with a Three State Kalman Filter??..??... 72
4.5 Kalman Filter Comparisons?????????????????... 74
4.6 Position Estimation with a Six State Kalman Filter????????.. 78
4.7 Summary and Conclusion??????????????????.. 81
5. INTEGRATION ERRORS OF A SIX DEGREE OF FREEDOM IMU
5.1 Introduction????????.....??...??.?????????... 82
5.2 Six Degree of Freedom Inertial Measurement Unit Model?????... 82
5.3 Six Degree of Freedom Inertial Measurement Unit Integration?.??... 87
5.4 Six Dof IMU Integration Heading Errors?????..??????.... 91
5.5 Six Dof IMU Integration Velocity Errors????????????.. 93
5.6 Six Dof IMU Integration Position Errors????????????.. 95
5.7 Summary and Conclusion??????????????????.. 97
6. ADVANCED SENSOR MODELING
ix
6.1 Introduction????????.....??...??.?????????? 99
6.2 The Inertial Measurement Unit Accelerometer Triad Error Model??? 99
6.3 The Inertial Measurement Unit Rate Gyro Triad Error Model????... 101
6.4 Model Error Parameter Descriptions??????????????.. 104
6.5 Inertial Measurement Unit Initialization Errors??????????. 107
6.6 Summary and Conclusion??????????????????... 110
7. INTEGRATION ERRORS OF AN ADVANCED IMU MODEL
7.1 Introduction???????????????????????? 111
7.2 Launch Trajectory Profile?????????.?????????. 111
7.2 Comparison of the Simple and Advanced IMU Models????..??. 114
7.3 Advanced Model Error Parameter Contribution Levels??????? 116
7.4 Summary and Conclusion??????????????????.. 118
8. CONCLUSIONS
8.1 Summary????????????????????????.... 119
8.2 Recommendation for Future Work??????????????? 122
REFERENCES??...????????????????????????? 124
APPENDICES?...?????????????????????????? 128
A The Allan Variance????????????????????... 129
B Markov Process Statistics??????????????????. 135
C An Introduction to the Kalman Filter????????????...? 141
D The Autocorrelation Function????????????????... 146
x
LIST OF FIGURES
2.1 KVH-5000 Experimental Data Filtered and Unfiltered????????... 10
2.2 Allan Variance Chart for the Unfiltered Experimental Data of the KVH-
5000?????????????????????????.
11
2.3 AQRS-104 Experimental Data Filtered and Unfiltered????????... 12
2.4 Allan Variance Chart for the Unfiltered Experimental Data of the AQRS-
104?????????????????????????...
13
2.5 AQRS-104 Experimental Autocorrelation Plot???????.????... 15
2.6 Allan Variance Comparison I of the KVH 5000 Experimental and
Simulated Data?????????????????????
17
2.7 Allan Variance Comparison II of the KVH 5000 Experimental and
Simulated Data?????????????????????
18
2.8 Allan Variance Comparison of the AQRS-104 Experimental and Simulated
Data?????????????????????????.
19
2.9 AQRS-104 Autocorrelation Comparison of the Filtered Experimental Data.. 20
2.10 Humphrey Accelerometer Actual Data filtered and Unfiltered Data???... 21
2.11 Humphrey Accelerometer Autocorrelation of the Filtered Experimental
Data?????????????????????????.
23
2.12 Allan Variance Comparison of the Humphrey Accelerometer Experimental
and Simulated Data???????????????????.
24
3.1 Tactical Grade IMU Position Error Bounds (Static)?????????? 34
3.2 Tactical Grade IMU Velocity Error Bounds (Static)?????????... 35
3.3 Tactical Grade IMU Heading Error Bounds (Static)?????????... 36
3.4 Consumer Grade IMU Position Error Bounds (Static)????????? 37
3.5 Consumer Grade IMU Velocity Error Bounds (Static)????????? 38
3.6 Consumer Grade IMU Heading Error Bounds (Static)????????? 39
xi
3.7 KVH Error Bound Comparison I?????????????????. 41
3.8 KVH Error Bound Comparison II????????????????? 42
3.9 KVH Error Bound Comparison III????????????????.. 43
3.10 Comparison of a Tactical Grade IMU Position Error Bounds (Static)??? 51
3.11 Comparison of a Tactical Grade IMU Velocity Error Bounds (Static)??? 52
3.12 Effect of Tactical Grade IMU Biases on Position Error (Static)?????.. 53
3.13 Effect of Tactical Grade IMU Biases on Velocity Error (Static)?????. 54
3.14 Tactical Grade IMU Position Error Bounds with Gravity Field?????.. 56
3.15 Tactical Grade IMU Velocity Error Bounds with Gravity Field?????. 57
3.16 Consumer Grade IMU Position Error Bounds with Gravity Field????.. 58
3.17 Consumer Grade IMU Velocity Error Bounds with Gravity Field????.. 59
4.1 Consumer Grade Heading Error Comparison between the 2 state Kalman
Filter Model and Rugge-Kutta Order Four??????????.
65
4.2 Tactical Grade Heading Error Comparison between the 2 state Kalman
Filter Model and Rugge-Kutta Order Four??????????.
66
4.3 State Estimates for a Two State Kalman Filter with the Second State
Modeled as a 1
st
Order Markov Process???????????.
68
4.4 State Estimates for a Two State Kalman Filter with the Second State
Modeled as a Random Walk???????????????...
69
4.5 Error Bound Comparison between the Two State Kalman Filter Models
when the Dominate Bias is not Constant???????????
70
4.6 Error Bound Comparison between the Two State Kalman Filter Models
when the Dominate Bias is a Constant????????????
71
4.7 Consumer Grade Heading Error Comparison between the 3 State Kalman
Filter Model, the 2 State Kalman Filter Model and A Rugge-Kutta
Order Four Integration Method??????????????..
74
4.8 Systron-Donner State Estimates for a Two State Kalman Filter with the
Second State Modeled as a Random Walk??????????.
75
4.9 Systron-Donner State Estimates for a Two State Kalman Filter with the
Second State Modeled as a 1
st
Order Markov Process??????
76
xii
4.10 Systron-Donner State Estimates for a Three State Kalman Filter with the
Second State Modeled as a 1
st
Order Markov Process and the Third
State Modeled as a Random Walk?????????????..
78
5.1 Definition of the Six Degrees of Freedom?????????????... 83
5.2 Heading Error in a Six DoF Tactical Grade IMU??????????? 92
5.3 Heading Error in a Six DoF Consumer Grade IMU?????????? 93
5.4 Velocity Errors in a Six DoF Tactical Grade IMU??????????.. 94
5.5 Velocity Error in a Six DoF Consumer Grade IMU?????????? 95
5.6 Position Error in a Six DoF Tactical Grade IMU??????????? 96
5.7 Position Error in a Six DoF Consumer Grade IMU??????????. 97
6.1 Inertial Measurement Unit Input/Output Error Types?????????.. 104
6.2 Inertial Measurement Unit Triad Misalignment???????????... 105
6.3 Inertial Measurement Unit Triad Nonorthogonality?????????? 106
6.4 Initialization Vectors of the Body Coordinate Frame?????????.. 109
7.1 Rocket Launch Trajectory I: Body Accelerations, Body Headings and ENU
Velocities???????????????????????
113
7.2 Rocket Launch Trajectory I Altitude Vs. Range???????????.. 114
7.3 Rocket Launch Trajectory Impact Point Scatter Plot?????????... 115
7.4 Rocket Launch Trajectory I Error Parameters Contribution Levels???? 117
xiii
LIST OF TABLES
2.1 Accelerometer Characteristics Used for this Analysis?????????. 25
2.2 Classification Characteristics Used to Categorize Accelerometers????. 26
2.3 Gyro Characteristics Used for this Analysis????????????? 27
2.4 Classification Characteristics Used to Categorize Rate Gyros?????? 29
6.1 Advanced IMU Error Parameters?????????????????. 102
6.2 IMU Initialization Error Parameters???????????????? 108
xiv
CHAPTER 1
INTRODUCTION
1.1. Motivation
When an instrument is used to collect data, the measurement obtained from that
device is always corrupted by some error sources. Some of these errors are constant,
some have known frequency content, and some are random. These errors are common in
instruments used to calculate position, velocity, or heading of a vehicle. Because the
errors are in the instruments measurement, it is inevitable that the calculations will
contain errors.
A common technique used to calculate position, velocity, and heading of a vehicle
is dead reckoning. One form of dead reckoning is to document an initial position,
velocity, and heading relative to some known coordinate frame and then measure the
accelerations and rotation rates of the vehicle versus time. As the accelerations and
rotation rates are measured, they are integrated to calculate the vehicle?s position,
velocity, and heading. Because the acceleration and rotation rate measurements are noisy
or imprecise, the calculated position, velocity, and heading will contain errors as well.
This work is an analysis of these errors and how we as engineers can evaluate them,
classify them, and remove them from the position, velocity, and heading calculations
when accelerometers and rate gyros are used to dead reckon.
1
1.2. Background and Literature Review
Position, velocity and heading calculations have errors. In order to compensate for
these errors, the errors found in the accelerometer and rate gyro used must be identified.
A simple model has been developed that does this [Demoz]. The simple model classifies
the common errors found in accelerometers and rate gyros into three categories: a
constant offset error source, a moving bias error source and a random error source. A
procedure has also been developed to obtain the statistics of each of these error sources.
When the statistical values for each of the error sources are known, it is possible to use
the simple model to statistically simulate an accelerometer?s or rate gyro?s output.
The outputs of rate gyros are normally integrated to obtain a heading estimate.
Because the rate gyro measurements have errors, the calculated heading will contain
errors as well. Error equations that depict the growth of the error in the heading
calculation have been derived [Bevly]. However, these error equations were derived with
the assumption that the rate gyro has only one error source, wide band noise. The value
of this error model is that it may be used to obtain an estimation of the standard deviation
of the heading estimate error over time.
The simple sensor model used to statistically represent the output of an
accelerometer and rate gyro has been used in conjunction with the Global Position
System to bound the errors found in position, velocity and heading calculations [Demoz].
A two state Kalman filter is used to estimate heading, and a five state Kalman filter is
used to estimate position and velocity. In each of these filters, the error sources are
assumed to be wide-band noise and either a moving bias error or a constant offset error.
2
More advanced models have been developed that incorporate errors that are
affected by the true accelerations or rotation rates [Grewal]. These errors include scale
factor errors, misalignment errors and nonorthogonality errors.
1.3. Purpose
The primary purpose of this work is to use the existing simple sensor model to
identify the common error sources found in accelerometers and rate gyros, and then
validate the procedure used to evaluate the statistical properties of each of these error
sources. The simple model is then used to classify accelerometers and rate gyros into
different grades. Error bounds of some component grades are provided which can be
used to determine the utility of a specific grade sensor.
The second purpose of this work is to develop Kalman filter models that contain
enough states so that all the errors in the simple sensor model can be estimated. With
these Kalman filter models, it is shown that the errors during a loss of a GPS signal are
minimized when compared to the existing Kalman filter models. A Kalman Filter is a
state estimator that uses the knowledge of the input error sources to assist in estimating
the true state over time. It is an ideal coupling tool to use in this scenario due to the fact
that the GPS measurements are very precise but are received at a very low frequency, and
the IMU measurements are relatively imprecise and can be received at relatively high
frequencies when compared to GPS.
The third purpose of this work is the evaluation of the effects gravity has on a
group of accelerometers and rate gyros when used in an inertial measurement unit. The
3
error growths of the position, velocity and heading calculation are shown when a gravity
field is in place, and then the error growths are shown when the gravity field is removed.
The comparison reveals the effect a gravity field has on an inertial measurement unit?s
total error.
The final purpose of this work is to identify the effects the error terms found in
the more advanced sensor models have on the position, velocity and heading calculations
[Grewal]. Simulated trajectories will be used to evaluate the contribution level each of
these errors has on the calculated position, velocity, and heading. The simulated
trajectories are designed to be some what practical and still excite the advance sensor
model?s error terms
1.4. Outline
In this thesis a simple sensor model is first defined that is used to classify error
sources commonly found in accelerometers and rate gyros. Many error sources are
incorporated in the model; they include a constant bias error source, a moving bias error
source, and a random error source. The model is used to statistically simulate the output
of an accelerometer and rate gyro [Demoz]. Each term in the simple model will be
described statistically in the form of norms and variances.
In this work a navigation technique known as dead reckoning is then used to
determine a vehicle?s position on the earth. Dead reckoning is the determination, without
the aid of celestial observations, of the position of a ship or aircraft from the record of the
courses sailed or flown, the distance made, and the known or estimated drift [Webster].
4
The position is calculated by integrating three accelerometers and three rate gyroscopes
mounted to the vehicle platform. These sensor packages are commonly referred to as
inertial measurement units or IMU?s. The category of the sensors contained in the IMU
determines the precision of the corresponding vehicle?s position estimate. The dead
reckoning technique is analyzed to determine the magnitude of the errors in the position,
velocity and heading calculations. The magnitude of the error is then analyzed to
determine how it is affected by the different error sources found in accelerometers and
rate gyros
Using the simple sensor model, a one and two degree of freedom system
simulations are set up to predict the error growth of tactical and consumer grade sensors.
The Global Position System or GPS is used to correct the IMU position errors in a
coupling algorithm. The algorithm chosen to couple the GPS and IMU?s measurements
is a Kalman Filter. Experimental data is used to show the utility of the Kalman filter
models.
Finally, in this thesis a six degree of freedom system is developed. It is used to
show the error growth of position velocity and heading over time. The accuracy of the
simple model as well as a more advanced model is shown. The effects of each of the
advanced IMU model?s parameters are taken into account as well as the introduction of a
gravity field. A Monte Carlo Simulation is used to show the bounds on the error growths
for the position, velocity and heading calculations.
5
CHAPTER 2
SIMPLE SENSOR MODEL
2.1 Introduction
In this chapter a simple sensor model is laid out that incorporates three common
error sources. They are a moving bias, a constant bias, and a random error. Other error
sources such as gravitational errors, cross-coupling errors and sensor scale factor errors
are considered in later chapters. In this chapter the biases and random error sources are
analyzed statistically. The numerical values for the time constants, means, and variances
are obtained by using common statistical methods, Allan variance charts, autocorrelation
functions, and Monte Carlo simulations. The statistical parameters of the error sources
are used to categorize and characterize the sensors. Inertial sensors can be categorized
into four basic grades: navigational, tactical, automotive, and consumer. While the
navigation sensors are the most precise they also cost the most at around $100,000. The
tactical sensors cost in the $20,000 range, the automotive cost in the $1,000 range and the
consumer sensors, the least precise cost less than $1,000. Several sensor grades are
simulated and experimentally validated. The simulated sensors are used in later chapters
to determine resulting levels of performance that can be achieved when combined with
GPS. A table is provided at the end of this chapter which contains a number commonly
6
used in rate gyros and accelerometers with their specifications. An additional chart is
provided that is used to classify the sensors into the grades mentioned above.
2.2 Sensor Model
One simple model of a gyro assumes that the gyro output ( ) consists of the true
vehicle rotation rate (
r
g
r ) plus a constant offset ( c ), a moving or walking bias ( ), and
wide band sensor noise ( ). It is shown below.
r r
b
gyro
w
gyrorrr
wbcrg +++= (2.1)
The wide band sensor noise ( ) is assumed to be normally distributed with a zero
mean and sampled covariance
gyro
w
[ ]
sgyrogyro
fwE
22
?=
(2.2)
Observe in Equation (2.2) that wide band noise increases as the sample rate ( ) of the
sensor increases [Demoz]. The gyros moving bias ( ) will be modeled as a first order
Markov process. The Markov process takes in account the sampling frequency of the
sensor. This is shown in Equations (2.3 ? 2.5). For a more explicit definition of a first
order Markov Process see Appendix B.
s
f
r
b
7
We assume the moving bias has the following statistics.
[ ] 0=
r
bE and [ ]
22
bias
gyror
bE ?=
(2.3)
and that
bias
gyror
r
r
wbb +?=
?
1
&
(2.4)
where
v
f
w
r
gyros
gyro
bias
bias
?
?
2
2
= (2.5)
The noise that drives the bias, , is normally distributed with zero mean and a sampled
covariance of one.
v
[ ]1,0~ Nv (2.6)
An Allan variance and autocorrelation analysis is used to determine the time
constant (
r
? ), random walk (
gyro
? ), and bias variation (
bias
gyro
? ). The numerical values
are evaluated with experimental data and verified with the manufactures specifications
and documents containing published values. Once this method is validated, it is used for
future analysis when exact sensor specifications are not known.
An accelerometer is modeled in the same fashion as a rate gyro. A simple model
that relates the accelerometer output ( ) to the true vehicle acceleration ( ), a constant
offset or bias ( ), a moving or walking bias (b ) and a white sensor noise ( ) can be
written as
x
a
&&
x&&
w
x
c
&& x&& accel
8
accelxxx
wbcxa +++=
&&&&&&
&&
(2.7)
The accelerometer sensor noise is assumed to be normally distributed with
zero mean. The bias is modeled as a first order Markov process as shown in the gyro
model where the noise is also normally distributed with zero mean and the sampled
covariance [4]
accel
w
[ ]
saccelaccel
fwE
22
?= (2.8)
[]
x
accels
accel
bias
bias
f
wE
&&
?
?
2
2
2
= (2.9)
2.3 Random Walk Evaluation
Figure 2.1 is a plot of a fourteen hour static run for a KVH-5000 rate gyro. The
top plot is the actual sensor output. The bottom plot is a filtered version of the sensor
output. The moving bias can partially be seen in the filtered inertial sensors data. For
more thorough and theoretical treatment of the filtering methodology, see reference
[Demoz].
9
Figure 2.1 KVH-5000 Experimental data (top) and filtered experimental data (bottom)
An Allan variance chart is used to determine the statistics of the wide band noise
present in a sensor. This noise is commonly referred to as the random walk of a sensor
and is represented by the in the model given in Equation (2.2) above. Allan variance
charts have been used extensively to characterize noise and bias drift of inertial sensors
[Allan]. Figure 2.2 shows the Allan variance of the raw unfiltered experimental data
collected on the KVH-5000 rate gyroscope.
w
10
Figure 2.2.
Allan variance chart for the experimental data of the KVH-5000
The Allan variance chart provides a plot of the gyros Allan variance ( )
av
??
versus the time averaging blocks
av
? . The angular random walk can be found at the
intersection of
gyro
w
1=
av
? . From the Allan Variance chart in Figure 2.2, the angular random
walk of the KVH-5000?s experimental data is found to be 8 deg/sec. The
slope of the data in the chart determines the error mechanism that dominates the data.
For this data the slope is near - ? , which is characteristic of wide-band noise [4]. This
indicates that this data is dominated by the random walk statistics, and all other error
statistics are nearly negligible.
gyro
w
4
10
?
?5.
11
Shown in Figure 2.3 is a plot of a 3 hour static run for a Systron-Donner AQRS-
104 rate gyro. The top plot is the actual sensor output while the bottom plot is a filtered
version of the sensor output. Unlike the KVH-5000 static data, a moving bias can be
seen in both the unfiltered data and in the filtered data.
Figure 2.3 AQRS-104 Experimental Data Filtered and Unfiltered
Observe in Figure 2.4 that the angular random walk for the AQRS-104 is
equal to deg/sec. Observe that the curve in Figure 2.4 changes slope from ? ?
to + ? at approximately
gyro
w
2
101.2
?
?
10=
av
? seconds. The initial slope of the curve is approximately
12
- ? , which is characteristic of wide-band noise. The slope of the region after
av
? is equal
to ten seconds is approximately + ? , which is characteristic of exponentially correlated
noise, or a first order Markov Process.
Figure 2.4
Allan Variance Chart for the Unfiltered Experimental Data of the AQRS-104
13
2.4 Walking Bias Evaluation
From the Allan Variance chart in Figure 2.4, it is clear that the effect of the
exponentially correlated noise begins to dominate other sensor errors after a time constant
of 10 seconds. This information is good, but in order to model the sensor accurately the
time constant of the gyros walking bias (b ) must be evaluated with much more
precision. This is done with an autocorrelation function. Appendix D contains a detailed
description of the autocorrelation function. Figure 2.5 shows the autocorrelation function
of the AQRS-104 filtered experimental data. Since the gyros walking bias (b ) is
modeled as a first order Markov process, the time constant (
r
r
r
? ) of the process can be
determined by picking the value of the autocorrelation function after it decays to a point
36.8% of its point of origin, which is the defining point for a first order differential
equations time constant. For this sensor,
r
? has a value of 1171 seconds.
14
Figure 2.5 AQRS-104 Experimental Autocorrelation Plot
In order to model the gyro?s walking bias (b ) as a Markov process, the variance
of the input noise ( ) must be known as well as the time constant (
r
bias
gyro
w
r
? ). This is
obtained by using the filtered experimental data and calculating its standard deviation.
This standard deviation is considered to be
bias
gyro
? . Calculate the variance of by
using the following equation.
bias
gyro
w
[]
r
gyros
gyro
bias
bias
f
wE
?
?
2
2
2
= (2.10)
15
2.5 Constant Bias Evaluation
The constant offset bias of a sensor is simply the mean of the sensor output. It is
important that the data used to calculate the constant offset bias be of a sufficient length
to ensure that the actual sensor measurement has reached a steady state output. When the
sensor has reached a steady state output, the calculated mean will be consistent with a
constant offset bias of the actual sensor measurement. This may be observed in the
Systron-Donner and KVH rate gyro measurements in Figures 2.1 and 2.3.
2.6 Rate Gyro Model Validation
Figure 2.6 is a comparison of the Allan Variance Charts for simulated data of a
KVH-5000 rate gyro and the experimental data shown previously in Figure 2.2. The
simulated data was created by using the simple sensor model presented in Equation (2.1).
The initial numerical values used for the model error sources are found in the
manufactures specification sheet [Bennett]. It is possible to obtain all of the values
needed to model the sensor from the manufactures specification sheet because the sensor
is dominated by wide-band noise.
16
Figure 2.6
Allan Variance Comparison I of the KVH 5000 Experimental and Simulated Data
A comparison of experimental data to simulated data is given in Figure 2.6.
Observe that the angular random walk for the experimental data and simulated data do
not match. The angular random walk of the simulated data is 1 deg/sec and the
angular random walk of the experimental data is 8 deg/sec. Figure 2.7 is a
comparison of the Allan Variance charts after the angular random walk of the simulated
data was set to the value of the experimental data.
3
103.
?
?
4
105.
?
?
17
Figure 2.7
Allan Variance Comparison II of the KVH 5000 Experimental and Simulated Data
In Figure 2.8 a comparison of the Systron-Donner AQRS-104 Allan Variance
Charts is shown. The experimental data used is the same as presented in Figure 2.4, and
simulated data was produced by using the sensor model presented in Equation (2.1). The
angular random walk and bias drift statistics for the model errors were found in the
manufactures specification sheet [BEI]. The time constant of the Markov Process was
found in previous work [Demoz]. The published angular random walk and bias drift for
this sensor are 0.025 deg/sec and 0.05 deg/sec, respectively. The angular random walk
18
and bias drift of the experimental data are 0.02373 deg/sec and 0.054 deg/sec,
respectively.
Figure 2.8
Allan Variance Comparison of the AQRS-104 Experimental and Simulated Data
The time constant of the sensor is published to be 1000 seconds. Figure 2.9 is a
comparison of a decaying exponential curve with a time constant of 1000 seconds and the
autocorrelation of the experimental data. The experimental data decays reasonable close
to the exponential curve envelope. The actual time constant of the experimental data is
1171 seconds. The discrepancies in the slopes of the Allan variances are due to the fact
19
that the experimental data contains many more error sources that contribute to the sensors
output. Recall that the simple model provided in this chapter only accounts for two error
sources, wide-band noise and exponential correlated noise modeled as a 1
st
Order Markov
Process. Therefore, the possibility of exact slope alignment is rare. However, the fact
that the angular random walk values, the bias drift, and bias time constants values are
very close provides some level of confidence that the simulated data is characteristic of
the experimental data.
Figure 2.9
AQRS-104 Autocorrelation Comparison of the Filtered Experimental Data
20
2.7 Accelerometer Model Validation
The same type of data and algorithms used to validate the rate gyros can be used
to validate the accelerometer model. Figure 2.10 is static data from a Humphrey
accelerometer. The top plot is raw unfiltered data. The bottom plot is the same data
filtered through a Butterworth filter.
Figure 2.10 Humphrey Accelerometer Actual Data filtered and Unfiltered Data
In Figure 2.11 the autocorrelation plot of the filtered data is shown. From the
autocorrelation, the time constant of the moving bias can be estimated to be 700 seconds.
21
The standard deviation of the moving bias is estimated to be 8 g?s. Using an
Allan variance chart, the wide-band noise is estimated to be
5
105.
?
?
6
105
?
?.9
Hz
sg'
. Figure
2.12 is a comparison of the Allan variance charts using the experimental and simulated
data. The simulated data was produced by using the estimated values for the time
constant, bias standard deviation and random walk. From the figure, it is clear that the
wide-band noise is modeled correctly for a period of time. Unfortunately, the point at
which the exponentially correlated errors become dominate is not as defined in the
simulated data as it is in the experimental data. This validation is similar to that of the
rate gyro model. Even with the discrepancies, the analytical model?s accuracy is
sufficient enough to describe the dominate errors seen in the actual data.
22
Figure 2.11
Humphrey Accelerometer Autocorrelation of the Filtered Experimental Data
23
Figure 2.12
Allan Variance Comparison of the Humphrey Accelerometer Experimental and
Simulated Data
2.8 Sensor Categorization
Table 2.1 lists three sample accelerometers and their respective sensor model
characteristic values. Table 2.3 lists seven sample rate gyros and their respective sensor
model characteristic values. The various sensor characteristics were grouped into three
categories [Demoz]: consumer, automotive and tactical grade. Table 2.2 and Table 2.4
show the characteristic values for each grade. Representative values from the actual
24
sensors in Table 2.I and Table 2.III can be compared to the values in Table 2.II and Table
2.IV to categorize a specific sensor.
Table 2.1 Accelerometer Characteristics Used for this Analysis
Accelerometer Attribute Units Specification
LN-200 IMU
White noise Micro g/?Hz 50
Scale Factor Stability ppm 300
Bias Variation micro-g 50
Rate ?g 40
HG-1700 IMU
Random walk Micro-g/?Hz 30
Scale Factor Stability ppm 300
Bias Variation Micro-g 50.00
Rate ?g 50
HG-1900 IMU
Random walk Micro-g/?Hz 30
Scale Factor Stability ppm 300
Bias Variation Micro-g 1
Rate ?g 70
25
Table 2.2 Classification Characteristics Used to Categorize Accelerometers
Accelerometer Attribute Units Specification
Consumer
Random Walk g/?Hz .003
Bias Time Constant sec 100
Bias Variation g
3
104.2
?
?
Automotive
Random Walk g/?Hz .001
Bias Time Constant sec 100
Bias Variation g
3
102.1
?
?
Tactical
Random walk g/?Hz .0005
Bias Time Constant sec 60
Bias Variation g
5
1050
?
?
26
Table 2.3 Gyro Characteristics Used for this Analysis
Gyro Attribute Units Specification
LN-200 IMU
Random walk ?/?hr 0.04 - 0.10
Scale Factor Stability ppm 100
Bias Variation ?/hr 0.35
Rate ??/sec 1000
HR-1700 IMU
Random walk ?/?hr 0.125 - 0.300
Scale Factor Stability ppm 150
Bias Variation ?/hr 2.00
Rate ??/sec 1000
HG-1900 IMU
Random walk ?/?hr 0.1
Scale Factor Stability ppm 150
Bias Variation ?/hr 0.3 - 10.0
Rate ??/sec 1000
KVH-5000
Random walk ?/?hr 0.083
Scale Factor Stability
(?200?/sec) ppm 500
27
Scale Factor Stability
(?500?/sec) ppm 1000
Bias Variation ?/hr 1.00
Rate ??/sec 500
Autogyro Navigator
Random walk ?/?hr 0.33
Scale Factor Stability % rms 0.5
Bias Variation ?/hr 90.00
Rate ?/sec 100
Systron-Donner
AQRS-000640-104
Random walk ?/sec/?Hz ?0.025
Scale Factor Stability ?% 3
Bias Variation ?/hr ?180.00
Rate ??/sec 64
Systron-Donner
AQRS-000640-109
Random walk ?/sec/?Hz ?0.025
Scale Factor Stability ?% 5
Bias Variation ?/hr ?180.00
Rate ??/sec 75
28
Table 2.4 Classification Characteristics Used to Categorize Rate Gyros
Rate Gyro Attribute Units Specification
Consumer
Random walk ?/sec/?Hz .05
Bias Time Constant sec 300
Bias Variation ?/hr 360
Automotive
Random walk ?/sec/?Hz .05
Bias Time Constant sec 300
Bias Variation ?/hr 180
Tactical
Random walk ?/sec/?Hz .0017
Bias Time Constant sec 100
Bias Variation ?/hr 0.35
29
2.9 Summary and Conclusions
In this chapter, a simple sensor model was laid out that incorporated a moving
bias term, a constant bias term, and a random error term. The biases and random error
sources were analyzed statistically. The numerical values for the time constants, means,
and variances were obtained by using common statistical methods, Allan Variance charts,
Autocorrelation functions, and Monte Carlo Simulations. The statistical parameters of the
error sources were used to categorize and characterize the sensors. The Inertial sensors
were categorized into four basic grades: navigational, tactical, automotive, and consumer.
Several sensor grades were simulated and experimentally validated. The simulated
sensors will be used in later chapters to determine resulting levels of performance that
can be achieved when combined with GPS. A group of tables was provided which
contained the specification for a number of commonly used rate gyros and
accelerometers. Additional tables were provided which are used to classify the sensors.
30
CHAPTER 3
INTEGRATION ERRORS IN A TWO DEGREE OF FREEDOM
INERTIAL MEASUREMENT UNIT
3.1 Introduction
In this chapter the errors associated with the numerical integration of a single rate
gyro and single accelerometers, to calculate position, velocity and heading in a planar
setting, are presented. The error growth of the position, velocity, and heading calculation
are depicted with the use of a Monte Carlo Simulation. Both consumer and tactical grade
sensor characteristics are compared and experimental data is used to validate the bounds
of the Monte Carlo simulations. The Monte Carlo simulations are validated by showing
the error growths of rate gyros and accelerometers, which contain moving biases, and
comparing these error growths to simulations where the only error source is wide-band
noise. Analytical equations for position, velocity and heading errors are derived that plot
the error growths depicted by the integration of rate gyros and accelerometers, which
contain only one error source, wide-band noise. Finally the effects a gravity field as on
the error growths of the position, velocity and heading calculations are shown. The errors
resulting form the gravity field are shown by a comparison. The comparison contains the
error growths when there is a gravity field in the system and the error growths when there
is not a gravity field in the system.
31
3.2 IMU Integration Errors
The heading of the platform must first be calculated in order to determine the
position of a 2 DOF IMU. This is accomplished by numerically integrating the
gyroscope yaw rate measurement. The method chosen to do this is the Runge-Kutta
Method [Burden]. This method?s procedure is laid out below in the state space format.
The following equations are the state space matrices used to calculate platform heading.
[ ]0=A [ ]1=B
(3.1)
[ ] [ ]?? &BAf
heading
+=
(3.2)
A Runge-Kutta order four integration routine is used to propagate the heading
state ? forward to the next time step. The routine is as follows
()
()
()
622
,
,2
,2
,
0
43211
34
23
12
1
0
kkkk
kfTk
kfTk
kfTk
fTk
ii
iiheadings
iiheadings
iiheadings
iiheadings
++++=
+=
+=
+=
=
=
+
??
??
??
??
??
?
&
&
&
&
(3.3)
A yaw angle (? ) of zero corresponds to due north. Therefore any positive angle
change is a rotation in the eastwardly direction. Likewise any negative angle change is a
rotation in the westward direction.
32
After the yaw angle of the platform is calculated, the position and velocity of the
platform must be calculated by integrating the accelerometer measurements. The Runge-
Kutta method was also used for this calculation. The state space matrices required to
calculate position and velocity are given below.
?
?
?
?
?
?
?
?
?
?
?
?
=
0000
0000
1000
0100
A
()
()
?
?
?
?
?
?
?
?
?
?
?
?
=
?
?
sin
cos
0
0
B
(3.4)
[ ] [ ]xBstatesAf
positon
&&+=
(3.5)
?
?
?
?
?
?
?
?
?
?
?
?
=
y
x
y
x
states
&
&
(3.6)
Again, a Runga-Kutta order four integration routine shown below is used to
propagate the position and velocity states forward.
()
()
()622
,,
,,2
,,2
,,
0
43211
34
23
12
1
0
kkkkstatesstates
xkstatesfTk
xkstatesfTk
xkstatesfTk
xstatesfTk
states
ii
iiipositons
iiipositons
iiipositons
iiipositons
++++=
+=
+=
+=
=
=
+
&&
&&
&&
&&
?
?
?
?
(3.7)
33
The initialization of the states vector to zero is convenient for this analysis.
However, if this were done in practice, the initial longitude and latitude of the vehicle
platform would need to be recorded. With the initial longitude and latitude recorded, it
would be possible to convert the position calculations into longitude and latitude
Figures 3.1-3.6 are results from a Monte Carlo simulation when the above
integration is performed with the constant bias removed from the static sensor?s
measurements. The results for a tactical grade IMU are presented in Figures 3.1-3.3.
The results for a consumer grade IMU are presented in Figures 3.4-3.6
Figure 3.1 Tactical Grade IMU Position Error Bounds (Static)
34
Figure 3.2 Tactical Grade IMU Velocity Error Bounds (Static)
35
Figure 3.3 Tactical Grade IMU Yaw Angle Error Bounds (Static)
36
Figure 3.4 Consumer Grade IMU Position Error Bounds (Static)
37
Figure 3.5 Consumer Grade IMU Velocity Error Bounds (Static)
38
Figure 3.6 Consumer Grade IMU Yaw Angle Error Bounds (Static)
From the figures it is clear that the errors on the calculated position, velocity, and
yaw angle do not stay constant. In fact, they appear to grow at an exponential rate.
When a consumer grade IMU is used to calculate the north position, the error after a
twenty second period has a standard deviation of nearly one meter. An additional ten
seconds brings the position calculation standard deviation to nearly four meters. A
tactical grade IMU has similar error characteristics; however, the error is much lower in
magnitude. After a 20 second period, the standard deviation in the north position
calculation is close to 0.05 meters. An additional ten seconds brings the standard
deviation to 0.10 meters. The difference in the errors between the tactical and consumer
39
grade IMU?s is significant. There is a 1900% improvement in the northerly position
calculation when a tactical grade IMU is used. Although this improvement is great, it is
accompanied by significant increase in price. A tactical grade IMU costs in the
neighborhood of $20K where the consumer grade IMU costs less than $1000.
3.3 Monte Carlo Validation
In this section the Monte Carlo simulations used in the previous section are
validated by showing the error growths of rate gyros and accelerometers which contain
moving biases and comparing them to Monte Carlo simulations where the only error
source is wide-band noise. Figure 3.7 is a plot of the error bounds for the calculated yaw
angle of a KVH-5000 rate gyro. Two error bounds are presented, one is derived from the
Chapter 2 sensor model with a Monte Carlo simulation, and the other is derived from the
equation below. The equation below is the analytical solution for the standard deviation
of integrated wide band noise. A full derivation of the below equation can be found in
reference [Bevly].
kT
sw
?? = (3.8)
[ ]
w
Nw ?,0~
(3.9)
40
The KVH was chosen for this comparison because it is dominated by wide-band
noise. Therefore the results from a Monte Carlo simulations produced from its
specifications should match the results from the analytical solution reasonably well. It is
clear from Figure 3.7 that after a period of time the error bounds of the Monte Carlo
simulation begin to grow faster than the error bounds of the analytical equaiton. This is
caused by the moving bias modeled in the sensor model.
Figure 3.7 KVH Error Bound Comparison I
Figure 3.8 is also a comparison of the error bounds; however, now the moving
bias has been removed from the sensor model. From the comparison, it is clear that the
41
Monte Carlo results match Equation (3.8). Because of this, it is reasonable to assume that
the bounds derived from the Monte Carlo simulation with all errors present are valid. To
help further this validation, the data form the KVH-5000 rate gyro presented in Chapter 2
will be used as well as data produced by the sensor model. Figure 3.9 is the calculated
heading from that experimental data presented in Figure 2.1 and simulated data produced
using the simple sensor model. As seen in Figure 3.9, both data sets fall well within the
bounds resulting from the Monte Carlo simulations.
Figure 3.8 KVH Error Bound Comparison II
42
Figure 3.9 KVH Error Bound Comparison III
3.4 Longitudinal Velocity Error Equations
In the above section 3.3 it was shown that the moving bias definitely plays a key
role in the heading error growth. To determine if the effects are similar in the position
and velocity error growths analytical equation are derived. The derivations are for the
analytical error growth of position and velocity errors due only to the integration of the
wide band noise in the IMU.
To begin the derivations the bias of the accelerometer is assumed to be negligible.
Therefore, the accelerometer measurement becomes
43
accelx
wxa += &&
&&
(3.10)
where is uncorrelated wide band noise of the accelerometer.
accel
w
Integration of the accelerometer times the heading angle to get longitudinal
velocity (V ) results in
North
( ) ( )dtwxVV
e
accel
e
NorthNorth
?? ++=+
?
cos&&
(3.11)
Assume the mean acceleration and heading are equal to zero to obtain
0
0
0
=
=
=
North
V
a
?
(3.12)
Therefore, the longitudinal velocity error (V ) is due to the integration of the
accelerometer?s wide band noise time heading error.
e
North
( ) ( )dtwV
e
accel
e
North
?cos
?
=
(3.13)
An Euler integration of the accelerometer output results in,
()
?
=
=+?+=
+
k
k
accelsaccels
e
North
e
accels
e
North
e
North
kkkkkk
wTwTVwTVV
1
cos
1
?
(3.14)
Squaring and take the expected value of both equations results in
()[]
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
??
==
T
k
k
accels
k
k
accels
T
e
North
e
North
kk
wTwTEVVE
11
(3.15)
Simplify to get the following
()[] ()[ ]
?
=
=
k
k
T
accelaccels
T
e
North
e
North
kk
wwETVVE
1
2
(3.16)
Substitute in the covariance of the accelerometer wide band noise to obtain
44
( )[ ] tkTkTVVE
sws
T
e
North
e
North
accel
==
22
?
(3.17)
The above equations yield the following approximation for the longitudinal velocity error
growth due to the integration of the yaw gyroscopes and accelerometers noises.
( )[ ]
2
accel
ws
T
e
North
e
North
tTVVE ?=
(3.18)
Simplify to solve for the standard deviation of the longitudinal velocity.
tT
swV
accelNorth
?? =
(3.19)
3.5 Longitudinal Position Error Equations
The integration of the longitudinal velocity results in the longitudinal position.
( )dtVVPP
e
NorthNorth
e
NorthNorth
?
+=+
(3.20)
Assume the mean longitudinal velocity and position are zero.
0
0
=
=
North
North
V
P
(3.21)
Therefore, the longitudinal position error ( ) is due to the integration of the
longitudinal velocity error (V ).
e
North
P
e
North
dtVP
e
North
e
North
?
=
(3.22)
Again, an Euler integration method is used and results in
?
=
=+=
+
k
k
e
Norths
e
Norths
e
North
e
North
kkkk
VTVTPP
1
1
(3.23)
Squaring and take the expected value of both equations results in,
45
()[]
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
??
==
T
k
k
e
Norths
k
k
e
Norths
T
e
North
e
North
kk
VTVTEPPE
11
(3.24)
Simplify to get the following.
()[] ()[ ]
??
==
==
k
k
Vs
k
k
T
e
North
e
Norths
T
e
North
e
North
kTVVETPPE
Northkk
1
22
1
2
?
(3.25)
From the above section 3.4 the covariance of the longitudinal velocity was found to be,
kT
accelNorth
wsV
222
?? =
(3.26)
Therefore, substituting in this for the longitudinal velocity covariance results in the
following.
()[]
?
?
?
?
?
?
++==
?
=
kkkTkTPPE
accelaccel
ws
k
k
ws
T
e
North
e
North
6
1
2
1
3
1
2324
1
224
??
(3.27)
The above equations result in the following approximation for the longitudinal position
error growth due to the integration of the longitudinal velocity error:
()[]
23243
3
1
3
1
accelaccel
wsws
T
e
North
e
North
TtTkPPE ?? =? (3.28)
Simplify to solve for the standard deviation of the Northerly Position
3
3
1
tT
swP
accelNorth
?? = (3.29)
3.6 Lateral Velocity Error Equations
As in the longitudinal case the bias of the accelerometer will be assumed to be
negligible in the accelerometer measurement. Because of this the following equation is
derived:
46
accelx
wxa += &&
&&
(3.30)
where is uncorrelated wide band noise of the accelerometer. Integration of the
accelerometer times the heading angle to get lateral velocity (V ) yeilds
accel
w
East
( ) ( )
e
accel
e
EastEast
wxVV ?? ++=+
?
sin&&
(3.31)
Assume the mean acceleration and heading are equal to zero, i.e
0
0
0
=
=
=
North
V
a
?
(3.32)
Then, the lateral velocity error (V ) is due to the integration of the accelerometer?s
wide band noise time heading error.
e
East
( ) ( )
e
accel
e
East
wV ?sin
?
=
(3.33)
An Euler integration of the accelerometer output results in
()
?
=
=+?+=
+
k
k
e
kaccels
e
kaccels
e
East
e
accels
e
East
e
East
kkkkkk
wTwTVwTVV
1
sin
1
???
(3.34)
Squaring and take the expected value of both equations results in,
()[]
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
??
==
T
k
k
e
kaccels
k
k
e
kaccels
T
e
East
e
East
kk
wTwTEVVE
11
??
(3.35)
Knowing that the expected value of the square of the accelerometer noise is simply the
covariance of the accelerometer wide-band noise ( ) and that the gyroscope wide-
band noise is uncorrelated to the accelerometer wide-band noise the lateral variance can
be simplified to the following.
2
accel
w
?
47
()[] ()[ ] ()[ ]
T
e
k
e
k
k
k
T
accelaccels
T
e
East
e
East
EwwETVVE
kk
??
?
=
=
1
2
(3.36)
()[] ()[ ]
T
e
k
e
k
k
k
ws
T
e
East
e
East
ETVVE
accel
???
?
=
=
1
22
(3.37)
From reference [Bevly] and the above section 3.5:
( )[ ]
222
gyro
ws
T
e
k
e
k
kTE ????
?
== (3.38)
Therefore,
()[] (kkTkTkTVVE
gyroaccelgyroaccelgyroaccel
wws
k
k
wws
k
k
wws
T
e
East
e
East
+===
??
==
2224
1
224
1
224
2
1
?????? )
(3.39)
tkTkT
swws
gyroaccel
=?
2224
2
1
?? (3.40)
The above equations result in the following approximation for the lateral velocity error
growth due to the integration of the yaw gyroscopes and accelerometers noises:
( )[]
2222
2
1
gyroaccel
wws
T
e
East
e
East
tTVVE ??= (3.41)
Simplify to solve for the standard deviation of the lateral velocity,
2
1
tT
swwV
gyroaccelEast
??? = (3.42)
3.7 Lateral Position Error Equations
The integration of the lateral velocity results in the lateral position.
48
( )dtVVPP
e
EastEast
e
EastEast
?
+=+
(3.43)
Assume the mean lateral velocity and position are zero,
0
0
=
=
East
East
V
P
(3.44)
So that, the longitudinal position error ( ) is due to the integration of the longitudinal
velocity error (V ), i.e.
e
East
P
e
East
dtVP
e
East
e
East
?
=
(3.45)
Again, an Euler integration method is used and results in,
?
=
=+=
+
k
k
e
Easts
e
Easts
e
East
e
East
kkkk
VTVTPP
1
1
(3.46)
Squaring and take the expected value of both equations results in,
()[]
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
??
==
T
k
k
e
Easts
k
k
e
Easts
T
e
East
e
East
kk
VTVTEPPE
11
(3.47)
Simplify the position variance to get the following
()[] ()[ ]
??
==
==
k
k
Easts
k
k
T
e
East
e
Easts
T
e
East
e
East
kTVVETPPE
kk
1
22
1
2
?
(3.48)
From the above section the covariance of the longitudinal velocity was found to be,
22242
2
1
kT
gyroaccelEast
wwsV
??? ? (3.49)
Therefore substituting in this for the longitudinal velocity covariance results in,
()[]
?
?
?
?
?
?
++==
?
=
234226
1
3226
4
1
2
1
4
1
2
1
2
1
kkkTkTPPE
gyroaccelgyroaccel
wws
k
k
wws
T
e
East
e
East
????
(3.50)
49
The above equations result in the following approximation for the longitudinal position
error growth due to the integration of the longitudinal velocity error:
()[ ]
22242264
8
1
8
1
gyroaccelgyroaccel
wwswws
T
e
East
e
East
TtTkPPE ???? =? (3.51)
Simplify to solve for the standard deviation of the Northerly Position
8
1
2
tT
swwP
gyroaccelEast
??? = (3.52)
3.8 Bias Contribution in the IMU Integration Error
The equations for the position and velocity errors provided in sections 3.4 -3.7
make it possible to evaluate the effect a moving bias has on the error growth. Figures
3.10 and 3.11 are a comparison of these analytical equations and the position and velocity
error growths from a Monte Carlo simulation. The Monte Carlo simulation was done
with the simple model in chapter 2 using Tactical Grade IMU characteristics.
50
Figure 3.10 Comparison of a Tactical Grade IMU Position Error Bounds (Static)
51
Figure 3.11 Comparison of a Tactical Grade IMU Velocity Error Bounds (Static)
The error growth rates in the curves of the Monte Carlo simulations, which
contain both the wide band noise and bias errors, are much faster than the growth rates of
the curves which only contain the wide band noise error source. This is confirmation that
the moving bias, even in the best of sensors, plays a key roll in error growth over time.
Figures 3.12 and 3.13 contain curves of the difference between the error bound equations
derived in sections 3.4-3.7 above and the error bounds which resulted from the Monte
Carlo simulations, which contain all of the error sources.
52
Figure 3.12 Effect of Tactical Grade IMU Biases on Position Error (Static)
53
Figure 3.13 Effect of Tactical Grade IMU Biases on Velocity Error (Static)
54
3.9 Gravitational Effects
The navigation scenario laid out in section 3.1 is one which involves planar
motion in the East-North coordinates. In some cases it is necessary to measure the
distance traveled in the North-Up coordinates. When it is necessary navigate up the
accelerometer is affected by the gravitational field of the earth. This introduces an
additional error in the upward acceleration measurement because of errors in the pitch
angle ? calculation. Below are the state space matrices modified to account for the
gravitational effects on the accelerometer.
The state space gravity input matrix is:
()
?
?
?
?
?
?
?
?
?
?
?
?
=
?sin
0
0
0
G
(3.53)
Where,
2
80665.9
s
m
g = (3.54)
The position calculation then becomes
[ ] [ ] [ ]gGxBstatesAf
positon
?+= &&
(3.55)
Figures 3.14 and 3.15 are the position and velocity bounds when tactical grade
IMU characteristics are used. Figures 3.16 and 3.17 are the position and velocity bounds
when consumer grade IMU characteristics are used
.
55
Figure 3.14 Tactical Grade IMU Position Error Bounds with Gravity Field
56
Figure 3.15 Tactical Grade IMU Velocity Error Bounds with Gravity Field
57
Figure 3.16 Consumer Grade IMU Position Error Bounds with Gravity Field
58
Figure 3.17 Consumer Grade IMU Velocity Error Bounds with Gravity Field
59
3.10 Summary and Conclusions
In this chapter the errors associated with the integration of a single rate gyro and
single accelerometers to calculate position, velocity and heading in a planar setting were
discussed. The error growths of the position, velocity, and heading calculation were
depicted with the use of a Monte Carlo Simulation. Both consumer and tactical grade
sensor characteristics were compared and experimental data was used to validate the
bounds of the Monte Carlo simulations. The Monte Carlo simulations were validated by
showing the error growths of rate gyros and accelerometers which contain moving biases,
and comparing them to simulations where the only error source was wide-band noise.
Analytical equations for position, velocity and heading errors were derived which plot the
error growths depicted by the integration of rate gyros and accelerometers, which contain
only one error source, wide-band noise. Finally, a gravity field was introduced. The error
growths of the position and velocity calculations were compared with a gravity field and
without.
60
CHAPTER 4
INTEGRATION ERROR CORRECTIONS FOR A TWO DEGREE
OF FREEDOM INERTIAL MEASUREMENT UNIT
4.1 Introduction
A Kalman Filter will be implemented to combine the GPS position, velocity and
heading measurements with the inertial sensor measurements. The Kalman filter will
incorporate the sensor model presented in Chapter 2. The coupling of the GPS
measurements and the sensor model to calculate the position, velocity and heading from
the combined GPS and IMU measurements will result in new error bounds for position,
velocity and heading. The Kalman Filter will also provide an estimation of the biases
associated with each sensor. Experimental data will be used to validate the bias and error
bound estimations. This will be useful when designing controllers that need a better
estimation of position, velocity and heading than a normally received from an integrated
IMU measurement.
4.2 Heading Error Reduction with a Two State Kalman Filter
The position, velocity and heading calculations from an integrated IMU contain
several errors, as shown in the previous chapters. The goal of this chapter is to reduce
61
these errors with the aid of the Global Positioning System. GPS returns a calculated
position, velocity and heading that is very precise. However, in order to preserver the
precision of GPS, the messages are typically restricted to an output frequency of 1-10 Hz.
This is not a very desirable output rate when precise and high update rate navigation is
required. To increase the output rate, the GPS and an IMU can be coupled together with
a Kalman filter.
A Kalman filter is a state estimator that uses the knowledge of the input error
sources to assist in estimating the true state through time. A Kalman filter is a good
coupling tool to use in the above scenarios due to the fact that the GPS measurements are
unbiased, but are received at a very low frequency, and the IMU measurements are
relatively imprecise and can be received at relatively high frequencies. For a more
detailed description of a Kalman filter refer to Appendix C.
Below is the first state space navigation model that is commonly implemented in
a Kalman filter to estimate heading. This two state model allows the Kalman filter to
estimate the heading of the platform and approximates the bias in the rate gyro
measurement as a constant. Note that the actual variance on the bias is not zero. If it
were zero the bias state would be modeled as a constant. Since there is variance in place,
the bias state is actually modeled as a random walk. This is to keep the Kalman filter
from ?going to sleep?. The Kalman filter is considered to be ?asleep? when the gains are
unchangeable. In ether case, the bias estimate will remain as a constant when no GPS
measurement is available.
2-State Kalman filter state space matrices:
62
?
?
?
?
?
?
=
tcons
bias
x
tan
?
(4.1)
?
?
?
?
?
? ?
=
00
10
kal
A
(4.2)
?
?
?
?
?
?
=
0
1
kal
B
(4.3)
[ ]01=
on
GPS
kal
C
(4.4)
[ ]00=
off
GPS
kal
C
(4.5)
[ ]0=
kal
D
(4.6)
The following noise covariance matrices were used to calculate the Kalman filter
gains.
?
?
?
?
?
?
=
10
01
w
B
(4.7)
?
?
?
?
?
?
?
?
=
00
0
2
s
noise
w
fQ
?
(4.8)
[ ]
2
GPS
R ?= (4.9)
82
10
?
=
const
bias
?
Below is the second state space navigation model that is commonly implemented
in a Kalman filter to estimate heading. This two state model allows the Kalman filter to
estimate the heading of the platform and approximates the bias in the rate gyro
measurement as a first order Markov process. Note that the bias estimate will tend to
zero when there is no GPS measurement available. This is because the model assumes
63
the Markov process is zero mean. Therefore, the best estimate is that the bias is zero over
all time if no other information is available.
2-State Kalman filter state space matrices:
?
?
?
?
?
?
=
walking
bias
x
?
(4.10)
?
?
?
?
?
?
?
?
?
?
=
?
1
0
10
kal
A
(4.11)
?
?
?
?
?
?
=
0
1
kal
B
(4.12)
[ ]01=
on
GPS
kal
C
(4.13)
[ ]00=
off
GPS
kal
C
(4.14)
[ ]0=
kal
D
(4.15)
The following noise covariance matrices were used to calculate the Kalman filter
gains.
?
?
?
?
?
?
=
10
01
w
B
(4.16)
?
?
?
?
?
?
?
?
?
?
?
?
=
?
?
?
2
2
2
0
0
biass
s
noise
w
f
f
Q
(4.17)
[ ]
2
GPS
R ?= (4.18)
The selection of each of these models is important before setting up a Kalman
filter. However, the effects are only seen during GPS outages and will be discussed in
the following section.
64
The following figures are a comparison of the error bounds when the 2-State
Kalman Filter is used versus when the rate gyro is integrated using Rugge-Kutta order
four. Figure 4.1 is a plot of the results from a consumer grade rate gyro. Figure 4.2 is a
plot of the results from a Tactical grade rate gyro.
Figure 4.1
Consumer Grade Heading Error Comparison between the 2 state Kalman Filter Model
and Rugge-Kutta Order Four.
As seen in Figure 4.1, coupling the rate gyro with GPS bounds the error. For a
consumer grade rate gyro, the Kalman filter reaches steady state around 15 seconds.
65
Once the Kalman filter has reached steady state, the heading error is bound between 0.1
degrees and 0.2 degrees.
Figure 4.2
Tactical Grade Heading Error Comparison between the 2 state Kalman Filter Model and
Rugge-Kutta Order Four.
For a tactical grade rate gyro, the Kalman filter takes much longer, approximately
700 seconds, to reach steady state. However, once the Kalman filter reaches steady state,
the heading error is bound between .015 degrees and .02 degrees.
66
4.3 Heading Errors during a GPS Outage
Since it has been shown that the errors of an inertial sensor can be bound by
coupling them with GPS, the effects of a GPS outage are now investigated. The first case
illustrates what happens during a GPS outage while modeling the bias as a first order
Markov process using a consumer grade sensor. Figure 4.3 shows the heading and bias
states of the Kalman filter. GPS is denied to the Kalman filter at seconds, this is
known as a GPS outage. As seen in Figure 4.3, once the outage begins, the bias state
converges to zero. This is expected since the bias is being modeled as a first order
Markov process which has a zero mean.
180=t
67
Figure 4.3
State Estimates for a Two State Kalman Filter with the Second State Modeled as a 1
st
Order Markov Process
The second case is a GPS outage with the bias modeled as a constant (random
walk). Figure 4.4 shows the heading and bias states of the Kalman filter. Again a GPS
outage was simulated at 180 seconds. As seen in the figure, the bias state remains
constant after the GPS outage. This is expected since the bias is modeled as a constant.
68
Figure 4.4
State Estimates for a Two State Kalman Filter with the Second State Modeled as a
Random Walk
Two Monte Carlo simulations were performed with a consumer grade gyro to
determine which bias model (constant or Markov) performs better. The first Monte Carlo
simulation assumes a moving bias dominates the sensor measurement. This simulation
used a constant bias of zero. The second Monte Carlo simulation assumes a constant bias
of 1 deg/sec dominates the sensor measurement.
As seen in Figure 4.5, the error bounds on the heading estimate grow much faster
when the bias was modeled as a constant. The difference between the error bound
69
estimated by the Kalman filter and the error bound calculated when the bias was modeled
as a Markov process can most likely be contributed to the number of iteration used in the
Monte Carlo simulation. Sixty iterations were used for this simulation because of
computer memory constraints.
Figure 4.5
Error Bound Comparison between the Two State Kalman Filter Models when the
Dominate Bias is not Constant
As seen in Figure 4.6 the error bounds on the heading estimate grows much faster
when the bias is modeled as a Markov process. The difference between the error bound
70
estimated by the Kalman filter and the error bound calculated when the bias was modeled
as a constant is again most likely contributed to the number of iteration used in the Monte
Carlo simulation. Sixty iterations were used because of computer memory constraints.
Figure 4.6
Error Bound Comparison between the Two State Kalman Filter Models when the
Dominate Bias is a Constant
71
4.4 Heading Error Reduction with a Three State Kalman Filter
Accurately modeling a sensor bias with a two state Kalman filter is difficult
because the bias can contain both constant and moving components. To improve the
heading estimate of a Kalman filter, the following three state model was created. With
this model, it is possible to estimate both the moving and constant biases of an inertial
sensor, instead of estimating only one or the other with the two state Kalman filter. Note
that during a GPS outage the constant bias estimate will remain constant, and the moving
bias estimate will tend to zero.
3-State Kalman Filter state space matrices:
?
?
?
?
?
?
?
?
?
?
=
tcons
walking
bias
biasx
tan
?
(4.19)
?
?
?
?
?
?
?
?
?
?
?
??
=
000
0
1
0
110
?
kal
A
(4.20)
?
?
?
?
?
?
?
?
?
?
=
0
0
1
kal
B
(4.21)
[ ]001=
on
GPS
kal
C
(4.22)
[ ]000=
off
GPS
kal
C
(4.23)
[ ]0=
kal
D
(4.24)
72
The following noise covariance matrices were used to calculate the Kalman filter
gains.
?
?
?
?
?
?
?
?
?
?
=
100
010
001
w
B
(4.25)
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
000
0
2
0
00
2
2
?
?
?
moving
biass
s
noise
w
f
f
Q
(4.26)
[ ]
2
GPS
R ?= (4.27)
82
10
?
=
const
bias
?
This three state model allows the Kalman Filter to estimate the heading of the
platform as well as the moving bias and constant bias of the rate gyro. The following
figures are a comparison of the error bounds when a 3-State Kalman Filter is used, a 2
state Kalman Filter is used and a Rugge-Kutta order four integration scheme is used.
Figure 4.7 is a plot of the results from a consumer grade rate gyro. As seen in the figure,
when the inertial sensor is integrated the error bounds continue to grow through time.
When the two state Kalman filter is implemented, the error bounds grow and are
eventually bound between a thresholds. The three state Kalman filter is also able to
bound the errors to the same threshold but not before a time of extreme uncertainty. This
is due to the initialization of the Kalman filter covariance matrix.
73
Figure 4.7
Consumer Grade Heading Error Comparison between the 3 State Kalman Filter Model,
the 2 State Kalman Filter Model and A Rugge-Kutta Order Four Integration Method.
4.5 Kalman Filter Comparison
Two show the utility of the three state Kalman filter, the experimental data taken
form the Systron-Donner AQRS-104 presented in Figure 2.3 will be filtered with each of
the two state Kalman filter models and then the three state Kalman filter. During the
filtering process, a GPS outage was created after 30 minutes. Figure 4.8 is a plot of the
heading and bias states estimated by the Kalman filter when the two state Kalman filter
74
that models the bias as a constant (random walk) is used. As seen in the figure the bias
remains constant after the GPS outage begins. The important thing to notice is at what
value the bias remains. Because the actual bias is not constant but rather moving about
some mean value that is not zero when the outage begins, the Kalman filter holds the bias
state at its last estimated value. Since the value is not exactly equal to the actual bias,
there is an error introduced into the heading estimate.
Figure 4.8
Systron-Donner State Estimates for a Two State Kalman Filter with the Second State
Modeled as a Random Walk
75
The sensor data is now filtered by using the two state Kalman filter model that
models the bias as a first order Markov process. Figure 4.9 is a plot of the heading and
bias states estimated by the Kalman filter. As seen in the figure, the bias estimate begins
to converge to zero after the GPS outage begins. Because the bias estimate converges to
zero and the actual bias is walking about a mean value less than zero degrees per second,
the heading estimate will contain large errors after the GPS outage begins.
Figure 4.9
Systron-Donner State Estimates for a Two State Kalman Filter with the Second State
Modeled as a 1
st
Order Markov Process.
76
This actual sensor is dominated by a bias that contains more than one component,
and the components are of similar magnitudes. The bias has a definite mean value that is
not equal to zero, but this mean value has stability issues. Because of drift issues, the
bias is not always at that mean value. This occurs frequently in consumer grade sensors.
To show the utility of the three state Kalman filter model, the experimental date taken
from the Systron-Donner rate gyro is filtered using the three state Kalman filter. Again a
GPS outage was created after 30 minutes. Figure 4.10 are the Kalman filter states shown
versus time while the three states model was used. As seen in the figure, the filter is able
to correctly estimate the heading, moving bias and constant bias while GPS is in place.
When the GPS outage occurs, the moving bias estimate converges to zero, and the
constant bias estimate remains constant. With the bias states behaving according to their
statistical values, the error in the heading estimate is minimized.
77
Figure 4.10
Systron-Donner State Estimates for a Three State Kalman Filter with the Second State
Modeled as a 1
st
Order Markov Process and the Third State Modeled as a Random Walk.
4.6 Position Estimation with a Six State Kalman Filter
Like the heading calculation, the position calculation can also be improved by
using a Kalman filter. However, because the inertial sensor measurement is acceleration,
the Kalman filter must double integrate to estimate the position. The following is a six
state Kalman filter which estimates the accelerometer moving bias and constant bias, as
well as the velocity and position of the platform.
78
The six state Kalman filter model includes the following states:
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
tcons
walking
y
x
y
x
bias
bias
position
position
velocity
velocity
x
tan
(4.28)
where the state space matrices are,
( ) ( )
() ()
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
??
=
000000
0
1
0000
000010
000001
sinsin0000
coscos0000
?
??
??
gg
gg
A
kal
(4.29)
( )
()
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
0
0
0
0
sin
cos
?
?
g
g
B
kal
(4.30)
?
?
?
?
?
?
?
?
?
?
?
?
=
001000
000100
000010
000001
on
GPS
kal
C
(4.31)
79
?
?
?
?
?
?
?
?
?
?
?
?
=
000000
000000
000000
000000
off
GPS
kal
C
(4.32)
?
?
?
?
?
?
?
?
?
?
?
?
=
0
0
0
0
kal
D
(4.33)
The noise covariance matrices for the system are defined as:
( )
()
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
100
010
000
000
00sin
00cos
?
?
g
g
B
w
(4.34)
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
000
0
2
0
00
2
2
?
?
?
moving
biass
s
noise
w
f
f
Q
(4.35)
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
2
2
2
2
000
000
000
000
GPS
GPS
GPS
GPS
R
?
?
?
?
(4.36)
82
10
?
=
const
bias
?
80
The utility of this six state model is similar to the three state model presented in
section 4.4. This six state Kalman filter model is able to estimate two components in a
bias. These components include a constant offset or mean that is not zero as well as a
moving component.
4.7 Summary and Conclusion
A Kalman Filter was implemented which combined the GPS position, velocity
and heading measurements with the inertial sensor measurements. The Kalman filter
incorporated the sensor model presented in Chapter 2. It was shown that the coupling of
the GPS measurements and the sensor model to calculate the position, velocity and
heading from the combined GPS and IMU measurements resulted in new error bounds
for position, velocity and heading. It was shown that the steady state error bounds were
the same for both the two state Kalman filter and the three state Kalman filter when used
to estimate heading. The two state Kalman filter provided an estimation of the rate gyros
biases. However, the bias had to be either modeled as a random walk or a first order
Markov process. Monte Carlo simulations were used to demonstrate the difficulty in
choosing the model type. The three state Kalman filter provided an estimation of the rate
gyros bias and was able to estimate both a constant component as well as a moving
component of the actual sensor bias. Experimental data was used to validate the bias
estimations in the three state Kalman filter. A five state model was laid out which
estimates two bias components in an accelerometer. These components include a
constant part and a moving part.
81
CHAPTER 5
INTEGRATION ERRORS IN A SIX DEGREE OF FREEDOM
INERTIAL MEASUREMENT UNIT
5.1 Introduction
In this chapter the errors associated with the integration of three rate gyros and
three accelerometers to calculate position, velocity and heading in a 3 dimensional setting
are discussed. First, the model used to simulate the inertial measurement unit (IMU) will
be laid out along with the integration method. The error growth of the position, velocity
and heading calculation is then depicted using Monte Carlo simulations. Additionally,
both a consumer grade and tactical grade inertial measurement unit will be compared
with and without a gravity field in place.
5.2 Six Degree of Freedom Inertial Measurement Unit Model
In Chapter 2, a simple model for a rate gyro and an accelerometer were presented.
This chapter will utilize these models to simulate an inertial measurement unit with six
degrees of freedom. The degrees of freedom include three translational degrees of
freedom (vertical, lateral, and longitudinal) and three rotational degrees of freedom (roll,
pitch, and yaw). The following figures are a pictorial representation of these degrees of
freedom.
82
Figure 5.1a Longitudinal (X)
(Motion Forward and Backward)
Figure 5.1b Vertical (Y)
(Motion Upward and Downward)
Figure 5.1c Lateral (Z)
(Motion Left and Right)
Figure 5.1d Pitch (? )
(Nose Pitches Up or Down)
Figure 5.1e Roll (? )
(Wings Roll Up or Down)
Figure 5.1e Yaw (? )
(Nose Moves from Side to Side)
Figure 5.1 Definition of the Six Degrees of Freedom
83
The vertical, lateral, and longitudinal accelerations will be modeled with three
simple accelerometer models as was used in Chapter 2. The roll, pitch and yaw rates will
also be modeled with the three simple rate gyro models from Chapter 2. These rate gyro
triad model equations and accelerometer triad model equations for the entire simple six
degree of freedom inertial measurement unit are shown below.
?
???? gyro
wbcrg +++=
(5.1)
?
???? gyro
wbcrg +++=
(5.2)
?
???? gyro
wbcrg +++=
(5.3)
xaccelxxx
wbcxa
&&&&&&&&
&& +++=
(5.4)
yaccelyyy
wbcya
&&&&&&&&
&& +++=
(5.5)
zaccelzzz
wbcza
&&&&&&&&
&& +++=
(5.6)
The above equations contain the actual gyro measurements for roll rate ( ),
pitch rate ( ), and yaw rate ( ) as well as the actual roll rate ( ), the actual pitch rate
( ) and the actual yaw rate ( r ). Each rate gyro contains a constant bias term and a
moving bias term b . The moving bias in each rate gyro is modeled as a first order
Markov process with the following variances.
?
g
?
g
?
g
?
?
r
?
r c
[ ]
22
?
?
?
bias
gyro
bE =
(5.7)
84
[ ]
2
2
?
?
?
bias
gyro
bE =
(5.8)
[ ]
2
2
?
?
?
bias
gyro
bE =
(5.9)
Each rate gyro also contains a wide band noise term . The variances and
means of the wide band noise term for each of the rate gyros are defined as follows.
gyro
w
[]
s
gyro
gyro
f
wE
2
2 ?
?
?
= (5.10)
[ ] 0=
?
gyro
wE
(5.11)
[]
s
gyro
gyro
f
wE
2
2
?
?
?
= (5.12)
[ ] 0=
?
gyro
wE
(5.13)
[]
s
gyro
gyro
f
wE
2
2 ?
?
?
= (5.14)
[ ] 0=
?
gyro
wE
(5.15)
The above equations contain the longitudinal acceleration measurement ( ), the
vertical acceleration measurement ( a ), and the lateral acceleration measurement ( a ).
They also contain the actual longitudinal acceleration ( ), the actual vertical acceleration
( ) and the actual lateral acceleration ( ). Each accelerometer contains a constant bias
x
a
&&
y&& z&&
x&&
y&& z&&
85
term and a moving bias term b . The moving bias in each accelerometer is modeled as
a first order Markov process with the following variances.
c
E
E
E
[E
E
[E
[ ]
22
xbias
accelx
b
&&
&&
?=
(5.16)
[ ]
22
ybias
accely
b
&&
&&
?=
(5.17)
[ ]
22
zbias
accelz
b
&&
&&
?=
(5.18)
Each accelerometer also contains a wide band noise term . The variances
and means of the wide band noise term for each of the accelerometers are defined as
follows.
accel
w
]
s
xaccel
xaccel
f
w
2
2
&&
&&
?
= (5.19)
[ ] 0=
xaccel
wE
&&
(5.20)
[]
s
yaccel
yaccel
f
w
2
2 &&
&&
?
= (5.21)
[ ] 0=
yaccel
wE
&&
(5.22)
]
s
zaccel
zaccel
f
w
2
2
&&
&&
?
= (5.23)
[ ] 0=
zaccel
wE
&&
(5.24
86
5.3 Six Degree of Freedom Inertial Measurement Unit Integration
To determine the position of a six degree of freedom IMU the roll, pitch, and yaw
of the platform must first be calculated. This is accomplished by numerical integrating
the rate gyro measurement. The method chosen to do this is the Runge-Kutta order four
integration routine. This method is laid out below for a State Space format
The state space matrices for calculating heading of a six DoF IMU are:
?
?
?
?
?
?
?
?
?
?
=
000
000
000
A
(5.25
?
?
?
?
?
?
?
?
?
?
=
100
010
001
B
(5.26
?
?
?
?
?
?
?
?
?
?
+
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
&
&
&
BAf
heading
(5.27)
The Runge-Kutta order four procedure for integrating the above state space
equations is defined in the following equations.
[][ ] ( ) 622
4321111
kkkk
T
iii
T
iii
++++=
+++
?????? (5.28)
where
87
[ ] [ ]( )
[][]( )
[][]( )
[][]( )
T
iii
T
iiiheadings
T
iii
T
iiiheadings
T
iii
T
iiiheadings
T
iii
T
iiiheadings
kfTk
kfTk
kfTk
fTk
??????
??????
??????
??????
&
&&
&
&&
&
&&
&
&&
,
,2
,2
,
34
23
12
1
+=
+=
+=
=
(5.29)
The initial conditions in this thesis were always set to zero such that:
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
?
?
0
0
0
0
0
0
?
?
?
(5.30)
The initialization of the roll angle (? ), pitch angle (? ), and yaw angle (? ) all to
zero corresponds to an alignment where the x-axis is pointing due North, the y-axis is
pointing straight Up, and the z-axis is pointing due East.
After the roll, pitch, and yaw angles of the platform are calculated, the position
and velocity of the platform must be calculated by integrating the accelerometer
measurements. The Runge-Kutta method was also used in a state space format:
The state space matrices for calculating position of a six DoF IMU are:
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
000100
000010
000001
000000
000000
000000
A
(5.31)
88
[]
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
000
000
000
ENU
R
B
(5.32)
Where is a transformation matrix from the x-y-z body frame to the ENU
coordinate frame as defined below.
ENU
R
[ ]
111 ???
=
zyxENU
RRRR (5.33)
The individual body coordinate transformation matrices ( , , ) are:
x
R
y
R
z
R
() ()
() ()?
?
?
?
?
?
?
?
?
?
?
=
??
??
cossin0
sincos0
001
x
R
(5.34)
( ) ( )
() ()?
?
?
?
?
?
?
?
?
? ?
=
??
??
cos0sin
010
sin0cos
y
R
(5.35)
( ) ( )
() ()
?
?
?
?
?
?
?
?
?
?
?=
100
0cossin
0sincos
??
??
z
R
(5.36)
The state vector derivative ( ) can then be defined as shown below.
position
f
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
+
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
+
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
0
0
0
0
1
0
0
0
0
G
z
y
x
B
z
y
x
z
y
x
Af
positon
&&
&&
&&
&
&
&
(5.37)
Where G is the gravity field (
2
81.9 sm )
89
The Runge-Kutta order four procedure for integrating the above state space
equations is shown below.
()622
4321
1
1
1
1
1
1
kkkk
z
y
x
z
y
x
z
y
x
z
y
x
i
i
i
i
i
i
i
i
i
i
i
i
++++
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
+
+
+
+
+
+
&
&
&
&
&
&
(5.38)
where:
[][ ] [ ]( )
[][()
[]
]
[][
[][
T
iii
T
iii
T
iiiiiipositons
T
iii
T
iii
T
iiiiiipositons
T
iii
T
iii
T
iiiiiipositons
T
iii
T
iii
T
iiiiiipositons
zyxkzyxzyxfTk
zyxkzyxzyxfTk
zyxkzyxzyxfTk
zyxzyxzyxfTk
&&&&&&&&&
&&&&&&&&&
&&&&&&&&&
&&&&&&&&&
,,
,,2
,,2
,,
34
23
12
1
???
???
???
???
+=
+=
+=
=
]
]
(5.39)
Again the initial conditions for the system are set to zero.
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
0
0
0
0
0
0
0
0
0
0
0
0
z
y
x
z
y
x
&
&
&
(5.40)
The initialization of the states vector to zero is convenient for this analysis.
However, if this were done in practice, the initial longitude, latitude and altitude of the
vehicle platform would need to be recorded in order to properly propagate the vehicle
motion.
90
In the next sections, a six degree of freedom inertial measurement unit is
simulated to study how the heading, velocity, and position errors grow over time. A
tactical and consumer grade IMU is used. The simulation used the model provided in
Equations (5.1-5.6), which include a Markov bias, wide band noise and a constant bias.
The constant bias term was removed in order to focus on the errors generated from the
combination of a Markov bias and wide band noise. The simulations were run with a
gravity field in place and with it removed. This was done to emphasize the effect gravity
has on the navigation capabilities of an IMU.
5.4 Six Degree of Freedom IMU Integration Heading Errors
Figure 5.2 is a plot of the roll, pitch, and yaw angle errors produced when a
tactical grade IMU is integrated. The angles shown are between the body frame and the
ENU coordinate frame. The Monte Carlo simulation was run with a gravity field and
without a gravity field. As seen in the plot the introduction of gravity does not effect the
heading calculations for a tactical grade IMU. This is due to the fact that the gravity field
is only seen by the accelerometers. Since the heading calculation only uses the
measurements form the three rate gyros the introduction of a gravity field will never
affect there error growths.
91
Figure 5.2 Heading Error in a Six DoF Tactical Grade IMU
Figure 5.3 is a plot of the roll, pitch, and yaw angle errors produced when a
consumer grade IMU is integrated. The angles shown are between the body frame and
the NEU coordinate frame. Again, the Monte Carlo simulation was run with and without
a gravity field. As seen in the figure the introduction of gravity does not effect the
heading calculations for a consumer grade IMU due to the reasons mentioned above.
92
Figure 5.3 Heading Error in a Six DoF Consumer Grade IMU
5.5 Six Dof Inertial Measurement Unit Velocity Errors
Figure 5.4 is a plot of the longitudinal, lateral, and vertical velocity errors
produced when a tactical grade IMU is integrated. The velocities shown are in the North
East Up coordinate frame. The Monte Carlo simulation was run with and without a
gravity field. As seen in the figure, the introduction of a gravity field causes the
longitudinal and lateral velocity errors to grow at a rate slightly higher than the vertical
velocity errors.
93
Figure 5.4 Velocity Errors in a Six DoF Tactical Grade IMU
Figure 5.5 is a plot of the longitudinal, lateral, and vertical velocity errors
produced when a consumer grade IMU is integrated. The velocities shown are in the
North East Up coordinate frame. The Monte Carlo simulation was run again and without
a gravity field. As seen in the figure, the introduction of a gravity field causes the
longitudinal and lateral velocity errors to grow at a much faster rate than the vertical
velocity errors.
From an observational stand point, it is important to notice that the introduction of
a gravity field causes the velocity error bounds to change. The magnitude of the change
is dependent on the grade IMU.
94
Figure 5.5 Velocity Error in a Six DoF Consumer Grade IMU
5.6 Six Dof Inertial Measurement Unit Position Errors
Figure 5.6 is a plot of the longitudinal, lateral and vertical position errors
produced when a tactical grade IMU is integrated. The positions shown are in the North
East Up coordinate frame. In the previous figures, it was shown that the introduction of a
gravity field causes the longitudinal and lateral velocity errors to grow at a slightly faster
rate than the vertical velocity errors. It should be expected that these increased errors will
95
induce similar errors in the longitudinal and lateral positions. This can be seen in Figure
5.6.
Figure 5.6 Position Error in a Six DoF Tactical Grade IMU
Figure 5.7 is a plot of the longitudinal, lateral, and vertical position errors
produced when a consumer grade IMU is integrated. The positions shown are in the
North East Up coordinate frame. In the previous figures it was shown that the
introduction of a gravity field causes the longitudinal and lateral velocity errors to grow
at a much faster rate than the vertical velocity errors. It should be expected that the
96
increased errors will induce similar errors in the longitudinal and lateral positions. This
can be seen in Figure 5.7.
Figure 5.7 Position Error in a Six DoF Consumer Grade IMU
5.7 Summary and Conclusion
This chapter discussed the errors associated with the integration of three rate
gyros and three accelerometers to calculate position, velocity and heading in a 3
dimensional setting. First, the IMU model was laid out along with the Rugge-Kutta
integration method. Then the error growth of the position, velocity, and heading
97
calculations were depicted using Monte Carlo simulations. Both a consumer grade and
tactical grade inertial measurement unit were simulated and the effects of a gravity field
were pictorially shown for each.
98
CHAPTER 6
ADVANCED SENSOR MODELING
6.1 Introduction
This chapter will lay out a more advanced six degree of freedom inertial sensor
model for the rate gyros and accelerometers in the inertial measurement unit. The model
presented in this chapter will introduce a variety of new terms. Each term is an error
source which can be seen in the inertial sensors measurements. The advanced terms in
the accelerometer model will include misalignment, nonorthagonality, scale factor, scale
factor asymmetry ,and nonlinearity. The advanced terms in the rate gyro model will
include misalignment, nonorthagonality and scale factor. Each of these terms will be
defined and pictorially presented. Finally, this chapter will define the errors due to
incorrect initial conditions given to the integration routine. These errors are commonly
known as the inertial measurement unit?s initialization errors.
6.2 The Inertial Measurement Unit Accelerometer Triad Error Model
The inertial measurement accelerometer triad error model [Grewal], which
contains a scale factor error, an asymmetric scale factor error, a nonlinear scale factor
99
error, misalignment errors, nonorthogonality errors and all the error terms presented in
Chapter 5 is given as,
() ( ) ( )
xaccelxx
AyAyAzAzxxxx
wbc
zyxSFNxSFAxSFa
+++
??++?++++=
&&&&
&&
&&&&&&&&&& ?? sinsin1
2
(6.1)
( ) ( ) ( )
yaccelyy
AxAxAzAzyyyy
wbc
zxySFNySFAySFa
+++
+?+??++++=
&&&&
&&
&&&&&&&&&& ?? sinsin1
2
(6.2)
() ( ) ( )
zaccelxz
AxAxAyAyzzzz
wbc
yxzSFNzSFAzSFa
+++
??++?+++++=
&&
&&&&
&&
&&&&&&&&&& ?? sinsin1
2
(6.3)
where,
x
a
&&
, and are the measured accelerations,
y
a
&& z
a
&&
x&& , and are the true accelerations, y&& z&&
and
x&& , y&& and z&& are the absolute value of the true accelerations.
The definitions of the error terms for the accelerometer are provided in Table 6.1.
The wide band sensor noise, , is assumed to be normally distributed with a
zero mean and have the following sampled covariance:
accel
w
[]
s
accel
accel
f
wE
2
2
?
=
(6.4)
The gyros walking bias, b , will be modeled as a first order Markov Process with
defined statistics which account for its drift and are shown below:
100
[ ]
2
,0
bias
accel
b ??
(6.5)
bias
gyro
wbb +?=
?
1
&
(6.6)
6.3 The Inertial Measurement Unit Rate Gyro Triad Error Model
The inertial measurement rate gyro triad error model [9], which contains scale
factor errors, misalignment errors, nonorthogonality errors and all the error terms
presented in Chapter 5 is given as,
()( ) ( )
?
????
?????
gyroGzGzGyGy
wbcSFg +++??++?++= &
&&
sinsin1
(6.7)
()( ) ( )
?
????
?????
gyroGxGxGyGy
wbcSFg +++??++?++= &
&&
sinsin
(6.8)
()()( )
?
????
?????
gyroGxGxGzGz
wbcSFg ++++?+??++=
&&
& sinsin1
(6.9)
where,
?
g , and are the measured rotation rates,
?
g
?
g
?
&
, and ?
&
?& are the true rotation rates,
and
?
&
, ?
&
and ?& are the absolute value of the true accelerations.
The wide band sensor noise, , is assumed to be normally distributed with a
zero mean and have the following sampled covariance:
gyro
w
101
[]
s
gyro
gyro
f
wE
2
2
?
= (6.10)
The gyros walking bias, b , will be modeled as a first order Markov Process with
defined statistics which account for its drift and are shown below:
[ ]
2
,0
bias
gyro
b ??
(6.11)
bias
gyro
wbb +?=
?
1
&
(6.12)
A list of all of the error terms used in the triad accelerometer model (6.1-6.3) and
the triad rate gyro model (6.7-6.9) are given below in Table 6.I
Table 6.1 Advanced IMU Error Parameters
Error Parameter Symbol Units
Roll Gyro Wide Band Noise Variance
?
?
2
gyro
( )
2
seco
Pitch Gyro Wide Band Noise Variance
?
?
2
gyro
( )
2
seco
Yaw Gyro Wide Band Noise Variance
?
?
2
gyro
( )
2
seco
Roll Gyro Constant Offset
?
c
seco
Pitch Gyro Constant Offset
?
c seco
Yaw Gyro Constant Offset
?
c
seco
Roll Gyro Bias Variance
2
?
?
bias
gyro
( )
2
seco
Pitch Gyro Bias Variance
2
?
?
bias
gyro
( )
2
seco
Yaw Gyro Bias Variance
2
?
?
bias
gyro
( )
2
seco
Roll Gyro Bias Time Constant
?
?
sec
Pitch Gyro Bias Time Constant
?
? sec
Yaw Gyro Bias Time Constant
?
?
sec
Roll Gyro Scale Factor Error
?
SF ppm
Pitch Gyro Scale Factor Error
?
SF
ppm
Yaw Gyro Scale Factor Error
?
SF ppm
102
Gyro Triad Misalignment about X
Gx
? rad?
Gyro Triad Misalignment about Y
Gy
? rad?
Gyro Triad Misalignment about Z
Gz
? rad?
Gyro Triad Nonorthogonality about X
Gx
? rad?
Gyro Triad Nonorthogonality about Y
Gy
? rad?
Gyro Triad Nonorthogonality about Z
Gz
? rad?
Longitudinal Accelerometer Wide Band Noise Variance
xaccel
2
?
2
g
Vertical Accelerometer Wide Band Noise Variance
yaccel
2
?
2
g
Lateral Accelerometer Wide Band Noise Variance
zaccel
2
?
2
g
Longitudinal Accelerometer Constant Offset
x
c
&&
g
Vertical Accelerometer Constant Offset
y
c
&&
g
Lateral Accelerometer Constant Offset
z
c
&&
g
Longitudinal Accelerometer Bias Variance
2
?
?
bias
accel
2
g
Vertical Accelerometer Bias Variance
2
?
?
bias
accel
2
g
Lateral Accelerometer Bias Variance
2
?
?
bias
accel
2
g
Longitudinal Accelerometer Bias Time Constant
x
? sec
Vertical Accelerometer Bias Time Constant
y
?
sec
Lateral Accelerometer Bias Time Constant
z
? sec
Longitudinal Accelerometer Scale Factor Error
x
SF
ppm
Vertical Accelerometer Scale Factor Error
y
SF ppm
Lateral Accelerometer Scale Factor Error
z
SF
ppm
Longitudinal Accelerometer Scale Factor Asymmetry
x
SFA
ppm
Vertical Accelerometer Scale Factor Asymmetry
y
SFA ppm
Lateral Accelerometer Scale Factor Asymmetry
z
SFA
ppm
Longitudinal Accelerometer Scale Factor Nonlinearity
x
SFN
ppm
Vertical Accelerometer Scale Factor Nonlinearity
y
SFN ppm
Lateral Accelerometer Scale Factor Nonlinearity
z
SFN
ppm
Accelerometer Triad Misalignment about X
Ax
? rad?
Accelerometer Triad Misalignment about Y
Ay
? rad?
Accelerometer Triad Misalignment about Z
Az
? rad?
Accelerometer Triad Nonorthogonality about X
Ax
? rad?
Accelerometer Triad Nonorthogonality about Y
Ay
? rad?
Accelerometer Triad Nonorthogonality about Z
Az
? rad?
103
6.4 Model Error Parameter Descriptions
Figure 6.1 illustrates the individual input/output error types found in the advanced
sensor model. Figure 6.1a, 6.1b and 6.1c are the graphical representation of scale factor,
an offset bias and asymmetry, respectfully. Figure 6.1d is the graphical representations
of nonlinearity.
(a) Scale Factor (b) Offset Bias
(c) ? Asymmetry (d) Nonlinearity
Figure 6.1 Inertial Measurement Unit Input/Output Error Types
Input
Output
Input
Output
Input
OutputOutput
Input
104
Figure 6.2 illustrates the individual input axis misalignments. The large arrows in
the figure represent the nominal input axis directions (labeled X, Y and Z). The smaller
arrows (labeled m
ij
) represent the directions of scale factor deviations when ji = and
axis misalignment when ji ? .
m
yy
m
yz
m
yx
Y
m
zy
m
xz
X
Z
m
zx
m
xx
Figure 6.2 Inertial Measurement Unit Triad Misalignment
m
zz
m
xy
The following equations can be used to obtain the angular misalignment about a
particular axis.
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
=
Z
m
Y
m
zyyz
x
arcsinarcsin?
(6.13)
105
?
?
?
?
?
?
=?
?
?
?
?
?
=
Z
m
X
m
zxxz
y
arcsinarcsin?
(6.14
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
=
Y
m
X
m
yxzy
z
arcsinarcsin?
(6.15)
Figure 6.3 illustrates the individual input axis nonorthogonality. The large arrows
in the figure represent the nominal input axis directions (labeled X, Y and Z). The smaller
arrows (labeled m
ij
) represent the directions of scale factor deviations when ji = and
axis nonorthogonality when ji ? .
n
yy
n
yx
n
yz
Y
n
xy
n
zy
X
Z
Figure 6.3 Inertial Measurement Unit Triad Nonorthogonality
n
xx
n
zx
n
xz
n
zz
106
The following equations can be used to obtain the angular nonorthogonality about
a particular axis.
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
=?
Z
n
Y
n
zyyz
x
arcsinarcsin
(6.13)
?
?
?
?
?
?
=?
?
?
?
?
?
=?
Z
n
X
n
zxxz
y
arcsinarcsin
(6.14
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
=?
Y
n
X
n
yxzy
z
arcsinarcsin
(6.15)
6.5 Inertial Measurement Unit Initialization Errors
Before navigation can begin, the initial condition of the states must be given to
the integration routine. The mean error of the calculated initial condition for each of the
states are the initialization errors. In the previous chapter, the position of the body frame
was represented in terms of the North-East-Up coordinate frame. This chapter will
continue with that transformation. The initial attitude and position of the body frame will
be input into the integration procedure. As the body frame moves, the position will then
be calculated in terms of the NEU coordinate frame. The accuracies of the initial attitude
and position measurement for the body frame are the initialization errors of the inertial
measurement unit. Each degree of freedom will contain an initialization error, which are
defined in Table 6.2 below.
107
Table 6.2 IMU Initialization Error Parameters
Error Parameter Symbol Units
North Position Error @ Beginning of Navigation
N
?
Degrees
East Position Error @ Beginning of Navigation
E
? Degrees
Up Position Error @ Beginning of Navigation
U
?
Degrees
Roll Error @ Beginning of Navigation
error
?
Meters
Pitch Error @ Beginning of Navigation
error
?
Meters
Yaw Error @ Beginning of Navigation
error
?
Meters
Figure 6.4 shows the initialization vectors for a body coordinate frame. The
perceived origin of the body frame is located at point ( )
000
,, UENO = , The perceived
heading of the body frame is ( )
000
,, ??? . From Figure 6.4 the initialization errors can be
defined as follows:
xxN
i=?
(6.16)
yyE
i=?
(6.17)
zzU
i=?
(6.18
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
=
Z
i
Y
i
zyyz
error
arcsinarcsin?
(6.19)
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
=
X
i
Y
i
xyyx
error
arcsinarcsin?
(6.20)
?
?
?
?
?
?
=
?
?
?
?
?
?
=
Z
i
X
i
zxxz
error
arcsinarcsin?
(6.21)
The actual origin of the body frame can be found with the following equations.
( )
UENactual
UENO ??? +++=
000
,,
(6.22)
( )
orrorerrorerroractual
Heading ?????? +++=
000
,
(6.23)
108
Up
i
yz
Y
i
yx
i
yy
i
zy
O
i
xx
i
zz
Z i
xz
i
zx
X
North
i
xy
Figure 6.4 Initialization Vectors of the Body Coordinate Frame
East
109
6.6 Summary And Conclusion
This chapter laid out a more advanced six degree of freedom inertial sensor model
which contained three rate gyros and three accelerometers with a variety of input/output
error terms. The input/output error terms in the three accelerometer models included
misalignment error, nonorthagonality error, scale factor error, scale factor asymmetry
error and scale factor nonlinearity error. The input/output error terms in the three rate
gyro models included misalignment errors, nonorthagonality errors and scale factor error.
Each of these terms was defined and pictorially presented. Finally, this chapter defined
the inertial measurement unit?s initialization errors.
110
CHAPTER 7
INTEGRATION ERRORS OF THE SIX DEGREE OF FREEDOM
ADVANCED INERTIAL MEASURMENT UNIT MODEL
7.1 Introduction
This chapter will discus the integration errors of the six degree of freedom
advanced inertial measurement unit model presented in Chapter 6. A comparison of the
advanced model versus the simple model of chapter five will be illustrated with a
common rocket launch trajectory. The comparison will identify the effects the advanced
models error parameters have on the inertial measurement unit?s navigation capabilities.
7.2 Launch Trajectory Profile I
The trajectory chosen for this comparison is one which simulates a rocket launch.
The rocket is elevated to a pitch angle of 55 degrees before ignition. The longitudinal
and lateral axes are aligned with the north and east axis. A maximum thrust of 8 g?s is
introduced in the rockets longitudinal axis. The thrust is sufficient to project the rocket a
distance of 84 km due north. The body frame accelerations and heading angles are
shown in Figure 7.1a and Figure 7.1b. The rocket velocities in the NEU coordinate frame
111
are shown in Figure 7.1c. A graph of the rockets altitude versus range is shown in Figure
7.2.
As seen in Figure 7.1a, an initial thrust of 8 g?s is introduced in the rockets
longitudinal axis. Since the rocket is elevated to a pitch angle of 55 degrees, the
longitudinal accelerometer will read a negative component of gravity. As the rocket
peaks and begins its downward slope, the longitudinal accelerometer will begin to read a
positive component of gravity. The existence of the gravity field will introduce a slight
error in the position calculation as shown previously in Chapter 5
As seen in Figure 7.1b, the rocket is elevated to a 55 degree pitch angle. After the
initial thrust is introduced, the rocket maintains this pitch angle for approximately 6
seconds. After 6 seconds, a -10 degree per second pitch rate begins for 15 seconds. This
results in a final pitch angle of -88 degrees.
As seen in Figure 7.1c, the initial thrust brings the northern and upward velocities
to approximately 550 meters per second. After the rocket peaks and begins its downward
part of the trajectory, it reaches a final impact velocity of nearly 600 meters per second.
As seen in Figure 7.2, the rocket travels down range 86 kilometers in 165
seconds. In 40 seconds it reaches a peak altitude of 24 kilometers
112
Figure 7.1
Rocket Launch Trajectory I
Body Accelerations, Body Headings and ENU Velocities
113
Figure 7.2 Rocket Launch Trajectory I Altitude Vs. Range
7.3 Comparison of the Simple and Advanced IMU Models
To compare the simple model to the advanced model, the above trajectory was
used. Each six degree of freedom model was run in a Monte Carlo simulation for 200
iterations. Tactical grade IMU specifications were used in both the simple and advanced
models. The purpose of this comparison is to evaluate the effects that the additional
terms in the advanced model have on the navigation capabilities of an IMU. To do this,
the constant bias offset must be very small or non existent. Normally a tactical grade
IMU contains as little as 1 deg per hour of bias offset in the rate gyros and as little as 400
114
micro g?s of bias offset in the accelerometers. For this comparison, the constant bias
offset of the tactical IMU was set to zero. The initialization errors discussed in chapter 6
were also neglected in order to identify the input/output parameter with the larges impact.
The comparison point was chosen to be at the point of impact. Figure 7.3 is a scatter plot
of the calculated impact point for both of the inertial measurement unit models.
Figure 7.5 Rocket Launch Trajectory Impact Point Scatter Plot
The blue data clusters represent the impact point calculated by the simple model.
The red data clusters represent the impact point calculated by the advanced model. The
standard deviation for both clusters is 50 meters downrange and 50 meters cross range.
115
However, from the plot it is evident that the two clusters have different downrange and
cross range means. The simple model of chapter five has a mean center line at the origin
of the downrange and cross range axes. This is expected because the wide band noise
and Markov process for each inertial sensor in the simple model have a mean of zero.
The advanced model has a mean cross range center line of 450 meters east of the actual
impact point and a mean downrange center line of 86 meters south of the actual impact.
Because the moving bias and wide band noise statistics are the same for each model, the
shifting of the center line can only be contributed to the additional input/output
parameters modeled in the advanced model.
7.4 Advanced Model Error Parameter Contribution Levels
To determine the contribution level each of the input output error parameters has
on the total error, a simulation was run for each error parameter individually. The
magnitude of the contribution level for each error parameter was calculated by taking the
second norm of an error vector at the impact point. Figure 7.4 is a bar graph of each of
the error parameters contribution levels as well as the total error level.
116
0
50
100
150
200
250
300
350
400
450
500
G
y
r
o
M
is
a
li
n
g
m
e
nt
ab
o
u
t
X
Gy
r
o
M
i
s
a
l
i
gn
men
t
ab
o
u
t
Y
Gy
r
o
M
i
s
a
l
i
gn
men
t
ab
o
u
t
Z
Gy
r
o
N
o
n
o
r
t
h
o
go
na
l
i
t
y
ab
o
ut
X
G
yr
o N
on
or
th
og
on
a
l
i
t
y
a
bo
ut
Y
G
yr
o N
on
o
r
th
og
on
a
l
i
t
y
ab
o
u
t
Z
Ro
l
l
G
y
r
o
S
c
al
e
F
act
o
r
P
itc
h G
yr
o S
c
al
e F
a
c
to
r
Y
aw
G
yr
o
S
c
a
le
F
a
ct
o
r
E
r
r
or
A
c
ce
le
ro
m
e
t
e
r
M
is
a
l
ig
n
m
e
nt
a
b
ou
t
X
Ac
c
e
le
r
o
met
e
r
Mi
sa
l
i
g
n
m
en
t
a
bo
ut
Y
Ac
c
el
er
o
m
e
te
r
Mi
s
al
i
g
nm
en
t
ab
ou
t
Z
Ac
c
e
l
e
r
o
m
e
te
r
N
o
n
o
r
t
ho
go
na
l
i
t
y
ab
o
u
t
X
Ac
c
e
l
e
r
o
m
e
te
r
N
o
n
o
r
t
ho
go
na
l
i
t
y
ao
u
t
Y
A
c
cel
er
o
m
et
e
r
No
n
o
r
t
h
o
go
n
a
l
ity
a
bo
ut
Z
X
Ac
c
e
l
er
o
me
t
e
r
S
ca
l
e
Fa
c
t
o
r
Y A
c
ce
l
e
ro
m
e
te
r
S
c
al
e
Fa
c
to
r
Z A
c
cel
e
r
o
m
et
e
r
S
c
a
le
F
a
c
t
o
r
X
A
cc
e
le
r
o
me
te
r
A
s
s
y
mm
e
t
ry
Y
A
cc
e
le
r
o
me
te
r
A
s
s
y
met
r
y
Z
A
c
ce
le
r
om
e
t
e
r A
s
s
ym
met
r
y
X
A
cc
e
le
r
o
me
te
r
N
on
l
i
n
ea
ri
t
y
Y
A
cc
e
le
r
o
me
te
r
N
on
l
i
n
ea
ri
t
y
Z A
c
ce
le
r
o
met
e
r
N
on
l
i
ne
a
r
i
t
y
A
ll
E
rro
rs
C
o
n
t
r
i
b
u
t
i
on
L
e
ve
l
(m
e
t
e
r
s
)
Figure 7.4 Rocket Launch Trajectory I Error Parameters Contribution Levels
From the bar graph, it is now possible to identify the error parameters which
contribute the greatest to the total error in the impact point calculation for this rocket
launch trajectory. The gyro misalignment about Y has the highest contribution level for
this simple trajectory. This is due to the fact that the roll measurement is directly affected
when there is a pitch rate applied to the body frame, as seen in Equation (6.7). The
accelerometer misalignment about Y is the second highest contributor to error. This
effect is due to the fact that the lateral acceleration is directly affected when there is a
longitudinal acceleration applied to the body frame, as shown previously in Equation
(6.3).
117
7.5 Summary and Conclusion
This chapter discussed the integration errors of the six degree of freedom
advanced inertial measurement unit model presented in Chapter 6. A comparison of the
advanced model versus the simple model of Chapter 5 was illustrated with a common
rocket launch trajectory. The comparison showed that the advanced models error
parameters caused the calculated position to be skewed away from the actual position. It
was also shown that the contribution level for each of the error parameters was dependent
on the trajectory.
118
CHAPTER 8
CONCLUSION
8.1 Summary
In Chapter 2, a simple sensor model was laid out that incorporated a moving bias,
a constant bias, and a random error. The biases and random error sources were analyzed
statistically. The numerical values for the time constants, means, and variances were
obtained by using common statistical methods, Allan Variance charts, Autocorrelation
functions, and Monte Carlo Simulations. The statistical parameters of the error sources
were used to categorize and characterize the sensors. The inertial sensors were
categorized into four basic grades: navigational, tactical, automotive, and consumer.
Several sensor grades were simulated and experimentally validated. A group of tables
was provided which contained the specification for a number commonly used rate gyros
and accelerometers. Additional tables were also provided which can be used to classify
the sensors.
Chapter 3 discussed the errors associated with the integration of a single rate gyro
and single accelerometers to calculate position, velocity and heading in a planar setting.
The error growths of the position, velocity, and heading calculation were depicted with
the use of a Monte Carlo Simulation. Both consumer and tactical grade sensor
characteristics were compared and experimental data was used to validate the bounds of
119
the Monte Carlo simulations. The Monte Carlo simulations were validated by showing
the error growths of rate gyros and accelerometers which contain moving biases and
comparing them to simulations where the only error source was wide-band noise.
Analytical solutions for position, velocity and heading errors were derived which plot the
error growths depicted by the integration of rate gyros and accelerometers which contain
only one error source, wide-band noise. Finally, a gravity field was introduced. The
error growths of the position and velocity calculations were compared with a gravity field
and without.
Chapter 4 implemented a Kalman Filter which combined the GPS position,
velocity and heading measurements with the inertial sensor measurements. The Kalman
filter incorporated the sensor model presented in Chapter 2. It was shown that the
coupling of the GPS measurements and the sensor model to calculate the position,
velocity and heading from the combined GPS and IMU measurements resulted in new
error bounds for position, velocity and heading. It also was shown that the steady state
error bounds were the same for both the two state Kalman filter and the three state
Kalman filter when used to estimate heading. The two state Kalman filter provided an
estimation of the rate gyros biases. However, the bias had to be either modeled as a
random walk or a first order Markov process. Monte Carlo simulations were used to
demonstrate the difficulty in choosing the model type. The three state Kalman filter
provided an estimation of the rate gyros bias and was able to estimate both a constant
component as well as a moving component. Experimental data was used to validate the
bias estimations in the three state Kalman filter. A five state model was laid out which
120
estimates two bias components in an accelerometer. These components include a
constant part and a moving part.
Chapter 5 discussed the errors associated with the integration of three rate gyros
and three accelerometers to calculate position, velocity and heading in a 3 dimensional
setting. First the IMU model was laid out along with the Rugge-Kutta order four
integration routine. Then the error growth of the position, velocity, and heading
calculations were depicted using Monte Carlo simulations. Both a consumer grade and
tactical grade inertial measurement unit were simulated and the effects of a gravity field
were pictorially shown for each.
Chapter 6 laid out a more advanced six degree of freedom inertial sensor model
which contained three the rate gyros and three accelerometers with a variety of
input/output error terms. The input/output error terms in the three accelerometer models
included misalignment error, nonorthagonality error, scale factor error, scale factor
asymmetry error and scale factor nonlinearity error. The input/output error terms in the
three rate gyro models included misalignment errors, nonorthagonality errors and scale
factor error. Each of these terms was defined and pictorially presented. Finally, this
chapter defined the inertial measurement unit?s initialization errors.
Chapter 7 discussed the integration errors of the six degree of freedom advanced
inertial measurement unit model presented in Chapter 6. A comparison of the advanced
model versus the simple model of Chapter 5 was illustrated with a common rocket launch
trajectory. The comparison showed that the advanced models error parameters caused
the calculated position to be skewed away from the actual position. It was also shown
121
that the contribution level for each of the error parameters was dependent on the
trajectory.
8.2 Recommendations for Future Work
Future work should include using the procedure laid out in Chapter 2 to determine
the statistical properties of a variety of other consumer, automotive and tactical grade
sensors not listed in this work. The error analysis performed in Chapter 3 should be done
for cases where the sensors are not static in order to validate the robustness of the
procedure. Equations should be derived that represent the error growth for a non static
sensor and compare these equations to the ones derived in Chapter 3. This comparison
can be used to analyze the effects dynamics play in the sensor error growth. Equations
should be derived which depict the error growth for the contribution of the walking bias.
These equations, in conjunction with the equations derived in Chapter 3, then can be used
to predict the total error growth due to wide band noise and the walking bias modeled as
a first order Markov process. Equations should be derived which depict the error growth
due to the introduction of a gravity field. These equations can then be used to analyze the
effects gravity has on the error growth in a two degree of freedom scenario and possibly a
six degree of freedom scenario. The bounded error shown in Chapter 4, which is the
result of the coupling of the IMU with a GPS signal, should be derived analytically. With
the analytical solution for these error bounds, it would be possible to determine what
grade sensor is necessary for a particular coupling application. The error bounds for a six
degree of freedom in Chapter 5 should be derived analytically. With the analytical
122
solution of a six degree of freedom system, it would be possible to compare them to the
error bounds of a two degree of freedom system. The comparison could reveal the effects
the rotation matrix has on the error growth. The stability of the input/output terms
presented in Chapter 6 should be studied and a procedure to determine their stability
should be created. Finally, more trajectories should be used to analyze the contribution
levels of these input/output terms.
123
REFERENCES
124
[1] Allan D.W, ?Statistics of Atomic Frequency Standards? 1966 Vol. 54 pp 221-230,
Institute of Electrical and Electronics Engineers Proceedings.
[2] Beckwith, Thomas G. Mechanical Measurements 5
th
Ed. Addison-Wesley
Publishing Company. 1993.
[3] BEI. Systron Donner Inertial Division. ?BEI Gyrochip Model AQRS?. BEI
technologies, INC. 1998.
[4] Bennett, S.M. ?Fiber Optic Rate gyros as Replacements fro Mechanical Gyros?.
KVH Industries. 1998.
[5] Bevly David M. ?Comparison of INS vs. Carrier-Phase DGPS for Attitude
Determination in the Control of Off Road Vehicles?. Navigation: Journal
of the Institute of Navigation. Vol. 47, Number 4. Winter 2000-2001, pp
257-266.
[6] Bevly, David M. ?Evaluation of a Blending Dead Reckoning and Carrier Phase
Differential GPS System for Control of an Off-Road Vehicle? 1999
ION/GPS Proceedings
[7] Bevly, David M. High Speed, Dead Reckoning, Towed Implement Control for
Automatically Steered Farm Tractors Using GPS. A Dissertation
submitted to the Department of Aeronautics and Astronautics and the
committee on graduate studies of Stanford University, Stanford University
2001.
[8] Borenstein, Johann, ?Experimental Evaluation of a Fiber Optics Gyroscope for
Improving Dead Reckoning Accuracy in Mobile Robots? Prceedings of
the IEEE International Conference on Robotics and Automation. 1998
pp3456-3461
[9] Bosch, P. P. J. van den. Modeling Identification and Simulation of Dynamic
Systems. CRC Press. 1994.
[10] Brogan, Willam L. Modern Control Theory. Prentice Hall. 1991.
[11] Brown, Robert G. Introduction to Random Signals and Applied Kalman Filtering
3
rd
Ed. John Wiley and Sons. 1997.
[12] Burden, Richard L. Numerical Analysis 7
th
Ed. Wadsworth Group. 2001.
[13] Chatfield, Averil B. Fundamentals of High Accuracy Inertial Navigation.
American Institute of Aeronautics and Astronautics, Inc. 1997.
[14] Chobotov, Vladimir A. Orbital Mechanics 3
rd
Ed. American Institute of
Aeronautics and Astronautics, Inc. 2002.
[15] Demoz, Gebre-Egziabher. ?An Inexpensive DME-Aided Dead reckoning
Navigator?. Navigation: Journal of the Institute of Navigation. Vol. 50,
Number 4. Winter 2003-2004, pp 247-264.
[16] Demoz, Gebre-Eqziabher Design and Performance Analysis of a Low-Cost Aided
Dead Reckoning Navigator A Dissertation submitted to the Department
of Aeronautics and Astronautics and the committee on graduate studies of
Stanford University, Stanford University 2003.
[17] Devore, Jay L. Probability and Statistics. Duxbury. 2000.
[18] Dorf, Richard C. Modern Control Systems. Addison-Wesley. 1998.
[19] Ellum, Cameron. ?Inexpensive Kinematic Attitude Determination from MEMS-
Based Accelerometers and GPS-Derived Accelerations?. Navigation:
Journal of the Institute of Navigation. Vol. 49, Number 3. Fall 2002, pp
117-126.
[20] Farrel, J. L. ?Comments on Performance Analysis of a Tightly Couple
GPS/Inertial System for Two integrity Monitoring Methods?. Navigation:
Journal of the Institute of Navigation. Vol. 48, Number 2. Summer 2001,
pp 131-132.
[21] Ford, Tom. ?MEMS Inertial on RTK GPS Receiver: Integration Options and Test
Results?. Honeywell International Inc. 2002
[22] Franklin, Gene F. Feedback Control of Dynamic Systems. Prentice Hall. 2002.
[23] Gelb, Arthur. Applied Optimal Estimation. The M.I.T. Press. 1974.
[24] Ginsberg, Ferry H. Advanced Engineering Dynamics 2
nd
Ed. Cambridge
University Press 1998
[25] Ginsberg, Jerry H. Advanced Engineering Dynamics 2
nd
Ed. Cambridge
University Press. 1998.
[26] Grewal, Mohinder S. Global Positioning System, Inertial Navigation and
Integration. A John Wiley and Sons, Inc. 2001
125
[27] Hamm, C., Flenniken, W., Bevly, D.M., and Lawrence, D. ?Comparative
Performance Analysis of Aided Carrier Tracking Loop Algorithms in
High Noise/High Dynamic Environments?. Proceedings of the 2004 ION-
GNSS Conference.
[28] Honeywell. ?HG1700 Inertial Measurement Unit?. Honeywell International Inc.
2002.
[29] Honeywell. ?HG1900 Inertial Measurement Unit?. Honeywell International Inc.
2002.
[30] Kreyszig, Erwin. Advanced Engineering Mathematics 8
th
ED. John Wiley and
Sons. 1999.
[31] Leach, Barrie W. ?Operational Experience with Optimal Integration of Low-Cost
Inertial Sensors and GPS for Flight Test Requirements.? Flight Test
Laboratory. Volume 49, Number 2 June 2003. pp 41-54
[32] Lee, Young. ?Reply to Comments on Performance Analysis of a Tightly Couple
GPS/Inertial System for Two integrity Monitoring Methods?. Navigation:
Journal of the Institute of Navigation. Vol. 48, Number 2. Summer 2001,
pp 133.
[33] Leon, Steven J. Linear Algebra with Applications. Prentice Hall. 1998.
[34] Nassar, Sameh. ?Modeling Inertial Sensor Errors Using Autoregressive (AR)
Models?. Navigation: Journal of the Institute of Navigation. Vol. 51,
Number 4. Fall Winter 2004, pp 259-268.
[35] Northrup Grumman. ?Summary of LN-200 IMU Characteristics?. The Northrup
Grumman Group. 2002.
[36] Ogata, Katsuhiko. System Dynamics 4
th
Ed. Prentice Hall. 2004
[37] Petovello, M.G. . ?Benefits of Using a Tactical-Grade IMU for High Accuracy
Positioning?. Navigation: Journal of the Institute of Navigation. Vol. 51,
Number 1. Spring 2004, pp 1-12.
[38] Selby, Samuel M. Standard Mathematical Tables 12
th
Ed. The Chemical Rubber
Co. 1972.
[39] Skaloud, Jan. ?Reducing the GPS Ambiguity Search Space by Including Inertial
Data?. 2004 ION/GPS Proceedings
[40] Stengel, Rober F. Optimal Control and Estimation. Dover Publications. 1994.
[41] Webster, Noah. Webster?s third New International Dictionary. G and C Merriam
126
Company. 1966.
[42] Welch, Greg. ?An Introduction to the Kalman Filter?. UNC-Chapel Hill, TR 95-
041, April 5, 2004.
[43] Zhang, Yufeng. ?Performance Analysis of a Tightly Coupled Kalman Filter for
the Integration of Un-differenced GPS and Inertial Data? 2004 ION/GPS
Proceedings
[44] Zifel, Peter H. Modeling ans Simulation of Aerospace Vehicle Dynamics.
American Institute of Aeronautics and Astronautics, Inc. 2000.
127
APPENDICES
128
APPENDIX A
THE ALLAN VARIANCE
A.1 Introduction
The Allan variance was developed by David W. Allan. A full explanation of the
technique can be found in [3]. The Allan variance can be viewed as the time domain
equivalent of the power spectrum. However, the Allan variance plots power as a function
of time averaging blocks as opposed to plotting power as a function of frequency. This
appendix introduces the basic equations needed to plot an Allan variance. Examples are
provided showing how the Allan variance chart changes for various values of wide band
noise. Finally Allan variance charts are given for various Markov process parameters.
For a more detailed description of the Allan variance, including error identification,
review Appendix C [4].
A.2 The Allan Variance Computation
To construct an Allan variance for some process ( )t? , the data is sampled at a
predefined frequency ( ). The total number of data points will then be defined as,
s
f
TfN
s
=
(A.1)
129
where T is the duration of the series in seconds
Once the data series has been sampled, the Allan variance chart is constructed
using the following procedure:
1. Create a vector of averaging times,
av
? :
?
?
?
?
?
?
=
2
....21
T
av
? seconds
(A.2)
2. For each
av
? , divide the series ( )t? into M clusters
( )
av
av
T
M
?
? =
(A.3)
3. For each cluster M compute? , the time average of ( )t? .
()
?
=
=
L
i
iav
L
k
1
1
, ??? ,
avs
fL ?= , and ()[ ]
av
Mk ?....21=
(A.4)
4. Use each of these cluster averages from a specific
av
? to form a new variable
called the Allan variance ( )
av
??
2
.
() ()()[kkE
avavav
,1,
2
1
2
?????? ?+= ] (A.5)
5. Plot each Allan variance, ( )
av
??
2
, versus
av
? on a log log scale. This plot is
known as the Allan variance chart.
A.3 Wide Band Noise Characteristics
The power of the Allan variance can be seen when analyzing data that is
dominated by wide band noise, which is defined below.
130
[ ]
noise
Nw ?,0~
(A.6)
By using the Allan variance chart, it is possible to determine the random walk
statistics of a data series. The random walk of a series ( )tw is defined at the point where
crosses (
av
??
2
) 1=
av
? . The follow equation can be used to calculate this intersection.
()
s
noise
f
?
? =1
2
(A.6)
Figure A.1 is an Allan variance chart for 7 sets of data. The data used was created
with a random number generator with a sampling frequency of 100 Hertz, each having a
different variance The sets of random series have a range of variances from 10 to 10 .
It is clear that maps exactly where the above equation predicts.
6? 6
(1
2
=?? )
131
Figure A.1 Wide Band Noise Allan Variance Comparison
A.4 Markov Process Characteristics
Another strength of the Allan variance is the ability to recognize noise
characteristics which are easily modeled as 1
st
Order Markov Processes. A 1
st
Order
Markov Process is defined as:
?
?
+= bb
1
&
[ ]
b
Nb ?,0~
132
The rate at which the differential equation decays is determined by the time
constant, ? . When evaluating noise characteristics with an Allan variance chart, an
upward sloping curve of
2
1
denotes a noise which is best modeled as a Markov process
with a very slow decay, other Allan variance characteristic errors are given in Table A.1.
Curves with flatter slopes denote noises which are best modeled as Markov processes
with faster decay time (as determined by the time constant (? ). Downward sloping
curves indicate noises with very fast time constants and are better modeled as wide band
noise but can be modeled as a Markov Process if enough information is available. Figure
A.2 is a comparison of Allan variances from data generated by Markov Processes with
time constants ranging from 10 to 10 seconds and a variance of equal to 1.
2? 4 2
b
?
133
Figure A.1 Wide Band Noise Allan Variance Comparison
Table A.1 Allan Variance Characteristic Error Mechanisms
Error Mechanism Allan Variance Slope
Wide-Band Noise - 1/2
Exponentially Correlated Noise (First
Order Markov Process
1/2
Rate Random Walk 1/2
Linear Rate Ramp 1
Quantization Noise -1
Sinusoidal Input 1
Flicker Noise 0
134
APPENDIX B
MARKOV PROCESS STATISTICS
B.1 Introduction
This appendix presents a derivation of the statistics for the input white noise of a
First Order Markov Process. A Markov process is a stochastic process where all future
values are scaled values of past values plus a random input (described by a first order
differential equation). In the case of a first order process, the future value is only
dependent on a scaled portion of the first previous value. The follow differential
equation represents this:
?
?
+= bb
1
&
(B.1)
One purpose of a the Markov Process is to filter wide band noise and produce a
data set which is zero mean specific standard deviation. The following is a statistical
representation of this.
[ ]
b
b ?,0~ (B.2)
135
B.2 Derivation
The ability to control the output statistics is the key to the Markov process. The
derivation of the standard deviation of the input noise (? ) is obtained by:
( )dtbb
?
=
&
(B.3)
Substituting the Markov differential equation (B.1):
dtbb
?
?
?
?
?
?
?
+?= ?
?
1
(B.4)
Use Euler integration to simplify,
?
?
?
?
?
?
+?+=
+ kkskk
bTbb ?
?
1
1
(B.5)
()
ksk
s
k
Tb
T
b ?
?
+
?
?
?
?
?
?
?=
+
1
1
(B.6)
The expected value of both sides is then taken to find the covariance of the bias.
[] ()[]
T
ss
T
ssT
TTEb
T
b
T
EbbE ??
??
+
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
?
?
?
?
?
?= 11
(B.7)
222
2
2
1
?
??
?
?
sb
s
b
T
T
+?
?
?
?
?
?
?=
(B.8)
136
2
2
2
12
b
s
T
?
??
?
?
?
?
?
?
?
?
?
?
+=
(B.9)
Because the time constant (? ) is generally a large value, the inverse square can be
assumed to be very small and the second term in Equation (B.9) can be ignored. This
results in the following equation for the standard deviation of the input noise.
?
?
?
?
s
b
T
2
2
=
(B.10)
Where is the sampling interval.
s
T
B.3 Example
To help better understand the use of the Markov process an example with the
following desired statistics is performed.
[ ]
01.
100
1,0~
=
=
s
T
b
?
Note: ? and are both expressed in seconds.
s
T
Using Euler?s integration method and a random number generator the following
data series was created using the following statistical values.
137
1
2
1
2
==
=
?
?
?
?
?
s
b
b
T
Figure B.1a provides a plot of the Markov process input noise. Equation B.1 is
used to produce the Markov output seen in Figure B.1b.
Figure B.1 Input and Output Data for a Markov Process
The actual statistical values which are represented by the red lines in Figure B.1
are as follows.
138
9029170.99952355
2
6289491.01246110
2
==
=
?
?
?
?
?
s
b
b
T
The desired output statistic was met to within 1%. An autocorrelation function
shown below in Figure B.2 was used to validate that the time constant (? ) was preserved.
Figure B.2 Markov Process Autocorrelation function
139
B.5 Monte Carlo Simulation
A Monte Carlo simulation (using 100 iterations) was used to verify that as the
frequency, time constant and bias standard deviation are changed, the first order Markov
process continues to meet the desired output requirements. The results of the Monte
Carlo simulation are shown in Figure B.3. Although there are some discrepancies in the
magnitude of the Markov outputs, as the desired bias standard deviation increases these
errors are still acceptable. The errors are acceptable for this work because the average
Markov output used will be much less than ten.
Figure B.2 Markov Process Comparison Plot
140
APPENDIX C
AN INTROCUCTION TO THE KALMAN FILTER
C.1 Introduction
The Kalman filter is a set of mathematical equations that provides an efficient
computational (recursive) means to estimate the state of a process in a way that
minimizes the mean of the squared error. The filter is very powerful in several aspects. It
supports estimations of past, present, and even future states, even when the precise nature
of the modeled system is unknown. It also allows for methods to fuse high rate input,
such as rate gyroscopes and accelerometers with GPS. Although these estimates are not
observable, they have been shown capable of being used in many navigation and control
systems. [Bevly] [Demoz]
C.2 System Format
The Kalman Filter will estimate the states, , of a discrete system that are
governed by the linear stochastic difference equation
n
x ??
111 ???
++=
kkdxdk
wuBxAx
(C.1)
and are observed with the measurements which are represented as
n
y ?=?
141
kkdk
vxCy +=
(C.2)
The random variables and v represent the process and measurement noises.
They are assumed to be white, normally distributed and independent of each other.
k
w
k
[ ] 0=wvE
[ ] 0=wE , [ ] 0=vE
[ ]
2
w
T
d
wwEQ ?== , [ ]
2
v
T
d
vvER ?==
[ ]
d
QNw ,0~, [ ]
d
RNv ,0~
In practice, the process noise covariance matrix, Q and the measurement noise
covariance matrix, might change with each time step. However, in this work both are
assumed to be constant for all time.
d
d
R
The matrix relates the state at the current time step to the state at the
next time step k . The
nn?
+
d
A
n
k
1 l? matrix relates the input u to the states
d
B
l
?? x . The
matrix relates the states to the measurements . In this work, the matrix C
will change depending on the output rate of the IMU and the output rate of the GPS
receiver.
nm?
d
C z
d
C.3 State Format
Defining to be the state estimate at time step given knowledge of the
process prior to step k , and defining
() n
k
x ??
?
? k
( ) n
k
x ??
+
? to be the state estimate at step given
measurement , the state estimate error then becomes
k
k
z
142
( ) ( )??
?=
kkk
xxe
( ) ( )++
?=
kkk
xxe
(C.3)
(C.4)
The state estimate error e covariance matrix is defined as
()?
k
() () ()
( )[ ]
T
kkk
eeEP
???
=
(C.5)
and the state estimate error
( )+
k
e covariance matrix is defined as
() () ()
( )[ ]
T
kkk
eeEP
+++
=
(C.6)
The equation which computes the state estimate
( )+
k
x? is a liner combination of the state
estimate and a weighted difference between an actual measurement and a
predicted measurement . This measurement update equation is as follows:
()?
k
x?
k
z
()?
kd
xC
( ) ( ) ( )
( )
??+
?+=
kdkdkk
xCzLxx
(C.7)
The difference
()
( )
?
?
kdk
xCz is called the measurement innovation. It reflects the
discrepancy between the predicted measurement and the actual measurement. A
innovation of zero would mean that both are in complete agreement.
C.4 Gain Format
The matrix is the gain that minimizes the error covariance matrix mn?
k
L
( )+
k
P ,
known as the Kalman gain. The following equation is used to calculate the Kalman gain.
( )
()
d
T
dkd
T
dk
k
RCPC
CP
L
?
=
?
?
(C.8)
143
By observation, it can be seen that when the measurement noise covariance matrix R
approaches zero, the gain matrix weights the measurement innovation more heavily. L
1
0
lim
?
?
=
dk
R
CL
d
However, when the state estimation error covariance matrix
( )?
k
P approaches zero, the
gain matrix weights the measurement innovation less heavily. L
()
0lim
0
=
?
?
k
P
L
k
Therefore, the measurement noise covariance matrix approaches zero and the actual
measurement is trusted more and more while the predicted measurement is trusted less
and less. Likewise, when the state estimation error covariance matrix approaches the
actual measurement it is trusted less and the predicted measurement is trusted more
144
C.5 Kalman Filter Flow Chart
The steps laid out in the above sections are shown a flow chart below.
( ) ( )
0,,?
00
=
??
kPx
() ()
[ ]
1?
??
+=
k
T
dkd
T
dkk
RCPCCPL
() () ( )
( )
??+
?+=
kdkkkk
xCyLxx ???
( )
( )
( )?+
?=
kdkk
PCLIP
() ()
k
T
dkdk
QAPAP +=
+?
+1
() ()
kdkdk
uBxAx +=
+?
+
??
1
k
y
1+? kk
Initial Conditions:
Compute Kalman Gain:
Propagate to next time step:
Update Estimate with :
Update Covariance:
Figure C.1 Kalman Filter Flow Chart
145
APPENDIX D
THE AUTOCORRELATION FUNCTION
D.1 Introduction
For a discrete-time stochastic series, ( )kx , the autocorrelation function ( )?
x
R is
defined by the following equation:
() ()( )[] ()( )??? ?=?=
?
=??
kxkx
N
kxkxER
N
kN
x
0
1
lim
(D.1)
Note that the autocorrelation function is symmetric therefore the following is true:
( ) ( )?? ?=
xx
RR
(D.2)
The 2
nd
Moment of a stochastic series is defined as:
()[ ] ()
?
=??
=
N
kN
kx
N
kxE
0
22
1
lim
(D.3)
Therefore the autocorrelation function is equal to the 2
nd
Moment of the stochastic series
when ? is equal to zero as shown below
() ()( )[] ()( )0
1
00
0
lim
?=?=
?
=??
kxkx
N
kxkxER
N
kN
x
(D.4)
() ()
?
=??
=
N
kN
x
kx
N
R
0
2
1
0
lim
(D.5)
( ) ( )[ ]
2
0 kxER
x
= (D.6)
146
The autocorrelation function can be thought of as a means to determine how much
the series, , looks like the series, ()kx ( )??kx , a delayed version of . To understand
the utility of the autocorrelation function, a known discrete series will be shown.
()kx
Given the function:
()
?
?
?
?
?
?
= kkx
100
2
sin
?
(D.7)
where the second moment is:
( )[ ] ??= kkxE 5.0
2
(D.8)
Figure D.1 contains a plot of the above function. It also contains a plot of the
above function shifted for various values of ? . As expected, the plot shows the function
diverging from itself as 0 50??
x
R
(D.12)
Figure D.4 is a plot of the autocorrelation for the above random series. At ?
equal to zero the autocorrelation is equal to the 2
nd
Moment of the series and for 0>?
the autocorrelation is approximately equal to zero. The error in the autocorrelation for
the values of 0>? can be contributed to the number of data point in the series and the
integrity of the random number generator used.
150
151
Figure D.4 Autocorrelation of a Random Series