Navigation LWS

download Navigation LWS

of 140

  • date post

  • Category


  • view

  • download


Embed Size (px)


Navigation algoritthms

Transcript of 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









    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



    Li CCC




    : Level velocity with respect to earth, meters per second.

    LiC : Direction cosine matrix from instrument coordinates to level navigation



    : 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).


    : Current earth rate components in navigation frame.


    r: Gyroscope output; space rate of i coordinates with respect to inertial ( I )

    frame coordinatized in instrument frame ( i ).


    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









    Locally Level Frame coincidentwith ISA position.




    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