2013 International Conference on Unmanned Aircraft Systems (ICUAS)
May 28-31, 2013, Grand Hyatt Atlanta, Atlanta, GA
ESTIMATING VEHICLE STATE BY GPS/IMU FUSION WITH VEHICLE
DYNAMICS
Kamal Saadeddin
Mechatronics Engineering Graduate Program
American University of Sharjah
Sharjah, UAE
b00024496@aus.edu
Mohammad Amin Jarrah
Department of Mechanical Engineering
American University of Sharjah
Sharjah, UAE
mjarrah@aus.edu
Abstract— In this paper, a low-cost navigation system with
high integrity and reliability is proposed. A high-integrity
estimation filter is proposed to obtain a high accuracy state
estimate. The filter utilizes a vehicle velocity constraint
measurement to enhance the accuracy of the estimate. Two
estimations filters, the extended Kalman filter (EKF) and the
extended information filter (EIF), are designed and compared
to obtain the estimate of the vehicle state. An instrumentation
system that consists of a microcontroller, a GPS receiver, an
IMU, velocity encoder, and a Zigbee transceiver is constructed.
The microcontroller provides a vehicle navigation solution at 50
Hz by fusing the measurements of the IMU, the GPS receiver
and the digital compass using the proposed filter design.
Extensive experimental tests are conducted to verify the
accuracy of the proposed algorithm. These results are processed
with and without the velocity constraints. The estimation
accuracy improvement with the addition of the velocity
constraints is demonstrated. More than 16% reduction in the
computational time is demonstrated when using the EIF in
comparison to the EKF approach.
Keywords; GPS; IMU; State Estimation; Velocity Constraint
I. INTRODUCTION
High-accuracy and low-cost estimation of vehicle’s state
is vital for autonomous navigation and control applications.
If the vehicle state is estimated accurately with low
computational demand, then the vehicle can be guided along
the required trajectory. Also, if more than one vehicle is
assigned a certain task, then the accurate knowledge of
relative vehicle’s state will ensure the safe operation of the
overall system.
In this paper, a navigation system that calculates the state
of each vehicle at 50 Hz rate is proposed. Using Zigbee
transceivers, this state is continuously transmitted to
neighboring vehicles. In the proposed navigation system, a
low-cost GPS receiver measures the position and velocity of
the vehicle at a frequency of 5 Hz. Three-axis accelerometers
and gyroscopes measure the linear acceleration and angular
velocity of the vehicle at 50 Hz. Also, velocity encoders are
978-1-4799-0817-2/13/$31.00 ©2013 IEEE 905
Mamoun F. Abdel-Hafez
Department of Mechanical Engineering
American University of Sharjah
Sharjah, UAE
mabdelhafez@aus.edu
used to measure forward velocity of the vehicle. The velocity
measurement can be obtained from the airspeed
measurement of an Unmanned Aircraft System (UAS) or
from wheel encoders for ground vehicles.
Using measurements from the GPS receiver and the
forward velocity constraint, the three-axis acceleration and
angular velocity measurements are integrated to obtain the
position and velocity of the vehicle at 50 Hz [1-5]. The
position and velocity along with the identity code of the
vehicle (ID) are then transmitted to neighboring vehicles
using a Zigbee transceiver [6-8], see Figure 1.
There have been a number of studies pursued to reduce
vehicles’ accidents using different approaches and navigation
algorithms. The use of encoders, GPS receivers, along with a
digital map to estimate ground vehicle’s state is proposed in
[9]. However, such a system would is applicable to ground
vehicles only, expensive to implement, and may not be
applicable if a precise digital map is not available. Some
researchers suggest using radar in addition to vision based
systems to predict traffic accidents [10]. However, unlike the
proposed solution, using a radio communication system,
these systems do not provide a 360 degree field of view.
In reference [11], the implementation of vehicle to
infrastructure communication is suggested. This approach is
suitable for applications aided with real-time traffic data or
internet connectivity. However, collision prediction
communication is only required between vehicles present
within the vicinity of each other. Vehicle to Vehicle
communication has the advantage of not requiring an
infrastructure. So, it can work in any area where all vehicles
are provided with a collision avoidance system along with a
compatible communication protocol. Different issues faced
with the implementation of this system are discussed in [12].
In reference [13], a solution is proposed using
measurements from a gyroscope, GPS receiver and a
speedometer. In the solution proposed in [13], a nonlinear
observer which consists of three subfilters is used in
developing a vehicle navigation solution. One of the
subfilters has two Kalman gains and estimates yaw angle and
gyro bias. Another subfilter is responsible for estimating the
speedometer’s scaling. The third subfilter estimates the
vehicle’s velocity and position. In the filtering stage
proposed in [13], nonholomonic constraints are taken into
consideration. When tested, the algorithm gave reliable
results. However, the algorithm does not take into
consideration the accelerometer bias and it also does not use
quaternion approach which is known for its stability in
finding a vehicle’s attitude.
Constraints are also fused in a navigation solution in
reference [14]. These constraints depend on the orientation of
the vehicle relative to the earth and the relationship between
the attitude of the vehicle with its velocity. Velocity
constraints are also fused with an INS and GPS in reference
[15] for a navigation solution consisting of nine states. The
results of this fusion of the velocity constraints along with
the vehicle’s speed showed that the observability of the
attitude of the vehicle is guaranteed. This leads to a decrease
in the attitude’s estimation errors.
In this work, vehicle’s velocity constraints are fused
with INS/GPS measurements to enhance the accuracy of
state estimation. In addition to the extended Kalman filter,
an extended information filter is used to fuse the
measurements to reduce the computational complexity of
the system. The EIF estimation accuracy is compared to that
of the EKF approach to ensure that the state estimate’s
accuracy is not substantially affected. First, the suggested
system’s setup is described. The fusion algorithm of the
low-cost GPS, velocity constraints, and three-axis
gyroscopes and accelerometers is shown next. Experimental
results of the estimation algorithm are verified and
compared with a standalone commercial solution. Single as
well as two-vehicle experimental results are shown in
section VI.
In the next section, the proposed system’s setup is first
illustrated.
II. SYSTEM SETUP
Figure 1 shows the components of the proposed system.
The system is composed of an IMU/GPS unit, a velocity
encoder, a Zigbee transceiver, and a microcontroller. The
measurements of the IMU and GPS are fused to obtain an
estimate of the vehicle state. The body frame velocity
constraint measurement, which is obtained from the airspeed
velocity measurement of an UAS or from wheel encoders for
ground vehicles, is fused with the GPS/IMU measurements
to enhance the accuracy of the state estimate. The
microcontroller estimates the state of each vehicle at 50 Hz
rate using the algorithm proposed in this paper. Then, the
Zigbee transceiver is responsible for communicating the
position, velocity, attitude and vehicle ID to other vehicles in
the vicinity through a network of Zigbee routers.
For development purposes, PC104 computer is used whereas
for production, use of a low cost microcontroller such as PIC
is proposed. The microcontroller is capable of interfacing
with a ZigBee communication module, 3-axis accelerometer,
906
3-axis gyroscope, encoder (or odometer) and a GPS unit.
Figure 1 shows the input and output ports of the PC104
microcontroller.
Figure 1: Overall System Architecture
To estimate the position and velocity of the vehicle, GPS
measurements and velocity constraints are fused with
inertial sensors. Measurements from a three-axis
accelerometer and a three-axis gyroscope are integrated to
find position, velocity and heading of the vehicle. However,
due to errors accumulation in the INS estimate, the results
soon drift away from their actual values. Therefore, GPS
measurements are fused with the INS measurements using
the EKF to obtained a high-accuracy estimate of the state. In
addition, to reduce the computational demand of the
algorithm, an EIF is used instead of the EKF for sensor
fusion. To enhance the accuracy of the state estimation,
velocity constraints are also fused with the GPS/IMU
measurements. These constraints improve the results by
constraining the velocities in two axes, y and z, in the body
frame to zero mean with a small magnitude white noise
process. The white noise process modeled on the body y and
z axes correct for any possible, small magnitude, side slip of
the UAS or ground vehicle. The next section of the paper
presents the algorithms used in the proposed solution.
III. INERTIAL NAVIGATION SYSTEM (INS)
Using accelerometers and gyroscopes to find velocity and
position is illustrated in [1-5]. The accelerometer measures
the vehicle’s specific force vector, , in the body frame.
The gyroscope provides the vehicle’s angular velocity
vector, , in the body frame. Here, the superscript b is used
to denote the vectors’ representation in the body frame. The
encoder measures the body frame velocity in the x direction.
Also, body frame velocities in y and z directions are
constrained to zero mean with a white Gaussian noise. These
constraints are known as nonholomonic velocity constraints.
Before fusing them with the GPS measurements, the
measurements are rotated to the earth centered, earth fixed
(ECEF) frame using the transformation matrix . In the
sections below, the dynamics of the vehicle are described. In
the equations to follow, Ω represents the skew matrix of
angular velocity of earth, P is the vehicle’s position, V is the
vehicle’s velocity, and G is local gravitation vector. The
superscript e is used to denote the vectors’ representation in
the ECEF frame.
A. Velocity and Position
The vehicle’s dynamics are obtained from the
measurements of accelerometers and gyroscopes. The time
rate of change of the velocity with respect to the ECEF represented in the body frame. The quaternions are updated
frame is given as [5]: as follows.

