Research ArticleError Prediction for SINSGPS after GPSOutage Based on Hybrid KF-UKF

Baiqiang Zhang12 Hairong Chu1 Tingting Sun12 Hongguang Jia1

Lihong Guo1 and Yue Zhang1

1Changchun Institute of Optics Fine Mechanics and Physics Chinese Academy of Sciences Changchun Jilin 130033 China2University of Chinese Academy of Sciences Beijing 10039 China

Correspondence should be addressed to Baiqiang Zhang xgdzbq163com

Received 1 July 2015 Accepted 14 September 2015

Academic Editor Rafael Morales

Copyright copy 2015 Baiqiang Zhang et alThis is an open access article distributed under the Creative CommonsAttribution Licensewhich permits unrestricted use distribution and reproduction in any medium provided the original work is properly cited

Theperformance ofMEMS-SINSGPS integrated systemdegrades evidently duringGPS outage due to the poor error characteristicsof low-cost IMU sensorsThe normal EKF is unable to estimate SINS error accurately after GPS outage owing to the large nonlinearerror caused by MEMS-IMU Aiming to solve this problem a hybrid KF-UKF algorithm for real-time SINSGPS integration ispresented in this paper The linear and nonlinear SINS error models are discussed respectively When GPS works well we fuseSINS and GPS with KF with linear SINS error model using normal EKF In the case of GPS outage we implement UnscentedTransform to predict SINS error covariance with nonlinear SINS error model until GPS signal recovers In the simulation test thatwe designed an evident accuracy improvement in attitude and velocity could be noticed compared to the normal EKF methodafter the GPS signal recovered

1 Introduction

Strapdown Inertial Navigation System (SINS) is a highly self-contained navigation system utilizing Inertial MeasurementUnits (IMU) fixed to the vehicle to determine its attitudevelocity and position by calculating the integral of theangular rates and accelerations that IMU measures GlobalPositioning System (GPS) is a satellite based radio naviga-tion system that can provide accurate velocity and positioninformation for a vehicle equipped with a GPS receiver [1]SINS is commonly integrated with GPS using Kalman Filter(KF) to combine both advantages of these two techniquesso that SINSGPS has complete navigation information highupdating rate good long-term accuracy and high reliability

In recent years with the development of Microelectroni-cal Mechanical System (MEMS) IMU can be manufacturedquite small with very low costs So MEMS-SINSGPS inte-grated navigation systems have been widely used in manyareas such as land-vehicle navigation Unmanned AerialVehicle (UAV) control and tactical missile guidance [2]Unfortunately these low-cost MEMS inertial sensors have

relatively poor error characteristics Although we can com-pensate the deterministic part by calibration experimentsthe random error including noise bias-drifts and random-walk will still cause further degradation of SINS performance[3] In a practical application GPS signal may encounterdisturbance or obstacle and KF fails to estimate SINS errorswhen no GPS information is available which causes twoproblems

The first problem is that the performance of SINSdegrades very fast since MEMS-IMU has low accuracy andno measurement information can be used for KF to correctthe errors And when GPS signal recovers another problemoccurs KF or the extended KF (EKF) only works whenthe system is linear or slightly nonlinear so that it canbe approximated by linearization However the SINS errormodel may have changed and became strongly nonlinearduringGPS outages since SINS errors have been growing veryfast Even after GPS signal recovery the KF cannot estimatethe system error correctly

The first problem has been studied for many years andseveral methods have been proposed to solve it In [4]

Hindawi Publishing CorporationMathematical Problems in EngineeringVolume 2015 Article ID 239426 9 pageshttpdxdoiorg1011552015239426

2 Mathematical Problems in Engineering

under the assumption that the land-vehicle has no slip onthe ground SINS errors are constrained by considering themovement path of the vehicle In [5] an odometer is usedto offer extra observation information for the KF in GPSoutagesThese twomethods are easy to achieve but only workwhen the movement path of the vehicle is simple So themajority of researches have been focusing on the online studymethods using Artificial Neural Network (ANN) or SupportVector Machine (SVM) [6] Nowadays several advancedinformation fusion algorithms have been proposed suchas strong-tracking Kalman Filter (STKF) combined withwavelet neural network (WNN) [7] genetic algorithm basedadaptive neurofuzzy inference system (GANFIS) [8] andDempster-Shafer augmented Support Vector Machine (DS-SVM) [9] By online training when GPS is working thesealgorithms can be used to estimate the system errors andcorrect them duringGPS outages Although proved to be effi-cient in theory these methods are seldom applied practicallyfor their huge amounts of calculation The second problemwementioned above however has not drawnmuch attentionall these years In fact most MEMS-SINSGPS integratednavigation systems today still work in SINS alone mode ifGPS signal breaks down And there will be a significantdegradation in the system performance when GPS signalrecovers owing to the drawback of KF

In order to overcome the limitation of KF Julier andUhlmann proposed the Unscented Kalman Filter (UKF) in1995 [10]The basic approach to predict the state of a stronglynonlinear system in UKF is the Unscented Transform (UT)Based on UT UKF is able to estimate a strongly nonlinearsystem and a three-order accuracy can be attainable [11]According to thework of [12 13] on low-cost INS initial align-ment the errors converge more quickly in UKF comparedto EKF when the initial attitude errors and uncertainties arelarge So the problem that the SINS error model becomesstrongly nonlinear could be solved by implementing UKFBut UKF has a larger amount of calculation compared withKF or EKF because a great number of sample points mustbe calculated in UT [14] For a MEMS-SINSGPS integratednavigation system the system model is slightly nonlinearwhen GPS is functional and it is not necessary to predict thestatewithUTThenwe comeupwith an idea of a hybridUKF-EKF SINSGPS fusion method In this algorithm UKF andEKF can be switched so that the filter is able to estimate theerror of system in a nonlinear case but has a low calculationamount when the system is linear or slightly nonlinear

In this research we aimed at improving the MEMS-SINSGPS integrated navigation system performance afterGPS outage A hybrid KF-UKF algorithm for real-timeMEMS-SINSGPS integration is presented in the paper First

we will discuss two kinds of SINS error models which arethe nonlinear model and the approximately linear modelThen we will present the hybrid KF-UKF algorithm and itscalculation progress When GPS works well we fuse SINSand GPS with KF as usual In the case of GPS outagewe implement UT to predict SINS error covariance untilGPS signal recovers Finally a simulation and an in-carexperiment were designed to test the algorithm and theresults are compared with common KF method

2 SINS Error Model

21 The Definition of SINS Error Usually the integratednavigation systemfilter is designed as indirect filtering whichmeans the system state is selected as the error of the systemparameters instead of the parameters themselves so that themodel is less complex The navigation system model is afunction of SINS error and the errors of SINS are selected asthe system state They are a vector of the errors of positionattitude velocity gyroscopes and accelerometers

x = [120575r 120575k 120595 120575f119887119894119887

120575120596119887

119894119887]

119879

(1)

The attitude error is defined as the Euler angle 120595 =

[120595119909 120595119910 120595119911]119879 between the real navigation platform 119899 (119899-

frame local north east and down) and the computednavigation platform 119899

1015840 The 1198991015840 frame is achieved by rotating119899-frame with respect to axes 119911 119910 and 119909 by the angles 120595119911120595119910 and 120595119909 respectively Then we have three coordinatetransformation matrixes

C119885 =[

[

[

cos120595119911 sin120595119911 0

minus sin120595119911 cos120595119911 0

0 0 1

]

]

]

C119884 =[

[

[

[

cos120595119910 0 minus sin1205951199100 1 0

sin120595119910 0 cos120595119910

]

]

]

]

C119883 =[

[

[

1 0 0

0 cos120595119909 sin1205951199090 minus sin120595119909 cos120595119909

]

]

]

(2)

Denoted by attitude transition matrix the attitude erroris expressed as

C1198991015840

119887= C119899

1015840

119899C119899119887

C1198991015840

119899= C119885C119884C119883 =

[

[

[

[

cos120595119910 cos120595119911 cos120595119910 sin120595119911 minus sin120595119910minus cos120595119909 sin120595119911 + sin120595119909 sin120595119910 cos120595119911 cos120595119909 cos120595119911 + sin120595119909 sin120595119910 sin120595119911 sin120595119909 cos120595119910sin120595119909 sin120595119911 + cos120595119909 sin120595119910 cos120595119911 minus sin120595119909 cos120595119911 + cos120595119909 sin120595119910 sin120595119911 cos120595119909 cos120595119910

]

]

]

]

(3)

Mathematical Problems in Engineering 3

where C1198991015840

119887is the direction cosine matrix (DCM) from the

body-frame (119887-frame) to the computed navigation frame (1198991015840-frame) C119899

119887is the DCM from 119887-frame to the real navigation

frame (119899-frame) C1198991015840

119899is the DCM from 119899-frame to 1198991015840-frame

[15]

22 Linear SINS Error Model KF is only able to estimatethe state when the system is linear Fortunately when SINSintegrated with GPS the SINS errors can be corrected everyfiltering period And SINS errors accumulated in this periodare pretty small So we can neglect the higher order termsof the SINS error function and the model is approximatelylinear

The attitude error is approximated as

C1198991015840

119899asymp I minus [120595times] (4)

The attitude the velocity and the position error equa-tions respectively are

asymp minus120596119899

119894119899times 120595 + 120575120596

119899

119894119899minus C119899119887120575120596119887

119894119887+ C119899119887w119887119892

120575k119899 asymp f119899119894119887times 120595 minus (2120596

119899

119894119890+ 120596119899

119890119899) times 120575k119899 + 120575g119899 + C119899

119887120575f119887119894119887

+ C119899119887w119887119886

120575 r119899 asymp minus120596119899

119890119899times 120575r119899 + 120575k119899

(5)

where 120596119899119894119899is the angular rotation velocity of the navigation

coordinate systemwith respect to the inertial frame120596119899119894119890is the

earth rotation velocity 120596119899119890119899is the rotation vector from the 119890-

frame to the 119899-frame f119899119894119887is the specific force vector in 119899-frame

120575g119899 is the error of the gravity vector in 119899-framew119887119892andw119887

119886are

the noise of the gyroscopes and accelerators respectively [16]And the SINS error model can be written as

x = F (119905) x + G (119905)w (6)

So it is able to predict the system state by using atransform matrix And KF is available when GPS works well

23 Nonlinear SINS Error Model Although the linear SINSmodel has been proved to be efficient in SINSGPS inte-gration it may be not accurate enough if the SINS errorsaccumulate with time when the navigation system works inSINS alone mode In this situation we cannot neglect thenonlinear parts of the SINS error function and it is necessaryto develop the nonlinear SINS error model

Now let us review (3) If we define 1205961198991015840

1198991198991015840as the angular

rotation vector from the 119899-frame to the 1198991015840-frame and =

[119909 119910 119911]119879 as the Euler angle rates then we have their

relationship with the Euler angle

1205961198991015840

1198991198991015840=

[

[

[

119909

0

0

]

]

]

+ C119883[

[

[

0

119910

0

]

]

]

+ C119883C119884[

[

[

0

0

119911

]

]

]

(7)

Without linearization the attitude error differential equa-tion can be derived from (7) as

= Cminus11205951205961198991015840

1198991198991015840=

[

[

[

[

1 0 minus sin1205951199100 cos120595119909 sin120595119909 cos1205951199100 minus sin120595119909 cos120595119909 cos120595119910

]

]

]

]

minus1

1205961198991015840

1198991198991015840 (8)

where C120595 is the transition matrix between 119899-frame and 119899

1015840-frame [17]

Based on (8) the attitude the velocity and the positionerror equations are expressed as

= Cminus1120595[(119868 minus C119899

1015840

119899) 119899

119894119899+ C119899

1015840

119899120575120596119899

119894119899minus C119899

1015840

119887120575120596119887

119894119887]

+ Cminus1120595C1198991015840

119887w119887119892

(9)

120575k119899 = (I minus C1198991198991015840)C1198991015840

119887f119887119894119887minus (2

119899

119894119890+ 119899

119890119899) times 120575k119899

minus (2120575120596119899

119894119890+ 120575120596119899

119890119899) times k119899 + (2120575120596119899

119894119890+ 120575120596119899

119890119899)

times 120575k119899 + 120575g119899 + C1198991198991015840C1198991015840

119887120575f119887119894119887+ C1198991198991015840C1198991015840

119887w119887119886

(10)

120575119871 =

V119873119877119873 +

ℎ

minus

V119873 minus 120575V119873119877119873 minus 120575119877119873 +

ℎ minus 120575ℎ

120575120582 =

V119864sec 119871(119877119864 +

ℎ)

minus

(V119864 minus 120575V119864) sec (119871 minus 120575119871)

(119877119864 minus 120575119877119864) + (

ℎ minus 120575ℎ)

120575ℎ = minus120575V119863

(11)

where 119899119894119899is the computed angular rotation velocity of the

navigation coordinate system with respect to the inertialframe 119899

119894119890is the computed earth rotation velocity 119899

119890119899is the

computed rotation vector from the 119890-frame to the 119899-frametheir errors are 120575120596119899

119894119899 120575120596119899119894119890 and 120575120596

119899

119890119899 f119887119894119887is the specific force

that the accelerators measure k119899 119871 120582 and ℎ and 120575119871 120575120582 120575ℎand 120575k119899 are the computed velocity latitude longitude andheight and their errors respectively 119877119873 119877119864 and 120575119877119873 120575119877119864are the radius of the meridian and the prime vertical circleand their errors respectively [18]

And the SINS error model is written as

x = f (x 119905) + g (x 119905)w (12)

Now we cannot predict the SINS errors by transformmatrix As we presented in the next part the UT method isused to predict the system state

3 SINSGPS Integration Algorithm

31 Kalman Filter Developed in 1960s by Kalman KF hasbeen proved to be a powerful optimal estimation theory forlinear systems In SINSGPS integration KF is triggered ineveryGPSupdate epoch using themeasurement informationthat is the difference of the velocity and position betweenSINS and GPS Then the errors of SINS could be estimatedand corrected When GPS works well no SINS error is

4 Mathematical Problems in Engineering

accumulated and the navigation errors are bounded Thusthe linear SINS error model is accurate enough since thenonlinear parts could be ignored

The linear SINS error transition matrix is discretized asfollows [19]

Φ119896|119896minus1 asymp I + F119896minus1Δ119879 +

1

2

F2119896minus1

Δ119879

2

Q119896minus1

asymp G119896minus1qG119879

119896minus1Δ119879

+

1

2

(F119896minus1G119896minus1qG119879

119896minus1+ G119896minus1qG

119879

119896minus1F119879119896minus1

) Δ119879

2

(13)

where F(119905) is the coefficient matrix of the state equation Δ119879is the filter cycle q is the covariance matrix of SINS sensorsand G(119905) is the coefficient matrix of the state noise

For a loosely coupled SINSGNSS system the measure-ment equation is [20]

z = Hx + k (14)

where z is the difference of the velocity and position betweenSINS and GPS H = [I6times6 06times9] k is the noise standarddeviation vector

KF algorithm is committed by using such formulas [21]x119896|119896minus1 = Φ119896|119896minus1x119896minus1 (15)

P119896|119896minus1 = Φ119896|119896minus1P119896minus1Φ119879

119896|119896minus1+Q119896minus1 (16)

K119896 = P119896|119896minus1H119879

119896(H119896P119896|119896minus1H

119879

119896+ R119896)

minus1

(17)

x119896 = x119896|119896minus1 + K119896 (z119896 minusH119896x119896|119896minus1) (18)

P119896 = (I minus K119896H119896)P119896|119896minus1 (19)

In SINSGPS integration a close-loop configuration isused In every KF epoch when the SINS errors are correctedthe state x119896 is set to be zero

32 Unscented Kalman Filter As we mentioned above KFis only available when the state equation is linear because itis not feasible to predict the state by transition matrix for anonlinear system If we neglect the higher order terms KFwill introduce errors which cannot be ignored for a stronglynonlinear system [22] To predict the state of a nonlinearsystem UT is used in UKF by generating a series of samplepoints to simulate the transfer of the state which couldachieve an accuracy of three orders

In UT the state is predicted in three steps [23] Firstsigma points should be constructed

120594(0)

119896minus1= x119896minus1

120594(119894)

119896minus1= x119896minus1 + (radic(119899 + 120582)P119896minus1)

119888(119894)

119894 = 1 2 119899

120594(119894)

119896minus1= x119896minus1 minus (radic(119899 + 120582)P119896minus1)

119888(119894minus119899)

119894 = 119899 + 1 119899 + 2 2119899

(20)

where 119899 is the dimension of the state vector x 120582 = 120572

2(119899 +

120581) minus 119899 which decides the weights of the distribution of thesigma points and generally 120581 = 3 minus 119899 while 10minus4 le 120572 le

1 And (radic(119899 + 120582)P119896minus1)119888(119894) is the 119894 column of the Choleskydecomposition of the matrix (119899 + 120582)P119896minus1

Second the states are predicted with every sigma pointThis step is executed by solving the error state differentialequations with four-order Runge-Kutta method

120585(119894)

119896|119896minus1= 119891 (120594

(119894)

119896minus1) 119894 = 1 2 2119899 (21)

Finally the states are weighted and summarized so weobtain the predicted state x119896|119896minus1 and the error covariancematrix P119896|119896minus1

x119896|119896minus1 =2119899

sum

119894=0

119882

(119898)

119894120585(119894)

119896|119896minus1

P119896|119896minus1 =2119899

sum

119894=0

119882

(119888)

119894[(120585(119894)

119896|119896minus1minus x119896|119896minus1) (120585

(119894)

119896|119896minus1minus x119896|119896minus1)

119879

]

+Q119896minus1

(22)

where Q119896minus1 is the covariance matrix of the state noise119882(119898)119894

and119882(119888)119894

are the weights of the sigma points which could becalculated as

119882

(119898)

0=

120582

119899 + 120582

119882

(119888)

0=

120582

119899 + 120582

+ 1 minus 120572

2+ 120573

119882

(119898)

119894= 119882

(119888)

119894=

120582

2 (119899 + 120582)

119894 = 1 2 2119899

(23)

where 120572 and 120582 have been given in the first step and 120573 isassigned according to the distribution character of the stateerror In this case 120573 = 2

In loosely coupled SINGPS the measurement equationis linear as we saw in (14) So after we predict the state x119896|119896minus1and the error covariance matrix P119896|119896minus1 we could estimate theSINS errors with formulas (17) to (19)

33 Hybrid KF-UKF Algorithm In this part we discussed thearchitecture of the hybrid KF-UKF algorithm As shown inFigure 1 SINS information is updated together with GPSWhenGPS information is received its availability is judged sothat whichmethod is to going be executed can be determined

If GPS information is available SINS and GPS can beintegrated by normal KF method as shown in Figure 2 Firstlinear SINS error equation (6) is discretized by formula (13)with SINS information at time 119905 = 119896 minus 1 Then one-step stateprediction and error covariance prediction are calculatedwith formulas (15) and (16) and the filter gain is calculatedwith formula (17) Afterwards the state vector which is theSINS error and error covariance at time 119905 = 119896 are estimatedwith GPS information at 119905 = 119896 Finally the SINS errorestimated at 119905 = 119896 is feedback to SINS correcting the SINSerror and the same procedure is committed in the next cycle

Mathematical Problems in Engineering 5

Velocity position Attitude velocity position angular rate and acceleration

GPS information SINS information

GPS available

One-step state prediction

No

Error estimation

Yes

Correct SINSerror

at t = k at t = k minus 1

Estimate SINSerrors xk by KF

Predict state xk|kminus1 andcovariance Pk|kminus1 by UT

k = k + 1

minus

Figure 1 The architecture of the hybrid KF-UKF algorithm

SINS update

One-step prediction

SINSinformation

CorrectSINS

Filter update

CalculateΦk|kminus1 Qkminus1

with F(t) G(t)

xk|kminus1Pk|kminus1

Calculate filter gain Kk

State estimate xkError covariance Pk rGPS k

t = k minus 1

t = kvGPS k

Figure 2 Filter updates procedure when GPS is functioning well

Table 1 Main specifications of experiment instruments in thesimulation test

Equipment Parameters

GPS receiverVelocity accuracy 01ms (1120590)

Position accuracy 5m (horizontal)10m (vertical)

Output frequency 10Hz

MEMS-IMU

GyroscopeBias 50∘h (1120590)White noise 35∘hradicHz

AccelerometerBias 2mg (1120590)White noise 03mgradicHzOutput frequency 200Hz

During GPS signal blockage the GPS information isunavailable and one-step state prediction is executed withoutmeasurement updateNow the nonlinear SINS error equationand UT are applied to predict the state and error covarianceAs shown in Figure 3 let us assume that theGPS signal breaksoff at 119905 = 119896 and recovers at 119905 = 119896 + 119899 In order to estimateSINS error at 119905 = 119896 + 119899 it is necessary to predict the statex119896+119899|119896minus1 and error covariance P119896+119899|119896minus1 Suppose that 119896 + 119904 is atime instant in GPS outage and x119896+119904|119896minus1P119896+119904|119896minus1 are iterated

Table 2 Aircraft maneuvers in the test

Stage Period Aircraft maneuver Parameters1 0sim30 s Stationary v = 0ms2 30sim60 s Acceleration v = 0ms to v = 100ms3 60sim75 s Pitching 120579 = 0∘ to 120579 = 30∘

4 75sim95 s Straight flight v = 100ms5 95sim120 s Pitching 120579 = 30∘ to 120579 = 0∘

6 120sim140 s Straight flight v = 100ms7 140sim165 s Coordinate turn 120595 = 0∘ to 120595 = 40∘

8 165sim190 s Straight flight v = 100ms9 190sim215 s Coordinate turn 120595 = 40∘ to 120595 = 0∘

10 215sim300 s Straight flight v = 100ms

by using x119896+119904minus1|119896minus1P119896+119904minus1|119896minus1 and SINS information at time119905 = 119896 + 119904 minus 1 119905 = 119896 + 119904 minus 05 and 119905 = 119896 + 119904 with formulas (24)to (27) This calculation is executed cycle by cycle until GPSsignal recovers at 119905 = 119896 + 119899 When GPS receiver offered thevelocity andposition information of the vehicle at 119905 = 119896+119899 wealready have the state x119896+119899|119896minus1 and error covariance P119896+119899|119896minus1Then we can estimate SINS error at 119905 = 119896 + 119899 with formulas(18) and (19) correct it and go into the next filter cycle

120594(0)

119896+119904minus1|119896minus1= x119896+119904minus1|119896minus1

120594(119894)

119896+119904minus1|119896minus1= x119896+119904minus1|119896minus1 + (radic(119899 + 120582)P119896+119904minus1|119896minus1)

119888(119894)

119894 = 1 2 119899

120594(119894)

119896+119904minus1|119896minus1= x119896+119904minus1|119896minus1 minus (radic(119899 + 120582)P119896+119904minus1|119896minus1)

119888(119894minus119899)

119894 = 119899 + 1 119899 + 2 2119899

(24)

120585119896+119904|119896minus1 = 119891 (120594119896+119904minus1|119896minus1

) 119894 = 1 2 2119899 (25)

x119896+119904|119896minus1 =2119899

sum

119894=0

119882

(119898)

119894120585(119894)

119896+119904|119896minus1 (26)

P119896+119904|119896minus1 =2119899

sum

119894=0

119882

(119888)

119894[(120585(119894)

119896+119904|119896minus1minus x119896+119904|119896minus1)

sdot (120585(119894)

119896+119904|119896minus1minus x119896+119904|119896minus1)

119879

] +Q119896minus1

(27)

4 Simulation Test

41 Simulation Description and Parameters For the sake oftesting the hybrid KF-UKF algorithm we design a simulationcomparing this algorithm with normal EKF algorithm Theschematic diagram of the simulation is shown in Figure 4 Byusing a path generator we calculate the acceleration angularrate velocity position and attitude information of an aircraftStochastic errors were added to the acceleration and angularrate data to simulate the IMU sensor error And the sameprocess is done to the velocity and position information tosimulate the GPS error Besides GPS outage flag is added to

6 Mathematical Problems in Engineering

One-step predictionand SINS information

One-step prediction

GPS signalbreaks off

SINS update Filter update

CorrectSINS

GPS receiver

GPS signalrecovers

and SINS information

SINSinformation

One-step prediction

State estimate xk+n

CalculateΦk|kminus1 Qkminus1

with F(t) G(t)

xk|kminus1Pk|kminus1

xk+n|kminus1Pk+n|kminus1

Calculate 120585k+n|kminus1 with 120594k+nminus1|kminus1

Calculate 120594k+nminus1|kminus1 withxk+nminus1|kminus1Pk+nminus1|kminus1

xk+s|kminus1Pk+s|kminus1

Calculate 120585k+s|kminus1 with 120594k+sminus1|kminus1

Calculate 120594k+sminus1|kminus1 withxk+sminus1|kminus1Pk+sminus1|kminus1

t = k minus 1

t = k

middot middot middotmiddot middot middot

middot middot middotmiddot middot middot

t = k + s minus 1

t = k + s minus 05

t = k + s

t = k + n minus 1

t = k + n minus 05

t = k + n

rGPS k+n

vGPS k+n Error covariance Pk+n

Figure 3 Filter updates procedure during GPS outage

