Integration of the INS and GPS navigation systems using a

Kalman Filter on a mobile phone

B. Fons-Albert J. Vila-Carbó

Universitat Politècnica de València

borjafons@gmail.com, jvila@disca.upv.es

Abstract

This paper shows the feasibility of implementing and execute with real-time performance a
complex INS/GNS integrated filter for attitude and navigation estimation on a small mobile device
that can be equipped on a UAV. The proposed integration is based on a Kalman filter and improves
the performance of both INS and GPS systems: the attitude estimation of the INS is made more
accurate during turns and sudden accelerations while the frequency of the GPS estimations can be
increased by making predictions with the INS. The paper shows that a good accuracy in attitude
estimation and navigation can be obtained using the low cost sensors of mobile devices.

keywords: Inertial Navigation Systems (INS), Global navigation, GPS, Kalman filtering, mobile devices.

1 Introduction
The proliferation of small devices equipped with inertial sensors, GPS, cams and various connectivity
facilities (3G, wifi) has made highly attractive to use these devices in UAV control and navigation.
The UAV (Unmanned Aerial Vehicle) project that we are involved at the UPV uses an Apple iPhone
for the control of two UAV prototypes developed for research purposes: a quadrotor (fig. 1) and a delta
wing. The iPhone terms of service prohibits the device being used to control remote vehicles and being
used for emergency or life-saving purposes (iPhone SDK, Section 3.3.7). However, this is not the only
nor the most serious safety limitation or regulations that UAVs will have to face in the future if they
are widely used. Despite this inconvenient, we have found it very useful for rapid prototyping since
it has a very powerful and compact hardware and also a highly available and excellent cross Software
Development Kit (SDK).
This paper only concentrates on the attitude and navigation filters that have been designed for
our UAV prototypes. Several INS/GPS integration methods have been proposed in the bibliography,
from loosely-coupled to tightly-coupled implementations: see [3] [4] [5] [?], among others. This paper
introduces improvements on loosely-coupled schemes based on low-cost MEMs devices.

1
2 System description

This section presents a loosely-coupled INS/GPS

Figure 1: Quadrotor prototype of the UPV
integration method for attitude, position, and
speed computation. The implementation is based
on the iPhone 4, although it can be easily ported
to any embedded system, since no iPhone specific
facility for INS or GPS filtering has been used; only
the raw access to the sensors. Furthermore, some
iPhone limitations, like the maximum frequency of
sensor reading (50 Hz) or the sensor accuracy, could
be easily overcome on other platforms.
As a matter of fact, we have ported it to an
LPC2468 (ARM7 based) microcontroller.

Figure 2 shows the system structure where the INS is composed by the three axis gyroscope /
accelerometer / compass that equip the iPhone They are low cost MEMs technology sensors. The
iPhone GPS is also a low cost unit that provides location, speed and course. The software design to
compute attitude and position has two main blocks: the Attitude Kalman Filter, which provides the
Euler angles, and the Navigation Kalman Filter, which provides position and speed. Getting a high
accuracy in attitude estimation has shown to be key for making the navigation filter feasible. This has
motivated a lot of efforts for improving attitude estimation.
The implementation of this filters on the iPhone uses the graphical interface of fig. 2. It has three
main views: the artificial horizon, the map view (not shown) and the control panel, which allows to
make real-time transmissions of data sensors and filter results to a host computer using the wifi.

2.1 Attitude filter

Attitude estimation is the result of filtering two independent measures: the gyroscopes on one hand,
and accelerometers and magnetometers on the other. The attitude filter is an Extended Kalman Filter
filter with seven states: the four quaternion components plus the three gyros biases. The quaternion
has been chosen as the attitude representation because it highly improves the accuracy of floating point
calculations and avoids singularities of trigonometric functions.
Computing the attitude using the gyros basically consists of integrating the angular speeds Ωb that
they measure. This is performed using the quaternion propagation eq. [1]: q̇ = 21 q×Ωb . The integration
accuracy relies on determining the gyros bias [5], since its integration can lead to non-bounded errors
in attitude. For this reason, the gyros biases have been chosen as a part of the filter state.
Computing the attitude using the accelerometers and magnetometers is based on finding out the

2
 Figure 2: Block diagram of the system and iPhone Graphical User Interface

angles of the gravity vector ~g and the geomagnetic field vector m

~ with the Body Fixed Frame (BFF) axis
system. Based on this, the attitude angles (φ, θ, ψ) can be calculated using well known formulas [1] [3]:

φf = atan2 (−gy , −gz ) mE = my cos φ + mz sin φ

 q 
θf = atan2 gx , gy2 + gz2 mN = mx cos θ + my sin φ sin θ + mz cos φ sin θ
ψ = arctan(mE , mN )

