Mathematical Model and Matlab Simulation of Strapdown Inertial ...

Hindawi Publishing CorporationModelling and Simulation in EngineeringVolume 2012, Article ID 264537, 25 pagesdoi:10.1155/2012/264537

Research Article

Mathematical Model and Matlab Simulation of StrapdownInertial Navigation System

Wen Zhang,1 Mounir Ghogho,2, 3 and Baolun Yuan1

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

Correspondence should be addressed to Wen Zhang, [email protected]

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.

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

1. Introduction

Many navigation books and papers on inertial navigationsystem (INS) provide readers with the basic principle of INS.Some also superficially describe simulation methods andrarely provide the free code which can be used by new INSusers to help them understand the theory and develop INSapplications. Commercial simulation software is availablebut is not free. The objective of this paper is to developan easy-to-understand step-by-step development method forsimulating INS. Here we consider the most popular INSwhich is the strapdown inertial navigation system (SINS).The mathematical operations required in our work aremostly matrix manipulations and more generally basic linearalgebra [1]. In this paper, Matlab [2] is chosen as the simula-tion environment. It is a popular computing environment toperform complex matrix calculations and to produce sophis-ticated graphics in a relatively easy manner. A large collectionof Matlab scripts are now available for a wide variety ofapplications and are often used for university courses. Matlabis also becoming more and more popular in industrialresearch centers in the design and simulation stages.

The main purposes of this paper are to establish amathematical model and to develop a comprehensive Matlabimplementation for SINS. The structure of the proposedmathematical model and Matlab simulation of SINS is

shown in Figure 1. In Section 2, the INS-related orthogonalcoordinates (the body frame, the inertial frame, the Earthframe, the navigation frame, the ENU-frame, and the wanderazimuth navigation frame) are described and figures to illus-trate the relationship between the frames are provided. Thebasic principle of SINS is described in the wander azimuthnavigation frame (p-frame). In Section 3, two importantdirection cosine matrices (DCMs), the vehicle attitude DCMand the position DCM, and the related important attitudeand position angles are defined. In Section 4, the simulationfor data generation of gyros and accelerometers is describedin ENU-frame. Instead of p-frame, ENU-frame is chosenbecause the outputs of gyros and accelerometers are easierto obtain in this frame. The Matlab implementation isgiven and described step by step. Four kinds of scenarios(static, straight, circle, and s-shape) are set as examplesof different kinds of vehicle trajectories. In Section 5, themathematical model of SINS is set up and the calculationsteps in p-frame are provided. In Section 6, the requiredinitial parameters and other initial data calculation for theSINS model are given for the different simulation scenarios.In Section 7, Matlab implementation code functions arelisted and described. Further, simulation results for thefour above-mentioned scenarios are presented; two examplesfrom real SINS experiment data are also provided to verifythe validity of the developed codes. Finally, conclusions

2 Modelling and Simulation in Engineering

Introduction of SINSrelated frames;

principle of SINS

Gyro and accelerometerdata generator proposed in

the ENU-framemechanization

Initial parameters andintial data calculation

Trajectory simulationbased on sensor data

proposed in the p-frame

mechanization

(1) Static

(2) Straight line

(3) Circle shape(4) S-shape(5) Real static data set A

(6) Real static data set B

Matlab implementationand 6 SINS examples:

Definition of importantDCMs to show the

relations among b-, e-, p-,and ENU-frames

-

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

P

xb

yb

zb

Pitch

Roll

Yaw

Figure 2: The b-frame illustration and the definition of axis rotations.

are drawn. Mathematical details are given in AppendicesA–D.

2. Principles

A fundamental aspect of inertial navigation is the precise def-inition of a number of Cartesian coordinate reference frames.Each frame is an orthogonal, right-handed, coordinate frameor axis set. For all the coordinate frames used in this paper, apositive rotation about each axis is taken to be in a clockwisedirection looking along the axis from the origin, as indicatedin Figure 2. A negative rotation corresponds to an anti-clockwise direction. This convention is used throughout thispaper. It is also worth pointing out that a change in attitudeof a body, which is subjected to a series of rotations aboutdifferent axes, is not only a function of the rotation angles,but also on the order in which the rotations occur. In thispaper, the following coordinate frames are used [3].

(1) The body frame (b-frame): the b-frame, depicted inFigure 2, is an orthogonal axis set which has its originat the center of the vehicle, point P, and is aligned

with the pitch Pxb axis, roll Pyb axis, and yaw Pzbaxis of the vehicle in which the navigation system isinstalled.

(2) The inertial frame (i-frame): the i-frame, depicted inFigure 3, has its origin at the center of the Earth andits axes nonrotating with respect to fixed stars; theseaxes are denoted by Oxi, Oyi, and Ozi, with Ozi beingcoincident with the Earth polar axis.

(3) The Earth frame (e-frame): the e-frame, depicted inFigure 3, has its origin at the center of the Earth andaxes nonrotating with respect to the Earth; these axesare denoted by Oxe, Oye, and Oze. The axis Oze is theEarth polar axis. The axisOxe is along the intersectionof the plane of the Greenwich meridian and theEarth equatorial plane. The Earth frame rotates withrespect to the inertial frame at a rateωie about the axisOzi.

(4) The navigation frame (n-frame): the n-frame,depicted in Figure 3, is a local geographic navigation

Modelling and Simulation in Engineering 3

frame which has its origin at the location of the navi-gation system, point P (the navigation system is fixedinside the vehicle and we assume that the navigationsystem is located exactly at the center of the vehicle),and axes aligned with the directions of east PE, northPN and the local vertical up PU . When the n-frameis defined in this way, it is called the “ENU-frame.”The turn rate of the navigation frame with respect tothe Earth-fixed frame, ωen, is governed by the motionof the point P with respect to the Earth. This is oftenreferred to as the transport rate.

(5) The wander azimuth navigation frame (p-frame):the p-frame, depicted in Figure 3, may be used toavoid the singularities in the computation whichoccur at the poles of the n-frame. Like the n-frame,it is of a local level but rotates through the wanderangle about the local vertical. Here we do not call thisframe w-frame (w for wander) for notation claritysince w and ω may look similar when printed. Letterp in p-frame stands for platform; indeed the wanderazimuth navigation frame is of a local level and thusforms a horizontal platform.

In this paper, we choose the p-frame as the navigationframe for vehicle trajectory calculation, for the followingreason. In the local geographic navigation frame mecha-nization, the n-frame is required to rotate continuously asthe system moves over the surface of the Earth in order tokeep its PyN axis pointing to the true north. In order toachieve this condition worldwide, the n-frame must rotateat much greater rates about its PzU axis as the navigationsystem moves over the surface of the Earth in the polarregions, compared to the rates required at lower latitudes.It is clear that near the polar areas the local geographicnavigation frame must rotate about its PzU axis rapidlyin order to maintain the PyN axis pointing to the pole.The heading direction will abruptly change by 180◦ whenmoving past the pole. In the most extreme case, the turnrate becomes infinite when passing over the pole. One wayof avoiding the singularity, and also providing a navigationsystem with worldwide capability, is to adopt a wanderazimuth mechanization in which the z-component of ω

pep is

always set to zero, that is, ωpepz = 0. A wander axis system

is a local level frame which moves over the Earth surfacewith the moving vehicle, as depicted in Figure 3. However,as the name implies, the azimuth angle α between PyN axisand Pyp axis varies with the vehicle position on Earth. Thisvariation is chosen in order to avoid discontinuities in theorientation of the wander frame with respect to Earth as thevehicle passes over either the north or south pole.

In the remainder of this section, the main principle ofSINS in the p-frame is described.

Along the same lines as in [3], a navigation equation fora wander azimuth system can be constructed as follows:

vpe = C

pb fb −

(2C

peωe

ie + ωpep

)vpe + gp, (1)

where Cpb is the direction cosine matrix used to transform

