Research Article Navigation System Heading and...

Research ArticleNavigation System Heading and Position AccuracyImprovement through GPS and INS Data Fusion

Ji Hyoung Ryu1 Ganduulga Gankhuyag2 and Kil To Chong23

1Electronics and Telecommunications Research Institute Daejeon 34129 Republic of Korea2Electronics Engineering Chonbuk National University (CBNU) Jeonju 54896 Republic of Korea3Advanced Electronics and Information Research Center CBNU Jeonju 54896 Republic of Korea

Correspondence should be addressed to Kil To Chong kitchongjbnuackr

Received 5 July 2015 Revised 13 November 2015 Accepted 3 December 2015

Academic Editor Hana Vaisocherova

Copyright copy 2016 Ji Hyoung Ryu et al This is an open access article distributed under the Creative Commons Attribution Licensewhich permits unrestricted use distribution and reproduction in any medium provided the original work is properly cited

Commercial navigation systems currently in use have reduced position and heading error but are usually quite expensive It isproposed that extended Kalman filter (EKF) and Unscented Kalman Filter (UKF) be used in the integration of a global positioningsystem (GPS) with an inertial navigation system (INS) GPS and INS individually exhibit large errors but they do complement eachother by maximizing the advantage of each in calculating the heading angle and position through EKF and UKF The proposedmethod was tested using low cost GPS a cheap electronic compass (EC) and an inertial management unit (IMU) which providedaccurate heading and position information verifying the efficacy of the proposed algorithm

1 Introduction

Computation of accurate heading and position data is a keytask in a navigation system Numerous products are availablein the market which can perform above computationsHowever they are expensive Hence there is a lot of researchwork devoted towards developing a precise navigation systemwhich is reasonably priced

Global positioning system (GPS) is used in a variety offields such as scientific and engineering due to its supe-rior capability of providing accurate navigation informationHowever there are issues in the system such as the followingwhen the GPS does not receive the satellite signal leading toissue in generation of valid data The heading informationfrom only one GPS is not highly accurate An inertialnavigation system (INS) provides more accurate headinginformation than the GPS but its position data is less reliablethan that of the GPS It can be seen that the GPS and INSdo complement each other leading to various research workson integrated INSGPS navigation systems for the past twodecades The above given problems are resolved in this workby fusing data from GPS INS and electronic compass (EC)

Several types of complementary filters utilizing low-passand high-pass filters were proposed due to the frequencyfiltering properties of a linear system [1 2] A ship headingcontrol obtained using disturbance compensatingmodel pre-dictive control (DC-MPC) algorithm inwave fieldswas previ-ously suggested [3] Some works also combined Kalman filter(KF) and fuzzy inference in improving the accuracy of a nav-igation system [4ndash6] The sensor selection was done throughfuzzy logic while KF was utilized for sensor fusion KF andits variations have been widely used for sensor fusion such asin determining an accurate azimuth of a pedestrian systemby fusing EC and INS gyroscope information through decen-tralized Kalman filter [7] while another work fused EC INSand multiple GPS using extended Kalman filter-covarianceintersection (EKF-CI) to determine heading information[8] A dual-rate Kalman filter (DRKF) was designed tointegrate GPS pseudoranges and time-differenced GPS car-rier phases (possessing low noise and millimeter measure-ment precision) with INS measurements [9] Estimationin nonlinear systems is extremely important since mostsystems are inherently nonlinear in nature The successfulpractical nonlinear system implementation of a GPSMEMS

Hindawi Publishing CorporationJournal of SensorsVolume 2016 Article ID 7942963 6 pageshttpdxdoiorg10115520167942963

2 Journal of Sensors

(microelectromechanical systems) based-IMU (inertial mea-surement unit) with heading update in an extended Kalmanfilter (EKF) framework has been previously demonstrated[10] Another work utilized adaptive two-stage KF since EKFcannot effectively track time-varying or unknown param-eters [11] A major drawback of EKF is that it does noteffectively estimate if the system is highly nonlinear sinceit uses first-order Taylor series expansion for linearizingthe nonlinear system The mean and covariance estimatesacquired using the Unscented Kalman Filter (UKF) on theother hand are accurate at least up to the second order[12] Several works have confirmed the higher estimationaccuracy of UKF against the EKF [13ndash16] but their keylimitation is that both cannot undertake the problem ofnonlinear systems with non-Gaussian noise An unscentedparticle filter algorithm which is a particle filter using EKF togenerate the proposal distribution was proposed to solve thisissue [17] One study compared complementary filter and KFdepending on the system model simplicity [16 18] anothershowed that EKF exhibited good performance in estimatingattitudeorientation using low cost sensor in environmentswhere GPS signals are denied [19]