Table 3 Comparison of navigation error between EKF and hybrid KF-UKF after GPS outage

Outages length Adopted approach RMS attitude error (∘) RMS velocity error (ms) RMS position error (m)Roll Pitch Yaw North East Down Latitude Longitude Height

50 s EKF 00451 00566 02605 00256 00233 00331 11990 08245 33450Hybrid KF-UKF 00438 00549 02499 00255 00233 00331 11984 08199 32310

100 s EKF 00553 01050 02630 00286 00273 00367 11832 37463 30911Hybrid KF-UKF 00508 00767 01706 00240 00259 00339 11701 37434 39175

150 s EKF 00616 00590 01282 00363 00341 00479 13913 22053 31303Hybrid KF-UKF 00489 00367 01140 00219 00237 00333 13876 21949 39078

200 s EKF 00969 00560 17882 00431 00544 00619 15559 39014 36364Hybrid KF-UKF 00586 00403 17131 00231 00318 00360 15552 38998 35416

Path generator

Integration algorithm

IMU stochastic error

GPS stochastic errorGPS outage flag

Results comparison

a 120596 v r 120595

aIMU 120596IMU vGPS rGPS

vnav rnav120595nav

120575v

120575r

120575120595

Figure 4 Schematic diagram of the simulation

GPS data so the integration algorithm is able to judgewhetherGPS signal is lost Finally velocity position and attitude data

calculated by hybrid KF-UKF and normal EKF algorithm arecompared The simulation parameters are shown in Table 1

42 Simulation Results and Analysis In the simulation testthe aircraft has done a series of maneuvers which are listed inTable 2 and its acceleration angular rate velocity positionand attitude information was recorded We calculate thenavigation parameters of the aircraft by integrating SINS andGPS using the normal EKF and hybrid KF-UKF respectivelyThen the results were compared as is shown in Figure 5 andTable 3

Figure 5 is the comparison of attitude error curves of EKFand hybrid KF-UKF which are taken for example As weobserve the GPS signal was lost at 119905 = 70 s and recovered

Mathematical Problems in Engineering 7

270 275minus04

minus02

0

02

04

270 272 274minus02

minus01

0

Hybrid KF-UKFEKF

Hybrid KF-UKFEKF

GPS recovery timeGPS recovery time

GPS lost time GPS lost time

minus05

0

05

1

15

Roll

erro

r (∘)

minus05

0

05

1

Pitc

h er

ror (

∘)

100 150 200 250 30050

t (s)100 150 200 250 30050

t (s)

Figure 5 Comparison of attitude error between EKF and hybrid KF-UKF

270 271 272minus04

minus02

0

02

270 271 272minus01

001020304

Hybrid KF-UKFEKF

Hybrid KF-UKFEKF

GPS lost timeGPS lost time

GPS recovery time

GPS recovery time

minus12

minus10

minus8

minus6

minus4

minus2

02

Nor

th v

eloci

ty er

ror (

ms

)

100 150 200 250 30050t (s)

0

5

10

15

East

velo

city

erro

r (m

s)

100 150 200 250 30050t (s)

Figure 6 Comparison of velocity error between EKF and hybrid KF-UKF

at 119905 = 270 s During the 200 seconds GPS outage the rolland pitch angle error grew with time because the accuracy ofMEMS inertial sensors is very low and no GPS measurementinformation could be used for the filter to correct the SINSerror When the navigation system received GPS signal againat 119905 = 270 s the filter started to correct SINS error againIn the figure we can notice that after GPS information wasrecovered the attitude error reduced quickly However theattitude calculated by using hybrid KF-UKF is more accuratethan that calculated by EKF which is shown in the circle inthe figure This phenomenon appears for the reason that aswe described before the MEMS-IMU sensor caused largenonlinear error during GPS outage Based on the nonlinearSINS error model UT is able to predict SINS errors betterthan EKF thus the filter can correct the SINS errors quicklyand accurately And the similar phenomenon also occurs tothe velocity which is shown in Figure 6

Table 3 is the comparison of navigation error betweenEKF and hybrid KF-UKF after GPS outage For attitude errorwe can notice that the error of yaw is larger than that of rolland pitch owing to the poor observability of yawing angle invelocityposition integrationmode thus the filter is unable toestimate its error correctly By comparing the RMS attitudeerror and velocity error after GPS outage it is shown that thenavigation error is lower if the hybrid KF-UKF algorithm isapplied Also it is shown in Table 3 that with the GPS outageperiod increases the hybrid KF-UKF algorithm was betterand better compared to the EKF algorithm That is becausethe error of an inertial navigation system accumulates withthe increase The longer the GPS outage is the larger the

nonlinear error the MEMS-IMU causes And the hybridKF-UKF algorithm will show more superiority to normalEKF algorithm We can also notice that the latitude andlongitude accuracy has little improvement on the contrary toattitude and velocity And this can be explained by examiningthe SINS error equations (9) to (11) In formulas (9) and(10) the attitude error and velocity error are affected by thenonlinear terms C119899

1015840

119899and Cminus1

120595whose values grew fast during

GPS outage However when we refer to formula (11) wecan see that the numerical value of 120575119871 120575119877119864 and 120575119877119873 isvery small and little nonlinear error is caused during GPSoutage because the aircraft did notmove quite far away duringthe simulation so the EKF is still able to correct its erroraccurately when GPS signal recovers

In Table 4 we list the calculation amounts of EKF andUKF so that we can estimate the computational cost of theproposed combined algorithm The calculation amounts ofthe algorithm are evaluated using the floating-point opera-tions (flops) which is defined as the operation of addingdecreasing multiplying or dividing between two floatingnumbers For example if we add one (119899 times 119898) dimensionmatrix to another (119899 times 119898) dimension matrix then 119899119898 flopshave been generated in the computer In Table 4 we supposethe dimension of the state vector is 119899 and the measurementvector dimension is 119897 During the prediction updating period4119899

3+119899

2minus119899 flops are generated in EKFwhile 6(23)1198993+121198992+

3119899 flops are generated in UKF In SINSGNSS integration119899 = 15 and 119897 = 6 So there will be 13710 flops in EKF and25245 flops in UKF during the prediction updating periodWhat is more in Unscented Transform the state prediction

8 Mathematical Problems in Engineering

Table 4 Comparison of computational cost of EKF and UKF

EKF UKF (for linear measurement equation)Step Flops Step Flops

Prediction update

Generate sigmapoints120594119896minus1

23119899

3+ 3119899

2+ 119899

State predictionx119896|119896minus1

2119899

2minus 119899

State predictionx119896|119896minus1

4119899

2+ 119899

Error covarianceprediction P119896|119896minus1

4119899

3minus 119899

2

Error covarianceprediction

P119896|119896minus16119899

3+ 5119899

2+ 119899

Subtotal 4119899

3+ 119899

2minus 119899 Subtotal 6(23)119899

3+ 12119899

2+ 3119899

Measurement update

Filter gainK119896

4119899

2119897 + 4119899119897

2+ 119897

3minus 3119899119897

Filter gainK119896

4119899

2119897 + 4119899119897

2+ 119897

3minus 3119899119897

State updatex119896

4119899119897

State updatex119896

4119899119897

Error covariance updateP119896

2119899

3+ 2119899

2119897 minus 119899

2

Error covarianceupdateP119896

2119899

3+ 2119899

2119897 minus 119899

2

Subtotal 2119899

3+6119899

2119897+4119899119897

2+119897

3+119899119897minus119899

2 Subtotal 2119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+ 119899119897 minus 119899

2

In total 6119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+119899119897 minus 119899 In total 8(23)119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+ 11119899

2+

119899119897 + 3119899

119899 = 15 119897 = 6 30801 119899 = 15 l = 6 42336

is performed by solving the error state differential equationswith Runge-Kutta method (2119899 + 1) times since each sigmapoint has to be predictedThat is to say more than 25245 flopsare generated in one filter recycle during GPS outages whenwe predict the SINS error using Unscented Transform So infact the computational cost of UKF is much larger than thatin EKF in the prediction updating part thus the hybrid UKF-EKF is recommended to reduce the computational burden forthe computer In hybridUKF-EKF theUnscented Transformwhich is necessary to predict the nonlinear SINS error onlyperforms during GPS outages Last but not least the amountof flops in EKF during the measurement updating period is2119899

3+6119899

2119897+4119899119897

2+119897

3+119899119897minus119899

2 in this case 17091 as well as thatin UKFThis is for the reason that in SINSGNSS integrationthe measurement equation is linear and no sigma point isneeded in the measurement updating part When GPS signalis functioning well there will be 30801 flops in each filtercycle

5 Conclusion

This paper presents a hybrid KF-UKF algorithm for real-timeMEMS-SINSGPS integrationThe linear and nonlinearSINS models are discussed The flowchart of the hybrid KF-UKF algorithm is described The SINS error is estimated andcorrected with linear SINS model when GPS is functioningwell while it is predicted with nonlinear SINS model byapplying Unscented Transform during GPS outage Thesimulation results indicate that compared to normal EKFalgorithm the hybrid KF-UKF algorithm is able to predictthe SINS error more accurately if GPS suffers from long-time GPS outage and the navigation error is lower after GPS

signal was regainedThe results also show that the hybrid KF-UKF algorithm is more efficient for attitude error predictionbut the effect on position error is not evident Comparedwith normal UKF the hybrid KF-UKF algorithm has lowcalculation amount In this paper the remaining problemwhich we have not solved is that SINS errors still grow fastduring GPS outage So in our future work we may combinethe UKF with ANN or SVM to create a new algorithm toreduce the SINS errors during GPS outage

Conflict of Interests

The authors declare that there is no conflict of interestsregarding the publication of this paper

Acknowledgments

This research is supported by the 3rd Innovation Fund ofChangchun Institute of Optics Fine Mechanics and Physics(CIOMP) and the UAV semi-physical simulation platformresearch project Chinese Science Academy (CSA)

References

[1] P D Groves Principles of GNSS Inertial and MultisensorIntegrated Navigation Systems Artech House Norwood MassUSA 2008

[2] Y Yuksel N El-Sheimy andANoureldin ldquoErrormodeling andcharacterization of environmental effects for low cost inertialMEMS unitsrdquo in Proceedings of the IEEEION Position Locationand Navigation Symposium pp 598ndash612 Indian Wells CalifUSA May 2010

Mathematical Problems in Engineering 9

[3] P Aggarwal Z Syed X Niu and N El-Sheimy ldquoA standardtesting and calibration procedure for low cost MEMS inertialsensors and unitsrdquo The Journal of Navigation vol 61 no 2 pp323ndash336 2008

[4] S Godha and M E Cannon ldquoGPSMEMS INS integratedsystem for navigation in urban areasrdquo GPS Solutions vol 11 no3 pp 193ndash203 2007

[5] M Ilyas Y Yang Q S Qian and R Zhang ldquoLow-cost IMUodometerGPS integrated navigation aided with two antennaeheading measurement for land vehicle applicationrdquo in Proceed-ings of the 25th Chinese Control andDecision Conference (CCDCrsquo13) pp 4521ndash4526 Guiyang China May 2013

[6] Z Jiang C Liu G Zhang Y P Wang C Huang and J LiangldquoGPSINS integrated navigation based on UKF and simulatedannealing optimized SVMrdquo in Proceedings of the IEEE 78thVehicular Technology Conference (VTC Fall rsquo13) vol 14 pp 1ndash5 Las Vegas Nev USA September 2013

[7] L Chen and J Fang ldquoA hybrid prediction method for bridgingGPS outages in high-precision POS applicationrdquo IEEE Transac-tions on Instrumentation and Measurement vol 63 no 6 pp1656ndash1665 2014

[8] A M Hasan K Samsudin A R Ramli and R S AzmirldquoAutomatic estimation of inertial navigation system errors forglobal positioning system outage recoveryrdquo Proceedings of theInstitution of Mechanical Engineers Part G Journal of AerospaceEngineering vol 225 no 1 pp 86ndash96 2011

[9] X Chen C Shen W-B Zhang M Tomizuka Y Xu andK Chiu ldquoNovel hybrid of strong tracking Kalman filter andwavelet neural network for GPSINS during GPS outagesrdquoMeasurement vol 46 no 10 pp 3847ndash3854 2013

[10] S J Julier and J K Uhlmann ldquoA new approach for filteringnonlinear systemrdquo in Proceedings of the American ControlConference vol 3 pp 1628ndash1632 IEEE SeattleWashUSA June1995

[11] P-B Ma H-X Baoyin and J-S Mu ldquoAutonomous navigationof Mars probe based on optical observation of Martian moonrdquoOptics and Precision Engineering vol 22 no 4 pp 863ndash8692014

[12] E Shin Estimation Techniques for Low-Cost Inertial NavigationThe University of Calgary Calgary Canada 2005

[13] Y Yi On improving the accuracy and reliability of GPSINS-based direct sensor georeferencing [PhD thesis] Ohio StateUniversity Columbus Ohio USA 2007

[14] Y Hao Z Xiong F Sun and X Wang ldquoComparison ofunscented Kalman filtersrdquo in Proceedings of the IEEE Interna-tional Conference on Mechatronics and Automation pp 895ndash899 Harbin China August 2007

[15] R M Rogers Applied Mathematics in Integrated NavigationSystems American Institute of Aeronautics and AstronauticsReston Va USA 3rd edition 2007

[16] D H Titterton and J L Weston Strapdown Inertial NavigationTechnology The Institution of Electrical Engineers MichaelFaraday House Stevenage UK 2nd edition 2004

[17] G Yan W Yan and D Xu ldquoA SINS nonlinear error modelreflecting better characteristics of SINS errorsrdquo Journal ofNorthwestern Polytechnical University vol 27 no 4 pp 511ndash5162009

[18] G M Yan W S Yan and D M Xu ldquoApplication of simplifiedUKF in SINS initial alignment for large misalignment anglesrdquoJournal of Chinese Inertial Technology vol 16 no 3 pp 253ndash2642008

[19] P S Maybeck Stochastic Models Estimation and ControlAcademic Press New York NY USA 1979

[20] S Liu Research and implementation of GPSINS integrated nav-igation algorithm [MS thesis] PLA Information EngineeringUniversity Zhengzhou China 2012 (Chinese)

[21] M S Grewal and A P Andrews Kalman Filtering Theory andPractice Using MATLAB John Wiley amp Sons Hoboken NJUSA 3rd edition 2008

[22] M C Vandyke J L Schwartz and C D Hall ldquoUnscentedKalman filtering for spacecraft attitude state and parameterestimationrdquo Advances in the Astronautical Sciences vol 119 no1 pp 217ndash228 2005

[23] Y Y Qin H Y Zhang and S S Wang Theory of KalmanFilter and Integrated Navigation North-Western PolytechnicalUniversity Press Xirsquoan China 2012 (Chinese)

Submit your manuscripts athttpwwwhindawicom

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical Problems in Engineering

Hindawi Publishing Corporationhttpwwwhindawicom

Differential EquationsInternational Journal of

Volume 2014

Applied MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical PhysicsAdvances in

Complex AnalysisJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

OptimizationJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Operations ResearchAdvances in

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Function Spaces

Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of Mathematics and Mathematical Sciences

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Algebra

Discrete Dynamics in Nature and Society

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Decision SciencesAdvances in

Discrete MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom

Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Stochastic AnalysisInternational Journal of

2 Mathematical Problems in Engineering

under the assumption that the land-vehicle has no slip onthe ground SINS errors are constrained by considering themovement path of the vehicle In [5] an odometer is usedto offer extra observation information for the KF in GPSoutagesThese twomethods are easy to achieve but only workwhen the movement path of the vehicle is simple So themajority of researches have been focusing on the online studymethods using Artificial Neural Network (ANN) or SupportVector Machine (SVM) [6] Nowadays several advancedinformation fusion algorithms have been proposed suchas strong-tracking Kalman Filter (STKF) combined withwavelet neural network (WNN) [7] genetic algorithm basedadaptive neurofuzzy inference system (GANFIS) [8] andDempster-Shafer augmented Support Vector Machine (DS-SVM) [9] By online training when GPS is working thesealgorithms can be used to estimate the system errors andcorrect them duringGPS outages Although proved to be effi-cient in theory these methods are seldom applied practicallyfor their huge amounts of calculation The second problemwementioned above however has not drawnmuch attentionall these years In fact most MEMS-SINSGPS integratednavigation systems today still work in SINS alone mode ifGPS signal breaks down And there will be a significantdegradation in the system performance when GPS signalrecovers owing to the drawback of KF

In order to overcome the limitation of KF Julier andUhlmann proposed the Unscented Kalman Filter (UKF) in1995 [10]The basic approach to predict the state of a stronglynonlinear system in UKF is the Unscented Transform (UT)Based on UT UKF is able to estimate a strongly nonlinearsystem and a three-order accuracy can be attainable [11]According to thework of [12 13] on low-cost INS initial align-ment the errors converge more quickly in UKF comparedto EKF when the initial attitude errors and uncertainties arelarge So the problem that the SINS error model becomesstrongly nonlinear could be solved by implementing UKFBut UKF has a larger amount of calculation compared withKF or EKF because a great number of sample points mustbe calculated in UT [14] For a MEMS-SINSGPS integratednavigation system the system model is slightly nonlinearwhen GPS is functional and it is not necessary to predict thestatewithUTThenwe comeupwith an idea of a hybridUKF-EKF SINSGPS fusion method In this algorithm UKF andEKF can be switched so that the filter is able to estimate theerror of system in a nonlinear case but has a low calculationamount when the system is linear or slightly nonlinear

In this research we aimed at improving the MEMS-SINSGPS integrated navigation system performance afterGPS outage A hybrid KF-UKF algorithm for real-timeMEMS-SINSGPS integration is presented in the paper First

we will discuss two kinds of SINS error models which arethe nonlinear model and the approximately linear modelThen we will present the hybrid KF-UKF algorithm and itscalculation progress When GPS works well we fuse SINSand GPS with KF as usual In the case of GPS outagewe implement UT to predict SINS error covariance untilGPS signal recovers Finally a simulation and an in-carexperiment were designed to test the algorithm and theresults are compared with common KF method

2 SINS Error Model

21 The Definition of SINS Error Usually the integratednavigation systemfilter is designed as indirect filtering whichmeans the system state is selected as the error of the systemparameters instead of the parameters themselves so that themodel is less complex The navigation system model is afunction of SINS error and the errors of SINS are selected asthe system state They are a vector of the errors of positionattitude velocity gyroscopes and accelerometers

x = [120575r 120575k 120595 120575f119887119894119887

120575120596119887

119894119887]

119879

(1)

The attitude error is defined as the Euler angle 120595 =

[120595119909 120595119910 120595119911]119879 between the real navigation platform 119899 (119899-

frame local north east and down) and the computednavigation platform 119899

1015840 The 1198991015840 frame is achieved by rotating119899-frame with respect to axes 119911 119910 and 119909 by the angles 120595119911120595119910 and 120595119909 respectively Then we have three coordinatetransformation matrixes

C119885 =[

[

[

cos120595119911 sin120595119911 0

minus sin120595119911 cos120595119911 0

0 0 1

]

]

]

C119884 =[

[

[

[

cos120595119910 0 minus sin1205951199100 1 0

sin120595119910 0 cos120595119910

]

]

]

]

C119883 =[

[

[

1 0 0

0 cos120595119909 sin1205951199090 minus sin120595119909 cos120595119909

]

]

]

(2)

Denoted by attitude transition matrix the attitude erroris expressed as

C1198991015840

119887= C119899

1015840

119899C119899119887

C1198991015840

119899= C119885C119884C119883 =

[

[

[

[

cos120595119910 cos120595119911 cos120595119910 sin120595119911 minus sin120595119910minus cos120595119909 sin120595119911 + sin120595119909 sin120595119910 cos120595119911 cos120595119909 cos120595119911 + sin120595119909 sin120595119910 sin120595119911 sin120595119909 cos120595119910sin120595119909 sin120595119911 + cos120595119909 sin120595119910 cos120595119911 minus sin120595119909 cos120595119911 + cos120595119909 sin120595119910 sin120595119911 cos120595119909 cos120595119910

]

]

]

]

(3)

Mathematical Problems in Engineering 3

where C1198991015840

119887is the direction cosine matrix (DCM) from the

body-frame (119887-frame) to the computed navigation frame (1198991015840-frame) C119899

119887is the DCM from 119887-frame to the real navigation

frame (119899-frame) C1198991015840

119899is the DCM from 119899-frame to 1198991015840-frame

[15]

22 Linear SINS Error Model KF is only able to estimatethe state when the system is linear Fortunately when SINSintegrated with GPS the SINS errors can be corrected everyfiltering period And SINS errors accumulated in this periodare pretty small So we can neglect the higher order termsof the SINS error function and the model is approximatelylinear

The attitude error is approximated as

C1198991015840

119899asymp I minus [120595times] (4)

The attitude the velocity and the position error equa-tions respectively are

asymp minus120596119899

119894119899times 120595 + 120575120596

119899

119894119899minus C119899119887120575120596119887

119894119887+ C119899119887w119887119892

120575k119899 asymp f119899119894119887times 120595 minus (2120596

119899

119894119890+ 120596119899

119890119899) times 120575k119899 + 120575g119899 + C119899

119887120575f119887119894119887

+ C119899119887w119887119886

120575 r119899 asymp minus120596119899

119890119899times 120575r119899 + 120575k119899

(5)

where 120596119899119894119899is the angular rotation velocity of the navigation

coordinate systemwith respect to the inertial frame120596119899119894119890is the

earth rotation velocity 120596119899119890119899is the rotation vector from the 119890-

frame to the 119899-frame f119899119894119887is the specific force vector in 119899-frame

120575g119899 is the error of the gravity vector in 119899-framew119887119892andw119887

119886are

the noise of the gyroscopes and accelerators respectively [16]And the SINS error model can be written as

x = F (119905) x + G (119905)w (6)

So it is able to predict the system state by using atransform matrix And KF is available when GPS works well

23 Nonlinear SINS Error Model Although the linear SINSmodel has been proved to be efficient in SINSGPS inte-gration it may be not accurate enough if the SINS errorsaccumulate with time when the navigation system works inSINS alone mode In this situation we cannot neglect thenonlinear parts of the SINS error function and it is necessaryto develop the nonlinear SINS error model

Now let us review (3) If we define 1205961198991015840

1198991198991015840as the angular

rotation vector from the 119899-frame to the 1198991015840-frame and =

[119909 119910 119911]119879 as the Euler angle rates then we have their

relationship with the Euler angle

1205961198991015840

1198991198991015840=

[

[

[

119909

0

0

]

]

]

+ C119883[

[

[

0

119910

0

]

]

]

+ C119883C119884[

[

[

0

0

119911

]

]

]

(7)

Without linearization the attitude error differential equa-tion can be derived from (7) as

= Cminus11205951205961198991015840

1198991198991015840=

[

[

[

[

1 0 minus sin1205951199100 cos120595119909 sin120595119909 cos1205951199100 minus sin120595119909 cos120595119909 cos120595119910

]

]

]

]

minus1

1205961198991015840

1198991198991015840 (8)

where C120595 is the transition matrix between 119899-frame and 119899

1015840-frame [17]

Based on (8) the attitude the velocity and the positionerror equations are expressed as

= Cminus1120595[(119868 minus C119899

1015840

119899) 119899

119894119899+ C119899

1015840

119899120575120596119899

119894119899minus C119899

1015840

119887120575120596119887

119894119887]

+ Cminus1120595C1198991015840

119887w119887119892

(9)

120575k119899 = (I minus C1198991198991015840)C1198991015840

119887f119887119894119887minus (2

119899

119894119890+ 119899

119890119899) times 120575k119899

minus (2120575120596119899

119894119890+ 120575120596119899

119890119899) times k119899 + (2120575120596119899

119894119890+ 120575120596119899

119890119899)

times 120575k119899 + 120575g119899 + C1198991198991015840C1198991015840

119887120575f119887119894119887+ C1198991198991015840C1198991015840

119887w119887119886

(10)

120575119871 =

V119873119877119873 +

ℎ

minus

V119873 minus 120575V119873119877119873 minus 120575119877119873 +

ℎ minus 120575ℎ

120575120582 =

V119864sec 119871(119877119864 +

ℎ)

minus

(V119864 minus 120575V119864) sec (119871 minus 120575119871)

(119877119864 minus 120575119877119864) + (

ℎ minus 120575ℎ)

120575ℎ = minus120575V119863

(11)

where 119899119894119899is the computed angular rotation velocity of the

navigation coordinate system with respect to the inertialframe 119899

119894119890is the computed earth rotation velocity 119899

119890119899is the

computed rotation vector from the 119890-frame to the 119899-frametheir errors are 120575120596119899

119894119899 120575120596119899119894119890 and 120575120596

119899

119890119899 f119887119894119887is the specific force

that the accelerators measure k119899 119871 120582 and ℎ and 120575119871 120575120582 120575ℎand 120575k119899 are the computed velocity latitude longitude andheight and their errors respectively 119877119873 119877119864 and 120575119877119873 120575119877119864are the radius of the meridian and the prime vertical circleand their errors respectively [18]

And the SINS error model is written as

x = f (x 119905) + g (x 119905)w (12)

Now we cannot predict the SINS errors by transformmatrix As we presented in the next part the UT method isused to predict the system state