the measured specific force vector in b-frame into p-frame.

This matrix propagates in accordance with the followingequation:

Cpb = C

pbΩ

bpb, (2)

where Ωbpb is the skew symmetric form of ωb

pb, the b-frameangular rate with respect to the p-frame.

Equation (1) is integrated to generate estimates of thevehicle speed in the wander azimuth frame, v

pe . This is then

used to generate the turn rate of the wander frame withrespect to the Earth frame, ω

pep. The direction cosine matrix

which relates the wander frame to the Earth frame, Cpe , may

be updated using the following equation:

Cep = Ce

pΩpep, (3)

(Cep

)T =(Ωpep

)T(Cep

)T = −Ωpep

(Cep

)T, (4)

where the superscript T means matrix transposition.Since (Ce

p)T = Cpe , (Ce

p)T = Cpe and skew symmetric

matrix is (Ωpep)T = −Ωp

ep (see Appendix A), (4) can berewritten as

Cpe = −Ωp

epCpe , (5)

where Ωpep is a skew symmetric matrix formed from the

elements of the angular rate vector ωpep; we could have ω

pep =

−ωepe when the rotation angles are reciprocal. Because the

z-component of ωpep is set to zero, ω

pepz = 0, the matrix

expression of ωpep is ω

pep = [ω

pepx ω

pepy 0]T . This process is

implemented iteratively and enables any singularities to beavoided.

In the next section, the two important DCMs, the vehicleattitude DCM and vehicle position DCM, are defined, as wellas 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 corre-sponding attitude angles and the vehicle position DCM withthe corresponding position angles are described separately.

3.1. Vehicle Attitude DCM Cpb . The definition of the rotation

sequence from p-frame to b-frame is (see Figure 4)

xp ypzpzp , ψG−−−→ x′e y

′ez′e

y′′p , θ−−−→ x′′e y′′e z′′e

y′′p , γ−−−→ xb ybzb, (6)

where ψG is the gird azimuth angle (0–360◦), θ is the pitchangle (−90◦–90◦), and γ is the roll angle (−180◦–180◦). The


above rotation can be written in the following matrix form:

Cbp = C3C2C1

=

⎡⎢⎢⎢⎣

cos γ 0 − sin γ

0 1 0

sin γ 0 cos γ

⎤⎥⎥⎥⎦

⎡⎢⎢⎢⎣

1 0 0

0 cos θ sin θ

0 − sin θ cos θ

⎤⎥⎥⎥⎦

×

⎡⎢⎢⎢⎣

cosψG sinψG 0

− sinψG cosψG 0

0 0 1

⎤⎥⎥⎥⎦.

(7)

The vehicle attitude DCM Tpb is then obtained as

Cpb =

(Cbp

)T =

⎡⎢⎢⎢⎣

cos γ cosψG − sin γ sin θ sinψG − cos θ sinψG sin γ cosψG + cos γ sin θ sinψG

cos γ sinψG + sin γ sin θ cosψG cos θ cosψG sin γ sinψG − cos γ sin θ cosψG

− sin γ cos θ sin θ cos γ cos θ

⎤⎥⎥⎥⎦. (8)

For the p-frame system, the angle between the grid north ypand the true north yN is the wander azimuth angle α. So theangle between the horizontal projection along y′p axis of thevehicle’s vertical axis zb and the real north yN is the headingangle ψ. We have that

ψ = ψG + α. (9)

So the direction cosine matrix Cnb from b-frame to n-frame is

Cnb =

⎡⎢⎢⎢⎣

cos γ cosψ − sin γ sin θ sinψ − cos θ sinψ sin γ cosψ + cos γ sin θ sinψ

cos γ sinψ + sin γ sin θ cosψ cos θ cosψ sin γ sinψ − cos γ sin θ cosψ


⎤⎥⎥⎥⎦. (10)

The gimbal angles ψ, θ, and γ and the gimbal rates ψ,θ, and γ are related to the body rate ωbnb, which is the turnrate of the b-frame with respect to n-frame and measured inb-frame as follows:

⎡⎢⎢⎢⎣

ωbnbx

ωbnby

ωbnbz

⎤⎥⎥⎥⎦ =

⎡⎢⎢⎢⎣

0

γ

0

⎤⎥⎥⎥⎦ + C3

⎡⎢⎢⎢⎣

θ

0

0

⎤⎥⎥⎥⎦ + C3C2

⎡⎢⎢⎢⎣

0

0

ψ

⎤⎥⎥⎥⎦

=

⎡⎢⎢⎢⎣

cos γθ − sin γ cos θψ

γ + sin θψ

sin γθ + cos γ cos θψ

⎤⎥⎥⎥⎦.

(11)

3.2. Vehicle Position DCM Cpe . Position matrix C

pe is the

DCM from e-frame to p-frame. It has the following rotatingsequence (see Figure 5):

xe yezeze , λ−−→ x′e y

′ez′e

y′′p ,90◦−ϕ−−−−−→ x′′e y′′e z′′e

z′′p , 90◦−−−−→ xE yNzUzU , α−−−→ xp ypzp,

(12)

where λ is the longitude angle (−180◦–180◦), ϕ is the latitudeangle (−90◦–90◦), and α is the wander azimuth angle (0–360◦). The above rotation can be written in the followingmatrix form:

Cpe =

⎡⎢⎢⎢⎣

− 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 theENU-frame is described step by step to generate sen-sor data. In Section 5, a trajectory and attitude simu-lator method in the p-frame is described step by step

to derive the desired trajectory and attitude from thesimulated sensor data or real sensor data; Section 6provides the initial parameters and initial data calcula-tion.


Greenwichmeridian

i-frame

North pole

n-frame

p-frame

Localmeridian

plane

e-frame

Equatorialplane

ωie

zize yN

yp zpzUxExpα

xe

yi

yexi

P

O

Figure 3: The reference frames.

PitchRoll

Yawγ

θ

xp

xbα

ytyp

ΨG

ΨG

γ θ

Ψ

P

zp , zp

xp , xp yb , ypyp

zp

�γ

�θ

�ΨG

zb

Figure 4: The relation between b-frame and p-frame.

4. Sensor Data Generator

The purpose of trajectory simulation is to generate data ofthe 3 orthogonal gyros and the 3 orthogonal accelerometersaccording to the designed trajectory. It is mentioned inSection 2 that p-frame is set up to avoid the singularitieswhen the vehicle passes over either the north or southpole. But in most applications, the SINS systems are seldomoperated under this extreme environment. The ENU-framecan be implemented easier than p-frame, so it is chosen asthe navigation frame. Figure 6 shows the whole process of theSINS principal in the ENU-frame mechanization. First, thevehicle trajectory in the ENU-frame is set. Then, the sensorideal output is derived using the inverse principle of INS.The sensor simulation data can be obtained by adding noiseto the ideal data. Then, we use the simulated sensor datato derive the noise-corrupted simulated trajectory. Besides,the difference between the ideal and simulated state vectorscan be set as the input for the observed measurements in theKalman filter.

4.1. The Initial Parameters. For the designed trajectory, theinitial parameters are

(1) initial position, latitude ϕ0, longitude λ0, height h0;

(2) initial velocity v = [vE0, vN0, vU0];

(3) the designed variation of acceleration a, which varieswith 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 thedesigned trajectory.

4.2. The Update of Velocity. The velocity is updated as

v ←− v + aΔt, (14)

where Δt is the time step.

4.3. The Update of Position. The position is updated as

latitude: L←− L +vNΔt

RN,

longitude: λ←− λ +vEΔt secL

RE

altitude: h←− h + vUΔt.

, (15)

4.4. The Update of Attitude. The attitude angles are updatedas

pitch: θ ←− θ + Δθ,

roll: γ ←− γ + Δγ,

heading: ψ ←− ψ + Δψ.

(16)

The attitude rates are updated as

