22 Scientific-Technical Review,Vol.LV,No.2,2005

UDK: 681.883.48:519.62 COSATI: 17-07

Comparison of Different Computation Methods for Strapdown Inertial Navigation Systems

Mohamed S. Ahmed, MSc (Eng)1) Danilo V. uk, PhD (Eng)1)

A series of numerical experiments were conducted to test three different methods for solving the SDINS navigation equations: Runge-Kutta method, Runge-Kutta method with sampling process and three speed navigation algorithms. A stochastic numerical simulator, which solves the navigation equations in navigation frame by using the given func-tions of Eulers angles and velocity components as inputs, is proposed to simulate the IMU sensors output. The gen-erated angular body rates and specific force were inputs to the three integration methods. For the case when the sen-sors outputs are contaminated by white noise the obtained results shows that the ratio of the absolute error by naviga-tion algorithm to the absolute error obtained by Runge-Kutta sampling method is reduced compared to the same quantity without the noise.

Key words: navigation system, inertial navigation, numerical methods, Runge-Kutta method.

1) Faculty of Mechanical Engineering, Kraljice Marije 16, 11120 Belgrade

Nomenclature

Convention 21

AAC direction cosine matrix which transforms

vector from 1A to 2A frame 1Ab vector b with components in 1A frame

21 2

AA A angular rate of 2A frame relative to 1A frame

expressed with components in 2A frame

Axis systems (reference frames), angles and transformation operators i inertial reference frame e Earth-fixed reference frame n navigation reference frame b body reference frame

i i iO x y z inertial axis system

e e eO x y z Earth-fixed axis system

n n n nO x y z navigation axis system

b b bO x y z body axis system h altitude latitude longitude roll angle pitch angle yaw angle

nbC direction cosine matrix transforming quanti-

ties from b frame to n frame

bnC direction cosine matrix transforming

quantities from n frame to b frame where T( )b nn b=C C

Earth quantities ng nominal gravitational acceleration ( )45 = D0R mean radius of Earth, 0 6356766mR = ie Earth rate with respect to i frame, 57.292116x10 rad/ie s=

lg local gravity column matrix

Dynamic quantities ,t T time, sampling time

eV kinematic velocity (velocity of the vehicle relative to the Earth)

, ,N E DV V V the north, east and down components of kinematic velocity in n frame

neV kinematic velocity expressed in n frame bnb angular rate of b frame relative to n frame

expressed in b frame nen angular rate of n frame relative to e frame expressed in n frame

bib angular rate of b frame relative to i frame

expressed in b frame , , x y zf f f components of body specific force expressed

in b frame bf specific force in b frame nf specific force expressed in n frame

M.S.AHMED, D.UK: COMPARISON OF DIFFERENT COMPUTATION METHODS FOR STRAPDOWN INERTIAL NAVIGATION SYSTEMS 23

Definition of important matrices ( x)A skew symmetric matrix with components

of in A frame

( x) ( )A A= 0 -

0 -- 0

A A

A A

A A

z y

z x

y x

=

Subscripts l, m and n indexes for high speed computer cycle (l-

cycle), moderate speed computer cycle (m-cycle) and low speed computer cycle(n-cycle) respectively

N, E and D North, East and Down components (nframe components)

Abbreviations IMU inertial measurement unit INS inertial navigation system SDINS strapdown inertial navigation system DCM direction cosine matrix RK Runge-Kutta method RKS Runge-Kutta method with sampling

process NA navigation algorithm

Introduction The original applications of inertial navigation technol-

ogy used stable platform techniques. In such navigation technology the inertial sensors are mounted on stabilized platform and are mechanically isolated from the rotational motion of the vehicle. Platform systems are still in common use particularly for those applications requiring very accu-rate estimates of navigation data such as ships and subma-rine. This type of inertial navigation system is referred to as Space Stabilized Inertial Navigation System (SSINS). Most of the mechanical complexity of the platform systems has been removed by having the sensors attached rigidly or strapped down to the body of the host vehicle. This type of inertial navigation system is referred to as StrapDown Iner-tial Navigation System (SDINS).

The basic strapdown inertial navigation concept was originally formulated in the 1950s. The main benefits of this type are lower cost, size reduction and greater reliabil-ity compared with equivalent platform systems. As a result a small, light weight and accurate inertial navigation sys-tems can now be fitted to a small vehicle. The major draw-backs are a substantial increase in computing complexity and the need to use sensors capable of measuring much higher rates of turn. However, recent advance in computer technology combined with the development of suitable sen-sors have allowed such a design to become reality.

