Professional Documents
Culture Documents
H INS: A Low-Cost Gps Aided Inertial Navigation System For Vehicle Applications
H INS: A Low-Cost Gps Aided Inertial Navigation System For Vehicle Applications
APPLICATIONS
Isaac Skog and Peter Händel
KTH Signals, Sensors and Systems, Royal Institute of Technology
SE–100 44 Stockholm, Sweden
phone: + (46) 8 790 8742, fax: + (46) 8 790 7260, email: skog@kth.se.
web: www.s3.kth.se
ABSTRACT Navigation
In this paper an approach for integration between GPS and inertial
INS input INS output
navigation systems (INS) is described. The continuous-time nav-
igation and error equations for an earth-centered earth-fixed INS
system are presented. Using zero order hold sampling, the set of H
equations is discretized. An extended Kalman filter for closed loop
integration between the GPS and INS is derived. The filter propa- -
gates and estimates the error states, which are fed back to the INS
for correction of the internal navigation states. The integration al-
KF +
GPS input
gorithm is implemented on a host PC, which receives the GPS and
inertial measurements via the serial port from a tailor made hard-
Figure 1: Loosely coupled position aided closed loop implementa-
ware platform, which is briefly discussed. Using a battery operated
tion of a GPS aided INS system. KF denotes the extended Kalman
PC the system is fully mobile and suitable for real-time vehicle nav-
filter, and H the map between navigation output and GPS data.
igation. Simulation results of the system are presented.
1. INTRODUCTION
2. NAVIGATION DYNAMICS
Today many vehicles are equipped with global positioning system
(GPS) receivers that constantly can provide the driver with infor- A strap-down INS comprises two distinguished parts. The iner-
mation about the vehicles position with an accuracy in the order of tial measurement unit (IMU) housing the accelerometers and gyros.
15-100 meters [1]. However, the GPS receiver has two major weak- The computational part, consisting of several differential equations,
nesses. The slow update rate, only once a second for most receivers translates the measurements into position, velocity and attitude es-
and the sensitivity to blocking of the satellite signals. An inertial timates. The calculations are performed in two steps. From the
navigation system (INS) is an alternative tool for positioning and gyro measurements the directional cosine matrix relating the body
navigation. A classical reference on low-cost INS for mobile robot coordinate frame to the used navigation frame is propagated. The
applications is [2]. In the opposite of the GPS receiver an INS is coordinate rotation matrix is then used when solving the differential
self contained and can provide position, velocity and attitude esti- equations relating accelerations in the body frame to the navigation
mates at a high rate, typically 100 times per second [3]. However, coordinate system. An earth-centered earth-fixed (ECEF) naviga-
due to the integrative nature of the INS, low frequency noise and tion coordinate system implementation of the INS has the advan-
sensor biases are amplified. The unaided INS may therefore have tage of producing positions estimate in the same coordinate system
unbounded position and velocity errors [2]. These complementary as used by the GPS system, which simplifies the integration between
properties make an integration of the two systems suitable, which is the two systems [1].
the topic of this paper.
The primary motivation for the reported work is the in-house 2.1 Navigation equations
need for a GPS aided INS test-bed for education and research in The continuous-time navigation equations in the ECEF frame
the area of vehicle navigation and performance analysis. A goal are [6]
is to develop a hardware platform and additional software, which
together with a standard host-computer will work as a basic GPS ṙe = ve
aided INS. From the software skeleton the student/researcher can v̇e = Reb f b − 2 Ωeie ve + ge (1)
then build an application meeting their demands. Related work do
exist, for example in [4] a field evaluation of a low-cost GPS aided Ṙeb = Reb Ωbeb
INS installed in a car is presented. In [4], the strap-down INS is in-
tegrated with two different GPS solutions (pseudo range and carrier where re and ve denote the position and velocity in 3-dimensional
phase differential GPS, respectively) using a Kalman filter. ECEF coordinates, respectively. The superscripts e, b and i (that
In this paper a loosely coupled position aided method is pro- will be used below) are used to denote in which coordinate frame
posed which allows the designer to keep the costs low by using an a variable is resolved in, that is the ECEF, body or inertial frame.
off-the-shelf GPS receiver that provides position estimates employ- Further, f b is the measured acceleration in the body-frame, g e is
ing NMEA (National Marine Electronics Association) data trans- the position dependent, but known earth acceleration in ECEF co-
mission protocol and sentence format. In Section 2, the INS equa- ordinates, that may be compensated for. The rotation matrix, R eb
tions and the corresponding error models are introduced. Next the transforms a vector in the body frame to the ECEF frame. Although
navigation dynamics are discretized in Section 3. Section 4 presents Reb has nine elements, it has only three degrees of freedom and can
an indirect extended Kalman filter (EKF) for the integration be- be uniquely described by the three Euler angles, in the sequel gath-
tween the GPS and INS data. The INS provides the reference tra- ered in the vector q [5]. The matrices Ωbeb and Ωeie are the skew-
jectory and output. The EKF then estimates the errors, which are symmetric matrix representations of the angular rates web b and w e ,
fed back to the INS for correction of its internal states, resulting ie
b b e e
defined such that a × web = Ωeb a and a × wie = Ωie a, where a is
in a closed loop integration, see Figure 1. Implementation aspects,
simulations results and conclusions are presented in Section 5. a 3 × 1 dimensional vector. The matrix Ωbeb reads
The error equations (7) are time-varying, since Reb depends on the
b b attitude and Fe (as defined below) on the acceleration of the vehicle.
0 −web web
In (8), I3 (03 ) denotes the unity (zero) matrix of order 3 and Fe is
z y
b b
Ωbeb = w 0 −web
ebz x
(2) the skew symmetric matrix, defined as
−webb b
web 0
y x
0 − f3e f2e
e e 0 e
b , w b and w b are the angular rates of the F = f3 − f1 (10)
where the elements web x eby ebz − f2e f1e 0
body (vehicle) frame relative to the ECEF frame, resolved in the
body navigation frame. The matrix, Ωeie has the structure of (2) where f`e denotes the acceleration along the `:th coordinate axis in
with the components of web b replaced by the corresponding com- the ECEF frame.
ponents of the earth rotational rate wiee . Since variations in the The constructed IMU platform houses three separate ac-
earth rotational rate wiee are neglectable, Ωeie is assumed constant celerometers and gyros, therefore the sensor noises are assumed un-
and known, while Ωbeb depends on the body to ECEF angular rates, correlated [9]. However, the accelerometers respectively gyros are
b = [w b w b w b ]∗ and thus is time-varying. Here (·)∗ denotes
of the same model and thus assumed to have similar noise character-
web 2 and s 2
istics. Let sacc
ebx eby ebz gyro denote the variance of the accelerometer
the transpose operation. The body to ECEF angular rates are ob- and the gyro noise, respectively. Then the covariance matrix, Q c (t)
tained by subtracting the angular rate of the earth, resolved in body of the Gaussian measurement noise uc (t) in (6) is given by
coordinates from the gyro outputs wib b , that is
e b
web = wib − Rbe wiee (3) 2 I
sacc
3 03 4
E{uc (t + t ) u∗c (t)}= 2 I d (t ) = Qc (t ) (11)
where the rotation matrix from the ECEF to body coordinates is 03 sgyro 3
d ẋ(t) = F(t) d x(t) + G(t) uc (t) (7) 3.2 Discrete time error equations
where F(t) is the 15 × 15 matrix Having a continuous-time equation as in (7) with a known solution
at time t0 , the solution at a time t > t0 can be represented as [7, 8].
03 I3 03 03 03
03 −2Ωeie −Fe Reb 03
−Ωeie 03 Reb
Z t
F(t) = 03 03 (8)
d x(t) = Φ(t,t0 ) d x(t0 ) + Φ(t, t ) G(t ) uc (t )d t (16)
0 0 03 03 03
3 3
t0
03 03 03 03 03
and G(t) is of size 15 × 6 where the state transition matrix Φ(t,t0 ) is defined as the unique
solution to ¶ Φ(t, t )/¶ t = F(t) Φ(t, t ). If the state transition matrix
F(t) in (8) is assumed time invariant, the homogenous differential
03 03
Reb 03 equation has the solution of the matrix exponential function, that
G(t) = 03
Reb
(9) is Φ(t, t ) = exp (F · (t − t0 )) [8]. In the case of F(t) being time
0
3 03 varying, F(t) can be approximated as a constant matrix F between
03 03 the sampling instants, if the sample rate is high compared to the rate
of change in F(t). Using the power series definition of the matrix
exponential, the state transition matrix between time instants kTs zk = znom
k + d zk (26)
and (k + 1)Ts can be approximated as
ãk = anom
k + d ak (27)
where znom
k and anom
are the nominal trajectory and input, respec-
k
Φ((k + 1)Ts , kTs ) ≈ I15 + F(kTs )Ts (17) tively. The quantity d zk is the perturbation away from the true tra-
jectory and d ak the bias of the measurements. Assuming that d zk
Hence, the discrete-time error equation becomes and d ak are small and applying a first order Taylor series expansion
of c(z, a), equation (23) can be approximated as
d xk+1 = Ψk d xk + ud,k (18)
where the state transition matrix Ψk = Φ((k + 1)Ts , kTs ) is approx- znom nom nom
k+1 + d zk+1 ≈ c(zk , ak ) + C1,k d zk + C2,k d ak + uk
0
(28)
imated as in (17) and the discrete-time process noise, uk is
Z (k+1)Ts where
ud,k = Φ((k + 1)Ts , s)G(s)uc (s)ds (19)
¶ c(z, a) ¶ c(z, a)
kTs
C1,k = C2,k = (29)
Since ud,k is a linear combination of Gaussian noise, it is Gaussian ¶z z=znom ¶a a=anom
k k
distributed and described by its first and second order moments. The
mean of ud,k is zero, since uc (t) is assumed zero mean. Applying The Jacobians of c(z, a) are updated with nominal trajectory and
the definition of covariance and assuming Ts small, the covariance input for each sample. Choosing znom
k and anom
k to fulfill the deter-
of the discrete-time noise Qd,k can be approximated as [1] ministic difference equation
where diag(·) denotes a block diagonal matrix. The last equality is d zk+1 = C1,k d zk + C2,k d ak + u0k (31)
a result of the orthonormality property of the rotation matrix Reb , ∗
Note that d xk = d zk d ak , and thus it becomes clear that C1,k
∗ ∗
and that Qc (t) is a diagonal matrix according to (11). and C2,k correspond to the upper part of the navigation error state
The definition of the state observation equation is straightfor- transition matrix Ψk . The lower 6 × 6 block matrix of Ψk is a de-
ward since the GPS position estimate is used, and not the pseudo scription of how the IMU biases d ak develop with time. Since this
ranges. Let d y be the difference between the GPS and INS position is a linear model the standard Kalman filter equations can be applied
estimate and wd,k the error in the GPS position estimates. Then the to estimate d xk [8]. The Kalman filter equations read
observation equation can be written as
" #
d ẑ− d ẑk
d yk = Hk d xk + wd,k (21) k+1 = Ψk (32)
d âk+1
− d âk
with the state observation matrix Hk of size 3 × 15, defined as
[I3 03×12 ], k = n `, n = 1, 2, 3... " −# nom " #!
d ẑk d ẑk d ẑ−
Hk = (22) zk k
03×15 , otherwise = +K f ,k y k − H k anom − H k (33)
d âk d â−
k k d â−
k
where ` denotes the ratio between the INS and GPS sampling fre-
quency. K f ,k = P− ∗ − ∗
k Hk (Hk Pk Hk + Rd,k )
−1
(34)
k = ãk + d âk
â− −
− − −
ẑk+1 = c(ẑk , âk ) RS232 RS232
d â− GPS mC Host PC
k+1 = [Ψk ]10:15,10:15 d âk
−
− − ∗
Pk+1 = Ψk Pk Ψk + Qd,k
Figure 2: Block diagram of the hardware.
GPS data available. (k = 100, 200, ...)
K f ,k = P− H∗ (Hk P− H∗ + Rk )−1
k k k k
d ẑk
−
09×1 ẑk 150
= + K f ,k yk − Hk
d âk d â−
k 0 6×1
k + d ẑk
ẑk = ẑ− GPS reading
North [m]
ẑ−
k+1 = c(ẑk , âk )
d â−
k+1 = [Ψk ]10:15,10:15 d âk
50
P− ∗
k+1 = Ψk Pk Ψk + Qd,k True trajectory
0
Table 1: The algorithm for integration between GPS and INS data,
with a ratio between the sample rates equal to 100 times.
PSfrag replacements
−50