pitch: θ ←− θ + Δθ,

roll: γ ←− γ + Δγ,

heading: ψ ←− ψ + Δψ.

(17)

The expressions for Δθ, Δγ, Δψ, Δθ, Δγ, and Δψ depend onthe designed trajectory.

The direction cosine matrix Cnb can be calculated using

matrix expression (10). We have that Cbn = (Cn

b)T .

4.5. Gyro Data Generator. The output of the gyros is

ωbib =

(I− Sg

)(Cbn

(ωnie + ωn

en

)+ ωb

nb

)+ εb, (18)

where ωbib is the simulated actual output, I is the 3 × 3

unit matrix, Sg is the 3 × 3 diagonal matrix whose diagonalelements correspond to the 3 gyros’ scale factor errors, and


yN yp

ϕ yeλ

α

λxe

α

North pole

ωie

n-frame

p-frame

e-frame

90◦ − ϕ

xp

90◦ − ϕ

P

ze , zp , zU

xE , ye

ye , ye

ze , ze

xe

xe

xe

O

Figure 5: The relation between e-frame and p-frame.

Local gravity vector

Gravitycomputer

Position

Velocity

Corioliscomputer

Attitude

Navigationcomputer

Attitudecomputer

Attitude calculation

Body mountedaccelerometers

Body mountedgyroscopes

nie = [0 ie cosL ie sinL]T

g = 9.7803 + 0.51799 233

− 0.94114× 10−6h, g0 = 9.791719

−(2 nie + n

en)× vne

nb

nb

bib

nie + n

en

˙nb = n

bΩbnb

rne

vne = [vN vE vD]

Resolution ofspecific forcemeasurement

Initial estimates of attitude

bnb = b

ib − bn ( n

ie + nen)

Initial estimates ofvelocity and position

WGS84: Re = 6378140 m,

ie = 7.2921151467e−5 rad/se = 0.00335281066475

nen = [

−vNRN + h

vERE + h

vE tanL

RN + h]T

RN =Re(1− e2)

(1− e2 sin2 L)3/2, RE =

Re

(1− e2 sin2 L)1/2

S

vne = nb

b − (2 nie + n

en) × vne + gnl

S

ω

ω

ω

ω ω ω ω

ω ω

ωω

ωω

f f

f

C

C

C

C

Cω ω

ω

C

Figure 6: SINS ENU-frame mechanization.

εb is the gyro’s drift and can be simulated as the sum of aconstant noise and a random white noise: εb = εbconst+ε

brandom:

ωnie =

⎡⎢⎢⎢⎣

0

ωie cosL

ωie sinL

⎤⎥⎥⎥⎦. (19)

In a static base, ωbnb is equal to zero, whereas, in a moving

base it is obtained as

ωbnb =

⎡⎢⎢⎢⎣

cos γθ − sin γ cos θψ

γ + sin θψ

sin γθ + cos γ cos θψ

⎤⎥⎥⎥⎦. (20)


Gravitycomputer

Position

Corioliscomputer

Attitude

Navigationcomputer

Attitudecomputer

Attitude calculation

accelerometers

gyroscopes

Resolution ofspecific forcemeasurement

g = 9.7803 + 0.51799 C233

− 0.94114× 10−6h, g0 = 9.791719

b

bib

Cpe

[λ ϕ α]

Tpb

[ΨG θ γ]

Ψ = ΨG + α

vPe = [vpex v

pey v

pez]

vne = [vN vE vD]T

˙ pe = −Ωpep

pe

pep = [ω

pepx ω

pepy 0]T

1/Ryp = (1/Re)(1− eC233 + 2eC2

23)

1/Rxp = (1/Re)(1− eC233 + 2eC2

13)

Tpb

p

−(2pie +

pep)×vpe

bpb = b

ib − (Cpb )−1(

pie +

pep)

vpe = C

pb f

b − (2pie +

pep)× v

pe + g p

pie +

pep

Results

Initial velocityand position

ωpepy v

pey

1/Rxp 1/τa

−1/τa −1/Ryp

Velocity

ωpepx

=vpex

Cpb = C

pbΩ

bpb

pie = p

e ωeie = ωie [

pe13

pe23

pe33]T

1/τa = (2e/Re)C13C23

Initialattitude

WGS84: Re = 6378140 m,ωie = 7.2921151467e−5 rad/s

e = 0.00335281066475

S S

=Apey cos(α)− A

pex sin(α)

Apey sin(α) + A

pex cos(α)

Apez

Body-mounted

Body-mounted f

C C

C CCCω

ω

ω ω

ωω

ωω

ωω

ω

f

ω ω

Figure 7: SINS p-frame mechanization.

Begin

StopGyro and

accelerometer data

Initial parametersand initial data calculation

Time stepincreases one

Computeangular velocity

Computequaternion

Computeattitude matrix

Computevelocity

Computeattitude matrix

Computeposition matrix

Computeposition

Computeattitude angle

End?

Yes

No

Figure 8: SINS program structure.


0 2000 4000−3

−2.5

−2

−1.5

−1

−0.5

Time (s)

Tru

e lo

ngi

tude

(de

g)

(a)

0 2000 4000

Time (s)

52

52.2

52.4

52.6

52.8

53

53.2

53.4

53.6

53.8

54

Tru

e la

titu

de (

deg)

(b)

52

52.2

52.4

52.6

52.8

53

53.2

53.4

53.6

53.8

54

−4 −2 0

Tru

e la

titu

de (

deg)

True longitude (deg)

(c)

Figure 9: The designed trajectory of static simulation.

ωnen is related to velocity v = [vE, vN , vU]T and can be

expressed as

ωnen =

⎡⎢⎢⎢⎢⎢⎢⎢⎣

−vNRNvERE

vE tanLRN

⎤⎥⎥⎥⎥⎥⎥⎥⎦. (21)

4.6. Accelerometer Data Generator. The measurement of theaccelerometer is the specific force:

fb = (I− Sa)Cbnfn + ηb,

fn = a +(2ωn

ie + ωnen

)× v − g,(22)

where fb is the simulated actual output, I is the 3 × 3 unitmatrix. Sa is the 3 × 3 diagonal matrix whose diagonalelements correspond to the 3 accelerometers’ scale factorerrors, ηb is the bias considered as the sum of a constant noiseand a random white noise ηb = ηbconst +ηbrandom. g = [0 0 g]T ,and g = 9.7803 + 0.051799C2

33 − 0.94114 × 10−6h (m/s2),where C33 is the 9th element of C

pe and h is the vehicle

altitude.

4.7. Examples. For four examples of static, straight line,circle, and s-shape situations, details will be given next underthe conditions that the vehicle is moving on the surface of theEarth with no attitude change except for the heading angle,

which means that the pitch angle, roll angle, and altitude areconstants during the simulation process:

Δθ = 0,

Δγ = 0,

Δθ = 0,

Δγ = 0,

(23)

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

(1) Static:

L = constant,

λ = constant,

vE = constant,

vN = constant,

Δψ = 0,

Δψ = 0.

(24)

(2) Straight line:

aE = constant,

aN = constant,

vE = vE + aEΔt,

vN = vN + aNΔt,


0 2000 4000−2.5

−2

−1.5

−1

−0.5

0

0.5

1

1.5

Time (s)

cou

rseA

ngl

e er

ror

(min

ute

)

(a)

−0.6

−0.4

−0.2

0

0.2

0.4

0.6

0.8

1

1.2

0 2000 4000

Time (s)

pitc

hA

ngl

e er

ror

(min

ute

)

(b)

0 2000 4000

Time (s)

−2

−1.5

−1

−0.5

0

0.5

1

1.5

tilt

An

gle

erro

r (m

inu

te)

(c)

Figure 10: Angle error of static simulation.

ψ = ψ0 + arctan(vEvN

),

ψ = aNvE − aEvNv2E + v2

N

