Table of Contents Author Guidelines Submit a Manuscript
Modelling and Simulation in Engineering
Volume 2012, Article ID 264537, 25 pages
http://dx.doi.org/10.1155/2012/264537
Research Article

Mathematical Model and Matlab Simulation of Strapdown Inertial Navigation System

1College of Opto-Electronic Science and Technology, National University of Defense Technology, Changsha 410073, China
2School of Electronic and Electrical Engineering, University of Leeds, Leeds LS2 9JT, UK
3International University of Rabat, Rabat 11 100, Morocco

Received 24 December 2010; Accepted 5 September 2011

Academic Editor: Ahmed Rachid

Copyright © 2012 Wen Zhang et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

Abstract

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.

1. Introduction

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 [1]. In this paper, Matlab [2] is chosen as the simulation environment. It is a popular computing environment to perform complex matrix calculations and to produce sophisticated graphics in a relatively easy manner. A large collection of Matlab scripts are now available for a wide variety of applications and are often used for university courses. Matlab is also becoming more and more popular in industrial research centers in the design and simulation stages.

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 1. In Section 2, the INS-related orthogonal coordinates (the body frame, the inertial frame, the Earth frame, the navigation frame, the ENU-frame, and the wander azimuth navigation frame) are described and figures to illustrate the relationship between the frames are provided. The basic principle of SINS is described in the wander azimuth navigation frame (𝑝-frame). In Section 3, two important direction cosine matrices (DCMs), the vehicle attitude DCM and the position DCM, and the related important attitude and position angles are defined. In Section 4, the simulation for data generation of gyros and accelerometers is described in ENU-frame. Instead of 𝑝-frame, ENU-frame is chosen because the outputs of gyros and accelerometers are easier to obtain in this frame. The Matlab implementation is given and described step by step. Four kinds of scenarios (static, straight, circle, and s-shape) are set as examples of different kinds of vehicle trajectories. In Section 5, the mathematical model of SINS is set up and the calculation steps in 𝑝-frame are provided. In Section 6, the required initial parameters and other initial data calculation for the SINS model are given for the different simulation scenarios. In Section 7, Matlab implementation code functions are listed and described. Further, simulation results for the four above-mentioned scenarios are presented; two examples from real SINS experiment data are also provided to verify the validity of the developed codes. Finally, conclusions are drawn. Mathematical details are given in Appendices AD.

264537.fig.001
Figure 1: The schema of the proposed mathematical model and Matlab simulation of SINS.

2. Principles

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 2. A negative rotation corresponds to an anti-clockwise direction. This convention is used throughout this paper. It is also worth pointing out that a change in attitude of a body, which is subjected to a series of rotations about different axes, is not only a function of the rotation angles, but also on the order in which the rotations occur. In this paper, the following coordinate frames are used [3].(1)The body frame (𝑏-frame): the 𝑏-frame, depicted in Figure 2, is an orthogonal axis set which has its origin at the center of the vehicle, point 𝑃, and is aligned with the pitch 𝑃𝑥𝑏 axis, roll 𝑃𝑦𝑏 axis, and yaw 𝑃𝑧𝑏 axis of the vehicle in which the navigation system is installed.(2)The inertial frame (𝑖-frame): the 𝑖-frame, depicted in Figure 3, has its origin at the center of the Earth and its axes nonrotating with respect to fixed stars; these axes are denoted by 𝑂𝑥𝑖, 𝑂𝑦𝑖, and 𝑂𝑧𝑖, with 𝑂𝑧𝑖 being coincident with the Earth polar axis.(3)The Earth frame (𝑒-frame): the 𝑒-frame, depicted in Figure 3, has its origin at the center of the Earth and axes nonrotating with respect to the Earth; these axes are denoted by 𝑂𝑥𝑒, 𝑂𝑦𝑒, and 𝑂𝑧𝑒. The axis 𝑂𝑧𝑒 is the Earth polar axis. The axis 𝑂𝑥𝑒 is along the intersection of the plane of the Greenwich meridian and the Earth equatorial plane. The Earth frame rotates with respect to the inertial frame at a rate 𝜔𝑖𝑒 about the axis 𝑂𝑧𝑖.(4)The navigation frame (𝑛-frame): the 𝑛-frame, depicted in Figure 3, is a local geographic navigation frame which has its origin at the location of the navigation system, point 𝑃 (the navigation system is fixed inside the vehicle and we assume that the navigation system is located exactly at the center of the vehicle), and axes aligned with the directions of east 𝑃𝐸, north 𝑃𝑁 and the local vertical up 𝑃𝑈. When the 𝑛-frame is defined in this way, it is called the “ENU-frame.” The turn rate of the navigation frame with respect to the Earth-fixed frame, 𝝎𝑒𝑛, is governed by the motion of the point 𝑃 with respect to the Earth. This is often referred to as the transport rate.(5)The wander azimuth navigation frame (𝑝-frame): the 𝑝-frame, depicted in Figure 3, may be used to avoid the singularities in the computation which occur at the poles of the 𝑛-frame. Like the 𝑛-frame, it is of a local level but rotates through the wander angle about the local vertical. Here we do not call this frame 𝑤-frame (𝑤 for wander) for notation clarity since 𝑤 and 𝜔 may look similar when printed. Letter 𝑝 in 𝑝-frame stands for platform; indeed the wander azimuth navigation frame is of a local level and thus forms a horizontal platform.

264537.fig.002
Figure 2: The 𝑏-frame illustration and the definition of axis rotations.
264537.fig.003
Figure 3: The Reference Frames.

In this paper, we choose the 𝑝-frame as the navigation frame for vehicle trajectory calculation, for the following reason. In the local geographic navigation frame mechanization, the 𝑛-frame is required to rotate continuously as the system moves over the surface of the Earth in order to keep its 𝑃𝑦𝑁 axis pointing to the true north. In order to achieve this condition worldwide, the 𝑛-frame must rotate at much greater rates about its 𝑃𝑧𝑈 axis as the navigation system moves over the surface of the Earth in the polar regions, compared to the rates required at lower latitudes. It is clear that near the polar areas the local geographic navigation frame must rotate about its 𝑃𝑧𝑈 axis rapidly in order to maintain the 𝑃𝑦𝑁 axis pointing to the pole. The heading direction will abruptly change by 180 when moving past the pole. In the most extreme case, the turn rate becomes infinite when passing over the pole. One way of avoiding the singularity, and also providing a navigation system with worldwide capability, is to adopt a wander azimuth mechanization in which the 𝑧-component of 𝝎𝑝𝑒𝑝 is always set to zero, that is, 𝜔𝑝𝑒𝑝𝑧=0. A wander axis system is a local level frame which moves over the Earth surface with the moving vehicle, as depicted in Figure 3. However, as the name implies, the azimuth angle 𝛼 between 𝑃𝑦𝑁 axis and 𝑃𝑦𝑝 axis varies with the vehicle position on Earth. This variation is chosen in order to avoid discontinuities in the orientation of the wander frame with respect to Earth as the vehicle passes over either the north or south pole.

In the remainder of this section, the main principle of SINS in the 𝑝-frame is described.