3 SINSGPS Integration Algorithm

31 Kalman Filter Developed in 1960s by Kalman KF hasbeen proved to be a powerful optimal estimation theory forlinear systems In SINSGPS integration KF is triggered ineveryGPSupdate epoch using themeasurement informationthat is the difference of the velocity and position betweenSINS and GPS Then the errors of SINS could be estimatedand corrected When GPS works well no SINS error is

4 Mathematical Problems in Engineering

accumulated and the navigation errors are bounded Thusthe linear SINS error model is accurate enough since thenonlinear parts could be ignored

The linear SINS error transition matrix is discretized asfollows [19]

Φ119896|119896minus1 asymp I + F119896minus1Δ119879 +

1

2

F2119896minus1

Δ119879

2

Q119896minus1

asymp G119896minus1qG119879

119896minus1Δ119879

+

1

2

(F119896minus1G119896minus1qG119879

119896minus1+ G119896minus1qG

119879

119896minus1F119879119896minus1

) Δ119879

2

(13)

where F(119905) is the coefficient matrix of the state equation Δ119879is the filter cycle q is the covariance matrix of SINS sensorsand G(119905) is the coefficient matrix of the state noise

For a loosely coupled SINSGNSS system the measure-ment equation is [20]

z = Hx + k (14)

where z is the difference of the velocity and position betweenSINS and GPS H = [I6times6 06times9] k is the noise standarddeviation vector

KF algorithm is committed by using such formulas [21]x119896|119896minus1 = Φ119896|119896minus1x119896minus1 (15)

P119896|119896minus1 = Φ119896|119896minus1P119896minus1Φ119879

119896|119896minus1+Q119896minus1 (16)

K119896 = P119896|119896minus1H119879

119896(H119896P119896|119896minus1H

119879

119896+ R119896)

minus1

(17)

x119896 = x119896|119896minus1 + K119896 (z119896 minusH119896x119896|119896minus1) (18)

P119896 = (I minus K119896H119896)P119896|119896minus1 (19)

In SINSGPS integration a close-loop configuration isused In every KF epoch when the SINS errors are correctedthe state x119896 is set to be zero

32 Unscented Kalman Filter As we mentioned above KFis only available when the state equation is linear because itis not feasible to predict the state by transition matrix for anonlinear system If we neglect the higher order terms KFwill introduce errors which cannot be ignored for a stronglynonlinear system [22] To predict the state of a nonlinearsystem UT is used in UKF by generating a series of samplepoints to simulate the transfer of the state which couldachieve an accuracy of three orders

In UT the state is predicted in three steps [23] Firstsigma points should be constructed

120594(0)

119896minus1= x119896minus1

120594(119894)

119896minus1= x119896minus1 + (radic(119899 + 120582)P119896minus1)

119888(119894)

119894 = 1 2 119899

120594(119894)

119896minus1= x119896minus1 minus (radic(119899 + 120582)P119896minus1)

119888(119894minus119899)

119894 = 119899 + 1 119899 + 2 2119899

(20)

where 119899 is the dimension of the state vector x 120582 = 120572

2(119899 +

120581) minus 119899 which decides the weights of the distribution of thesigma points and generally 120581 = 3 minus 119899 while 10minus4 le 120572 le

1 And (radic(119899 + 120582)P119896minus1)119888(119894) is the 119894 column of the Choleskydecomposition of the matrix (119899 + 120582)P119896minus1

Second the states are predicted with every sigma pointThis step is executed by solving the error state differentialequations with four-order Runge-Kutta method

120585(119894)

119896|119896minus1= 119891 (120594

(119894)

119896minus1) 119894 = 1 2 2119899 (21)

Finally the states are weighted and summarized so weobtain the predicted state x119896|119896minus1 and the error covariancematrix P119896|119896minus1

x119896|119896minus1 =2119899

sum

119894=0

119882

(119898)

119894120585(119894)

119896|119896minus1

P119896|119896minus1 =2119899

sum

119894=0

119882

(119888)

119894[(120585(119894)

119896|119896minus1minus x119896|119896minus1) (120585

(119894)

119896|119896minus1minus x119896|119896minus1)

119879

]

+Q119896minus1

(22)

where Q119896minus1 is the covariance matrix of the state noise119882(119898)119894

and119882(119888)119894

are the weights of the sigma points which could becalculated as

119882

(119898)

0=

120582

119899 + 120582

119882

(119888)

0=

120582

119899 + 120582

+ 1 minus 120572

2+ 120573

119882

(119898)

119894= 119882

(119888)

119894=

120582

2 (119899 + 120582)

119894 = 1 2 2119899

(23)

where 120572 and 120582 have been given in the first step and 120573 isassigned according to the distribution character of the stateerror In this case 120573 = 2

In loosely coupled SINGPS the measurement equationis linear as we saw in (14) So after we predict the state x119896|119896minus1and the error covariance matrix P119896|119896minus1 we could estimate theSINS errors with formulas (17) to (19)

33 Hybrid KF-UKF Algorithm In this part we discussed thearchitecture of the hybrid KF-UKF algorithm As shown inFigure 1 SINS information is updated together with GPSWhenGPS information is received its availability is judged sothat whichmethod is to going be executed can be determined

If GPS information is available SINS and GPS can beintegrated by normal KF method as shown in Figure 2 Firstlinear SINS error equation (6) is discretized by formula (13)with SINS information at time 119905 = 119896 minus 1 Then one-step stateprediction and error covariance prediction are calculatedwith formulas (15) and (16) and the filter gain is calculatedwith formula (17) Afterwards the state vector which is theSINS error and error covariance at time 119905 = 119896 are estimatedwith GPS information at 119905 = 119896 Finally the SINS errorestimated at 119905 = 119896 is feedback to SINS correcting the SINSerror and the same procedure is committed in the next cycle

Mathematical Problems in Engineering 5

Velocity position Attitude velocity position angular rate and acceleration

GPS information SINS information

GPS available

One-step state prediction

No

Error estimation

Yes

Correct SINSerror

at t = k at t = k minus 1

Estimate SINSerrors xk by KF

Predict state xk|kminus1 andcovariance Pk|kminus1 by UT

k = k + 1

minus

Figure 1 The architecture of the hybrid KF-UKF algorithm

SINS update

One-step prediction

SINSinformation

CorrectSINS

Filter update

CalculateΦk|kminus1 Qkminus1

with F(t) G(t)

xk|kminus1Pk|kminus1

Calculate filter gain Kk

State estimate xkError covariance Pk rGPS k

t = k minus 1

t = kvGPS k

Figure 2 Filter updates procedure when GPS is functioning well

Table 1 Main specifications of experiment instruments in thesimulation test

Equipment Parameters

GPS receiverVelocity accuracy 01ms (1120590)

Position accuracy 5m (horizontal)10m (vertical)

Output frequency 10Hz

MEMS-IMU

GyroscopeBias 50∘h (1120590)White noise 35∘hradicHz

AccelerometerBias 2mg (1120590)White noise 03mgradicHzOutput frequency 200Hz

During GPS signal blockage the GPS information isunavailable and one-step state prediction is executed withoutmeasurement updateNow the nonlinear SINS error equationand UT are applied to predict the state and error covarianceAs shown in Figure 3 let us assume that theGPS signal breaksoff at 119905 = 119896 and recovers at 119905 = 119896 + 119899 In order to estimateSINS error at 119905 = 119896 + 119899 it is necessary to predict the statex119896+119899|119896minus1 and error covariance P119896+119899|119896minus1 Suppose that 119896 + 119904 is atime instant in GPS outage and x119896+119904|119896minus1P119896+119904|119896minus1 are iterated

Table 2 Aircraft maneuvers in the test

Stage Period Aircraft maneuver Parameters1 0sim30 s Stationary v = 0ms2 30sim60 s Acceleration v = 0ms to v = 100ms3 60sim75 s Pitching 120579 = 0∘ to 120579 = 30∘

4 75sim95 s Straight flight v = 100ms5 95sim120 s Pitching 120579 = 30∘ to 120579 = 0∘

6 120sim140 s Straight flight v = 100ms7 140sim165 s Coordinate turn 120595 = 0∘ to 120595 = 40∘

8 165sim190 s Straight flight v = 100ms9 190sim215 s Coordinate turn 120595 = 40∘ to 120595 = 0∘

10 215sim300 s Straight flight v = 100ms

by using x119896+119904minus1|119896minus1P119896+119904minus1|119896minus1 and SINS information at time119905 = 119896 + 119904 minus 1 119905 = 119896 + 119904 minus 05 and 119905 = 119896 + 119904 with formulas (24)to (27) This calculation is executed cycle by cycle until GPSsignal recovers at 119905 = 119896 + 119899 When GPS receiver offered thevelocity andposition information of the vehicle at 119905 = 119896+119899 wealready have the state x119896+119899|119896minus1 and error covariance P119896+119899|119896minus1Then we can estimate SINS error at 119905 = 119896 + 119899 with formulas(18) and (19) correct it and go into the next filter cycle

120594(0)

119896+119904minus1|119896minus1= x119896+119904minus1|119896minus1

120594(119894)

119896+119904minus1|119896minus1= x119896+119904minus1|119896minus1 + (radic(119899 + 120582)P119896+119904minus1|119896minus1)

119888(119894)

119894 = 1 2 119899

120594(119894)

119896+119904minus1|119896minus1= x119896+119904minus1|119896minus1 minus (radic(119899 + 120582)P119896+119904minus1|119896minus1)

119888(119894minus119899)

119894 = 119899 + 1 119899 + 2 2119899

(24)

120585119896+119904|119896minus1 = 119891 (120594119896+119904minus1|119896minus1

) 119894 = 1 2 2119899 (25)

x119896+119904|119896minus1 =2119899

sum

119894=0

119882

(119898)

119894120585(119894)

119896+119904|119896minus1 (26)

P119896+119904|119896minus1 =2119899

sum

119894=0

119882

(119888)

119894[(120585(119894)

119896+119904|119896minus1minus x119896+119904|119896minus1)

sdot (120585(119894)

119896+119904|119896minus1minus x119896+119904|119896minus1)

119879

] +Q119896minus1

(27)

4 Simulation Test

41 Simulation Description and Parameters For the sake oftesting the hybrid KF-UKF algorithm we design a simulationcomparing this algorithm with normal EKF algorithm Theschematic diagram of the simulation is shown in Figure 4 Byusing a path generator we calculate the acceleration angularrate velocity position and attitude information of an aircraftStochastic errors were added to the acceleration and angularrate data to simulate the IMU sensor error And the sameprocess is done to the velocity and position information tosimulate the GPS error Besides GPS outage flag is added to

6 Mathematical Problems in Engineering

One-step predictionand SINS information

One-step prediction

GPS signalbreaks off

SINS update Filter update

CorrectSINS

GPS receiver

GPS signalrecovers

and SINS information

SINSinformation

One-step prediction

State estimate xk+n

CalculateΦk|kminus1 Qkminus1

with F(t) G(t)

xk|kminus1Pk|kminus1

xk+n|kminus1Pk+n|kminus1

Calculate 120585k+n|kminus1 with 120594k+nminus1|kminus1

Calculate 120594k+nminus1|kminus1 withxk+nminus1|kminus1Pk+nminus1|kminus1

xk+s|kminus1Pk+s|kminus1

Calculate 120585k+s|kminus1 with 120594k+sminus1|kminus1

Calculate 120594k+sminus1|kminus1 withxk+sminus1|kminus1Pk+sminus1|kminus1

t = k minus 1

t = k

middot middot middotmiddot middot middot

middot middot middotmiddot middot middot

t = k + s minus 1

t = k + s minus 05

t = k + s

t = k + n minus 1

t = k + n minus 05

t = k + n

rGPS k+n

vGPS k+n Error covariance Pk+n

Figure 3 Filter updates procedure during GPS outage

Table 3 Comparison of navigation error between EKF and hybrid KF-UKF after GPS outage

Outages length Adopted approach RMS attitude error (∘) RMS velocity error (ms) RMS position error (m)Roll Pitch Yaw North East Down Latitude Longitude Height

50 s EKF 00451 00566 02605 00256 00233 00331 11990 08245 33450Hybrid KF-UKF 00438 00549 02499 00255 00233 00331 11984 08199 32310

100 s EKF 00553 01050 02630 00286 00273 00367 11832 37463 30911Hybrid KF-UKF 00508 00767 01706 00240 00259 00339 11701 37434 39175

150 s EKF 00616 00590 01282 00363 00341 00479 13913 22053 31303Hybrid KF-UKF 00489 00367 01140 00219 00237 00333 13876 21949 39078

200 s EKF 00969 00560 17882 00431 00544 00619 15559 39014 36364Hybrid KF-UKF 00586 00403 17131 00231 00318 00360 15552 38998 35416

Path generator

Integration algorithm

IMU stochastic error

GPS stochastic errorGPS outage flag

Results comparison

a 120596 v r 120595

aIMU 120596IMU vGPS rGPS

vnav rnav120595nav

120575v

120575r

120575120595

Figure 4 Schematic diagram of the simulation

GPS data so the integration algorithm is able to judgewhetherGPS signal is lost Finally velocity position and attitude data

calculated by hybrid KF-UKF and normal EKF algorithm arecompared The simulation parameters are shown in Table 1

42 Simulation Results and Analysis In the simulation testthe aircraft has done a series of maneuvers which are listed inTable 2 and its acceleration angular rate velocity positionand attitude information was recorded We calculate thenavigation parameters of the aircraft by integrating SINS andGPS using the normal EKF and hybrid KF-UKF respectivelyThen the results were compared as is shown in Figure 5 andTable 3

Figure 5 is the comparison of attitude error curves of EKFand hybrid KF-UKF which are taken for example As weobserve the GPS signal was lost at 119905 = 70 s and recovered

Mathematical Problems in Engineering 7

270 275minus04

minus02

0

02

04

270 272 274minus02

minus01

0

Hybrid KF-UKFEKF

Hybrid KF-UKFEKF

GPS recovery timeGPS recovery time

GPS lost time GPS lost time

minus05

0

05

1

15

Roll

erro

r (∘)

minus05

0

05

1

Pitc

h er

ror (

∘)

100 150 200 250 30050

t (s)100 150 200 250 30050

t (s)

Figure 5 Comparison of attitude error between EKF and hybrid KF-UKF

270 271 272minus04

minus02

0

02

270 271 272minus01

001020304

Hybrid KF-UKFEKF

Hybrid KF-UKFEKF

GPS lost timeGPS lost time

GPS recovery time

GPS recovery time

minus12

minus10

minus8

minus6

minus4

minus2

02

Nor

th v

eloci

ty er

ror (

ms

)

100 150 200 250 30050t (s)

0

5

10

15

East

velo

city

erro

r (m

s)

100 150 200 250 30050t (s)

Figure 6 Comparison of velocity error between EKF and hybrid KF-UKF

at 119905 = 270 s During the 200 seconds GPS outage the rolland pitch angle error grew with time because the accuracy ofMEMS inertial sensors is very low and no GPS measurementinformation could be used for the filter to correct the SINSerror When the navigation system received GPS signal againat 119905 = 270 s the filter started to correct SINS error againIn the figure we can notice that after GPS information wasrecovered the attitude error reduced quickly However theattitude calculated by using hybrid KF-UKF is more accuratethan that calculated by EKF which is shown in the circle inthe figure This phenomenon appears for the reason that aswe described before the MEMS-IMU sensor caused largenonlinear error during GPS outage Based on the nonlinearSINS error model UT is able to predict SINS errors betterthan EKF thus the filter can correct the SINS errors quicklyand accurately And the similar phenomenon also occurs tothe velocity which is shown in Figure 6

Table 3 is the comparison of navigation error betweenEKF and hybrid KF-UKF after GPS outage For attitude errorwe can notice that the error of yaw is larger than that of rolland pitch owing to the poor observability of yawing angle invelocityposition integrationmode thus the filter is unable toestimate its error correctly By comparing the RMS attitudeerror and velocity error after GPS outage it is shown that thenavigation error is lower if the hybrid KF-UKF algorithm isapplied Also it is shown in Table 3 that with the GPS outageperiod increases the hybrid KF-UKF algorithm was betterand better compared to the EKF algorithm That is becausethe error of an inertial navigation system accumulates withthe increase The longer the GPS outage is the larger the

nonlinear error the MEMS-IMU causes And the hybridKF-UKF algorithm will show more superiority to normalEKF algorithm We can also notice that the latitude andlongitude accuracy has little improvement on the contrary toattitude and velocity And this can be explained by examiningthe SINS error equations (9) to (11) In formulas (9) and(10) the attitude error and velocity error are affected by thenonlinear terms C119899

1015840

119899and Cminus1

120595whose values grew fast during

GPS outage However when we refer to formula (11) wecan see that the numerical value of 120575119871 120575119877119864 and 120575119877119873 isvery small and little nonlinear error is caused during GPSoutage because the aircraft did notmove quite far away duringthe simulation so the EKF is still able to correct its erroraccurately when GPS signal recovers

In Table 4 we list the calculation amounts of EKF andUKF so that we can estimate the computational cost of theproposed combined algorithm The calculation amounts ofthe algorithm are evaluated using the floating-point opera-tions (flops) which is defined as the operation of addingdecreasing multiplying or dividing between two floatingnumbers For example if we add one (119899 times 119898) dimensionmatrix to another (119899 times 119898) dimension matrix then 119899119898 flopshave been generated in the computer In Table 4 we supposethe dimension of the state vector is 119899 and the measurementvector dimension is 119897 During the prediction updating period4119899

3+119899

2minus119899 flops are generated in EKFwhile 6(23)1198993+121198992+

3119899 flops are generated in UKF In SINSGNSS integration119899 = 15 and 119897 = 6 So there will be 13710 flops in EKF and25245 flops in UKF during the prediction updating periodWhat is more in Unscented Transform the state prediction

8 Mathematical Problems in Engineering

Table 4 Comparison of computational cost of EKF and UKF

EKF UKF (for linear measurement equation)Step Flops Step Flops

Prediction update

Generate sigmapoints120594119896minus1

23119899

3+ 3119899

2+ 119899

State predictionx119896|119896minus1

2119899

2minus 119899

State predictionx119896|119896minus1

4119899

2+ 119899

Error covarianceprediction P119896|119896minus1

4119899

3minus 119899

2

Error covarianceprediction

P119896|119896minus16119899

3+ 5119899

2+ 119899

Subtotal 4119899

3+ 119899

2minus 119899 Subtotal 6(23)119899

3+ 12119899

2+ 3119899

Measurement update

Filter gainK119896

4119899

2119897 + 4119899119897

2+ 119897

3minus 3119899119897

Filter gainK119896

4119899

2119897 + 4119899119897

2+ 119897

3minus 3119899119897

State updatex119896

4119899119897

State updatex119896

4119899119897

Error covariance updateP119896

2119899

3+ 2119899

2119897 minus 119899

2

Error covarianceupdateP119896

2119899

3+ 2119899

2119897 minus 119899

2

Subtotal 2119899

3+6119899

2119897+4119899119897

2+119897

3+119899119897minus119899

2 Subtotal 2119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+ 119899119897 minus 119899

2

In total 6119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+119899119897 minus 119899 In total 8(23)119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+ 11119899

2+

119899119897 + 3119899

119899 = 15 119897 = 6 30801 119899 = 15 l = 6 42336

is performed by solving the error state differential equationswith Runge-Kutta method (2119899 + 1) times since each sigmapoint has to be predictedThat is to say more than 25245 flopsare generated in one filter recycle during GPS outages whenwe predict the SINS error using Unscented Transform So infact the computational cost of UKF is much larger than thatin EKF in the prediction updating part thus the hybrid UKF-EKF is recommended to reduce the computational burden forthe computer In hybridUKF-EKF theUnscented Transformwhich is necessary to predict the nonlinear SINS error onlyperforms during GPS outages Last but not least the amountof flops in EKF during the measurement updating period is2119899

3+6119899

2119897+4119899119897

2+119897

3+119899119897minus119899

2 in this case 17091 as well as thatin UKFThis is for the reason that in SINSGNSS integrationthe measurement equation is linear and no sigma point isneeded in the measurement updating part When GPS signalis functioning well there will be 30801 flops in each filtercycle

5 Conclusion

This paper presents a hybrid KF-UKF algorithm for real-timeMEMS-SINSGPS integrationThe linear and nonlinearSINS models are discussed The flowchart of the hybrid KF-UKF algorithm is described The SINS error is estimated andcorrected with linear SINS model when GPS is functioningwell while it is predicted with nonlinear SINS model byapplying Unscented Transform during GPS outage Thesimulation results indicate that compared to normal EKFalgorithm the hybrid KF-UKF algorithm is able to predictthe SINS error more accurately if GPS suffers from long-time GPS outage and the navigation error is lower after GPS

signal was regainedThe results also show that the hybrid KF-UKF algorithm is more efficient for attitude error predictionbut the effect on position error is not evident Comparedwith normal UKF the hybrid KF-UKF algorithm has lowcalculation amount In this paper the remaining problemwhich we have not solved is that SINS errors still grow fastduring GPS outage So in our future work we may combinethe UKF with ANN or SVM to create a new algorithm toreduce the SINS errors during GPS outage

Conflict of Interests

The authors declare that there is no conflict of interestsregarding the publication of this paper

Acknowledgments

This research is supported by the 3rd Innovation Fund ofChangchun Institute of Optics Fine Mechanics and Physics(CIOMP) and the UAV semi-physical simulation platformresearch project Chinese Science Academy (CSA)

References

[1] P D Groves Principles of GNSS Inertial and MultisensorIntegrated Navigation Systems Artech House Norwood MassUSA 2008

[2] Y Yuksel N El-Sheimy andANoureldin ldquoErrormodeling andcharacterization of environmental effects for low cost inertialMEMS unitsrdquo in Proceedings of the IEEEION Position Locationand Navigation Symposium pp 598ndash612 Indian Wells CalifUSA May 2010

Mathematical Problems in Engineering 9

[3] P Aggarwal Z Syed X Niu and N El-Sheimy ldquoA standardtesting and calibration procedure for low cost MEMS inertialsensors and unitsrdquo The Journal of Navigation vol 61 no 2 pp323ndash336 2008

[4] S Godha and M E Cannon ldquoGPSMEMS INS integratedsystem for navigation in urban areasrdquo GPS Solutions vol 11 no3 pp 193ndash203 2007

[5] M Ilyas Y Yang Q S Qian and R Zhang ldquoLow-cost IMUodometerGPS integrated navigation aided with two antennaeheading measurement for land vehicle applicationrdquo in Proceed-ings of the 25th Chinese Control andDecision Conference (CCDCrsquo13) pp 4521ndash4526 Guiyang China May 2013

[6] Z Jiang C Liu G Zhang Y P Wang C Huang and J LiangldquoGPSINS integrated navigation based on UKF and simulatedannealing optimized SVMrdquo in Proceedings of the IEEE 78thVehicular Technology Conference (VTC Fall rsquo13) vol 14 pp 1ndash5 Las Vegas Nev USA September 2013

[7] L Chen and J Fang ldquoA hybrid prediction method for bridgingGPS outages in high-precision POS applicationrdquo IEEE Transac-tions on Instrumentation and Measurement vol 63 no 6 pp1656ndash1665 2014

[8] A M Hasan K Samsudin A R Ramli and R S AzmirldquoAutomatic estimation of inertial navigation system errors forglobal positioning system outage recoveryrdquo Proceedings of theInstitution of Mechanical Engineers Part G Journal of AerospaceEngineering vol 225 no 1 pp 86ndash96 2011

[9] X Chen C Shen W-B Zhang M Tomizuka Y Xu andK Chiu ldquoNovel hybrid of strong tracking Kalman filter andwavelet neural network for GPSINS during GPS outagesrdquoMeasurement vol 46 no 10 pp 3847ndash3854 2013

[10] S J Julier and J K Uhlmann ldquoA new approach for filteringnonlinear systemrdquo in Proceedings of the American ControlConference vol 3 pp 1628ndash1632 IEEE SeattleWashUSA June1995

[11] P-B Ma H-X Baoyin and J-S Mu ldquoAutonomous navigationof Mars probe based on optical observation of Martian moonrdquoOptics and Precision Engineering vol 22 no 4 pp 863ndash8692014

[12] E Shin Estimation Techniques for Low-Cost Inertial NavigationThe University of Calgary Calgary Canada 2005

[13] Y Yi On improving the accuracy and reliability of GPSINS-based direct sensor georeferencing [PhD thesis] Ohio StateUniversity Columbus Ohio USA 2007

[14] Y Hao Z Xiong F Sun and X Wang ldquoComparison ofunscented Kalman filtersrdquo in Proceedings of the IEEE Interna-tional Conference on Mechatronics and Automation pp 895ndash899 Harbin China August 2007

[15] R M Rogers Applied Mathematics in Integrated NavigationSystems American Institute of Aeronautics and AstronauticsReston Va USA 3rd edition 2007

[16] D H Titterton and J L Weston Strapdown Inertial NavigationTechnology The Institution of Electrical Engineers MichaelFaraday House Stevenage UK 2nd edition 2004

[17] G Yan W Yan and D Xu ldquoA SINS nonlinear error modelreflecting better characteristics of SINS errorsrdquo Journal ofNorthwestern Polytechnical University vol 27 no 4 pp 511ndash5162009

