Open access peer-reviewed chapter

Code Optimization for Strapdown Inertial Navigation System Algorithm

By Ivana Todić and Vladimir Kuzmanović

Submitted: May 26th 2017Reviewed: October 17th 2017Published: December 20th 2017

DOI: 10.5772/intechopen.71732

Abstract

Inertial navigation systems are in common use for decades due to its advantages. Since INS outputs are usually used for inputs in different control algorithms (depending on applications), INS will induce certain errors and limitations. This chapter deals with optimization of the inertial navigation algorithm against limitations due to the accuracy and stability of signals from the sensors and constraints resulting from the integration step and processor speed used for embedded applications. Inertial navigation considered here is “strapdown” inertial navigation system (SINS) which assumes a fixed inertial measurement unit (IMU). In this chapter, fundamentals of strapdown inertial navigation will be presented as well as three different algorithms which will be analyzed in regard to numerical stability, time consumption and processor load criteria.

Keywords

• quaternions
• forward Euler integration
• code optimization
• code analyses

1. Introduction

INS is inertial navigation system, the system that determines the position based on the output of the motion sensors: accelerometers and gyroscopes. The first INS was based on accelerometers mounted on gimbal platform, to ensure measurement of acceleration in navigational frame. Nowadays “strapdown” inertial navigation system (SINS) is in common use, due to its mechanical simplicity, reduced size and price compered to platform INS. Strapdown inertial navigation system implies a fixed inertial measurement unit (IMU), whereby the analytical picture of the navigation system is obtained from the integration of the gyroscope rates.

The main problem that arises when SINS is used is the exact determination of the orientation based on the gyroscopes outputs. Every error made in this stage will affect the error of projection of the gravitational acceleration. Accelerations are integrated twice in order to determine the position, so any errors made when determining the orientation will cause the error in position determination to increase exponentially with integration time.

Errors when determining orientation are caused by the gyroscope performance and precision, as well as signal processing methods used for processing gyroscope outputs. Besides hardware limitations of the gyroscopes, algorithms used for orientation calculation also cause errors. This chapter focuses only on errors caused by applied algorithms and on optimization of these algorithms in terms of time consumption and processor load.

The basic idea of inertial navigation is based on the integration of acceleration measured by the accelerometers; see . The accelerometers measure the specific force that can be represented as:

f=agE1

where a is the absolute acceleration, acceleration in relation to the inertial coordinate frame, g is the gravitational acceleration.

In this chapter, the effect of the rotation of the Earth (which can simply be introduced into equations for the needs of systems operating in a longer time interval) is neglected.

In accordance with the previous assumption, the following relationship between acceleration and velocity in relation to the inertial coordinate frame is:

a=dVdtIdVdtI=dVdtN+ωN×VE2

where dVdtNis the speed derivative relative to the navigation coordinate frame, ωN is the absolute angular velocity of the navigation coordinate frame.

In the inertial navigation algorithm, for the navigation coordinate frame, the ENUp coordinate frame has been adopted; see . This choice is made due to the desire to have the height coordinate positive and on the other hand in order to more accurately determine the azimuth numerically.

In accordance with the ENUp coordinate frame, the following relations apply:

fE=dVEdt+ωNVupωupVNfN=dVNdtωEVup+ωupVEfup=dVupdt+ωEVNωNVE+gE3

As a result of the WGS84 standard for the Earth shape (see ), projection of angular speeds of the ENUp coordinate frame has been adopted in the following form:

ωE=VNRϕ+hωN=VERλ+hωup=VERλ+htanϕE4

where h is the height above the reference ellipsoid, ,  is the radius of the curvature of the reference ellipsoid in the north-south and east-west directions, respectively.

Rϕ=Re1e21e2sin2ϕ32Rλ=Re1e2sin2ϕ12E5

where Re is the equatorial radius of the Earth, e2=1b2a2is the eccentricity of the reference ellipsoid.