= − 2ΩV − Ω + (1) − − − 1
−
= ⊕ = (8)
To propagate the velocity in the ECEF frame, equation 1 −
is integrated numerically. Subsequently, the vehicle’s −
velocity is integrated to find its position using the equation
below. where is an estimated quaternion and is the a
priori estimate of the attitude of the vehicle.
= (2) The gyroscope provides angular velocity of the vehicle
relative to the inertial frame. The angular velocity of the
Due to the bias error in the accelerometers’ specific earth relative to the inertial frame is subtracted from the
force, , the propagated velocity and position of the angular velocity of the body frame relative to the inertial
vehicle will diverge quickly from the correct position and frame to get the angular velocity of the body relative to the
velocity. The measurements of the IMU are modeled to have ECEF frame as
the following dynamics:
= − (9)
= − − (3)
= − − (4) This equation is represented as:
= (5)
= − =[ ] (10)
= (6)
And its skew-symmetric cross matrix is represented as:
where are the measured vehicle specific force in the
body frame, is the measured angular velocity of the 0 − − −
vehicle relative to the inertial frame measured in the body
frame of the vehicle, is the bias in the specific force, 0 −
Ω = (11)
is the bias in the angular velocity, is the noise on the − 0
specific force measurement, is the noise on the angular − 0
velocity measurement, is a noise modeling the drift in
the bias of specific force, and finally is a noise Figure 2 illustrates the role of the INS in obtaining the
modeling the drift in the bias of the angular velocity position, velocity, and attitude of the vehicle by integration
measurement. the dynamics measured by the IMU. The fusion of the
velocity constraint and the GPS measurements with the a
Next, the attitude representation of the vehicle is priori vehicle state obtained by the INS is shown in the
discussed. bottom of the figure. The fusion algorithms are described in
section IV. First, the velocity constraint measurement is
B. Attitude derived in next section.
The attitude of the vehicle is represented in terms of
quaternions. In [16], the quaternion algorithm is used to
avoid uncertainty in the initial attitude. This algorithm is
computed using the computer frame. Also, for fusion of
nonlinear data, the distribution approximation filter (DAF)
is used in [17]. The DAF has an advantage of avoiding the
complications that are usually accompanied with calculating
Jacobians in the EKF algorithm. It does this by
approximating a distribution rather than approximating an
arbitrary nonlinear function, as is the case in EKF. More is
explained about DAF algorithm in [18].
The vehicle attitude is therefore represented in the
[
quaternion vector as: q = q0 q1 q2 q3 . The time ]T
rate of change of the attitude of the vehicle is given in
equation 7 as:

= Ω (7)
Figure 2: Microcontroller inputs and outputs
b
where Ω is skew symmetric matrix of the angular
eb
velocity of the body frame relative to the ECEF frame
907
C. Velocity Constraint ( )− ( )=
The body-frame velocity in the y and z axes, and , V − 2 [ ]× + (19)
are constrained to zero mean with an additional, small
magnitude, white noise. The white noise processes on the x Therefore, the velocity constraint measurement matrix is
and y axes compensate for any small-magnitude velocities given as:
that the vehicle can have along these directions. The fusion
of these velocity constraints helps in improving the results ( )= 0 −2 [ ]× 0 0 (20)
of the estimated states. The body frame x-axis velocity
measurement is represented as: The covariance of the velocity constraint measurement
( ) error is represented as:
( )= 0 = V + (12) 0 0
0 ( )= 0 0 (21)
The measurement equation between the INS readings and 0 0
the velocity constraint is obtained by first evaluating the
above measurement constraint at the nominal, a priori, To eliminate the propagated bias in position, velocity
and attitude estimates, a EKF and EIF are used to fuse the
values as:
measurements of the GPS, velocity constraint, and the a
V = (13a) priori vehicle’s estimates obtained from the integration of
the gyroscopes and accelerometers. This is described in the
But the nominal states are related to the true states as next section.
follows:
IV. MEASUREMENTS FUSION ALGORITHM USING EKF
= (13b) The Kalman filter estimates of vehicle’s position,
velocity and attitude by fusing the GPS and velocity
V = − V (13c) constraint with the a priori INS propagated estimates.

Therefore, substituting equations (13a), (13b) and (13c) in The error state vector used is described as:
equation (12), we obtain:
= (22)
( )= ( − V )+ (14)
and with the error in the cosine matrix, assuming small Performing perturbation analysis using equations (1-7),
the dynamics of this error state can be obtained and
attitude error, represented as = ( − 2[ ]× ), equation represented in vector form as, see reference [3]:
(14) is written as:
= + (23)
( )= ( − 2[ ]× )( − V ) + (15)
where:
Expanding the terms in the equation (15):
0 0 0 0
( )= − V − 2 [ ]× + ̅
∇ −Ω −2Ω −2 0
2 [ ]× + (16)
= 0 0 −Ω 0 ,
The term 2 [ ]× is assumed negligible to first
0 0 0 0 0
order. Also, by substituting:
0 0 0 0 0
2 [ ]× = − 2 [ ]× 0 0 0 0
0 0 0
equation (16) can be written as: B= 0 ,
0 0
( )= − V + 2 [ ]× + 0 0 0
(17) 0 0 0
By subtracting equation (17) from equation (13a), the
following is obtained: and w = waT [ wTg T
wba T
wbg . ]
T