[18] G M Yan W S Yan and D M Xu ldquoApplication of simplifiedUKF in SINS initial alignment for large misalignment anglesrdquoJournal of Chinese Inertial Technology vol 16 no 3 pp 253ndash2642008

[19] P S Maybeck Stochastic Models Estimation and ControlAcademic Press New York NY USA 1979

[20] S Liu Research and implementation of GPSINS integrated nav-igation algorithm [MS thesis] PLA Information EngineeringUniversity Zhengzhou China 2012 (Chinese)

[21] M S Grewal and A P Andrews Kalman Filtering Theory andPractice Using MATLAB John Wiley amp Sons Hoboken NJUSA 3rd edition 2008

[22] M C Vandyke J L Schwartz and C D Hall ldquoUnscentedKalman filtering for spacecraft attitude state and parameterestimationrdquo Advances in the Astronautical Sciences vol 119 no1 pp 217ndash228 2005

[23] Y Y Qin H Y Zhang and S S Wang Theory of KalmanFilter and Integrated Navigation North-Western PolytechnicalUniversity Press Xirsquoan China 2012 (Chinese)

Submit your manuscripts athttpwwwhindawicom

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical Problems in Engineering

Hindawi Publishing Corporationhttpwwwhindawicom

Differential EquationsInternational Journal of

Volume 2014

Applied MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical PhysicsAdvances in

Complex AnalysisJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

OptimizationJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Operations ResearchAdvances in

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Function Spaces

Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of Mathematics and Mathematical Sciences

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Algebra

Discrete Dynamics in Nature and Society

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Decision SciencesAdvances in

Discrete MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom

Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Stochastic AnalysisInternational Journal of

Mathematical Problems in Engineering 3

where C1198991015840

119887is the direction cosine matrix (DCM) from the

body-frame (119887-frame) to the computed navigation frame (1198991015840-frame) C119899

119887is the DCM from 119887-frame to the real navigation

frame (119899-frame) C1198991015840

119899is the DCM from 119899-frame to 1198991015840-frame

[15]

22 Linear SINS Error Model KF is only able to estimatethe state when the system is linear Fortunately when SINSintegrated with GPS the SINS errors can be corrected everyfiltering period And SINS errors accumulated in this periodare pretty small So we can neglect the higher order termsof the SINS error function and the model is approximatelylinear

The attitude error is approximated as

C1198991015840

119899asymp I minus [120595times] (4)

The attitude the velocity and the position error equa-tions respectively are

asymp minus120596119899

119894119899times 120595 + 120575120596

119899

119894119899minus C119899119887120575120596119887

119894119887+ C119899119887w119887119892

120575k119899 asymp f119899119894119887times 120595 minus (2120596

119899

119894119890+ 120596119899

119890119899) times 120575k119899 + 120575g119899 + C119899

119887120575f119887119894119887

+ C119899119887w119887119886

120575 r119899 asymp minus120596119899

119890119899times 120575r119899 + 120575k119899

(5)

where 120596119899119894119899is the angular rotation velocity of the navigation

coordinate systemwith respect to the inertial frame120596119899119894119890is the

earth rotation velocity 120596119899119890119899is the rotation vector from the 119890-

frame to the 119899-frame f119899119894119887is the specific force vector in 119899-frame

120575g119899 is the error of the gravity vector in 119899-framew119887119892andw119887

119886are

the noise of the gyroscopes and accelerators respectively [16]And the SINS error model can be written as

x = F (119905) x + G (119905)w (6)

So it is able to predict the system state by using atransform matrix And KF is available when GPS works well

23 Nonlinear SINS Error Model Although the linear SINSmodel has been proved to be efficient in SINSGPS inte-gration it may be not accurate enough if the SINS errorsaccumulate with time when the navigation system works inSINS alone mode In this situation we cannot neglect thenonlinear parts of the SINS error function and it is necessaryto develop the nonlinear SINS error model

Now let us review (3) If we define 1205961198991015840

1198991198991015840as the angular

rotation vector from the 119899-frame to the 1198991015840-frame and =

[119909 119910 119911]119879 as the Euler angle rates then we have their

relationship with the Euler angle

1205961198991015840

1198991198991015840=

[

[

[

119909

0

0

]

]

]

+ C119883[

[

[

0

119910

0

]

]

]

+ C119883C119884[

[

[

0

0

119911

]

]

]

(7)

Without linearization the attitude error differential equa-tion can be derived from (7) as

= Cminus11205951205961198991015840

1198991198991015840=

[

[

[

[

1 0 minus sin1205951199100 cos120595119909 sin120595119909 cos1205951199100 minus sin120595119909 cos120595119909 cos120595119910

]

]

]

]

minus1

1205961198991015840

1198991198991015840 (8)

where C120595 is the transition matrix between 119899-frame and 119899

1015840-frame [17]

Based on (8) the attitude the velocity and the positionerror equations are expressed as

= Cminus1120595[(119868 minus C119899

1015840

119899) 119899

119894119899+ C119899

1015840

119899120575120596119899

119894119899minus C119899

1015840

119887120575120596119887

119894119887]

+ Cminus1120595C1198991015840

119887w119887119892

(9)

120575k119899 = (I minus C1198991198991015840)C1198991015840

119887f119887119894119887minus (2

119899

119894119890+ 119899

119890119899) times 120575k119899

minus (2120575120596119899

119894119890+ 120575120596119899

119890119899) times k119899 + (2120575120596119899

119894119890+ 120575120596119899

119890119899)

times 120575k119899 + 120575g119899 + C1198991198991015840C1198991015840

119887120575f119887119894119887+ C1198991198991015840C1198991015840

119887w119887119886

(10)

120575119871 =

V119873119877119873 +

ℎ

minus

V119873 minus 120575V119873119877119873 minus 120575119877119873 +

ℎ minus 120575ℎ

120575120582 =

V119864sec 119871(119877119864 +

ℎ)

minus

(V119864 minus 120575V119864) sec (119871 minus 120575119871)

(119877119864 minus 120575119877119864) + (

ℎ minus 120575ℎ)

120575ℎ = minus120575V119863

(11)

where 119899119894119899is the computed angular rotation velocity of the

navigation coordinate system with respect to the inertialframe 119899

119894119890is the computed earth rotation velocity 119899

119890119899is the

computed rotation vector from the 119890-frame to the 119899-frametheir errors are 120575120596119899

119894119899 120575120596119899119894119890 and 120575120596

119899

119890119899 f119887119894119887is the specific force

that the accelerators measure k119899 119871 120582 and ℎ and 120575119871 120575120582 120575ℎand 120575k119899 are the computed velocity latitude longitude andheight and their errors respectively 119877119873 119877119864 and 120575119877119873 120575119877119864are the radius of the meridian and the prime vertical circleand their errors respectively [18]

And the SINS error model is written as

x = f (x 119905) + g (x 119905)w (12)

Now we cannot predict the SINS errors by transformmatrix As we presented in the next part the UT method isused to predict the system state

3 SINSGPS Integration Algorithm

31 Kalman Filter Developed in 1960s by Kalman KF hasbeen proved to be a powerful optimal estimation theory forlinear systems In SINSGPS integration KF is triggered ineveryGPSupdate epoch using themeasurement informationthat is the difference of the velocity and position betweenSINS and GPS Then the errors of SINS could be estimatedand corrected When GPS works well no SINS error is

4 Mathematical Problems in Engineering

accumulated and the navigation errors are bounded Thusthe linear SINS error model is accurate enough since thenonlinear parts could be ignored

The linear SINS error transition matrix is discretized asfollows [19]

Φ119896|119896minus1 asymp I + F119896minus1Δ119879 +

1

2

F2119896minus1

Δ119879

2

Q119896minus1

asymp G119896minus1qG119879

119896minus1Δ119879

+

1

2

(F119896minus1G119896minus1qG119879

119896minus1+ G119896minus1qG

119879

119896minus1F119879119896minus1

) Δ119879

2

(13)

where F(119905) is the coefficient matrix of the state equation Δ119879is the filter cycle q is the covariance matrix of SINS sensorsand G(119905) is the coefficient matrix of the state noise

For a loosely coupled SINSGNSS system the measure-ment equation is [20]

z = Hx + k (14)

where z is the difference of the velocity and position betweenSINS and GPS H = [I6times6 06times9] k is the noise standarddeviation vector

KF algorithm is committed by using such formulas [21]x119896|119896minus1 = Φ119896|119896minus1x119896minus1 (15)

P119896|119896minus1 = Φ119896|119896minus1P119896minus1Φ119879

119896|119896minus1+Q119896minus1 (16)

K119896 = P119896|119896minus1H119879

119896(H119896P119896|119896minus1H

119879

119896+ R119896)

minus1

(17)

x119896 = x119896|119896minus1 + K119896 (z119896 minusH119896x119896|119896minus1) (18)

P119896 = (I minus K119896H119896)P119896|119896minus1 (19)

In SINSGPS integration a close-loop configuration isused In every KF epoch when the SINS errors are correctedthe state x119896 is set to be zero

32 Unscented Kalman Filter As we mentioned above KFis only available when the state equation is linear because itis not feasible to predict the state by transition matrix for anonlinear system If we neglect the higher order terms KFwill introduce errors which cannot be ignored for a stronglynonlinear system [22] To predict the state of a nonlinearsystem UT is used in UKF by generating a series of samplepoints to simulate the transfer of the state which couldachieve an accuracy of three orders

In UT the state is predicted in three steps [23] Firstsigma points should be constructed

120594(0)

119896minus1= x119896minus1

120594(119894)

119896minus1= x119896minus1 + (radic(119899 + 120582)P119896minus1)

119888(119894)

119894 = 1 2 119899

120594(119894)

119896minus1= x119896minus1 minus (radic(119899 + 120582)P119896minus1)

119888(119894minus119899)

119894 = 119899 + 1 119899 + 2 2119899

(20)

where 119899 is the dimension of the state vector x 120582 = 120572

2(119899 +

120581) minus 119899 which decides the weights of the distribution of thesigma points and generally 120581 = 3 minus 119899 while 10minus4 le 120572 le

1 And (radic(119899 + 120582)P119896minus1)119888(119894) is the 119894 column of the Choleskydecomposition of the matrix (119899 + 120582)P119896minus1

Second the states are predicted with every sigma pointThis step is executed by solving the error state differentialequations with four-order Runge-Kutta method

120585(119894)

119896|119896minus1= 119891 (120594

(119894)

119896minus1) 119894 = 1 2 2119899 (21)

Finally the states are weighted and summarized so weobtain the predicted state x119896|119896minus1 and the error covariancematrix P119896|119896minus1

x119896|119896minus1 =2119899

sum

119894=0

119882

(119898)

119894120585(119894)

119896|119896minus1

P119896|119896minus1 =2119899

sum

119894=0

119882

(119888)

119894[(120585(119894)

119896|119896minus1minus x119896|119896minus1) (120585

(119894)

119896|119896minus1minus x119896|119896minus1)

119879

]

+Q119896minus1

(22)

where Q119896minus1 is the covariance matrix of the state noise119882(119898)119894

and119882(119888)119894

are the weights of the sigma points which could becalculated as

119882

(119898)

0=

120582

119899 + 120582

119882

(119888)

0=

120582

119899 + 120582

+ 1 minus 120572

2+ 120573

119882

(119898)

119894= 119882

(119888)

119894=

120582

2 (119899 + 120582)

119894 = 1 2 2119899

(23)

where 120572 and 120582 have been given in the first step and 120573 isassigned according to the distribution character of the stateerror In this case 120573 = 2

In loosely coupled SINGPS the measurement equationis linear as we saw in (14) So after we predict the state x119896|119896minus1and the error covariance matrix P119896|119896minus1 we could estimate theSINS errors with formulas (17) to (19)

33 Hybrid KF-UKF Algorithm In this part we discussed thearchitecture of the hybrid KF-UKF algorithm As shown inFigure 1 SINS information is updated together with GPSWhenGPS information is received its availability is judged sothat whichmethod is to going be executed can be determined

If GPS information is available SINS and GPS can beintegrated by normal KF method as shown in Figure 2 Firstlinear SINS error equation (6) is discretized by formula (13)with SINS information at time 119905 = 119896 minus 1 Then one-step stateprediction and error covariance prediction are calculatedwith formulas (15) and (16) and the filter gain is calculatedwith formula (17) Afterwards the state vector which is theSINS error and error covariance at time 119905 = 119896 are estimatedwith GPS information at 119905 = 119896 Finally the SINS errorestimated at 119905 = 119896 is feedback to SINS correcting the SINSerror and the same procedure is committed in the next cycle

Mathematical Problems in Engineering 5

Velocity position Attitude velocity position angular rate and acceleration

GPS information SINS information

GPS available

One-step state prediction

No

Error estimation

Yes

Correct SINSerror

at t = k at t = k minus 1

Estimate SINSerrors xk by KF

Predict state xk|kminus1 andcovariance Pk|kminus1 by UT

k = k + 1

minus

Figure 1 The architecture of the hybrid KF-UKF algorithm

SINS update

One-step prediction

SINSinformation

CorrectSINS

Filter update

CalculateΦk|kminus1 Qkminus1

with F(t) G(t)

xk|kminus1Pk|kminus1

Calculate filter gain Kk

State estimate xkError covariance Pk rGPS k

t = k minus 1

t = kvGPS k

Figure 2 Filter updates procedure when GPS is functioning well

Table 1 Main specifications of experiment instruments in thesimulation test

Equipment Parameters

GPS receiverVelocity accuracy 01ms (1120590)

Position accuracy 5m (horizontal)10m (vertical)

Output frequency 10Hz

MEMS-IMU

GyroscopeBias 50∘h (1120590)White noise 35∘hradicHz

AccelerometerBias 2mg (1120590)White noise 03mgradicHzOutput frequency 200Hz

During GPS signal blockage the GPS information isunavailable and one-step state prediction is executed withoutmeasurement updateNow the nonlinear SINS error equationand UT are applied to predict the state and error covarianceAs shown in Figure 3 let us assume that theGPS signal breaksoff at 119905 = 119896 and recovers at 119905 = 119896 + 119899 In order to estimateSINS error at 119905 = 119896 + 119899 it is necessary to predict the statex119896+119899|119896minus1 and error covariance P119896+119899|119896minus1 Suppose that 119896 + 119904 is atime instant in GPS outage and x119896+119904|119896minus1P119896+119904|119896minus1 are iterated

Table 2 Aircraft maneuvers in the test

Stage Period Aircraft maneuver Parameters1 0sim30 s Stationary v = 0ms2 30sim60 s Acceleration v = 0ms to v = 100ms3 60sim75 s Pitching 120579 = 0∘ to 120579 = 30∘

4 75sim95 s Straight flight v = 100ms5 95sim120 s Pitching 120579 = 30∘ to 120579 = 0∘

6 120sim140 s Straight flight v = 100ms7 140sim165 s Coordinate turn 120595 = 0∘ to 120595 = 40∘

8 165sim190 s Straight flight v = 100ms9 190sim215 s Coordinate turn 120595 = 40∘ to 120595 = 0∘

10 215sim300 s Straight flight v = 100ms

by using x119896+119904minus1|119896minus1P119896+119904minus1|119896minus1 and SINS information at time119905 = 119896 + 119904 minus 1 119905 = 119896 + 119904 minus 05 and 119905 = 119896 + 119904 with formulas (24)to (27) This calculation is executed cycle by cycle until GPSsignal recovers at 119905 = 119896 + 119899 When GPS receiver offered thevelocity andposition information of the vehicle at 119905 = 119896+119899 wealready have the state x119896+119899|119896minus1 and error covariance P119896+119899|119896minus1Then we can estimate SINS error at 119905 = 119896 + 119899 with formulas(18) and (19) correct it and go into the next filter cycle

120594(0)

119896+119904minus1|119896minus1= x119896+119904minus1|119896minus1

120594(119894)

119896+119904minus1|119896minus1= x119896+119904minus1|119896minus1 + (radic(119899 + 120582)P119896+119904minus1|119896minus1)

119888(119894)

119894 = 1 2 119899

120594(119894)

119896+119904minus1|119896minus1= x119896+119904minus1|119896minus1 minus (radic(119899 + 120582)P119896+119904minus1|119896minus1)

119888(119894minus119899)

119894 = 119899 + 1 119899 + 2 2119899

(24)

120585119896+119904|119896minus1 = 119891 (120594119896+119904minus1|119896minus1

) 119894 = 1 2 2119899 (25)

x119896+119904|119896minus1 =2119899

sum

119894=0

119882

(119898)

119894120585(119894)

119896+119904|119896minus1 (26)

P119896+119904|119896minus1 =2119899

sum

119894=0

119882

(119888)

119894[(120585(119894)

119896+119904|119896minus1minus x119896+119904|119896minus1)

sdot (120585(119894)

119896+119904|119896minus1minus x119896+119904|119896minus1)

119879

] +Q119896minus1

(27)

4 Simulation Test

41 Simulation Description and Parameters For the sake oftesting the hybrid KF-UKF algorithm we design a simulationcomparing this algorithm with normal EKF algorithm Theschematic diagram of the simulation is shown in Figure 4 Byusing a path generator we calculate the acceleration angularrate velocity position and attitude information of an aircraftStochastic errors were added to the acceleration and angularrate data to simulate the IMU sensor error And the sameprocess is done to the velocity and position information tosimulate the GPS error Besides GPS outage flag is added to

6 Mathematical Problems in Engineering

One-step predictionand SINS information

One-step prediction

GPS signalbreaks off

SINS update Filter update

CorrectSINS

GPS receiver

GPS signalrecovers

and SINS information

SINSinformation

One-step prediction

State estimate xk+n

CalculateΦk|kminus1 Qkminus1

with F(t) G(t)

xk|kminus1Pk|kminus1

xk+n|kminus1Pk+n|kminus1

Calculate 120585k+n|kminus1 with 120594k+nminus1|kminus1

Calculate 120594k+nminus1|kminus1 withxk+nminus1|kminus1Pk+nminus1|kminus1

xk+s|kminus1Pk+s|kminus1

Calculate 120585k+s|kminus1 with 120594k+sminus1|kminus1

Calculate 120594k+sminus1|kminus1 withxk+sminus1|kminus1Pk+sminus1|kminus1

t = k minus 1

t = k

middot middot middotmiddot middot middot

middot middot middotmiddot middot middot

t = k + s minus 1

t = k + s minus 05

t = k + s

t = k + n minus 1

t = k + n minus 05

t = k + n

rGPS k+n

vGPS k+n Error covariance Pk+n

Figure 3 Filter updates procedure during GPS outage

Table 3 Comparison of navigation error between EKF and hybrid KF-UKF after GPS outage

Outages length Adopted approach RMS attitude error (∘) RMS velocity error (ms) RMS position error (m)Roll Pitch Yaw North East Down Latitude Longitude Height

50 s EKF 00451 00566 02605 00256 00233 00331 11990 08245 33450Hybrid KF-UKF 00438 00549 02499 00255 00233 00331 11984 08199 32310

100 s EKF 00553 01050 02630 00286 00273 00367 11832 37463 30911Hybrid KF-UKF 00508 00767 01706 00240 00259 00339 11701 37434 39175

150 s EKF 00616 00590 01282 00363 00341 00479 13913 22053 31303Hybrid KF-UKF 00489 00367 01140 00219 00237 00333 13876 21949 39078

200 s EKF 00969 00560 17882 00431 00544 00619 15559 39014 36364Hybrid KF-UKF 00586 00403 17131 00231 00318 00360 15552 38998 35416

Path generator

Integration algorithm

IMU stochastic error

GPS stochastic errorGPS outage flag

Results comparison

a 120596 v r 120595

aIMU 120596IMU vGPS rGPS

vnav rnav120595nav

120575v

120575r

120575120595

Figure 4 Schematic diagram of the simulation

GPS data so the integration algorithm is able to judgewhetherGPS signal is lost Finally velocity position and attitude data

calculated by hybrid KF-UKF and normal EKF algorithm arecompared The simulation parameters are shown in Table 1

42 Simulation Results and Analysis In the simulation testthe aircraft has done a series of maneuvers which are listed inTable 2 and its acceleration angular rate velocity positionand attitude information was recorded We calculate thenavigation parameters of the aircraft by integrating SINS andGPS using the normal EKF and hybrid KF-UKF respectivelyThen the results were compared as is shown in Figure 5 andTable 3

Figure 5 is the comparison of attitude error curves of EKFand hybrid KF-UKF which are taken for example As weobserve the GPS signal was lost at 119905 = 70 s and recovered

Mathematical Problems in Engineering 7

270 275minus04

minus02

0

02

04

270 272 274minus02

minus01

0

Hybrid KF-UKFEKF

Hybrid KF-UKFEKF

GPS recovery timeGPS recovery time

GPS lost time GPS lost time

minus05

0

05

1

15

Roll

erro

r (∘)

minus05

0

05

1

Pitc

h er

ror (

∘)

100 150 200 250 30050

t (s)100 150 200 250 30050

t (s)

Figure 5 Comparison of attitude error between EKF and hybrid KF-UKF

270 271 272minus04

minus02

0

02

270 271 272minus01

001020304

Hybrid KF-UKFEKF

Hybrid KF-UKFEKF

GPS lost timeGPS lost time

GPS recovery time

GPS recovery time

minus12

minus10

minus8

minus6

minus4

minus2

02

Nor

th v

eloci

ty er

ror (

ms

)

100 150 200 250 30050t (s)

0

5

10

15

East

velo

city

erro

r (m

s)

100 150 200 250 30050t (s)

Figure 6 Comparison of velocity error between EKF and hybrid KF-UKF

at 119905 = 270 s During the 200 seconds GPS outage the rolland pitch angle error grew with time because the accuracy ofMEMS inertial sensors is very low and no GPS measurementinformation could be used for the filter to correct the SINSerror When the navigation system received GPS signal againat 119905 = 270 s the filter started to correct SINS error againIn the figure we can notice that after GPS information wasrecovered the attitude error reduced quickly However theattitude calculated by using hybrid KF-UKF is more accuratethan that calculated by EKF which is shown in the circle inthe figure This phenomenon appears for the reason that aswe described before the MEMS-IMU sensor caused largenonlinear error during GPS outage Based on the nonlinearSINS error model UT is able to predict SINS errors betterthan EKF thus the filter can correct the SINS errors quicklyand accurately And the similar phenomenon also occurs tothe velocity which is shown in Figure 6

Table 3 is the comparison of navigation error betweenEKF and hybrid KF-UKF after GPS outage For attitude errorwe can notice that the error of yaw is larger than that of rolland pitch owing to the poor observability of yawing angle invelocityposition integrationmode thus the filter is unable toestimate its error correctly By comparing the RMS attitudeerror and velocity error after GPS outage it is shown that thenavigation error is lower if the hybrid KF-UKF algorithm isapplied Also it is shown in Table 3 that with the GPS outageperiod increases the hybrid KF-UKF algorithm was betterand better compared to the EKF algorithm That is becausethe error of an inertial navigation system accumulates withthe increase The longer the GPS outage is the larger the

nonlinear error the MEMS-IMU causes And the hybridKF-UKF algorithm will show more superiority to normalEKF algorithm We can also notice that the latitude andlongitude accuracy has little improvement on the contrary toattitude and velocity And this can be explained by examiningthe SINS error equations (9) to (11) In formulas (9) and(10) the attitude error and velocity error are affected by thenonlinear terms C119899

1015840

119899and Cminus1

120595whose values grew fast during

GPS outage However when we refer to formula (11) wecan see that the numerical value of 120575119871 120575119877119864 and 120575119877119873 isvery small and little nonlinear error is caused during GPSoutage because the aircraft did notmove quite far away duringthe simulation so the EKF is still able to correct its erroraccurately when GPS signal recovers

In Table 4 we list the calculation amounts of EKF andUKF so that we can estimate the computational cost of theproposed combined algorithm The calculation amounts ofthe algorithm are evaluated using the floating-point opera-tions (flops) which is defined as the operation of addingdecreasing multiplying or dividing between two floatingnumbers For example if we add one (119899 times 119898) dimensionmatrix to another (119899 times 119898) dimension matrix then 119899119898 flopshave been generated in the computer In Table 4 we supposethe dimension of the state vector is 119899 and the measurementvector dimension is 119897 During the prediction updating period4119899

3+119899

2minus119899 flops are generated in EKFwhile 6(23)1198993+121198992+

3119899 flops are generated in UKF In SINSGNSS integration119899 = 15 and 119897 = 6 So there will be 13710 flops in EKF and25245 flops in UKF during the prediction updating periodWhat is more in Unscented Transform the state prediction

8 Mathematical Problems in Engineering

Table 4 Comparison of computational cost of EKF and UKF

EKF UKF (for linear measurement equation)Step Flops Step Flops

Prediction update

Generate sigmapoints120594119896minus1

23119899

3+ 3119899

2+ 119899

State predictionx119896|119896minus1

2119899

2minus 119899

State predictionx119896|119896minus1

4119899

2+ 119899

Error covarianceprediction P119896|119896minus1

4119899

3minus 119899

2

Error covarianceprediction

P119896|119896minus16119899

3+ 5119899

2+ 119899

Subtotal 4119899

3+ 119899