As the accelerometers measure acceleration in the coordinate frame related to the object, it is necessary to determine the transformation matrix from the body frame (see ) into the navigation frame, using information from the gyroscopes.

The navigation algorithm adopted here can be divided into two parts. The first part that works with higher frequency plays the role of determining velocity and angle increments, while the other part of the algorithm that works eight times slower provides information on the position and the speed in the navigation coordinate frame (usually required by the guidance law in the case of the missile application). Such algorithm is advantageous from the point of optimization of the calculation time in the control computer, which can be divided into eight different steps. Also, this SINS algorithm proved to be mathematically more stable in relation to others, in determining the quaternion position at the same sampling time. Namely, when integrating angular velocities in order to obtain the angular position, depending on the size of the integration step, the quaternion error increases over time, and in addition to renormalization, it also affects the overall error in position and velocity. This error does not occur with this algorithm.

2.1. Determination of angular increments and transformation matrix

The first step in determining the transformation matrix is the determination of angular inclusions, and as explained above, this process is repeated with the basic integration step which in this example is ts = 2 ms:

αxb,yb,zb=tktk+tsωxb,yb,zbdtE6

where ωxb, yb, zb is the gyroscope signals in the body coordinate frame.

The process of calculating the position quaternion or the transformation matrix is also divided into two parts.

The first part is the calculation of the quaternion between the navigation coordinate frame and the body frame, assuming that the navigation coordinate frame can be considered inert during one step of integration.

The second part is used for the quaternion correction due to the rotation of the navigation coordinate frame.

If we compare these two transformations, we can conclude that the first transformation is the rotation of “fast” motion. One of the reasons why this algorithm proved to be numerically more stable is the separation of the integration of the “fast” rotation from the integration of the “slow” rotation.

If we compare the angular rates of those two motions, we can conclude that the “slow” rotation rates are four or more times lower than the “fast” rotation rates which leads to numerical integral errors when these two rotations are combined.

In accordance with the above, the following relations apply:

qn+1I=qnΔqfqn+1=Δqsqn+1IE7

where qI is the quaternion of rotation from the body to the inertial coordinate frame, q is the quaternion of rotation from the body to the navigational coordinate frame, Δqf is the quaternion of fast rotation increment, Δqs is the quaternion of slow rotation increment.

The quaternion of fast rotation can be represented in the form of a rotary vector as follows:

Δqf=Δqf0Δqf1Δqf2Δqf3=cosΔΦ2ΔΦxbΔΦsinΔΦ2ΔΦybΔΦsinΔΦ2ΔΦzbΔΦsinΔΦ2E8

The following relationship holds for small angles:

ΔΦ=tntn+tmωdt+12tntn+tmΦ×ωdtE9

where tm = 8ts is the slow integration step.

To solve the previous equation, a four-step algorithm will be used (Conning correction [5, 6, 7]):

ΔΦ=ΔΦxbΔΦybΔΦzb=j=14αxbjj=14αybjj=14αzbj+23P1αxb2αyb2αzb2+P3αxb4αyb4αzb4+12P1+P2αxb3αyb3αzb3+αxb4αyb4αzb4+130P1P2αxb3αyb3αzb3αxb4αyb4αzb4E10

where

αj=αkts+αk1ts
Pj=0αzbjαybjαzbj0αxbjαybjαxbj0

If we return to the quaternion of slow rotation, the following relationship is valid:

Δqs=cosΩtm2ΩxΩsinΩtm2ΩyΩsinΩtm2ΩzΩsinΩtm2E11

where. Ωx, Ωy, Ωz is the projections of the absolute angular velocity of the navigation coordinate frame on its axes.

If we neglect the rotation of the Earth, the following applies:

Ωx=VyRyVxae2b13b23Ωy=VxRx+Vyae2b13b23Ωz=01Rx=1a1e2b3322+e2b132ha1Ry=1a1e2b3322+e2b232haE12

where bij are members of the transformation matrix from the Earth-coordinate frame (ECEF) into the navigation coordinate frame BECEFn.

