2012 Second International Conference on Business Computing and Global Informatization
Tightly Coupled GPS/INS Integrated Navigation Algorithm Based on Kalman
Filter
Jinliang Zhang1, Tao Zhang1, Xuehuan Jiang1, Sishan Wang1

School of Electrical and Information Engineering
Hubei University of Automotive Technology
Shiyan, China
qcxy-zjl@163.com; zttyler@163.com; changyuan009@163.com; gnuarmfuns@yahoo.com.cn
Abstract—For the respective performance characteristics
of the global positioning system (GPS) and inertial
navigation system (INS), a tightly coupled GPS/INS
integrated navigation system based on the pseudo-distance
difference was proposed. The paper presents the detail study
and theoretical analysis of the kalman filter algorithm for
the GPS/INS integrated navigation system, and focuses on
the establishment of the system model state equation and
observation equation, which is the basis of the system
simulation. The simulation result shows that the kalman
filter algorithm and tightly coupling mode for the integrated
navigation system effectively improve the long-term
positioning accuracy, and enhance the anti-interference
performance of the GPS navigation equipment.
Keywords-tightly coupled GPS/INS; integrated navigation
system; pseudo-distance difference; kalman filter
I. INTRODUCTION
Inertial navigation system (INS) is a self-contained
navigation system with good concealment, high
independent and strong anti-interference ability, which can
give many complete information of the moving carrier,
including speed, position, height, orientation and attitude.
However the time integral principle of inertial navigation
determine that the systematic errors will be accumulated
with the time and eventually lead the navigation accuracy
to divergence over the time, which is the most prominent
drawback of the inertial navigation system [1][2].
The GPS is a most widely used positioning system
with the world-wide, continuous, real-time, high-precision,
all-weather and some other features. However, in
high-speed moving scenarios, there will be a lock-loss or
other unexpected situations, which lead to the poor
real-time controllability and reliability. By the GPS-related
policies, the positioning accuracy for the application in
different field is limited within a certain range. In addition,
the GPS will lose its navigating and positioning
capabilities in some GPS blind area (such as tunnels,
basements, et al.) where the GPS signal can not reach [3].
According to the above problems of GPS, the
integrated navigation system attracts attentions. Integrated
navigation combines more than two different navigation
systems in the specific way to measure the same source,
then corrects subsystem errors to improve the navigation
performance of the whole combined navigation system [4].
The most common mode of the integrated navigation
system is the combination of inertial navigation and GPS
navigation. GPS/INS integrated navigation system makes
full use of the subsystems’ complementary advantages. It
978-0-7695-4854-8/12 $26.00 © 2012 IEEE
DOI 10.1109/BCGIN.2012.159
not only overcomes the pure inertial navigation system
error divergence by time accumulating, but also improves
a single GPS navigation system’s anti-jamming
performance and reliability in dynamic environment [5].
Therefore, the combination of GPS/INS is an ideal and
practical integrated navigation system which has a very
broad prospect for the applications in many fields [6].
For GPS/INS integrated navigation system, good
information fusion algorithm is very important for the
performance. Kalman filter is an optimal estimation theory
and method, which has many notable features, including
simple structure, clear design method, real-time recurrence
and small storage requirement. It has aroused the relevant
researchers’ attention in practical applications of many
different engineering areas [7].
In this paper, kalman filter is used in the GPS/INS
integrated navigation system. The brief introduction of
Kalman filtering algorithm is presented in Section II. In
Section III, the GPS/INS Integrated Navigation System
Modeling based on the Kalman filter is proposed, followed
by the simulation results and performance analysis in
Section IV. Finally, the paper is concluded in Section V.
II. KALMAN FILTERING ALGORITHM
A. Stochastic Discrete Linear System Kalman Filter
Equations
Assumption I: don’t consider the effect of system
control input. The equation of stochastic discrete linear
systems is defined as
X k = Φ k , k −1 X k −1 + Γ k , k −1Wk −1 , Z k = H k X k + Vk (1)
where Xk is the n dimensional states sequence; Φ k , k −1 is
the n × n dimension state transfer matrix; Wk-1 is
the p dimensional process noise sequence; Γ k , k −1 is the n ×
p dimensional noise input matrix; Zk is the m dimensional
observation sequence; Hk is the m × n dimensional
observation matrix; Vk is the m dimensional observation
noise sequence.
For the noise statistical properties of the system, we
set forth below
­ E [Wk ] = 0, E ªWk W jT º = Qk δ k , j
° ¬ ¼
°
® E [Vk ] = 0, E ¬ k j ¼ = Rk δ k , j ,
ª V V T
º (2)
°
°¯ E ª¬Wk V jT º¼ = 0
588
where δ k , j is the Kronecker-δ function. P0 , k = 1
∧
X 0,k =1
Assumption II: Xk and Zk satisfy the constraints of (1), Qk −1
Wk and Vk satisfy the conditions of (2). Qk is the Pk , k −1 ∧
non-negative definite process noise covariance matrix of X k , k −1
the system, Rk is the systematic positive definite
Rk k +1→k
observation noise covariance matrix of the system, Zk is Kk
the observed value of the system at time k, and the
∧ ∧
optimal state estimation for the Xk-1 at time k-1 is X k −1 . Xk
∧
Pk Zk
∧
Then X k is the optimal state estimation Xk , which can Pk Xk
be solved in accordance with the following filter equation.
The one step state prediction
Figure 2. Kalman filter algorithm.
∧ ∧
X k , k −1 = Φ k , k −1 X k −1 . (3) III. MODELING THE GPS/INS INTEGRATED
NAVIGATION SYSTEM BASED ON KALMAN FILTER
The state estimation A. State Equations of GPS/INS Integrated Navigation
System
∧ ∧ ∧
ª º
X k = X k , k −1 + K k « Z k − H k X k , k −1 » . (4) In the design process of the Kalman filter for
¬ ¼ GPS/INS integrated navigation system, the system state
equations and observation equations are established by the
The filter gain matrix combined system errors, including inertial navigation
system errors and GPS errors. Kalman filter uses the
−1 minimum variance estimation criterion to calculate the
K k = Pk , k −1 H kT ª¬ H k Pk , k −1 H kT + Rk º¼ . (5) estimated value of the system errors, then corrects the
parameters of the integrated navigation system to further
The one step prediction error variance matrix reduce the system errors, finally improve the navigation
accuracy and the overall performance of the combined
Pk , k −1 = Φ k , k −1 Pk −1ΦTk , k −1 + Γ k , k −1Qk −1ΓTk , k −1 . (6) system [8].
According to the inertial navigation error sources and
characteristics analysis, we can get the errors to establish
The estimation error covariance matrix the system state equation as follows