2minus 119899 Subtotal 6(23)119899

3+ 12119899

2+ 3119899

Measurement update

Filter gainK119896

4119899

2119897 + 4119899119897

2+ 119897

3minus 3119899119897

Filter gainK119896

4119899

2119897 + 4119899119897

2+ 119897

3minus 3119899119897

State updatex119896

4119899119897

State updatex119896

4119899119897

Error covariance updateP119896

2119899

3+ 2119899

2119897 minus 119899

2

Error covarianceupdateP119896

2119899

3+ 2119899

2119897 minus 119899

2

Subtotal 2119899

3+6119899

2119897+4119899119897

2+119897

3+119899119897minus119899

2 Subtotal 2119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+ 119899119897 minus 119899

2

In total 6119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+119899119897 minus 119899 In total 8(23)119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+ 11119899

2+

119899119897 + 3119899

119899 = 15 119897 = 6 30801 119899 = 15 l = 6 42336

is performed by solving the error state differential equationswith Runge-Kutta method (2119899 + 1) times since each sigmapoint has to be predictedThat is to say more than 25245 flopsare generated in one filter recycle during GPS outages whenwe predict the SINS error using Unscented Transform So infact the computational cost of UKF is much larger than thatin EKF in the prediction updating part thus the hybrid UKF-EKF is recommended to reduce the computational burden forthe computer In hybridUKF-EKF theUnscented Transformwhich is necessary to predict the nonlinear SINS error onlyperforms during GPS outages Last but not least the amountof flops in EKF during the measurement updating period is2119899

3+6119899

2119897+4119899119897

2+119897

3+119899119897minus119899

2 in this case 17091 as well as thatin UKFThis is for the reason that in SINSGNSS integrationthe measurement equation is linear and no sigma point isneeded in the measurement updating part When GPS signalis functioning well there will be 30801 flops in each filtercycle

5 Conclusion

This paper presents a hybrid KF-UKF algorithm for real-timeMEMS-SINSGPS integrationThe linear and nonlinearSINS models are discussed The flowchart of the hybrid KF-UKF algorithm is described The SINS error is estimated andcorrected with linear SINS model when GPS is functioningwell while it is predicted with nonlinear SINS model byapplying Unscented Transform during GPS outage Thesimulation results indicate that compared to normal EKFalgorithm the hybrid KF-UKF algorithm is able to predictthe SINS error more accurately if GPS suffers from long-time GPS outage and the navigation error is lower after GPS

signal was regainedThe results also show that the hybrid KF-UKF algorithm is more efficient for attitude error predictionbut the effect on position error is not evident Comparedwith normal UKF the hybrid KF-UKF algorithm has lowcalculation amount In this paper the remaining problemwhich we have not solved is that SINS errors still grow fastduring GPS outage So in our future work we may combinethe UKF with ANN or SVM to create a new algorithm toreduce the SINS errors during GPS outage

Conflict of Interests

The authors declare that there is no conflict of interestsregarding the publication of this paper

Acknowledgments

This research is supported by the 3rd Innovation Fund ofChangchun Institute of Optics Fine Mechanics and Physics(CIOMP) and the UAV semi-physical simulation platformresearch project Chinese Science Academy (CSA)

References

[1] P D Groves Principles of GNSS Inertial and MultisensorIntegrated Navigation Systems Artech House Norwood MassUSA 2008

[2] Y Yuksel N El-Sheimy andANoureldin ldquoErrormodeling andcharacterization of environmental effects for low cost inertialMEMS unitsrdquo in Proceedings of the IEEEION Position Locationand Navigation Symposium pp 598ndash612 Indian Wells CalifUSA May 2010

Mathematical Problems in Engineering 9

[3] P Aggarwal Z Syed X Niu and N El-Sheimy ldquoA standardtesting and calibration procedure for low cost MEMS inertialsensors and unitsrdquo The Journal of Navigation vol 61 no 2 pp323ndash336 2008

[4] S Godha and M E Cannon ldquoGPSMEMS INS integratedsystem for navigation in urban areasrdquo GPS Solutions vol 11 no3 pp 193ndash203 2007

[5] M Ilyas Y Yang Q S Qian and R Zhang ldquoLow-cost IMUodometerGPS integrated navigation aided with two antennaeheading measurement for land vehicle applicationrdquo in Proceed-ings of the 25th Chinese Control andDecision Conference (CCDCrsquo13) pp 4521ndash4526 Guiyang China May 2013

[6] Z Jiang C Liu G Zhang Y P Wang C Huang and J LiangldquoGPSINS integrated navigation based on UKF and simulatedannealing optimized SVMrdquo in Proceedings of the IEEE 78thVehicular Technology Conference (VTC Fall rsquo13) vol 14 pp 1ndash5 Las Vegas Nev USA September 2013

[7] L Chen and J Fang ldquoA hybrid prediction method for bridgingGPS outages in high-precision POS applicationrdquo IEEE Transac-tions on Instrumentation and Measurement vol 63 no 6 pp1656ndash1665 2014

[8] A M Hasan K Samsudin A R Ramli and R S AzmirldquoAutomatic estimation of inertial navigation system errors forglobal positioning system outage recoveryrdquo Proceedings of theInstitution of Mechanical Engineers Part G Journal of AerospaceEngineering vol 225 no 1 pp 86ndash96 2011

[9] X Chen C Shen W-B Zhang M Tomizuka Y Xu andK Chiu ldquoNovel hybrid of strong tracking Kalman filter andwavelet neural network for GPSINS during GPS outagesrdquoMeasurement vol 46 no 10 pp 3847ndash3854 2013

[10] S J Julier and J K Uhlmann ldquoA new approach for filteringnonlinear systemrdquo in Proceedings of the American ControlConference vol 3 pp 1628ndash1632 IEEE SeattleWashUSA June1995

[11] P-B Ma H-X Baoyin and J-S Mu ldquoAutonomous navigationof Mars probe based on optical observation of Martian moonrdquoOptics and Precision Engineering vol 22 no 4 pp 863ndash8692014

[12] E Shin Estimation Techniques for Low-Cost Inertial NavigationThe University of Calgary Calgary Canada 2005

[13] Y Yi On improving the accuracy and reliability of GPSINS-based direct sensor georeferencing [PhD thesis] Ohio StateUniversity Columbus Ohio USA 2007

[14] Y Hao Z Xiong F Sun and X Wang ldquoComparison ofunscented Kalman filtersrdquo in Proceedings of the IEEE Interna-tional Conference on Mechatronics and Automation pp 895ndash899 Harbin China August 2007

[15] R M Rogers Applied Mathematics in Integrated NavigationSystems American Institute of Aeronautics and AstronauticsReston Va USA 3rd edition 2007

[16] D H Titterton and J L Weston Strapdown Inertial NavigationTechnology The Institution of Electrical Engineers MichaelFaraday House Stevenage UK 2nd edition 2004

[17] G Yan W Yan and D Xu ldquoA SINS nonlinear error modelreflecting better characteristics of SINS errorsrdquo Journal ofNorthwestern Polytechnical University vol 27 no 4 pp 511ndash5162009

[18] G M Yan W S Yan and D M Xu ldquoApplication of simplifiedUKF in SINS initial alignment for large misalignment anglesrdquoJournal of Chinese Inertial Technology vol 16 no 3 pp 253ndash2642008

[19] P S Maybeck Stochastic Models Estimation and ControlAcademic Press New York NY USA 1979

[20] S Liu Research and implementation of GPSINS integrated nav-igation algorithm [MS thesis] PLA Information EngineeringUniversity Zhengzhou China 2012 (Chinese)

[21] M S Grewal and A P Andrews Kalman Filtering Theory andPractice Using MATLAB John Wiley amp Sons Hoboken NJUSA 3rd edition 2008

[22] M C Vandyke J L Schwartz and C D Hall ldquoUnscentedKalman filtering for spacecraft attitude state and parameterestimationrdquo Advances in the Astronautical Sciences vol 119 no1 pp 217ndash228 2005

[23] Y Y Qin H Y Zhang and S S Wang Theory of KalmanFilter and Integrated Navigation North-Western PolytechnicalUniversity Press Xirsquoan China 2012 (Chinese)

Submit your manuscripts athttpwwwhindawicom

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical Problems in Engineering

Hindawi Publishing Corporationhttpwwwhindawicom

Differential EquationsInternational Journal of

Volume 2014

Applied MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical PhysicsAdvances in

Complex AnalysisJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

OptimizationJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Operations ResearchAdvances in

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Function Spaces

Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of Mathematics and Mathematical Sciences

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Algebra

Discrete Dynamics in Nature and Society

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Decision SciencesAdvances in

Discrete MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom

Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Stochastic AnalysisInternational Journal of

4 Mathematical Problems in Engineering

accumulated and the navigation errors are bounded Thusthe linear SINS error model is accurate enough since thenonlinear parts could be ignored

The linear SINS error transition matrix is discretized asfollows [19]

Φ119896|119896minus1 asymp I + F119896minus1Δ119879 +

1

2

F2119896minus1

Δ119879

2

Q119896minus1

asymp G119896minus1qG119879

119896minus1Δ119879

+

1

2

(F119896minus1G119896minus1qG119879

119896minus1+ G119896minus1qG

119879

119896minus1F119879119896minus1

) Δ119879

2

(13)

where F(119905) is the coefficient matrix of the state equation Δ119879is the filter cycle q is the covariance matrix of SINS sensorsand G(119905) is the coefficient matrix of the state noise

For a loosely coupled SINSGNSS system the measure-ment equation is [20]

z = Hx + k (14)

where z is the difference of the velocity and position betweenSINS and GPS H = [I6times6 06times9] k is the noise standarddeviation vector

KF algorithm is committed by using such formulas [21]x119896|119896minus1 = Φ119896|119896minus1x119896minus1 (15)

P119896|119896minus1 = Φ119896|119896minus1P119896minus1Φ119879

119896|119896minus1+Q119896minus1 (16)

K119896 = P119896|119896minus1H119879

119896(H119896P119896|119896minus1H

119879

119896+ R119896)

minus1

(17)

x119896 = x119896|119896minus1 + K119896 (z119896 minusH119896x119896|119896minus1) (18)

P119896 = (I minus K119896H119896)P119896|119896minus1 (19)

In SINSGPS integration a close-loop configuration isused In every KF epoch when the SINS errors are correctedthe state x119896 is set to be zero

32 Unscented Kalman Filter As we mentioned above KFis only available when the state equation is linear because itis not feasible to predict the state by transition matrix for anonlinear system If we neglect the higher order terms KFwill introduce errors which cannot be ignored for a stronglynonlinear system [22] To predict the state of a nonlinearsystem UT is used in UKF by generating a series of samplepoints to simulate the transfer of the state which couldachieve an accuracy of three orders

In UT the state is predicted in three steps [23] Firstsigma points should be constructed

120594(0)

119896minus1= x119896minus1

120594(119894)

119896minus1= x119896minus1 + (radic(119899 + 120582)P119896minus1)

119888(119894)

119894 = 1 2 119899

120594(119894)

119896minus1= x119896minus1 minus (radic(119899 + 120582)P119896minus1)

119888(119894minus119899)

119894 = 119899 + 1 119899 + 2 2119899

(20)

where 119899 is the dimension of the state vector x 120582 = 120572

2(119899 +

120581) minus 119899 which decides the weights of the distribution of thesigma points and generally 120581 = 3 minus 119899 while 10minus4 le 120572 le

1 And (radic(119899 + 120582)P119896minus1)119888(119894) is the 119894 column of the Choleskydecomposition of the matrix (119899 + 120582)P119896minus1

Second the states are predicted with every sigma pointThis step is executed by solving the error state differentialequations with four-order Runge-Kutta method

120585(119894)

119896|119896minus1= 119891 (120594

(119894)

119896minus1) 119894 = 1 2 2119899 (21)

Finally the states are weighted and summarized so weobtain the predicted state x119896|119896minus1 and the error covariancematrix P119896|119896minus1

x119896|119896minus1 =2119899

sum

119894=0

119882

(119898)

119894120585(119894)

119896|119896minus1

P119896|119896minus1 =2119899

sum

119894=0

119882

(119888)

119894[(120585(119894)

119896|119896minus1minus x119896|119896minus1) (120585

(119894)

119896|119896minus1minus x119896|119896minus1)

119879

]

+Q119896minus1

(22)

where Q119896minus1 is the covariance matrix of the state noise119882(119898)119894

and119882(119888)119894

are the weights of the sigma points which could becalculated as

119882

(119898)

0=

120582

119899 + 120582

119882

(119888)

0=

120582

119899 + 120582

+ 1 minus 120572

2+ 120573

119882

(119898)

119894= 119882

(119888)

119894=

120582

2 (119899 + 120582)

119894 = 1 2 2119899

(23)

where 120572 and 120582 have been given in the first step and 120573 isassigned according to the distribution character of the stateerror In this case 120573 = 2

In loosely coupled SINGPS the measurement equationis linear as we saw in (14) So after we predict the state x119896|119896minus1and the error covariance matrix P119896|119896minus1 we could estimate theSINS errors with formulas (17) to (19)

33 Hybrid KF-UKF Algorithm In this part we discussed thearchitecture of the hybrid KF-UKF algorithm As shown inFigure 1 SINS information is updated together with GPSWhenGPS information is received its availability is judged sothat whichmethod is to going be executed can be determined

If GPS information is available SINS and GPS can beintegrated by normal KF method as shown in Figure 2 Firstlinear SINS error equation (6) is discretized by formula (13)with SINS information at time 119905 = 119896 minus 1 Then one-step stateprediction and error covariance prediction are calculatedwith formulas (15) and (16) and the filter gain is calculatedwith formula (17) Afterwards the state vector which is theSINS error and error covariance at time 119905 = 119896 are estimatedwith GPS information at 119905 = 119896 Finally the SINS errorestimated at 119905 = 119896 is feedback to SINS correcting the SINSerror and the same procedure is committed in the next cycle

Mathematical Problems in Engineering 5

Velocity position Attitude velocity position angular rate and acceleration

GPS information SINS information

GPS available

One-step state prediction

No

Error estimation

Yes

Correct SINSerror

at t = k at t = k minus 1

Estimate SINSerrors xk by KF

Predict state xk|kminus1 andcovariance Pk|kminus1 by UT

k = k + 1

minus

Figure 1 The architecture of the hybrid KF-UKF algorithm

SINS update

One-step prediction

SINSinformation

CorrectSINS

Filter update

CalculateΦk|kminus1 Qkminus1

with F(t) G(t)

xk|kminus1Pk|kminus1

Calculate filter gain Kk

State estimate xkError covariance Pk rGPS k

t = k minus 1

t = kvGPS k

Figure 2 Filter updates procedure when GPS is functioning well

Table 1 Main specifications of experiment instruments in thesimulation test

Equipment Parameters

GPS receiverVelocity accuracy 01ms (1120590)

Position accuracy 5m (horizontal)10m (vertical)

Output frequency 10Hz

MEMS-IMU

GyroscopeBias 50∘h (1120590)White noise 35∘hradicHz

AccelerometerBias 2mg (1120590)White noise 03mgradicHzOutput frequency 200Hz

During GPS signal blockage the GPS information isunavailable and one-step state prediction is executed withoutmeasurement updateNow the nonlinear SINS error equationand UT are applied to predict the state and error covarianceAs shown in Figure 3 let us assume that theGPS signal breaksoff at 119905 = 119896 and recovers at 119905 = 119896 + 119899 In order to estimateSINS error at 119905 = 119896 + 119899 it is necessary to predict the statex119896+119899|119896minus1 and error covariance P119896+119899|119896minus1 Suppose that 119896 + 119904 is atime instant in GPS outage and x119896+119904|119896minus1P119896+119904|119896minus1 are iterated

Table 2 Aircraft maneuvers in the test

Stage Period Aircraft maneuver Parameters1 0sim30 s Stationary v = 0ms2 30sim60 s Acceleration v = 0ms to v = 100ms3 60sim75 s Pitching 120579 = 0∘ to 120579 = 30∘

4 75sim95 s Straight flight v = 100ms5 95sim120 s Pitching 120579 = 30∘ to 120579 = 0∘

6 120sim140 s Straight flight v = 100ms7 140sim165 s Coordinate turn 120595 = 0∘ to 120595 = 40∘

8 165sim190 s Straight flight v = 100ms9 190sim215 s Coordinate turn 120595 = 40∘ to 120595 = 0∘

10 215sim300 s Straight flight v = 100ms

by using x119896+119904minus1|119896minus1P119896+119904minus1|119896minus1 and SINS information at time119905 = 119896 + 119904 minus 1 119905 = 119896 + 119904 minus 05 and 119905 = 119896 + 119904 with formulas (24)to (27) This calculation is executed cycle by cycle until GPSsignal recovers at 119905 = 119896 + 119899 When GPS receiver offered thevelocity andposition information of the vehicle at 119905 = 119896+119899 wealready have the state x119896+119899|119896minus1 and error covariance P119896+119899|119896minus1Then we can estimate SINS error at 119905 = 119896 + 119899 with formulas(18) and (19) correct it and go into the next filter cycle

120594(0)

119896+119904minus1|119896minus1= x119896+119904minus1|119896minus1

120594(119894)

119896+119904minus1|119896minus1= x119896+119904minus1|119896minus1 + (radic(119899 + 120582)P119896+119904minus1|119896minus1)

119888(119894)

119894 = 1 2 119899

120594(119894)

119896+119904minus1|119896minus1= x119896+119904minus1|119896minus1 minus (radic(119899 + 120582)P119896+119904minus1|119896minus1)

119888(119894minus119899)

119894 = 119899 + 1 119899 + 2 2119899

(24)

120585119896+119904|119896minus1 = 119891 (120594119896+119904minus1|119896minus1

) 119894 = 1 2 2119899 (25)

x119896+119904|119896minus1 =2119899

sum

119894=0

119882

(119898)

119894120585(119894)

119896+119904|119896minus1 (26)

P119896+119904|119896minus1 =2119899

sum

119894=0

119882

(119888)

119894[(120585(119894)

119896+119904|119896minus1minus x119896+119904|119896minus1)

sdot (120585(119894)

119896+119904|119896minus1minus x119896+119904|119896minus1)

119879

] +Q119896minus1

(27)

4 Simulation Test

41 Simulation Description and Parameters For the sake oftesting the hybrid KF-UKF algorithm we design a simulationcomparing this algorithm with normal EKF algorithm Theschematic diagram of the simulation is shown in Figure 4 Byusing a path generator we calculate the acceleration angularrate velocity position and attitude information of an aircraftStochastic errors were added to the acceleration and angularrate data to simulate the IMU sensor error And the sameprocess is done to the velocity and position information tosimulate the GPS error Besides GPS outage flag is added to

6 Mathematical Problems in Engineering

One-step predictionand SINS information

One-step prediction

GPS signalbreaks off

SINS update Filter update

CorrectSINS

GPS receiver

GPS signalrecovers

and SINS information

SINSinformation

One-step prediction

State estimate xk+n

CalculateΦk|kminus1 Qkminus1

with F(t) G(t)

xk|kminus1Pk|kminus1

xk+n|kminus1Pk+n|kminus1

Calculate 120585k+n|kminus1 with 120594k+nminus1|kminus1

Calculate 120594k+nminus1|kminus1 withxk+nminus1|kminus1Pk+nminus1|kminus1

xk+s|kminus1Pk+s|kminus1

Calculate 120585k+s|kminus1 with 120594k+sminus1|kminus1

Calculate 120594k+sminus1|kminus1 withxk+sminus1|kminus1Pk+sminus1|kminus1

t = k minus 1

t = k

middot middot middotmiddot middot middot

middot middot middotmiddot middot middot

t = k + s minus 1

t = k + s minus 05

t = k + s

t = k + n minus 1

t = k + n minus 05

t = k + n

rGPS k+n

vGPS k+n Error covariance Pk+n

Figure 3 Filter updates procedure during GPS outage

Table 3 Comparison of navigation error between EKF and hybrid KF-UKF after GPS outage

Outages length Adopted approach RMS attitude error (∘) RMS velocity error (ms) RMS position error (m)Roll Pitch Yaw North East Down Latitude Longitude Height

50 s EKF 00451 00566 02605 00256 00233 00331 11990 08245 33450Hybrid KF-UKF 00438 00549 02499 00255 00233 00331 11984 08199 32310

100 s EKF 00553 01050 02630 00286 00273 00367 11832 37463 30911Hybrid KF-UKF 00508 00767 01706 00240 00259 00339 11701 37434 39175

150 s EKF 00616 00590 01282 00363 00341 00479 13913 22053 31303Hybrid KF-UKF 00489 00367 01140 00219 00237 00333 13876 21949 39078

200 s EKF 00969 00560 17882 00431 00544 00619 15559 39014 36364Hybrid KF-UKF 00586 00403 17131 00231 00318 00360 15552 38998 35416

Path generator

Integration algorithm

IMU stochastic error

GPS stochastic errorGPS outage flag

Results comparison

a 120596 v r 120595

aIMU 120596IMU vGPS rGPS

vnav rnav120595nav

120575v

120575r

120575120595

Figure 4 Schematic diagram of the simulation

GPS data so the integration algorithm is able to judgewhetherGPS signal is lost Finally velocity position and attitude data

calculated by hybrid KF-UKF and normal EKF algorithm arecompared The simulation parameters are shown in Table 1

42 Simulation Results and Analysis In the simulation testthe aircraft has done a series of maneuvers which are listed inTable 2 and its acceleration angular rate velocity positionand attitude information was recorded We calculate thenavigation parameters of the aircraft by integrating SINS andGPS using the normal EKF and hybrid KF-UKF respectivelyThen the results were compared as is shown in Figure 5 andTable 3

Figure 5 is the comparison of attitude error curves of EKFand hybrid KF-UKF which are taken for example As weobserve the GPS signal was lost at 119905 = 70 s and recovered

Mathematical Problems in Engineering 7

270 275minus04

minus02

0

02

04

270 272 274minus02

minus01

0

Hybrid KF-UKFEKF

Hybrid KF-UKFEKF

GPS recovery timeGPS recovery time

GPS lost time GPS lost time

minus05

0

05

1

15

Roll

erro

r (∘)

minus05

0

05

1

Pitc

h er

ror (

∘)

100 150 200 250 30050

t (s)100 150 200 250 30050

t (s)

Figure 5 Comparison of attitude error between EKF and hybrid KF-UKF

270 271 272minus04

minus02

0

02

270 271 272minus01

001020304

Hybrid KF-UKFEKF

Hybrid KF-UKFEKF

GPS lost timeGPS lost time

GPS recovery time

GPS recovery time

minus12

minus10

minus8

minus6

minus4

minus2

02

Nor

th v

eloci

ty er

ror (

ms

)

100 150 200 250 30050t (s)

0

5

10

15

East

velo

city

erro

r (m

s)

100 150 200 250 30050t (s)

Figure 6 Comparison of velocity error between EKF and hybrid KF-UKF

at 119905 = 270 s During the 200 seconds GPS outage the rolland pitch angle error grew with time because the accuracy ofMEMS inertial sensors is very low and no GPS measurementinformation could be used for the filter to correct the SINSerror When the navigation system received GPS signal againat 119905 = 270 s the filter started to correct SINS error againIn the figure we can notice that after GPS information wasrecovered the attitude error reduced quickly However theattitude calculated by using hybrid KF-UKF is more accuratethan that calculated by EKF which is shown in the circle inthe figure This phenomenon appears for the reason that aswe described before the MEMS-IMU sensor caused largenonlinear error during GPS outage Based on the nonlinearSINS error model UT is able to predict SINS errors betterthan EKF thus the filter can correct the SINS errors quicklyand accurately And the similar phenomenon also occurs tothe velocity which is shown in Figure 6

Table 3 is the comparison of navigation error betweenEKF and hybrid KF-UKF after GPS outage For attitude errorwe can notice that the error of yaw is larger than that of rolland pitch owing to the poor observability of yawing angle invelocityposition integrationmode thus the filter is unable toestimate its error correctly By comparing the RMS attitudeerror and velocity error after GPS outage it is shown that thenavigation error is lower if the hybrid KF-UKF algorithm isapplied Also it is shown in Table 3 that with the GPS outageperiod increases the hybrid KF-UKF algorithm was betterand better compared to the EKF algorithm That is becausethe error of an inertial navigation system accumulates withthe increase The longer the GPS outage is the larger the

nonlinear error the MEMS-IMU causes And the hybridKF-UKF algorithm will show more superiority to normalEKF algorithm We can also notice that the latitude andlongitude accuracy has little improvement on the contrary toattitude and velocity And this can be explained by examiningthe SINS error equations (9) to (11) In formulas (9) and(10) the attitude error and velocity error are affected by thenonlinear terms C119899

1015840

119899and Cminus1

120595whose values grew fast during

GPS outage However when we refer to formula (11) wecan see that the numerical value of 120575119871 120575119877119864 and 120575119877119873 isvery small and little nonlinear error is caused during GPSoutage because the aircraft did notmove quite far away duringthe simulation so the EKF is still able to correct its erroraccurately when GPS signal recovers