The general structure of the inertial navigation system is shown in Fig.1 along with other computational procedures implemented onto on board computer of a flying vehicle. Fig.1 shows that the measured vehicle angular rates bib and specific forces bf taken by the gyroscopes and acceler-ometers respectively are passed through filtering algorithms to provide the estimated values of these measurements which might be corrupted by noise.

The filtered angular rate measurements obtained from filtering algorithms are input to both the attitude algorithms and the integrated control and guidance law. The attitude algorithms compute the transformation matrix (Direction Cosine Matrix or Quaternion) that will transform the meas-

ured specific forces (non-gravitational acceleration) into the desired navigation frame.

The velocity and position are computed by the trans-formed specific forces measurements using the computed transformation matrix obtained from attitude algorithms. These computed values of velocity and position are also in-put to the control and guidance law. The proper commands are sent to the vehicle in order to correct its course accord-ing to the comparison of the computed values of the veloc-ity position provided by the INS system and the nominal course. The vehicle response to the control commands is sensed by the INS sensors (gyroscopes and accelerometers), the measured values are passed again through the filter al-gorithms and new cycle of computations starts.

The problem of computing the translational velocity and position relative to the Earth, which has to be solved in INS computer, was addressed by Itzhack in 1977 [1]. Various computational schemes were considered and the split-coordinate computational scheme was selected in which the differential equations are solved in three different computa-tional rates. A computer simulation was carried out on a coning motion example (the same example adopted in this paper). The integration routine utilized a fixed Runge-Kutta procedure with a time step of 0.001s. The obtained results showed a good accuracy.

Figure 1. Strapdown inertial navigation system

Dorobantu in 1999 [2] presented some labor simulation results for a low cost SDINS system with zero Earth rota-tion and constant gravity, due to poor INS sensors and small area of trajectory, using the graphical programming language Simulink. The principles of the implementation of SDINS algorithm and the influence of integration time step and method were tested. The attitude was updated by solv-ing Eulers angles differential equation and then a DCM was constructed using these angles. The sampling fre-quency up to 50Hz and the 4th order Runge-Kutta integra-tion method were recommended.

Savage in 1998 [3], [4] developed three speed naviga-tion algorithms (NA) for the computation of attitude pa-rameters, velocity and position.

Waldmann in 2002 [5] presented a novel derivation of a discrete-time version of the relative quaternion differential equation. The proposed quaternion algorithms were simu-lated assuming perfect sensors and showed robustness to different trajectories. The performance degraded after rais-ing the cone angle.

24 M.S.AHMED, D.UK: COMPARISON OF DIFFERENT COMPUTATION METHODS FOR STRAPDOWN INERTIAL NAVIGATION SYSTEMS

Apart from, references [3] and [4] probably there are no other published materials dealing with the design of SDINS complete navigation algorithms, though these are without numerical analysis in the presence of random inputs. The general flow chart and computer program for the proposed three speed algorithms were given in [6].

The aim of this paper is to test the accuracy and per-formances of this numerical algorithms (NA) and compare it with other methods of solving navigation equations (Ap-pendix A) such as the classical fourth order Runge-Kutta method (RK) and the Runge-Kutta method with sampling process (RKS). The numerical analysis will be given for different types of motion without noise and in the presence of the white noise.

Generation of specific force and angular rate The integrated specific force and angular rate are used as

inputs to the navigation algorithms. The numerical simula-tor used for generating these input data was developed by solving the navigation equation expressed in n frame given in [7] (Titterton, page 51). This equation can be rewritten for the specific force expressed n frame as

( ) 2 ( )n n n n n ne en ie e l= + + f V V g (1) where the kinematic velocity is

N

ne E

D

VVV

= V (2)

To solve Eq.(1) the velocity components and Eulers an-gles of a vehicle should be available as functions of time and their first derivatives should be computed. The velocity components and Eulers angles and their derivatives are used as input for the simulator. Also, the angular rate vec-tors ( nen and )nie and the gravity vector nlg should be computed:

cos

sin

nen

=

(3)

cos0sin

ienie

ie

= (4)

Substituting Eqs.(3) and (4) into Eq.(1) the specific force vector can be obtained. he position in terms of latitude, longitude and altitude can be calculated as a function of the velocity vector components from [7] (Titterton, page 53):

Dh V=

0

1 ( ) cos EVR h = + (5)

0

1( ) NVR h = +

The specific force vector expressed in n frame com-puted by Eq.(1) should be transformed to b frame using the direction cosine transformation matrix in terms of Eulers angles given in [7] (page 49). This transformation is written as

b b nn=f C f (6) Mathematically the absolute angular velocity is deter-