Along the same lines as in [3], a navigation equation for a wander azimuth system can be constructed as follows:̇𝐯𝑝𝑒=𝐂𝑝𝑏𝐟𝑏2𝐂𝑝𝑒𝝎𝑒𝑖𝑒+𝝎𝑝𝑒𝑝𝐯𝑝𝑒+𝐠𝑝,(1) where 𝐂𝑝𝑏 is the direction cosine matrix used to transform the measured specific force vector in 𝑏-frame into 𝑝-frame. This matrix propagates in accordance with the following equation:̇𝐂𝑝𝑏=𝐂𝑝𝑏𝛀𝑏𝑝𝑏,(2) where Ω𝑏𝑝𝑏 is the skew symmetric form of 𝝎𝑏𝑝𝑏, the 𝑏-frame angular rate with respect to the 𝑝-frame.

Equation (1) is integrated to generate estimates of the vehicle speed in the wander azimuth frame, 𝐯𝑝𝑒. This is then used to generate the turn rate of the wander frame with respect to the Earth frame, 𝝎𝑝𝑒𝑝. The direction cosine matrix which relates the wander frame to the Earth frame, 𝐂𝑝𝑒, may be updated using the following equation:̇𝐂𝑒𝑝=𝐂𝑒𝑝𝛀𝑝𝑒𝑝,̇𝐂(3)𝑒𝑝𝑇=𝛀𝑝𝑒𝑝𝑇𝐂𝑒𝑝𝑇=𝛀𝑝𝑒𝑝𝐂𝑒𝑝𝑇,(4) where the superscript 𝑇 means matrix transposition.

Since (̇𝐂𝑒𝑝)𝑇=̇𝐂𝑝𝑒, (𝐂𝑒𝑝)𝑇=𝐂𝑝𝑒 and skew symmetric matrix is (Ω𝑝𝑒𝑝)𝑇=Ω𝑝𝑒𝑝 (see Appendix A), (4) can be rewritten aṡ𝐂𝑝𝑒=𝛀𝑝𝑒𝑝𝐂𝑝𝑒,(5) where Ω𝑝𝑒𝑝 is a skew symmetric matrix formed from the elements of the angular rate vector 𝝎𝑝𝑒𝑝; we could have 𝝎𝑝𝑒𝑝=𝝎𝑒𝑝𝑒 when the rotation angles are reciprocal. Because the 𝑧-component of 𝝎𝑝𝑒𝑝 is set to zero, 𝜔𝑝𝑒𝑝𝑧=0, the matrix expression of 𝝎𝑝𝑒𝑝 is 𝝎𝑝𝑒𝑝=[𝜔𝑝𝑒𝑝𝑥𝜔𝑝𝑒𝑝𝑦0]𝑇. This process is implemented iteratively and enables any singularities to be avoided.

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.

3. Direction Cosine Matrices (DCMs)

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.

3.1. Vehicle Attitude DCM 𝐶𝑝𝑏

The definition of the rotation sequence from 𝑝-frame to 𝑏-frame is (see Figure 4)𝑥𝑝𝑦𝑝𝑧𝑝𝑧𝑝,𝜓𝐺𝑥𝑒𝑦𝑒𝑧𝑒𝑦𝑝,𝜃𝑥𝑒𝑦𝑒𝑧𝑒𝑦𝑝,𝛾𝑥𝑏𝑦𝑏𝑧𝑏,(6) where 𝜓𝐺 is the gird azimuth angle (0–360°), 𝜃 is the pitch angle (−90°–90°), and 𝛾 is the roll angle (−180°–180°). The above rotation can be written in the following matrix form:𝐂𝑏𝑝=𝐂3𝐂2𝐂1=×cos𝛾0sin𝛾010sin𝛾0cos𝛾1000cos𝜃sin𝜃0sin𝜃cos𝜃cos𝜓𝐺sin𝜓𝐺0sin𝜓𝐺cos𝜓𝐺0.001(7)

264537.fig.004
Figure 4: The Relation Between 𝑏-Frame and 𝑝-Frame.

The vehicle attitude DCM 𝑇𝑝𝑏 is then obtained as𝐂𝑝𝑏=𝐂𝑏𝑝𝑇=cos𝛾cos𝜓𝐺sin𝛾sin𝜃sin𝜓𝐺cos𝜃sin𝜓𝐺sin𝛾cos𝜓𝐺+cos𝛾sin𝜃sin𝜓𝐺cos𝛾sin𝜓𝐺+sin𝛾sin𝜃cos𝜓𝐺cos𝜃cos𝜓𝐺sin𝛾sin𝜓𝐺cos𝛾sin𝜃cos𝜓𝐺sin𝛾cos𝜃sin𝜃cos𝛾cos𝜃.(8)

For the 𝑝-frame system, the angle between the grid north 𝑦𝑝 and the true north 𝑦𝑁 is the wander azimuth angle 𝛼. So the angle between the horizontal projection along 𝑦𝑝 axis of the vehicle’s vertical axis 𝑧𝑏 and the real north 𝑦𝑁 is the heading angle 𝜓. We have that𝜓=𝜓𝐺+𝛼.(9) So the direction cosine matrix 𝐂𝑛𝑏 from 𝑏-frame to 𝑛-frame is𝐂𝑛𝑏=cos𝛾cos𝜓sin𝛾sin𝜃sin𝜓cos𝜃sin𝜓sin𝛾cos𝜓+cos𝛾sin𝜃sin𝜓cos𝛾sin𝜓+sin𝛾sin𝜃cos𝜓cos𝜃cos𝜓sin𝛾sin𝜓cos𝛾sin𝜃cos𝜓sin𝛾cos𝜃sin𝜃cos𝛾cos𝜃.(10)

The gimbal angles 𝜓, 𝜃, and 𝛾 and the gimbal rates ̇𝜓, ̇𝜃, and ̇𝛾 are related to the body rate 𝜔𝑏𝑛𝑏, which is the turn rate of the 𝑏-frame with respect to 𝑛-frame and measured in 𝑏-frame as follows:𝜔𝑏𝑛𝑏𝑥𝜔𝑏𝑛𝑏𝑦𝜔𝑏𝑛𝑏𝑧=00̇𝛾+𝐶3̇𝜃00+𝐶3𝐶200=̇̇.̇𝜓cos𝛾𝜃sin𝛾cos𝜃̇𝜓̇𝛾+sin𝜃̇𝜓sin𝛾𝜃+cos𝛾cos𝜃̇𝜓(11)

3.2. Vehicle Position DCM 𝐶𝑝𝑒

Position matrix 𝐂𝑝𝑒 is the DCM from 𝑒-frame to 𝑝-frame. It has the following rotating sequence (see Figure 5):𝑥𝑒𝑦𝑒𝑧𝑒𝑧𝑒,𝜆𝑥𝑒𝑦𝑒𝑧𝑒𝑦𝑝,90𝜑𝑥𝑒𝑦𝑒𝑧𝑒𝑧𝑝,90𝑥𝐸𝑦𝑁𝑧𝑈𝑧𝑈,𝛼𝑥𝑝𝑦𝑝𝑧𝑝,(12) where 𝜆 is the longitude angle (−180°–180°), 𝜑 is the latitude angle (−90°–90°), and 𝛼 is the wander azimuth angle (0–360°). The above rotation can be written in the following matrix form:𝐂𝑝𝑒=sin𝛼sin𝜑cos𝜆cos𝛼sin𝜆sin𝛼sin𝜑sin𝜆+cos𝛼cos𝜆sin𝛼cos𝜑cos𝛼sin𝜑cos𝜆+sin𝛼sin𝜆cos𝛼sin𝜑sin𝜆sin𝛼cos𝜆cos𝛼cos𝜓cos𝜑cos𝜆cos𝜑sin𝜆sin𝜑.(13)