Locally weighted regression and smoothing scatterplotwere previously utilized in deriving a more accurate headingangle using GPS data but it was limited for postprocessingonly [20ndash23]Theseworkswere expanded by further integrat-ing EC with INS gyroscopic data for more accurate headingwhile the position informationwas determined by integratingGPS with ISN accelerometer data through EKF and UKFIt was observed that sensor fusion through KF improvedaccuracy similar to results of previous works [22ndash26]

The remainder of this paper is organized as follows EKFand UKF are explained in detail in Section 2 through thenavigation systemmodel utilized in this workThe simulationand experimental tests are presented in Section 3 followed bythe concluding remarks in Section 4

2 Background and Algorithm

21 Extended Kalman Filter (EKF) KF is an efficient recur-sive filter that estimates the state of a linear or nonlineardynamic system from a series of noisy measurements whileEKF is a representative algorithmofKF expanded and appliedto nonlinear system The study of EKF necessitates theunderstanding of a nonlinear system model first Considerthe following general nonlinear system equation

119909119896 = 119891 (119909119896minus1) + 119908119896minus1 (1)

119911119896 = ℎ (119909119896) + V119896 (2)

where (1) and (2) are the state and measurement modelsof the nonlinear system while random variables 119908119896 and V119896are the process and measurement noise respectively Bothnoises are assumed to be zero-mean white noise with normalprobability distribution such that

119901 (119908) sim 119873 (0 119876)

119901 (V) sim 119873 (0 119877) (3)

The process noise 119876 and measurement noise 119877 covariancematrices might change with each time step or measurementbut they are assumed to be constant for this study

TheEKF can nowbe applied to the systemupon obtainingthe system model The schematic flow chart of the EKF inFigure 1(a) shows that the algorithm consists of two mainoperations namely the prediction and update processes [23]

(1) Prediction process (Step (I)) is responsible for pro-jecting forward the current state and error covarianceestimates to obtain a priori estimates for the next step

(2) Correction or update process (Steps (II)ndash(IV)) whichis the main point of the EKF is responsible forthe feedback to incorporate the new measurementinto the a priori estimate to obtain an improved aposteriori estimate

Jacobian is utilized to linearize the nonlinear model in EKFThe119860 and119867matrices in Figure 1(a) can be derived using thefollowing equations

119860 equiv

120597119891

120597119909

10038161003816100381610038161003816100381610038161003816119896

119867 equiv

120597ℎ

120597119909

10038161003816100381610038161003816100381610038161003816minus

119896

(4)

22 Unscented Kalman Filter EKF is not efficient enoughwhen the system is highly nonlinear with the following maindrawbacks (a) linearization may lead to highly unstablesystem and (b) the derivation of Jacobian matrices is notimportant inmost applications andmost often leads to imple-mentation difficulties

UKF utilizes unscented transformation to perform stateestimation of nonlinear systems without linearizing thesystem and measurement models A set of carefully anddeterministically chosen weighted samples that parameterizethe mean and covariance of the belief is used in the trans-formation Suppose 119909 is a random variable with dimension 119899through a nonlinear function 119910 = 119891(119909) and 119909 has covariance119875119909 and mean 119909119898 (119909 sim 119873(119909119898 119875119909)) A set of points calledsigma points are chosen such that their mean and covarianceare 119909119898 and 119875119909 respectively These points are applied in thenonlinear function 119910 = 119891(119909) to get 119910119898 and 119875119910 The 119899-dimen-sional random variable 119909 is approximated by 2119899 + 1 weighted119909 sigma points The sigma points (120594119894) and weights (119882119894) for 119909are defined as follows

1205941 = 119909119898

120594119894+1 = 119909119898 + 119906119894 119894 = 1 2 119899

120594119894+119899+1 = 119909119898 + 119906119894 119894 = 1 2 119899

(5)

1198821 =

119896

119899 + 119896

119882119894+1 =

119896

2 (119899 + 119896)

119894 = 1 2 119899

119882119894+119899+1 =

119896

2 (119899 + 119896)

119894 = 1 2 119899

(6)

Journal of Sensors 3

Measurements Estimate

(0) Set initial values

(I) Predict state and error covariance

(II) Compute Kalman gain

(III) Compute the estimate

(IV) Compute the error covariance

x0 P0

Pminusk = APkminus1A

T + Q

Pk = Pminusk minus KHPminus

k

zk xk