mined by summing the angular velocity of the b frame relative to n frame nb and the angular velocity of n frame relative to i frame in : ib in nb= + (7)

The body angular velocity relative to the n frame nb (relative angular velocity) with components in b frame is given by

r = b b b nnb ib n in= C (8)

n n nin ie en= + (9) where

bi b is the angular velocity column vector of b frame

relative to i frame expressed in b frame; ni n is the angular velocity column vector of n frame re-

lative to i frame expressed in n frame and it is given in by (9), (3) and (4);

bnb is the angular velocity column vector of b frame

relative to n frame expressed in b frame; bnC is the transformation matrix related b frame relative to n frame and it is given in [7] (page 49). The relative angular velocity of the b frame to the n

frame is related to the angular rates of Eulers angles as fol-lows [7]:

r = x

y

z

=

sincos cos sincos cos sin

+

(10)

The angular velocity column vector bib (measured an-gular rates) can be found form (8).

Figure 2. Flow chart of the numerical simulator

M.S.AHMED, D.UK: COMPARISON OF DIFFERENT COMPUTATION METHODS FOR STRAPDOWN INERTIAL NAVIGATION SYSTEMS 25

The flow chart of the generator (simulator) of the spe-cific force and the angular velocity is shown in Fig.2. The numerical simulator for the generation of the angular veloc-ity and specific force is included into the computer program [6] for the computation of the kinematic parameters accord-ing to the chosen method of integration navigation equa-tions. Depending on the applied numerical method (RK, RKS, NA), it is possible to generate instantaneous values of the specific force and the angular velocity of the vehicle or their integrated increments at the desired time during high frequency cycle.

Simulation examples In order to evaluate the performance of the proposed

navigation digital algorithms a computer code was written using the developed algorithms shown in [3], [4] and [6]. series of numerical experiments were conducted for two examples.

Ballistic trajectory (Example 1) This example represents a simple motion where the ob-

ject has a pure ballistic trajectory without any disturbances and with the gravitational acceleration which is equal to g=9.81 m/s2.

Velocity components The velocity components in North, East and Down

directions are given by the following expression

( )

0 0

0 0

cos0

sin

N

E

D

V V VV V gt

= = V (11)

The first derivative of the velocity is

00

N

E

D

VV

gV

= = V

(12)

The calculated numerical value of the initial velocity for ~150 km is 1200 m/s. The initial latitude, longitude and al-titude are: 0 0.5 = radian, 0 0 = and 0 0h = m. Eulers angles

The values of the yaw and roll angles and their deriva-tives are set to equal zero

00

= == = (13)

The pitch angle expression and its derivative can be obtained from (11).

0 00 0

sintan cosV gt V

= (14)

The numerical value for the launch angle 0 =45o was selected to have maximum down range.

The derivative of the pitch angle is obtained as

20 0

coscosg V = (15)

In the case of a pure ballistic trajectory gravitational ac-

celeration is constant (for o45 = , g = 9.81 m/s2) and the specific force equal zero. The angular rate is produced only in pitch plane due to the gravity. It is important to remark that for solving navigation equations the gravitational ac-celeration is variable and it depends on the altitude. Also there is the effect of Coriolis acceleration due to Earth rota-tion. So, in order to produce a pure ballistic trajectory in the navigation frame under these conditions it is necessary to generate specific force and angular rate to compensate vari-able gravitational and the Coriolis acceleration. The body angular rates components for this example are shown in Fig.3. The specific force profiles are shown in Fig.4.

Figure 3. Body angular body rates profiles for Example 1

Conning motion (Example 2) This example demonstrates the effect of the vehicle

coning motion of low amplitude. The dynamics of the vehi-cle were described by a velocity whose components were

300 100300 100(300 100 )

N

E

D

V tV tV t

+ = = + + V (16)

and by three Eulers angles which described the rotation of the vehicle with respect to n frame.

26 M.S.AHMED, D.UK: COMPARISON OF DIFFERENT COMPUTATION METHODS FOR STRAPDOWN INERTIAL NAVIGATION SYSTEMS

Figure 4. Specific force profiles for Example 1

The three angles were

( ) ( )( ) ( )( ) ( )

2 1 2sin sin300 2 1.7

2 1 2sin sin300 2 0.85

2 1 2sin sin 0.3300 2 1.7

t t

t t

t t

= +

= +

= + +

(17)

Numerical analysis The navigation equations (Appendix A) were solved by