264537.fig.005
Figure 5: The Relation Between 𝑒-Frame and 𝑝-Frame.

In Section 4, a trajectory simulation method in the ENU-frame is described step by step to generate sensor data. In Section 5, a trajectory and attitude simulator method in the 𝑝-frame is described step by step to derive the desired trajectory and attitude from the simulated sensor data or real sensor data; Section 6 provides the initial parameters and initial data calculation.

4. Sensor Data Generator

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 2 That 𝑝-Frame is Set up to Avoid the Singularities When the Vehicle Passes Over Either the North or South Pole. But in Most Applications, The SINS Systems Are Seldom Operated under This Extreme Environment. The ENU-Frame Can be Implemented Easier Than 𝑝-Frame, so it is Chosen as the Navigation Frame. Figure 6 Shows the Whole Process of the SINS Principal in the ENU-Frame Mechanization. First, The Vehicle Trajectory in the ENU-Frame is Set. Then, The Sensor Ideal Output is Derived using the Inverse Principle of INS. The Sensor Simulation Data Can be Obtained by Adding Noise to the Ideal Data. Then, we Use the Simulated Sensor Data to Derive the Noise-Corrupted Simulated Trajectory. Besides, The Difference Between the Ideal and Simulated State Vectors Can be Set as the Input for the Observed Measurements in the Kalman Filter.

264537.fig.006
Figure 6: SINS ENU-Frame Mechanization.
4.1. The Initial Parameters

For the designed trajectory, the initial parameters are(1)initial position, latitude 𝜑0, longitude 𝜆0, height 0;(2)initial velocity 𝐯=[𝑣𝐸0,𝑣𝑁0,𝑣𝑈0];(3)the designed variation of acceleration 𝐚, which varies with time according to the designed trajectory;(4)the designed variations of the attitude angles, pitch 𝜃, roll 𝛾, and heading 𝜓, and attitude angle rates, ̇𝜃, ̇𝛾, and ̇𝜓, which vary with time according to the designed trajectory.

4.2. The Update of Velocity

The velocity is updated as𝐯𝐯+𝐚Δ𝑡,(14)

where Δ𝑡 is the time step.

4.3. The Update of Position

The position is updated as𝑣latitude:𝐿𝐿+𝑁Δ𝑡𝑅𝑁,𝑣longitude:𝜆𝜆+𝐸Δ𝑡sec𝐿𝑅𝐸altitude:+𝑣𝑈Δ𝑡.,(15)

4.4. The Update of Attitude

The attitude angles are updated aspitch:𝜃𝜃+Δ𝜃,roll:𝛾𝛾+Δ𝛾,heading:𝜓𝜓+Δ𝜓.(16)

The attitude rates are updated aṡ̇̇pitch:𝜃𝜃+Δ𝜃,roll:̇𝛾̇𝛾+Δ̇𝛾,heading:̇𝜓̇𝜓+Δ̇𝜓.(17)

The expressions for Δ𝜃, Δ𝛾, Δ𝜓, Δ̇𝜃, Δ̇𝛾, and Δ̇𝜓 depend on the designed trajectory.

The direction cosine matrix 𝐂𝑛𝑏 can be calculated using matrix expression (10). We have that 𝐂𝑏𝑛=(𝐂𝑛𝑏)𝑇.

4.5. Gyro Data Generator

The output of the gyros is𝝎𝑏𝑖𝑏=𝐈𝐒𝑔𝐂𝑏𝑛𝝎𝑛𝑖𝑒+𝝎𝑛𝑒𝑛+𝝎𝑏𝑛𝑏+𝜀𝑏,(18)

where 𝝎𝑏𝑖𝑏 is the simulated actual output, 𝐈 is the 3×3 unit matrix, 𝐒𝑔 is the 3×3 diagonal matrix whose diagonal elements correspond to the 3 gyros’ scale factor errors, and 𝜀𝑏 is the gyro’s drift and can be simulated as the sum of a constant noise and a random white noise: 𝜀𝑏=𝜀𝑏const+𝜀𝑏random:𝝎𝑛𝑖𝑒=0𝜔𝑖𝑒𝜔cos𝐿𝑖𝑒sin𝐿.(19)

In a static base, 𝝎𝑏𝑛𝑏 is equal to zero, whereas, in a moving base it is obtained as𝝎𝑏𝑛𝑏=̇̇cos𝛾𝜃sin𝛾cos𝜃̇𝜓̇𝛾+sin𝜃̇𝜓sin𝛾𝜃+cos𝛾cos𝜃̇𝜓.(20)

𝝎𝑛𝑒𝑛 is related to velocity 𝐯=[𝑣𝐸,𝑣𝑁,𝑣𝑈]𝑇 and can be expressed as𝝎𝑛𝑒𝑛=𝑣𝑁𝑅𝑁𝑣𝐸𝑅𝐸𝑣𝐸tan𝐿𝑅𝑁.(21)

4.6. Accelerometer Data Generator

The measurement of the accelerometer is the specific force:𝐟𝑏=𝐈𝐒𝑎𝐂𝑏𝑛𝐟𝑛+𝜼𝑏,𝐟𝑛=𝐚+2𝝎𝑛𝑖𝑒+𝝎𝑛𝑒𝑛×𝐯𝐠,(22)

where 𝐟𝑏 is the simulated actual output, 𝐈 is the 3×3 unit matrix. 𝐒𝑎 is the 3×3 diagonal matrix whose diagonal elements correspond to the 3 accelerometers’ scale factor errors, 𝜼𝑏 is the bias considered as the sum of a constant noise and a random white noise 𝜼𝑏=𝜼𝑏const+𝜼𝑏random. 𝐠=[00𝑔]𝑇, and 𝑔=9.7803+0.051799𝐶2330.94114×106(m/s2), where 𝐶33 is the 9th element of 𝐂𝑝𝑒 and is the vehicle altitude.

4.7. Examples

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:Δ̇Δ𝜃=0,Δ𝛾=0,𝜃=0,Δ̇𝛾=0,(23)

