^{1}

^{2,3}

^{1}

^{1}

^{2}

^{3}

Basic principles of the strapdown inertial navigation system (SINS) using the outputs of strapdown gyros and accelerometers are explained, and the main equations are described. A mathematical model of SINS is established, and its Matlab implementation is developed. The theory is illustrated by six examples which are static status, straight line movement, circle movement, s-shape movement, and two sets of real static data.

Many navigation books and papers on inertial navigation system (INS) provide readers with the basic principle of INS. Some also superficially describe simulation methods and rarely provide the free code which can be used by new INS users to help them understand the theory and develop INS applications. Commercial simulation software is available but is not free. The objective of this paper is to develop an easy-to-understand step-by-step development method for simulating INS. Here we consider the most popular INS which is the strapdown inertial navigation system (SINS). The mathematical operations required in our work are mostly matrix manipulations and more generally basic linear algebra [

The main purposes of this paper are to establish a mathematical model and to develop a comprehensive Matlab implementation for SINS. The structure of the proposed mathematical model and Matlab simulation of SINS is shown in Figure

The schema of the proposed mathematical model and Matlab simulation of SINS.

A fundamental aspect of inertial navigation is the precise definition of a number of Cartesian coordinate reference frames. Each frame is an orthogonal, right-handed, coordinate frame or axis set. For all the coordinate frames used in this paper, a positive rotation about each axis is taken to be in a clockwise direction looking along the axis from the origin, as indicated in Figure

The body frame (

The inertial frame (

The Earth frame (

The navigation frame (

The wander azimuth navigation frame (

The

The Reference Frames.

In this paper, we choose the

In the remainder of this section, the main principle of SINS in the

Along the same lines as in [

Equation (

Since

In the next section, the two important DCMs, the vehicle attitude DCM and vehicle position DCM, are defined, as well as the vehicle-attitude-related attitude angles and vehicle-position-related position angles.

In this section, the vehicle attitude DCM with the corresponding attitude angles and the vehicle position DCM with the corresponding position angles are described separately.

The definition of the rotation sequence from

The Relation Between

The vehicle attitude DCM

For the

The gimbal angles

Position matrix

The Relation Between

In Section

The Purpose of Trajectory Simulation is to Generate Data of the 3 Orthogonal Gyros and the 3 Orthogonal Accelerometers According to the Designed Trajectory. It is Mentioned in Section

SINS

For the designed trajectory, the initial parameters are

initial position, latitude

initial velocity

the designed variation of acceleration

the designed variations of the attitude angles, pitch

The velocity is updated as

where

The position is updated as

The attitude angles are updated as

The attitude rates are updated as

The expressions for

The direction cosine matrix

The output of the gyros is

where

In a static base,

The measurement of the accelerometer is the specific force:

where

For four examples of static, straight line, circle, and s-shape situations, details will be given next under the conditions that the vehicle is moving on the surface of the Earth with no attitude change except for the heading angle, which means that the pitch angle, roll angle, and altitude are constants during the simulation process:

The calculation method for the other parameters for the four situations is described as follows.

Static:

Straight line:

Circle:

S-shape:

After the Gyro and Accelerometer Data Are Simulated using the Method Described in the Previous Section under the Designed Scenario, The Next Step we Have to do is to Figure Out the Mathematical Model of SINS and the Calculation Steps to Process the Sensor Data to Get the Calculated Trajectories. Based on the Basic Principles of Strapdown Inertial Navigation System [

SINS

There are three kinds of strapdown attitude representations: DCM, Euler angle, and quaternion. In this paper, we choose quaternion. The reason why quaternion is chosen is explained in [

The quaternion formed by a rotating body frame around the platform frame is

After obtaining

The specific force

The velocity

The update for the position matrix

In the chosen wander azimuth navigation frame, we have

We Have That

The relation between attitude matrix

Thus, the principal values of

The relation between position matrix

Thus, the principal values of

The heading angle

We Have That

For the calculation of the altitude, damped methods should be used because it diverges with time. To simplify problems, in our simulations, we set the altitude to zero, that is, surface of the Earth.

The local gravity

Before we carry out the implementation of the above described mathematical model of SINS, we have to know the initial parameters of the system, which will be described in the following Section.

For the calculations in Section

Initial position, latitude

Initial wander azimuth angle

Initial velocity

If barometric altimeter applied, initial external reference height

Initial attitude matrix is determined by initial alignment process

Initial position matrix is determined by initial alignment process

Initial attitude angles

Initial quaternion calculation: From the diagonal elements in (

Initial position matrix

Initial Earth angular velocity

Initial position angular velocity

Initial gravity

Initial ground velocity

At this point, the whole SINS model, including sensor data generator and initial parameters, is fully described. The following Section will provide a Matlab implementation of the SINS theory.

First, the Matlab program structure and the main codes are given. The Matlab implementation is illustrated using six examples: static, straight, circle, s-shape, and the other two from real SINS experimental data.

The program structure is given in Figure

SINS Program Structure.

The main Matlab codes are presented next.

Initial settings:

Trajectory part:

Simulation part:

In this subsection, there are 6 SINS simulation examples. Example

In order to verity the validity of the Matlab codes further, two sets of real static data are used, and we refer to these as Examples

All the errors (the angle error, the velocity error, and the position error) will contribute to the distance error in the INS trajectory calculation. Thus, the distance error is a key index of an INS system. The distance error will increase with time, so it is always associated with a time stamp.

The static situation is the most basic and simple situation where the output of the gyro is the Earth rotating angular velocity and the output of the accelerometer is the gravity. Figure

The designed trajectory of static simulation.

Angle error of static simulation.

Position and velocity error of static simulation.

The straight line situation corresponds to a vehicle moving along the northwest direction. Figure

The designed trajectory of straight line simulation.

Angle error of straight line simulation.

Position and velocity error of straight line simulation.

The circle situation corresponds to a vehicle moving along a circle. Figure

The designed trajectory of circle simulation.

Angle error of circle simulation.

Position and velocity error of circle simulation.

The s-shape situation corresponds to a vehicle moving along an s-shaped line. Figure

The designed trajectory of s-shape simulation.

Angle error of s-shape simulation.

Position and velocity error of s-shape simulation.

First, we process data set A [

The trajectory of real data set A.

Angle error of real data set A.

Position and velocity error of real data set A.

Figure

The trajectory of real data set B.

Angle error of real data set B.

Position and Velocity Error of Real Data Set B.

In this paper, a mathematical model for the strapdown inertial navigation system (SINS) is built and its Matlab implementation is developed. First, a number of Cartesian coordinate reference frames that relate to SINS are introduced, the basic principle of SINS in the wander azimuth navigation frame (

For a vector

We can easily show that

For numerical analysis, the fourth-order Runge-Kutta method is an important iterative method for the approximation of solutions of ordinary differential equations. Here, in this paper, the fourth-order Runge-Kutta method is adopted to update the quaternion.

The steps for the fourth-order Runge-Kutta method are the following.

Calculate slope

Calculate slope

Calculate slope

Calculate slope

Average the four slopes; greater weights are given to the slopes at the midpoint:

Finally, using the average slope

From Appendix

For the direction cosine matrix

Strapdown inertial navigation system

Direction cosine matrix

Center of the Earth

Center of the vehicle

3 orthogonal axes or the 3 components of a Cartesian coordinate

Body frame

Inertial frame

Earth frame

Navigation frame

East-North-UP navigation frame, which is identical to the

Wander azimuth frame

Transpose of matrix

Velocity vector measured in

Vehicle attitude DCM used to transform the measured angle in

Transpose of

Vehicle position DCM used to transform the measured vector in

Transpose of

Vehicle attitude DCM used to transform the measured angle in

Specific force vector measured in

Specific force vector measured in

Specific force vector measured in

Constant value of the turn rate of the Earth,

Turn rate of the Earth measured in

Turn rate of the

Transport rate of the

Turn rate of the

Turn rate of the

Turn rate of the

Turn rate of the

Turn rate of the

Skew matrix form of

Skew matrix form of

Gravity vector measured in

Local gravity scalar

Local gravity scalar at sea level

Grid azimuth angle of the vehicle in

Wander azimuth angle of

Heading angle of the vehicle in

Grid pitch angle of the vehicle in

Grid roll angle of the vehicle in

Increase of the heading angle

Increase of the grid pitch angle

Increase of the grid roll angle

Longitude of the vehicle

Latitude of the vehicle

Altitude of the vehicle above the sea level of the Earth

Initial vehicle position (latitude, longitude, height)

Time step

Vehicle acceleration

Initial vehicle velocity (east, north, up)

Vehicle velocity (east, north, up)

Vehicle ground velocity

Vehicle position measured in

Major eccentricity of the ellipsoid of the Earth

Length of the semi-major axis of the Earth

Meridian radius of curvature of the Earth

Transverse radius of curvature of the Earth

Free curvature radiuses

Turn torsion

Quaternion

Four components of the quaternion

Period of the circle trajectory in simulation

Period of the s-shape trajectory in simulation

Amplitude of the s-shape trajectory in simulation

Position and velocity.