In Table 4 we list the calculation amounts of EKF andUKF so that we can estimate the computational cost of theproposed combined algorithm The calculation amounts ofthe algorithm are evaluated using the floating-point opera-tions (flops) which is defined as the operation of addingdecreasing multiplying or dividing between two floatingnumbers For example if we add one (119899 times 119898) dimensionmatrix to another (119899 times 119898) dimension matrix then 119899119898 flopshave been generated in the computer In Table 4 we supposethe dimension of the state vector is 119899 and the measurementvector dimension is 119897 During the prediction updating period4119899

3+119899

2minus119899 flops are generated in EKFwhile 6(23)1198993+121198992+

3119899 flops are generated in UKF In SINSGNSS integration119899 = 15 and 119897 = 6 So there will be 13710 flops in EKF and25245 flops in UKF during the prediction updating periodWhat is more in Unscented Transform the state prediction

8 Mathematical Problems in Engineering

Table 4 Comparison of computational cost of EKF and UKF

EKF UKF (for linear measurement equation)Step Flops Step Flops

Prediction update

Generate sigmapoints120594119896minus1

23119899

3+ 3119899

2+ 119899

State predictionx119896|119896minus1

2119899

2minus 119899

State predictionx119896|119896minus1

4119899

2+ 119899

Error covarianceprediction P119896|119896minus1

4119899

3minus 119899

2

Error covarianceprediction

P119896|119896minus16119899

3+ 5119899

2+ 119899

Subtotal 4119899

3+ 119899

2minus 119899 Subtotal 6(23)119899

3+ 12119899

2+ 3119899

Measurement update

Filter gainK119896

4119899

2119897 + 4119899119897

2+ 119897

3minus 3119899119897

Filter gainK119896

4119899

2119897 + 4119899119897

2+ 119897

3minus 3119899119897

State updatex119896

4119899119897

State updatex119896

4119899119897

Error covariance updateP119896

2119899

3+ 2119899

2119897 minus 119899

2

Error covarianceupdateP119896

2119899

3+ 2119899

2119897 minus 119899

2

Subtotal 2119899

3+6119899

2119897+4119899119897

2+119897

3+119899119897minus119899

2 Subtotal 2119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+ 119899119897 minus 119899

2

In total 6119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+119899119897 minus 119899 In total 8(23)119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+ 11119899

2+

119899119897 + 3119899

119899 = 15 119897 = 6 30801 119899 = 15 l = 6 42336

is performed by solving the error state differential equationswith Runge-Kutta method (2119899 + 1) times since each sigmapoint has to be predictedThat is to say more than 25245 flopsare generated in one filter recycle during GPS outages whenwe predict the SINS error using Unscented Transform So infact the computational cost of UKF is much larger than thatin EKF in the prediction updating part thus the hybrid UKF-EKF is recommended to reduce the computational burden forthe computer In hybridUKF-EKF theUnscented Transformwhich is necessary to predict the nonlinear SINS error onlyperforms during GPS outages Last but not least the amountof flops in EKF during the measurement updating period is2119899

3+6119899

2119897+4119899119897

2+119897

3+119899119897minus119899

2 in this case 17091 as well as thatin UKFThis is for the reason that in SINSGNSS integrationthe measurement equation is linear and no sigma point isneeded in the measurement updating part When GPS signalis functioning well there will be 30801 flops in each filtercycle

5 Conclusion

This paper presents a hybrid KF-UKF algorithm for real-timeMEMS-SINSGPS integrationThe linear and nonlinearSINS models are discussed The flowchart of the hybrid KF-UKF algorithm is described The SINS error is estimated andcorrected with linear SINS model when GPS is functioningwell while it is predicted with nonlinear SINS model byapplying Unscented Transform during GPS outage Thesimulation results indicate that compared to normal EKFalgorithm the hybrid KF-UKF algorithm is able to predictthe SINS error more accurately if GPS suffers from long-time GPS outage and the navigation error is lower after GPS

signal was regainedThe results also show that the hybrid KF-UKF algorithm is more efficient for attitude error predictionbut the effect on position error is not evident Comparedwith normal UKF the hybrid KF-UKF algorithm has lowcalculation amount In this paper the remaining problemwhich we have not solved is that SINS errors still grow fastduring GPS outage So in our future work we may combinethe UKF with ANN or SVM to create a new algorithm toreduce the SINS errors during GPS outage

Conflict of Interests

The authors declare that there is no conflict of interestsregarding the publication of this paper

Acknowledgments

This research is supported by the 3rd Innovation Fund ofChangchun Institute of Optics Fine Mechanics and Physics(CIOMP) and the UAV semi-physical simulation platformresearch project Chinese Science Academy (CSA)

References

[1] P D Groves Principles of GNSS Inertial and MultisensorIntegrated Navigation Systems Artech House Norwood MassUSA 2008

[2] Y Yuksel N El-Sheimy andANoureldin ldquoErrormodeling andcharacterization of environmental effects for low cost inertialMEMS unitsrdquo in Proceedings of the IEEEION Position Locationand Navigation Symposium pp 598ndash612 Indian Wells CalifUSA May 2010

Mathematical Problems in Engineering 9

[3] P Aggarwal Z Syed X Niu and N El-Sheimy ldquoA standardtesting and calibration procedure for low cost MEMS inertialsensors and unitsrdquo The Journal of Navigation vol 61 no 2 pp323ndash336 2008

[4] S Godha and M E Cannon ldquoGPSMEMS INS integratedsystem for navigation in urban areasrdquo GPS Solutions vol 11 no3 pp 193ndash203 2007

[5] M Ilyas Y Yang Q S Qian and R Zhang ldquoLow-cost IMUodometerGPS integrated navigation aided with two antennaeheading measurement for land vehicle applicationrdquo in Proceed-ings of the 25th Chinese Control andDecision Conference (CCDCrsquo13) pp 4521ndash4526 Guiyang China May 2013

[6] Z Jiang C Liu G Zhang Y P Wang C Huang and J LiangldquoGPSINS integrated navigation based on UKF and simulatedannealing optimized SVMrdquo in Proceedings of the IEEE 78thVehicular Technology Conference (VTC Fall rsquo13) vol 14 pp 1ndash5 Las Vegas Nev USA September 2013

[7] L Chen and J Fang ldquoA hybrid prediction method for bridgingGPS outages in high-precision POS applicationrdquo IEEE Transac-tions on Instrumentation and Measurement vol 63 no 6 pp1656ndash1665 2014

[8] A M Hasan K Samsudin A R Ramli and R S AzmirldquoAutomatic estimation of inertial navigation system errors forglobal positioning system outage recoveryrdquo Proceedings of theInstitution of Mechanical Engineers Part G Journal of AerospaceEngineering vol 225 no 1 pp 86ndash96 2011

[9] X Chen C Shen W-B Zhang M Tomizuka Y Xu andK Chiu ldquoNovel hybrid of strong tracking Kalman filter andwavelet neural network for GPSINS during GPS outagesrdquoMeasurement vol 46 no 10 pp 3847ndash3854 2013

[10] S J Julier and J K Uhlmann ldquoA new approach for filteringnonlinear systemrdquo in Proceedings of the American ControlConference vol 3 pp 1628ndash1632 IEEE SeattleWashUSA June1995

[11] P-B Ma H-X Baoyin and J-S Mu ldquoAutonomous navigationof Mars probe based on optical observation of Martian moonrdquoOptics and Precision Engineering vol 22 no 4 pp 863ndash8692014

[12] E Shin Estimation Techniques for Low-Cost Inertial NavigationThe University of Calgary Calgary Canada 2005

[13] Y Yi On improving the accuracy and reliability of GPSINS-based direct sensor georeferencing [PhD thesis] Ohio StateUniversity Columbus Ohio USA 2007

[14] Y Hao Z Xiong F Sun and X Wang ldquoComparison ofunscented Kalman filtersrdquo in Proceedings of the IEEE Interna-tional Conference on Mechatronics and Automation pp 895ndash899 Harbin China August 2007

[15] R M Rogers Applied Mathematics in Integrated NavigationSystems American Institute of Aeronautics and AstronauticsReston Va USA 3rd edition 2007

[16] D H Titterton and J L Weston Strapdown Inertial NavigationTechnology The Institution of Electrical Engineers MichaelFaraday House Stevenage UK 2nd edition 2004

[17] G Yan W Yan and D Xu ldquoA SINS nonlinear error modelreflecting better characteristics of SINS errorsrdquo Journal ofNorthwestern Polytechnical University vol 27 no 4 pp 511ndash5162009

[18] G M Yan W S Yan and D M Xu ldquoApplication of simplifiedUKF in SINS initial alignment for large misalignment anglesrdquoJournal of Chinese Inertial Technology vol 16 no 3 pp 253ndash2642008

[19] P S Maybeck Stochastic Models Estimation and ControlAcademic Press New York NY USA 1979

[20] S Liu Research and implementation of GPSINS integrated nav-igation algorithm [MS thesis] PLA Information EngineeringUniversity Zhengzhou China 2012 (Chinese)

[21] M S Grewal and A P Andrews Kalman Filtering Theory andPractice Using MATLAB John Wiley amp Sons Hoboken NJUSA 3rd edition 2008

[22] M C Vandyke J L Schwartz and C D Hall ldquoUnscentedKalman filtering for spacecraft attitude state and parameterestimationrdquo Advances in the Astronautical Sciences vol 119 no1 pp 217ndash228 2005

[23] Y Y Qin H Y Zhang and S S Wang Theory of KalmanFilter and Integrated Navigation North-Western PolytechnicalUniversity Press Xirsquoan China 2012 (Chinese)

Submit your manuscripts athttpwwwhindawicom

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical Problems in Engineering

Hindawi Publishing Corporationhttpwwwhindawicom

Differential EquationsInternational Journal of

Volume 2014

Applied MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical PhysicsAdvances in

Complex AnalysisJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

OptimizationJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Operations ResearchAdvances in

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Function Spaces

Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of Mathematics and Mathematical Sciences

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Algebra

Discrete Dynamics in Nature and Society

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Decision SciencesAdvances in

Discrete MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom

Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Stochastic AnalysisInternational Journal of

Mathematical Problems in Engineering 5

Velocity position Attitude velocity position angular rate and acceleration

GPS information SINS information

GPS available

One-step state prediction

No

Error estimation

Yes

Correct SINSerror

at t = k at t = k minus 1

Estimate SINSerrors xk by KF

Predict state xk|kminus1 andcovariance Pk|kminus1 by UT

k = k + 1

minus

Figure 1 The architecture of the hybrid KF-UKF algorithm

SINS update

One-step prediction

SINSinformation

CorrectSINS

Filter update

CalculateΦk|kminus1 Qkminus1

with F(t) G(t)

xk|kminus1Pk|kminus1

Calculate filter gain Kk

State estimate xkError covariance Pk rGPS k

t = k minus 1

t = kvGPS k

Figure 2 Filter updates procedure when GPS is functioning well

Table 1 Main specifications of experiment instruments in thesimulation test

Equipment Parameters

GPS receiverVelocity accuracy 01ms (1120590)

Position accuracy 5m (horizontal)10m (vertical)

Output frequency 10Hz

MEMS-IMU

GyroscopeBias 50∘h (1120590)White noise 35∘hradicHz

AccelerometerBias 2mg (1120590)White noise 03mgradicHzOutput frequency 200Hz

During GPS signal blockage the GPS information isunavailable and one-step state prediction is executed withoutmeasurement updateNow the nonlinear SINS error equationand UT are applied to predict the state and error covarianceAs shown in Figure 3 let us assume that theGPS signal breaksoff at 119905 = 119896 and recovers at 119905 = 119896 + 119899 In order to estimateSINS error at 119905 = 119896 + 119899 it is necessary to predict the statex119896+119899|119896minus1 and error covariance P119896+119899|119896minus1 Suppose that 119896 + 119904 is atime instant in GPS outage and x119896+119904|119896minus1P119896+119904|119896minus1 are iterated

Table 2 Aircraft maneuvers in the test

Stage Period Aircraft maneuver Parameters1 0sim30 s Stationary v = 0ms2 30sim60 s Acceleration v = 0ms to v = 100ms3 60sim75 s Pitching 120579 = 0∘ to 120579 = 30∘

4 75sim95 s Straight flight v = 100ms5 95sim120 s Pitching 120579 = 30∘ to 120579 = 0∘

6 120sim140 s Straight flight v = 100ms7 140sim165 s Coordinate turn 120595 = 0∘ to 120595 = 40∘

8 165sim190 s Straight flight v = 100ms9 190sim215 s Coordinate turn 120595 = 40∘ to 120595 = 0∘

10 215sim300 s Straight flight v = 100ms

by using x119896+119904minus1|119896minus1P119896+119904minus1|119896minus1 and SINS information at time119905 = 119896 + 119904 minus 1 119905 = 119896 + 119904 minus 05 and 119905 = 119896 + 119904 with formulas (24)to (27) This calculation is executed cycle by cycle until GPSsignal recovers at 119905 = 119896 + 119899 When GPS receiver offered thevelocity andposition information of the vehicle at 119905 = 119896+119899 wealready have the state x119896+119899|119896minus1 and error covariance P119896+119899|119896minus1Then we can estimate SINS error at 119905 = 119896 + 119899 with formulas(18) and (19) correct it and go into the next filter cycle

120594(0)

119896+119904minus1|119896minus1= x119896+119904minus1|119896minus1

120594(119894)

119896+119904minus1|119896minus1= x119896+119904minus1|119896minus1 + (radic(119899 + 120582)P119896+119904minus1|119896minus1)

119888(119894)

119894 = 1 2 119899

120594(119894)

119896+119904minus1|119896minus1= x119896+119904minus1|119896minus1 minus (radic(119899 + 120582)P119896+119904minus1|119896minus1)

119888(119894minus119899)

119894 = 119899 + 1 119899 + 2 2119899

(24)

120585119896+119904|119896minus1 = 119891 (120594119896+119904minus1|119896minus1

) 119894 = 1 2 2119899 (25)

x119896+119904|119896minus1 =2119899

sum

119894=0

119882

(119898)

119894120585(119894)

119896+119904|119896minus1 (26)

P119896+119904|119896minus1 =2119899

sum

119894=0

119882

(119888)

119894[(120585(119894)

119896+119904|119896minus1minus x119896+119904|119896minus1)

sdot (120585(119894)

119896+119904|119896minus1minus x119896+119904|119896minus1)

119879

] +Q119896minus1

(27)

4 Simulation Test

41 Simulation Description and Parameters For the sake oftesting the hybrid KF-UKF algorithm we design a simulationcomparing this algorithm with normal EKF algorithm Theschematic diagram of the simulation is shown in Figure 4 Byusing a path generator we calculate the acceleration angularrate velocity position and attitude information of an aircraftStochastic errors were added to the acceleration and angularrate data to simulate the IMU sensor error And the sameprocess is done to the velocity and position information tosimulate the GPS error Besides GPS outage flag is added to

6 Mathematical Problems in Engineering

One-step predictionand SINS information

One-step prediction

GPS signalbreaks off

SINS update Filter update

CorrectSINS

GPS receiver

GPS signalrecovers

and SINS information

SINSinformation

One-step prediction

State estimate xk+n

CalculateΦk|kminus1 Qkminus1

with F(t) G(t)

xk|kminus1Pk|kminus1

xk+n|kminus1Pk+n|kminus1

Calculate 120585k+n|kminus1 with 120594k+nminus1|kminus1

Calculate 120594k+nminus1|kminus1 withxk+nminus1|kminus1Pk+nminus1|kminus1

xk+s|kminus1Pk+s|kminus1

Calculate 120585k+s|kminus1 with 120594k+sminus1|kminus1

Calculate 120594k+sminus1|kminus1 withxk+sminus1|kminus1Pk+sminus1|kminus1

t = k minus 1

t = k

middot middot middotmiddot middot middot

middot middot middotmiddot middot middot

t = k + s minus 1

t = k + s minus 05

t = k + s

t = k + n minus 1

t = k + n minus 05

t = k + n

rGPS k+n

vGPS k+n Error covariance Pk+n

Figure 3 Filter updates procedure during GPS outage

Table 3 Comparison of navigation error between EKF and hybrid KF-UKF after GPS outage

Outages length Adopted approach RMS attitude error (∘) RMS velocity error (ms) RMS position error (m)Roll Pitch Yaw North East Down Latitude Longitude Height

50 s EKF 00451 00566 02605 00256 00233 00331 11990 08245 33450Hybrid KF-UKF 00438 00549 02499 00255 00233 00331 11984 08199 32310

100 s EKF 00553 01050 02630 00286 00273 00367 11832 37463 30911Hybrid KF-UKF 00508 00767 01706 00240 00259 00339 11701 37434 39175

150 s EKF 00616 00590 01282 00363 00341 00479 13913 22053 31303Hybrid KF-UKF 00489 00367 01140 00219 00237 00333 13876 21949 39078

200 s EKF 00969 00560 17882 00431 00544 00619 15559 39014 36364Hybrid KF-UKF 00586 00403 17131 00231 00318 00360 15552 38998 35416

Path generator

Integration algorithm

IMU stochastic error

GPS stochastic errorGPS outage flag

Results comparison

a 120596 v r 120595

aIMU 120596IMU vGPS rGPS

vnav rnav120595nav

120575v

120575r

120575120595

Figure 4 Schematic diagram of the simulation

GPS data so the integration algorithm is able to judgewhetherGPS signal is lost Finally velocity position and attitude data

calculated by hybrid KF-UKF and normal EKF algorithm arecompared The simulation parameters are shown in Table 1

42 Simulation Results and Analysis In the simulation testthe aircraft has done a series of maneuvers which are listed inTable 2 and its acceleration angular rate velocity positionand attitude information was recorded We calculate thenavigation parameters of the aircraft by integrating SINS andGPS using the normal EKF and hybrid KF-UKF respectivelyThen the results were compared as is shown in Figure 5 andTable 3

Figure 5 is the comparison of attitude error curves of EKFand hybrid KF-UKF which are taken for example As weobserve the GPS signal was lost at 119905 = 70 s and recovered

Mathematical Problems in Engineering 7

270 275minus04

minus02

0

02

04

270 272 274minus02

minus01

0

Hybrid KF-UKFEKF

Hybrid KF-UKFEKF

GPS recovery timeGPS recovery time

GPS lost time GPS lost time

minus05

0

05

1

15

Roll

erro

r (∘)

minus05

0

05

1

Pitc

h er

ror (

∘)

100 150 200 250 30050

t (s)100 150 200 250 30050

t (s)

Figure 5 Comparison of attitude error between EKF and hybrid KF-UKF

270 271 272minus04

minus02

0

02

270 271 272minus01

001020304

Hybrid KF-UKFEKF

Hybrid KF-UKFEKF

GPS lost timeGPS lost time

GPS recovery time

GPS recovery time

minus12

minus10

minus8

minus6

minus4

minus2

02

Nor

th v

eloci

ty er

ror (

ms

)

100 150 200 250 30050t (s)

0

5

10

15

East

velo

city

erro

r (m

s)

100 150 200 250 30050t (s)

Figure 6 Comparison of velocity error between EKF and hybrid KF-UKF

at 119905 = 270 s During the 200 seconds GPS outage the rolland pitch angle error grew with time because the accuracy ofMEMS inertial sensors is very low and no GPS measurementinformation could be used for the filter to correct the SINSerror When the navigation system received GPS signal againat 119905 = 270 s the filter started to correct SINS error againIn the figure we can notice that after GPS information wasrecovered the attitude error reduced quickly However theattitude calculated by using hybrid KF-UKF is more accuratethan that calculated by EKF which is shown in the circle inthe figure This phenomenon appears for the reason that aswe described before the MEMS-IMU sensor caused largenonlinear error during GPS outage Based on the nonlinearSINS error model UT is able to predict SINS errors betterthan EKF thus the filter can correct the SINS errors quicklyand accurately And the similar phenomenon also occurs tothe velocity which is shown in Figure 6

Table 3 is the comparison of navigation error betweenEKF and hybrid KF-UKF after GPS outage For attitude errorwe can notice that the error of yaw is larger than that of rolland pitch owing to the poor observability of yawing angle invelocityposition integrationmode thus the filter is unable toestimate its error correctly By comparing the RMS attitudeerror and velocity error after GPS outage it is shown that thenavigation error is lower if the hybrid KF-UKF algorithm isapplied Also it is shown in Table 3 that with the GPS outageperiod increases the hybrid KF-UKF algorithm was betterand better compared to the EKF algorithm That is becausethe error of an inertial navigation system accumulates withthe increase The longer the GPS outage is the larger the

nonlinear error the MEMS-IMU causes And the hybridKF-UKF algorithm will show more superiority to normalEKF algorithm We can also notice that the latitude andlongitude accuracy has little improvement on the contrary toattitude and velocity And this can be explained by examiningthe SINS error equations (9) to (11) In formulas (9) and(10) the attitude error and velocity error are affected by thenonlinear terms C119899

1015840

119899and Cminus1

120595whose values grew fast during

GPS outage However when we refer to formula (11) wecan see that the numerical value of 120575119871 120575119877119864 and 120575119877119873 isvery small and little nonlinear error is caused during GPSoutage because the aircraft did notmove quite far away duringthe simulation so the EKF is still able to correct its erroraccurately when GPS signal recovers

In Table 4 we list the calculation amounts of EKF andUKF so that we can estimate the computational cost of theproposed combined algorithm The calculation amounts ofthe algorithm are evaluated using the floating-point opera-tions (flops) which is defined as the operation of addingdecreasing multiplying or dividing between two floatingnumbers For example if we add one (119899 times 119898) dimensionmatrix to another (119899 times 119898) dimension matrix then 119899119898 flopshave been generated in the computer In Table 4 we supposethe dimension of the state vector is 119899 and the measurementvector dimension is 119897 During the prediction updating period4119899

3+119899

2minus119899 flops are generated in EKFwhile 6(23)1198993+121198992+

3119899 flops are generated in UKF In SINSGNSS integration119899 = 15 and 119897 = 6 So there will be 13710 flops in EKF and25245 flops in UKF during the prediction updating periodWhat is more in Unscented Transform the state prediction

8 Mathematical Problems in Engineering

Table 4 Comparison of computational cost of EKF and UKF

EKF UKF (for linear measurement equation)Step Flops Step Flops

Prediction update

Generate sigmapoints120594119896minus1

23119899

3+ 3119899

2+ 119899

State predictionx119896|119896minus1

2119899

2minus 119899

State predictionx119896|119896minus1

4119899

2+ 119899

Error covarianceprediction P119896|119896minus1

4119899

3minus 119899

2

Error covarianceprediction

P119896|119896minus16119899

3+ 5119899

2+ 119899

Subtotal 4119899

3+ 119899

2minus 119899 Subtotal 6(23)119899

3+ 12119899

2+ 3119899

Measurement update

Filter gainK119896

4119899

2119897 + 4119899119897

2+ 119897

3minus 3119899119897

Filter gainK119896

4119899

2119897 + 4119899119897

2+ 119897

3minus 3119899119897

State updatex119896

4119899119897

State updatex119896

4119899119897

Error covariance updateP119896

2119899

3+ 2119899

2119897 minus 119899

2

Error covarianceupdateP119896

2119899

3+ 2119899

2119897 minus 119899

2

Subtotal 2119899

3+6119899

2119897+4119899119897

2+119897

3+119899119897minus119899

2 Subtotal 2119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+ 119899119897 minus 119899

2

In total 6119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+119899119897 minus 119899 In total 8(23)119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+ 11119899

2+

119899119897 + 3119899

119899 = 15 119897 = 6 30801 119899 = 15 l = 6 42336

is performed by solving the error state differential equationswith Runge-Kutta method (2119899 + 1) times since each sigmapoint has to be predictedThat is to say more than 25245 flopsare generated in one filter recycle during GPS outages whenwe predict the SINS error using Unscented Transform So infact the computational cost of UKF is much larger than thatin EKF in the prediction updating part thus the hybrid UKF-EKF is recommended to reduce the computational burden forthe computer In hybridUKF-EKF theUnscented Transformwhich is necessary to predict the nonlinear SINS error onlyperforms during GPS outages Last but not least the amountof flops in EKF during the measurement updating period is2119899

3+6119899

2119897+4119899119897

2+119897

3+119899119897minus119899

2 in this case 17091 as well as thatin UKFThis is for the reason that in SINSGNSS integrationthe measurement equation is linear and no sigma point isneeded in the measurement updating part When GPS signalis functioning well there will be 30801 flops in each filtercycle

5 Conclusion

This paper presents a hybrid KF-UKF algorithm for real-timeMEMS-SINSGPS integrationThe linear and nonlinearSINS models are discussed The flowchart of the hybrid KF-UKF algorithm is described The SINS error is estimated andcorrected with linear SINS model when GPS is functioningwell while it is predicted with nonlinear SINS model byapplying Unscented Transform during GPS outage Thesimulation results indicate that compared to normal EKFalgorithm the hybrid KF-UKF algorithm is able to predictthe SINS error more accurately if GPS suffers from long-time GPS outage and the navigation error is lower after GPS

signal was regainedThe results also show that the hybrid KF-UKF algorithm is more efficient for attitude error predictionbut the effect on position error is not evident Comparedwith normal UKF the hybrid KF-UKF algorithm has lowcalculation amount In this paper the remaining problemwhich we have not solved is that SINS errors still grow fastduring GPS outage So in our future work we may combinethe UKF with ANN or SVM to create a new algorithm toreduce the SINS errors during GPS outage