The problem of attitude computation using the accelerometers is that they do not only measure the
gravity vector, but the total acceleration. That includes so called effective forces, like the ones due to
tangential or centripetal accelerations. These forces make the above attitude computation erroneous.
Kalman filtering of the attitude estimation of the gyros on one hand, and the accelerometers /
magnetometers on the other, produces a sensor fusion that improves the estimation. However, in our
experience the attitude estimation still results inaccurate, especially during turns and accelerations.
Solving this requires to compensate the accelerometers readings with the effective forces, so the gravity
vector can be correctly evaluated. We evaluate the effective forces using the GPS.
Tangential accelerations can be obtained from the speed difference of two consecutive measures of the
GPS along a time interval. This acceleration is in the direction of motion, and needs to be transformed
through the rotation matrix to the BFF. Keep in mind that if the longitudinal axis of the device remains
aligned with the direction of motion or forming a given angle, then the rotation between the reference
systems is straightforward; otherwise it should be rotated an additional angle, which is the angle between
the direction of motion and the longitudinal axis.
Estimating centripetal accelerations involves more measures. They can be calculated through the
following expression: ac = Ωb × v b , where Ωb is the angular speed vector obtained from the gyros and

3
v b is the speed vector of the BFF obtained from the GPS. Based on the speed, course and attitude
readings, the components of v b can be obtained. This components have to be first rotated from the
NED (North East Down) to the BFF axis and then, they can be used to calculate the cross product of
the previous equation.
The former computed tangential and centripetal accelerations are subtracted from the readings of
the accelerometers for the correct estimation of the gravity vector.

2.2 Navigation filter

The navigation filter computes the speed and position concurrently with the attitude filter. It was
originally a six states filter composed by the three components of the speed and the three of the position.
It was later extended to twelve, in order to include an error model for compensating the accelerometers
bias and gain factors. The prediction of these magnitudes basically involves the integration of the
accelerations due to effective forces. The Kalman filter measurements are the GPS readings. The
navigation equations of the system model for the filter prediction relate the derivatives of speed and
position to the filter state variables and the inputs [1] [3]:
2
vN vD −vE tan L
L̇= Rv0N+h v̇N =fN + R0 +h
˙
l= vE
v̇E =fE + vE
(R0 +h) cos L R0 +h (vD + vN tan L)
2 2
vN +vE
ḣ=−vD v̇D =fD − R0 +h +g

where L, l and h are the latitude, longitude and height, vN , vE and vD are the North-East-Down
(NED) components of the speed, R0 is the radius of the Earth and the terms fN , fE and fD are the
NED components of the specific forces. They are calculated using the accelerometers readings and
transforming them to the NED system through the rotation matrix computed in the attitude block.

3 Results
This section shows the results of the attitude and navigation filters shown in section 2.
Effective forces : the first important result is the feasibility of accurately calculating effective forces
(tangential and centripetal acc.) based on the GPS. This is used to compensate the accelerometers
readings in order to compute the gravity vector. It improves quite a lot the attitude determination
in turns and sudden accelerations. Figure 3 (1) shows the tangential acceleration measured using the
GPS and the measures of the motion oriented accelerometer. It can be seen that there is a vertical
displacement between both signals which is due to the gravity component in that direction. Some delay
can also be appreciated, since GPS calculations are delayed by one iteration. Figure 3 (1) shows similar
results for the centripetal acceleration of the y axis of the BFF.
Attitude determination: The calculation of the roll (φ) and pitch (ψ) angles with the proposed
attitude filter is really accurate, once the compensation of the effective forces is performed. The most
challenging and difficult issue is the calculation of the yaw angle ψ. Figure 4 (1) shows the comparison for

4
 Tangential acceleration
Centripetal acceleration
0.4
Accelerometer
0.3 Accelerometer
GPS
GPS
0.3

0.2

0.2

0.1
acceleration (g)

acceleration (g)
0.1

0
0

−0.1 −0.1

−0.2 −0.2

−0.3
40 50 60 70 80 90 100 110 0 50 100 150 200 250 300
time (s) time (s)

Figure 3: (1): Comparison of tangential acc. measured using the GPS and the motion oriented ac-
celerometer. (2): Comparison of centripetal acc. measured using the GPS and the y (BFF) axis accel.

this angle using the the GPS and the attitude filter. Except for the initial moments, when the vehicle
is stopped, and the GPS readings are incorrect, both estimations match almost perfectly. Another
remarkable result of the attitude filter is its ability to calculate an stationary value for the bias of the
gyros without any initial calibration (figure 4 (2)).
−3 Bias gyroscopes
Course x 10
10
200
GPS
Kalman filter
150 8

100 6

bx
50
4 by
course (deg)

bias

0
bz
2

−50

0
−100

−2
−150

−4
−200 0 50 100 150 200 250 300 350 400 450
0 50 100 150 200 250 300 350 400 450
t (s) t (s)