V − ( )= V − 2 [ ]× + The dynamic equation (23) is descritized to obtain:

(18)
with V = ( ) the constraint measurement equation ( ) = ( ) ( − 1) + ( ) ( ) (24)
is written as:

908
 Initial calibration of the IMU is performed to minimize 0 0
the magnitude of the state error in the propagation stage, see
reference [1]. At the start of operation, (1|0) is set to zero ( )= 0 0 (31c)
and initial position, velocity and attitude are obtained from 0 0
the GPS and compass. Subsequently, the dynamics equations
1-8 are integrated numerically to obtain the state of the where, in equations 31b and 31c, 0 is a 3 × 3 matrix of zeros
vehicle when GPS measurements are not available. During and is a 3 × 3 identity matrix.
this stage, the error in the state will accumulate due to the
bias error in the IMU. Also, the covariance of the propagated Once the state is updated using equation 27, the cosine
state will grow in time due to the noise driven dynamics rotation matrix is updated to obtain using the equation,
equation, see equation 24. Notice that the covariance matrix, see reference [3]:
( | − 1), is propagated in time as:
= (32a)
( | − 1) = ( ) ( − 1| − 1) ( ) + ( ) (25)
where is the error in computation and is represented,
Where Q(k) is the process noise covariance matrix at assuming small error, as
time k. Therefore, to reduce the error in the propagated state
estimate, the error measurement is used to update the state = ( − 2[ ]× ) (32b)
using the EKF. The error measurement is given as:
where is the attitude between and frame and is
( )− ( )
represented as: δq = [δq1 δq2 δq3 ]
e T
( )= ( )− ( ) (26)
( )− ( ) In the next section, the fusion of the measurements and
dynamics based on an information filter formulation is
Using an EKF the error estimate is updated every time introduced to reduce the computational requirement of the
step through the following equation. EKF formulation.

( | ) = ( | − 1) + ( ) ( ) (27) V. MEASUREMENTS FUSION ALGORITHM USING EIF

( ) is a Kalman gain matrix and ( ) is known as the Theoretically, the extended information filter should
innovation matrix. These are obtained as: give the same result as that of an extended Kalman filter.
However, it is computationally easier to apply in cases
( ) = ( | − 1) ( ) ( ) (28) where measurements from multiple sensors are fused. This
is because it uses information space rather than state space.
( ) = ( ) − ( ) ( | − 1) (29) The main components of the information filter are the
information state matrix and the information state vector.
The matrix, ( ) , which is known as the innovation The information state matrix, which is the inverse of the
covariance is propagated as: covariance matrix, is represented as:

( )= ( ) ( | − 1) ( )+ ( ) (30) ( )= ( ) (33a)

where R(k) is the covariance of the measurements noise at and the information state vector is represented as:
time k. Then, the update of the covariance matrix is
performed as: ( )= ( ) ( ) (33b)

( | ) = − ( ) ( ) ( | − 1) − The predicted information state matrix is given as:

( ) ( ) + ( ) ( ) ( ) (31a)
( | − 1) = [ ( ) ( − 1| − 1) ( ) + ( )] (34a)
where,
while the predicted information vector is evaluated a
0 0 0 0 ( | − 1) = ( − 1| − 1) ( ) ( − 1| − 1) ×
( )= 0 0 0 0 (31b) ( − 1| − 1) (34b)
0 [ ×
−2 ] 0 0 Then, the information observation vector, ( ) , that
represents the contribution of the observation, is represented
and as:

( )= ( ) ( ) ( ) (35a)

909
and the information observation matrix, ( ), that represents Attitude Accuracy (Tilt) 0.4° (1 )
the certainty, is given as: Position Accuracy 2 m CEP, WAAS/EGNOS available
Physical
( )= ( ) ( ) ( ) (35b) Size 1.50" × 0.87" × 1.67"
Weight 55 grams
The information observation vector and the information
observation matrix are evaluated when an observation ( ) First, the results of a single vehicle dynamic test is
is available. The estimates are updated as presented next.
A. Single Vehicle Dynamic Test using EKF without Velocity
( | ) = ( | − 1) + ( ) (36a) Constraints
( | ) = ( | − 1) + ( ) (36b) Figures 3 through 6, show the results of a single vehicle
dynamic test, where the vehicle is driven around the path
For multiple sensors, the estimates are updated by shown in figure 3. The results were obtained using the EKF
adding the summation of the information observation without using the velocity constraints. Overall, the estimates
vectors and matrices of the multiple sensors used as given in closely follow the results from the MIDG. Nevertheless,
equations 32a and 32b below. since the velocity constraints are not fused, it is expected that
both the EKF solution and the MIDG solution are biased
( | ) = ( | − 1) + ∑ ( ) (37a) from the true path.
( | ) = ( | − 1) + ∑ ( ) (37b)

The recursive update algorithm of the extended