The Poisson equation for the transformation matrix from the coordinate frame related to the Earth (ECEF) in the navigation coordinate frame can be written in the following form:

ḂnECEF=BnECEFΔωnECEFBECEFn=BnECEFTΔωnECEF=00Ωy00ΩxΩyΩx0E13

The recursive solution of the Poisson equation can be represented in the following way:

b12N=b12N1Ωyb32N1tmb22N=b22N1+Ωxb32N1tmb32N=b32N1+Ωyb12N1Ωxb22N1tmb13N=b13N1Ωyb33N1tmb23N=b23N1+Ωxb33N1tmb33N=b33N1+Ωyb13N1Ωxb23N1tmb31N=b12Nb23Nb22Nb13NE14

With the quaternion of fast and the quaternion of slow rotations defined above, on the basis of Eq. (7), the quaternion of total rotation can be determined and with its direct cosine matrix representing the transformation from the body to the navigation coordinate frame. This matrix will be updated with the time step of the slow integration:

Cbn=12q22+q322q1q2q0q32q0q2+q1q32q1q2+q0q312q12+q322q2q3q0q12q1q3q0q22q0q1+q2q312q12+q22E15

2.2. Determination of speed and position in space

Previously defined method used for determining the angle increments based on measured gyroscope signals can now be used in the same way to define the speed increments based on signals from the accelerometer. These increments are also determined by the fast integration step ts:

ΔWxb,yb,zb=tktk+tsaxb,yb,zbdtE16

where axb, yb, zb is the signals from the accelerometer in the body coordinate frame.

The absolute acceleration can be written in the following form:

dVdtI=dVdtb+ωb×VE17

where dVdtbis the total speed derivatives with respect to the body coordinate frame, dVdtIis the total speed derivatives with respect to the inertial coordinate frame, ωb is the absolute angular velocity of the body coordinate frame.

The specific force projections acting in the body coordinate frame are obtained from the accelerometer. Accordingly, the integration will be performed in the body coordinate frame, and the previous equation can be written like

dVdtb=dVdtIωb×VE18

If we apply integration with the slow integration step to the previous equation, we obtain the following:

tktk+tmdV˜xbdtdt=tktk+tmdV¯xbdtdt+tktk+tmωzbVybωybVzbdttktk+tmdV˜ybdtdt=tktk+tmdV¯ybdtdt+tktk+tmωxbVzbωzbVxbdttktk+tmdV˜zbdtdt=tktk+tmdV¯zbdtdt+tktk+tmωybVxbωxbVybdtE19

The recursive solution of the previous equations is done in eight steps (sculling correction; see [5, 6, 7]) from which the step of slow integration was adopted as tm = 8ts:

Wxb,k=Wxb,k1+Wyb,k1αzb,kWzb,k1αyb,k+ΔWxb,kWyb,k=Wyb,k1+Wzb,k1αxb,kWxb,k1αzb,k+ΔWyb,kWzb,k=Wzb,k1+Wxb,k1αyb,kWyb,k1αxb,k+ΔWzb,kWzb,k=Wzb,k1+Wxb,kαyb,kWyb,kαxb,k+ΔWzb,kWyb,k=Wyb,k1+Wzb,kαxb,kWxb,kαzb,k+ΔWyb,kWxb,k=Wxb,k1+Wyb,kαzb,kWzb,kαyb,k+ΔWxb,kE20

The initial values in each new step of slow integration are Wxb = Wyb = Wzb = 0.

After calculating the velocity increments in the body coordinate frame, it is possible to determine the increment of the velocities in the navigation coordinate frame, since the matrix of transformation between the body and the navigational coordinate frame has already been defined:

ΔWxΔWyΔWz=CbnWxbWybWzbE21

The speed of the object relative to the Earth in the navigation coordinate frame can now be represented by the following relations, with the remark that the Earth’s rotation that is neglected:

Vx=Wxt0tVzΩydtVy=Wy+t0tVzΩxdtVz=Wzt0tVyΩxVxΩy+gdtE22

where Ωx, Ωy, Ωz is the projections of the absolute angular velocity of the navigation coordinate frame on its axes, Wx, Wy, Wz is the sums of projections of velocity increments in the navigational frame.

The determination of the position in the navigation coordinate frame can be solved in two ways: by integration of the velocities, which is the case in determining the height, or by the relationship between the matrix defined by Poisson’s equation and its definitions:

BECEFn=sinφcosλsinεsinλcosεsinφsinλsinε+cosλcosεcosφsinεsinφcosλcosε+sinλsinεsinφsinλcosεcosλsinεcosφcosεcosφcosλcosφsinλsinφE23

where φ is the latitude, λ is the longitude, ε is the azimuth.

Geographical navigation parameters can be determined from the relation of the preceding equation and Eq. (14):

ϕ=arctanb33b090+90λ=arctanb32b31180180ε=arctanb13b230360b0=b132+b232E24

As the azimuth is now defined, projections of speed in the ENUp coordinate frame can be determined:

VN=Vycosε+VxsinεVE=Vysinε+VxcosεE25

The position in the ENUp frame can be determined as

E=180πλλ0cosφ0aN=180πφφ0ah=t0tVzdtE26

Similarly, using the matrix definition from the navigation coordinate frame and the body frame, we can get to the relations for angular positions:

ψ=arctanCbn11Cbn21φ=arctanCbn32Cbn33θ=arcsinCbn31E27

3. Strapdown INS (SINS) algorithms

Three SINS algorithms based on previously defined mathematical model will be presented here.

The basic solution of SINS is forward Euler method applied to the main equations for rotation and translation. Block diagram of this method is presented in Figure 1. In this algorithm there is no division to the fast and the slow rotation, and all calculation is done in each step.

The other solution of SINS algorithm—the regular SINS—based on mathematical model previously defined is presented in Figure 2 as block diagram. The regular SINS algorithm calculates the velocity and angle increments eight times, and in the last step, Conning and Sculling corrections are implemented including all the other equations in Figure 2.

The last solution that is considered is SINS algorithm divided in eight steps. This eight-step algorithm naturally arose as a consequence of Conning equation, and it is presented in the following flowcharts.

From Figure 3, it can be seen that in each step, the main algorithm will call IMU and navigation procedures. This means that in each step, some part of calculation will be completed.

From Figure 4, it can be seen that sculling correction will be calculated in each step (Figures 5 and 6).

Similar to the IMU algorithm, the navigation procedure is also divided into several steps shown in Figure 7.

Availability of output data calculated by all three SINS algorithms is presented in Table 1.

Algorithm step01234567
Forward Euler methodVxyz,BECEFn,H,lat,lon,Cbn,ϕ,θ,ψVxyz,BECEFn,H,lat,lon,Cbn,ϕ,θ,ψVxyz,BECEFn,H,lat,lon,Cbn,ϕ,θ,ψVxyz,BECEFn,H,lat,lon,Cbn,ϕ,θ,ψVxyz,BECEFn,H,lat,lon,Cbn,ϕ,θ,ψVxyz,BECEFn,H,lat,lon,Cbn,ϕ,θ,ψVxyz,BECEFn,H,lat,lon,Cbn,ϕ,θ,ψVxyz,BECEFn,H,lat,lon,Cbn,ϕ,θ,ψ
Regular SINSVxyz,BECEFn,H,lat,lon,Cbn,ϕ,θ,ψ
Divided SINSVx,Vy,Vz,HCbn,ϕ,θ,ψBECEFn,lat,lon

Table 1.

Comparison of available data in each step for different SINS algorithms.

From Table 1, it can be seen that forward Euler algorithm provides all SINS output values in every step unlike regular and divided SINS which will provide outputs eight times slower. Generally, guidance and autopilot algorithms do not require inputs with such high frequency, and both regular and divided SINS will usually satisfy requirements; see . On the other hand, if we compare the regular and the divided SINS algorithm, we can see that in the case of the divided SINS algorithm, the entire mission algorithm can be optimized in these eight steps.