.

(25)

(3) Circle:

vg = constant,

Δψ = mod(

2πΔtTcircle

, 2π)

,

Δψ = 2πTcircle

,

aE = −2πvg cosψ

Tcircle,

aN = −2πvg sinψ

Tcircle.

(26)

(4) S-shape:

vg = constant,

aE = −vg cos

(ψ0 + Asshape sin

(2πt/Tsshape

))

Tsshape,

·2πAsshape cos

(2πt/Tsshape

)

Tsshape,

aN = −vg sin

(ψ0 +Asshape sin

(2πt/Tsshape

))

Tsshape

·2πAsshape cos

(2πt/Tsshape

)

Tsshape,

ψ = ψ0 + Asshape sin

(2πtTsshape

).

ψ =2πAsshape cos

(2πt/Tsshape

)

Tsshape.

(27)

5. Mathematical Model and TrajectoryCalculation Steps

After the gyro and accelerometer data are simulated using themethod described in the previous section under the designedscenario, the next step we have to do is to figure out the math-ematical model of SINS and the calculation steps to processthe sensor data to get the calculated trajectories. Based on thebasic principles of strapdown inertial navigation system [4],we draw the mathematical model in the p-frame mechaniza-tion in Figure 7. The calculation steps are described below.Although the situation that the vehicle passes over either thenorth or south pole seldom happens, the universal p-frameis still chosen instead of the simpler ENU-frame to give anavigation illustration in a different frame.


0 1000 2000 3000 40000

2

4

6

Time (s)

Lon

gitu

deer

ror

(min

ute

)

(a)

−3

−2

−1

0

0 1000 2000 3000 4000

Time (s)

Lati

tude

erro

r(m

inu

te)

(b)

0 1000 2000 3000 4000

Time (s)

0

1

2

3

4

Dis

tan

ceer

ror

(nm

)

(c)

0 1000 2000 3000 4000

Time (s)

0

1

2

3

4

Gro

un

dve

loci

tyer

ror

(m/s

)

(d)

Figure 11: Position and velocity error of static simulation.

5.1. Quaternion Q Update and Optimal Normalization.There are three kinds of strapdown attitude representations:DCM, Euler angle, and quaternion. In this paper, wechoose quaternion. The reason why quaternion is chosen isexplained in [3].

The quaternion formed by a rotating body frame aroundthe platform frame is

Q = q0 + q1ib + q2 jb + q3kb. (28)

The update for the quaternion can be obtained by solving thefollowing quaternion differential equation:

⎡⎢⎢⎢⎢⎢⎢⎣

q0

q1

q2

q3

⎤⎥⎥⎥⎥⎥⎥⎦= 1

2

⎡⎢⎢⎢⎢⎢⎢⎢⎣

0 −ωbpbx −ωbpby −ωbpbzωbpbx 0 ωbpbz −ωbpbyωbpby −ωbpbz 0 ωbpbx

ωbpbz ωbpby −ωbpbx 0

⎤⎥⎥⎥⎥⎥⎥⎥⎦

⎡⎢⎢⎢⎢⎢⎢⎣

q0

q1

q2

q3

⎤⎥⎥⎥⎥⎥⎥⎦. (29)

Based on the Euclide norm minimized indicator [4], theoptimal normalization for the quaternion is

Q←− Q√q2

0 + q21 + q2

2 + q23

. (30)

5.2. Cpb Calculation. C

pb is vehicle attitude DCM which

transforms the measured angle in the b-frame to the p-frame, with its 9 components Tij , i, j = 1, 2, 3. (Here we useTij to distinguish it from the components Cij , i, j = 1, 2, 3 of

Cpe which is used below.)

After obtaining q0, q1, q2, and q3 using (29), Cpb can be

calculated as

Cpb

=

⎡⎢⎢⎢⎣

T11 T12 T13

T21 T22 T23

T31 T32 T33

⎤⎥⎥⎥⎦

=⎡⎢⎣q2

0 + q21 − q2

2 − q23 2

(q1q2 − q0q3

)2(q1q3 + q0q2

)

2(q1q2 + q0q3

)q2

0 − q21 + q2

2 − q23 2

(q2q3 − q0q1

)

2(q1q3 − q0q2

)2(q2q3 + q0q1

)q2

0 − q21 − q2

2 + q23

⎤⎥⎦.

(31)

5.3. Specific Force Transformation from fb in b-Frame to f p

in p-Frame. The specific force fb in the b-frame can betransformed to f p in the p-frame by multiplication with


0 2000 4000−1.7

−1.6

−1.5

−1.4

−1.3

−1.2

−1.1

−1

−0.9

−0.8

−0.7

Time (s)

Tru

e lo

ngi

tude

(de

g)

(a)

0 2000 4000

Time (s)

53

53.2

53.4

53.6

53.8

54

54.2

54.4

Tru

e la

titu

de (

deg)

(b)

53

53.2

53.4

53.6

53.8

54

54.2

54.4

−2 −1 0

Tru

e la

titu

de (

deg)


(c)

Figure 12: The designed trajectory of straight line simulation.

DCM Cpb :

f p = Cpb fb,

⎡⎢⎢⎢⎣

fpx

fpy

fpz

⎤⎥⎥⎥⎦ = C

pb

⎡⎢⎢⎢⎣

f bx

f by

f bz

⎤⎥⎥⎥⎦,

(32)

5.4. Velocity vpe Calculation. The velocity v

pe update can be

obtained by solving the following differential equation:

vpe = f p −

(2ω

pie + ω

pep

)vpe + gp,

⎡⎢⎢⎢⎣

vx

vy

vz

⎤⎥⎥⎥⎦ =

⎡⎢⎢⎢⎣

fpx

fpy

fpz

⎤⎥⎥⎥⎦−

⎡⎢⎢⎢⎣

0

0

g

⎤⎥⎥⎥⎦

+

⎡⎢⎢⎢⎢⎣

0 2ωpiez −

(2ω

piey+ω

pepy

)

−2ωpiez 0 2ω

piex + ω

pepx

2ωpiey+ω

pepy −

(2ω

piex+ω

pepx

)0

⎤⎥⎥⎥⎥⎦

×

⎡⎢⎢⎢⎣

vx

vy

vz

⎤⎥⎥⎥⎦.

(33)

The ground speed is the vehicle velocity projection on thehorizontal plane:

vg =√v2x + v2

y. (34)

5.5. Position Matrix Cpe Update. The update for the position

matrix Cpe can be obtained by solving the following differen-

tial equation, noticing that ωpepz = 0:

Cpe = −Ωp

epCpe ,

Cpe =

⎡⎢⎢⎢⎣

C11 C12 C13

C21 C22 C23

C31 C32 C33

⎤⎥⎥⎥⎦,

⎡⎢⎢⎢⎣

C11 C12 C13

C21 C22 C23

C31 C32 C33

⎤⎥⎥⎥⎦ =

⎡⎢⎢⎢⎣

0 0 −ωpepy

0 0 ωpepx

ωpepy −ωp

epx 0

⎤⎥⎥⎥⎦

⎡⎢⎢⎢⎣

C11 C12 C13

C21 C22 C23

C31 C32 C33

⎤⎥⎥⎥⎦.

(35)

5.6. Position Angular Velocity ωpep Update. In the chosen

wander azimuth navigation frame, we have ωpepz = 0, and

⎡⎣ω

pepx

ωpepy

⎤⎦ =

⎡⎢⎢⎢⎣− 1τa− 1Ryp

1Rxp

1τa

⎤⎥⎥⎥⎦

⎡⎣v

pex

vpey

⎤⎦, (36)


−2.5

−2

−1.5

−1

−0.5

0

0.5

1

0 2000 4000

Time (s)

cou

rseA

ngl

e er

ror

(min

ute

)

(a)

0 2000 4000

Time (s)

−0.2

0

0.2

0.4

0.6