Figure 4: (1): Course measured using the GPS and the Attitude Kalman filter. (2): Bias of the gyros.

Position determination The proposed navigation filter allows to make the position calculation at
a higher frequency (50 Hz) than the GPS (1 Hz) and still estimate it with reasonable accuracy during
short periods of GPS unavailability. Figure 5 (1) shows the path used for one of the experiments.
The most innacurate measure with the GPS is always the altitude. Figure 5 (2) shows a comparison
between the GPS reading and the improved estimation using the filter. Experiments show that the
GPS readings are better at higher speeds and when no turns are performed. Despite that, the IMU
significantly improves the GPS readings in turns and sudden accelerations or decelerations. On the
other hand the filter also allows to decompose the GPS speed and get its values in each navigation axis
(NED). Figure 5 (2) shows an example of the N component which is very good, except for the initial
moment when the gyros bias is still not compensated and the attitude calculation is not reliable.

5
 North speed
Path 4
Latitude
39.346 Kalman filter
Kalman filter GPS
2
39.344 39.3425 GPS

39.342 39.342 0

39.34
39.3415 −2

north speed (m/s)

latitude (deg)
39.338
39.341
latitude (deg)

−4
39.336
39.3405
39.334 −6
39.34
39.332
−8
39.3395
39.33

39.339 −10
39.328

39.3385 −12
39.326 100 110 120 130 140 150 160 170 180 190 0 50 100 150 200 250 300 350 400 450
−0.492 −0.49 −0.488 −0.486 −0.484 −0.482 −0.48 −0.478 −0.476 −0.474
longitude (deg) t (s) t (s)

Figure 5: (1): Path of the experiment. (2): Height using the Kalman filter and GPS (3): North
component of the speed using the Kalman filter and GPS.

GPS signal losses. Finally, it’s intended to show the behavior of the system in periods of GPS
signal losses. In these cases, it gains importance the effort made in the correction of the errors of the
sensors and in the accuracy of the attitude estimation. In order to demonstrate this, two models of
attitude estimation have been used, and their results compared. The first model relies heavily on the
attitude estimation made by the accelerometers while the second one relies heavily on the estimation
made by the gyroscopes.
It’s important to remember that now, it’s not possible to make the estimations of the inertial forces
as the speed measured by the GPS is no longer available. So the first model is expected to work worse
in this period.
The results are shown in figure 6. As it has been said, the first model is badly estimating the attitude
and its errors affect the navigation calculations. On the other hand, the second model is less affected
by the inertial forces and the attitude estimation is still good. The problem is that once the GPS signal
has been recovered, the accumulated errors stay in the system much longer in the second model than
in the first one. These is because the corrections made by the accelerometers are more agressive in first
model. So it seems that each model works better in certain conditions. For this reason, the implemented
solution consists in changing the working mode of the system depending on the GPS signal reception.

4 Conclusions
The paper has shown the feasibility of implementing a complex INS/GNS integrated attitude and
navigation filter on a small device that can be equipped on a UAV and getting a reasonable accuracy.
One of the most interesting conclusions is that the in order to be able to improve the GPS results
with the INS, it is required a highly accurate attitude determination, specially during turns and sudden
accelerations. This improvement in attitude only can be achieved by estimating the effective forces with
some other sensor which, in this case is the GPS itself. Another important problem that was solved was
the model errors for the bias compensation of the gyros (attitude filter) and accelerometers (navigation
filter). In both cases they were included as part of the filter state and it proved to be effective in

6
Figure 6: Behavior of the system in GPS signal losses periods. Comparison between the model based
on accelerometers (blue line) and the one based on gyroscopes (red line).

its calculation. Finally, the accuracy of the attitude determination can be considered excellent. The
navigation accuracy is also high, even in GPS signal losses periods of about 15 seconds long, although
in the altitude determination is somewhat lower.

References
[1] Mohinder, S. et al. Global Positioning Systems, Inertial Navigation, and Integration, Wiley.

[2] Kumar, V. Integration of inertial navigation system and global positioning system using kalman filter,
Masters thesis, Indian Institute of Technology, 2004.

[3] Rönnbäck, S. Development of a INS/GPS navigation loop for an UAV, Lulea Univ. of Technology, Feb.2000.

[4] Kingston, D.B. and Beard, R.W. Real-Time Attitude and Position Estimation for Small UAVs Using Low-
Cost Sensors, Department of Electrical and Computer Engineering, Brigham Young University, Provo,
Utah.

[5] El Hadri A. and Benallegue A. Attitude estimation with gyros-bias compensation using low-cost sensors,
Joint 48th IEEE Conference on Decision and Control and 28th Chinese Control Conference Shanghai, P.R.
China, December 16-18, 2009

[6] Jung, B. and Tsiotras, P. Inertial Attitude and Position Reference System Development for a Small UAV,
Georgia Institute of Technology, Atlanta, GA, 30332-0150