information filter, as seen in the above equations, makes its
use advantageous for real-time applications as it requires less
computational demand than the EKF approach. By fusing the
dynamics of the vehicle measured by the accelerometers and
gyroscopes with the GPS measurements and the velocity
constraints, the errors in the position, velocity and attitude
are reduced. Experimental results are given the next section
to verify the accuracy of the presented algorithms.
VI. EXPERIMENTAL TESTING Figure 3: Path Estimate of the vehicle, Dynamic Test Using EKF
The algorithms presented in this paper were implemented without Velocity Constraints
in code and tested. Multiple experiments were conducted to
test the performance of the proposed algorithm. Results
relative frequency

400

obtained from these experiments are shown below. In the 200

X: -0.3406
Y: 153.3

results below, the estimated state obtained from the proposed

0
algorithms (green line) is validated against a commercial off- -1.4 -1.2 -1 -0.8 -0.6 -0.4
x P error [m]
-0.2 0 0.2 0.4 0.6

the shelf (COTS) MIDG solution (blue line). The

relative frequency

400

specifications of the MIDG unit are shown in the table 200

X: 0.1963
Y: 159.7

below. The authors are currently working on installing the

system on an UAS. Therefore, Additional UAS experimental 0
-1 -0.5 0 0.5 1 1.5
y P error [m]
results will be presented during the conference.
relative frequency

400

X: 0.6918
200
TABLE I. MIDG SPECIFICATIONS Y: 122.6

0
Power Requirements -2 -1.5 -1 -0.5 0 0.5
z P error [m]
1 1.5 2 2.5 3

Input Voltage 10 VDC-32VDC

Power 1.2 W max
Figure 4: Position Error in ECEF frame, Dynamic Test Using EKF
Measurements without Velocity Constraints
Angular Rate
Range ±300°/
Non-Linearity 0.1% of FS
Noise Density 0.05°/ /√
3 dB Bandwidth 20 Hz
Acceleration
Range ±6
Non-Linearity 0.3% of FS
Noise Density 150 /√
3 dB Bandwidth 20 Hz

910
 against the MIDG solution. Figure 7 shows the path of
relative frequency

400
X: -0.04039
Y: 192
motion of the vehicle as estimated by the EKF along with the
200
MIDG solution.
0
-4 -3 -2 -1 0 1 2 3
x V error [km/h]
relative frequency

400
X: -0.7209
Y: 182.6
200

0
-2.5 -2 -1.5 -1 -0.5 0 0.5 1
y V error [km/h]
relative frequency

X: -0.3173
400 Y: 303.3

200

0
-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5
z V error [km/h]

Figure 5: Velocity Error in body frame, Dynamic Test Using EKF without
Velocity Constraints

Figures 4 and 5 illustrate the normal Gaussian Figure 7: Path Estimate of the vehicle, Dynamic Test Using EKF with
distributions of the position in ECEF frame and velocity Velocity Constraints
errors in body frame of the vehicle, respectively. These are
the differences in the position and velocity estimates between

relative frequency
400
the MIDG solution and the estimated solution. From Figure
200
4, it can be noticed that the position errors in the body frame X: -0.1789
are, on the average, less than -0.8 meter to 0.4 meter for the x 0
-1.2 -1 -0.8 -0.6
Y: 153.5
-0.4 -0.2 0 0.2 0.4 0.6 0.8
direction, and from -0.5 to 1 meter in the y direction, and x P error [m]
relative frequency

400
from -0.8 to 2 meters in the z direction. Figure 5 shows that
X: -0.7634
the velocity error has a mean of -0.04 km/h in the body frame 200 Y: 122.9

x-axis, -0.72 km/h in the body frame y-axis, and -0.32 km/h 0
in the body frame z-axis. -4 -3 -2 -1
y P error [m]
0 1 2
relative frequency

400
Figure 6 shows the estimated attitude of the vehicle. The
change of the yaw angle during the first 20 seconds shown in 200

the figure indicates that the vehicle was travelling along 0

X: 0.896
Y: 132.5
-1.5 -1 -0.5 0 0.5 1 1.5 2 2.5 3
roundabout. It is clear that the estimate of the yaw angle z P error [m]

provided by the solution follows the yaw angle provided by

the MIDG unit despite the highly involved dynamics of the Figure 8: Position Error in ECEF frame, Dynamic Test Using EKF
vehicle in the first 20 seconds and the two turns at around the with Velocity Constraints
70th and 90th seconds.
relative frequency

400
350
yaw [Degrees]

300 MIDG 200

250 solution X: 0.004599
Y: 192.9
200 0
150 -4 -3 -2 -1 0 1 2 3
0 20 40 60 80 100 120 x V error [km/h]
relative frequency

Time [s]
400
10
pitch [Degrees]

MIDG
200 X: -0.03306
0 solution Y: 267.1

0
-10 -0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3 0.4
0 20 40 60 80 100 120 y V error [km/h]
Time [s]
relative frequency

2000
5
roll [Degrees]