The regular and the divided SINS algorithms are based on the same numerical integration, and the results of those two algorithms are equal in time. On the other side, we can compare quaternion stability of forward Euler integration and regular SINS algorithm in time. Quaternion norm which needs to be equal to one for quaternion of rotation is sensitive to the integration step for forward Euler integration. Both algorithms were implemented in MATLAB Simulink. Norm of quaternion is presented in Figure 8 for the same integration step of 2 ms and for the same input data of gyroscopes presented in Figure 9. From Figure 8 it can be seen that the quaternion norm will be affected whenever there is significant movement of the object.

Quaternion norm error will further affect all outputs of SINS algorithm, and that will lead to error accumulation over time. Figure 10 represents angle errors for the same simulation.

4. Time consumption and processor load comparison of the regular SINS, the divided SINS and the forward Euler algorithms

Forward Euler algorithm, regular SINS algorithm and SINS algorithm divided into eight different steps presented here were compared in terms of processor load and time it takes for all necessary calculations to complete. PC with Intel Core 2 Duo P8600 processor and 4 GB of RAM was used as a testbed for comparison of the three mentioned algorithms. Ubuntu 16.04 LTS operating system in real-time mode was used for time measurements and result generation.

Instead of using real sensors to feed the data to the algorithm, the data were read from the files that contained recorded sensor outputs from INS tests previously performed. All the data were memory mapped to avoid any loss of time due to IO operations, thus making the algorithm exclusively CPU bound. Real-time interval timer set to 2 ms was used as the time frame generator for the INS algorithm in order to mimic real-life operation. Every 2 ms, an interrupt would occur causing the next piece of data to be fed to the algorithm, and the next step of the algorithm would be performed. In the case of the regular SINS algorithm, the entire quaternion calculation will be performed in every eighth step. In the case of the divided SINS algorithm, a piece of that calculation will be calculated in all of those seven middle steps as well as in the final eighth step, thus optimizing processor load and dividing calculation time across all steps in the algorithm evenly.

Statistics that are compared after the completion of the two SINS algorithms are the total time spent in every eight steps of the algorithm, average amount of time spent in every step and average processor load in each of the steps of the algorithm. Total time spent in every step of the algorithm depends on the number of steps and as such is not important as a performance measure. Average time and average processor load in each step of the algorithm are used for performance comparison. In Linux, there are three distinct time measures of process execution. Those are wall clock time, user time and system time.

Wall clock time is the amount of calendar time that elapsed from starting the process or the stopwatch until moment “now”. Thus, wall clock time includes the time the process has spent waiting for its turn on the CPU besides the time it actually spent running on the CPU. User time is the time the process spent executing on the CPU in user mode, while system time is the time the process spent executing on the CPU in system or kernel mode. User and system time measure the actual time the process spent using the CPU, and total amount of time spent on the CPU is calculated as the sum of these two time measurements.

All of these considered, wall clock represents the time that would be measured using a stopwatch. Although wall clock time heavily relies on the operating system load, on the scheduling policy used by the operating system and on the number of cores the CPU has, it can be used as a measure of time since all versions of the algorithm are subjected to the same conditions during the testing procedure. Even though the wall clock time is measured, it is not actually used in time comparison of the two mentioned algorithms. Instead, user and system time are used for comparison, because they rely only on the performance of the CPU, and the actual time it takes for calculations in the algorithm is the sum of these two times.

All times mentioned are given in microseconds. Total amount of time spent in the step of the algorithm is calculated as the sum of user and system time. Average processor load is calculated as

PL=u¯+s¯tE28

where u¯is the average user time, s¯is the average system time, t is the time sample duration.

The results obtained after time measurements of the regular SINS algorithm are presented in Table 2.