three methods. The first one is solving these equations us-ing the classical fourth order Runge-Kutta integration method (RK) and continuous form solution for the specific force and angular velocity generated by numerical simula-tor. This method with integration time step of 0.001s was used for two purposes. The first is to check the accuracy of the generated specific forces and body angular rates ob-tained by simulator. This can be done by comparing the output results of velocity components and Eulers angles obtained by this method with input values of these parame-ters for the numerical simulator. The second purpose is to obtain values of the velocity components and Euler angles with maximum accuracy.

The second numerical method is solving navigation equations using Runge-Kutta method with sampling proc-ess (RKS) where the specific force and angular rate are constant during the interval of sampling, Tl=0.001.

Finally, these equations were solved by the navigation algorithms (NA), presented in [3], [4] and [6]. ]. The sam-pling rate chosen for these experiments is 1KHz, viz. the

data with high speed algorithm (l-cycle) are sampled at a period of Tl = 0.001s. The attitude and velocity updating are performed at moderate speed algorithm (m-cycle) slower than the l-cycle by 10 times (100Hz with updating period of Tm = 0.01 s). The position is updated at slower rate than the moderate cycle by 5 times that means the position algo-rithms (n-cycle) has an updating rate of 20Hz with updating period of Tn = 0.05s.

The generated specific forces and angular body rates were also corrupted by a white noise in order to test the per-formances of the numerical methods in noisy environment. The standard deviations of the white noise for the body an-gular rates and specific force were chosen for these experi-ments as: for body angular rates = 1o/hr and for specific forces f = 1mg.

The results of the maximum absolute errors of some ki-nematic parameters obtained by different numerical meth-ods are shown in Table 1.

The high accuracy of the computation of kinematic pa-rameters was achieved for both ballistic trajectory (example 1) and conning motion (example 2) by Runge-Kutta method with continuous form solution for the specific force and an-gular velocity. The sampling process in the Runge-Kutta method increases maximum absolute errors of order be-tween 10-11 and 10-7 to the errors of order between 10-10 to 10-6 (Table 1).

The results of the three speed navigation algorithms (NA) are good from the practical point of view, especially for the trajectory which is near to the pure ballistic type. The coning effect is better computed by using RK-sampling method. The ratio of the error in computed angle is of the order of 106 between NA and RKS method when there is no noise. The corruption of the input data with noise increases the error dramatically compared to the error obtained using the same method in free noise environment. For example, the absolute error in the presence of the white noise is increased ~25 times comparing with the case of no noise added for the example with coning motion (example 2) by using NA method. The absolute error EV obtained in the presence of the white noise at the end of simulation time (t = 200s) is less than 0.172% of the exact value (20300 m/s) for RK-sampling method, and 0.344% of the exact value for the NA method. The error obtained by NA in the presence of noise is 2 times the error of RK-sampling method.

Table 1. Maximum absolute errors in computed values of some kinematic parameters

No noise With noise example parameter RK RKS NA RKS NA

(degree)

4.5x10-11 2.6x10-10 2x10-8 1.5x10-4 5x10-4

1 DV(m/s)

3x10-10 6x10-9 2x10-6 5.3x10-3 3x10-3

(degree)

2x10-9 9x10-8 7.1x10-2 0.88 1.75 2

EV(m/s)

2.2x10-7 1.75x10-6 0.91 35 70

It can be said that the NA and RK-sampling methods give the errors of the same order when the noise is intro-duced. The ratio of the absolute error in VD obtained by the RK-sampling method to the error obtained by NA method for this particular simulation is ~1.8 (Table 1).The error of

M.S.AHMED, D.UK: COMPARISON OF DIFFERENT COMPUTATION METHODS FOR STRAPDOWN INERTIAL NAVIGATION SYSTEMS 27

the elevation angle obtained by NA is about 3-times greater than the error produced by Runge-Kutta sampling method.

Conclusions A series of numerical experiments were conducted to test

three different methods for solving the SDINS navigation equations. These methods are: Runge-Kutta method with continuous form solution for the specific force and angular velocity, Runge-Kutta method with sampling process and the navigation algorithms, given in [3], [4] and [6]. Two different examples representing different types of motion were used to test these methods: the pure ballistic trajectory and the conning motion.

The measured quantities (angular body rates and specific force) were generated by the proposed numerical simulator which solves the navigation equations in navigation frame by using the given function of time of Eulers angles and velocity components as inputs.

The free white noise experiments showed that the RK method with 1 KHz computation frequency is very accurate but time consuming (for ballistic trajectory the error in an-gles is of 10-11 degree order and the error in the velocity components is of 10-10 m/s order).

In order to study the effect of the white noise and to compare the proposed navigation algorithm with the RK-sampling method the white noise was added to the simu-lated measured quantities and then the navigation equations were solved by both methods.