Pk = [ I − K k H k ] Pk , k −1 [ I − K k H k ] + K k Rk K kT . (7)
T •
X I ( t )18×1 = FI ( t )18×18 X I (t )18×1 + GI ( t )18×9 WI (t )9×1 ,(8)

Equations (4) to (7) constitute the basic equations of
the discrete kalman filtering. Equations (4) and (5) can be
regarded as the kalman filter equations. The block
diagram of kalman filter is shown in Fig.1. It chooses the
system’s state observations as the input, and the estimated
value as the output.
B. Process of Kalman Filtering Algorithm
According to the basic equations of kalman filtering
process, the filtering algorithm is shown in Fig.2.
The whole algorithm in Fig.2 is laterally divided into
two loops: gain calculation loop and filtering calculation
loop. The gain calculation loop can be independently
calculated, but the filter calculation loop depends on the
gain calculation loop. According to the system state and
usage priorities of the observation information, the
Kalman filter algorithm can be vertically divided into two
level updates: time update and observation update.
+ Kk + T
Z k − Z k +
∧ WI = ª¬ωgx ωgy ωgz ωbx ωby ωbz ωax ωay ωaz º¼ .(10)
Xk
Time delay
∧
Hk X k , k −1 Φk , k −1
Figure 1. Discrete system block diagram with kalman filter.
where the state variables are
X I = [φE φN φU δ vE δ vN δ vU δ L δλ δ h
T .(9)
ε gx ε gy ε gz εrx εry εrz ∇x ∇y ∇z º¼
Among them, the first nine state variables are the
three axis platform error components, velocity error
components and position error components of the inertial
navigation system, the next six state variables are the
gyroscope related errors, and the last three state variables
are the accelerometer related errors.
Noise matrix of the inertial navigation system
consists of the three-axis gyroscope noise sequence on
inertial navigation platform, the first order Markov noise
sequence and three-axis accelerometer noise sequence. It
is shown as follows
∧
X k −1 ªF FS º
In (8), FI = « N » , where FN is a 9 basic
¬ 0 F M ¼18×18
parameters matrix of inertial navigation system.

589
 The following is the GPS error state equation. GPS ­δ x = δ hcos Lcos λ − R + h sin Lcos λδ L − R + h cos Lsin λδλ
has two main error sources: the distance corresponding to °° ( N ) ( N )
equivalent clock error δ tu and the distance rate ®δ y = δ hcos Lsinλ −( RN + h) sin Lsin λδ L −( RN + h) cos Lcos λδλ .
°
corresponding to the equivalent clock frequency error δ tru , δ z = δ hsin L + ¬ªRN (1−e2 ) + h¼º cos Lδ L
its differential equations are established as follows ¯°
Then pseudo-distance observation equations can be
obtained
•
½
δ tu = δ tu + ωtu °
• ¾. (11) Z ρ ( t )4×1 = H ρ ( t ) 4×20 X (t )20×1 + Vρ (t ) 4×1 , (17)
δ tru = − βtruδ tru + ωtru °¿
where H ρ = ª¬ 04×6 # H ρ1 # 04×9 # H ρ 2 º¼ .
Then, GPS error state equation can be obtained 4× 20

• IV. SIMULATION RESULTS AND ANALYSIS

X G ( t )2×1 = FG ( t ) 2× 2 X G (t ) 2×1 + GG ( t )2× 2 WG (t ) 2×1 ,(12)
Based on the analysis of the GPS/INS integrated
system state equations and observation equations, the
ª1 0 º ª1 0 º Kalman filter algorithm is simulated in Matlab
where FG = « » , GG = «0 1 » . environment [9][10]. The related parameters are shown in
¬ 0 − β tru ¼ ¬ ¼
TABLE I.
The integrated navigation system state equations are
obtained by the combination of (8) and (12) Initial values of the system kalman filter covariance
are
ª( 60′′)2 , ( 60′′)2 ,0, ( 0.1m / s )2 , ( 0.1m / s )2 , ( 0.1m / s ) 2 ,º
ª• º « »
« X I ( t ) » = ªFI ( t ) 0 º ª XI (t)º + ªGI ( t ) 0 º ªWI (t)º
,(13) P0 = Diag «( 0.2D ) , ( 0.2D ) , ( 0.3D ) , ( 0.1D / h) , ( 0.1D / h) ,
2 2 2 2 2
» .
«• » «¬ 0 FG ( t ) »¼ «¬XG (t)»¼ «¬ 0 GG ( t )»¼ «¬WG (t)»¼ « »
«¬XG ( t ) »¼ « D »
(¬« 0.1 / h) ,0,0,0, (10−4 g ) , (10−4 g ) , (10−4 g )
2 2 2 2

¼»
• The initial values of the system noise covariance
that is X ( t )20×1 = F ( t )20× 20 X (t ) 20×1 + G ( t )20×11 W (t )11×1 . matrix are
ª( 0.01D / h )2 , ( 0.01D / h )2 , ( 0.01D / h )2 , º
B. Observation Equations of GPS/INS Integrated « »
Navigation System « ».
Q0 = Diag «( 0.01 / h ) , ( 0.01 / h ) , ( 0.01 / h ) ,
D 2 D 2 D 2

The pseudo-distance corresponding to the output »

« »
«( 50μ g ) , ( 50μ g ) , ( 50μ g ) ,0,0, 0,0, 0,0»
2 2 2
location of the inertial navigation system is
¬ ¼
1 The initial values of the system state are
ρ Ii = ª( xi − xsi ) + ( yi − ysi ) + ( zi − zsi ) º 2 . (14)
2 2 2
ª30′′, 60′′, 0, 0.1m / s, 0.1m / s, 0, 0.2D , 0.2D , 0.3D , º
¬ ¼ X0 = « D ».
D D −4 −4 −4
«¬0.1 / h, 0.1 / h,0.1 / h,10 g ,10 g ,10 g , 0, 0, 0 »¼
The pseudo-distance measured by GPS is
ρGi = ri + δ tu + v pi . The pseudo-distance observed value TABLE I. PARAMETERS OF THE GPS/INS INTEGRATED
NAVIGATION SIMULATION MODEL
of the combined system is
Radius of the earth R = 6378137m
δρi = ρ Ii − ρGi = ei1δ x + ei 2δ y + ei 3δ z − δ tu − vρt ,(15) Angular rate of earth rotation ωie = 7.292115 × 10−5 rad / s
Initial longitude and latitude λ = 120D ; L = 32D
taking i = 1, 2,3, 4 , then Initial longitude and latitude δλ0 = 30′′; δ L0 = 60′′
error
Initial velocity vE 0 = 15m / s; vN 0 = vU 0 = 0
ª e11 e12 e13 1º ª δ x º ª vρ1 º Initial velocity error δ vE 0 = δ vN 0 = δ vU 0 = 0.1m / s
«e « »
e22 e23 1»» «« δ y »» «vρ2 » GPS position error 10m
δρ = « 21 + . (16)
« e31 e32 e33 1» « δ z » « vρ3 » Filtering cycle T = 1s
« »« » « » Acceleration of gravity 9.7836m / s 2
¬e41 e42 e43 1¼ ¬δ tu ¼ «vρ »
¬ 4¼ Gyro constant drift 0.1D / h
Gyro random drift mean square
deviation 0.01D / h
All the derivations of the equations mentioned above
choose the earth fixed coordinate system 0 xe ye ze ( ECEF )
Accelerometer constant 100ug
zero-bias
as the navigation coordinate system. While in practical Accelerometer random bias 50ug
applications the longitude, latitude and altitude are used to square deviation
First-order Markov related time 300s
locate, so δ x, δ y, δ z need to be represented
GPS velocity error 0.2m / s
by δ L, δλ , δ h . Simulation time 2h

590
 The position and velocity estimation errors of the
GPS/INS integrated system filter outputs are shown in
Fig.3, Fig.4 ,Fig.5, Fig.6, Fig.7 and Fig.8 respectively.
 combined system navigation position error is almost
Figure 3. The latitude estimation error.
 system and establishes the system mode. Simulation
Figure 4. The longitude estimation error.
 Shiyan City (Grant No. 20110271).
Figure 5. The height estimation error.
 an analysis of applications, current development and future
Figure 6. The east velocity estimation error.
 Inertial Technology, vol. 12, Feb. 2004, pp. 23-26.
Figure 7. The north velocity estimation error.
 [10] M. S. Grewal, and A. P. Andrews, “Kalman Filtering Theory and
Figure 8. The sky velocity estimation error.
591
Simulation results in Fig. 3~Fig. 5 show that all the
longitude and longitude errors of the GPS/INS integrated
navigation system are less than 0.5 × 10−4 degree. When
converting the latitude and longitude degree errors into
the distance, the latitude error is less than
0.5 × 10−4 × 111700 = 5.585 m, the longitude error is less
than 0.5 × 10−4 × 111700 × cos 32D = 4.736 m, and the
height error is less than 7 m, all of which are much
smaller than a single GPS position error 10 m. The
reduced by half. The velocity errors in east, north and sky
are less than 0.07 m/s, which are also smaller than a single
GPS velocity errors 0.2 m/s.
V. CONCLUSION
The paper designs a tightly coupled GPS/INS
integrated navigation system based on the pseudo-distance
differential, then gives a detail analysis and derivation of
the kalman filter algorithm for the integrated navigation
results show that the integrated system has effectively
improved the navigation accuracy and the overall
performance.
ACKNOWLEDGMENT
This research was supported by Hubei Key Laboratory
of Automotive Power Train and Electronic Control (Hubei
University of Automotive Technology) (Grant No.
ZDK201101), and Science and Technology Program of
REFERENCES
[1] O. J. Woodman, “An introduction to inertial navigation,”
Technical reports, UCAM-CL-TR-696, Aug. 2007, pp. 4-7.
[2] K. J. Walchko, and A. C. Mason, “Inertial Navigation,” Florida
Conference on Recent Advances in Robotics, 2002, pp. 1-8.
[3] A. Theiss, D. C. Yen, and Ch. Y. Ku, “Global Positioning Systems:
implementations, ” Computer Standards&Interfaces, vol. 27, Feb.
2005, pp. 89-100.
[4] M. S. Grewal, L. R. Weill, and A. P. Andrews, “Gloable
Positioning Systems, Inertial Navigation, and Intergration,” 2nd ed.,
BEIJING:Publishing Houuse of Electronics Industry, 2011.
[5] H. Wu, T. Lei, and R. Jian, “New Approach for GPS/INS
Integrated Navigation,” The 3rd International Symposium on
Intelligent Information Technology Application Workshops, IEEE
Computer Society, 2009, pp. 312-316.
[6] Y. J. Yang, H. W. Bian, W. F. Tian, and Zh. H. JIN, “A New
INS/GPS Integrated Navigation Technique,” Journal of Chinese
[7] D. Simon, “Optimal State Estimation: Kalman, H, and Nonlinear
Approaches,” Published by Wiley-Interscience, Jun. 2006.
[8] L. J. Wang, H. Ch. Zhao, and X. N. Yang, “The modeling and
simulation for GPS/INS integrated navigation system,”
International conference on microwave and millimeter wave
technology, Apr. 2008, pp. 1991-1994.
[9] J. L. Wang, H. K. Lee, and C. Rizos, “GPS/INS integration: A
performance sensitivity analysis,” Wuhan University Journal of
Natural Sciences, vol. 8, Feb. 2003, pp. 508-516.
Practice Using MATLAB,” 3rd ed., New Jersey: John Wiley&Sons,
2008.