Algorithm step01234567
Elapsed time (μs)2.073952.178313.112572.718982.041082.009862.068204.82662

Table 2.

Measured time of the regular SINS.

The results obtained after time measurements of the divided SINS algorithm are presented in Table 3.

Algorithm step01234567
Elapsed time (μs)2.080522.195713.098683.073952.071482.106822.270332.35067

Table 3.

Measured time of divided SINS.

Results presented in the tables are not comparable to the execution times on faster or slower processors. Even though exact times are not comparable when a switch to a different CPU is made, their ratio will still hold. Relative time gain and processor load gain of the divided SINS over the regular SINS are presented in Table 4.

Algorithm step01234567
Time (%)+0.32+0.80−0.45+13.05+1.49+4.82+9.77−51.30

Table 4.

Regular and divided SINS time and processor load ratio.

For the sake of completeness, forward Euler version of the SINS algorithm that performs all calculations in each timer interrupt was also taken into consideration. Every 2 ms both quaternions are calculated as well as navigation parameters. Basically, this approach has no notable steps, so the previous method of time measurement is not applicable here. Instead, the average time necessary for the calculation of both quaternions and navigation parameters is taken as the performance measure.

On average, it takes 6.16023 μs for the forward Euler algorithm to perform all calculations. This translates to average processor load of 0.00308 which is significantly worse compared to the regular SINS algorithm and even more so compared to the divided SINS algorithm. Processor load and time spent calculating in each step of the regular and the divided SINS algorithms vary from step to step, whereas the time it takes for the forward Euler algorithm to do its calculations can be considered as constant. Relative time gain (T) and processor load gain (PL) of the divided SINS (DSINS) and the regular SINS (RSINS) algorithm over the forward Euler algorithm are presented in Table 5.

Algorithm step01234567
RSINS T (%)−66.33−64.64−49.47−55.86−66.87−67.37−66.43−21.65
RSINS PL (%)−66.23−64.61−49.35−55.84−66.88−67.35−66.56−21.75
DSINS T (%)−66.23−64.36−49.70−50.01−66.37−65.80−63.14−61.84
DSINS PL (%)−66.23−64.61−50.00−50.32−66.56−65.91−63.31−62.66

Table 5.

Regular and divided SINS processor load and time gains over the forward Euler algorithm.

5. Conclusion

In this chapter, navigation algorithm based on strapdown inertial navigation system algorithm optimized for coding in eight steps is presented. This algorithm proved to be a good option in situations where time and processor speed are limiting factors. Average time necessary for the regular SINS algorithm to complete all the steps and perform one full calculation is 21.02957 μs, whereas the divided SINS algorithm needs 19.24816 μs to perform the same operation, which scales to 8.47% improvement in time consumption. Even more important than time consumption improvement is the processor load in each timer interval, which is more uniformly distributed across all the steps in the divided SINS algorithm. Uniformly distributed processor load allows for easier design and development of multithreaded applications, as well as more free resources for the control computer to gather information about its surroundings and to issue commands to other devices in the control chain accordingly.

Also this algorithm proved to be mathematically more stable in term of quaternion norm, which mean that there is less error in angle computation and cumulatively in trajectory calculation.

How to cite and reference

Cite this chapter Copy to clipboard

Ivana Todić and Vladimir Kuzmanović (December 20th 2017). Code Optimization for Strapdown Inertial Navigation System Algorithm, Space Flight, George Dekoulis, IntechOpen, DOI: 10.5772/intechopen.71732. Available from:

Related Content

Next chapter

On Six DOF Relative Orbital Motion of Satellites

By Daniel Condurache

First chapter

Introductory Chapter: Drones

By George Dekoulis

We are IntechOpen, the world's leading publisher of Open Access books. Built by scientists, for scientists. Our readership spans scientists, professors, researchers, librarians, and students, as well as business professionals. We share our knowledge and peer-reveiwed research papers with libraries, scientific and engineering societies, and also work with corporate R&D departments and government entities.

View all books