0.8

1

1.2

pitc

hA

ngl

e er

ror

(min

ute

)

(b)

0 2000 4000

Time (s)

−2

−1.5

−1

−0.5

0

0.5

1

1.5

tilt

An

gle

erro

r (m

inu

te)

(c)

Figure 13: Angle error of straight line simulation.

0 1000 2000 3000 40000

2

4

6

Time (s)

Lon

gitu

deer

ror

(min

ute

)

(a)

0 1000 2000 3000 4000

Time (s)

−3

−2

−1

0

1

Lati

tude

erro

r(m

inu

te)

(b)

0 1000 2000 3000 4000

Time (s)

0

1

2

3

4

Dis

tan

ceer

ror

(nm

)

(c)

0 1000 2000 3000 4000

Time (s)

Gro

un

dve

loci

tyer

ror

(m/s

)

−2

−1

0

1

(d)

Figure 14: Position and velocity error of straight line simulation.


0

2000

4000

−1.7045

−1.704

−1.7035

−1.703

−1.7025

−1.702

−1.7015

−1.701

−1.7005

−1.7

Time (s)

Tru

e lo

ngi

tude

(de

g)

(a)

52.9985

52.999

52.9995

53

53.0005

53.001

53.0015

53.002

0

2000

4000

Time (s)

Tru

e la

titu

de (

deg)

(b)

−1.7

04

−1.7

02

−1.7

52.9985

52.999

52.9995

53

53.0005

53.001

53.0015

53.002

Tru

e la

titu

de (

deg)


(c)

Figure 15: The designed trajectory of circle simulation.

0 2000 4000

Time (s)

−14

−12

−10

−8

−6

−4

−2

0

2

cou

rseA

ngl

e er

ror

(min

ute

)

(a)

0 2000 4000

Time (s)

−1.5

−1

−0.5

0

0.5

1

1.5

pitc

hA

ngl

e er

ror

(min

ute

)

(b)

0 2000 4000

Time (s)

−1.5

−1

−0.5

0

0.5

1

1.5

tilt

An

gle

erro

r (m

inu

te)

(c)

Figure 16: Angle error of circle simulation.


0 1000 2000 3000 4000

Time (s)

0

1

2

3

Lon

gitu

deer

ror

(min

ute

)

(a)

0 1000 2000 3000 4000

Time (s)

−3

−2

−1

0

1

Lati

tude

erro

r(m

inu

te)

(b)

0 1000 2000 3000 4000

Time (s)

0

1

2

3

Dis

tan

ceer

ror

(nm

)

(c)

0 1000 2000 3000 4000

Time (s)

2

4

Gro

un

dve

loci

tyer

ror

(m/s

)

−2

0

(d)

Figure 17: Position and velocity error of circle simulation.

where

1Ryp

= 1Re

(1− eC2

33 + 2eC223

),

1Rxp

= 1Re

(1− eC2

33 + 2eC213

),

1τa= 2eReC13C23,

(37)

where the elements of position matrix Cpe can be obtained

using (35).

5.7. Earth Angular Velocity ωpie and Attitude Angular Velocity

ωbpb Calculation. We have that

ωpie = C

peωe

ie =

⎡⎢⎢⎢⎣

C11 C12 C13

C21 C22 C23

C31 C32 C33

⎤⎥⎥⎥⎦

⎡⎢⎢⎢⎣

0

0

ωie

⎤⎥⎥⎥⎦ =

⎡⎢⎢⎢⎣

ωieC13

ωieC23

ωieC33

⎤⎥⎥⎥⎦, (38)

ωbpb = ωb

ib − ωbip = ωb

ib −(

Cpb

)−1(ωpie + ω

pep

), (39)

5.8. Attitude Angle Calculation. The relation between atti-tude matrix C

pb and the three attitude angles, grid azimuth

angle ψG, pitch angle θ, and roll angle γ, is

Cpb =

⎡⎢⎢⎢⎣

cos γ cosψG − sin γ sin θ sinψG − cos θ sinψG sin γ cosψG + cos γ sin θ sinψG

cos γ sinψG + sin γ sin θ cosψG cos θ cosψG sin γ sinψG − cos γ sin θ cosψG


⎤⎥⎥⎥⎦. (40)


0 2000 4000−1.703

−1.7025

−1.702

−1.7015

−1.701

−1.7005

−1.7

−1.6995

−1.699

Time (s)

Tru

e lo

ngi

tude

(de

g)

(a)

53

53.005

53.01

53.015

53.02

53.025

53.03

53.035

53.04

0 2000 4000

Time (s)

Tru

e la

titu

de (

deg)

(b)

−1.71 −1.7 −1.6953

53.005

53.01

53.015

53.02

53.025

53.03

53.035

53.04

Tru

e la

titu

de (

deg)


(c)

Figure 18: The designed trajectory of s-shape simulation.

Thus, the principal values of ψG, θ, and γ are

θprincipal = sin−1T32,

γprincipal = tan−1−T31

T33,

ϕGprincipal = tan−1−T12

T22.

(41)

Considering the defined range of the angles, the expressionsof the real values of ψG, θ, γ, and are

θ ←− θprincipal,

γ ←−

⎧⎪⎪⎪⎪⎨⎪⎪⎪⎪⎩

γprincipal, if T33 > 0,

γprincipal + 180◦, if T33 < 0, γprincipal < 0,

γprincipal − 180◦, if T33 < 0, γprincipal > 0,

ψG ←−

⎧⎪⎪⎪⎪⎨⎪⎪⎪⎪⎩

ψGprincipal, if T22 > 0, ψGprincipal > 0,

ψGprincipal + 360◦, if T22 > 0, ψGprincipal < 0,

ψGprincipal + 180◦, if T22 < 0.

(42)

5.9. Position Angle Calculation. The relation between posi-tion matrix C

pe and the 3 position angles, longitude λ, latitude

ϕ, and wander azimuth angle α, is

Cpe =

⎡⎢⎢⎢⎣

− 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−1C33,

λprincipal = tan−1C32

C31,

αprincipal = tan−1C13

C23.

(44)

Considering the defined range of the angles, the expressionsof the real values of ϕ, λ, and α are

ϕ←− ϕprincipal,

λ←−

⎧⎪⎪⎨⎪⎪⎩

λprincipal, if C31 > 0,

λprincipal + 180◦, if C31 < 0, λprincipal < 0,

λprincipal − 180◦, if C31 < 0, λprincipal > 0,


0 2000 4000

Time (s)

−2.5

−2

−1.5

−1

−0.5

0

0.5

1

1.5

cou

rseA

ngl

e er

ror

(min

ute

)

(a)

0 2000 4000

Time (s)

−1.5

−1

−0.5

0

0.5

1

1.5

pitc

hA

ngl

e er

ror

(min

ute

)

(b)

0 2000 4000

Time (s)

−2

−1.5

−1

−0.5

0

0.5

1

1.5

tilt

An

gle

erro

r (m

inu

te)

(c)

Figure 19: Angle error of s-shape simulation.

α←−

⎧⎪⎪⎪⎪⎨⎪⎪⎪⎪⎩

αprincipal, if C23 > 0, αprincipal > 0,

αprincipal + 360◦, if C23 > 0, αprincipal < 0,

αprincipal + 180◦, if C23 < 0.

(45)

5.10. Heading Angle Calculation. The heading angle ψ iscalculated as

ψ = ψG + α. (46)

To make sure that ψ will not be out of range, we shoulddetermine it according to

ψ ←−⎧⎨⎩ψ, if ψ < 360◦,

ψ − 360◦, if ψ ≥ 360◦.(47)

5.11. Velocity vne in n-Frame Calculation. We have that

vne =

⎡⎢⎢⎢⎣

vE

vN

vU

⎤⎥⎥⎥⎦ =

⎡⎢⎢⎢⎣

vpey cosα− vpex sinα