The calculation method for the other parameters for the four situations is described as follows.(1)Static:𝑣𝐿=constant,𝜆=constant,𝐸𝑣=constant,𝑁=constant,Δ𝜓=0,Δ̇𝜓=0.(24)(2)Straight line:𝑎𝐸𝑎=constant,𝑁𝑣=constant,𝐸=𝑣𝐸+𝑎𝐸𝑣Δ𝑡,𝑁=𝑣𝑁+𝑎𝑁Δ𝑡,𝜓=𝜓0𝑣+arctan𝐸𝑣𝑁,𝑎̇𝜓=𝑁𝑣𝐸𝑎𝐸𝑣𝑁𝑣2𝐸+𝑣2𝑁.(25)(3)Circle:𝑣𝑔=constant,Δ𝜓=mod2𝜋Δ𝑡𝑇circle,,2𝜋Δ̇𝜓=2𝜋𝑇circle,𝑎𝐸=2𝜋𝑣𝑔cos𝜓𝑇circle,𝑎𝑁=2𝜋𝑣𝑔sin𝜓𝑇circle.(26)(4)S-shape:𝑣𝑔𝑎=constant,𝐸𝑣=𝑔𝜓cos0+𝐴sshapesin2𝜋𝑡/𝑇sshape𝑇sshape,2𝜋𝐴sshapecos2𝜋𝑡/𝑇sshape𝑇sshape,𝑎𝑁𝑣=𝑔𝜓sin0+𝐴sshapesin2𝜋𝑡/𝑇sshape𝑇sshape2𝜋𝐴sshapecos2𝜋𝑡/𝑇sshape𝑇sshape,𝜓=𝜓0+𝐴sshapesin2𝜋𝑡𝑇sshape.̇𝜓=2𝜋𝐴sshapecos2𝜋𝑡/𝑇sshape𝑇sshape.(27)

5. Mathematical Model and Trajectory Calculation Steps

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 [4], we Draw the Mathematical Model in the 𝑝-Frame Mechanization in Figure 7. The Calculation Steps Are Described Below. Although the Situation That the Vehicle Passes Over Either the North or South Pole Seldom Happens, The Universal 𝑝-Frame is Still Chosen Instead of the Simpler ENU-Frame to Give a Navigation Illustration in a Different Frame.

264537.fig.007
Figure 7: SINS 𝑝-Frame Mechanization.
5.1. Quaternion 𝑄 Update and Optimal Normalization

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 [3].

The quaternion formed by a rotating body frame around the platform frame is𝐐=𝑞0+𝑞1𝑖𝑏+𝑞2𝑗𝑏+𝑞3𝑘𝑏.(28) The update for the quaternion can be obtained by solving the following quaternion differential equation:̇𝑞0̇𝑞1̇𝑞2̇𝑞3=120𝜔𝑏𝑝𝑏𝑥𝜔𝑏𝑝𝑏𝑦𝜔𝑏𝑝𝑏𝑧𝜔𝑏𝑝𝑏𝑥0𝜔𝑏𝑝𝑏𝑧𝜔𝑏𝑝𝑏𝑦𝜔𝑏𝑝𝑏𝑦𝜔𝑏𝑝𝑏𝑧0𝜔𝑏𝑝𝑏𝑥𝜔𝑏𝑝𝑏𝑧𝜔𝑏𝑝𝑏𝑦𝜔𝑏𝑝𝑏𝑥0𝑞0𝑞1𝑞2𝑞3.(29) Based on the Euclide norm minimized indicator [4], the optimal normalization for the quaternion is𝐐𝐐𝑞20+𝑞21+𝑞22+𝑞23.(30)

5.2. 𝐶𝑝𝑏 Calculation

𝐂𝑝𝑏 is vehicle attitude DCM which transforms the measured angle in the 𝑏-frame to the 𝑝-frame, with its 9 components 𝑇𝑖𝑗,𝑖,𝑗=1,2,3. (Here we use 𝑇𝑖𝑗 to distinguish it from the components 𝐶𝑖𝑗,𝑖,𝑗=1,2,3 of 𝐂𝑝𝑒 which is used below.)

After obtaining 𝑞0, 𝑞1, 𝑞2, and 𝑞3 using (29), 𝐂𝑝𝑏 can be calculated as𝐂𝑝𝑏=𝑇11𝑇12𝑇13𝑇21𝑇22𝑇23𝑇31𝑇32𝑇33=𝑞20+𝑞21𝑞22𝑞232𝑞1𝑞2𝑞0𝑞32𝑞1𝑞3+𝑞0𝑞22𝑞1𝑞2+𝑞0𝑞3𝑞20𝑞21+𝑞22𝑞232𝑞2𝑞3𝑞0𝑞12𝑞1𝑞3𝑞0𝑞22𝑞2𝑞3+𝑞0𝑞1𝑞20𝑞21𝑞22+𝑞23.(31)

5.3. Specific Force Transformation from 𝑓𝑏 in 𝑏-Frame to 𝑓𝑝 in 𝑝-Frame

The specific force 𝐟𝑏 in the 𝑏-frame can be transformed to 𝐟𝑝 in the 𝑝-frame by multiplication with DCM 𝐂𝑝𝑏:𝐟𝑝=𝐂𝑝𝑏𝐟𝑏,𝑓𝑝𝑥𝑓𝑝𝑦𝑓𝑝𝑧=𝐂𝑝𝑏𝑓𝑏𝑥𝑓𝑏𝑦𝑓𝑏𝑧,(32)

5.4. Velocity 𝑣𝑝𝑒 Calculation

The velocity 𝐯𝑝𝑒 update can be obtained by solving the following differential equation:̇𝐯𝑝𝑒=𝐟𝑝2𝝎𝑝𝑖𝑒+𝝎𝑝𝑒𝑝𝐯𝑝𝑒+𝐠𝑝,̇𝑣𝑥̇𝑣𝑦̇𝑣𝑧=𝑓𝑝𝑥𝑓𝑝𝑦𝑓𝑝𝑧00𝑔+02𝜔𝑝𝑖𝑒𝑧2𝜔𝑝𝑖𝑒𝑦+𝜔𝑝𝑒𝑝𝑦2𝜔𝑝𝑖𝑒𝑧02𝜔𝑝𝑖𝑒𝑥+𝜔𝑝𝑒𝑝𝑥2𝜔𝑝𝑖𝑒𝑦+𝜔𝑝𝑒𝑝𝑦2𝜔𝑝𝑖𝑒𝑥+𝜔𝑝𝑒𝑝𝑥0×𝑣𝑥𝑣𝑦𝑣𝑧.(33) The ground speed is the vehicle velocity projection on the horizontal plane:𝑣𝑔=𝑣2𝑥+𝑣2𝑦.(34)

5.5. Position Matrix 𝐶𝑝𝑒 Update

The update for the position matrix 𝐂𝑝𝑒 can be obtained by solving the following differential equation, noticing that 𝜔𝑝𝑒𝑝𝑧=0:̇𝐂𝑝𝑒=𝛀𝑝𝑒𝑝𝐂𝑝𝑒,𝐂𝑝𝑒=𝐶11𝐶12𝐶13𝐶21𝐶22𝐶23𝐶31𝐶32𝐶33,̇𝐶11̇𝐶12̇𝐶13̇𝐶21̇𝐶22̇𝐶23̇𝐶31̇𝐶32̇𝐶33=00𝜔𝑝𝑒𝑝𝑦00𝜔𝑝𝑒𝑝𝑥𝜔𝑝𝑒𝑝𝑦𝜔𝑝𝑒𝑝𝑥0𝐶11𝐶12𝐶13𝐶21𝐶22𝐶23𝐶31𝐶32𝐶33.(35)

