Modelling and Simulation in Engineering

Modelling and Simulation in Engineering / 2012 / Article

Research Article | Open Access

Volume 2012 |Article ID 264537 | 25 pages | https://doi.org/10.1155/2012/264537

Mathematical Model and Matlab Simulation of Strapdown Inertial Navigation System

Academic Editor: Ahmed Rachid
Received24 Dec 2010
Accepted05 Sep 2011
Published05 Jan 2012

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 Aโ€“D.

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.

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 asฬ‡๐‚๐‘๐‘’=โˆ’๐›€๐‘๐‘’๐‘๐‚๐‘๐‘’,(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๐›พ0โˆ’sin๐›พ010sin๐›พ0cos๐›พ1000cos๐œƒsin๐œƒ0โˆ’sin๐œƒcos๐œƒcos๐œ“๐บsin๐œ“๐บ0โˆ’sin๐œ“๐บcos๐œ“๐บ0โŽคโŽฅโŽฅโŽฅโŽฅโŽฆ.001(7)

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๐ถ2โŽกโŽขโŽขโŽขโŽขโŽฃ00โŽคโŽฅโŽฅโŽฅโŽฅโŽฆ=โŽกโŽขโŽขโŽขโŽขโŽฃฬ‡ฬ‡โŽคโŽฅโŽฅโŽฅโŽฅโŽฆ.ฬ‡๐œ“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)

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.

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 asฬ‡ฬ‡ฬ‡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๐ถ233โˆ’0.94114ร—10โˆ’6โ„Ž(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+๐ดsshape๎€ทsin2๐œ‹๐‘ก/๐‘‡sshape๎€ธ๎€ธ๐‘‡sshape,โ‹…2๐œ‹๐ดsshape๎€ทcos2๐œ‹๐‘ก/๐‘‡sshape๎€ธ๐‘‡sshape,๐‘Ž๐‘๐‘ฃ=โˆ’๐‘”๎€ท๐œ“sin0+๐ดsshape๎€ทsin2๐œ‹๐‘ก/๐‘‡sshape๎€ธ๎€ธ๐‘‡sshapeโ‹…2๐œ‹๐ดsshape๎€ทcos2๐œ‹๐‘ก/๐‘‡sshape๎€ธ๐‘‡sshape,๐œ“=๐œ“0+๐ดsshape๎‚ตsin2๐œ‹๐‘ก๐‘‡sshape๎‚ถ.ฬ‡๐œ“=2๐œ‹๐ดsshape๎€ทcos2๐œ‹๐‘ก/๐‘‡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.

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โŽคโŽฅโŽฅโŽฅโŽฅโŽฅโŽฅโŽฆ=12โŽกโŽขโŽขโŽขโŽขโŽขโŽขโŽฃ0โˆ’๐œ”๐‘๐‘๐‘๐‘ฅโˆ’๐œ”๐‘๐‘๐‘๐‘ฆโˆ’๐œ”๐‘๐‘๐‘๐‘ง๐œ”๐‘๐‘๐‘๐‘ฅ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๐‘ž3๎€ธ2๎€ท๐‘ž1๐‘ž3+๐‘ž0๐‘ž2๎€ธ2๎€ท๐‘ž1๐‘ž2+๐‘ž0๐‘ž3๎€ธ๐‘ž20โˆ’๐‘ž21+๐‘ž22โˆ’๐‘ž232๎€ท๐‘ž2๐‘ž3โˆ’๐‘ž0๐‘ž1๎€ธ2๎€ท๐‘ž1๐‘ž3โˆ’๐‘ž0๐‘ž2๎€ธ2๎€ท๐‘ž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๐ถ33โŽคโŽฅโŽฅโŽฅโŽฅโŽฆโŽกโŽขโŽขโŽขโŽขโŽฃ00๐œ”๐‘–๐‘’โŽคโŽฅโŽฅโŽฅโŽฅโŽฆ=โŽกโŽขโŽขโŽขโŽขโŽฃ๐œ”๐‘–๐‘’๐ถ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=sinโˆ’1๐‘‡32,๐›พprincipal=tanโˆ’1โˆ’๐‘‡31๐‘‡33,๐œ‘๐บprincipal=tanโˆ’1โˆ’๐‘‡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,principalโˆ’180โˆ˜,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=sinโˆ’1๐ถ33,๐œ†principal=tanโˆ’1๐ถ32๐ถ31,๐›ผprincipal=tanโˆ’1๐ถ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,principalโˆ’180โˆ˜,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๐ถ233โˆ’0.94114ร—10โˆ’6โ„Ž๎€ทm/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||=12โˆš1+๐‘‡11โˆ’๐‘‡22โˆ’๐‘‡33,||๐‘ž2||=12โˆš1โˆ’๐‘‡11+๐‘‡22โˆ’๐‘‡33,||๐‘ž3||=12โˆš1โˆ’๐‘‡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.

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

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.

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.

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.

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.

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.

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]:๎ƒฌ๐ท=3๎“3๐‘–=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:๐‘“๐‘›=3๎“3๐‘–=1๎“๐‘—=1๎€ท๐ถ๐‘–๐‘—โˆ’๐ถ๐‘–๐‘—๐‘›๎€ธ2(D.4) satisfies ๐‘“๐‘›+1โˆ’๐‘“๐‘›โ‰ค๐œ– (e.g., ๐œ–=10โˆ’10), 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ร—10โˆ’5โ€‰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

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.


More related articles

15062ย Views | 11182ย Downloads | 9ย Citations
 PDF  Download Citation  Citation
 Download other formatsMore
 Order printed copiesOrder

Related articles

We are committed to sharing findings related to COVID-19 as quickly and safely as possible. Any author submitting a COVID-19 paper should notify us at help@hindawi.com to ensure their research is fast-tracked and made available on a preprint server as soon as possible. We will be providing unlimited waivers of publication charges for accepted articles related to COVID-19. Sign up here as a reviewer to help fast-track new submissions.