Download - Navigation LWS

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.


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


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.


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.


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.


acBacBacBQ = 2


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


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


(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


(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


(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


(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


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)



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)



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


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


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


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


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


AAI Flight Test A flight test of the PADS system was accomplished on Friday December