5.6. Position Angular Velocity 𝜔𝑝𝑒𝑝 Update

In the chosen wander azimuth navigation frame, we have 𝜔𝑝𝑒𝑝𝑧=0, and𝜔𝑝𝑒𝑝𝑥𝜔𝑝𝑒𝑝𝑦=1𝜏𝑎1𝑅𝑦𝑝1𝑅𝑥𝑝1𝜏𝑎𝑣𝑝𝑒𝑥𝑣𝑝𝑒𝑦,(36) where1𝑅𝑦𝑝=1𝑅𝑒1𝑒𝐶233+2𝑒𝐶223,1𝑅𝑥𝑝=1𝑅𝑒1𝑒𝐶233+2𝑒𝐶213,1𝜏𝑎=2𝑒𝑅𝑒𝐶13𝐶23,(37) where the elements of position matrix 𝐂𝑝𝑒 can be obtained using (35).

5.7. Earth Angular Velocity 𝜔𝑝𝑖𝑒 and Attitude Angular Velocity 𝜔𝑏𝑝𝑏 Calculation

We Have That𝝎𝑝𝑖𝑒=𝐂𝑝𝑒𝝎𝑒𝑖𝑒=𝐶11𝐶12𝐶13𝐶21𝐶22𝐶23𝐶31𝐶32𝐶3300𝜔𝑖𝑒=𝜔𝑖𝑒𝐶13𝜔𝑖𝑒𝐶23𝜔𝑖𝑒𝐶33,𝝎(38)𝑏𝑝𝑏=𝝎𝑏𝑖𝑏𝝎𝑏𝑖𝑝=𝝎𝑏𝑖𝑏𝐂𝑝𝑏1𝝎𝑝𝑖𝑒+𝝎𝑝𝑒𝑝,(39)

5.8. Attitude Angle Calculation

The relation between attitude matrix 𝐂𝑝𝑏 and the three attitude angles, grid azimuth angle 𝜓𝐺, pitch angle 𝜃, and roll angle 𝛾, is𝐂𝑝𝑏=cos𝛾cos𝜓𝐺sin𝛾sin𝜃sin𝜓𝐺cos𝜃sin𝜓𝐺sin𝛾cos𝜓𝐺+cos𝛾sin𝜃sin𝜓𝐺cos𝛾sin𝜓𝐺+sin𝛾sin𝜃cos𝜓𝐺cos𝜃cos𝜓𝐺sin𝛾sin𝜓𝐺cos𝛾sin𝜃cos𝜓𝐺sin𝛾cos𝜃sin𝜃cos𝛾cos𝜃.(40)

Thus, the principal values of 𝜓𝐺, 𝜃, and 𝛾 are𝜃principal=sin1𝑇32,𝛾principal=tan1𝑇31𝑇33,𝜑𝐺principal=tan1𝑇12𝑇22.(41) Considering the defined range of the angles, the expressions of the real values of 𝜓𝐺, 𝜃, 𝛾, and are𝜃𝜃principal,𝛾𝛾principal,if𝑇33𝛾>0,principal+180,if𝑇33<0,𝛾principal𝛾<0,principal180,if𝑇33<0,𝛾principal𝜓>0,𝐺𝜓𝐺principal,if𝑇22>0,𝜓𝐺principal𝜓>0,𝐺principal+360,if𝑇22>0,𝜓𝐺principal𝜓<0,𝐺principal+180,if𝑇22<0.(42)

5.9. Position Angle Calculation

The relation between position matrix 𝐂𝑝𝑒 and the 3 position angles, longitude 𝜆, latitude 𝜑, and wander azimuth angle 𝛼, is𝐂𝑝𝑒=sin𝛼sin𝜑cos𝜆cos𝛼sin𝜆sin𝛼sin𝜑sin𝜆+cos𝛼cos𝜆sin𝛼cos𝜑cos𝛼sin𝜑cos𝜆+sin𝛼sin𝜆cos𝛼sin𝜑sin𝜆sin𝛼cos𝜆cos𝛼cos𝜓cos𝜑cos𝜆cos𝜑sin𝜆sin𝜑.(43)

Thus, the principal values of 𝜑, 𝜆, and 𝛼 are𝜑principal=sin1𝐶33,𝜆principal=tan1𝐶32𝐶31,𝛼principal=tan1𝐶13𝐶23.(44) Considering the defined range of the angles, the expressions of the real values of 𝜑, 𝜆, and 𝛼 are𝜑𝜑principal,𝜆𝜆principal,if𝐶31𝜆>0,principal+180,if𝐶31<0,𝜆principal𝜆<0,principal180,if𝐶31<0,𝜆principal𝛼>0,𝛼principal,if𝐶23>0,𝛼principal𝛼>0,principal+360,if𝐶23>0,𝛼principal𝛼<0,principal+180,if𝐶23<0.(45)

5.10. Heading Angle Calculation

The heading angle 𝜓 is calculated as𝜓=𝜓𝐺+𝛼.(46) To make sure that 𝜓 will not be out of range, we should determine it according to𝜓𝜓,if𝜓<360,𝜓360,if𝜓360.(47)

5.11. Velocity 𝑣𝑛𝑒 in 𝑛-Frame Calculation

We Have That𝐯𝑛𝑒=𝑣𝐸𝑣𝑁𝑣𝑈=𝑣𝑝𝑒𝑦cos𝛼𝑣𝑝𝑒𝑥𝑣sin𝛼𝑝𝑒𝑦sin𝛼+𝑣𝑝𝑒𝑥𝑣cos𝛼𝑝𝑒𝑧.(48)

5.12. Altitude Calculation

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.

5.13. Local Gravity g Calculation

The local gravity 𝑔 is calculated as [5]𝑔=9.7803+0.051799𝐶2330.94114×106m/s2,(49) where 𝐶33=sin𝜑, 𝜑 is the latitude and is the altitude above sea level.

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.

6. Initial Parameters and Initial Data Calculation

For the calculations in Section 5, we first need to know the given initial parameters and the corresponding initial data.