The results with white noise showed that the ratio between the absolute error by NA and the absolute error ob-tained by RK-sampling method is reduced compared to the same quantity without noise. his means that the proposed NA is effective for real time applications. The advantage of the RK-sampling method over the proposed NA in terms of the accuracy of solving navigation equations is dramatically reduced for the real environment application.

Appendix A: Navigation Equations In order to write navigation equations the following

frames were used: inertial (i), Earth fixed (e) and navigation (n), Fig.A.1.

Figure A.1. Inertial, Earth fixed and navigation reference frames

he navigation frame mechanization was chosen. For this type of mechanization the two frames of interest are the navigation frame (n) and the body frame (b). The rate of change of direction cosine matrix is

( ) ( )n n b n nb b ib in b= C C C (.1) where

bi b the angular rate of b frame (body angular rate) rela-

tive to i frame expressed in b frame, this quantity in fact measured by gyroscopes.

ni n the angular rate of n frame relative to i frame ex-

pressed in n frame. The general updating algorithm for direction cosine ma-

trix nbC was constructed by using direction cosine matrix product chain rule [3], [4]:

( )( )

( )( )

( )( )-1 -1 -1

-1ni n ni n bi mbi m bi m bi m=C C C (.2)

( )( )

( )( )

( )( )-1

-1ni n ni n ni nbi m ni n bi m=C C C (.3)

where

( )( )-1

1ni nbi m C DCM relating the b frame at time 1mt to the n

frame at time 1nt .

( )( )ni n

bi mC DCM relating the b frame at time mt to the nframe at time nt .

( )( )-1bi m

bi mC DCM that accounts for b frame rotation relative to inertial frame from its orientation at the time 1mt to its orientation at time mt .

( )( )

-1ni nni nC DCM that accounts for n frame rotation relative to inertial frame from its orientation at the time

1nt to its orientation at time nt . ( )i mb discrete orientation of the b frame in the non-

rotating i frame at computer update time mt . m computer cycle index for b frame angular motion

updates to nbC .

( )i nn discrete orientation of the n frame in the non-rotating i frame at computer update time nt .

n computer cycle index for n frame angular motion updates to nbC .

The orientations of both b frame and n frame relative to each other and to the non-rotating i frame is illustrated in Fig..2.

Figure .2. Relation between i, b and n frames orientations

he equations (A.2) and (.3) describe an algorithm that relates b frame and n frame orientations at separate times and provides, for both frames, inertial angular motion updates to

nbC at different update rates. This angular motion updates

are performed by ( )( )-1bi m

bi mC and ( )( )

-1ni nni nC terms in Eqs. (.2)

and (.3) for which algorithms are derived separately.

M.S.AHMED, D.UK: COMPARISON OF DIFFERENT COMPUTATION METHODS FOR STRAPDOWN INERTIAL NAVIGATION SYSTEMS 29

Sravnitelxnwj analiz razli~nwh ~islennwh metodov bezplo|adnwh inercialxnwh navigacionnwh sistem

Zdesx vwpolnen r}d ~islennwh &ksperimentov radi proverki razli~nwh metodov re{eni} navigacionnwh uravnenij bezplo|adnwh inercialxnwh navigacionnwh sistem: metod Runge-Kutta so vwborom diskretnwh dannwh izmer}emwh signalov i navigacionnwj algoritm so tri skorosti vw~isleni} kinemati~eskih veli~in. Zdesx pokazano stohasti~eskoe modeliruy|ee ustrojstvo, re{ay|ee vwhodnwe veli~inw ~uvstvitelxnogo &lementa inercialxnoj izmeritelxnoj edinicw primeneniem zadannwh funkcij ot vremeni dl} uglov Ojlera i dl} sostavl}y|ej skorosti. Re{ennwe uglovwe skorosti i udelxnwe silw polxzovanw v roli vhodnwh veli~in dl} vseh treh metodov integracii navigacionnwh uravnenij. Zdesx to`e pokazano, ~to otno{enie absolytnwh o{ibok kinemati~eskih veli~in, polu~enwh navigacionnwm algoritmom i metodom Runge-Kutta so vwborom diskretnwh dannwh izmer}emwh signalov, umenx{eno u sistem so "belwm" {umom v sravnenii s takoj `e veli~inoj idealxnoj sistemw (bez {uma).

Kly~evwe slova: mehanika poleta, navigaci}, navigacionna} sistema, inercialxnoe navedenie, opredelenie polo`eni}, ~islennwe mwtodw, metod Runge-Kutta.