Journal of Global Positioning Systems (2006)
Vol. 5, No. 1-2:82-87
GPS/INS/Seeker Integrated Navigation System for the Case of GPS
Blockage
Woo Hyun Kim, Jang Gyu Lee
School of Electrical Engineering and Computer Science, Seoul National Un i v e rsity,
133-dong, ASRI #610, San 56-1, Sillim-dong, Gwanak-gu, Seoul, 1 5 1-742, South Korea
Hyung Keun Lee
School of Electronics, Telecommunication & Computer Eng., Korea Aerospace University,
200-1, Hwajon-dong, De ok yang_gu, Goyang-city, Kyunggi-do, 412-791, South Korea
Chan Gook Park
School of Mechanical and Aerospace Engi ne ering, Seoul National University,
San 56-1, Sillim-dong, Gwanak-gu, Seoul, 151-744, South Korea
Abstract. When GPS blockage occurs for a loosely
coupled GPS/INS system, its navigation error diverges.
To deal with such cases, this paper introduces an
integration scheme for GPS, INS, and an image sensor.
The proposed integration scheme is attractive in that it
accomplished the position and velocity accuracy
improvement by the angular information only. The
angular information is provided by the gimbal angles of
the image sensor. A realistic scenario is studied by a
simulation to demonstrate that the GPS/INS/Image
integrated navigation system works effectively.
Keywords. Image sensor, inertial navigation system
(INS), GPS/INS/Image Integrated Navigation System,
GPS/INS loosely coupled system
1 Introduction
Loosely coupled GPS/INS is one of the most popular
integration schemes for modern navigation systems. This
scheme performs well when GPS signal conditions are
good. However when a GPS signal blockage occurs, its
navigation error diverges considerably even within a few
minutes. (Maybeck, 1994). GPS blockages occur due to
physical obstructions, vehicle dynamics, signal jamming,
receiver fault, and satellite faults.
Against these cases, additional information is required to
compensate the GPS blockage error (Titterton, 1997). For
such cases, an image sensor might be a good source of
aiding information to prevent the navigation error
divergence. In Fig. 1, a conventional configuration for a
gimballed image sensor is illustrated. As shown in Fig. 1,
the image sensor is suspended by a two-axis gimbal
system (Young, 2005). Thus, the pitch and yaw angle
measurements regarding the line-of-sight (LOS)
information can be provided by the image sensor.
For the integration of an INS and an image sensor, a
previous study focused on the INS attitude alignment by
utilizing a star tracker (Vathal, 1987). Extending the
previous research work, this paper investigates the
performance improvement of INS by an image sensor in
the cases of GPS blockages. As compared with the
previous research work that mainly deals with the attitud e
accuracy improvement, this paper focuses on the position
and velocity accuracy improvement.
Fig. 1 Configuration of a two-axis gimballed image sensor (Young,
2005)
Kim et al.: GPS/INS/Seeker Integrated Navigation System for the Case of GPS Blockage 83
This paper is organized as follows. At first, important
coordinate systems are introduced. Next, measurement
equations are derived. Thirdly, a simulation result is
demonstrated to validate the performance of the
GPS/INS/Image integration scheme. Finally, concluding
remarks will be given.
2 Design of GP S/ INS/ I mage integra ted navigation
system
When an image sensor searches and locks on to a
landmark’s image at the center of the image plane, at
least 4 frames are necessary to account for this geometry.
They are the navigation frame (n-frame), the body frame
(b-frame), the image sensor frame (s-frame), and the
landmark frame (l-frame) as shown in Fig. 2. The
transformation matrixn
s
C from the image sensor frame
to the navigation frame is decomposed into a
transformation matrixb
s
C from the image sensor frame to
the body frame and the transformation matrixn
b
C from
the body frame to the navig a tion frame.
nnb
s
bs
CCC= (1)
As shown in Fig. 2, the unit LOS vector e coincides with
the x-direction basis vector
s
x
of the image sensor frame.
Eq. (2) expresses the LOS vector e in the image sensor
frame.
[]
100
T
s
e= (2)
Eq. (3) shows how the LOS vector e with respect to the
navigation frame can be obtained by utilizing the
transformation matrixn
s
C.
nns
s
eCe= (3)
The exact LOS vectors shown in Eq. (3) cannot be
obtained in practice. Instead, an estimate of it can be
obtained as follows.
ˆˆ
nnb
s
bs
CCC=% (4)
where
()
ˆnn
bb
CC=Ι−Φ (5a)
(
)
ˆbb
ss
CC=Ι−Ε (5b)
ˆn
s
C : estimated transformation matrix from the image
sensor frame to the navigation frame
ˆn
b
C : transformation matrix from the body frame to
the navigation frame estimated by the INS
b
s
C
% : transformation matrix from the image sensor frame
to the body frame measured by the gimbal angels
Φ
: skew symmetric matrix of the INS attitude error
Ε
: skew symmetric matrix of the gimbal measurement
error of the image sensor
Fig. 2 Geometry of coordinate systems.
The matrixb
s
C in Eq. (5) is defined as follows
coscossincos sin
sin coscossin sin
sin0 cos
ssss
b
s
sss ss
ss
C
ψ
θψψθ
ψ
θψψθ
θθ
=
To construct an indirect Kalman filter, an indirect
measurement should be formed as the difference
between the raw measurement and the filter’s estimate of
its equivalency as follows.
.
ˆINS meas
zy y
=
% (6)
ˆINS
y : value estimate by INS
.meas
y
% : measured value
The gimbal angles provided by the image sensor indicate
the difference between the body frame and the image
sensor frame. Thus, they do not indicate the angles
between the body frame and the navigation frame. Due to
this characteristic, it was determined that the
straightforward derivation like Eq. (6) is difficult and too
complex. To simplify this problem, the unit LOS vector
n
e from the vehicle body to the landmark is utilized as
follows.
1) Derive n
INS
e
$.
2) Derive n
landmark
e
$.
3) Subtract n
INS
e
$ from n
landmark
e
$
n
INS
e
$ : unit LOS vector formed by the image sensor
84 Journal of Global Positioning Systems
84
gimbal angle measurements and INS’s attitude
information.
n
landmark
e
$ : unit LOS vector formed by position of
landmark and INS’s position information.
Indirect measurement is formed as the difference between
n
INS
e
$ and n
landmark
e
$.
As a result, the indirect measurement is obtained as
follows.
()()
LOS
,,,
nn
landmark INS
nn
landmark INS
Ze e
ee
fLlhg
δδ
δδδ
=−
=−
=−ΦΕ
$$
()( )()
()( )( )
()( )()
111 11
222 22
3333 3
1,11,21,3 000000000
2,12,22,3 000000000
3,13,23,3 000000000
A
BC PYPZ
A
BC PYPZ
A
BC PYPZ
⎡ ⎤
−−
⎢ ⎥
=−−
⎢ ⎥
⎢ ⎥
⎣ ⎦
[
]
GPS 333333333332
00000ZI
××××××
=
GPS
GPS-INS-LOS LOS
Z
ZZ
⎡⎤
=⎢⎥
⎢⎥
⎣⎦
(7)
As shown above, n
landmark
e
δ
is affected by the position
error, n
INS
e
δ
is affected by the function of vehicle’s
attitude error and the gimbal measurement error. For
convenience, they are denoted by
()
,,
f
Llh
δ
δδ
and
(
)
,gΦΕ , respectively.
2.1 Derivation of n
INS
e
$
To estimate the LOS vector resolved with respect to the
navigation frame, we utilize Eq. (4). For this purpose, the
attitude information of the INS and the gimbal angle
measurements of the image sensor are utilized. The LOS
vector apparently coincides with the x-axis basis vector
of the LOS frame. Thus, the LOS vector resolved with
respect to the navigation frame can be computed as
follows.
[]
ˆ100
nT
n
INS s
eC=
$ (8)
The transformation matrix ˆn
s
C shown in Eq. (8) contains
error terms due to Eqs. (5a) and (5b). It is summarized as
follows.
ˆnn n
s
ss
CC C
δ
=+
ˆ
ˆˆ
nnn
sss
nn b
s
bs
CCC
CC C
δ
=−
=−Ε−Φ % (9)
Combining Eqs. (3), (8), and, (9), n
I
NS
e
δ
can be derived
as follows.
nnn
INS INS INS
ee e
δ
=+
$
[]
()
100 ,
T
nn
INS s
eC g
δδ
=
=ΦΕ (10)
[][][][][ ]
[][][][][ ]
[][][][][ ]
()
()
()
11111
222 2 2
333 3 3
1,1
2,1
3,1
zy
zy
zy
CABPZPY
CABPZPY
CABPZPY
δγδα δβφφ
δγδα δβφφ
δγδαδβ φφ
+−+ −
=−+− +−
+++ −
⎡⎤
⎢⎥
=−⎢⎥
⎢⎥
⎣⎦
As shown in Eq. (10), n
I
NS
e
δ
is a 3-by-1 matrix. Denoting
,,,,
yz
δ
αδβδγφ φ
as the roll error, pitch error, yaw error,
gimbal pitch angle error, and gimbal yaw angle error,
respectively, the elements of n
INS
e
δ
are summarized as
follows.
()
[sin sincoscos sin][cos cos]
[coscos][cos sin]
[cos sincossinsin][cos sin]
[sinsincoscossin][sin ]
[coscos][sin ]
[cos sincos
1,1
bbbb bss
bb ss
bbbbbs s
bbbb bs
bb s
bb
φθ ψφψθ ψ
δγ θψ θψ
φθ ψφψθψ
δα φθ ψφψθ
θψ θ
δβ φθ ψ
⎡⎤
⎢⎥
⎣⎦
+
⎡⎤
+⎢⎥
+−
⎣⎦
+
=
sin sin][cos cos]
[coscos][ sin]
[sinsincoscossin] []
[cos ]
[cos cos][sin cos ]
[sin sincoscos sin]
[sin sin]
[cos sin
bbbss
bb s
bbbb bz
s
bb ss
bbbb b
ss
b
φψθ ψ
θψ ψ
φθ ψφψφ
ψ
θψ θψ
φθ ψφψ
θψ
φ
⎛⎞
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
+
⎝⎠
⎛⎞
⎜⎟
+−
⎜⎟
⎜⎟
⎝⎠
++−
+
[]
cossin sin]
[cos ]
y
bb bb
s
φ
θψ φψ
θ
⎛⎞
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎛⎞
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
+
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎝⎠
⎝⎠
()
[sin sinsincos cos][cos cos]
[cos sin][cos sin]
[cossinsin sincos][cossin]
[sinsinsincoscos][sin ]
[cossin][sin ]
[cos sinsin
2,1
bb bbbss
bb ss
bb bbbss
bb bbbs
bb s
bb
φθψφψ θψ
δγ θψ θψ
φθψφψ θψ
δα φθψφψ θ
θψ θ
δβ φθψ
+
⎡⎤
⎢⎥
⎣⎦
⎡⎤
+⎢⎥
++
⎣⎦
+
=
sin cos][cos cos]
[cossin][ sin]
[sinsinsincoscos] []
[cos ]
[cos sin][sin cos]
[sin sin sincos cos]
[sin sin]
[cos sin
bbbss
bb s
bb bbbz
s
bbs s
bb bbb
ss
b
φψ θψ
θψ ψ
φθψ φψφ
ψ
θψ θψ
φθψ φψ
θψ
φ
⎛⎞
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎝⎠
⎛⎞
⎜⎟
++
⎜⎟
⎜⎟
⎝⎠
+++
+
[]
sinsin cos]
[cos ]
y
bbb b
s
φ
θψ φψ
θ
⎛⎞
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎛⎞
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎝⎠
⎝⎠
Kim et al.: GPS/INS/Seeker Integrated Navigation System for the Case of GPS Blockage 85
()
[sincos ][coscos]
[sin ][cossin]
[coscos ][cossin]
[sincos ][sin ]
[sin][sin ]
[coscos ][coscos]
3,1
[sin ][sin]
[sin
bbs s
bss
bb ss
bbs
bs
bbs s
bs
b
φθ θψ
δγ θθψ
φθ θψ
δα φθ θ
θθ
δβ φθ θψ
θψ
φ
⎛⎞
⎡⎤
⎜⎟
⎢⎥
+
⎣⎦
⎜⎟
⎜⎟
⎡⎤
⎜⎟
+⎢⎥
+
⎜⎟
⎣⎦
⎜⎟
⎡⎤
⎜⎟
⎢⎥
⎜⎟
⎜⎟
=⎣⎦
⎝⎠
+
+
[]
cos ][cos]
[sin ][sincos]
[sincos][sinsin] []
[coscos][cos ]
z
bs
bss
bb ssy
bbs
φ
θψ
θθψ
φθ θψφ
φθθ
⎡⎤
⎢⎥
⎢⎥
⎢⎥
⎢⎥
⎢⎥
⎢⎥
⎢⎥
⎢⎥
⎢⎥
⎢⎥
⎛⎞
⎛⎞
⎢⎥
⎜⎟
⎜⎟
⎢⎥
⎝⎠
⎜⎟
⎢⎥
⎜⎟
⎛⎞
⎢⎥
⎜⎟
⎜⎟
⎢⎥
−+⎜⎟
⎜⎟
⎢⎥
⎜⎟
⎜⎟
⎜⎟
+
⎢⎥
⎢⎥
⎝⎠
⎝⎠
⎣⎦
2.2 Derivation of n
lan d ma rk
e
$
If the exact positions of the landmark and the vehicle are
known, the unit LOS vector n
e can be obtained,
differently from (3), as follows.
(
)
1
nnn
eDD
= (11)
nn
L
DCL=∆ (12)
Where Eq. (12) denotes the position difference between
the landmark and the vehicle. As shown in Eq. (12), it is
the function of the differences in latitude, longitude, and
height.
[] []
TT
landmark vehicle
LLlhLlh∆=− (13)
The scaling matrix n
L
C shown in Eq. (12) is purposed to
transform the latitude, longitude, and, height differences
to the difference vector resolved with respect to the
locally-level navigation frame.
()
00
0cos0
001
m
n
Lt
Rh
CRhL
⎡⎤
+
⎢⎥
=+
⎢⎥
⎢⎥
⎣⎦
(14)
In Eq. (14), m
R denotes the meridian radius of curvature
and t
R denotes the transverse radius of curvature of the
earth.
Since it is hard to obtain the exact po sition of vehicle and
the exact position of vehicle is an object of estimation, it
is not available in practice. Instead , th e estimated latitu d e,
longitude, and height info rmation of the INS is utilized as
follows.
()
1
ˆˆ
nnn
landmark
eDD
=
$ (15)
ˆˆ
nn
L
DCL
=
(16)
$
[]
$
$
T
T
landmark
I
NS
LLlh Llh
∆= −
$ (17)
Combining Eqs. (16) and (17), the following
relationshi p s c a n be derived.
[]
ˆT
nn n
LL INS
DCLCLlh
δδδ
=∆−
ˆnnn
D
DD
δ
=+ (18)
[]
T
nn
LINS
DCLlh
δδδδ
=− (19)
[]
$
[]
ˆT
TT
INS vehicle
INS
LlhLlh Llh
δδδ
⎡⎤
=−
⎣⎦
$ (20)
The estimated LOS vector n
landmark
e
$ can be obtained by
applying the perturbation method to Eqs. (11) and (15)
with having such error sources.
nnn
landmark landmark landmark
ee e
δ
=+
$
ˆˆ
...
ˆ
ˆˆ
n
nnn
nn
landmark n
nnn
D
DD D
eDHOT
D
DD D
δ
⎛⎞
⎜⎟
==+ +
⎜⎟
⎝⎠
$
ˆ
ˆˆ
n
n
nn
landmark nn
D
D
eD
DD
δ
δ
⎛⎞
⎜⎟
=⎜⎟
⎝⎠
33 3
ˆˆˆ
1
ˆˆˆ
ˆ
nnnT
nnn
n
DDD
I
DDD
D
×
⎛⎞
⎜⎟
=−
⎜⎟
⎝⎠
()
()
1
13
33 ˆˆˆˆ ,,
nnnnTnn
landmark
eDDDDDfLlh
δ
δδδδ
×
⎛⎞
⎛⎞
≈Ι −=
⎜⎟
⎜⎟
⎜⎟
⎝⎠
⎝⎠
[]
()( )()
()( )()
()()()
_
1,11, 21, 3
2,12,22,3
3,13, 23, 3
T
nn
landmark landmarkINS
INS
epriorie Llh
L
l
h
δδδδδ
δ
δ
δ
⎡⎤
⎡⎤
⎢⎥
⎢⎥
=⎢⎥
⎢⎥
⎢⎥
⎢⎥
⎣⎦
⎣⎦
(21)
2.3 Handling rank deficiency
As previously explained, the image sensor provides two
direct measurements, i.e., pitch and yaw gimbal angles.
However, the indirect measurement formed by the direct
measurements is 3-by-1 vector. Thus, a rank deficiency
can occur, which means that one element is a linear
combination of the other two elements. Since the Kalman
filter assumes independent measurements, the rank
deficiency should be eliminated.
86 Journal of Global Positioning Systems
86
To eliminate the rank deficiency, the observation matrix
H should be decomposed into two parts; INS part and
image sensor part.
()()()
()( )()
()( )()
111 11
222 22
33333
1,11,21,3 000000000
2,12,22,3 000000000
3,13,23,3 000000000
ABC PYPZ
H
ABC PYPZ
ABC PYPZ
⎡ ⎤
−−
⎢ ⎥
=−−
⎢ ⎥
⎢ ⎥
⎣ ⎦
()( )()
()()()
()()()
111
222
333
1,11,21,3000000000
2,12,22,3 000000000
3,13,23,3 000000000
INS
ABC
HABC
ABC
⎡⎤
⎢⎥
=−
⎢⎥
⎢⎥
⎣⎦
11
LOS2 2
33
PY PZ
H
PY PZ
PY PZ
⎡⎤
⎢⎥
=−
⎢⎥
⎢⎥
⎣⎦
(22)
Meanwhile, by the singular value decomposition, the
image sensor part observation matrix can be rewritten as
follows.
LOS T
H
USV=
T
UU I=
T
VV I= (23)
Where U and V denote orthonormal matrix and S
denotes quasi-diagonal matrix with the singular value as
its diagonal elements, respectively. If we denote
B
as the
first two columns of the orthonormal matrix U, it can be
verified that T
B
His a full rank matrix and the following
relationships are satisfied.
LOS LOS
T
ZBZ=
(
T
H
BH=
(
LOS LOS
T
HBH=
(
()
1
LOSLOS LOS
1
LOS LOS
TT
ZHZ
BH BZ
=
=
)
((
(24)
It can be verified that LOS
Z
)
shown in Eq. (24) is a 2-by-1
vector and is free of rank deficiency. As a result, the
Kalman filter utilizes the following full rank
measurement vector instead of that shown in Eq. (7).
-- LOS
GPS
GPS INS LOS
Z
ZZ
⎡⎤
=⎢⎥
⎢⎥
⎣⎦
) (25)
3 Simulation a nd result
To assess the performance of the GPS/INS/Image
integrated navigation system, a simulation has been
performed. For the simulation, the initial latitude error,
longitude error, and height error are set as 40/Ro[rad],
40/Ro[rad], and 40[m], respectively, where Ro denotes
the mean radius of the earth. The initial velocity error in
each direction is set as 10[m/sec], respectively. The initial
roll error of vehicle, pitch error of vehicle, and yaw err or
of vehicle are set as 1[deg], 1[deg], and, 5[deg],
respectively. The scale factor, misalignment error, white
noise, and, random constant of the accelerometer are set
as 200[ppm], 10[arcsec], 5[
g
µ
], and 100[
g
µ
],
respectively.
It is assumed that GPS blockage occurs at from 121[sec]
to 180[sec] during the flight. To enhance the
observability, an S-shape turn trajectory is applied to
vehicle trajectory.
The state variables used for the loosely-coupled GPS/INS
system are the position errors, velocity errors, attitude
errors, accelerometer bias, and, gyro drift (Young Bum
Park, 2001). The proposed GPS/INS/Image integrated
navigation system augments the conventional 15 error
states by the two bias values in the image sensor’s pitch
and yaw measurements.
111233 33 3333
21222324333 3
3132333 3353 3
GPS-INS-LOS 33 33 33 33 3333
33 33 33 33 3333
22 22 22 22 2266
0000
00
00
000000
000000
00000
FF
FFFF
FFF F
F
F
××××
××
××
××××××
××××××
×××××
=
T
LOS
T
T
INS
XX X
δδ δ
=
[ ]
INS
T
INSINSINSNEDXYZX Y Z
X
LlhVVV
δ
δ
δδ δδδδαδβδγεεε
=
∇∇∇
LOS
T
yz
X
δ
φφ
=
Fig. 3, Fig. 4, Fig. 5 show position, velocity, and, attitude
errors of the two integration schemes, respectively. As
shown in all figures, the GPS/INS loosely coupled system
shows an accumulation of navigation error during the
GPS blockage between 121[sec] and 180[sec].
As compared, the proposed GPS/INS/Image integrated
navigation system generates a trajectory which is close to
the truth trajectory during all the time.
Thus, it can be concluded that it is possible to bound all
the navigation errors by utilizing only the two angle
measurements.
It is interpreted that the error bounding is possible
tthrough the direct range information between the vehicle
and the landmark is not available. The attractive error
bounding feature is interpreted due to the observability
enhancement brought by the vehicle’s S-turn.
Kim et al.: GPS/INS/Seeker Integrated Navigation System for the Case of GPS Blockage 87
Fig. 3 Comparison position err ors.
Fig. 4 Comparison velocity errors.
Fig. 5 Comparison attitude errors.
4 Conclusions
The paper proposed a new GPS/INS/Image integration
scheme to prevent the INS error accumulation during
GPS blockages.
For the purpose, two gimbal measurements of the image
sensor are utilized. Assuming that the image sensor
acquires a known landmark in the center of its image
plane, a simulation was performed.
By the simulation result, it was confirmed that it is
possible to bound all the nav igation error s by utilizing the
image sensor’s two angle measurements only.
Acknowledgements
This work has b een suppor ted by the Agency for Defence
Development (ADD) and the Automatic Control
Research Center (ACRC), Seoul National University,
South Kor ea.
References
Maybeck P.S. (1994) Stochastic Models, Estimation, and
Control, Vol.1, Navtech Book & Software Store
pp.291~297.
Park Y.B. (2001) Design of Integrated INS/GPS/Odometer
Navigation System and Its Performance Analysis, School
of Electronical Engineering and Computer Science, Seoul
National University.
Titterton D.H., Weston J.L. (1997) Strapdown inertial
navigation technology, Peter Peregrinus Ltd., pp.363~394.
Vathal S. (1987) Spacecraft Attitude Determination Using a
Second-Order Nonlinear Filter, Journal of Guidance and
Control, Vol.10, No 6, pp.559~565.
Young R.M. (2005) Feedback Linearization Control of a 2-
Axis Gimbal Seeker in BTT Missile, School of
Electronical Engineering and Computer Science, Seoul
National University.