vpey sinα + v

pex cosα

vpez

⎤⎥⎥⎥⎦. (48)

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

5.13. Local Gravity g Calculation. The local gravity g iscalculated as [5]

g = 9.7803 + 0.051799C233 − 0.94114× 10−6h

(m/s2), (49)

where C33 = sinϕ, ϕ is the latitude and h is the altitude abovesea level.

Before we carry out the implementation of the above de-scribed mathematical model of SINS, we have to know theinitial parameters of the system, which will be described inthe following Section.

6. Initial Parameters and InitialData Calculation

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

6.1. Initial Parameters

(1) Initial position, latitude ϕ0, longitude λ0, height h0.The values of these parameters should be the same asthe corresponding ones in Section 4.1.

(2) Initial wander azimuth angle α0. We could chooseα0 = 0 at the very beginning. The value should bethe same as the corresponding ones in Section 4.1.

(3) Initial velocity vE0, vN0, vU0.

(4) If barometric altimeter applied, initial external refer-ence height href0 can be supplied.


6.2. Initial Alignment Data

(1) Initial attitude matrix is determined by initial align-ment process C

pb0. C

pb0 = Cn

b0 when α0 = 0.

(2) Initial position matrix is determined by initial align-ment process C

pe0. C

pe0 = Cn

e0 when α0 = 0.

6.3. Initial Data Calculation.

(1) Initial attitude angles ϕ0, λ0, and α0 determination:The initial attitude angles ψG0, θ0, and γ0 can becalculated using (41) and (42). Because α0 = 0,heading angle ψ0 = ψG0.

(2) Initial quaternion calculation: From the diagonalelements in (31) and the quaternion constraintequation, we have that

q20 + q2

1 − q22 − q2

3 = T11,

q20 − q2

1 + q22 − q2

3 = T22,

q20 − q2

1 − q22 + q2

3 = T33,

q20 + q2

1 + q22 + q2

3 = 1,

(50)

The solution to (50)

∣∣q1∣∣ = 1

2

√1 + T11 − T22 − T33,

∣∣q2∣∣ = 1

2

√1− T11 + T22 − T33,

∣∣q3∣∣ = 1

2

√1− T11 − T22 + T33,

∣∣q0∣∣ =

√1− q2

1 − q22 − q2

3.

(51)

Assuming q0 to be positive, according to (31), we havethat

sign(q0) = sign(1),

sign(q1) = sign(T32 − T23),

sign(q2) = sign(T13 − T31),

sign(q3) = sign(T21 − T12).

(52)

(3) Initial position matrix Cpe0: Substituting initial posi-

tion, latitude ϕ0, longitude λ0 and initial wanderazimuth α0 = 0 into (43), we can obtain the initialposition matrix C

pe0.

(4) Initial Earth angular velocity ωpie0 and initial attitude

angular velocity ωbpb0 calculations: use (38) and (39).

(5) Initial position angular velocity ωpep0 calculation: use

(36) and (37).

(6) Initial gravity g0 calculation: use (49) and elementC33

in Cpe0.

(7) Initial ground velocity vg0 calculation: use (34).

At this point, the whole SINS model, including sensordata generator and initial parameters, is fully described. Thefollowing Section will provide a Matlab implementation ofthe SINS theory.

7. Matlab Implementation andSimulation Examples

First, the Matlab program structure and the main codes aregiven. The Matlab implementation is illustrated using six ex-amples: static, straight, circle, s-shape, and the other twofrom real SINS experimental data.

7.1. Matlab Implementation and Codes. The program struc-ture is given in Figure 8. The program starts from “Begin”and ends at “Stop.” The gyro and accelerometer data areobtained either from a sensor data generator described inSection 4 or from the real SINS experiment logged files. Pro-cessing the sensor data with the initial parameters, using themethod described in Section 5, we get the attitude, velocityand position values of the system at specific times. After alldata are processed, the program will stop and the results willbe provided.

The main Matlab codes are presented next.

(1) Initial settings:

(a) initSettings.m contains initial parameters andconstants used in the simulation project.

(2) Trajectory part:

(a) initialCalculation static.m gives the initial cal-culation for the static situation;

(b) trajectorySimulater static.m simulates gyro andaccelerometer data for the static situation;

(c) initialCalculation straight.m gives the initialcalculation for the straight line situation;

(d) trajectorySimulater straight.m simulates gyroand accelerometer data for the straight linesituation;

(e) initialCalculation cirlce.m gives the initial cal-culation for the circle situation;

(f) trajectorySimulater circle.m simulates gyro andaccelerometer data for the circle situation;

(g) initialCalculation Sshape.m gives the initial cal-culation for the s-shape situation;

(h) trajectorySimulater Sshape.m simulates gyroand accelerometer data for the s-shape situa-tion;

(3) Simulation part:

(a) INSmain.m is the main program; the simulationstarts from here;

(b) AltitudeParamete.m calculates the four dampingparameters to damp the altitude error accordingto the input parameters k4 and τ, to be usedwith the external reference altitude;


0 1000 2000 3000 40000

2

4

6

Time (s)

Lon

gitu

deer

ror

(min

ute

)

(a)

−3

−2

−1

0

0 1000 2000 3000 4000

Time (s)

1

Lati

tude

erro

r(m

inu

te)

(b)

0 1000 2000 3000 4000

Time (s)

0

1

2

3

4

Dis

tan

ceer

ror

(nm

)

(c)

−2

0

0 1000 2000 3000 4000

Time (s)

Gro

un

dve

loci

tyer

ror

(m/s

)

2

4

(d)

Figure 20: Position and velocity error of s-shape simulation.

(c) InitializePosition.m gives the initial positioninitLong, initLat, initAlt, the external referencealtitude extAlt, and the wander azimuth anglewanderAzimuth; it calculates the initial positionmatrix and then orthogonalizes the matrix;

(d) InitializeAttitude.m gives the initial alignmenterror and calculates the attitude matrix (strap-down matrix);

(e) InitializeQuaternion.m calculates the quater-nion according to the input attitude matrix;

(f) ComputeAngularVelocity.m calculates the posi-tion angular velocity, earth angular velocity, andposition angle increment in the p-frame andresets the gyroscopes and accelerometers;

(g) ComputeQuaternionRungeKutta.m computesthe quaternion using Runge-Kutta method [6];see Appendices B and C;

(h) ComputeAttitudeMatrix.m computes the atti-tude matrix and transfers the raw data of theaccelerometers to the p-frame;

(i) ComputeVelocity.m computes the velocity, in thewander azimuth frame (p-frame) and ENU-frame, the ground velocity and altitude;

(j) ComputePositionMatrix.m computes the posi-tion matrix.

(k) ComputePosition.m computes latitude, longi-tude and wanderAzimuth;

(l) ComputeAttitudeAngle.m computes the attitudeangle of pitchAngle, tiltAngle, gridAzimuth andcourseAngle;

(m) OrthogonalizeMatrix.m computes matrix or-thogonalization; see Appendix D;

(n) QuaCofMatrix.m is called by ComputeQuater-nionRungeKutta.m;

(o) PlotResult.m plots the results of the simulationproject.


0 5 10×104

111.5

112

112.5

113

113.5

114

Time (s)

Tru

e lo

ngi

tude

(de

g)

(a)

0 5 10×104

Time (s)

27

27.5

28

28.5

29

29.5

Tru

e la

titu

de (

deg)

(b)

27

27.5

28

28.5

29

29.5

110 112 114

Tru

e la

titu

de (

deg)


(c)

Figure 21: The trajectory of real data set A.

0 5 10×104

Time (s)

−60

−50

−40

−30

−20

−10

0

10

20

30

40

cou

rseA

ngl

e er

ror

(min

ute

)

(a)

0 5 10×104

Time (s)

−3

−2

−1

0

1

2

3

pitc

hA

ngl

e er

ror