MIDG
X: -0.04325
solution 1000 Y: 561
0

-5 0
0 20 40 60 80 100 120 -1.5 -1 -0.5 0 0.5 1 1.5 2
Time [s] z V error [km/h]

Figure 6: Attitude Estimate, Dynamic Test Using EKF without Figure 9: Velocity Error in body frame, Dynamic Test Using EKF with
Velocity Constraints Velocity Constraints

B. Single Vehicle Dynamic Test using EKF with Velocity As noticed from the figures above, the use of the velocity
Constraints constraints improves the estimation accuracy. From Figure 8,
it can be noticed that the mean of the position errors in the
In figures 7-10, the estimated path, the vehicle’s velocity ECEF frame is around -0.18 meter for x direction, less than -
(in body frame) and position (in ECEF) estimated using an 0.76 meters for y direction, and 0.90 meters for z direction.
EKF with velocity constraints are shown and compared Figure 9 shows that the mean of the velocity error is around

911
0 km/h. This shows an improvement in the error compared to 100
results obtained using the EKF without velocity constraints. MIDG

x V [km/h]
solution
0
Figure 10 shows the attitude estimation of the vehicle.
The peaks in the estimated pitch angle show that the vehicle -100
0 20 40 60 80 100 120
Time [s]
passed over 4 bumps at seconds: 30, 40, 78 and 98. 5
MIDG

y V [km/h]
0 solution
350
yaw [Degrees]

MIDG
300
solution -5
250 0 20 40 60 80 100 120
200 Time [s]
150 2
0 20 40 60 80 100 120

z V [km/h]
MIDG
Time [s] solution
0
10
pitch [Degrees]

MIDG
solution -2
0 0 20 40 60 80 100 120
Time [s]
-10
0 20 40 60 80 100 120
Time [s]
5
Figure 12: Velocity Estimate, Dynamic Test Using EIF with Velocity
roll [Degrees]

MIDG Constraints
0 solution

-5
Figure 11 shows that the position error is around -0.18 m,
0 20 40 60
Time [s]
80 100 120
-0.76 m and 0.90 m in mean in the ECEF x, y and z axes. It
can be seen from Figure 12 that, since the velocity
Figure 10: Attitude Estimate, Dynamic Test Using EKF with constraints are applied, velocities closer to zero are obtained
Velocity Constraints in the body frame x and y axes. This is not the case for the
results reported by the MIDG solution. Figure 13 shows the
error in the attitude estimate between the EIF the MIDG
solution.
The next section discusses results for the same test but
using EIF algorithm.
relative frequency

1000

500 X: 0.703
Y: 187.5

C. Single Vehicle Dynamic Test using EIF with Velocity 0

-3 -2 -1 0 1 2 3 4
Constraints yaw error [deg]
relative frequency

400
The same dynamic test data is now fused with the GPS X: -0.1622
200 Y: 132.9
and encoder data using the EIF. The results of this test are
shown in figures 11 through 13. The estimation error 0
-2 -1.5 -1 -0.5 0 0.5 1 1.5
pitch error [deg]
resulting from this integration should be bounded by the GPS
relative frequency

500
measurement error and the fact that our nonholonomic X: 0.1892
Y: 166
constraints are zero with a small magnitude white noise.
0
-1.5 -1 -0.5 0 0.5 1 1.5 2 2.5
roll error [deg]
relative frequency

400

200 Figure 13: Attitude Error, Dynamic Test Using EIF with Velocity
X: -0.1788 Constraints
0 Y: 153
-1.2 -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8
x P error [m] Next, the result of multiple vehicle tests are shown and
relative frequency

400 discussed.
X: -0.7634
200 Y: 124.2

0 D. Two-Vehicle Dynamic Test 1 using EKF with Velocity

-3.5 -3 -2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5
y P error [m] Constraints
relative frequency

400

200
Figures 14 through 17 show the results of a two vehicle
X: 0.9023 experiment where two vehicles were heading towards each
0
-1.5 -1 -0.5 0 0.5 1
Y: 131.6
1.5 2 2.5 3 other on different lanes of the road. The measurements were
z P error [m]
fused using the EKF. Figure 14 shows the path estimates of
the two vehicles and that obtained using the MIDG unit.
Figure 11: Position Error in ECEF frame, Dynamic Test Using EIF with
Velocity Constraints

912
 Figure 17 shows the relative attitude between the two
vehicles estimated by the EKF and by the MIDG solution.
From the -180˚ heading difference, It can be seen that vehicle
1 was travelling southeast and vehicle 2 was travelling in the
opposite direction to northwest.

-175

yaw Diff [deg]

MIDG
solution
-180

-185
0 2 4 6 8 10 12 14
Time [s]
2

pitch Diff [deg]

MIDG
solution
0

-2
0 2 4 6 8 10 12 14
Figure 14: Path Estimates of the vehicles, Two-Vehicle Dynamic Time [s]
Test 1 Using EKF with Velocity Constraints 0