Conflict of Interests

The authors declare that there is no conflict of interestsregarding the publication of this paper

Acknowledgments

This research is supported by the 3rd Innovation Fund ofChangchun Institute of Optics Fine Mechanics and Physics(CIOMP) and the UAV semi-physical simulation platformresearch project Chinese Science Academy (CSA)

References

[1] P D Groves Principles of GNSS Inertial and MultisensorIntegrated Navigation Systems Artech House Norwood MassUSA 2008

[2] Y Yuksel N El-Sheimy andANoureldin ldquoErrormodeling andcharacterization of environmental effects for low cost inertialMEMS unitsrdquo in Proceedings of the IEEEION Position Locationand Navigation Symposium pp 598ndash612 Indian Wells CalifUSA May 2010

Mathematical Problems in Engineering 9

[3] P Aggarwal Z Syed X Niu and N El-Sheimy ldquoA standardtesting and calibration procedure for low cost MEMS inertialsensors and unitsrdquo The Journal of Navigation vol 61 no 2 pp323ndash336 2008

[4] S Godha and M E Cannon ldquoGPSMEMS INS integratedsystem for navigation in urban areasrdquo GPS Solutions vol 11 no3 pp 193ndash203 2007

[5] M Ilyas Y Yang Q S Qian and R Zhang ldquoLow-cost IMUodometerGPS integrated navigation aided with two antennaeheading measurement for land vehicle applicationrdquo in Proceed-ings of the 25th Chinese Control andDecision Conference (CCDCrsquo13) pp 4521ndash4526 Guiyang China May 2013

[6] Z Jiang C Liu G Zhang Y P Wang C Huang and J LiangldquoGPSINS integrated navigation based on UKF and simulatedannealing optimized SVMrdquo in Proceedings of the IEEE 78thVehicular Technology Conference (VTC Fall rsquo13) vol 14 pp 1ndash5 Las Vegas Nev USA September 2013

[7] L Chen and J Fang ldquoA hybrid prediction method for bridgingGPS outages in high-precision POS applicationrdquo IEEE Transac-tions on Instrumentation and Measurement vol 63 no 6 pp1656ndash1665 2014

[8] A M Hasan K Samsudin A R Ramli and R S AzmirldquoAutomatic estimation of inertial navigation system errors forglobal positioning system outage recoveryrdquo Proceedings of theInstitution of Mechanical Engineers Part G Journal of AerospaceEngineering vol 225 no 1 pp 86ndash96 2011

[9] X Chen C Shen W-B Zhang M Tomizuka Y Xu andK Chiu ldquoNovel hybrid of strong tracking Kalman filter andwavelet neural network for GPSINS during GPS outagesrdquoMeasurement vol 46 no 10 pp 3847ndash3854 2013

[10] S J Julier and J K Uhlmann ldquoA new approach for filteringnonlinear systemrdquo in Proceedings of the American ControlConference vol 3 pp 1628ndash1632 IEEE SeattleWashUSA June1995

[11] P-B Ma H-X Baoyin and J-S Mu ldquoAutonomous navigationof Mars probe based on optical observation of Martian moonrdquoOptics and Precision Engineering vol 22 no 4 pp 863ndash8692014

[12] E Shin Estimation Techniques for Low-Cost Inertial NavigationThe University of Calgary Calgary Canada 2005

[13] Y Yi On improving the accuracy and reliability of GPSINS-based direct sensor georeferencing [PhD thesis] Ohio StateUniversity Columbus Ohio USA 2007

[14] Y Hao Z Xiong F Sun and X Wang ldquoComparison ofunscented Kalman filtersrdquo in Proceedings of the IEEE Interna-tional Conference on Mechatronics and Automation pp 895ndash899 Harbin China August 2007

[15] R M Rogers Applied Mathematics in Integrated NavigationSystems American Institute of Aeronautics and AstronauticsReston Va USA 3rd edition 2007

[16] D H Titterton and J L Weston Strapdown Inertial NavigationTechnology The Institution of Electrical Engineers MichaelFaraday House Stevenage UK 2nd edition 2004

[17] G Yan W Yan and D Xu ldquoA SINS nonlinear error modelreflecting better characteristics of SINS errorsrdquo Journal ofNorthwestern Polytechnical University vol 27 no 4 pp 511ndash5162009

[18] G M Yan W S Yan and D M Xu ldquoApplication of simplifiedUKF in SINS initial alignment for large misalignment anglesrdquoJournal of Chinese Inertial Technology vol 16 no 3 pp 253ndash2642008

[19] P S Maybeck Stochastic Models Estimation and ControlAcademic Press New York NY USA 1979

[20] S Liu Research and implementation of GPSINS integrated nav-igation algorithm [MS thesis] PLA Information EngineeringUniversity Zhengzhou China 2012 (Chinese)

[21] M S Grewal and A P Andrews Kalman Filtering Theory andPractice Using MATLAB John Wiley amp Sons Hoboken NJUSA 3rd edition 2008

[22] M C Vandyke J L Schwartz and C D Hall ldquoUnscentedKalman filtering for spacecraft attitude state and parameterestimationrdquo Advances in the Astronautical Sciences vol 119 no1 pp 217ndash228 2005

[23] Y Y Qin H Y Zhang and S S Wang Theory of KalmanFilter and Integrated Navigation North-Western PolytechnicalUniversity Press Xirsquoan China 2012 (Chinese)

Submit your manuscripts athttpwwwhindawicom

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical Problems in Engineering

Hindawi Publishing Corporationhttpwwwhindawicom

Differential EquationsInternational Journal of

Volume 2014

Applied MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical PhysicsAdvances in

Complex AnalysisJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

OptimizationJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Operations ResearchAdvances in

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Function Spaces

Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of Mathematics and Mathematical Sciences

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Algebra

Discrete Dynamics in Nature and Society

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Decision SciencesAdvances in

Discrete MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom

Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Stochastic AnalysisInternational Journal of

6 Mathematical Problems in Engineering

One-step predictionand SINS information

One-step prediction

GPS signalbreaks off

SINS update Filter update

CorrectSINS

GPS receiver

GPS signalrecovers

and SINS information

SINSinformation

One-step prediction

State estimate xk+n

CalculateΦk|kminus1 Qkminus1

with F(t) G(t)

xk|kminus1Pk|kminus1

xk+n|kminus1Pk+n|kminus1

Calculate 120585k+n|kminus1 with 120594k+nminus1|kminus1

Calculate 120594k+nminus1|kminus1 withxk+nminus1|kminus1Pk+nminus1|kminus1

xk+s|kminus1Pk+s|kminus1

Calculate 120585k+s|kminus1 with 120594k+sminus1|kminus1

Calculate 120594k+sminus1|kminus1 withxk+sminus1|kminus1Pk+sminus1|kminus1

t = k minus 1

t = k

middot middot middotmiddot middot middot

middot middot middotmiddot middot middot

t = k + s minus 1

t = k + s minus 05

t = k + s

t = k + n minus 1

t = k + n minus 05

t = k + n

rGPS k+n

vGPS k+n Error covariance Pk+n

Figure 3 Filter updates procedure during GPS outage

Table 3 Comparison of navigation error between EKF and hybrid KF-UKF after GPS outage

Outages length Adopted approach RMS attitude error (∘) RMS velocity error (ms) RMS position error (m)Roll Pitch Yaw North East Down Latitude Longitude Height

50 s EKF 00451 00566 02605 00256 00233 00331 11990 08245 33450Hybrid KF-UKF 00438 00549 02499 00255 00233 00331 11984 08199 32310

100 s EKF 00553 01050 02630 00286 00273 00367 11832 37463 30911Hybrid KF-UKF 00508 00767 01706 00240 00259 00339 11701 37434 39175

150 s EKF 00616 00590 01282 00363 00341 00479 13913 22053 31303Hybrid KF-UKF 00489 00367 01140 00219 00237 00333 13876 21949 39078

200 s EKF 00969 00560 17882 00431 00544 00619 15559 39014 36364Hybrid KF-UKF 00586 00403 17131 00231 00318 00360 15552 38998 35416

Path generator

Integration algorithm

IMU stochastic error

GPS stochastic errorGPS outage flag

Results comparison

a 120596 v r 120595

aIMU 120596IMU vGPS rGPS

vnav rnav120595nav

120575v

120575r

120575120595

Figure 4 Schematic diagram of the simulation

GPS data so the integration algorithm is able to judgewhetherGPS signal is lost Finally velocity position and attitude data

calculated by hybrid KF-UKF and normal EKF algorithm arecompared The simulation parameters are shown in Table 1

42 Simulation Results and Analysis In the simulation testthe aircraft has done a series of maneuvers which are listed inTable 2 and its acceleration angular rate velocity positionand attitude information was recorded We calculate thenavigation parameters of the aircraft by integrating SINS andGPS using the normal EKF and hybrid KF-UKF respectivelyThen the results were compared as is shown in Figure 5 andTable 3

Figure 5 is the comparison of attitude error curves of EKFand hybrid KF-UKF which are taken for example As weobserve the GPS signal was lost at 119905 = 70 s and recovered

Mathematical Problems in Engineering 7

270 275minus04

minus02

0

02

04

270 272 274minus02

minus01

0

Hybrid KF-UKFEKF

Hybrid KF-UKFEKF

GPS recovery timeGPS recovery time

GPS lost time GPS lost time

minus05

0

05

1

15

Roll

erro

r (∘)

minus05

0

05

1

Pitc

h er

ror (

∘)

100 150 200 250 30050

t (s)100 150 200 250 30050

t (s)

Figure 5 Comparison of attitude error between EKF and hybrid KF-UKF

270 271 272minus04

minus02

0

02

270 271 272minus01

001020304

Hybrid KF-UKFEKF

Hybrid KF-UKFEKF

GPS lost timeGPS lost time

GPS recovery time

GPS recovery time

minus12

minus10

minus8

minus6

minus4

minus2

02

Nor

th v

eloci

ty er

ror (

ms

)

100 150 200 250 30050t (s)

0

5

10

15

East

velo

city

erro

r (m

s)

100 150 200 250 30050t (s)

Figure 6 Comparison of velocity error between EKF and hybrid KF-UKF

at 119905 = 270 s During the 200 seconds GPS outage the rolland pitch angle error grew with time because the accuracy ofMEMS inertial sensors is very low and no GPS measurementinformation could be used for the filter to correct the SINSerror When the navigation system received GPS signal againat 119905 = 270 s the filter started to correct SINS error againIn the figure we can notice that after GPS information wasrecovered the attitude error reduced quickly However theattitude calculated by using hybrid KF-UKF is more accuratethan that calculated by EKF which is shown in the circle inthe figure This phenomenon appears for the reason that aswe described before the MEMS-IMU sensor caused largenonlinear error during GPS outage Based on the nonlinearSINS error model UT is able to predict SINS errors betterthan EKF thus the filter can correct the SINS errors quicklyand accurately And the similar phenomenon also occurs tothe velocity which is shown in Figure 6

Table 3 is the comparison of navigation error betweenEKF and hybrid KF-UKF after GPS outage For attitude errorwe can notice that the error of yaw is larger than that of rolland pitch owing to the poor observability of yawing angle invelocityposition integrationmode thus the filter is unable toestimate its error correctly By comparing the RMS attitudeerror and velocity error after GPS outage it is shown that thenavigation error is lower if the hybrid KF-UKF algorithm isapplied Also it is shown in Table 3 that with the GPS outageperiod increases the hybrid KF-UKF algorithm was betterand better compared to the EKF algorithm That is becausethe error of an inertial navigation system accumulates withthe increase The longer the GPS outage is the larger the

nonlinear error the MEMS-IMU causes And the hybridKF-UKF algorithm will show more superiority to normalEKF algorithm We can also notice that the latitude andlongitude accuracy has little improvement on the contrary toattitude and velocity And this can be explained by examiningthe SINS error equations (9) to (11) In formulas (9) and(10) the attitude error and velocity error are affected by thenonlinear terms C119899

1015840

119899and Cminus1

120595whose values grew fast during

GPS outage However when we refer to formula (11) wecan see that the numerical value of 120575119871 120575119877119864 and 120575119877119873 isvery small and little nonlinear error is caused during GPSoutage because the aircraft did notmove quite far away duringthe simulation so the EKF is still able to correct its erroraccurately when GPS signal recovers

In Table 4 we list the calculation amounts of EKF andUKF so that we can estimate the computational cost of theproposed combined algorithm The calculation amounts ofthe algorithm are evaluated using the floating-point opera-tions (flops) which is defined as the operation of addingdecreasing multiplying or dividing between two floatingnumbers For example if we add one (119899 times 119898) dimensionmatrix to another (119899 times 119898) dimension matrix then 119899119898 flopshave been generated in the computer In Table 4 we supposethe dimension of the state vector is 119899 and the measurementvector dimension is 119897 During the prediction updating period4119899

3+119899

2minus119899 flops are generated in EKFwhile 6(23)1198993+121198992+

3119899 flops are generated in UKF In SINSGNSS integration119899 = 15 and 119897 = 6 So there will be 13710 flops in EKF and25245 flops in UKF during the prediction updating periodWhat is more in Unscented Transform the state prediction

8 Mathematical Problems in Engineering

Table 4 Comparison of computational cost of EKF and UKF

EKF UKF (for linear measurement equation)Step Flops Step Flops

Prediction update

Generate sigmapoints120594119896minus1

23119899

3+ 3119899

2+ 119899

State predictionx119896|119896minus1

2119899

2minus 119899

State predictionx119896|119896minus1

4119899

2+ 119899

Error covarianceprediction P119896|119896minus1

4119899

3minus 119899

2

Error covarianceprediction

P119896|119896minus16119899

3+ 5119899

2+ 119899

Subtotal 4119899

3+ 119899

2minus 119899 Subtotal 6(23)119899

3+ 12119899

2+ 3119899

Measurement update

Filter gainK119896

4119899

2119897 + 4119899119897

2+ 119897

3minus 3119899119897

Filter gainK119896

4119899

2119897 + 4119899119897

2+ 119897

3minus 3119899119897

State updatex119896

4119899119897

State updatex119896

4119899119897

Error covariance updateP119896

2119899

3+ 2119899

2119897 minus 119899

2

Error covarianceupdateP119896

2119899

3+ 2119899

2119897 minus 119899

2

Subtotal 2119899

3+6119899

2119897+4119899119897

2+119897

3+119899119897minus119899

2 Subtotal 2119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+ 119899119897 minus 119899

2

In total 6119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+119899119897 minus 119899 In total 8(23)119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+ 11119899

2+

119899119897 + 3119899

119899 = 15 119897 = 6 30801 119899 = 15 l = 6 42336

is performed by solving the error state differential equationswith Runge-Kutta method (2119899 + 1) times since each sigmapoint has to be predictedThat is to say more than 25245 flopsare generated in one filter recycle during GPS outages whenwe predict the SINS error using Unscented Transform So infact the computational cost of UKF is much larger than thatin EKF in the prediction updating part thus the hybrid UKF-EKF is recommended to reduce the computational burden forthe computer In hybridUKF-EKF theUnscented Transformwhich is necessary to predict the nonlinear SINS error onlyperforms during GPS outages Last but not least the amountof flops in EKF during the measurement updating period is2119899

3+6119899

2119897+4119899119897

2+119897

3+119899119897minus119899

2 in this case 17091 as well as thatin UKFThis is for the reason that in SINSGNSS integrationthe measurement equation is linear and no sigma point isneeded in the measurement updating part When GPS signalis functioning well there will be 30801 flops in each filtercycle

5 Conclusion

This paper presents a hybrid KF-UKF algorithm for real-timeMEMS-SINSGPS integrationThe linear and nonlinearSINS models are discussed The flowchart of the hybrid KF-UKF algorithm is described The SINS error is estimated andcorrected with linear SINS model when GPS is functioningwell while it is predicted with nonlinear SINS model byapplying Unscented Transform during GPS outage Thesimulation results indicate that compared to normal EKFalgorithm the hybrid KF-UKF algorithm is able to predictthe SINS error more accurately if GPS suffers from long-time GPS outage and the navigation error is lower after GPS

signal was regainedThe results also show that the hybrid KF-UKF algorithm is more efficient for attitude error predictionbut the effect on position error is not evident Comparedwith normal UKF the hybrid KF-UKF algorithm has lowcalculation amount In this paper the remaining problemwhich we have not solved is that SINS errors still grow fastduring GPS outage So in our future work we may combinethe UKF with ANN or SVM to create a new algorithm toreduce the SINS errors during GPS outage

Conflict of Interests

The authors declare that there is no conflict of interestsregarding the publication of this paper

Acknowledgments

This research is supported by the 3rd Innovation Fund ofChangchun Institute of Optics Fine Mechanics and Physics(CIOMP) and the UAV semi-physical simulation platformresearch project Chinese Science Academy (CSA)

References

[1] P D Groves Principles of GNSS Inertial and MultisensorIntegrated Navigation Systems Artech House Norwood MassUSA 2008

[2] Y Yuksel N El-Sheimy andANoureldin ldquoErrormodeling andcharacterization of environmental effects for low cost inertialMEMS unitsrdquo in Proceedings of the IEEEION Position Locationand Navigation Symposium pp 598ndash612 Indian Wells CalifUSA May 2010

Mathematical Problems in Engineering 9

[3] P Aggarwal Z Syed X Niu and N El-Sheimy ldquoA standardtesting and calibration procedure for low cost MEMS inertialsensors and unitsrdquo The Journal of Navigation vol 61 no 2 pp323ndash336 2008

[4] S Godha and M E Cannon ldquoGPSMEMS INS integratedsystem for navigation in urban areasrdquo GPS Solutions vol 11 no3 pp 193ndash203 2007

[5] M Ilyas Y Yang Q S Qian and R Zhang ldquoLow-cost IMUodometerGPS integrated navigation aided with two antennaeheading measurement for land vehicle applicationrdquo in Proceed-ings of the 25th Chinese Control andDecision Conference (CCDCrsquo13) pp 4521ndash4526 Guiyang China May 2013

[6] Z Jiang C Liu G Zhang Y P Wang C Huang and J LiangldquoGPSINS integrated navigation based on UKF and simulatedannealing optimized SVMrdquo in Proceedings of the IEEE 78thVehicular Technology Conference (VTC Fall rsquo13) vol 14 pp 1ndash5 Las Vegas Nev USA September 2013

[7] L Chen and J Fang ldquoA hybrid prediction method for bridgingGPS outages in high-precision POS applicationrdquo IEEE Transac-tions on Instrumentation and Measurement vol 63 no 6 pp1656ndash1665 2014

[8] A M Hasan K Samsudin A R Ramli and R S AzmirldquoAutomatic estimation of inertial navigation system errors forglobal positioning system outage recoveryrdquo Proceedings of theInstitution of Mechanical Engineers Part G Journal of AerospaceEngineering vol 225 no 1 pp 86ndash96 2011

[9] X Chen C Shen W-B Zhang M Tomizuka Y Xu andK Chiu ldquoNovel hybrid of strong tracking Kalman filter andwavelet neural network for GPSINS during GPS outagesrdquoMeasurement vol 46 no 10 pp 3847ndash3854 2013

[10] S J Julier and J K Uhlmann ldquoA new approach for filteringnonlinear systemrdquo in Proceedings of the American ControlConference vol 3 pp 1628ndash1632 IEEE SeattleWashUSA June1995

[11] P-B Ma H-X Baoyin and J-S Mu ldquoAutonomous navigationof Mars probe based on optical observation of Martian moonrdquoOptics and Precision Engineering vol 22 no 4 pp 863ndash8692014

[12] E Shin Estimation Techniques for Low-Cost Inertial NavigationThe University of Calgary Calgary Canada 2005

[13] Y Yi On improving the accuracy and reliability of GPSINS-based direct sensor georeferencing [PhD thesis] Ohio StateUniversity Columbus Ohio USA 2007

[14] Y Hao Z Xiong F Sun and X Wang ldquoComparison ofunscented Kalman filtersrdquo in Proceedings of the IEEE Interna-tional Conference on Mechatronics and Automation pp 895ndash899 Harbin China August 2007

[15] R M Rogers Applied Mathematics in Integrated NavigationSystems American Institute of Aeronautics and AstronauticsReston Va USA 3rd edition 2007

[16] D H Titterton and J L Weston Strapdown Inertial NavigationTechnology The Institution of Electrical Engineers MichaelFaraday House Stevenage UK 2nd edition 2004

[17] G Yan W Yan and D Xu ldquoA SINS nonlinear error modelreflecting better characteristics of SINS errorsrdquo Journal ofNorthwestern Polytechnical University vol 27 no 4 pp 511ndash5162009

[18] G M Yan W S Yan and D M Xu ldquoApplication of simplifiedUKF in SINS initial alignment for large misalignment anglesrdquoJournal of Chinese Inertial Technology vol 16 no 3 pp 253ndash2642008

[19] P S Maybeck Stochastic Models Estimation and ControlAcademic Press New York NY USA 1979

[20] S Liu Research and implementation of GPSINS integrated nav-igation algorithm [MS thesis] PLA Information EngineeringUniversity Zhengzhou China 2012 (Chinese)

[21] M S Grewal and A P Andrews Kalman Filtering Theory andPractice Using MATLAB John Wiley amp Sons Hoboken NJUSA 3rd edition 2008

[22] M C Vandyke J L Schwartz and C D Hall ldquoUnscentedKalman filtering for spacecraft attitude state and parameterestimationrdquo Advances in the Astronautical Sciences vol 119 no1 pp 217ndash228 2005

[23] Y Y Qin H Y Zhang and S S Wang Theory of KalmanFilter and Integrated Navigation North-Western PolytechnicalUniversity Press Xirsquoan China 2012 (Chinese)

Submit your manuscripts athttpwwwhindawicom

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical Problems in Engineering

Hindawi Publishing Corporationhttpwwwhindawicom

Differential EquationsInternational Journal of

Volume 2014

Applied MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical PhysicsAdvances in

Complex AnalysisJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

OptimizationJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Operations ResearchAdvances in

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Function Spaces

Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of Mathematics and Mathematical Sciences

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Algebra

Discrete Dynamics in Nature and Society

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Decision SciencesAdvances in

Discrete MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom

Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Stochastic AnalysisInternational Journal of

Mathematical Problems in Engineering 7

270 275minus04

minus02

0

02

04

270 272 274minus02

minus01

0

Hybrid KF-UKFEKF

Hybrid KF-UKFEKF

GPS recovery timeGPS recovery time

GPS lost time GPS lost time

minus05

0

05

1

15

Roll

erro

r (∘)

minus05

0

05

1

Pitc

h er

ror (

∘)

100 150 200 250 30050

t (s)100 150 200 250 30050

t (s)

Figure 5 Comparison of attitude error between EKF and hybrid KF-UKF

270 271 272minus04

minus02

0

02

270 271 272minus01

001020304

Hybrid KF-UKFEKF

Hybrid KF-UKFEKF

GPS lost timeGPS lost time

GPS recovery time

GPS recovery time

minus12

minus10

minus8

minus6

minus4

minus2

02

Nor

th v

eloci

ty er

ror (

ms

)

100 150 200 250 30050t (s)

0

5

10

15

East

velo

city

erro

r (m

s)

100 150 200 250 30050t (s)

Figure 6 Comparison of velocity error between EKF and hybrid KF-UKF

at 119905 = 270 s During the 200 seconds GPS outage the rolland pitch angle error grew with time because the accuracy ofMEMS inertial sensors is very low and no GPS measurementinformation could be used for the filter to correct the SINSerror When the navigation system received GPS signal againat 119905 = 270 s the filter started to correct SINS error againIn the figure we can notice that after GPS information wasrecovered the attitude error reduced quickly However theattitude calculated by using hybrid KF-UKF is more accuratethan that calculated by EKF which is shown in the circle inthe figure This phenomenon appears for the reason that aswe described before the MEMS-IMU sensor caused largenonlinear error during GPS outage Based on the nonlinearSINS error model UT is able to predict SINS errors betterthan EKF thus the filter can correct the SINS errors quicklyand accurately And the similar phenomenon also occurs tothe velocity which is shown in Figure 6

Table 3 is the comparison of navigation error betweenEKF and hybrid KF-UKF after GPS outage For attitude errorwe can notice that the error of yaw is larger than that of rolland pitch owing to the poor observability of yawing angle invelocityposition integrationmode thus the filter is unable toestimate its error correctly By comparing the RMS attitudeerror and velocity error after GPS outage it is shown that thenavigation error is lower if the hybrid KF-UKF algorithm isapplied Also it is shown in Table 3 that with the GPS outageperiod increases the hybrid KF-UKF algorithm was betterand better compared to the EKF algorithm That is becausethe error of an inertial navigation system accumulates withthe increase The longer the GPS outage is the larger the

nonlinear error the MEMS-IMU causes And the hybridKF-UKF algorithm will show more superiority to normalEKF algorithm We can also notice that the latitude andlongitude accuracy has little improvement on the contrary toattitude and velocity And this can be explained by examiningthe SINS error equations (9) to (11) In formulas (9) and(10) the attitude error and velocity error are affected by thenonlinear terms C119899

1015840

119899and Cminus1

120595whose values grew fast during

