Sensor Fusion
Content
GPS/INS Sensor Fusion with Extended-Kalman Filtering
Keerthana Atchutuni
Electrical and
[email protected]
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.
intensive or not compatible with the measurement model of
sensors in a smartphone. Many papers present the EKF algorithm
with satellite pseudo ranges as input, such models are not
suitable for the present work; discussed in section 2.1.1. Others
have nonlinear models in both state equation and measurement
equation, which leads to additional matrix multiplications in the
measurement update. The proposed algorithm is linear in
measurement model. Hence it is fast, and more importantly is
simple to implement in a real-time system. These claims will be
further explained in section 3.
In the section 2, a brief introduction to Kalman Filter and EKF
are given. The reader can refer to [1] for further details. Section
3 presents the design of EKF proposed in this paper. It also gives
prototype implementation details. In section 4, test scenarios and
results are discussed. Section 5 draws conclusions.
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 }
Historically, INS’s were primarily used for military and
commercial aircraft applications due to their high cost. However,
with the advent of cheaper sensors, especially micro-mechanical
ones, several new applications have become mainstream,
including navigation on smartphones. Although the relatively low
cost sensors on smartphones do not perform as well in terms of
drift and white noise errors, they offer an attractive approach to
the problem of position and attitude estimation of a smartphone
with the aid of GPS, in real-time.
The main contribution of the work presented in this paper is to
design a non-linear Kalman filter specifically tailored to
implement as a smartphone application. Existing state space
models presented in literature were found to be computationally
{ 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 }
Figure 1: Discrete Kalman Filter Cycle.
Where
xt 1/ t
: Prediction of state given past state values
xt / t
Pt 1/ t
: Filtered state vector
: Predicted Error covariance given the past state values
Pt / t
: Filtered Error Covariance
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 doesn’t expose such information to the user. Hence a
‘loosely coupled’ approach is followed for EKF design.
Update estimate with measurement yk
xt / t xt / t 1 Kt ( yt H .xt / t 1 )
2.1.1 Non Linear Kalman Filtering
GPS/INS data fusion is a non-linear filtering problem in which
the sensor data is corrupted by non-Gaussian noise. The
observation and state model is assumed to be a discrete time
Markov process.
Compute Kalman gain
d.
Update Covariance matrix
Pt / t ( I K t H ) Pt / t 1
The above version of EKF is implemented in MATLAB
simulation for the work presented in this paper.
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.
2.1.2 EKF recursions
For a state vector xt , and let f(.) denote the non-linear function
in process. The recursion equations for EKF are given as follows.
1.
Time Update
a.
Project the state ahead
xt / t 1 f ( xt 1 / t 1 , ut )
b.
Compute the Jacobean Matrix Ft
c.
Project the covariance matrix ahead
Pt / t 1 Ft Pt 1 / t 1 FtT Qt 1
2.
Figure 2: Definition of various reference frames [2]
Measurement update
a.
Compute Innovation
3.2 State space model
The sensor fusion formulation has state vector given by the
Cartesian position, velocity and attitude of the vehicle denoted by
Euler angles. The Euler angles are chosen for state vector instead
of quaternions to facilitate computation of Jacobean Matrix Ft.
State Vector : x 9 x1 {rx , ry , rz , v x , v y , v z , , , }T
Since the position and velocity states are given in the same
coordinate frame, their relation is given as
3.3 Modified Kalman Filter
The IMU provides measurements of linear acceleration and
angular rates given as follows.
Accelerometer {a x , a y , a z } Gyroscope { p, q, r}
,
.
These two measurements are combined to form an input vector,
u 6x1 {a x , a y , az , p, q, r}T
The acceleration measurements are in mobile body frame. The
rotation matrix from body frame to navigation frame, defines the
relation between velocity and attitude states as below.
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 filter’s 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 acceleration due to gravity ‘g’ is removed from the IMU
measurements, since these are relative to free fall.
Where fINS and fGPS are sampling frequencies of respective
measurements.
{x1 / 0 , P1/ 0 }
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
{xt 1/t , Pt 1/t }
Together, the above three matrices define the non-linear function
f(.) in the process equation.
Now, we need to linearize the state equation about t, in order to
implement a Kalman Filter. In 1st order EKF, this is done by
Taylor approximation given as follows.
x F {x1/1 , x2 / 2 , x3 / 3 ,.......}T
Figure 3. Modified Kalman Filter
x (t 1) x(t ) Ts . f c {x (t ), u (t )}
Where Ts is the sampling time of IMU.
3.4 Simulation
The Process noise and Measurement noise equations as given by
3.4.1 Data Acquisition
Q diag {var( ax ), var( a y ), var( a z ), var( p ), var( q ), var( r )}
With this, description of process model is complete.
The measurement model is specified as follows.
Measurements of GPS are collected in terms of latitude,
longitude and elevation of a vehicle. These measurements are
then converted to ECEF and NED frame using the relations
provided in [2]. These values are represented in vector y.
GPS readings : y 3x1 {rx,ry , rz }T : Measuremen t Vector
The measurement equation is given as follows.
y (t ) H.x(t ) v(t )
Due to the simplicity of measurement equation, the matrix
H = [I3x3 06x6]. The main advantage of such a measurement
equation is reduction in computation complexity of the system.
Essentially, as measurement equation is linear, the linear
Kalman filter measurement update suffices. The requirement to
linearize the equation in EKF is thus avoided. The measurement
noise matrix is computed as
R diag{var(rxgps ), var( rygps ), var(rzgps ))}
Hence, the state and measurement models for the filter are
specified.
IMU data is collected from an android application on a Galaxy
S5 Mobile equipped with an MPU6500 accelerometer and
gyroscope system. The IMU measurements are taken at 10Hz for
most test scenarios discussed in section 4.
For bias calibration of IMU, a set of measurements are taken
with the device at rest and placing it in suggested north-east
direction. Average values of this record are compensated for in
all further measurements.
GPS data is also collected with the same device along with IMU
measurements, simultaneously. When recording GPS data, the
mobile connectivity to tower is disconnected to ensure that
readings are taken only from the sensor present in smartphone.
Also tested by coupling the measurements from gravity sensor
present in the mobile but that offered no significant effect.
3.4.2 Software Implementation
To begin with, the initial state vector is given as xt = zero vector
and initial covariance matrix is taken as Pt/t= 10*I. where I
denotes a 9x9 identity matrix.
The measurements collected from smartphone are fed to a
Matlab simulation. The EKF is implemented in Matlab. The
filtered values represent device position estimate in NED
reference frame. These are in-turn converted to geodetic frame to
trace the route in a map.
3.4.3 Design challenges
In this section, the challenges encountered in design of EKF for
implementation on smartphones.
Average values of {p,q,r} = { 0.00019, -0.00659, 0.003035 }
Average values of {ax,ay,az} = {-0.02805, 0.170222, 9.477769}
Euler angles are chosen in state equation instead of
quaternions to facilitate the computation of Jacobean
matrix.
As observed, the bias values of both accelerometer and gyroscope
are rather small.
To match the sampling frequencies of GPS and IMU,
the Kalman filter is modified as in section 3.3
4.2 Test Scenario – 1
Measurement model is simplified to reduce
computation complexity and tailor the filter to real time
systems.
Debugging and Figuring out effects of weightage for
GPS and INS measurements on the fused
measurements
The IMU measurements for a test route-1 are shown below.
A mapping software called ‘GPS Visualizer’ is used to plot the
maps. The measurements were taken while driving car at about
10-15m/hr in the below route shown.
Now the Noise covariance’s are weighted as Q’ = 1.25*Q and
R’=R. This implies giving more trust to GPS data. The filtered
values closely follow GPS measurements.
3.5 Weighted Kalman Filter
Let x1 and x2 denote the measurements from GPS and INS unit
with variances
and . One way of obtaining a combined
measurement is to apply Central Limit Theorem to obtain fused
measurement by weighting x1 and x2 by their Noise variances
where
. Similarly, the GPS
and INS measurements can also be weighted by multiplying their
Noise variance matrices Q and R by a constant factor. An
experiment showing the results for one track are shown in Test
case -1. The motivation to do the weighted Kalman filter is
derived from [4]. The aim of the author is to simply observe the
effect of giving more trust to one sensor system than the other
rather than giving an extended study and analysis as in [4]. The
effects can also be observed by simply adjusting the noise
variances manually as described above.
Figure 4: True GPS and Filtered measurements with GPS given
4. TEST SCENARIOS AND RESULTS
4.1 Calibration of IMU
With device at initial rest, IMU measurements are taken as
follows for 15sec. Average values of each are computed and
compensated for in further measurements.
more trust
In the above map, filtered position values follow GPS values
closely. Hence, Kalman filtering doesn’t offer a significant
improvement.
Next, noise covariance’s are weighted as Q’ = Q and R’=1.25*R.
This implies giving more trust to INS measurements.
Figure 5: Measured GPS and Filtered GPS with INS given
more trust
With more weightage given to INS data, the E-Kalman filter
gives a clearly better position estimation. But when acceleration
and orientation values change rapidly, like while taking turns,
the filtered data is off from the GPS measurements.
4.3 Test Scenario – 2
The measurements were taken while driving car at about 30m/hr
in the below routes shown. Now the Noise covariance’s are
weighted as Q’ = 1.2*Q and R’=R. This implies giving more
trust to GPS data. The filtered data is shown against GPS
measurement data which is considered as true GPS data.
In Figures 6 & 7, it can be seen that the filtered data is smooth
when GPS measurements deviate and give erroneous path. The
improvement is shown in bottom part of the figure with more
clarity.
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 couldn’t 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
Figure 6: Measured GPS and Filtered GPS for a faster moving
vehicle
[2] John L. Crassidis. Sigma-Point Kalman Filtering for
Integrated GPS and Inertial Navigation.
http://www.acsu.buffalo.edu/~johnc/gpsins_gnc05.pdf
[3] Jeroen D. Hol, Thomas B. Schn, Fredrik Gustafsson,
Modeling and Calibration of Inertial and Vision Sensors,
International Journal of Robotic Research, 29(2-3), pp. 231244, 2010
http://user.it.uu.se/~thosc112/pubpdf/holsg2010.pdf
[4] Francois Caron, Emmanuel Duflos, Denis Pomorski,
Philippe Vanheeghe ‘GPS/IMU Data Fusion using
Multisensor Kalman Filtering : Introduction of Contextual
Aspects’ 2004
http://www.stats.ox.ac.uk/~caron/Publications/J_Informatio
n_Fusion_2004.pdf
[5] David McNeil Mayhew, ‘Multi-rate Sensor Fusion for GPS
Navigation Using Kalman Filtering’ Master’s Thesis.
http://scholar.lib.vt.edu/theses/available/etd-062899064821/unrestricted/etd.PDF
Figure 7: Measured GPS and Filtered GPS data for a
faster moving vehicle
Keerthana Atchutuni
Electrical and
[email protected]
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.
intensive or not compatible with the measurement model of
sensors in a smartphone. Many papers present the EKF algorithm
with satellite pseudo ranges as input, such models are not
suitable for the present work; discussed in section 2.1.1. Others
have nonlinear models in both state equation and measurement
equation, which leads to additional matrix multiplications in the
measurement update. The proposed algorithm is linear in
measurement model. Hence it is fast, and more importantly is
simple to implement in a real-time system. These claims will be
further explained in section 3.
In the section 2, a brief introduction to Kalman Filter and EKF
are given. The reader can refer to [1] for further details. Section
3 presents the design of EKF proposed in this paper. It also gives
prototype implementation details. In section 4, test scenarios and
results are discussed. Section 5 draws conclusions.
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 }
Historically, INS’s were primarily used for military and
commercial aircraft applications due to their high cost. However,
with the advent of cheaper sensors, especially micro-mechanical
ones, several new applications have become mainstream,
including navigation on smartphones. Although the relatively low
cost sensors on smartphones do not perform as well in terms of
drift and white noise errors, they offer an attractive approach to
the problem of position and attitude estimation of a smartphone
with the aid of GPS, in real-time.
The main contribution of the work presented in this paper is to
design a non-linear Kalman filter specifically tailored to
implement as a smartphone application. Existing state space
models presented in literature were found to be computationally
{ 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 }
Figure 1: Discrete Kalman Filter Cycle.
Where
xt 1/ t
: Prediction of state given past state values
xt / t
Pt 1/ t
: Filtered state vector
: Predicted Error covariance given the past state values
Pt / t
: Filtered Error Covariance
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 doesn’t expose such information to the user. Hence a
‘loosely coupled’ approach is followed for EKF design.
Update estimate with measurement yk
xt / t xt / t 1 Kt ( yt H .xt / t 1 )
2.1.1 Non Linear Kalman Filtering
GPS/INS data fusion is a non-linear filtering problem in which
the sensor data is corrupted by non-Gaussian noise. The
observation and state model is assumed to be a discrete time
Markov process.
Compute Kalman gain
d.
Update Covariance matrix
Pt / t ( I K t H ) Pt / t 1
The above version of EKF is implemented in MATLAB
simulation for the work presented in this paper.
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.
2.1.2 EKF recursions
For a state vector xt , and let f(.) denote the non-linear function
in process. The recursion equations for EKF are given as follows.
1.
Time Update
a.
Project the state ahead
xt / t 1 f ( xt 1 / t 1 , ut )
b.
Compute the Jacobean Matrix Ft
c.
Project the covariance matrix ahead
Pt / t 1 Ft Pt 1 / t 1 FtT Qt 1
2.
Figure 2: Definition of various reference frames [2]
Measurement update
a.
Compute Innovation
3.2 State space model
The sensor fusion formulation has state vector given by the
Cartesian position, velocity and attitude of the vehicle denoted by
Euler angles. The Euler angles are chosen for state vector instead
of quaternions to facilitate computation of Jacobean Matrix Ft.
State Vector : x 9 x1 {rx , ry , rz , v x , v y , v z , , , }T
Since the position and velocity states are given in the same
coordinate frame, their relation is given as
3.3 Modified Kalman Filter
The IMU provides measurements of linear acceleration and
angular rates given as follows.
Accelerometer {a x , a y , a z } Gyroscope { p, q, r}
,
.
These two measurements are combined to form an input vector,
u 6x1 {a x , a y , az , p, q, r}T
The acceleration measurements are in mobile body frame. The
rotation matrix from body frame to navigation frame, defines the
relation between velocity and attitude states as below.
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 filter’s 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 acceleration due to gravity ‘g’ is removed from the IMU
measurements, since these are relative to free fall.
Where fINS and fGPS are sampling frequencies of respective
measurements.
{x1 / 0 , P1/ 0 }
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
{xt 1/t , Pt 1/t }
Together, the above three matrices define the non-linear function
f(.) in the process equation.
Now, we need to linearize the state equation about t, in order to
implement a Kalman Filter. In 1st order EKF, this is done by
Taylor approximation given as follows.
x F {x1/1 , x2 / 2 , x3 / 3 ,.......}T
Figure 3. Modified Kalman Filter
x (t 1) x(t ) Ts . f c {x (t ), u (t )}
Where Ts is the sampling time of IMU.
3.4 Simulation
The Process noise and Measurement noise equations as given by
3.4.1 Data Acquisition
Q diag {var( ax ), var( a y ), var( a z ), var( p ), var( q ), var( r )}
With this, description of process model is complete.
The measurement model is specified as follows.
Measurements of GPS are collected in terms of latitude,
longitude and elevation of a vehicle. These measurements are
then converted to ECEF and NED frame using the relations
provided in [2]. These values are represented in vector y.
GPS readings : y 3x1 {rx,ry , rz }T : Measuremen t Vector
The measurement equation is given as follows.
y (t ) H.x(t ) v(t )
Due to the simplicity of measurement equation, the matrix
H = [I3x3 06x6]. The main advantage of such a measurement
equation is reduction in computation complexity of the system.
Essentially, as measurement equation is linear, the linear
Kalman filter measurement update suffices. The requirement to
linearize the equation in EKF is thus avoided. The measurement
noise matrix is computed as
R diag{var(rxgps ), var( rygps ), var(rzgps ))}
Hence, the state and measurement models for the filter are
specified.
IMU data is collected from an android application on a Galaxy
S5 Mobile equipped with an MPU6500 accelerometer and
gyroscope system. The IMU measurements are taken at 10Hz for
most test scenarios discussed in section 4.
For bias calibration of IMU, a set of measurements are taken
with the device at rest and placing it in suggested north-east
direction. Average values of this record are compensated for in
all further measurements.
GPS data is also collected with the same device along with IMU
measurements, simultaneously. When recording GPS data, the
mobile connectivity to tower is disconnected to ensure that
readings are taken only from the sensor present in smartphone.
Also tested by coupling the measurements from gravity sensor
present in the mobile but that offered no significant effect.
3.4.2 Software Implementation
To begin with, the initial state vector is given as xt = zero vector
and initial covariance matrix is taken as Pt/t= 10*I. where I
denotes a 9x9 identity matrix.
The measurements collected from smartphone are fed to a
Matlab simulation. The EKF is implemented in Matlab. The
filtered values represent device position estimate in NED
reference frame. These are in-turn converted to geodetic frame to
trace the route in a map.
3.4.3 Design challenges
In this section, the challenges encountered in design of EKF for
implementation on smartphones.
Average values of {p,q,r} = { 0.00019, -0.00659, 0.003035 }
Average values of {ax,ay,az} = {-0.02805, 0.170222, 9.477769}
Euler angles are chosen in state equation instead of
quaternions to facilitate the computation of Jacobean
matrix.
As observed, the bias values of both accelerometer and gyroscope
are rather small.
To match the sampling frequencies of GPS and IMU,
the Kalman filter is modified as in section 3.3
4.2 Test Scenario – 1
Measurement model is simplified to reduce
computation complexity and tailor the filter to real time
systems.
Debugging and Figuring out effects of weightage for
GPS and INS measurements on the fused
measurements
The IMU measurements for a test route-1 are shown below.
A mapping software called ‘GPS Visualizer’ is used to plot the
maps. The measurements were taken while driving car at about
10-15m/hr in the below route shown.
Now the Noise covariance’s are weighted as Q’ = 1.25*Q and
R’=R. This implies giving more trust to GPS data. The filtered
values closely follow GPS measurements.
3.5 Weighted Kalman Filter
Let x1 and x2 denote the measurements from GPS and INS unit
with variances
and . One way of obtaining a combined
measurement is to apply Central Limit Theorem to obtain fused
measurement by weighting x1 and x2 by their Noise variances
where
. Similarly, the GPS
and INS measurements can also be weighted by multiplying their
Noise variance matrices Q and R by a constant factor. An
experiment showing the results for one track are shown in Test
case -1. The motivation to do the weighted Kalman filter is
derived from [4]. The aim of the author is to simply observe the
effect of giving more trust to one sensor system than the other
rather than giving an extended study and analysis as in [4]. The
effects can also be observed by simply adjusting the noise
variances manually as described above.
Figure 4: True GPS and Filtered measurements with GPS given
4. TEST SCENARIOS AND RESULTS
4.1 Calibration of IMU
With device at initial rest, IMU measurements are taken as
follows for 15sec. Average values of each are computed and
compensated for in further measurements.
more trust
In the above map, filtered position values follow GPS values
closely. Hence, Kalman filtering doesn’t offer a significant
improvement.
Next, noise covariance’s are weighted as Q’ = Q and R’=1.25*R.
This implies giving more trust to INS measurements.
Figure 5: Measured GPS and Filtered GPS with INS given
more trust
With more weightage given to INS data, the E-Kalman filter
gives a clearly better position estimation. But when acceleration
and orientation values change rapidly, like while taking turns,
the filtered data is off from the GPS measurements.
4.3 Test Scenario – 2
The measurements were taken while driving car at about 30m/hr
in the below routes shown. Now the Noise covariance’s are
weighted as Q’ = 1.2*Q and R’=R. This implies giving more
trust to GPS data. The filtered data is shown against GPS
measurement data which is considered as true GPS data.
In Figures 6 & 7, it can be seen that the filtered data is smooth
when GPS measurements deviate and give erroneous path. The
improvement is shown in bottom part of the figure with more
clarity.
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 couldn’t 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
Figure 6: Measured GPS and Filtered GPS for a faster moving
vehicle
[2] John L. Crassidis. Sigma-Point Kalman Filtering for
Integrated GPS and Inertial Navigation.
http://www.acsu.buffalo.edu/~johnc/gpsins_gnc05.pdf
[3] Jeroen D. Hol, Thomas B. Schn, Fredrik Gustafsson,
Modeling and Calibration of Inertial and Vision Sensors,
International Journal of Robotic Research, 29(2-3), pp. 231244, 2010
http://user.it.uu.se/~thosc112/pubpdf/holsg2010.pdf
[4] Francois Caron, Emmanuel Duflos, Denis Pomorski,
Philippe Vanheeghe ‘GPS/IMU Data Fusion using
Multisensor Kalman Filtering : Introduction of Contextual
Aspects’ 2004
http://www.stats.ox.ac.uk/~caron/Publications/J_Informatio
n_Fusion_2004.pdf
[5] David McNeil Mayhew, ‘Multi-rate Sensor Fusion for GPS
Navigation Using Kalman Filtering’ Master’s Thesis.
http://scholar.lib.vt.edu/theses/available/etd-062899064821/unrestricted/etd.PDF
Figure 7: Measured GPS and Filtered GPS data for a
faster moving vehicle
