-
Filename Navigation
Page 1 of 140
Introduction This section discusses the overall theory of
operation of the Real Time Navigator (RTN) and presents test
results to verify its operation. The first part presents an
overview of the theory of operation of the software. Following that
is the mathematical details of the navigation and filtering
functions that comprise the RTN software package. Finally a
synopsis of the tests and test results are given.
Theory of Operation System Block Diagram
Figure 1 gives a block diagram of the basic RTN function. Some
of the processes are not pertinent to a discussion of the
Navigation function and its performance but are included for
completeness. The processes are as follows
GPS Server: This process receives the time tagged position,
velocity and timing data from the GPS NovAtel ProPak-LB receiver
via a serial interface. It also generates, via three Kalman Filters
utilizing GPS velocity, estimates of track, pitch and roll and
inertial acceleration for the IN-AIR restart mode.
IMU Server: This process receives the time of validity interrupt
from the Kearfott KI-4901 and the incremental velocities and angles
via a serial interface..
SocketTranslator: This process provides the Ethernet
communication link between the RTN and the Monitor/Control
program.
ModeManager: This process controls the automatic mode changes of
the system. Logger: This process provides an output capability to
dynamically save the raw
IMU and GPS data, the navigation data and other pertinent data.
Align: This process implements the three Alignment Kalman filters
used in the
RTN. Nav: This function is the heart of the RTN and implements a
strapdown
mechanization of the inertial navigation algorithm. Camera
Server: This process interfaces with the ROI camera. It receives
and time
tags the camera exposure interrupt. The process also receives
via a serial channel the Image Data Message and the Relative Roll
Angle Data Message from the camera SIU and it sends to the camera
the SIU Orientation Data Message.
Time Server: This process receives the GPS receiver 1PPS
interrupt and via a one state Kalman filter calibrates the
processor clock. The resultant clock bias estimate is used in a
Timer Software Class to time tag the various events.
-
Filename Navigation
Page 2 of 140
-
Filename Navigation
Page 3 of 140
Figure 1: RTN Block Diagram
-
Filename Navigation
Page 4 of 140
Mathematical Notation Throughout this text the following
conventions are used, except were noted.
1. Direction cosine matrices are denoted fiC where the subscript
indicates the initial coordinate frame and the superscript
indicates the final coordinate frame.
2. All coordinate frames are right-handed orthogonal and all
Euler angles are right-handed.
3. Three element Vectors are shown with an over arrow, e.g.
Vr
.
4. A skew symmetric matrix is denoted ar which indicates a
matrix with the form
00
0
xy
xz
yz
aa
aa
aa
Inertial Navigation Inertial navigation uses inertial
information to compute position and speed over the surface of the
earth and attitude with respect to north and local level. The
inertial information is in the form of linear acceleration and
rotational rate as measured by instruments (accelerometers and
gyroscopes) which utilize the affects predicted by Newtons laws of
motion. The purpose of the inertial navigation algorithms is to
cancel out from the inertial measurements those effects not due to
relative motion over the earths surface. These effects include the
rotation of the earth with respect to space, gravity and coriolis
forces. The resultant accelerations and rates are used to compute
the relative speed and position with respect to the earth and
attitude with respect to level and north.
There are many ways to mechanize the concept of inertial
navigation [1, 2, 3] using different types of instruments and
configurations. For the RTN, a strapdown mechanization is used. In
this approach the inertial instruments are fixed to the vehicle
body and the measurements are sampled and processed by algorithms
within a digital processor. Note that in this mechanization there
is no physical level platform, i.e. the level coordinate frame
(platform) only exists as a set of numbers in the computer. The
gyroscopes sense the rotation of the body with respect to inertial
space and after an adjustment for earth rotation and motion over
the surface of the earth are used to update the direction cosine
matrix from the instrument coordinates to a locally level frame.
This direction cosine matrix is used to transform the accelerometer
measurements from the instrument axes to the level frame. The
resultant components are then adjusted for gravity and coriolis
effects and numerically integrated to from velocity relative to the
earths surface. The level frame is called the navigation frame.
-
Filename Navigation
Page 5 of 140
Mathematically the strapdown navigation mechanization is
described by the following differential equations [1]. For level
acceleration
LesfLiL VgaCV
rrrrr&r++= )2(
and the derivative of the LiC matrix is given by
.+= LiLIL
iIi
Li
Li CCC
rr&
Where
LVr
: Level velocity with respect to earth, meters per second.
LiC : Direction cosine matrix from instrument coordinates to
level navigation
frame.
sfar
: Specific force measured by accelerometers in i
coordinates.
gr : Plumb gravity at current latitude and altitude.
r : Motion over the surface of the earth (transport rate in
radians per second).
er
: Current earth rate components in navigation frame.
iIi
r: Gyroscope output; space rate of i coordinates with respect to
inertial ( I )
frame coordinatized in instrument frame ( i ).
LIL
r: Rate of change of the level coordinates (navigation frame)
with respect to
inertial space. This is the summation of transport rate and
earth rate.
By correctly initializing and numerically integrating the above
equations, level velocity can be determined. From velocity,
position on the earth surface can be calculated by performing
another numerical integration.
The RTN exclusively uses the WGS84 physical constants and
gravity model given in [4].
The following describes the navigation equations used in the
RTN. Specifically the RTN uses a Wander Azimuth mechanization which
allows navigation over the poles without loss of information or
mathematical singularities in the computation [1].
Figure 2 presents the coordinate frames used in the RTN.
-
Filename Navigation
Page 6 of 140
Position Position in the RTN is represented by a quaternion ( qP
) and ellipsoidal altitude. The quaternion is used because of its
compact form, numerical accuracy and efficiency. The position
quaternion represents the direction cosine matrix from an earth
centered frame to the navigation frame which is a local level
wander azimuth frame. Altitude is computed by open-loop integration
of vertical velocity.
The earth centered frame is right handed orthogonal, is fixed to
the earth with its x axis along the earth spin axis and positive
through the north pole. The z axis is orthogonal to the x axis and
its positive axis is coincident with the Greenwich meridian. The y
axis is orthogonal to x and z, (see Figure 2).
Greenwich Meridian e
N
E
D,
Long
Lat
EZ
EX
EY
Locally Level Frame coincidentwith ISA position.
X
Y
Z
Wander Angle
Figure 2: Coordinate Frames
The wander azimuth frame is a right handed orthogonal frame
whose x axis is pointed north when wander angle is zero. The z axis
is pointed down. The direction cosine matrix
-
Filename Navigation
Page 7 of 140
from the earth centered frame to wander azimuth is given by the
following ordered set of single axis Euler rotations
)()()( LongRLatRRC xyze = where
is wander angle. Lat is latitude Long is latitude e as a
subscript or superscript denotes the earth centered frame as a
subscript or superscript denotes the wander azimuth frame
Position Initialization
The position quaternion represents the direction cosine matrix
eC . The RTN is initialized with the GPS latitude ( 0l ) and
longitude ( 0long ) and wander angle ( 0 ) is determined by the
Coarse Alignment Filter, i.e. gyrocompassing. The position
quaternion is only initialized with the latitude and wander angle.
Since initial longitude and the change in longitude during
navigation are both represented by single rotations about the same
axis (earth centered x) we need only initialize the position
quaternion to an initial longitude of zero. After that we simply
add the initial longitude to the change in longitude as computed
from the quaternion during navigation. The initial value of qP is
given by the following operation
=
=
)2
cos()2
cos(
)2
cos()2
sin(
)2
sin()2
cos(
)2
sin()2
sin(
00
00
00
00
0
0
0
0
l
l
l
l
hgfe
Pq
.
Position Update The position quaternion is time updated using
transport rate which is computed from the current velocity,
altitude and earth radii. Transport rate indicates the relative
motion of the navigator over the surface of the earth. Because the
RTN is mechanized as a wander azimuth navigator we need only update
the quaternion for level components of transport rate, i.e. x and y
. This means that the update can be simplified by the elimination
of the z component of integrated transport rate. The update
algorithm is
-
Filename Navigation
Page 8 of 140
=
=
+
+
4
3
3
1
)1(
FFF
hfegeffhgegh
hgfe
P yx
nn
nq
where
)2
cos(
)2
sin(
4
3
=
=
F
F
and yx, : Integrated value of transport rates over position
update interval minus the
position correction from the alignment process. : Magnitude of
integrated level transport rate.
Now the integrated transport rate is calculated via
=
z
x
y
x
y
x
where
x
x
is the position correction in radians.
and
=
=
0
1
1
01
1
Y
x
NED
N
E
NEDy
x
y
x VV
CR
R
Ctt
where t is the integration time ER is the prime radius of
curvature NR is the meridian radius of curvature
Position Direction Cosine The position correction cosine matrix
is calculated from the position quaternion via
-
Filename Navigation
Page 9 of 140
+++
+++
++
=
2222
2222
2222
)(2)(2)(2)(2)(2)(2
hgfeehfgfhegehfghgfegheffhegghefhgfe
C e
Position Angle Extraction Latitude, longitude and wander angles
are extracted from the position direction cosine matrix via
)(tan
)(tan
))(1
(tan
)1,1(
)2,1(1
0)3,3(
)3,2(1
2)3,1(
)3,1(1
e
e
e
e
e
e
CC
longCC
long
C
Clat
=
+=
=
.
Altitude Altitude is computed via trapezoidal integration of the
vertical component of velocity which includes corrections for
altitude error and vertical velocity error which are calculated by
the Alignment process.
Velocity Velocity is computed by transforming the accelerometer
measurements into the level coordinate frame and subtracting the
computed coriolis acceleration and gravity effects. The result is
then, after appropriate initialization, integrated to form level
velocity.
Velocity Initialization The three velocity integrals are
initialized to zero when the navigator is started using the Ground
Align mode. When an In-Air alignment is performed the GPS
velocities are used.
Velocity Update Velocity is updated using the compensated
accelerometer measurements. The raw accelerometer measurements are
compensated by bias estimates supplied by the Alignment process.
The accelerometer hardware performs an integration of the observed
inertial acceleration (specific force) and provides integrated
samples to the navigation processor. These samples represent the
incremental velocity change over the sample time.
-
Filename Navigation
Page 10 of 140
The velocity algorithm simply adds the incremental changes with
the appropriate corrections to the previous velocity. Thus
+
+
=
+ yznxyn
xznzxn
zynyzn
z
y
x
z
y
x
i
nz
y
x
nz
y
x
VVVVVV
t
altlatGt
VVV
VVV
CVVV
VVV
),(00
1
where
++
=
e
z
e
yy
e
xx
z
y
x
222
and zyxV ,, are the compensated accelerometer measurements. ),(
altlatG is Plumb gravity as a function of latitude and altitude.
yx, is the transport rate.
e is the earth rate vector in wander azimuth coordinates. t is
the time interval. zyxV ,, are velocity corrections supplied by the
Alignment process.
Attitude The attitude of the system is described to the outside
world by heading ( ), pitch ( ) and roll ( ) which are seen
pictorially in Figure 3. In this figure the axes labeled
ZYXI ,, are the instrument axes.
-
Filename Navigation
Page 11 of 140
North
EastDown
XI
YIZI
Figure 3: Attitude Angles
These angles are only for the convenience of the user and are
not the ones the navigation computer uses to describe attitude.
Attitude information in the RTN is contained in the quaternion qA .
We use the quaternion here for the same reasons as in the position
case. The direction cosine matrix relating the instrument axes ( i
) to the navigation frame ( ) is iC . This matrix is used to
transform the accelerometer measurements to the navigation
frame.
Attitude Initialization The attitude quaternion must be
initialized in either the Ground or In-Air alignment mode. For our
purposes here we will assume that either of these processes results
in a direction cosine matrix from the level frame to the instrument
frame and is made available to the navigation process.
Initialization comprises forming from this matrix an equivalent
quaternion ( qA ). Reference [3] gives the general procedure which
is more complicated that the position quaternion
initialization.
Attitude Update The attitude quaternion must be updated to
account for the motion of the inertial instruments with respect to
the level frame. This is done by using the compensated gyroscope
measurements. The raw gyroscope measurements are compensated for
bias estimates supplied by the Alignment process, earth rate and
transport rate. In terms of direction cosine matrices the attitude
update is represented as
)()1()()1(
nininini CCC ++ =
.
-
Filename Navigation
Page 12 of 140
The quaternion representation of this expression is
=
=
+ 4
3
3
3
1 FFFF
dcbacdabbadcabcd
dc
ba
Az
y
x
nn
q
where [ ]Tdcba ,,, are the quaternion elements
)2
cos(
)2
sin(
4
3
=
=
F
F
zyx ,, are the compensated gyroscope measurements
is the magnitude of r
.
The level frame rotates in inertial space due to earth rate and
transport rate. Thus in order to keep the navigation frame locally
level these rates must be subtracted from the measured rates. The
compensation for gyroscope measurements is
rrr
icor C=
where
++
+=e
z
e
yy
e
xx
t
rr
and cor
ris the gyroscope measurements corrected by the Alignment
process bias
estimates.
ris the attitude correction vector from the Alignment
process.
Attitude Direction Cosine Matrix The accelerometer measurements
must be transformed from their coordinate frame to the locally
level navigation frame. The direction cosine matrix to do this is
calculated from the attitude quaternion elements via
-
Filename Navigation
Page 13 of 140
++
++
++
=
2222
2222
2222
)(2)(2)(2)(2)(2)(2
cbadadbcbdacadbccbadcdabbdaccdabcbad
Ci
.
This matrix is also used to transform Alignment process
corrections, transport rate and earth rate to instrument
coordinates.
Attitude Angle Extraction The RTN has as an initial attitude
output matrix ciC which can be used to calculate the standard
attitude angles from any frame fixed with respect to the instrument
frame to the local level North frame. Heading, pitch and roll are
calculated via
iciC CCC
=
and
=
=
=
)3,3(
)2,3(1
2)1,3(
)1,3(1
)1,1(
)1,2(1
tan
)(1tan
tan
c
c
c
c
c
c
CC
C
C
CC
-
Filename Navigation
Page 14 of 140
Alignment Kalman Filters
The purpose of the Alignment filters is to correct the
navigation function for position, velocity and attitude and
calibrate the inertial instruments. The navigation corrections, as
generated by the various alignment filters, are incremental
corrections that are integrated by the navigation function. The
instrument corrections are only generated by the FAF and are summed
externally to the filter and applied to the instruments as total
corrections.
There are three Alignment filters in the RTN. The first is the
Coarse Align Filter (CAF) which is used to gyrocompass the system,
i.e. find true north. The CAF is only used when performing a ground
alignment and provides only level velocity, tilt and earth rate
corrections to the navigation function. The Fine Align Filter (FAF)
has two modes. The first mode is used during ground alignment and
after the CAF has completed and also utilizes the Navigator
velocity as a measurement. Velocity, attitude and instrument
corrections are generated by this filter and are applied to the
navigation function. The second mode of the FAF uses GPS position
measurements. The FAF using GPS measurements is the normal mode of
operation when the system is moving and provides position,
velocity, attitude and instrument corrections to the system.
Kalman Filter General Equations The CAF and FAF procedures
utilize the Kalman filter algorithm to process the data. The Kalman
filter is a formal procedure for calculation of state estimates of
a linear system given a measurement that has Gaussian additive
noise. The procedure guarantees optimality in a minimal least
squares sense. See reference [6] for a complete discussion.
The equations used in the filter are given here for
completeness. In the following sN is the number of states and mN is
the number of measurements.
nP is the state covariance matrix for iteration n of dimensions
sN square. n is the state transition matrix for iteration n of
dimensions sN square. This
transforms the state vector nx from time at 1n to time at n . nF
is the matrix of differential equations of state that are
integrated to form n of
dimensions sN square. nQ is the Model Noise covariance matrix,
of dimensions sN square, that is used
by the filter to represent un-modeled errors in the system. nK
is the Kalman gain matrix for iteration n of dimension sN by mN .
nH is the measurement transformation matrix of dimensions mN by sN
. This
relates the system state vector to the measurement observation.
nx is the system state vector of dimension sN .
-
Filename Navigation
Page 15 of 140
nz is the measurement vector of dimensions mN . nR is the
measurement covariance matrix of dimensions mN square.
In the following equation set a hat over a term, e.g. P ,
implies time propagation, and a bar, e.g. P , implies a measurement
update.
The time propagation of the state covariance is given by
n
Tnnnn QPP +=+1
where the state transition matrix is
= Fn e
and the Model noise matrix is
dqQ Tnnn ),(),(0
=
where q is the White Noise spectral density.
The Kalman gain is calculated via,
[ ] 1 += nTnnnTnnn RHPHHPK
which is used to update the state covariance via
[ ] [ ] TnnnTnnnnnn KRKHKIPHKIP += .
The state is time propagated using
nnn xx =+1
and updated using the Kalman gain and
[ ]nnnnnn xHzKxx += .
Coarse Align Filter (CAF) The CAF filter is used to initialize
the navigation system heading and provide leveling of the strapdown
platform. The CAF is invoked immediately following the initial
attitude determination of the system using the accelerometers. The
differential equations defining the CAF process model the velocity
errors introduced into the navigator due to an incorrect heading.
An incorrect heading causes the components of Earth Rate to be
-
Filename Navigation
Page 16 of 140
incorrectly subtracted from the measured space rates. This
causes the computational level platform to tilt which causes a
velocity error. The differential equations that define this process
are
YY
XX
XY
YX
Y
X
gVgV
==
=
=
==
&
&
&
&
&
&
00
Where YX , are incremental Earth Rate in Wander coordinates. YXV
, are velocity errors in Wander coordinates. YX , are the navigator
tilt errors about the level Wander coordinates. g is the
acceleration due to gravity.
The CAF process initially sets the system wander angle ( ) to
zero and position to the GPS values. Then velocity observations are
formed every second from the navigator and used as measurements to
the CAF. The CAF supplies incremental velocity and attitude
corrections to the navigator and adjusts the wander angle via
)tan(X
Ya
= .
The earth rate estimate is arrived at by summing the incremental
estimates ( YX , ).Note that all the states in the CAF are reset to
zero at the beginning of each filter cycle.
In Air Alignment
In-Air alignment is used to re-start the system when in flight.
In-Air alignment can be performed under the following
conditions:
1. GPS must be in at least single-point mode and provide
position and ground speed. 2. Speed must be greater than five
meters per second. 3. We assume speed is constant and that the
aircraft side slip and angle of attack are
small. Note that the aircraft may execute a coordinated turn
during In-Air Alignment.
Under these conditions the process is as follows: 1. Estimate
acceleration from GPS velocity. Three two state Kalman filters are
used
to accomplish this task. The filters are run using one second
velocity observations
-
Filename Navigation
Page 17 of 140
from the GPS. The filters are run continuously. The acceleration
estimates are [ ]TDEN vvv &&& ,, . Specific force is
formed from the acceleration estimates by subtracting gravity from
the down component of the acceleration estimates. Specific force
vector is then [ ]TDEN aaa ,, .
2. Heading is initialized to ground track using GPS velocities
(
=
N
E
VV
a tan ).
3. Pitch is initialized using GPS velocities (
+
=22
tanEN
D
VVV
a ).
4. The direction cosine matrix from the local level NED frame to
a frame fixed to the aircraft that is not rolled (body prime) is
calculated via, ( ) ( ) zyBN RRC = .
5. Transform the specific force estimates to the body prime and
calculate the roll
angle via
=
BZ
BY
a
aa tan .
6. Form the NED to body direction cosine matrix ( ) ( ) ( )
ZYXBN RRRC = . 7. Using the aircraft body to Pod DCM ( PBC ), the
Pod to camera DCM ( ( )PCPC )
and the camera to IMU DCM ( ICC ) calculate the initial
navigation DCM via ( TBNPBCPICNI CCCCC )(= . Initialize the
navigation DCM to this value.
8. Initialize the navigation position to the GPS position and
the navigation velocity to GPS velocity.
9. Command the PADS to navigation closed loop mode.
Fine Align Filter (FAF) The inertial navigation function in the
RTN utilizes a local level platform to compute the system attitude,
velocity and position. The purpose of the Kalman filter in the RTN
is to correct the navigation function for errors that arise because
of erroneous initialization or instrument (gyroscope and
accelerometer) errors. The FAF is a Kalman filter having the
following states
Nine INS error states, position error, velocity error, tilt
error and heading error. Three gyroscope bias errors coordinatized
in the instrument frame. Three accelerometer bias errors
coordinatized in the instrument frame. Three lever arm errors
coordinatized in the body frame. Three GPS position errors
coordinatized in the Wander frame.
The FAF can use as a measurement vector either velocity error or
position error. It is intended that when using the Navigator
velocity as the error measurement the system is not moving. The FAF
using velocity as a measurement is essentially an extension of
the
-
Filename Navigation
Page 18 of 140
ground alignment procedure. When the FAF uses position error as
a measurement the system can be in motion. In the RTN this is
called the Closed Loop mode.
The following sections describe the generalized INS error
equations. The specific equations used in the RTN FAF are then
given. This development includes the state differential equations,
instrument and lever arm error equations, Model noise functions and
the measurement equations.
Generalized INS Error Equations This section presents the
generalized INS error equations as developed in reference [5]. The
error equations given here are derived via first order perturbation
of the inertial navigation equations. Essentially these equations
describe mathematically how specific errors propagate through the
inertial navigation system to generate position, velocity and
attitude error. These equations provide the basic building blocks
for the Kalman filter design because they describe how errors
propagate through the navigation function.
Velocity error propagation is given by
AVAVrrrrrrrr
r& ++++= 22
where V
r is the velocity error.
r is the small angle vector of the level platform with respect
to the true reference. A
r is the true specific force applied to the system.
r is the error in transport rate.
r is the error in earth rate.
Vr
is the true velocity of the system. r is the true transport
rate.
ris the true earth rate.
Ar
is the accelerometer error.
The attitude error propagation is given by
rrrr& +=
where
+=rrr
and
-
Filename Navigation
Page 19 of 140
s is the small angle vector of the platform with respect to
where the navigation system computed reference is.
r
is the gyroscope error.
The small angle error between the true reference and the
computed reference is r
. This is a position error and its differential equation is
rrr
r& = .
Note that
rrr += .
The error in earth rate is given by
rrr
= .
These generalized error equations are used to derive the
specific error equations in the RTN by substituting the appropriate
constraints which include the Wander azimuth mechanization and
choice of coordinate system.
RTN Filter State Equations The following subsections provide the
details of the equations used in the FAF in the RTN.
INS Errors Table 1 presents the INS error equations that are
used in the FAF. In the table zyxSf .. is the specific force in
Wander coordinates as measured by the accelerometers. plumbg is
plumb gravity as given in reference [4].
-
Filename Navigation
Page 20 of 140
Table 1: INS Error Equations
States
XR YR ZR XV YV ZV X Y Z
XR&
y 1
YR&
x 1
ZR&
y x 1
XV& R
g plumb~
Z2 )(2 YY + ZSf YSf
YV& R
g plumb~
Z 2 )(2 XX + ZSf XSf
ZV& R
g plumb~
2
)(2 YY + )(2 XX + YSf XSf
X&
Z )( YY +
Y&
Z )( XX +
Z&
=
)( YY + )( XX +
-
Filename Navigation
Page 21 of 140
Instrument Errors The FAF in the RTN has states for gyroscope
and accelerometer biases. These are coordinatized in instrument
coordinates.
Gyroscope
Bias There are three bias states which are transformed in the
state transition matrix via
Bi gCrr = .
Accelerometer
Bias There are three bias states which are transformed in the
state transition matrix via
Bi aCArr = .
Lever Arm Errors The lever arm is the physical distance between
the GPS phase center of the antenna and the point of navigation of
the RTN. The lever arm, which is defined in the instrument
coordinates, must be transformed to the navigation frame for use in
calculating the position error between the GPS and the RTN. When
transforming this vector two error sources can corrupt the
resultant vector, these are INS tilt and heading error and the
error in measuring the lever arm in the instrument frame. An
analysis of this transformation results in the following expression
for the transformed lever arm. In this expression BL is the true
lever arm in the instrument coordinates, L is the measurement error
of the lever arm in instrument coordinates and is the tilt and
heading error of the INS and L is the erroneous lever arm in the
navigation frame.
BBBBB LCLCLCLrrrrr
++=
This expression is used to model the affects of these errors on
the position observation as described in a following section.
Model Noise There are several instrument error sources that are
not specifically modeled in the FAF. Typically these errors cause
position errors in the INS that have short time affects. For
example gyroscope scale factor error only comes into play during a
turn. The design goal of the FAF is to account for these errors in
such a manner that the FAF will reduce its Kalman gain matrix when
these un-modeled errors are affective. In this manner the FAF
-
Filename Navigation
Page 22 of 140
states will not be corrupted by the variations in position error
caused by the un-modeled error sources.
The Kalman filter Model Noise covariance matrix ( nQ ) is the
mechanism for adjusting the system state covariance matrix to
account for the un-modeled error sources. Essentially a covariance
function is designed to represent each of the un-modeled errors and
included in the Model Noise covariance elements that are
applicable. This means that gyroscope errors are added to the tilt
and heading ( ) covariance elements and accelerometer errors are
added to the velocity error ( V ) covariance elements.
Gyroscope The gyroscope is subjected to drifts due to a plethora
sources. The following are the most important for the ring laser
gyroscope as is used in the Kearfott KI-4901 IMU which the RTN
uses.
The following sections provide the mathematical models used to
represent the gyroscope un-modeled errors as covariance matrices.
These matrices all represent the individual errors in the
navigation frame and are used in the FAF state covariance time
propagation
nQ matrix state elements. The summed gyroscope un-modeled errors
are
grwgcBgn QQQQ ++= )( .
Axis Misalignment This error is caused by the non-orthogonal
nature of the instrument cluster assembly. Essentially some small
fraction of the rotation about one principal axis is coupled to
another. In this model 2 g is the uncertainty in the misalignment
and g is an effectiveness term adjusted by simulation and field
testing so that the FAF ignores the un-modeled error.
The expression that models this error is
ggit
xy
zx
zy
ig dtCCQ
+
+
+
= 2
022
22
22
Note that this model is driven by the current space rates the
system is being subjected to and that the error source is
coordinatized in the instrument frame. This results in a covariance
matrix with off-diagonal terms.
Correlated Noise
-
Filename Navigation
Page 23 of 140
This error source represents time correlated error, i.e. errors
that follow a first order Markov model. In this model 2gcB is the
uncertainty in the drift and gcB is an effectiveness term adjusted
by simulation and field testing so that the FAF ignores the
un-modeled error. The time correlation of the error is included in
the effectiveness term.
The expression that models this error is
gcBgcBgcBQ = 2 .
Note that this error can be modeled directly in the navigation
frame since the similarity transformation is not required. This
covariance matrix is diagonal.
Wide Band Noise Wide band noise refers to the classic Angle
Random Walk specification given for practically any gyroscope. In
this model 2grw is the random walk and t is the FAF iteration time
increment.
tQ grwgrw = 2
Note that this error can be modeled directly in the navigation
frame since the similarity transformation is not required. This
covariance matrix is diagonal.
Accelerometer
The following sections provide the mathematical models used to
represent the accelerometer un-modeled errors as covariance
matrices. These matrices all represent the individual errors in the
navigation frame and are used in the FAF state covariance time
propagation nQ matrix V state elements. The summed accelerometer
un-modeled errors are
arwacBaSFan QQQQVQ +++= )( .
Misalignment This error is caused by the non-orthogonal nature
of the instrument cluster assembly. Essentially some small fraction
of the specific force along one principal axis is coupled to
another. In this model 2 a is the uncertainty in the sensitivity
and a is an effectiveness term adjusted by simulation and field
testing so that the FAF ignores the un-modeled error.
The expression that models this error is
-
Filename Navigation
Page 24 of 140
aait
xy
xia dtCaa
aCQ
+
= 2
022
2
0
Note that this model is driven by the specific force the system
is being subjected to and that the error source is coordinatized in
the instrument frame. This results in a covariance matrix with
off-diagonal terms.
Scale Factor Scale factor error affects the axis of measurement
and is a function of the specific force being applied to the
system. In this model 2aSF is the uncertainty in the sensitivity
and
aSF is an effectiveness term adjusted by simulation and field
testing so that the FAF ignores the un-modeled error.
The expression that models this error is
aSFaSFit
z
y
x
iaSF dtCa
a
a
CQ
= 2
02
2
2
Note that this model is driven by the specific force the system
is being subjected to and that the error source is coordinatized in
the instrument frame. This results in a covariance matrix with
off-diagonal terms.
Correlated Noise This error source represents time correlated
error, i.e. errors that follow a first order Markov model. In this
model 2acB is the uncertainty in the drift and acB is an
effectiveness term adjusted by simulation and field testing so that
the FAF ignores the un-modeled error. The time correlation of the
error is included in the effectiveness term.
The expression that models this error is
acBacBacBQ = 2
Note that this error can be modeled directly in the navigation
frame since the similarity transformation is not required. This
covariance matrix is diagonal.
Wide Band Noise
-
Filename Navigation
Page 25 of 140
Wide band noise refers to the classic Velocity Random Walk
specification given for practically any accelerometer. In this
model 2arw is the random walk and t is the FAF iteration time
increment.
tQ arwawB = 2
Note that this error can be modeled directly in the navigation
frame since the similarity transformation is not required. This
covariance matrix is diagonal.
Zero Velocity Measurements When the FAF and CAF use velocity
error as an observation the system is constrained to be stationary.
Thus any velocity generated by the navigation function can be
attributed to an error. The measurement for the FAF and CAF filters
is therefore
nn Vzrr
= .
The transformation of the FAF and CAF states to the measurement
is then
=
z
y
x
xn
VVV
XH
11
1.
GPS Position Measurements
In the Pinpoint system there are two components of the lever arm
from the GPS antenna to the Inertial Sensor Assembly navigation
point. The first is defined in aircraft body coordinates and the
second is defined in the inner camera axis system. Figure 1
presents the coordinate frames and lever arm components. The GPS
position information is measured at the antenna phase center which
is located at the origin of the aircraft body fixed coordinate
frame. Essentially the GPS position information has to be
translated to the origin of the Navigation frame which is the
origin of the ISA ( zyxI ,, ) frame. Note that the ISA frame is
fixed to the inner camera axis frame ( zyxC ,, ) and rotates with
respect to the camera pod frame ( zyxPOD ,, ) through the pod roll
angle P . The pod roll angle is measured by a resolver and sampled
at a five Hertz rate. At the GPS observation time the total lever
arm is transformed to ISA axes via
IC
IB
rrr+=
where ICr
is the fixed distance from the origin of the camera inner axis
system to the Navigation axes origin and is coordinatized in the
ISA frame.
-
Filename Navigation
Page 26 of 140
BP
CAOPCP
IC
IB CTCC rr /))((=
where )( OP T is the roll angle at the observation time as
interpreted from the five Hertz data stream.
The total lever arm is then transformed to geographic axes
via
rr
IGG CC= .
The Kalman filter position observation is
GOO TGpsTNavf = ))()((
where ()f transforms the angular difference (latitude and
longitude in radians) to linear, i.e. meters. Note that there are
several DCMs and the navigator position involved in this
calculation that are time dependant. The GPS observation time OT is
the UTC time that the GPS receiver time tags the GPS position. This
time is typically two hundred milliseconds behind real time. This
necessitates that the navigation data, DCMs and position, be placed
in a circular buffer so that the data can be retrieved at the
observation time OT . A second order curve fit of the data is used
to calculate the position at the observation time. The position
error is finally transformed into the Wander azimuth frame for
inclusion in the Kalman filter as an observation.
To complete the observation equation we need to model the source
of the errors in the observation. The filter models the
following
Position errors due to the integration of gyroscope and
accelerometer errors by the inertial navigation function. These are
the zyxR ,. error states
Position errors in the GPS reference, these are the zyxGPS ,,
error states. Position errors due to GPS to instrument lever arm
errors, these are the
zyxL ,, states. Position errors due to incorrect transformation
of the lever arm into navigation
coordinates.
The measurement matrix relates the FAF error states to the error
measurement and is
+
+
+
=
z
y
x
z
y
x
iiz
iy
ix
i
xy
xz
yz
z
y
x
nn
GPSGPSGPS
LLL
CLLL
CRRR
XH
00
0
11
1
-
Filename Navigation
Page 27 of 140
XC
YC
ZC
Cr
XI ZI
YIYPOD
ZPOD
P
XCA /
YCA /
ZCA /
Br
Flight Direction
GPS Antenna Phase Center
r
Figure 4: Coordinate Frames and Lever Arm
RTN Operation
The RTN at startup is initialized in Standby mode. The operator
must command the RTN, via the monitor program, to initiate ground
alignment or in-air alignment. After either alignment is complete
the RTN can be commanded to closed-loop operation.
Monitor Program Operation The monitor program can be run on any
Windows machine that is connected to a LAN that is connected to the
PADS. This is a standalone GUI process. Once a connection is made
to the RTN the monitor can be used to change the mode of the RTN
processes or monitor the key system parameters such as attitude and
position. Note that in the following the acronym PADS refers to the
machine the RTN is running on.
Details of Operation
When the Monitor program is first initiated it starts in the
Setup mode. The Setup page is shown in Figure 5. The Monitor will
attempt to connect to the last location it used. Changes to the
connection address are made via the Location item in the PADS
Communications box. The Font Size box can be used to change the
overall font size
-
Filename Navigation
Page 28 of 140
of the GUI. Clicking on the Note button causes an edit dialog
box to be presented that the user can type to. When this box is
closed the note is time tagged and written to the .note file in the
PADS. Once the Monitor has established communication to the PADS
the Initial Conditions dialog box will be presented. In normal
operation the, if the GPS is operating, the current GPS position
will be presented on the left side of the box. The operator can
change the current initial position by using the arrow buttons or
enter the data in the appropriate box. There are three other items
that the operator must enter in this dialog.
1. The root filename that will be used to name all the data
files. 2. The amount of time, in seconds, used to level the system.
3. The amount of time, in seconds, to run the Coarse Alignment
Filter. This process
performs Gyro-compassing to establish true heading.
Once these parameters have been entered the OK button is clicked
on and the system enters in the Leveling mode. Clicking on CANCLE
keeps the system in Standby. The operator should then switch to the
Main page by clicking on the Main tab. The Main page is shown in
Figure 7. This page allows the operator to monitor the following
information
1. The time the system is in Level and Coarse align. 2. The
system position and attitude and speed. 3. The GPS position and the
GPS derived track, pitch and roll. 4. The GPS solution type, GDOP
and HDOP and the number of satellites used in the
solution. 5. The IMU time and a count of the check sum
errors.
In addition a time counter is displayed to the right of the Mode
pull-down that gives the running time. When in leveling and heading
alignment modes this timer counts down from the initial values
entered in the Initial Conditions dialog box.
Figure 9 presents the mode pull-down items. Using this menu the
RTN can be commanded into various modes. Table 2 presents the valid
mode transitions for the RTN.
Table 2: Mode State Transition Diagram
Transition TO Mode Current Mode Standby Level/Coarse
Align Fine Align
InAir Align
Closed Loop
Suspend Open Loop
Stop
Standby N X X X Level/Coarse Align
X N T X
Fine Align
X N X X X X
InAir Align
X N X X X X
Closed X N X X
-
Filename Navigation
Page 29 of 140
Loop Suspend X X N X Open Loop
X N X
Stop N Valid transitions indicated with X, N indicates no
transition, T is an automatic transition.
The only automatic mode transition is from Level/Coarse to Fine
Align. The sequence of mode changes, after the Level/Coarse Align
command is given, is
1. Perform the initial level alignment by averaging the
accelerometer measurements over a time interval specified by the
user.
2. Initiate the Coarse Alignment Filter (CAF) for a time
interval specified by the user. This initializes true heading via
gyro-compassing and refines the level alignment.
Typically the operator initiates Level/Coarse Align via the
Initial Conditions dialog box. The system will automatically
transition through Level and Coarse Align to Fine Align. During the
Level/Coarse and Fine Align modes the Aircraft/Camera should not be
moved. The system will stay in Fine Align until the operator
commands the system to a valid mode. The most common mode to
transition to is Closed Loop in which the ALIGN Kalman filter uses
GPS to continually correct the navigation function. If GPS is not
available the system can be commanded to Open Loop or Suspend. Once
in Open Loop the system can not be reverted to any mode other than
Standby or Stop. The Suspend mode invokes the ALIGN Kalman filter
but the navigation function is operated without using corrections.
The operator can transition, however, into the Closed Loop mode
from Suspend. Note that there is no minimum time limit that the
system has to be in Fine Align and the most common commanded mode
transition is directly to Closed Loop as soon as Level/Coarse Align
is complete.
Figure 10 gives the Align page selected by clicking on the Align
tab. This page presents data pertinent to the Fine Align and ALIGN
filter performance. The box labeled Corrections gives the total
instrument corrections being applied to the navigator. The Standard
Deviations box shows the Kalman filter state standard deviations.
The units for both displays are given next to the variable name.
The Residuals box contains the Kalman filter measurement residuals.
Note that in Fine Align the units are meters per second and in
ALIGN the units are meters. The Circular Buffer index is the index
in the buffer holding the navigation data where the current GPS
measurement is aligned in time. The Time variable is the GPS time
of the observation.
Figure 11 presents the GPS data page. The current position,
speed and track angles are shown at the top of the page. The
Dilutions of Precision box gives the various DOPs and the time of
validity. The Satellites box gives the solution type, satellite
count, OminStar signal strength and the satellite signal strength
(C/N) by PRN. Also shown are the azimuth and elevation angles of
the satellites in view.
-
Filename Navigation
Page 30 of 140
Figure 12 shows the Time Filter page. This page displays
information about the time synchronization process in the RTN. The
Time Filter Parameters box shows key elements of the single state
Kalman filter that calibrates the processor clock to the GPS clock
via the 1PPS interrupt. The Time Stamps box presents the current
time stamp for the various processes.
Figure 13 gives the IMU page as selected by clicking on the IMU
tab. The Data for 1 Second box gives the time, message count and
the average instrument outputs over the last second. The Status box
shows the status byte, checksum error count and dropout count.
Once the RTN has been initialized and running the operator need
only monitor the key parameters as the need arises.
If the system must be restarted while moving (in air) the
operator must first command the RTN to Standby. The GPS must be
operational and speed greater than five meters per second. The
Track, Pitch and Roll parameters must be displayed on the GPS data
line of the Main page. The operator commands the RTN into
in-air-alignment mode by selecting the INAIRALIGN item in the Mode
pull-down. While in this mode the Heading, Pitch and Roll items in
the System data line should follow the same items in the GPS data
line. The system can be commanded to INAIRALIGN in any orientation
of the aircraft or camera. The navigation system will be reset to
the GPS derived parameters every second and perform free inertial
navigation between the GPS observations. The operator can command
the RTN to OPEN LOOP, CLOSED LOOP or SUSPEND at any time. Typically
the system is commanded to CLOSED LOOP soon after selecting
INAIRALIGN.
-
Filename Navigation
Page 31 of 140
Figure 5: Monitor Setup Page
-
Filename Navigation
Page 32 of 140
Figure 6: Monitor Initial Conditions Dialog Box
-
Filename Navigation
Page 33 of 140
Figure 7: Monitor Main Page: During Leveling
-
Filename Navigation
Page 34 of 140
Figure 8: Monitor Main Page: During Heading Alignment (CAF)
-
Filename Navigation
Page 35 of 140
Figure 9: Monitor Mode Control Pull-down
-
Filename Navigation
Page 36 of 140
Figure 10: Monitor Align Page
-
Filename Navigation
Page 37 of 140
Figure 11: Monitor GPS Page
-
Filename Navigation
Page 38 of 140
Figure 12: Monitor Time Filter Page
-
Filename Navigation
Page 39 of 140
Figure 13: Monitor IMU Page
-
Filename Navigation
Page 40 of 140
Figure 14: Monitor Main Page: Commanding RTN to "Closed
Loop"
-
Filename Navigation
Page 41 of 140
-
Filename Navigation
Page 42 of 140
Testing of the RTN The inertial strapdown navigator and
alignment Kalman filters are embodied in a software package called
the Real Time Navigator (RTN). Testing and validation of this
function is based on well known differential error equations
describing the strapdown navigator behavior when subjected to
various instrument and alignment errors, see references 1 through 3
and 5. The development path of the RTN involved first programming
it in C and stimulating it with known controlled inputs, i.e. using
a simulation of the instruments which generated perfect instrument
outputs. This allowed the RTN to tested over a range of profiles
and instrument errors. Results of these tests are not reported
here. After the RTN passed these initial tests the program was
embedded in a real time wrapper which resulted in the RTN process
which is executed on the PADS hardware under the QNX operating
system. The wrapper allowed the process to acquire data from the
actual instruments and output the navigation data. A discussion of
this wrapper is not included here.
Testing of the RTN in its real time form is reported in the
following sections. Before any conclusions could be drawn from the
results of the various tests the instruments that were to be used
had to be characterized. This was accomplished with Cluster
Analysis [7] of the accelerometers and gyroscopes. The results of
this testing is reported first. These results were used to
determine what performance we should expect and also used to Tune
the Alignment Kalman filters to the real instrument noise.
-
Filename Navigation
Page 43 of 140
After the instruments had been characterized several types of
stationary tests were conducted. The first was the stationary free
inertial test, then a closed loop test, followed by rotation tests
and lastly heading alignment tests.
Following the stationary tests the system was installed in a
truck for road testing. This involved general navigation, ground
and simulation in-air alignment.
A short flight test was conducted by AAI to evaluate the RTN in
the flight environment. This is reported on after the truck
testing.
Finally the last section gives specifics about the NRL flight
tests.
Stationary Testing
Cluster Analysis Procedure Cluster Analysis is another name for
the methodology of generating an Alan Variance. Reference 7 gives a
discussion of Cluster Analysis as it applies to RLG Noise analysis.
Essentially Cluster Analysis provides an easy way to determine the
noise variance at a given correlation time. This is accomplished by
breaking the time sequential data into adjoining clusters of the
same time span. Within each cluster the data is averaged. This
results in a series of compressed data packets where each packet or
cluster contains the averaged data over the same length of time.
Each cluster is then differenced with its neighbor and the variance
of the series of differences calculated. Thus for one cluster
length we get one variance which represents the variation in the
data at that correlation time. This process is repeated for
different cluster lengths. Obviously, for a given fixed length data
set, the longer the cluster length is the fewer clusters are
available which means the estimation accuracy of the variance
decreases with increasing cluster length.
Reference 7 describes a technique that allows one to graphically
determine from a Cluster Analysis plot parameters such as random
walk, exponentially correlated noise, bias instability etc. This
technique was used to analyze the Cluster Analysis plots.
The data set used to generate the Cluster Analysis data was
approximately sixteen and a half hours long and was collected in
the AAI laboratory.
The data was post-processed which resulted in Cluster Analysis
plots.
Cluster Analysis Results For the sixteen and half hour IMU data
set, Figure 15 presents the estimated percentage error in the
Cluster standard deviation as a function of the correlation time.
This reflects the smaller number of samples available as the
correlation time gets larger. Figure 16 and Figure 17 presents the
Accelerometer and Gyroscope Cluster Analysis plots derived from
-
Filename Navigation
Page 44 of 140
the KI-4901 data. Figure 18 and Figure 19 present the Cluster
Analysis plots derived from the Phalanx IMU.
Table 3 summarizes the significant parameters from the Cluster
Analysis plots. Note that this table does not include all the
parameters that could be estimated from the Cluster Plots such as
rate random walk, quantization and rate ramp errors.
Comparison of the Kearfott and Phalanx parameters and Cluster
Plots shows that all the Kearfott accelerometers are comparable to
the z axis Phalanx accelerometer and that the y and x axis Phalanx
accelerometers have significantly less random walk then the
Kearfott accelerometers. The Phalanx gyroscope is not even in the
same class as the Kearfott device. All of the gyroscope parameters
are significantly better then the Phalanx.
Table 3: Parameter Estimates from Cluster Analysis
Instrument Random Walk Bias Instability Correlated Error
Measured Specification Measured Specification Time
Constant Uncertainty
KI-4901 Gyroscope
(1.60.4)e-3 /Hr
0.003 /Hr (2.870.6)e-3 /Hr
0.003 /Hr n.a. n.a.
KI-4901 Accelerometer
0.140.04 ft/s/Hr
0.164 ft/s/Hr (5 0.5)g 50 g 10 sec 40 g
Phalanx Gyroscope
(30.8)e-3 /Hr
n.a. (0.020.0014) /Hr
n.a. n.a. n.a.
Phalanx x & y Accelerometer
< 0.01 ft/s/Hr
n.a. X (1 0.05)g Y (1.5 0.08)g
n.a. n.a. n.a.
Phalanx z Accelerometer
0.130.03 ft/s/Hr
n.a. (2 0.1)g n.a. n.a. n.a.
-
Filename Navigation
Page 45 of 140
100 101 102 103 104 10510-1
100
101
102
Correlation Time T (sec)
Es
tima
ted
Pe
rce
nt
Err
or
Figure 15: Cluster Analysis Estimated Percentage Error
-
Filename Navigation
Page 46 of 140
100 101 102 103 104 10510-2
10-1
100
101
Correlation Time T (sec)
(T)
(ft
/se
c/h
r)
Cluster Analys is of KI4901 SN0008 Accelerometers26-Dec-2003
AxAyAz
Figure 16: KI-4901 Accelerometer Cluster Analysis
-
Filename Navigation
Page 47 of 140
100 101 102 103 104 10510-4
10-3
10-2
10-1
100
Correlation Time T (sec)
(T
) (de
g/hr
)
Cluster Analysis of K I4901 SN0008 Gyroscopes.26-Dec-2003
GxGyGz
Figure 17: KI-4901 Gyroscope Cluster Analysis
-
Filename Navigation
Page 48 of 140
100 101 102 103 104 10510-2
10-1
100
101
Correlation Time T (sec)
(T)
(ft
/se
c/h
r)
Cluster Analys is of Phalanx Accelerometers06-Jan-2004
AxAyAz
Figure 18: Phalanx Accelerometer Cluster Analysis
-
Filename Navigation
Page 49 of 140
100 101 102 103 104 10510-4
10-3
10-2
10-1
100
Correlation Time T (sec)
(T
) (de
g/hr
)
Cluster Analys is of Phalanx Gyroscopes.06-Jan-2004
GxGyGz
Figure 19: Phalanx Gyroscope Cluster Analysis
Stationary Bench Test Free Inertial Results
The purpose of this test was to demonstrate the free inertial
performance by allowing the inertial navigator to run for an
extended period of time in the free mode, i.e. no corrections from
the Alignment Kalman filter. In this case the run lasted sixteen
and half hours. The free performance was preceded by three minutes
of ground alignment. The IMU x axis was initially pointed south and
the unit was not rotated. Vertical position was not computed.
Figure 20 presents the position error, latitude and longitude, as a
function of time. Note the expected eighty four minute period
modulation and the peak error at twelve hours. These are related to
the Schuler tuned inertial navigation algorithm. See reference 1
for a discussion. Typically an inertial navigator performance is
quantified by its radial position error drift rate in nautical
miles per hour. Figure 21 presents the radial position error for
this test. The line labeled linear represents a least squares curve
fit of the radial error. The slope of this line is representative
of the drift rate of the system and is seen to be 7.8 meters per
minute which is 0.25 nautical miles per hour. This is consistent
with the inertial instrument performance derived from the Alan
variance analysis. Figure 22 presents the velocity error as a
function of time. Again, the magnitude of the velocity errors and
the modulation are all well within what is expected from an
inertial navigator using instruments of this quality.
-
Filename Navigation
Page 50 of 140
0 100 200 300 400 500 600 700 800 900 1000-8000
-6000
-4000
-2000
0
2000
4000
6000
8000Errors wrt reference
Lat,
Lo
n, A
lt E
rro
r (m
)
Time (min)
LatLonAlt
Figure 20: Free Inertial Performance Position Error
-
Filename Navigation
Page 51 of 140
0 100 200 300 400 500 600 700 800 900 10000
2000
4000
6000
8000
10000
12000Radial Error
Err
or
(m)
Time (min)
y = 7.8*x + 2.4e+003
data 1 linear
Figure 21: Free Inertial Performance, Radial Error
-
Filename Navigation
Page 52 of 140
0 100 200 300 400 500 600 700 800 900 1000-1.5
-1
-0.5
0
0.5
1
1.5
2Veloc ity Errors wrt to Reference
Time (min)
Ve
loc
ity E
rro
rs (m
/s)
Vx
Vy V
z
Figure 22: Free Inertial Performance Velocity Error
Stationary Bench Test Closed Loop Results In this test the
sixteen and half hour data set was processed by the Nav/Align
program using GPS data in the closed loop mode. GPS data was
simulated by using the fixed known position of the test stump at
AAI as the reference measurement. The purpose of the test was
twofold. First, to demonstrate that the Nav/Align process works
with real IMU data. Second, to determine the closed loop position
performance for the stationary case. Figure 23 shows the system
latitude and longitude position error in meters. Note that there is
no noise on the position reference so the variations seen in the
figure are only due to the IMU noise and processing. For this test
the GPS observation noise level was set to 0.3 meters for latitude
and longitude and 0.6 meters for altitude (one sigma).
-
Filename Navigation
Page 53 of 140
-0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15 0.2 0.25-0.2
-0.15
-0.1
-0.05
0
0.05
0.1
0.15
0.2Latitude vs Longitude (m)
Longitude Error (m)
Latit
ude
E
rro
r (m
)
Figure 23: Closed Loop Position Error
Bench Rotation Test Results
The purpose of the rotation test was to verify that the RTN
software correctly uncouples the accelerometer bias from the
initial tilt error. In addition the stability of the attitude
measurement, after instrument error estimation, was
demonstrated.
The RTN system performed a sixty second ground alignment
followed by forty seconds of Fine Alignment and then transition
into Nav Closed loop using GPS observations. The test sequence was
as follows
1. Initial heading of unit, East. 2. Rotate to West at 300
seconds. 3. Rotate to East at 720 seconds. 4. Rotate to West at
1080 seconds. 5. Rotate to East at 1620 seconds. 6. Run terminated
at 5280 seconds.
Note that the ISA axes are as follows:
-
Filename Navigation
Page 54 of 140
1. The z axis is along the center of the cylindrical case. The
positive direction is the end of the case where the high voltage
connector is. The z axis lies in the plane described by the
isolator mounting holes.
2. The x axis is in the plane described by the isolator mounting
holes and pointed to the right as one looks down on the cylindrical
case with the positive z axis up.
3. The y axis is positive down and is right-handed orthogonal to
x and z.
Note that heading is defined as the angle from north to the
projection of the ISA x axis onto the level plane.
Figure 24 presents the attitude variation about the ISA x axis (
). Note that as the ISA is rotated the angle variation becomes more
stable. This is because, first the accelerometer bias is estimated
and then the gyro bias. The result is the stable angle shown from
the last rotation time to the end of the test. The variation is
well within 25 radian. Figure 25 shows the attitude variation about
the ISA z axis ( ) which has similar characteristics. Figure 26
gives the Align Kalman filter attitude and heading uncertainties.
These are commensurate with the observed variations. Figure 27
presents the heading variation. In this case the heading drift rate
has not been calibrated to the extent the level gyroscopes were so
there is a dynamic drift. The heading error is being minimized by
the system gyro-compassing as can be seen by the decrease in
heading uncertainty in Figure 26. Finally, Figure 28 presents the
accelerometer bias estimates and uncertainties. Of note in the
accelerometer bias plot is the fact that the vertical channel bias
estimate is large, in fact its not shown on the plot. This was
eventually traced to an incorrect gravity model. This error was not
uncovered in simulation testing because the same gravity model was
used in the simulation as in the RTN. The other accelerometer
biases were commensurate with the KI-4901 specification.
-
Filename Navigation
Page 55 of 140
500 1000 1500 2000 2500 3000 3500 4000 4500 50009.9
9.95
10
10.05
10.1
10.15
10.2
10.25
10.3Errors wrt reference
He
adi
ng,
R
oll
an
d P
itch
Err
or
(mr)
Variation about X ax is
Figure 24: Rotation Test Attitude variation about the ISA case x
axis
-
Filename Navigation
Page 56 of 140
500 1000 1500 2000 2500 3000 3500 4000 4500 5000-9.4
-9.3
-9.2
-9.1
-9
-8.9
-8.8
-8.7
Errors wrt reference
He
adi
ng,
R
oll
an
d P
itch
Err
or
(mr)
Variation about z ax is
Figure 25: Rotation Test Attitude variation about the ISA case z
axis
-
Filename Navigation
Page 57 of 140
1000 2000 3000 4000 5000 6000
0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Attitude Error S tate Std Dev
x,
y,
z (m
r)
x
y
z
Figure 26: Rotation Test Align Attitude and Heading
Uncertainties
-
Filename Navigation
Page 58 of 140
1500 2000 2500 3000 3500 4000 4500 500016
16.2
16.4
16.6
16.8
17
17.2
17.4
17.6
17.8
18
Errors wrt reference
He
adi
ng,
R
oll
an
d P
itch
Err
or
(mr)
Variation in Heading
Figure 27: Rotation Test Heading variation
-
Filename Navigation
Page 59 of 140
1000 2000 3000 4000 5000
-250
-200
-150
-100
-50
0E
st
(Mic
roG
s)
Accel B ias Estimates
Time (sec)
AbxAbyAbz
0 2000 4000 60000
50
100
150
200
250
300
350
400
Es
t (M
icro
Gs
)
Accel B ias Std Dev
Time (sec)
Abx Aby Abz
Figure 28: Rotation Test Accelerometer bias estimates
Coarse Alignment (CAF) Testing The CAF provides an automatic
alignment of the system with true north via wide-angle
gyro-compassing when stationary.
One problem that was uncovered in translating the CAF from the
simulation environment to the RTN was the systems inability to
correctly accomplish multiple alignments without completely
restarting the system. In other words every time we terminated the
alignment by commanding Standby and then restarting the alignment
the resultant alignment got progressively worse. This was traced to
a counter not being reinitialized in the RTN on subsequent Standby
commands.
Multiple tests of the CAF mode were accomplished, however only
three examples will be presented here. Figure 29 and Figure 30
present the heading and CAF velocity residuals for a -90 degree
heading. Figure 31 and Figure 32 give the +90 degree case and
Figure 33 and Figure 34 present the 180 degree case.
-
Filename Navigation
Page 60 of 140
0 20 40 60 80 100 120-100
-90
-80
-70
-60
-50
-40
-30
-20
-10
0
Time (sec)
Wan
der
Angl
e (de
g)
Figure 29: CAF Test
-
Filename Navigation
Page 61 of 140
0 20 40 60 80 100 120-3.5
-3
-2.5
-2
-1.5
-1
-0.5
0
0.5
1x 10-3
Resi
dual
(m/s
)
Filter Residuals
Time (sec)
X
Y
Figure 30: CAF Test
-
Filename Navigation
Page 62 of 140
0 20 40 60 80 100 1200
10
20
30
40
50
60
70
80
90
Time (sec)
Wan
der
Angl
e (de
g)
Figure 31: CAF Test
-
Filename Navigation
Page 63 of 140
0 20 40 60 80 100 120-0.025
-0.02
-0.015
-0.01
-0.005
0
0.005
0.01
0.015
0.02
0.025Re
sidu
al(m
/s)
Filter Residuals
Time (sec)
X
Y
Figure 32: CAF Test
-
Filename Navigation
Page 64 of 140
0 20 40 60 80 100 120-20
0
20
40
60
80
100
120
140
160
180
Time (sec)
Wan
der
Angl
e (de
g)
Figure 33: CAF Test
-
Filename Navigation
Page 65 of 140
0 20 40 60 80 100 120-6
-5
-4
-3
-2
-1
0
1x 10-3
Resi
dual
(m/s
)
Filter Residuals
Time (sec)
X
Y
Figure 34: CAF Test
Ground Truck Testing The purpose of truck testing is to subject
the RTN to dynamic inputs such as linear acceleration, constant
speed and rotations. Two tests are reported on here, first a short
test that demonstrated the ability of the system to track the GPS
input and a gross check on the report heading and attitude and
overall response to dynamic variables such as GPS position
uncertainty. The second test subjected the system to a long road
test. Key parameters that were observed here were heading and
overall stability. Secondarily the RTN software was being tested
for memory leaks or any other term error anomaly.
Short Test The first truck road test of the system was conducted
on February 25, 2004. The test included the Ampro development
system, the Kearfott KI-4901 ISA its electronics and the NovAtel
ProPak-LB receiver. A laptop PC was connected to the system via the
Ethernet. The PADS/C control and monitor program was run on the
laptop and used to control the PADS software residing on the Ampro
development system. The PADS software included executive functions
to collect data from the GPS receiver and the KI-4901 and log all
the required data to the hard drive and manage the navigation
and
-
Filename Navigation
Page 66 of 140
Kalman filter processes. The system was started by transitioning
from standby to coarse align. Coarse align levels the navigation
frame and initializes heading and position. Coarse align was
followed by Fine align which continued to refine heading and
attitude. Finally the system was commanded to Nav-Closed-Loop. This
invokes the ALIGN Kalman filter that used GPS as the position
reference. At the time of this test the CAF was not available so an
initial heading had to be manually input into the system.
Figure 35 shows the test vehicle and with the equipment pallet.
Figure 36 presents the test pallet. The ISA was orientated on the
pallet with the X axis pointed to the front of the truck. The GPS
antenna was mounted on a tripod in the truck bed. The pallet
requires 115 VAC 60Hz power to run the AMPRO development system and
the KI-4901 power supply. The NovAtel ProPak-LB connects directly
to the truck 12 VDC power plug. AC power comes from a Tripp Lite DC
to AC converter, model PV1000FC. This unit supplies a low
distortion sine wave power source that can sustain 1000 watts. For
our purposes 180 watts was sufficient.
Figure 37 gives the test profile. Initial heading was north.
Figure 38 presents the vehicle heading. Figure 39 through Figure 42
present Kalman filter performance data during the test. Note in
Figure 40 and Figure 41 the transition from Fine Align to Nav
Closed Loop at around 40 seconds. At this point the measurement to
the filter changes from navigator velocity to GPS position. The
jump in innovation uncertainties (Figure 40) at around 530 seconds
was due to the vehicle passing under a railroad bridge. The loss of
GPS signal is apparent in the residuals (Figure 41). Note, however,
that whereas the innovation uncertainty jumped to a meter or more
the position uncertainty (Figure 42) only increased to a quarter
meter. This is attributable to the inertial navigators ability to
provide very good short term position information.
-
Filename Navigation
Page 67 of 140
Figure 35: Test Vehicle
-
Filename Navigation
Page 68 of 140
Figure 36: Test Pallet
-
Filename Navigation
Page 69 of 140
-85.585 -85.58 -85.575 -85.57 -85.565 -85.56 -85.555
-85.5542.886
42.888
42.89
42.892
42.894
42.896
42.898
42.9
42.902
Longitude (deg)
Latit
ude
(d
eg)
Road Test 1
Figure 37:Short Road Test Profile
-
Filename Navigation
Page 70 of 140
0 100 200 300 400 500 600 700
-150
-100
-50
0
50
100
150H
ea
din
g (de
g)
Time (sec)
Figure 38:Short Road Test Profile Heading
-
Filename Navigation
Page 71 of 140
0 100 200 300 400 500 600 7000
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
Es
t (m
illra
d)
Tilt and Heading Error Std Dev
Time (sec)
Figure 39:Short Road Test Kalman Filter Tilt and Heading Error
Uncertainties
-
Filename Navigation
Page 72 of 140
100 200 300 400 500 6000
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
Filte
r In
no
vatio
ns
(m
)
Kalman Filter Innovations
Time (sec)
Figure 40:Short Road Test Kalman Filter Innovation Covariance
Diagonal Elements
-
Filename Navigation
Page 73 of 140
100 200 300 400 500 600 700-0.5
-0.4
-0.3
-0.2
-0.1
0
0.1
0.2
0.3
0.4
0.5
Filte
r R
es
idu
als
(m
)
Kalman Filter Residulas
Time (sec)
Figure 41:Short Road Test Kalman Filter Residuals
-
Filename Navigation
Page 74 of 140
100 200 300 400 500 6000
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
0.45
0.5
Es
t (m
)
Pos ition Error Std Dev
Time (sec)
Figure 42:Short Road Test Kalman Filter Position Error
Uncertainties
Long Road Test On April 19, 2004, the PADS system was loaded
into the AAI test vehicle and driven to Illinois for camera
vibration testing. We took this opportunity to perform a long road
test of the system. The main purpose of this test was to see if
there was any degradation of the system with time as could be
caused by a memory leak. Figure 43 and Figure 44 presents position
and heading for the complete test. Figure 45 shows the GPS position
uncertainty. This was not up to expectations but is believed to be
due to interference from traffic and other obstructions.
-
Filename Navigation
Page 75 of 140
-88.5 -88 -87.5 -87 -86.5 -86 -85.541.5
42
42.5
43
Longitude (deg)
Latit
ude
(d
eg)
Road TestGR to ROI
Figure 43: Long Road Test PADS position
-
Filename Navigation
Page 76 of 140
0 2000 4000 6000 8000 10000 12000 140000
50
100
150
200
250
300
350
400
He
adi
ng
(de
g)
Time (sec)
Road TestAAI to ROI
Figure 44: Long Road Test PADS Heading
-
Filename Navigation
Page 77 of 140
0 2000 4000 6000 8000 10000 120000
2
4
6
8
10
12
14
16
18
20
Po
siti
on
S
td. D
ev.
(m
)
PADS GPS Data
Lat Lon A lt
Figure 45: Long Road Test GPS Position Uncertainty
In Air Alignment Ground Testing The In Air Alignment mode is
implemented in the RTN as a commanded mode. Note that the mode can
only be commanded when there is GPS data and the speed of the
system is greater than five meters per second. The operation of the
mode is as follows:
1. At the GPS data rate (currently one Hertz) the velocity data
is filtered via three two state Kalman filters. These filters
provide estimates of acceleration along the North, East and Down
axes. The GPS velocity and the derived acceleration estimates are
used to continuously provide heading, pitch and roll estimates for
initialization of the navigation function during In Air
Alignment.
2. The user invokes the In Air Alignment mode via the monitor
program. The monitor will not allow transition to this mode if the
speed is below five meters per second and there is not valid GPS
data. There are no other restrictions on transitioning into this
mode. The system will stay in In Air Alignment mode until the user
commands the system to another mode. Typically the user would
transition to Closed Loop mode, however, for testing purposes all
other modes are
-
Filename Navigation
Page 78 of 140
available. While in In Air Alignment the navigation function is
initialized every second (synchronous with the GPS data) to the
current attitude estimate, GPS position and velocity. Between
initializations the navigation function is run in an open loop
manner.
3. The user invokes a transition to Closed Loop mode. As stated
above this is the most useful transition and the one that will be
used in flight as it forces the system to the GPS reference. The
mode was ground tested using the truck. Figure 46 through Figure 48
present the pertinent data. Figure 46 shows the profile of the
test. The test started at the lower right corner and progressed
westerly. Figure 47 and Figure 48 present the north and east
velocity components. Note that the mode was commanded when the
vehicle speed was about seventeen meters per second. Figure 50,
Figure 51 and Figure 52 give the Align filter state standard
deviations for position velocity and attitude. These plots start at
the time of the transition to Closed Loop. Figure 53 shows the
summed tilt and heading corrections applied to the navigation
function by the Align filter. Note that the initial heading error
uncertainty is over 100 mill radians. This is significantly higher
than the initial value used when transitioning to the Fine Align
Filter from the CAF and reflects the larger heading uncertainty due
to aircraft crab angle. Essentially, in the In Air Alignment mode,
the heading is initialized to the ground track angle as derived
from the GPS velocity components which will introduce a heading
error commensurate with whatever the wind components are. Figure 54
presents the position residuals from the Align filter.
-
Filename Navigation
Page 79 of 140
-85.562 -85.56 -85.558 -85.556 -85.554 -85.552 -85.55
-85.54842.883
42.884
42.885
42.886
42.887
42.888
42.889
42.89
42.891
42.892
42.893
Longitude (deg)
Latit
ude
(de
g)InAir Align Ground TestPADS Nav Data
Figure 46: Ground In-Air Test
-
Filename Navigation
Page 80 of 140
0 20 40 60 80 100 120 140 160 180-10
-5
0
5
10
15
20
Velo
city
No
rth (m
/s)
Time (sec)
InAir Align Ground TestPADS Nav Data
Figure 47: Ground In-Air Test
-
Filename Navigation
Page 81 of 140
0 20 40 60 80 100 120 140 160 180-20
-15
-10
-5
0
5
10
15
20
Velo
city
Ea
st (m
/s)
Time (sec)
InAir Align Ground TestPADS Nav Data
Figure 48: Ground In-Air Test
-
Filename Navigation
Page 82 of 140
0 20 40 60 80 100 120 140 160 180-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
Velo
city
Do
wn (m
/s)
Time (sec)
InAir Align Ground TestPADS Nav Data
Figure 49: Ground In-Air Test
-
Filename Navigation
Page 83 of 140
0 20 40 60 80 100 120 140 1600.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1
Est (m
)
Position Error Std Dev
Time (sec)
X Y Z
Figure 50: Ground In-Air Test
-
Filename Navigation
Page 84 of 140
0 20 40 60 80 100 120 140 1600
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
Est (m
/s)
Velocity Error Std Dev
Time (sec)
VX VY VZ
Figure 51: Ground In-Air Test
-
Filename Navigation
Page 85 of 140
0 20 40 60 80 100 120 140 1600
20
40
60
80
100
120
Est (m
illrad
)
Tilt and Heading Error Std Dev
Time (sec)
X Y Z
Figure 52: Ground In-Air Test
-
Filename Navigation
Page 86 of 140
0 20 40 60 80 100 120 140 160-30
-20
-10
0
10
20
30
Corr
ectio
ns
(millr
ad)
Tilt and Heading Corrections
Time (sec)
X Y Z
Figure 53: Ground In-Air Test
-
Filename Navigation
Page 87 of 140
0 50 100 150-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
Filte
r Re
sidu
als
(m)
Kalman Filter Residulas
Time (sec)
Res XRes YRes Z
Figure 54: Ground In-Air Test
AAI Flight Test A flight test of the PADS system was
accomplished on Friday December