GPS outage However when we refer to formula (11) wecan see that the numerical value of 120575119871 120575119877119864 and 120575119877119873 isvery small and little nonlinear error is caused during GPSoutage because the aircraft did notmove quite far away duringthe simulation so the EKF is still able to correct its erroraccurately when GPS signal recovers

In Table 4 we list the calculation amounts of EKF andUKF so that we can estimate the computational cost of theproposed combined algorithm The calculation amounts ofthe algorithm are evaluated using the floating-point opera-tions (flops) which is defined as the operation of addingdecreasing multiplying or dividing between two floatingnumbers For example if we add one (119899 times 119898) dimensionmatrix to another (119899 times 119898) dimension matrix then 119899119898 flopshave been generated in the computer In Table 4 we supposethe dimension of the state vector is 119899 and the measurementvector dimension is 119897 During the prediction updating period4119899

3+119899

2minus119899 flops are generated in EKFwhile 6(23)1198993+121198992+

3119899 flops are generated in UKF In SINSGNSS integration119899 = 15 and 119897 = 6 So there will be 13710 flops in EKF and25245 flops in UKF during the prediction updating periodWhat is more in Unscented Transform the state prediction

8 Mathematical Problems in Engineering

Table 4 Comparison of computational cost of EKF and UKF

EKF UKF (for linear measurement equation)Step Flops Step Flops

Prediction update

Generate sigmapoints120594119896minus1

23119899

3+ 3119899

2+ 119899

State predictionx119896|119896minus1

2119899

2minus 119899

State predictionx119896|119896minus1

4119899

2+ 119899

Error covarianceprediction P119896|119896minus1

4119899

3minus 119899

2

Error covarianceprediction

P119896|119896minus16119899

3+ 5119899

2+ 119899

Subtotal 4119899

3+ 119899

2minus 119899 Subtotal 6(23)119899

3+ 12119899

2+ 3119899

Measurement update

Filter gainK119896

4119899

2119897 + 4119899119897

2+ 119897

3minus 3119899119897

Filter gainK119896

4119899

2119897 + 4119899119897

2+ 119897

3minus 3119899119897

State updatex119896

4119899119897

State updatex119896

4119899119897

Error covariance updateP119896

2119899

3+ 2119899

2119897 minus 119899

2

Error covarianceupdateP119896

2119899

3+ 2119899

2119897 minus 119899

2

Subtotal 2119899

3+6119899

2119897+4119899119897

2+119897

3+119899119897minus119899

2 Subtotal 2119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+ 119899119897 minus 119899

2

In total 6119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+119899119897 minus 119899 In total 8(23)119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+ 11119899

2+

119899119897 + 3119899

119899 = 15 119897 = 6 30801 119899 = 15 l = 6 42336

is performed by solving the error state differential equationswith Runge-Kutta method (2119899 + 1) times since each sigmapoint has to be predictedThat is to say more than 25245 flopsare generated in one filter recycle during GPS outages whenwe predict the SINS error using Unscented Transform So infact the computational cost of UKF is much larger than thatin EKF in the prediction updating part thus the hybrid UKF-EKF is recommended to reduce the computational burden forthe computer In hybridUKF-EKF theUnscented Transformwhich is necessary to predict the nonlinear SINS error onlyperforms during GPS outages Last but not least the amountof flops in EKF during the measurement updating period is2119899

3+6119899

2119897+4119899119897

2+119897

3+119899119897minus119899

2 in this case 17091 as well as thatin UKFThis is for the reason that in SINSGNSS integrationthe measurement equation is linear and no sigma point isneeded in the measurement updating part When GPS signalis functioning well there will be 30801 flops in each filtercycle

5 Conclusion

This paper presents a hybrid KF-UKF algorithm for real-timeMEMS-SINSGPS integrationThe linear and nonlinearSINS models are discussed The flowchart of the hybrid KF-UKF algorithm is described The SINS error is estimated andcorrected with linear SINS model when GPS is functioningwell while it is predicted with nonlinear SINS model byapplying Unscented Transform during GPS outage Thesimulation results indicate that compared to normal EKFalgorithm the hybrid KF-UKF algorithm is able to predictthe SINS error more accurately if GPS suffers from long-time GPS outage and the navigation error is lower after GPS

signal was regainedThe results also show that the hybrid KF-UKF algorithm is more efficient for attitude error predictionbut the effect on position error is not evident Comparedwith normal UKF the hybrid KF-UKF algorithm has lowcalculation amount In this paper the remaining problemwhich we have not solved is that SINS errors still grow fastduring GPS outage So in our future work we may combinethe UKF with ANN or SVM to create a new algorithm toreduce the SINS errors during GPS outage

Conflict of Interests

The authors declare that there is no conflict of interestsregarding the publication of this paper

Acknowledgments

This research is supported by the 3rd Innovation Fund ofChangchun Institute of Optics Fine Mechanics and Physics(CIOMP) and the UAV semi-physical simulation platformresearch project Chinese Science Academy (CSA)

References

[1] P D Groves Principles of GNSS Inertial and MultisensorIntegrated Navigation Systems Artech House Norwood MassUSA 2008

[2] Y Yuksel N El-Sheimy andANoureldin ldquoErrormodeling andcharacterization of environmental effects for low cost inertialMEMS unitsrdquo in Proceedings of the IEEEION Position Locationand Navigation Symposium pp 598ndash612 Indian Wells CalifUSA May 2010

Mathematical Problems in Engineering 9

[3] P Aggarwal Z Syed X Niu and N El-Sheimy ldquoA standardtesting and calibration procedure for low cost MEMS inertialsensors and unitsrdquo The Journal of Navigation vol 61 no 2 pp323ndash336 2008

[4] S Godha and M E Cannon ldquoGPSMEMS INS integratedsystem for navigation in urban areasrdquo GPS Solutions vol 11 no3 pp 193ndash203 2007

[5] M Ilyas Y Yang Q S Qian and R Zhang ldquoLow-cost IMUodometerGPS integrated navigation aided with two antennaeheading measurement for land vehicle applicationrdquo in Proceed-ings of the 25th Chinese Control andDecision Conference (CCDCrsquo13) pp 4521ndash4526 Guiyang China May 2013

[6] Z Jiang C Liu G Zhang Y P Wang C Huang and J LiangldquoGPSINS integrated navigation based on UKF and simulatedannealing optimized SVMrdquo in Proceedings of the IEEE 78thVehicular Technology Conference (VTC Fall rsquo13) vol 14 pp 1ndash5 Las Vegas Nev USA September 2013

[7] L Chen and J Fang ldquoA hybrid prediction method for bridgingGPS outages in high-precision POS applicationrdquo IEEE Transac-tions on Instrumentation and Measurement vol 63 no 6 pp1656ndash1665 2014

[8] A M Hasan K Samsudin A R Ramli and R S AzmirldquoAutomatic estimation of inertial navigation system errors forglobal positioning system outage recoveryrdquo Proceedings of theInstitution of Mechanical Engineers Part G Journal of AerospaceEngineering vol 225 no 1 pp 86ndash96 2011

[9] X Chen C Shen W-B Zhang M Tomizuka Y Xu andK Chiu ldquoNovel hybrid of strong tracking Kalman filter andwavelet neural network for GPSINS during GPS outagesrdquoMeasurement vol 46 no 10 pp 3847ndash3854 2013

[10] S J Julier and J K Uhlmann ldquoA new approach for filteringnonlinear systemrdquo in Proceedings of the American ControlConference vol 3 pp 1628ndash1632 IEEE SeattleWashUSA June1995

[11] P-B Ma H-X Baoyin and J-S Mu ldquoAutonomous navigationof Mars probe based on optical observation of Martian moonrdquoOptics and Precision Engineering vol 22 no 4 pp 863ndash8692014

[12] E Shin Estimation Techniques for Low-Cost Inertial NavigationThe University of Calgary Calgary Canada 2005

[13] Y Yi On improving the accuracy and reliability of GPSINS-based direct sensor georeferencing [PhD thesis] Ohio StateUniversity Columbus Ohio USA 2007

[14] Y Hao Z Xiong F Sun and X Wang ldquoComparison ofunscented Kalman filtersrdquo in Proceedings of the IEEE Interna-tional Conference on Mechatronics and Automation pp 895ndash899 Harbin China August 2007

[15] R M Rogers Applied Mathematics in Integrated NavigationSystems American Institute of Aeronautics and AstronauticsReston Va USA 3rd edition 2007

[16] D H Titterton and J L Weston Strapdown Inertial NavigationTechnology The Institution of Electrical Engineers MichaelFaraday House Stevenage UK 2nd edition 2004

[17] G Yan W Yan and D Xu ldquoA SINS nonlinear error modelreflecting better characteristics of SINS errorsrdquo Journal ofNorthwestern Polytechnical University vol 27 no 4 pp 511ndash5162009

[18] G M Yan W S Yan and D M Xu ldquoApplication of simplifiedUKF in SINS initial alignment for large misalignment anglesrdquoJournal of Chinese Inertial Technology vol 16 no 3 pp 253ndash2642008

[19] P S Maybeck Stochastic Models Estimation and ControlAcademic Press New York NY USA 1979

[20] S Liu Research and implementation of GPSINS integrated nav-igation algorithm [MS thesis] PLA Information EngineeringUniversity Zhengzhou China 2012 (Chinese)

[21] M S Grewal and A P Andrews Kalman Filtering Theory andPractice Using MATLAB John Wiley amp Sons Hoboken NJUSA 3rd edition 2008

[22] M C Vandyke J L Schwartz and C D Hall ldquoUnscentedKalman filtering for spacecraft attitude state and parameterestimationrdquo Advances in the Astronautical Sciences vol 119 no1 pp 217ndash228 2005

[23] Y Y Qin H Y Zhang and S S Wang Theory of KalmanFilter and Integrated Navigation North-Western PolytechnicalUniversity Press Xirsquoan China 2012 (Chinese)

Submit your manuscripts athttpwwwhindawicom

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical Problems in Engineering

Hindawi Publishing Corporationhttpwwwhindawicom

Differential EquationsInternational Journal of

Volume 2014

Applied MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical PhysicsAdvances in

Complex AnalysisJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

OptimizationJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Operations ResearchAdvances in

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Function Spaces

Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of Mathematics and Mathematical Sciences

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Algebra

Discrete Dynamics in Nature and Society

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Decision SciencesAdvances in

Discrete MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom

Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Stochastic AnalysisInternational Journal of

8 Mathematical Problems in Engineering

Table 4 Comparison of computational cost of EKF and UKF

EKF UKF (for linear measurement equation)Step Flops Step Flops

Prediction update

Generate sigmapoints120594119896minus1

23119899

3+ 3119899

2+ 119899

State predictionx119896|119896minus1

2119899

2minus 119899

State predictionx119896|119896minus1

4119899

2+ 119899

Error covarianceprediction P119896|119896minus1

4119899

3minus 119899

2

Error covarianceprediction

P119896|119896minus16119899

3+ 5119899

2+ 119899

Subtotal 4119899

3+ 119899

2minus 119899 Subtotal 6(23)119899

3+ 12119899

2+ 3119899

Measurement update

Filter gainK119896

4119899

2119897 + 4119899119897

2+ 119897

3minus 3119899119897

Filter gainK119896

4119899

2119897 + 4119899119897

2+ 119897

3minus 3119899119897

State updatex119896

4119899119897

State updatex119896

4119899119897

Error covariance updateP119896

2119899

3+ 2119899

2119897 minus 119899

2

Error covarianceupdateP119896

2119899

3+ 2119899

2119897 minus 119899

2

Subtotal 2119899

3+6119899

2119897+4119899119897

2+119897

3+119899119897minus119899

2 Subtotal 2119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+ 119899119897 minus 119899

2

In total 6119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+119899119897 minus 119899 In total 8(23)119899

3+ 6119899

2119897 + 4119899119897

2+ 119897

3+ 11119899

2+

119899119897 + 3119899

119899 = 15 119897 = 6 30801 119899 = 15 l = 6 42336

is performed by solving the error state differential equationswith Runge-Kutta method (2119899 + 1) times since each sigmapoint has to be predictedThat is to say more than 25245 flopsare generated in one filter recycle during GPS outages whenwe predict the SINS error using Unscented Transform So infact the computational cost of UKF is much larger than thatin EKF in the prediction updating part thus the hybrid UKF-EKF is recommended to reduce the computational burden forthe computer In hybridUKF-EKF theUnscented Transformwhich is necessary to predict the nonlinear SINS error onlyperforms during GPS outages Last but not least the amountof flops in EKF during the measurement updating period is2119899

3+6119899

2119897+4119899119897

2+119897

3+119899119897minus119899

2 in this case 17091 as well as thatin UKFThis is for the reason that in SINSGNSS integrationthe measurement equation is linear and no sigma point isneeded in the measurement updating part When GPS signalis functioning well there will be 30801 flops in each filtercycle

5 Conclusion

This paper presents a hybrid KF-UKF algorithm for real-timeMEMS-SINSGPS integrationThe linear and nonlinearSINS models are discussed The flowchart of the hybrid KF-UKF algorithm is described The SINS error is estimated andcorrected with linear SINS model when GPS is functioningwell while it is predicted with nonlinear SINS model byapplying Unscented Transform during GPS outage Thesimulation results indicate that compared to normal EKFalgorithm the hybrid KF-UKF algorithm is able to predictthe SINS error more accurately if GPS suffers from long-time GPS outage and the navigation error is lower after GPS

signal was regainedThe results also show that the hybrid KF-UKF algorithm is more efficient for attitude error predictionbut the effect on position error is not evident Comparedwith normal UKF the hybrid KF-UKF algorithm has lowcalculation amount In this paper the remaining problemwhich we have not solved is that SINS errors still grow fastduring GPS outage So in our future work we may combinethe UKF with ANN or SVM to create a new algorithm toreduce the SINS errors during GPS outage

Conflict of Interests

The authors declare that there is no conflict of interestsregarding the publication of this paper

Acknowledgments

This research is supported by the 3rd Innovation Fund ofChangchun Institute of Optics Fine Mechanics and Physics(CIOMP) and the UAV semi-physical simulation platformresearch project Chinese Science Academy (CSA)

References

[1] P D Groves Principles of GNSS Inertial and MultisensorIntegrated Navigation Systems Artech House Norwood MassUSA 2008

[2] Y Yuksel N El-Sheimy andANoureldin ldquoErrormodeling andcharacterization of environmental effects for low cost inertialMEMS unitsrdquo in Proceedings of the IEEEION Position Locationand Navigation Symposium pp 598ndash612 Indian Wells CalifUSA May 2010

Mathematical Problems in Engineering 9

[3] P Aggarwal Z Syed X Niu and N El-Sheimy ldquoA standardtesting and calibration procedure for low cost MEMS inertialsensors and unitsrdquo The Journal of Navigation vol 61 no 2 pp323ndash336 2008

[4] S Godha and M E Cannon ldquoGPSMEMS INS integratedsystem for navigation in urban areasrdquo GPS Solutions vol 11 no3 pp 193ndash203 2007

[5] M Ilyas Y Yang Q S Qian and R Zhang ldquoLow-cost IMUodometerGPS integrated navigation aided with two antennaeheading measurement for land vehicle applicationrdquo in Proceed-ings of the 25th Chinese Control andDecision Conference (CCDCrsquo13) pp 4521ndash4526 Guiyang China May 2013

[6] Z Jiang C Liu G Zhang Y P Wang C Huang and J LiangldquoGPSINS integrated navigation based on UKF and simulatedannealing optimized SVMrdquo in Proceedings of the IEEE 78thVehicular Technology Conference (VTC Fall rsquo13) vol 14 pp 1ndash5 Las Vegas Nev USA September 2013

[7] L Chen and J Fang ldquoA hybrid prediction method for bridgingGPS outages in high-precision POS applicationrdquo IEEE Transac-tions on Instrumentation and Measurement vol 63 no 6 pp1656ndash1665 2014

[8] A M Hasan K Samsudin A R Ramli and R S AzmirldquoAutomatic estimation of inertial navigation system errors forglobal positioning system outage recoveryrdquo Proceedings of theInstitution of Mechanical Engineers Part G Journal of AerospaceEngineering vol 225 no 1 pp 86ndash96 2011

[9] X Chen C Shen W-B Zhang M Tomizuka Y Xu andK Chiu ldquoNovel hybrid of strong tracking Kalman filter andwavelet neural network for GPSINS during GPS outagesrdquoMeasurement vol 46 no 10 pp 3847ndash3854 2013

[10] S J Julier and J K Uhlmann ldquoA new approach for filteringnonlinear systemrdquo in Proceedings of the American ControlConference vol 3 pp 1628ndash1632 IEEE SeattleWashUSA June1995

[11] P-B Ma H-X Baoyin and J-S Mu ldquoAutonomous navigationof Mars probe based on optical observation of Martian moonrdquoOptics and Precision Engineering vol 22 no 4 pp 863ndash8692014

[12] E Shin Estimation Techniques for Low-Cost Inertial NavigationThe University of Calgary Calgary Canada 2005

[13] Y Yi On improving the accuracy and reliability of GPSINS-based direct sensor georeferencing [PhD thesis] Ohio StateUniversity Columbus Ohio USA 2007

[14] Y Hao Z Xiong F Sun and X Wang ldquoComparison ofunscented Kalman filtersrdquo in Proceedings of the IEEE Interna-tional Conference on Mechatronics and Automation pp 895ndash899 Harbin China August 2007

[15] R M Rogers Applied Mathematics in Integrated NavigationSystems American Institute of Aeronautics and AstronauticsReston Va USA 3rd edition 2007

[16] D H Titterton and J L Weston Strapdown Inertial NavigationTechnology The Institution of Electrical Engineers MichaelFaraday House Stevenage UK 2nd edition 2004

[17] G Yan W Yan and D Xu ldquoA SINS nonlinear error modelreflecting better characteristics of SINS errorsrdquo Journal ofNorthwestern Polytechnical University vol 27 no 4 pp 511ndash5162009

[18] G M Yan W S Yan and D M Xu ldquoApplication of simplifiedUKF in SINS initial alignment for large misalignment anglesrdquoJournal of Chinese Inertial Technology vol 16 no 3 pp 253ndash2642008

[19] P S Maybeck Stochastic Models Estimation and ControlAcademic Press New York NY USA 1979

[20] S Liu Research and implementation of GPSINS integrated nav-igation algorithm [MS thesis] PLA Information EngineeringUniversity Zhengzhou China 2012 (Chinese)

[21] M S Grewal and A P Andrews Kalman Filtering Theory andPractice Using MATLAB John Wiley amp Sons Hoboken NJUSA 3rd edition 2008

[22] M C Vandyke J L Schwartz and C D Hall ldquoUnscentedKalman filtering for spacecraft attitude state and parameterestimationrdquo Advances in the Astronautical Sciences vol 119 no1 pp 217ndash228 2005

[23] Y Y Qin H Y Zhang and S S Wang Theory of KalmanFilter and Integrated Navigation North-Western PolytechnicalUniversity Press Xirsquoan China 2012 (Chinese)

Submit your manuscripts athttpwwwhindawicom

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical Problems in Engineering

Hindawi Publishing Corporationhttpwwwhindawicom

Differential EquationsInternational Journal of

Volume 2014

Applied MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical PhysicsAdvances in

Complex AnalysisJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

OptimizationJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Operations ResearchAdvances in

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Function Spaces

Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of Mathematics and Mathematical Sciences

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Algebra

Discrete Dynamics in Nature and Society

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Decision SciencesAdvances in

Discrete MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom

Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Stochastic AnalysisInternational Journal of

Mathematical Problems in Engineering 9

[3] P Aggarwal Z Syed X Niu and N El-Sheimy ldquoA standardtesting and calibration procedure for low cost MEMS inertialsensors and unitsrdquo The Journal of Navigation vol 61 no 2 pp323ndash336 2008

[4] S Godha and M E Cannon ldquoGPSMEMS INS integratedsystem for navigation in urban areasrdquo GPS Solutions vol 11 no3 pp 193ndash203 2007

[5] M Ilyas Y Yang Q S Qian and R Zhang ldquoLow-cost IMUodometerGPS integrated navigation aided with two antennaeheading measurement for land vehicle applicationrdquo in Proceed-ings of the 25th Chinese Control andDecision Conference (CCDCrsquo13) pp 4521ndash4526 Guiyang China May 2013

[6] Z Jiang C Liu G Zhang Y P Wang C Huang and J LiangldquoGPSINS integrated navigation based on UKF and simulatedannealing optimized SVMrdquo in Proceedings of the IEEE 78thVehicular Technology Conference (VTC Fall rsquo13) vol 14 pp 1ndash5 Las Vegas Nev USA September 2013

[7] L Chen and J Fang ldquoA hybrid prediction method for bridgingGPS outages in high-precision POS applicationrdquo IEEE Transac-tions on Instrumentation and Measurement vol 63 no 6 pp1656ndash1665 2014

[8] A M Hasan K Samsudin A R Ramli and R S AzmirldquoAutomatic estimation of inertial navigation system errors forglobal positioning system outage recoveryrdquo Proceedings of theInstitution of Mechanical Engineers Part G Journal of AerospaceEngineering vol 225 no 1 pp 86ndash96 2011

[9] X Chen C Shen W-B Zhang M Tomizuka Y Xu andK Chiu ldquoNovel hybrid of strong tracking Kalman filter andwavelet neural network for GPSINS during GPS outagesrdquoMeasurement vol 46 no 10 pp 3847ndash3854 2013

[10] S J Julier and J K Uhlmann ldquoA new approach for filteringnonlinear systemrdquo in Proceedings of the American ControlConference vol 3 pp 1628ndash1632 IEEE SeattleWashUSA June1995

[11] P-B Ma H-X Baoyin and J-S Mu ldquoAutonomous navigationof Mars probe based on optical observation of Martian moonrdquoOptics and Precision Engineering vol 22 no 4 pp 863ndash8692014

[12] E Shin Estimation Techniques for Low-Cost Inertial NavigationThe University of Calgary Calgary Canada 2005

[13] Y Yi On improving the accuracy and reliability of GPSINS-based direct sensor georeferencing [PhD thesis] Ohio StateUniversity Columbus Ohio USA 2007

[14] Y Hao Z Xiong F Sun and X Wang ldquoComparison ofunscented Kalman filtersrdquo in Proceedings of the IEEE Interna-tional Conference on Mechatronics and Automation pp 895ndash899 Harbin China August 2007

[15] R M Rogers Applied Mathematics in Integrated NavigationSystems American Institute of Aeronautics and AstronauticsReston Va USA 3rd edition 2007

[16] D H Titterton and J L Weston Strapdown Inertial NavigationTechnology The Institution of Electrical Engineers MichaelFaraday House Stevenage UK 2nd edition 2004

[17] G Yan W Yan and D Xu ldquoA SINS nonlinear error modelreflecting better characteristics of SINS errorsrdquo Journal ofNorthwestern Polytechnical University vol 27 no 4 pp 511ndash5162009

[18] G M Yan W S Yan and D M Xu ldquoApplication of simplifiedUKF in SINS initial alignment for large misalignment anglesrdquoJournal of Chinese Inertial Technology vol 16 no 3 pp 253ndash2642008

[19] P S Maybeck Stochastic Models Estimation and ControlAcademic Press New York NY USA 1979

[20] S Liu Research and implementation of GPSINS integrated nav-igation algorithm [MS thesis] PLA Information EngineeringUniversity Zhengzhou China 2012 (Chinese)

[21] M S Grewal and A P Andrews Kalman Filtering Theory andPractice Using MATLAB John Wiley amp Sons Hoboken NJUSA 3rd edition 2008

[22] M C Vandyke J L Schwartz and C D Hall ldquoUnscentedKalman filtering for spacecraft attitude state and parameterestimationrdquo Advances in the Astronautical Sciences vol 119 no1 pp 217ndash228 2005

[23] Y Y Qin H Y Zhang and S S Wang Theory of KalmanFilter and Integrated Navigation North-Western PolytechnicalUniversity Press Xirsquoan China 2012 (Chinese)

Submit your manuscripts athttpwwwhindawicom

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical Problems in Engineering

Hindawi Publishing Corporationhttpwwwhindawicom

Differential EquationsInternational Journal of

Volume 2014

Applied MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical PhysicsAdvances in

Complex AnalysisJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

OptimizationJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Operations ResearchAdvances in

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Function Spaces

Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of Mathematics and Mathematical Sciences

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Algebra

Discrete Dynamics in Nature and Society

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Decision SciencesAdvances in

Discrete MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom

Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Stochastic AnalysisInternational Journal of

Submit your manuscripts athttpwwwhindawicom

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical Problems in Engineering

Hindawi Publishing Corporationhttpwwwhindawicom

Differential EquationsInternational Journal of

Volume 2014

Applied MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Probability and StatisticsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Mathematical PhysicsAdvances in

Complex AnalysisJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

OptimizationJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

CombinatoricsHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Operations ResearchAdvances in

Journal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Function Spaces

Abstract and Applied AnalysisHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

International Journal of Mathematics and Mathematical Sciences

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Algebra

Discrete Dynamics in Nature and Society

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Decision SciencesAdvances in

Discrete MathematicsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom

Volume 2014 Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014

Stochastic AnalysisInternational Journal of