Professional Documents
Culture Documents
Navigation With IMU/GPS/Digital Compass With Unscented Kalman Filter
Navigation With IMU/GPS/Digital Compass With Unscented Kalman Filter
Dalhousie University
1360 Barrington Street, Halifax, NS, Canada B3J 1Z1
Email: {jason.gu, phuynh}@dal.ca
Abstract— Autonomous vehicle navigation with standard IMU under-water [5]. Therefore, for a reliable navigation system,
and differential GPS has been widely used for aviation and another type of sensor, such as IMU, is needed.
military applications. Our research interesting is focused on
The inertial navigation system (INS), originally developed
using some low-cost off-the-shelf sensors, such as strap-down
IMU, inexpensive single GPS receiver. In this paper, we present in the mid 60s for Missile Guidance systems, measures the
an autonomous vehicle navigation method by integrating the accelerations and rotations applied to a system’s inertial frame
measurements of IMU, GPS, and digital compass. Two steps are of reference. These measurements are done via an inertial
adopted to overcome the low precision of the sensors. The first is measurement unit (IMU) that consists of three linear ac-
to establish sophisticated dynamics models which consider Earth
celerometers (devices that measure acceleration) and three rate
self rotation, measurement bias, and system noise. The second
is to use a sigma Kalman filter for the system state estimation, gyroscopes (devices that measure angular rotation rate). An
which has higher accuracy compared with the extended Kalman IMU system assembled from low-cost solid state components
filter. The method was evaluated by experimenting on a land is always constructed in a strap-down configuration. The strap-
vehicle equipped with IMU, GPS, and digital compass. down means that the gyroscopes and accelerometers are fixed
to a common chassis and are not actively controlled on gimbals
Index Terms— Sensor fusion. Navigation. Unscented Kalman
filter. Estimation. to align themselves in a prespecified direction [4]. The strap-
down configuration makes the IMU widely accessible. But the
I. I NTRODUCTION tradeoff for a low cost IMU is its high noise and low accuracy.
These drawbacks of the strap-down IMU can be overcome by
Autonomous navigation vehicles, used for military missions, careful filter design which is based on appropriate dynamic
forest surveys, and ocean environment inspection, usually em- modeling.
ploy multiple sensors of various types. The sensors commonly In this paper, we present a general dynamic model for a
used in these applications can be classified into two broad strap-down IMU sensor for an autonomous navigation vehicle.
categories [4]: dead-reckoning sensors and external sensors. The initial state of the IMU is calibrated by using a digital
Generally dead-reckoning sensors are very robust, but accu- compass. And the IMU, GPS receiver, and digital compass
mulate errors with time. Therefore, they must be periodically are combined by using an unscented Kalman filter (UKF) [3]
reset by using information from external sensors. External to obtain an optimal state of the vehicle.
sensors provide absolute information by making measurements
from known landmarks. In our navigation platform, a low cost II. P REVIOUS W ORK
Inertial Measurement Unit (IMU) is used as a dead-reckoning
sensor, while a Global Positioning System (GPS) receiver and The integration of an inertial measurement unit (IMU) with
a digital compass are used as external sensors for the outdoor a global positioning system (GPS) receiver has been done for
navigation mission. application areas in which attitude information is indispensable
The rapidly expanding use of the Global Positioning System and rapid collection of geographic information is required.
(GPS) enables commercial navigation devices to be more Because of its high price and government regulation, the
popular and attainable for the civil users. GPS provides ab- high performance IMU is only used in military application
solute positioning information covering any part of the world and commercial airliners. Recent research efforts have been
during day and night. However, the signal quality from an focused on using a low-cost strap-down IMU.
adequate number of GPS satellites for the recipient is still Farrell and Barth [1] introduced a very general method to
critical in using GPS as navigation devices, such as under establish an error process model for the INS/GPS navigation
trees, inside buildings, in tunnels, between tall buildings, and system. The error process model is obtained by using first
1498
The transformation matrix Cbn is expressed by
⎛ ⎞ Prediction Estimation
cθcψ sφsθcψ − cφsψ sφsψ + cφsθcφ
Cbn = ⎝ cθsψ cφcψ + sφsθsψ cφsθsψ − sφcψ ⎠ (4)
−sθ sφcθ cφcθ
and measurement noise respectively. They are assumed to be χi =x̄ + ( (n + κ)Pxx )i i = 1, · · · , n (11)
independent of each other, white, and with normal probability
distributions p(w) ∼ N (0, Q) and p(v) ∼ N (0, R). The χi =x̄ − ( (n + κ)Pxx )i i = n + 1, · · · , 2n
1499
Predict (time update) Gyros rate
transform Unscented
Calculate the mean and IMU State prediction Kalman
Estimation Acceleration
covariance of the Filter
(measurement update) transform
transformed sigma points
Frame
Sensors
Transformation
Output
Transform the sigma points GPS State estimation
according to process & Generate sigma points
measurement model
small positive value. λ is a secondary scaling parameter which Ȳk = h(χ̄xk , χ̄vk ) (22)
is usually set to 0, and β is a parameter used to
incorporate any 2N
prior knowledge about the distribution of x. ( (n + κ)Pxx )i ȳk = Wim Ȳk (23)
is the ith row or column of the matrix square root of ((n + i=0
κ)Pxx ) and Wi is the weight which is associated with the ith c). Measurement update:
point. These sigma points are propagated through the function 2N
P yk yk = Wic [Ȳi,k − ȳk ][Ȳi,k−1 − ȳk ]T (24)
Yi = f (χi ) i = 0, · · · , 2n (13) i=0
2N
and the mean and covariance for Y are approximated using a Pxk yk = Wic [χ̄i,k − x̄k ][Ȳi,k − ȳk ]T (25)
weighted sample mean and covariance of the posterior sigma i=0
points, K = Pyk yk Pxk yk (26)
x̂k = x̂k−1 + K(zk − ȳk ) (27)
2n
ȳ = Wim Yi (14) Pk = Pk−1 − KPyk yk K T
(28)
i=0
2n
The N in the previous equations is the dimension of the
expended state space, which is equal to Nx +Nw +Nv . Where
Pyy = [Yi − ȳ][Yi − ȳ]T (15)
Nx is the dimension of the original state xk ; Nw and Nv are
i=0
the dimension of noise wk and vk , respectively. In Eq.(17)
The unscented Kalman filter (UKF) can be implemented Q is the process noise covariance, and R is the measurement
using UT by expanding the state space to include the noise noise covariance. Wi is the weight calculated by Eq.(12).
component: xak = (xTk , wkT , vkT )T . The UKF can be summa- V. S YSTEM I MPLEMENTATION
rized as follows [3], [9].
As soon as the process model and measurement model have
1. Initialization:
been established, the Unscented Kalman filter procedure can
T be implemented directly. The system diagram is shown in
x̂a0 = x̂T0 0 0 (16) Fig. 3. For our practical case, two issues must be addressed.
⎛ ⎞ A. Sigma point propagation of UKF
P0 0 0
P0a = ⎝ 0 Q 0 ⎠ (17) The implementation of the unscented Kalman filter needs
0 0 R to create sigma points from a mean and covariance. Before
creating the sigma points, the state vector has to be augmented
to handle nonlinear process noise np and measurement noise
2. Iteration for each time step k(∈ 1, · · · , ∞)
νgps . Let the augmented states be Y ∈ N ×1 , (N = Nx +
a). Calculation the sigma points
Nn +Nnu ). Where Nx is the number of state variables, and Nn
and Nnu are the dimension of process noise and the dimension
χak−1 = x̂ak−1 x̂ak−1 ± (n + κ)Pk−1
a (18) of measurement noise, respectively. Then in any time step k
1500
of the UKF, we can obtain a set of sigma points yi (i =
IMU initial position
t1
1 · · · 2N + 1). These sigma points are transformed by using a t
the dynamic process model (Eq. (7)) to obtain the points χi . IMU prediction
Since the process model is nonlinear equations, we can assume
t1 t2 t3
the nonlinear function is defined as b t
χi = F (yi , ab , ωib
b
, np )
IMU prediction System estimation
= f (yi , ab , ωib b
, np )dt (29)
c t
4th order Runge Kutta integration between the time step k − 1 GPS measurement
and k. The time interval is defined as ΔT . At time step k − 1,
we have y(k − 1). And at time step k, the IMU has provided Fig. 4. IMU and GPS measurement time steps. a. IMU initial
the measurement abib (k), ωibb
(k). The χi (k) can be obtained position obtained from calibration; b. system predict according to
using the algorithm 1. IMU measurement; c. system estimation when GPS measurement is
received
t
k1 = ΔT f (yi (k − 1), abib (k), ωib
b
(k))
k2 = ΔT f (yi (k − 1) + 12 ΔT k1 , abib (k), ωib
b
(k)) tGPS
B. Synchronization with INS measurement data for IMU is shown on Fig.7. Since the strap-
Usually, measurement frequency in IMU is higher than that down IMU is very sensitive to noise and environment, the
in GPS. And their measurement times will not be coincident measurement data are denoised with wavelet filter before they
(Fig. 4). In real time, linear extrapolation is used to obtain the are used for vehicle state estimation. Fig.8 is the measurement
IMU’s predict position at the time the GPS gets its position data after denoising and initial alignment based on the digital
by the following equation (Fig. 5) compass measurement at the static state of the system.
The IMU is a dead reckoning sensor, and any noise and
pIM U (tGP S ) =pIM U (tk−1 )+ bias in the measurement of accelerometer in the IMU will
pIM U (tk−1 ) − pIM U (tk−2 ) cause second order deviation for its position estimate. And
(tGP S − tk−1 )
tk−1 − tk−2 any noise and bias in the measurement of gyro in the IMU
(30) will change the estimate of the direction of vehicle greatly.
So the measurements from a strap-down IMU can only be
where P IM U means the predicted position of IMU. From
Fig. 4, we know the IMU always predicts the vehicle state as
soon as it gets measurement, and the navigation system starts
to estimate its position when the GPS receives measurement
data. The previous state prediction only based on the IMU can
be updated based on the estimation.
1501
Fig. 7. Raw measurement data from IMU. The top is the accelerations Fig. 9. The trajectory estimated from our field experiment
from accelerameters in IMU, and the bottom is the rotation rate from
gyros in IMU
experimentation of a land vehicle equipped with IMU, GPS,
and digital compass on a parking lot.
1502