Documente Academic
Documente Profesional
Documente Cultură
Keerthana Atchutuni
Electrical and
katchutuni@ucdavis.edu
ABSTRACT
A nonlinear Kalman Filter is derived for integrating GPS
measurements with inertial measurements from gyros and
accelerometers to determine both the position and the attitude of
a moving vehicle. The formulation of filter is based on
linearization of Extended Kalman filter (EKF). The EKF is
specifically designed for implementation as an application on a
present day smartphone. A prototype is developed and is tested
with real-time sensor data collected from an android smartphone.
Filter analysis and Simulation results are presented to compare
the performance of EKF with the standard GPS measurements
for position coordinates of a vehicle.
Keywords
GPS/INS Fusion, Kalman Filter, Nonlinear filter, Multi Sensor
fusion, Smart Phone Applications
1. INTRODUCTION
Many applications like autonomous land vehicles, aircrafts etc.,
require continuous and precise positioning information. Positon
information is given by Global positioning system (GPS).
However due to multipath, mask effects and other environmental
effects on measuring system, the GPS data can be erroneous over
short periods of time.
One approach to give a seamless positioning information on a
map is to use Kalman filter smoothing of the collected GPS data;
this eliminates sudden erroneous shifts/jumps in the location
information. To further improve the estimation of positon of an
object, the GPS data can be fused with information from other
class of sensors called Inertial Navigation system (INS).
Gyroscopes, accelerometers fall under INS. The downside of INS
subsystem is that the sensor measurements drift over time due to
bias errors.
GPS/INS integration has proven to be a very efficient means of
navigation due to the short term accuracy of the INS allied to the
long term accuracy of the GPS fixes.
2. PROBLEM FORMULATION
2.1 Kalman Filter
The Kalman filter is a set of mathematical equations that
provides a recursive means to estimate the state of a process, in a
way that minimizes the mean of the squared error. The state of
process in being position, velocity and attitude of a moving
vehicle.
A linear Kalman filter exploits both Markov structure of the
model and its linear Gaussian property, which allows for
conditional densities to be finitely parameterized by their mean
and covariance matrix. Hence Kalman filter is described in
terms of mean Xt (mean of conditional density turns out to be the
state of system) and covariance Pt/t.
In simple form, a Kalman Filter may be described in two steps.
1. Time update and 2. Measurement update. Let xt denote the
state of a process and Pt denote the Covariance matrix of a state
at time t. Now Kalman Filter recursion at time t+1 is given as
shown in figure below.
{ x1 / 0 , P1 / 0 }
{ x t / t 1 , Pt / t 1 }
M e a su re m en t U p d a te
{ x t / t , Pt / t }
T im e U p d a te
{xt 1/ t , Pt 1/ t }
xt 1/ t
xt / t
Pt 1/ t
Pt / t
t H t Pt / t 1 H tT Rt
b.
K t Pt / t 1 H tT t1
c.
When the observed system is non-linear and the noises added are
non-Gaussian, these equations for linear Kalman filter do not
hold good. Instead of propagating mean Xt (the state) and
covariance matrix Pt/t in each recursion of the filter, for nonlinear case, we need to propagate the conditional densities of the
state given past observations in each recursion. This is not a
realistic approach as we can't compute conditional densities in
real time. Hence various techniques which approximate the
above process are investigated by researchers in the past.
The first one is Extended Kalman filtering (EKF) which
linearizes the state dynamics of the system about current estimate
and then apply Kalman filter. In navigation problem, this
procedure works only when deviations of the actual trajectory
compared to the nominal trajectory is small.
More recently, other approximation techniques called unscented
Kalman filter (UKF) and particle Kalman filters were proposed.
The particle filters are based on Monte-Carlo methods to
evaluate the conditional mean and error variances for the
densities of the state.
In this work, a 1st order EKF approach is used for blending GPS
and INS measurements. There are many approaches to design an
EKF though. One aspect involves how observations of GPS are
used in the filter design. The term loosely coupled is used to
signify that latitude, longitude and altitude are used in the filter.
On the other hand, tightly coupled means pseudo ranges of
satellites are used in filter design, which is shown to give better
position estimates. But the satellite pseudo range distances are
not readily available in systems like a smartphone. Android
platform doesnt expose such information to the user. Hence a
loosely coupled approach is followed for EKF design.
d.
3. MODELING
3.1 Reference Frames
In this section, the reference frames used to derive GPS/INS
equations are introduced [2].
NED frame: North-East-Down frame is used for local navigation
purposes.
ECEF frame: Earth-Centered Earth-Fixed frame is shown in
Figure-2. This frame is used as an intermediate step in
conversion from GPS sensor measurements to NED frame.
Body frame (b): This is the coordinate frame of IMU and it is
fixed onto smartphone and rotates with it. All the inertial
measurements are resolved in this frame.
NED decouples the unstable vertical axis measurements from the
more stable horizontal axes and hence provides better EKF
performance. Hence, NED frame is chosen to implement EKF.
All measurements of GPS and INS are converted to NED and
EKF is run in this frame.
Time Update
a.
b.
c.
2.
Measurement update
a.
Compute Innovation
Since the position and velocity states are given in the same
coordinate frame, their relation is given as
The GPS and INS subsystem have different sampling rates. INS
sensors give measurements at higher sampling rates ~10 to 50Hz.
The highest sampling rate of GPS is 1Hz, an order lower, then
the INS sampling rate. If INS measurements are also taken at
slower rate, then the filters accuracy reduces.
In this work, the Kalman filter is modified to take advantage of
faster sampling rates of INS. The IMU measurements and GPS
measurements are purposely separated in two different vectors in
0state model. This facilitates running time-update independent of
GPS measurements. Hence it is run N times in each update loop,
where N is given as
N= floor(fINS/fGPS)
The IMU measurements of roll, pitch and yaw rates are related to
Euler angles by inverted kinematic equations as shown below.
{ xt / t 1 , Pt / t 1 }
Measurement Update
N times
{ xt / t , Pt / t }
Time Update
x F {x1/1 , x2 / 2 , x3 / 3 ,.......}T
x (t 1) x(t ) Ts . f c {x (t ), u (t )}
3.4 Simulation
Hence, the state and measurement models for the filter are
specified.
more trust
In the above map, filtered position values follow GPS values
closely. Hence, Kalman filtering doesnt offer a significant
improvement.
Next, noise covariances are weighted as Q = Q and R=1.25*R.
This implies giving more trust to INS measurements.
5. CONCLUSIONS
In this report, the design and implementation details of a multi
sensor fusion filter are presented. The EKF is specifically
designed for GPS and INS measurement fusion on a smartphone.
Modifications of EKF are proposed to suit the sensor sampling
rates available on the device. Weighted Kalman filtering is
explored to gain insight into the effect of weight of each sensor
measurement. The filter is tested for various possible scenarios
and the filtered outputs show better position information than
true GPS measurements. Experiments and results are presented.
If the true location coordinates values were available for a
given path, the author believes that it the filter could be trained
to give a better performance. It would also serve as a better
comparison metric than measured GPS values which are
inherently noisy. But the author couldnt find a way to get true
location coordinates for a given path at required sampling rate.
6. REFERENCES
[1] Introduction to Kalman filter. DOI=
http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf