Research Article Error Prediction for SINS/GPS after GPS ...downloads.hindawi.com/journals/mpe/2015/239426.pdf · Research Article Error Prediction for SINS/GPS after ... SINS and
Post on 22-Jun-2018
227 Views
Preview:
Transcript
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
top related