6.1. Initial Parameters
(1)Initial position, latitude 𝜑0, longitude 𝜆0, height 0. The values of these parameters should be the same as the corresponding ones in Section 4.1.(2)Initial wander azimuth angle 𝛼0. We could choose 𝛼0=0 at the very beginning. The value should be the same as the corresponding ones in Section 4.1.(3)Initial velocity 𝑣𝐸0, 𝑣𝑁0, 𝑣𝑈0.(4)If barometric altimeter applied, initial external reference height ref0 can be supplied.
6.2. Initial Alignment Data
(1)Initial attitude matrix is determined by initial alignment process 𝐂𝑝𝑏0. 𝐂𝑝𝑏0=𝐂𝑛𝑏0 when 𝛼0=0.(2)Initial position matrix is determined by initial alignment process 𝐂𝑝𝑒0. 𝐂𝑝𝑒0=𝐂𝑛𝑒0 when 𝛼0=0.
6.3. Initial Data Calculation
(1)Initial attitude angles 𝜑0, 𝜆0, and 𝛼0 determination: The initial attitude angles 𝜓𝐺0, 𝜃0, and 𝛾0 can be calculated using (41) and (42). Because 𝛼0=0, heading angle 𝜓0=𝜓𝐺0.(2)Initial quaternion calculation: From the diagonal elements in (31) and the quaternion constraint equation, we have that 𝑞20+𝑞21𝑞22𝑞23=𝑇11,𝑞20𝑞21+𝑞22𝑞23=𝑇22,𝑞20𝑞21𝑞22+𝑞23=𝑇33,𝑞20+𝑞21+𝑞22+𝑞23=1,(50) The solution to (50) ||𝑞1||=121+𝑇11𝑇22𝑇33,||𝑞2||=121𝑇11+𝑇22𝑇33,||𝑞3||=121𝑇11𝑇22+𝑇33,||𝑞0||=1𝑞21𝑞22𝑞23.(51) Assuming 𝑞0 to be positive, according to (31), we have that 𝑞sign0𝑞=sign(1),sign1𝑇=sign32𝑇23,𝑞sign2𝑇=sign13𝑇31,𝑞sign3𝑇=sign21𝑇12.(52)(3)Initial position matrix 𝐂𝑝𝑒0: Substituting initial position, latitude 𝜑0, longitude 𝜆0 and initial wander azimuth 𝛼0=0 into (43), we can obtain the initial position matrix 𝐂𝑝𝑒0.(4)Initial Earth angular velocity 𝝎𝑝𝑖𝑒0 and initial attitude angular velocity 𝝎𝑏𝑝𝑏0 calculations: use (38) and (39).(5)Initial position angular velocity 𝝎𝑝𝑒𝑝0 calculation: use (36) and (37).(6)Initial gravity 𝑔0 calculation: use (49) and element 𝐶33 in 𝐂𝑝𝑒0.(7)Initial ground velocity 𝑣𝑔0 calculation: use (34).

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.

7. Matlab Implementation and Simulation Examples

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.

7.1. Matlab Implementation and Codes

The program structure is given in Figure 8. The program starts from “Begin” and ends at “Stop.” The gyro and accelerometer data are obtained either from a sensor data generator described in Section 4 or from the real SINS experiment logged files. Processing the sensor data with the initial parameters, using the method described in Section 5, we get the attitude, velocity and position values of the system at specific times. After all data are processed, the program will stop and the results will be provided.

264537.fig.008
Figure 8: SINS Program Structure.

The main Matlab codes are presented next.(1)Initial settings:(a)initSettings.m contains initial parameters and constants used in the simulation project.(2)Trajectory part:(a)initialCalculation_static.m gives the initial calculation for the static situation;(b)trajectorySimulater_static.m simulates gyro and accelerometer data for the static situation;(c)initialCalculation_straight.m gives the initial calculation for the straight line situation;(d)trajectorySimulater_straight.m simulates gyro and accelerometer data for the straight line situation;(e)initialCalculation_cirlce.m gives the initial calculation for the circle situation;(f)trajectorySimulater_circle.m simulates gyro and accelerometer data for the circle situation;(g)initialCalculation_Sshape.m gives the initial calculation for the s-shape situation;(h)trajectorySimulater_Sshape.m simulates gyro and accelerometer data for the s-shape situation;(3)Simulation part:(a)INSmain.m is the main program; the simulation starts from here;(b)AltitudeParamete.m calculates the four damping parameters to damp the altitude error according to the input parameters 𝑘4 and 𝜏, to be used with the external reference altitude;(c)InitializePosition.m gives the initial position initLong, initLat, initAlt, the external reference altitude extAlt, and the wander azimuth angle wanderAzimuth; it calculates the initial position matrix and then orthogonalizes the matrix;(d)InitializeAttitude.m gives the initial alignment error and calculates the attitude matrix (strapdown matrix);(e)InitializeQuaternion.m calculates the quaternion according to the input attitude matrix;(f)ComputeAngularVelocity.m calculates the position angular velocity, earth angular velocity, and position angle increment in the 𝑝-frame and resets the gyroscopes and accelerometers;(g)ComputeQuaternionRungeKutta.m computes the quaternion using Runge-Kutta method [6]; see Appendices B and C;(h)ComputeAttitudeMatrix.m computes the attitude matrix and transfers the raw data of the accelerometers to the 𝑝-frame;(i)ComputeVelocity.m computes the velocity, in the wander azimuth frame (𝑝-frame) and ENU-frame, the ground velocity and altitude;(j)ComputePositionMatrix.m computes the position matrix.(k)ComputePosition.m computes latitude, longitude and wanderAzimuth;(l)ComputeAttitudeAngle.m computes the attitude angle of pitchAngle, tiltAngle, gridAzimuth and courseAngle;(m)OrthogonalizeMatrix.m computes matrix orthogonalization; see Appendix D;(n)QuaCofMatrix.m is called by ComputeQuaternionRungeKutta.m;(o)PlotResult.m plots the results of the simulation project.

7.2. Simulation Examples

In this subsection, there are 6 SINS simulation examples. Example 1 is the static situation simulation, where the vehicle trajectory in the 𝑛-frame is a fixed point. Example 2 is the straight line situation simulation, where the vehicle trajectory in the 𝑛-frame is a straight line. Example 3 is the circle situation simulation, where the vehicle trajectory in the 𝑛-frame is a circle. Example 4 is the s-shape situation simulation, where the vehicle trajectory in the 𝑛-frame is an s-shape line. Here, high-accuracy SINS simulation is applied to the four situations. The initial latitude and longitude errors are set to be 1 minute. The simulation time is set to 3600 seconds. The initial positions are dependent on the designed trajectories.

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 5 and 6. The two sets of real data, set A and set B, are collected from the same SINS in the same place but at different times. The 2 data sets are 24 hours long.

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.

Example 1 (Static situation simulation). 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 9 shows the designed true trajectory. Figure 10 shows the difference between the calculated angle and the true angle. Figure 11 shows the differences between the calculated PV (position and velocity) and the true PV. The maximum value of the distance error in 1 hour is 3.5 nm (nautical mile).

fig9
Figure 9: The designed trajectory of static simulation.
fig10
Figure 10: Angle error of static simulation.
fig11
Figure 11: Position and velocity error of static simulation.

Example 2 (Straight line situation simulation). The straight line situation corresponds to a vehicle moving along the northwest direction. Figure 12 shows the designed true trajectory. Figure 13 shows the difference between the calculated angle and the true angle. Figure 14 shows the differences between the calculated PV and the true PV. The maximum value of the distance error in 1 hour is 3.7 nm.

fig12
Figure 12: The designed trajectory of straight line simulation.
fig13
Figure 13: Angle error of straight line simulation.
fig14
Figure 14: Position and velocity error of straight line simulation.

Example 3 (Circle situation simulation). The circle situation corresponds to a vehicle moving along a circle. Figure 15 shows the designed true trajectory. Figure 16 shows the difference between the calculated angle and the true angle. Figure 17 shows the difference between the calculated PV and the true PV. The maximum value of the distance error in 1 hour is 3.0 nm.

fig15
Figure 15: The designed trajectory of circle simulation.
fig16
Figure 16: Angle error of circle simulation.
fig17
Figure 17: Position and velocity error of circle simulation.