roll Diff [deg]

MIDG
solution
-1
Figure 15 shows the relative velocity of vehicle 2 with
respect to vehicle 1 represented in the body frame of -2
0 2 4 6 8 10 12 14
Time [s]
vehicle 2. It is seen that the relative velocity estimate
obtained by the EKF is closely matching that obtained
Figure 17: Vehicle 2 Attitude Relative to Vehicle 1 Velocity in
from the MIDG. It can also be seen that the relative Test 2 Using EKF with Velocity Constraints
velocity in the body y and z axis is estimated to be very
close to zero while the estimate obtained by the MIDG is The next section shows the results of the second two-
biased from zero, especially along the body y axis. vehicle test using the EIF with velocity constraints.

50
E. Two-Vehicle Dynamic Test 2 using EIF with Velocity
x V Diff [km/h]

MIDG
solution
0 Constraints
-50
0 2 4 6
Time [s]
8 10 12 14 Figures 18 through 20 show the results of a two-vehicle
5 dynamic test where the two vehicles were following each
y V Diff [km/h]

MIDG
solution other along the same lane. The proposed EIF algorithm was
0
implemented onboard each vehicle. Figure 18 shows the
-5
0 2 4 6 8 10 12 14 relative position estimates of the two vehicles using MIDG
2
Time [s]
and EIF. In this test, vehicle 2 was following vehicle 1.
z V Diff [km/h]

MIDG
solution
0 10
x V Diff [km/h]

MIDG
-2 0 solution
0 2 4 6 8 10 12 14
Time [s]
-10
0 2 4 6 8 10 12 14 16 18 20
Time [s]
2
Figure 15: Vehicle 2 Velocity Relative to Vehicle 1 Velocity in Test 1
y V Diff [km/h]

MIDG
Using EKF with Velocity Constraints 0 solution

-2
Figure 16 shows the position of vehicle 2 relative to 0 2 4 6 8 10
Time [s]
12 14 16 18 20

vehicle 1 represented in the body frame of vehicle 2. 1

z V Diff [km/h]

MIDG
0 solution

500
MIDG -1
x P Diff [m]

X: 5.435 0 2 4 6 8 10 12 14 16 18 20
Y: -0.0006706 solution Time [s]
0

-500 Figure 18: Vehicle 2 Velocity Relative to Vehicle 1 Velocity in Test 2 Using
0 2 4 6 8 10 12 14
Time [s] EIF with Velocity Constraints
15
Figure 19 shows the relative velocity between vehicle 2
y P Diff [m]

MIDG
10 solution
and vehicle 1 in the body frame of vehicle 2. It can be
5
0 2 4 6 8 10 12 14
noticed that at the beginning, the forward velocity of vehicle
Time [s] 2 was more than that of vehicle 1. At around the seventh
5
second of the test, the forward velocity of vehicle 1 exceeds
z P Diff [m]

MIDG
0 solution
that of vehicle 2.
-5
0 2 4 6 8 10 12 14
Time [s]

Figure 16: Vehicle 2 Position Relative to Vehicle 1 Velocity in Test 1 Using

EKF with Velocity Constraints

913
 As shown in the experimental results, the velocity constraint
60
enhances the accuracy of the estimate. This is especially seen
x P Diff [m]

MIDG
solution
40
in the body-frame y and z velocity estimates as they
20
0 2 4 6 8 10
Time [s]
12 14 16 18 20 converge to zero in comparison to results obtained without
2 using the constraints which are shown to oscillate away from
y P Diff [m]

0
MIDG
solution
zero. Experimental results were shown to verify the accuracy
of the estimation algorithm. The results obtained were
-2
0 2 4 6 8 10
Time [s]
12 14 16 18 20 checked against a commercial standalone solution.
1
z P Diff [m]

0
MIDG REFERENCES
solution

-1
[1] L. R. Sahawneh, M. A. Al-Jarrah, K. Assaleh, and M. F. Abdel-
0 2 4 6 8 10
Time [s]
12 14 16 18 20 Hafez, “Real-Time Implementation of GPS Aided Low Cost
Strapdown Inertial Navigation System,” Journal of Intelligent &
Robotic Systems, Vol. 61, No. 1-4, 2011, pp. 527-544.
Figure 19: Vehicle 2 Position Relative to Vehicle 1 Position in Test 2 Using [2] S. Sukkarieh, “Low Cost, High Integrity, Aided Inertial Navigation
EIF with Velocity Constraints Systems for Autonomous Land Vehicles,” Ph.D. dissertation,
Mechanical and Mechatronic Engineering, Australian Centre for Field
Figure 20 shows the relative attitude of vehicle 2 relative Robotics, The University of Sydney, Sydney, Australia, March, 2000.
to vehilce 1. The estimated relative yaw, which is close to [3] M. F. Abdel-Hafez. “The Autocovariance Least Squares Technique
for GPS Measurement Noise Estimation,” in IEEE Transactions on
zero degrees, shows that the two vehicles were travelling in Vehicular Technology, Vol. 59, No. 2, January 2010, pp. 574-588.
the same direction for the whole test. [4] A. Noureldin, T. B. Karamat, M. D. Eberts and A. El-Shafie,
“Performance Enhancement of MEMS-Based INS/GPS Integration
for Low-Cost Navigation Applications,” IEEE Transactions on
2
Vehicular Technology, Vol.58, issue 3, March, 2009, pp.1077 - 1096.
yaw Diff [deg]