xminusk = f(xminuskminus1 (

Kk = Pminusk H

T(HPminuskH

T + Rminus1(

xk = xminusk + K(zk minus h(xminusk ((

(a) Extended Kalman filter



(II) Predict state and error covariance

(III) Predict measurement and covariance

(IV) Compute Kalman gain

(I) Compute sigma points and weights

(V) Compute the estimate

(VI) Compute the error covariance

x0 P0

zk xk

Kk = PxzPminus1z

xk = xminusk + Kk(zk minus zminusk

Pk = Pminusk minus KkPzK

Tk

(120594iWi larr (xkminus1 Pkminus1 k( (

(xminusk Pminusk = UT(f(120594i Wi Q( ((

(zk Pz = UT(h(120594i Wi R( ( (

(

h minus zkT(120594i ( Pxz =

2n+1

sumi=1

Wif(120594i) minus xminusk

(b) Unscented Kalman Filter

Figure 1 Filtering algorithms

where 119906119894 is a row vector from the matrix 119880 and 119896 is anarbitrary constant

119880119879119880 = (119899 + 119896) 119875119909

(7)

The method instantiates each sigma point through thefunction 119910 = 119891(119909) resulting in a set of transformed sigmapoints mean and covariance as

119910119898 =

2119899+1

sum

119894=1

119882119894119891 (120594119894)

119875119910 =

2119899+1

sum

119894=1

119882119894 119891 (120594119894) minus 119910119898 119891 (120594119894) minus 119910119898119879

(8)

The state and measurement with their covariance canbe predicted upon understanding unscented transformationThe remaining steps of the UKF shown in Figure 1(b) aresimilar to that of KF

The system function is applied to each sample resultingin a group of transformed points The propagated mean andcovariance are the mean and covariance of this group ofpoints The Jacobians of the system and measurement modeldo not have to be calculated since there is no linearizationinvolved in the propagation of the mean and covariancewhich makes UKF practically attractive

23 Navigation System Model The kinematics of a vehicle isgiven by the following state equation [24]

[

[

[

[

1198731015840

1198641015840

1205951015840

]

]

]

]

=[

[

[

c120595 minuss120595 0

s120595 c120595 0

0 0 1

]

]

]

[

[

[

119906

V

119903

]

]

]

+ 119908

=[

[

[

119906c120595 minus Vs120595119906s120595 + Vc120595

119903

]

]

]

+ 119908 = 119891 (119909) + 119908

(9)

where 119906 V and119908 are the linear velocities and 119903 is the angularvelocity along 119911-axis (yaw) while c and s represent cosineand sine functions The measurement model can be definedas follows based on the need to identify the position (119909 119910)and heading angle (120595)

119911119896 =[

[

[

1 0 0

0 1 0

0 0 1

]

]

]

[

[

[

119873

119864

120595

]

]

]

+ V = 119867119909 + V (10)


(a) Experimental board setup (b) Experimental car setup (c) Experiment location

Figure 2 Experimental setup

Jacobian matrices 119860 and 119867 are derived using (4) in order tolinearize the system

119860 =

[

[

[

[

[

[

[

[

[

[

1205971198911

120597119873

1205971198911

120597119864

1205971198911

120597120595

1205971198912

120597119873

1205971198912

120597119864

1205971198912

120597120595

1205971198913

120597119873

1205971198913

120597119864

1205971198913

120597120595

]

]

]

]

]

]

]

]

]

]

=[

[

[

0 0 minus119906s120595 minus Vc1205950 0 119906c120595 minus Vs1205950 0 0

]

]

]

119867 = 1198683times3

(11)

Matrix 119867 does not change since measurement model (6) islinear Process noise covariance 119876 and measurement noisecovariance 119877 are chosen based on experimental data

119876 =[

[

[

001 0 0

0 001 0

0 0 1

]

]

]

119877 =[

[

[

03 0 0

0 007 0

0 0 003

]

]

]

(12)

3 Simulation Setup and Result

The GPS EC and INS (with gyroscope and accelerometer)mounted on an experimental board as shown in Figure 2(a)were utilized for data gathering The CMPS03 EC has aheading accuracy of 3 to 4 degrees and costs $44 The uBloxGPS receiver with model UIGGUB01-V02R01 has a positionaccuracy of 10m and data rate of 10Hz utilizes NMEA 0183protocol and costs $40 The MySEN-M INS has a data rateof 100Hz and heading accuracy of 4 degrees A Furuno SC50satellite compass system was utilized as the reference systemIt has a heading accuracy of 05 degrees position accuracy of2 to 3m and setting time of 3min and costs $4995

The experiment board and reference system are placedon top of the car as shown in Figure 2(b) A car was utilizedinstead of a ship due to the difficulty of performing the testout on the ocean A roadway on a newly reclaimed areain Gunsan city of South Korea was chosen for the locationof the experiment due to the availability of open-sky area

GPSUKF

EKFFuruno

1263081263041263 126312126296Longitude (deg)

35505

3551

35515

3552

35525

Latit

ude (

deg

)

12629561262954

1263027

355186

355185

355036

355034

Experiment trajectory

Figure 3 Comparative position information (GPS EKF UKF andFuruno)

withminimal trafficThe actual experimental location as seenthrough Google Earth is shown in Figure 2(c)

GPS and accelerometer data from INS were set as themeasurement state and the state to estimate the positionrespectively The resulting position information when usingboth EKF and UKF is shown in Figure 3 with several partszoomed in order to clearly distinguish the performance ofeach The raw gyroscope and EC data shown in Figure 4 arefused to obtain accurate heading information for navigationHeading information based on the GPS COG EKF UKFand reference systems is shown in Figure 5 with EKF givingvalues closest to that of the reference system COG is ldquoCourseOver Groundrdquo heading information from the GPS devicewhile HDT is ldquoTrue Headingrdquo of the GPS device obtained byprocessing RF cycle in the GPS carrier frequency

RMSEposition = radic1

119899

119899

sum

119894=1

(Δ119864119894

2+ Δ119873119894

2)

RMSEheading = radic1

119899

119899

sum

119894=1

(119894minus 120595119894)2

(13)

The rootmean square errors (RMSE) calculated using (13) areshown in Table 1 to give a better perspective in the differences


minus10

minus8

minus6

minus4

minus2

0

2

4

Ang

ular

vel

ocity

alon

g z

-axi

s (de

gs

)

05 1 15 2 250Sample times104

(a) Raw gyroscope data

10

15

20

25

30

35

40

45

50

Hea

ding

(deg

)

times10405 1 15 2 250

Sample

(b) Raw compass data

Figure 4 Heading information

GPS COGUKF

EKFFuruno HDT

0

5

10

15

20

25

30

35

40

45

50

Hea

ding

(deg

)

times10405 1 15 2 250

Sample

Figure 5 Comparative heading information (GPS COG EKF UKFand Furuno HDT)

of the position and heading resultTheEKF took 1324128 secswhile UKF took 1370245 secs to fully simulate the process

4 Conclusion

Commercial navigation systems produce accurate informa-tion with reduced errors but are usually expensiveThis workproposes a system that can estimate accurate heading andposition information comparable to more expensive onesat a fraction of the cost Low cost sensors such as globalpositioning system (GPS) and electronic compass (EC) arefused with inertial navigation system (INS)inertial mea-surement unit (IMU) through extended Kalman filter (EKF)

Table 1 Heading and position errors

EKF UKF GPS COGHeading (RMSEheading) 0326 deg 02196 deg 81218 degPosition (RMSEposition) 112m 112m 133m

and Unscented Kalman Filter (UKF) to achieve the desiredresults It was shown that individual heading informationfrom several cheaper devices with big errors can produce amuch more accurate fused heading value

Position estimation results for both EKF and UKF showthe same slight increase in accuracy but heading estimationresults showed that UKF output was closer to the referencethan that of EKF It was seen that EKF is better than UKFwhen taking into consideration the time it took to performthe process It can be eventually concluded that accurateheading and position information can be obtained from thefusion of several low cost sensors verifying the effectivenessof the proposed algorithm

Conflict of Interests

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

Acknowledgments

This work was supported by ldquoResearch Base ConstructionFund Support Programrdquo funded by Chonbuk National Uni-versity in 2015 the Brain Korea 21 PLUS Project andNationalResearch Foundation (NRF) of Korea grant funded by theKorean government (MEST) (no 2013R1A2A2A01068127and no 2013R1A1A2A10009458)


References

[1] V Kubelka and M Reinstein ldquoComplementary filtering app-roach to orientation estimation using inertial sensors onlyrdquo inProceedings of the IEEE International Conference onRobotics andAutomation (ICRA rsquo12) pp 599ndash605 IEEE Saint Paul MinnUSA May 2012

[2] T S Yoo S K Hong H M Yoon and S Park ldquoGain-scheduledcomplementary filter design for a MEMS based attitude andheading reference systemrdquo Sensors vol 11 no 4 pp 3816ndash38302011

[3] Z Li and J Sun ldquoDisturbance compensating model predictivecontrol with application to ship heading controlrdquo IEEE Transac-tions on Control Systems Technology vol 20 no 1 pp 257ndash2652012

[4] F Kobayashi D Masumoto and F Kojima ldquoSensor selectionbased on fuzzy inference for sensor fusionrdquo in Proceedings ofthe IEEE International Conference on Fuzzy Systems vol 1 pp305ndash310 IEEE July 2004

[5] X Li X Song and C Chan ldquoReliable vehicle sideslip anglefusion estimation using low-cost sensorsrdquoMeasurement vol 51no 1 pp 241ndash258 2014

[6] F Caron E Duflos D Pomorski and P Vanheeghe ldquoGPSIMUdata fusion using multisensor Kalman filtering introduction ofcontextual aspectsrdquo Information Fusion vol 7 no 2 pp 221ndash230 2006

[7] Q Ladetto J van Seeters S Sokolowski Z Sagan and BMerminod ldquoDigital magnetic compass and gyroscope for dis-mounted soldier position amp navigationrdquo in Proceedings of theMilitary Capabilities Enabled by Advances in Navigation SensorsSensors amp Electronics Technology Panel NATO-RTO MeetingsIstanbul Turkey 2002

[8] F P Vista D J Lee and K T Chong ldquoDesign of an EKF-CIbased sensor fusion for robust heading estimation of marinevehiclerdquo International Journal of Precision Engineering andManufacturing vol 16 no 2 pp 403ndash407 2015

[9] S Han and J Wang ldquoIntegrated GPSINS navigation systemwith dual-rate Kalman Filterrdquo GPS Solutions vol 16 no 3 pp389ndash404 2012

[10] A Vydhyanathan H Luinge M U de Haag and M BraaschldquoIntegrating GPSMEMS-based-IMU with single GPS baselinefor improved heading performancerdquo in Proceedings of the IEEEAerospace Conference pp 1ndash10 IEEE Big Sky Mont USAMarch 2012

[11] K H Kim J G Lee and C G Park ldquoAdaptive two-stageextended Kalman filter for a fault-tolerant INS-GPS looselycoupled systemrdquo IEEE Transactions on Aerospace and ElectronicSystems vol 45 no 1 pp 125ndash137 2009

[12] S J Julier J K Uhlmann and H F Durrant-Whyte ldquoA newapproach for filtering nonlinear systemsrdquo in Proceedings ofthe American Control Conference vol 3 pp 1628ndash1632 IEEESeattle Wash USA June 1995

[13] R Van Der Merwe and E A Wan ldquoSigma-point Kalman filtersfor integrated navigationrdquo in Proceedings of the 60th AnnualMeeting of the Institute of Navigation (ION rsquo04) pp 641ndash654June 2004

[14] P Zhang J Gu E E Milios and P Huynh ldquoNavigation withIMUGPSdigital compass with unscented Kalman filterrdquo inProceedings of the IEEE International Conference on Mechatron-ics and Automation (ICMA rsquo05) pp 1497ndash1502 IEEE NiagaraFalls Canada August 2005

[15] J L Crassidis K-L Lai and R R Harman ldquoReal-time attitude-independent three-axis magnetometer calibrationrdquo Journal ofGuidance Control and Dynamics vol 28 no 1 pp 115ndash1202005

[16] R Kandepu B Foss and L Imsland ldquoApplying the unscentedKalman filter for nonlinear state estimationrdquo Journal of ProcessControl vol 18 no 7-8 pp 753ndash768 2008

[17] R Van Der Merwe A Doucet N De Freitas and E WanldquoThe unscented particle filterrdquo inNIPS pp 584ndash590MIT Press2000

[18] W T Higgins Jr ldquoA comparison of complementary andKalmanfilteringrdquo IEEE Transactions on Aerospace and Electronic Sys-tems vol 11 no 3 pp 321ndash325 1975

[19] M H Afzal V Renaudin and G Lachapelle ldquoUse of earthrsquosmagnetic field for mitigating gyroscope errors regardless ofmagnetic perturbationrdquo Sensors vol 11 no 12 pp 11390ndash114142011

[20] G Gankhuyag and K T Chong ldquoAccurate heading estimationof a vehicle using GPS onlyrdquo in Proceedings of the Institute ofControl Robotics and Systems Conference (ICROS rsquo13) DaejeonRepublic of Korea 2013

[21] G Gankhuyag D J Lee and K T Chong ldquoA simulation oflow cost GPSINS integration using extended Kalman filterrdquoin Proceedings of the 2014 Information and Control SymposiumDaejeon South Korea 2014

[22] T I Fossen Handbook of Marine Craft Hydrodynamics andMotion Control John Wiley amp Sons 2011

[23] P Kim and L Huh Kalman Filter for Beginners with MATLABExamples CreateSpace 2011

[24] T Perez and T I Fossen ldquoKinematic models for manoeuvringand seakeeping of marine vesselsrdquoModeling Identification andControl vol 28 no 1 pp 19ndash30 2007

[25] Y Liu ldquoNavigation and control of mobile robot using sensorfusionrdquo in Robot Vision InTech 2010

[26] G Gankhuyag W C Young and K T Chong ldquoLoosely-coupled Kalman filter integration of GPS and INS for marinevehiclerdquo in Proceedings of the Information and Control Sympo-sium pp 187ndash184 Incheon South Korea 2013

International Journal of

AerospaceEngineeringHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

RoboticsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014


Active and Passive Electronic Components

Control Scienceand Engineering

Journal of



RotatingMachinery


Hindawi Publishing Corporation httpwwwhindawicom

Journal ofEngineeringVolume 2014

Submit your manuscripts athttpwwwhindawicom

VLSI Design



Shock and Vibration


Civil EngineeringAdvances in

Acoustics and VibrationAdvances in



Electrical and Computer Engineering

Journal of

Advances inOptoElectronics


Volume 2014

The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014

SensorsJournal of


Modelling amp Simulation in EngineeringHindawi Publishing Corporation httpwwwhindawicom Volume 2014


Chemical EngineeringInternational Journal of Antennas and

Propagation




Navigation and Observation



DistributedSensor Networks



(microelectromechanical systems) based-IMU (inertial mea-surement unit) with heading update in an extended Kalmanfilter (EKF) framework has been previously demonstrated[10] Another work utilized adaptive two-stage KF since EKFcannot effectively track time-varying or unknown param-eters [11] A major drawback of EKF is that it does noteffectively estimate if the system is highly nonlinear sinceit uses first-order Taylor series expansion for linearizingthe nonlinear system The mean and covariance estimatesacquired using the Unscented Kalman Filter (UKF) on theother hand are accurate at least up to the second order[12] Several works have confirmed the higher estimationaccuracy of UKF against the EKF [13ndash16] but their keylimitation is that both cannot undertake the problem ofnonlinear systems with non-Gaussian noise An unscentedparticle filter algorithm which is a particle filter using EKF togenerate the proposal distribution was proposed to solve thisissue [17] One study compared complementary filter and KFdepending on the system model simplicity [16 18] anothershowed that EKF exhibited good performance in estimatingattitudeorientation using low cost sensor in environmentswhere GPS signals are denied [19]

Locally weighted regression and smoothing scatterplotwere previously utilized in deriving a more accurate headingangle using GPS data but it was limited for postprocessingonly [20ndash23]Theseworkswere expanded by further integrat-ing EC with INS gyroscopic data for more accurate headingwhile the position informationwas determined by integratingGPS with ISN accelerometer data through EKF and UKFIt was observed that sensor fusion through KF improvedaccuracy similar to results of previous works [22ndash26]

The remainder of this paper is organized as follows EKFand UKF are explained in detail in Section 2 through thenavigation systemmodel utilized in this workThe simulationand experimental tests are presented in Section 3 followed bythe concluding remarks in Section 4

2 Background and Algorithm

21 Extended Kalman Filter (EKF) KF is an efficient recur-sive filter that estimates the state of a linear or nonlineardynamic system from a series of noisy measurements whileEKF is a representative algorithmofKF expanded and appliedto nonlinear system The study of EKF necessitates theunderstanding of a nonlinear system model first Considerthe following general nonlinear system equation

119909119896 = 119891 (119909119896minus1) + 119908119896minus1 (1)

119911119896 = ℎ (119909119896) + V119896 (2)

where (1) and (2) are the state and measurement modelsof the nonlinear system while random variables 119908119896 and V119896are the process and measurement noise respectively Bothnoises are assumed to be zero-mean white noise with normalprobability distribution such that

119901 (119908) sim 119873 (0 119876)

119901 (V) sim 119873 (0 119877) (3)

The process noise 119876 and measurement noise 119877 covariancematrices might change with each time step or measurementbut they are assumed to be constant for this study

TheEKF can nowbe applied to the systemupon obtainingthe system model The schematic flow chart of the EKF inFigure 1(a) shows that the algorithm consists of two mainoperations namely the prediction and update processes [23]

(1) Prediction process (Step (I)) is responsible for pro-jecting forward the current state and error covarianceestimates to obtain a priori estimates for the next step

(2) Correction or update process (Steps (II)ndash(IV)) whichis the main point of the EKF is responsible forthe feedback to incorporate the new measurementinto the a priori estimate to obtain an improved aposteriori estimate

Jacobian is utilized to linearize the nonlinear model in EKFThe119860 and119867matrices in Figure 1(a) can be derived using thefollowing equations

119860 equiv

120597119891

120597119909

10038161003816100381610038161003816100381610038161003816119896

119867 equiv

120597ℎ

120597119909

10038161003816100381610038161003816100381610038161003816minus

119896

(4)

22 Unscented Kalman Filter EKF is not efficient enoughwhen the system is highly nonlinear with the following maindrawbacks (a) linearization may lead to highly unstablesystem and (b) the derivation of Jacobian matrices is notimportant inmost applications andmost often leads to imple-mentation difficulties

UKF utilizes unscented transformation to perform stateestimation of nonlinear systems without linearizing thesystem and measurement models A set of carefully anddeterministically chosen weighted samples that parameterizethe mean and covariance of the belief is used in the trans-formation Suppose 119909 is a random variable with dimension 119899through a nonlinear function 119910 = 119891(119909) and 119909 has covariance119875119909 and mean 119909119898 (119909 sim 119873(119909119898 119875119909)) A set of points calledsigma points are chosen such that their mean and covarianceare 119909119898 and 119875119909 respectively These points are applied in thenonlinear function 119910 = 119891(119909) to get 119910119898 and 119875119910 The 119899-dimen-sional random variable 119909 is approximated by 2119899 + 1 weighted119909 sigma points The sigma points (120594119894) and weights (119882119894) for 119909are defined as follows

1205941 = 119909119898

120594119894+1 = 119909119898 + 119906119894 119894 = 1 2 119899

120594119894+119899+1 = 119909119898 + 119906119894 119894 = 1 2 119899

(5)

1198821 =

119896

119899 + 119896

119882119894+1 =

119896

2 (119899 + 119896)

119894 = 1 2 119899

119882119894+119899+1 =

119896

2 (119899 + 119896)

119894 = 1 2 119899

(6)








x0 P0


T + Q


k

zk xk


Kk = Pminusk H

T(HPminuskH

T + Rminus1(











x0 P0

zk xk

Kk = PxzPminus1z



Tk



(zk Pz = UT(h(120594i Wi R( ( (

(


2n+1

sumi=1





119880119879119880 = (119899 + 119896) 119875119909

(7)


119910119898 =

2119899+1

sum

119894=1

119882119894119891 (120594119894)

119875119910 =

2119899+1

sum

119894=1

119882119894 119891 (120594119894) minus 119910119898 119891 (120594119894) minus 119910119898119879

(8)




[

[

[

[

1198731015840

1198641015840

1205951015840

]

]

]

]

=[

[

[

c120595 minuss120595 0

s120595 c120595 0

0 0 1

]

]

]

[

[

[

119906

V

119903

]

]

]

+ 119908

=[

[

[

119906c120595 minus Vs120595119906s120595 + Vc120595

119903

]

]

]

+ 119908 = 119891 (119909) + 119908

(9)


119911119896 =[

[

[

1 0 0

0 1 0

0 0 1

]

]

]

[

[

[

119873

119864

120595

]

]

]

+ V = 119867119909 + V (10)





119860 =

[

[

[

[

[

[

[

[

[

[

1205971198911

120597119873

1205971198911

120597119864

1205971198911

120597120595

1205971198912

120597119873

1205971198912

120597119864

1205971198912

120597120595

1205971198913

120597119873

1205971198913

120597119864

1205971198913

120597120595

]

]

]

]

]

]

]

]

]

]

=[

[

[


]

]

]

119867 = 1198683times3

(11)


119876 =[

[

[

001 0 0

0 001 0

0 0 1

]

]

]

119877 =[

[

[

03 0 0

0 007 0

0 0 003

]

]

]

(12)




GPSUKF

EKFFuruno

1263081263041263 126312126296Longitude (deg)

35505

3551

35515

3552

35525

Latit

ude (

deg

)

12629561262954

1263027

355186

355185

355036

355034






119899

119899

sum

119894=1

(Δ119864119894

2+ Δ119873119894

2)


119899

119899

sum

119894=1

(119894minus 120595119894)2

(13)



minus10

minus8

minus6

minus4

minus2

0

2

4

Ang

ular

vel

ocity

alon

g z

-axi

s (de

gs

)

05 1 15 2 250Sample times104


10

15

20

25

30

35

40

45

50

Hea

ding

(deg

)

times10405 1 15 2 250

Sample



GPS COGUKF

EKFFuruno HDT

0

5

10

15

20

25

30

35

40

45

50

Hea

ding

(deg

)

times10405 1 15 2 250

Sample



4 Conclusion








Acknowledgments



References





























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation
















x0 P0


T + Q


k

zk xk


Kk = Pminusk H

T(HPminuskH

T + Rminus1(











x0 P0

zk xk

Kk = PxzPminus1z



Tk



(zk Pz = UT(h(120594i Wi R( ( (

(


2n+1

sumi=1





119880119879119880 = (119899 + 119896) 119875119909

(7)


119910119898 =

2119899+1

sum

119894=1

119882119894119891 (120594119894)

119875119910 =

2119899+1

sum

119894=1

119882119894 119891 (120594119894) minus 119910119898 119891 (120594119894) minus 119910119898119879

(8)




[

[

[

[

1198731015840

1198641015840

1205951015840

]

]

]

]

=[

[

[

c120595 minuss120595 0

s120595 c120595 0

0 0 1

]

]

]

[

[

[

119906

V

119903

]

]

]

+ 119908

=[

[

[

119906c120595 minus Vs120595119906s120595 + Vc120595

119903

]

]

]

+ 119908 = 119891 (119909) + 119908

(9)


119911119896 =[

[

[

1 0 0

0 1 0

0 0 1

]

]

]

[

[

[

119873

119864

120595

]

]

]

+ V = 119867119909 + V (10)





119860 =

[

[

[

[

[

[

[

[

[

[

1205971198911

120597119873

1205971198911

120597119864

1205971198911

120597120595

1205971198912

120597119873

1205971198912

120597119864

1205971198912

120597120595

1205971198913

120597119873

1205971198913

120597119864

1205971198913

120597120595

]

]

]

]

]

]

]

]

]

]

=[

[

[


]

]

]

119867 = 1198683times3

(11)


119876 =[

[

[

001 0 0

0 001 0

0 0 1

]

]

]

119877 =[

[

[

03 0 0

0 007 0

0 0 003

]

]

]

(12)




GPSUKF

EKFFuruno

1263081263041263 126312126296Longitude (deg)

35505

3551

35515

3552

35525

Latit

ude (

deg

)

12629561262954

1263027

355186

355185

355036

355034






119899

119899

sum

119894=1

(Δ119864119894

2+ Δ119873119894

2)


119899

119899

sum

119894=1

(119894minus 120595119894)2

(13)



minus10

minus8

minus6

minus4

minus2

0

2

4

Ang

ular

vel

ocity

alon

g z

-axi

s (de

gs

)

05 1 15 2 250Sample times104


10

15

20

25

30

35

40

45

50

Hea

ding

(deg

)

times10405 1 15 2 250

Sample



GPS COGUKF

EKFFuruno HDT

0

5

10

15

20

25

30

35

40

45

50

Hea

ding

(deg

)

times10405 1 15 2 250

Sample



4 Conclusion








Acknowledgments



References





























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation













119860 =

[

[

[

[

[

[

[

[

[

[

1205971198911

120597119873

1205971198911

120597119864

1205971198911

120597120595

1205971198912

120597119873

1205971198912

120597119864

1205971198912

120597120595

1205971198913

120597119873

1205971198913

120597119864

1205971198913

120597120595

]

]

]

]

]

]

]

]

]

]

=[

[

[


]

]

]

119867 = 1198683times3

(11)


119876 =[

[

[

001 0 0

0 001 0

0 0 1

]

]

]

119877 =[

[

[

03 0 0

0 007 0

0 0 003

]

]

]

(12)




GPSUKF

EKFFuruno

1263081263041263 126312126296Longitude (deg)

35505

3551

35515

3552

35525

Latit

ude (

deg

)

12629561262954

1263027

355186

355185

355036

355034






119899

119899

sum

119894=1

(Δ119864119894

2+ Δ119873119894

2)


119899

119899

sum

119894=1

(119894minus 120595119894)2

(13)



minus10

minus8

minus6

minus4

minus2

0

2

4

Ang

ular

vel

ocity

alon

g z

-axi

s (de

gs

)

05 1 15 2 250Sample times104


10

15

20

25

30

35

40

45

50

Hea

ding

(deg

)

times10405 1 15 2 250

Sample



GPS COGUKF

EKFFuruno HDT

0

5

10

15

20

25

30

35

40

45

50

Hea

ding

(deg

)

times10405 1 15 2 250

Sample



4 Conclusion








Acknowledgments



References





























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation










minus10

minus8

minus6

minus4

minus2

0

2

4

Ang

ular

vel

ocity

alon

g z

-axi

s (de

gs

)

05 1 15 2 250Sample times104


10

15

20

25

30

35

40

45

50

Hea

ding

(deg

)

times10405 1 15 2 250

Sample



GPS COGUKF

EKFFuruno HDT

0

5

10

15

20

25

30

35

40

45

50

Hea

ding

(deg

)

times10405 1 15 2 250

Sample



4 Conclusion








Acknowledgments



References





























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation










References





























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation











RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation









Research Article Navigation System Heading and...

Documents

Transcript of Research Article Navigation System Heading and...