Example 4 (S-shape situation simulation). The s-shape situation corresponds to a vehicle moving along an s-shaped line. Figure 18 shows the designed true trajectory of s-shape situation simulation. Figure 19 shows the difference between the calculated angle and the true angle. Figure 20 shows the differences between the calculated PV and the true PV. The maximum value of the distance error in 1 hour is 3.3 nm.

fig18
Figure 18: The designed trajectory of s-shape simulation.
fig19
Figure 19: Angle error of s-shape simulation.
fig20
Figure 20: Position and velocity error of s-shape simulation.

Example 5 (Real static data set A simulation). First, we process data set A [7]. Figure 21 shows the trajectory for the real data set A; from the figure we can conclude that the system is static. In Figure 22, the red line corresponds to the three attitude angle errors of the real system, while the blue line corresponds to the three attitude angle errors processed by the Matlab code. We can also show that the difference between the red and blue lines is negligible. In Figure 23, the red line corresponds to the position and velocity errors of the real system, while the blue line corresponds to the position and velocity errors processed by the Matlab code. We can also see that the difference between the red and blue lines is negligible and this validates the correctness of the Matlab code. The error described by the red lines (output from the real system) is slightly smaller than that described by the blue lines (simulation). This is due to the fact that the real system is processed in a much higher rate and thus its input is more accurate than the simulated system.

fig21
Figure 21: The trajectory of real data set A.
fig22
Figure 22: Angle error of real data set A.
fig23
Figure 23: Position and velocity error of real data set A.

Example 6 (Real static data set B simulation). Figure 24 shows the trajectory of the real data set B; from the figure we can conclude that the system is static too. In Figure 25, the red line corresponds to the three attitude angle errors of the real system, while the blue line corresponds to the three attitude angle errors obtained by the Matlab code when applied to the real raw sensor data set B. We can also see that the difference between the red and blue lines is negligible. In Figure 26, the red line corresponds to the position and velocity errors of the real system, while the blue line corresponds to the position and velocity errors obtained by the Matlab code when applied to the real raw sensor data set B. We can also see that the difference between the red and blue lines is negligible, and this further validates the correctness of the Matlab code.

fig24
Figure 24: The trajectory of real data set B.
fig25
Figure 25: Angle error of real data set B.
fig26
Figure 26: Position and Velocity Error of Real Data Set B.

8. Conclusions

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 (𝑝-frame) is explained, and the main equations are described. Second, the important attitude direction cosine matrix and position direction cosine matrix in the 𝑝-frame are defined in detail. Third, the mathematical model for SINS simulation is described in detail. Fourth, a trajectory simulator model is set up to generate data from three orthogonal gyros and three orthogonal accelerometers. The initial parameters and initial data calculations for the mathematical model are also carried out. Finally, a Matlab implementation of SINS is developed. The proposed simulation method is illustrated with four examples, static, straight line, circle, and s-shape trajectories; details are given under the condition that the pitch angle, roll angle, and altitude are constant during the simulation process. Further, two sets of real experimental data are processed to verify the validity of the Matlab code.

Appendices

A. Symmetric Matrix Basic Operation

For a vector 𝝎=[𝜔𝑥𝜔𝑦𝜔𝑧]𝑇, its skew symmetric matrix Ω is𝛀=0𝜔𝑧𝜔𝑦𝜔𝑧0𝜔𝑥𝜔𝑦𝜔𝑥0.(A.1)

We can easily show that𝛀𝑇=𝛀.(A.2)

B. Fourth-Order Runge-Kutta Method

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.(1)Calculate slope 𝑘1, the slope at the beginning of the interval, to determine the value of 𝑦𝑖+1/2 at the point 𝑡𝑖+1/2 using the Euler method: 𝑘1𝜔=𝑓𝑖,𝑦𝑖,𝑡𝑖,𝑦𝑖+1/2=𝑦𝑖+𝜏2𝑘1,(B.1) where 𝜏 is the time step between time 𝑡𝑖 and time 𝑡𝑖+1, 𝜏=𝑡𝑖+1𝑡𝑖.(2)Calculate slope 𝑘2, the slope at the midpoint of the interval, to determine the value of 𝑦𝑖+1/2 at the point 𝑡𝑖+1/2 using Euler’s method:𝑘2𝜔=𝑓𝑖+1/2,𝑦𝑖+1/2,𝑡𝑖+1/2,𝑦𝑖+1/2=𝑦𝑖+𝜏2𝑘2.(B.2)(3)Calculate slope 𝑘3, again the slope at the midpoint, to determine the 𝑦𝑖+1 value: 𝑘3𝜔=𝑓𝑖+1/2,𝑦𝑖+1/2,𝑡𝑖+1/2,𝑦𝑖+1=𝑦𝑖+𝜏𝑘3.(B.3)(4)Calculate slope 𝑘4, the slope at the end of the interval, with its 𝑦𝑖+1 value determined using 𝑘3: 𝑘4𝜔=𝑓𝑖+1,𝑦𝑖+1,𝑡𝑖+1.(B.4)(5)Average the four slopes; greater weights are given to the slopes at the midpoint: 1𝑘=6𝑘1+2𝑘2+2𝑘3+𝑘4.(B.5)(6)Finally, using the average slope 𝑘, the value of 𝑦𝑖+1 is 𝑦𝑖+1=𝑦𝑖+𝜏𝑘.(B.6)

C. Angular Velocity Extraction

From Appendix B and (29), we need to provide the attitude angular velocity 𝜔𝑏𝑝𝑏 in a period of 𝜏/2 to update the quaternion. By inspecting the expression of 𝜔𝑏𝑝𝑏 in (39), we know that the variations of 𝜔𝑝𝑒𝑝 and 𝜔𝑝𝑖𝑒 are slow, while 𝜔𝑏𝑖𝑏 changes quickly. So, only 𝜔𝑏𝑖𝑏 needs to be given in a period of 𝜏/2. We know that 𝜔𝑏𝑖𝑏 (we next use 𝜔 to simplify notation) is the output of gyro which gives data in the form of angle increment Δ𝜃𝑖 during the time interval 𝜏. For first-order angular velocity extraction, we have that𝜔=Δ𝜃𝑖𝜏.(C.1) In order to provide 𝜔(𝑡𝑖)𝜔(𝑡𝑖+1/2) and 𝜔(𝑡𝑖+1), we need to do second-order angular velocity extraction:𝜔𝑡𝑖=3Δ𝜃𝑖1Δ𝜃𝑖2𝜏,𝜔𝑡𝑖+1/2=Δ𝜃𝑖1+Δ𝜃𝑖2𝜏,𝜔𝑡𝑖+1=Δ𝜃𝑖1+3Δ𝜃𝑖2𝜏,(C.2) where Δ𝜃𝑖1 is the angle increment from time 𝑡𝑖 to 𝑡𝑖+1/2 and Δ𝜃𝑖2 is the angle increment from time 𝑡𝑖+1/2 to time 𝑡𝑖+1.

D. Matrix Orthogonalization Method