(min

ute

)

(b)

0 5 10×104

Time (s)

−3

−2

−1

0

1

2

3

tilt

An

gle

erro

r (m

inu

te)

(c)

Figure 22: Angle error of real data set A.


0 5 10×104

Time (s)

−100

−50

0

50

Lon

gitu

deer

ror

(min

ute

)

(a)

0 5 10×104

Time (s)

−50

0

50

100

Lati

tude

erro

r(m

inu

te)

(b)

0 5 10×104

Time (s)

Dis

tan

ceer

ror

(nm

)

0

20

40

60

80

100

(c)

0 5 10×104

Time (s)

Gro

un

dve

loci

tyer

ror

(m/s

)

0

5

10

15

20

(d)

Figure 23: Position and velocity error of real data set A.

7.2. Simulation Examples. In this subsection, there are 6SINS simulation examples. Example 1 is the static situationsimulation, where the vehicle trajectory in the n-frameis a fixed point. Example 2 is the straight line situationsimulation, where the vehicle trajectory in the n-frameis a straight line. Example 3 is the circle situation sim-ulation, where the vehicle trajectory in the n-frame is acircle. Example 4 is the s-shape situation simulation, wherethe vehicle trajectory in the n-frame is an s-shape line.Here, high-accuracy SINS simulation is applied to the foursituations. The initial latitude and longitude errors areset to be 1 minute. The simulation time is set to 3600seconds. The initial positions are dependent on the designedtrajectories.

In order to verity the validity of the Matlab codes further,two sets of real static data are used, and we refer to these asExamples 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 atdifferent times. The 2 data sets are 24 hours long.

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

Example 1 (Static situation simulation). The static situationis the most basic and simple situation where the output of thegyro is the Earth rotating angular velocity and the output ofthe accelerometer is the gravity. Figure 9 shows the designedtrue trajectory. Figure 10 shows the difference between thecalculated angle and the true angle. Figure 11 shows thedifferences between the calculated PV (position and velocity)and the true PV. The maximum value of the distance error in1 hour is 3.5 nm (nautical mile).

Example 2 (Straight line situation simulation). The straightline situation corresponds to a vehicle moving along thenorthwest direction. Figure 12 shows the designed true tra-jectory. Figure 13 shows the difference between the calculatedangle and the true angle. Figure 14 shows the differencesbetween the calculated PV and the true PV. The maximumvalue of the distance error in 1 hour is 3.7 nm.

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


0 5 10×104

111.5

112

112.5

113

113.5

114

Time (s)

Tru

e lo

ngi

tude

(de

g)

(a)

0 5 10×104

Time (s)

27

27.5

28

28.5

29

29.5

Tru

e la

titu

de (

deg)

(b)

27

27.5

28

28.5

29

29.5

110 112 114

Tru

e la

titu

de (

deg)


(c)

Figure 24: The trajectory of real data set B.

0 5 10×104

Time (s)

−50

−40

−30

−20

−10

0

10

20

30

40

cou

rseA

ngl

e er

ror

(min

ute

)

50

(a)

0 5 10×104

Time (s)

−3

−2

−1

0

1

2

3

pitc

hA

ngl

e er

ror

(min

ute

)

(b)

0 5 10×104

Time (s)

−2

−1.5

−1

0

0.5

1

1.5

tilt

An

gle

erro

r (m

inu

te)

2

(c)

Figure 25: Angle error of real data set B.


0 5 10×104

Time (s)

−60

−40

0

20

Lon

gitu

deer

ror

(min

ute

)

−20

(a)

0 5 10×104

Time (s)

−50

0

50

100

Lati

tude

erro

r(m

inu

te)

(b)

0 5 10×104

Time (s)

Dis

tan

ceer

ror

(nm

)

0

20

40

60

(c)

0 5 10×104

Time (s)

Gro

un

dve

loci

tyer

ror

(m/s

)

0

5

10

15

(d)

Figure 26: Position and velocity error of real data set B.

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

Example 5 (Real static data set A simulation). First, we proc-ess data set A [7]. Figure 21 shows the trajectory for thereal data set A; from the figure we can conclude that thesystem is static. In Figure 22, the red line corresponds to thethree attitude angle errors of the real system, while the blueline corresponds to the three attitude angle errors processedby the Matlab code. We can also show that the differencebetween the red and blue lines is negligible. In Figure 23, thered line corresponds to the position and velocity errors of thereal system, while the blue line corresponds to the positionand velocity errors processed by the Matlab code. We canalso see that the difference between the red and blue linesis negligible and this validates the correctness of the Matlabcode. The error described by the red lines (output from thereal system) is slightly smaller than that described by the bluelines (simulation). This is due to the fact that the real system

is processed in a much higher rate and thus its input is moreaccurate than the simulated system.

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

8. Conclusions

In this paper, a mathematical model for the strapdowninertial navigation system (SINS) is built and its Matlabimplementation is developed. First, a number of Cartesian


coordinate reference frames that relate to SINS are intro-duced, the basic principle of SINS in the wander azimuthnavigation frame (p-frame) is explained, and the mainequations are described. Second, the important attitudedirection cosine matrix and position direction cosine matrixin the p-frame are defined in detail. Third, the mathematicalmodel for SINS simulation is described in detail. Fourth, atrajectory simulator model is set up to generate data fromthree orthogonal gyros and three orthogonal accelerometers.The initial parameters and initial data calculations forthe mathematical model are also carried out. Finally, aMatlab implementation of SINS is developed. The proposedsimulation method is illustrated with four examples, static,straight line, circle, and s-shape trajectories; details are givenunder the condition that the pitch angle, roll angle, andaltitude are constant during the simulation process. Further,two sets of real experimental data are processed to verify thevalidity of the Matlab code.

Appendices

A. Symmetric Matrix Basic Operation

For a vector ω = [ωx ωy ωz]T , its skew symmetric matrix Ω

is

Ω =

⎡⎢⎢⎢⎣

0 −ωz ωy

ωz 0 −ωx−ωy ωx 0

⎤⎥⎥⎥⎦. (A.1)

We can easily show that

ΩT = −Ω. (A.2)

B. Fourth-Order Runge-Kutta Method

For numerical analysis, the fourth-order Runge-Kuttamethod is an important iterative method for the approxima-tion of solutions of ordinary differential equations. Here, inthis paper, the fourth-order Runge-Kutta method is adoptedto update the quaternion.

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

(1) Calculate slope k1, the slope at the beginning of theinterval, to determine the value of yi+1/2 at the pointti+1/2 using the Euler method:

k1 = f(ωi, yi, ti

),

yi+1/2 = yi +τ

2k1,

(B.1)

where τ is the time step between time ti and time ti+1,τ = ti+1 − ti.

(2) Calculate slope k2, the slope at the midpoint of theinterval, to determine the value of y′i+1/2 at the pointti+1/2 using Euler’s method:

k2 = f(ωi+1/2, yi+1/2, ti+1/2

),

y′i+1/2 = yi +τ

2k2.

(B.2)

(3) Calculate slope k3, again the slope at the midpoint, todetermine the yi+1 value:

k3 = f(ωi+1/2, y′i+1/2, ti+1/2

),

yi+1 = yi + τk3.(B.3)

(4) Calculate slope k4, the slope at the end of the interval,with its yi+1 value determined using k3:

k4 = f(ωi+1, yi+1, ti+1

). (B.4)

(5) Average the four slopes; greater weights are given tothe slopes at the midpoint:

k = 16

(k1 + 2k2 + 2k3 + k4). (B.5)

(6) Finally, using the average slope k, the value of yi+1 is

yi+1 = yi + τk. (B.6)

C. Angular Velocity Extraction

From Appendix B and (29), we need to provide the attitudeangular velocity ωbpb in a period of τ/2 to update the

quaternion. By inspecting the expression of ωbpb in (39), we

know that the variations of ωpep and ω