MIDG
0 solution [5] R. Schelling, “A Low-Cost Angular Rate Sensor for Automotive
-2
Applications in Surface Micromachining Technology,” Third Annual
0 2 4 6 8 10 12 14 16 18 20
Time [s] International Conference on Advanced Microsystems for Automotive
2 Applications Proceedings, March, 1999.
pitch Diff [deg]

0
MIDG
solution
[6] P. Belanovié, D. Valerio, A. Paier, T. Zemen, F. Ricciato, and C. F.
Mecklenbräuker. “On Wireless Links for Vehicle-to-Infrastructure
-2
0 2 4 6 8 10 12 14 16 18 20 Communications,” in IEEE Transactions on Vehicular Technology,
Time [s] 59(1), 2010, 269-282.
2
[7] P. Kinney, “ZigBee Technology: Wireless Control that Simply
roll Diff [deg]

0 solution
-2
0 2 4 6 8 10 12 14 16 18
Time [s]
Figure 20: Vehicle 2 Attitude Relative to Vehicle 1 Attitude in Test 2 Using
EIF with Velocity Constraints
The next section compares computational time of the
different algorithms implemented in this work.
F. Processing Speed of the Two Filters
The computational time of the two algorithms was
compared by the time needed for updating the state by one
measurement epoch on a 2.33 GHz dual core processor. On
average, with the velocity constraints fused, the EKF takes
1.84 × 10 seconds for one cycle, while the EIF takes
1.535 × 10 seconds per cycle. This is about 16.58%
reduction in computational time.
Finally, concluding remarks are presented in the next
section.
VII. CONCLUSION
In this paper, low-cost GPS/IMU system is fused with
vehicle dynamics using two approaches. An extended
Kalman filter and an extended information filter were used in
the fusion process. Both filters gave close results; however,
the EIF implementation is computationally less expensive
than the EKF. Velocity measurement and nonholomonic
constraints are used to improve the accuracy of the estimate.
MIDG
Works,” Communications Design Conference, October, 2003.
[8] S. Biswas, R. Tatchikou and F. Dion. “Vehicle-to-Vehicle Wireless
20
Communication Protocols for Enhancing Highway Traffic Safety,” in
IEEE Communications Magazine, 44(1), 2006, 74-82.
[9] A. Lahrech, C. Boucher and J.-C. Noyer, “Accurate vehicle
positioning in urban areas,” IMTC 2007 - IEEE Instrumentation and
Measurement Technology Conference, Warsaw, Poland, May 1-3,
2007.
[10] L. Che-Chung, L. Chi-Wei, H. Dau-Chen & C. Yung-Hsin , "Design
a Support Vector Machine-based Intelligent System for Vehicle
Driving Safety Warning," 11th International IEEE Conference on
Intelligent Transportation Systems, pp.938-943, 12-15 Oct. 2008.
[11] D. Hightower. “Wireless Technology Advances Crash Avoidance,”
in Microwaves & RF, 2010, pp. 22.
[12] J. Santa, R. Toledo-Moreo, M.A. Zamora-Izquierdo, B. Ubeda, A.F.
Gomez-Skarmeta "An analysis of communication and navigation
issues in collision avoidance support systems," in Transportation
Research Part C: Emerging Technologies, 18 (3), 2010, pp. 351-366.
[13] S. Bonnabel and E. Sala n, "Design and prototyping of a low-cost
vehicle localization system with guaranteed convergence properties,"
Control Engineering Practice, vol. 19, pp. 591-601, 2011.
[14] A.Brandt and J. F. Gardner, “Constrained Navigation Algorithms for
Strapdown Inertial Navigation Systems with Reduced Set of
Sensors,” Proceedings of the American Control Conference, June,
1998.
[15] G. Dissanayake and S. Sukkarieh, “The Aiding of a Low-Cost
Strapdown Inertial Measurement Unit Using Vehicle Model
Constraints for Land Vehicle Applications,” IEEE Transactions on
Robotics and automation, vol. 17, no. 5, October, 2001, pp. 731-747.
[16] A. Noureldin, A. El-Shafie, and M. Bayoumi, "GPS/INS integration
utilizing dynamic neural networks for vehicular navigation,"
Information Fusion, vol. 12, pp. 48-57, 2011.
[17] X. Kong, “INS algorithm using quaternion model for low cost IMU,”
Robotics and Autonomous Systems, vol. 46, pp. 221-246, 2004.
914