For the direction cosine matrix 𝐂, the optimal orthogonalization method is to get 𝐂 which makes the following Euclidian function have the minimum value [8]:𝐷=33𝑖=1𝑗=1𝐶𝑖𝑗𝐶𝑖𝑗.1/2(D.1) The expression for 𝐂 is thus𝐂𝐂=±𝐂𝑇𝐂3/2,(D.2) where the superscript 𝑇 means the transpose operator. It is difficult to solve the above equation directly. Instead, we use an iterative method. Assume 𝐂0 to be initial matrix, and 𝐂𝑛 to be the matrix obtained after 𝑛 iterations. The iteration process is as follows:𝐂0𝐂=𝐂,𝑛+1=𝐂𝑛12𝐂𝑛𝐂𝑇𝐂𝑛.𝐂(D.3) If at the 𝑛+1 step, the following function:𝑓𝑛=33𝑖=1𝑗=1𝐶𝑖𝑗𝐶𝑖𝑗𝑛2(D.4) satisfies 𝑓𝑛+1𝑓𝑛𝜖 (e.g., 𝜖=1010), then the iteration procedure can be stopped and 𝐂𝑛+1 is taken to be the final result.

Abbreviations and Symbols

SINS:Strapdown inertial navigation system
DCM: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
ENU: East-North-UP navigation frame, which is identical to the 𝑛-frame in this paper
𝑝: Wander azimuth frame
𝐀𝑇: Transpose of matrix 𝐀
𝐯𝑝𝑒: Velocity vector measured in 𝑝-frame with respect to 𝑒-frame
𝐂𝑝𝑏: Vehicle attitude DCM used to transform the measured angle in𝑏-frame to 𝑝-frame, with its 9 components 𝑇𝑖𝑗,𝑖,𝑗=1,2,3
𝐂𝑏𝑝: Transpose of 𝐂𝑝𝑏 is used to transform the measured vector in 𝑝-frame to 𝑏-frame
𝐂𝑝𝑒: Vehicle position DCM used to transform the measured vector in𝑒-frame to 𝑝-frame, with its 9 components 𝐶𝑖𝑗,𝑖,𝑗=1,2,3
𝐂𝑒𝑝: Transpose of 𝐂𝑝𝑒 is used to transform the measured vector in 𝑝-frame to 𝑒-frame
𝐂𝑛𝑏: Vehicle attitude DCM used to transform the measured angle in𝑏-frame to 𝑛-frame
𝐟𝑝: Specific force vector measured in 𝑝-frame
𝐟𝑛: Specific force vector measured in 𝑛-frame
𝐟𝑏: Specific force vector measured in 𝑏-frame; the output of the 3 accelerometers
𝜔𝑖𝑒: Constant value of the turn rate of the Earth, 𝜔𝑖𝑒=7.2921151467×105 rad/s
𝝎𝑛𝑖𝑒: Turn rate of the Earth measured in 𝑛-frame
𝝎𝑏𝑖𝑏: Turn rate of the 𝑏-frame with respect to 𝑖-frame, which is measured in 𝑏-frame; the output of the 3 gyros
𝝎𝑛𝑒𝑛: Transport rate of the 𝑛-frame with respect to 𝑒-frame, which is measured in 𝑛-frame
𝝎𝑒𝑖𝑒: Turn rate of the 𝑒-frame with respect to 𝑖-frame, which is measured in 𝑒-frame
𝝎𝑝𝑒𝑝: Turn rate of the 𝑝-frame with respect to 𝑒-frame, which is measured in 𝑝-frame
𝝎𝑒𝑝𝑒: Turn rate of the 𝑒-frame with respect to 𝑝-frame, which is measured in 𝑒-frame
𝝎𝑏𝑝𝑏: Turn rate of the 𝑏-frame with respect to 𝑝-frame, which is measured in 𝑏-frame
𝝎𝑏𝑛𝑏: Turn rate of the 𝑏-frame with respect to 𝑛-frame, which is measured in 𝑏-frame
Ω𝑏𝑝𝑏: Skew matrix form of 𝝎𝑏𝑝𝑏
Ω𝑝𝑒𝑝: Skew matrix form of 𝝎𝑝𝑒𝑝
𝐠𝑝: Gravity vector measured in 𝑝-frame
𝑔: Local gravity scalar
𝑔0: Local gravity scalar at sea level
𝜓𝐺: Grid azimuth angle of the vehicle in 𝑏-frame with respect to 𝑝-frame
𝛼: Wander azimuth angle of 𝑝-frame with respect to 𝑛-frame
𝜓: Heading angle of the vehicle in 𝑏-frame with respect to 𝑛-frame
𝜃: Grid pitch angle of the vehicle in 𝑏-frame with respect to 𝑛-frame or𝑝-frame
𝛾: Grid roll angle of the vehicle in 𝑏-frame with respect to 𝑛-frame or𝑝-frame
Δ𝜓: 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
𝜑0, 𝜆0, 0:Initial vehicle position (latitude, longitude, height)
Δ𝑡: Time step
𝐚: Vehicle acceleration
𝐯0=[𝑣𝐸0,𝑣𝑁0,𝑣𝑈0]: Initial vehicle velocity (east, north, up)
𝐯=[𝑣𝐸,𝑣𝑁,𝑣𝑈] Vehicle velocity (east, north, up)
𝑣𝑔: Vehicle ground velocity
𝐫𝑛𝑒: Vehicle position measured in 𝑛-frame with respect to 𝑒-frame
𝑒: 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
1/𝜏𝑎: Turn torsion
𝐐: Quaternion
𝑞1,𝑞2,𝑞3,𝑞4: Four components of the quaternion 𝐐
𝑇circle: Period of the circle trajectory in simulation
𝑇sshape: Period of the s-shape trajectory in simulation
𝐴sshape: Amplitude of the s-shape trajectory in simulation
PV: Position and velocity.

References

  1. H. Schneider and N. E. George Philip Barker, Matrices and Linear Algebra, Dover Publications, New York, NY, USA, 1989.
  2. A. Gilat, Matlab: An Introduction with Applications, John Wiley & Sons, New York, NY, USA, 3rd edition, 2008.
  3. D. H. Titterton and J. L. Weston, Strapdown Inertial Navigation Technology, Institution of Engineering and Technology, Stevenage, UK, 2004.
  4. Z. Chen, Strapdown Inertial Navigation System Principles, China Astronautic Publishing House, Beijng, China, 1986.
  5. P. S. Maybeck, “Wander azimuth implimentation algorithm for a strapdown inertial system,” Air Force Flight Dynamics Laboratory AFFDL-TR-73-80, Tech. Rep., Ohio, USA, 1973. View at Google Scholar
  6. J. C. Butcher, Numerical Methods for Ordinary Differencial Equations, John Wiley & Sons, New York, NY, USA, 2003.
  7. B. Yuan, Research on Rotating Inertial Navigation System with Four-Frequency Differential Laser Gyroscope, Graduate School of National University of Defense Technology, Changsha, China, 2007.
  8. I. Y. Bar-Itzhack, “Iterative optimal orthogonalization of the strapdowm matrix,” IEEE Transactions on Aerospace and Electronic Systems, vol. 11, no. 1, pp. 30–37, 1975. View at Google Scholar · View at Scopus