pie are slow, while ωbib

changes quickly. So, only ωbib needs to be given in a period ofτ/2. We know that ωbib (we next use ω to simplify notation)is the output of gyro which gives data in the form of angleincrement Δθi during the time interval τ. For first-orderangular velocity extraction, we have that

ω = Δθiτ. (C.1)

In order to provide ω(ti)ω(ti+1/2) and ω(ti+1), we need to dosecond-order angular velocity extraction:

ω(ti) = 3Δθi1 − Δθi2τ

,

ω(ti+1/2) = Δθi1 + Δθi2τ

,

ω(ti+1) = −Δθi1 + 3Δθi2τ

,

(C.2)

where Δθi1 is the angle increment from time ti to ti+1/2 andΔθi2 is the angle increment from time ti+1/2 to time ti+1.


D. Matrix Orthogonalization Method

For the direction cosine matrix C, the optimal orthogonaliza-tion method is to get C which makes the following Euclidianfunction have the minimum value [8]:

D =⎡⎣

3∑

i=1

3∑

j=1

(Cij − Ci j

)⎤⎦.1/2

(D.1)

The expression for C is thus

C = ±C(

CTC)−3/2

, (D.2)

where the superscript T means the transpose operator. It isdifficult to solve the above equation directly. Instead, we usean iterative method. Assume C0 to be initial matrix, and Cn

to be the matrix obtained after n iterations. The iterationprocess is as follows:

C0 = C,...

Cn+1 = Cn − 12

(CnCTCn − C

).

(D.3)

If at the n + 1 step, the following function:

fn =3∑

i=1

3∑

j=1

(Cij − Cijn

)2(D.4)

satisfies fn+1 − fn ≤ ε (e.g., ε = 10−10), then the iterationprocedure can be stopped and Cn+1 is taken to be the finalresult.

Abbreviations and Symbols

SINS: Strapdown inertial navigation systemDCM: Direction cosine matrixO: Center of the EarthP: Center of the vehiclex, y, z: 3 orthogonal axes or the 3

components of a Cartesiancoordinate

b: Body framei: Inertial framee: Earth framen: Navigation frameENU : East-North-UP navigation frame,

which is identical to the n-frame inthis paper

p: Wander azimuth frame

AT : Transpose of matrix Avpe : Velocity vector measured in p-frame

with respect to e-frameCpb : Vehicle attitude DCM used to

transform the measured anglein b-frame to p-frame, with its 9components Tij , i, j = 1, 2, 3

Cbp: Transpose of C

pb is used to transform

the measured vector in p-frame tob-frame

Cpe : Vehicle position DCM used to

transform the measured vectorin e-frame to p-frame, with its 9components Cij , i, j = 1, 2, 3

Cep: Transpose of C

pe is used to transform

the measured vector in p-frame toe-frame

Cnb : Vehicle attitude DCM used to

transform the measured anglein b-frame to n-frame

f p: Specific force vector measured inp-frame

fn: Specific force vector measured inn-frame

fb: Specific force vector measured inb-frame; the output of the 3accelerometers

ωie: Constant value of the turn rate of theEarth, ωie = 7.2921151467 ×10−5 rad/s

ωnie: Turn rate of the Earth measured in

n-frameωbib: Turn rate of the b-frame with respect

to i-frame, which is measured inb-frame; the output of the 3 gyros

ωnen: Transport rate of the n-frame with

respect to e-frame, which ismeasured in n-frame

ωeie: Turn rate of the e-frame with respect

to i-frame, which is measured ine-frame

ωpep: Turn rate of the p-frame with respect

to e-frame, which is measured inp-frame

ωepe: Turn rate of the e-frame with respect

to p-frame, which is measured ine-frame

ωbpb: Turn rate of the b-frame with respect

to p-frame, which is measured inb-frame

ωbnb: Turn rate of the b-frame with respect

to n-frame, which is measured inb-frame

Ωbpb: Skew matrix form of ωb

pb

Ωpep: Skew matrix form of ω

pep

gp: Gravity vector measured in p-frameg: Local gravity scalarg0: Local gravity scalar at sea level


ψG: Grid azimuth angle of the vehicle inb-frame with respect to p-frame

α: Wander azimuth angle of p-framewith respect to n-frame

ψ: Heading angle of the vehicle inb-frame with respect to n-frame

θ: Grid pitch angle of the vehicle inb-frame with respect to n-frameor p-frame

γ: Grid roll angle of the vehicle inb-frame with respect to n-frameor p-frame

Δψ: Increase of the heading angle ψΔθ: Increase of the grid pitch angle θΔγ: Increase of the grid roll angle γλ: Longitude of the vehicleϕ,L: Latitude of the vehicleh: Altitude of the vehicle above the sea

level of the Earthϕ0, λ0, h0: Initial vehicle position (latitude,

longitude, height)Δt: Time stepa: Vehicle accelerationv0 = [vE0, vN0, vU0]: Initial vehicle velocity (east, north,

up)v = [vE, vN , vU] : Vehicle velocity (east, north, up)vg : Vehicle ground velocityrne : Vehicle position measured in n-frame

with respect to e-framee: Major eccentricity of the ellipsoid of

the EarthRe: Length of the semi-major axis of the

EarthRN : Meridian radius of curvature of the

EarthRE: Transverse radius of curvature of the

EarthRxp,Ryp: Free curvature radiuses1/τa: Turn torsionQ: Quaternionq1, q2, q3, q4: Four components of the quaternion

QTcircle: Period of the circle trajectory in

simulationTsshape: Period of the s-shape trajectory in

simulationAsshape: Amplitude of the s-shape trajectory

in simulationPV: Position and velocity.

References

[1] H. Schneider and N. E. George Philip Barker, Matrices andLinear 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 NavigationTechnology, Institution of Engineering and Technology, Steve-nage, UK, 2004.

[4] Z. Chen, Strapdown Inertial Navigation System Principles, ChinaAstronautic Publishing House, Beijng, China, 1986.

[5] P. S. Maybeck, “Wander azimuth implimentation algorithmfor a strapdown inertial system,” Air Force Flight DynamicsLaboratory AFFDL-TR-73-80, Tech. Rep., Ohio, USA, 1973.

[6] J. C. Butcher, Numerical Methods for Ordinary DifferencialEquations, John Wiley & Sons, New York, NY, USA, 2003.

[7] B. Yuan, Research on Rotating Inertial Navigation System withFour-Frequency Differential Laser Gyroscope, Graduate School ofNational University of Defense Technology, Changsha, China,2007.

[8] I. Y. Bar-Itzhack, “Iterative optimal orthogonalization of thestrapdowm matrix,” IEEE Transactions on Aerospace and Elec-tronic Systems, vol. 11, no. 1, pp. 30–37, 1975.

International Journal of

AerospaceEngineeringHindawi Publishing Corporationhttp://www.hindawi.com Volume 2010

RoboticsJournal of

Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014


Active and Passive Electronic Components

Control Scienceand Engineering

Journal of



RotatingMachinery


Hindawi Publishing Corporation http://www.hindawi.com

Journal ofEngineeringVolume 2014

Submit your manuscripts athttp://www.hindawi.com

VLSI Design



Shock and Vibration


Civil EngineeringAdvances in

Acoustics and VibrationAdvances in



Electrical and Computer Engineering

Journal of

Advances inOptoElectronics

Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014

The Scientific World JournalHindawi Publishing Corporation http://www.hindawi.com Volume 2014

SensorsJournal of


Modelling & Simulation in EngineeringHindawi Publishing Corporation http://www.hindawi.com Volume 2014


Chemical EngineeringInternational Journal of Antennas and

Propagation




Navigation and Observation



DistributedSensor Networks


Mathematical Model and Matlab Simulation of Strapdown Inertial ...

Documents

Transcript of Mathematical Model and Matlab Simulation of Strapdown Inertial ...