You are on page 1of 5

An Accurate Device for Real-Time Altitude

Estimation Using Data Fusion Algorithms


Paola Pierleoni, Alberto Belli, Lorenzo Palma, Luca Pernini and Simone Valenti
Department of Information Engineering
Marche Polytechnic University
Ancona, Italy
Email: s.valenti@univpm.it
AbstractThis paper presents an accurate system to estimate
the altitude of a rigid body by fusing data from four low-cost
sensors such as an accelerometer, a gyroscope, a magnetometer
and an altimeter. Usually a MEMS altimeter barometric sensor
allows to obtain the altitude signal from measures of atmospheric
pressure and temperature but these measures are affected by
noise that causes a significant error in the calculated altitude
values. In order to get an accurate estimation of the altitude,
in this work a complementary filter is used to fuse the raw
signal of the altitude obtained from barometer sensor and vertical
displacement signal calculated through a data fusion algorithm
applied to the signals of the other three sensors. In order to
evaluate the performance in human activity monitoring applications, the proposed device has been tested and compared with
the system that currently presents the better performance for
the same technology according to its experimental protocols. The
results show that our device exceeds the performance provided
by the currently systems reported in literature.

Through complementary filter, the altimeter raw data are


fused with vertical displacement measurements obtained by
double-integration of dynamic acceleration component perpendicular to the terrestrial surface. In order to derive this
useful acceleration component we have used the estimation
of orientation computed by AHRS algorithm implemented
into the device. Our implementation of the complementary
filter together with AHRS algorithm is particularly suitable
for real-time applications into low cost microcontrollers and it
significantly reduces the computational effort associated with
the conventional Kalman-based approaches. Essentially, the
proposed system performs data fusion algorithms with low
computational load and has the benefit of being low cost and
easy to wear.

I. I NTRODUCTION

The device includes an accelerometer, a gyroscope, a magnetometer and an altimeter that communicates with the MCU
via I2C bus. A RF Bluetooth module interfaces the board with
external devices as personal computer, smartphone and tablet.
SD card memory allows data storing. The block diagram
of the system is shown in Fig. 1 and the main features of
each component of the device are described in the following
subsubsections.
1) MCU Module: ATmega328 (ATMEL, USA) is a lowpower single chip 8-bit microcontroller belongs to the
megaAVR series. This module combines 32-kb ISP flash
memory with read-while-write capabilities, 1-kb EEPROM, 2kb SRAM, 23 general purpose I/O lines, 32 general purpose
working registers, internal and external interrupts, serial programmable USART, byte-oriented a 2-wire serial interface, a
SPI serial port, 6-channel 10-bit A/D converter, programmable
watchdog timer with internal oscillator, and five software
selectable power saving modes. The device operates in the
range 1.8-5.5 V.
2) Accelerometer and Gyroscope: MPU-6050 (InvenSense
Inc., USA) combines a MEMS 3-axis gyroscope and a MEMS
3-axis accelerometer on the same silicon die together. The
gyroscope is a tri-axis angular rate sensor with a sensitivity
up to 131 LSBs/dps and a full-scale ranges of 250, 500,
1000, and 2000dps. The accelerometer is a tri-axis sensor
with a programmable full scale range of 2g, 4g, 8g and
16g. MPU-6050 communicates with MCU via I2C bus.

Information about the altitude of a rigid body is used in


various applications such as in human activity monitoring systems [1], indoor navigation systems [2], fall detection systems
[3], smartphones applications [4], bike computers and others.
The MEMS altimeter barometric sensor allows to calculate
the altitude signal from measurements of atmospheric pressure
and temperature. These measures are affected by noise that
causes wide and rapid fluctuations of the altitude signal [5]. In
order to obtain an improved altitude estimation, measures from
altimeter sensor are fused with additional altitude information
derivated from the GPS [2] or accelerometer signals [6], [7]
using Kalman filter. However, Kalman filter is characterized
by a significant complexity and demands large computational
load for its implementation. Moreover in indoor applications,
information about the vertical position of a body can not use
the GPS signal because it might be too weak.
II. M ETHODS & M ATERIALS
Our work presents an accurate device for real-time altitude
estimation mainly composed of a triaxial MEMS accelerometer, a triaxial MEMS gyroscope, a triaxial magnetometer and
a MEMS altimeter sensor. The first three sensors implement
an Attitude and Heading Reference System (AHRS) and
are used to estimate the orientation of the device. Altimeter barometric sensor is used to calculate the raw altitude
from measurements of atmospheric pressure and temperature.
978-1-4799-2280-2/14/$31.00 2014 IEEE

A. Hardware of the Proposed Device

this measure, it is therefore necessary an appropriate filtering


operation of the raw altitude signal.
C. AHRS to Estimate Vertical Displacement

Fig. 1. Block diagram of the proposed device.

3) Magnetometer: HMC5883L (Honeywell, USA) is a 3axis magnetoresistive sensor designed for low-field magnetic
sensing. It has an embedded 12-bit ADC achieving 4 mG
field resolution in 8 G fields. The module measures both
the direction and the magnitude of Earths magnetic field.
HMC5883L communicates with MCU via I2C bus.
4) Altimeter Sensor: MS5611-01BA (MEAS, Switzerland)
is a new generation of high resolution MEMS altimeter sensors
with SPI and I2C bus interface. It provides a precise digital
24 bit pressure and temperature values and different operation
modes that allow the user to optimize the conversion speed
and the current consumption.
5) Wireless Module: HC-05 module (Honeywell, USA) is
an easy to use Bluetooth SPP (Serial Port Protocol) module,
designed for transparent wireless serial connection setup. It is
a small low-power transceiver ideal for embedded applications.
6) Storage Module: MicroSD card is used to provide
the device with mass-storage capability. Communication with
microSD card is achieved over SPI interface.
The data from sensors are sampled at 50 Hz. A calibration
procedure of the device was performed offline to compensate
systematic errors in the AHRS.
B. Altimeter to Calculate Altitude
The altimeter sensor allows to calculate altutitude Hbaro ,
from temperature and pressure measurements [8]:
(T + 273.15)
(1 (P/P0 )(0.19) )
(1)
0.0065
where Hbaro is measured in meters, T is the temperature
measured in C, P is the pressure measured in Pa and P0
is the pressure at sea level. P0 is approximately 101.325
kPa in normal conditions, but varies according to the time.
The altitude measure is affected by high frequency noise that
causes a measurement error. For this reason, the raw values of
altitude calculated by the altimeter are unusable in all systems
that require great accuracy. In order to reduce the error about
Hbaro =

The vertical displacement signal is obtained by a double integration of the dynamic acceleration component perpendicular
to the terrestrial surface. This acceleration must be referred
to the terrestrial reference system, called earth f rame, in
which the Z-axis is the vertical component of the Earths
surface while X-axis and Y-axis are the parallel components.
Therefore, to estimate vertical displacement we use the Z
component of the vector of acceleration referred to earth
f rame Eaz . A 3-axis accelerometer measures the acceleration
in three dimensions in space expressed in the coordinate system aligned with the sensor board and called sensor f rame.
It is necessary to obtain the acceleration components in the
earth f rame from the acceleration components measured in
the sensor f rame. This is possible rotating the acceleration
components Sa to the earth f rame by means of the rotation
matrix ESR [9]. In general, the rotation matrix of a rigid body
may be represented by Yaw, Pitch and Roll angles. Yaw is
a rotation around the Z-axis of the earth f rame, Pitch is a
rotation around the Y-axis of the earth f rame and Roll is a
rotation around the X-axis of the earth f rame. The sensor
fusion algorithm was implemented to compute the rotation
matrix to obtain an absolute measurement of orientation.
The AHRS filtering procedure combines information acquired
from a 3-axis accelerometer, a 3-axis gyroscope and a 3-axis
magnetometer in order to obtain an unique estimate of a rigid
body orientation.
Madgwick et al. [9] proposed an efficient algorithm applicable to AHRS which employs a three-dimension representation of orientation referring to earth f rame. In this algorithm
the rotation matrix ESR is described directly from quaternion
E
data
q [9]. Madgwicks algorithm uses a quaternion
S
representation to prevent problems such as gimbal lock that
may occur when using Euler angles. A quaternion q, is a
four-element vector constituted of one real part, q0 , and three
complex parts, q1 q2 and q3 . The representation of the Euler
angles starting from the components of a quaternion q can be
expressed by the following equations:
Y aw = atan2(2(q2 q3
P itch =

arctan( p

Roll = atan2(2(q1 q2

q0 q1 ), 2qo2
2(q1 q3
1

1 + 2q32 )

q0 q2 )

(q2 q3

q0 q3 ), 2qo2

q0 q2 ) 2

1 + 2q12 )

(2)
(3)
(4)

Our AHRS implementation is based on Madgwicks sensor


fusion algorithm in order to obtain the quaternions representation of the device orientation. The quaternions are used to
derive acceleration components in the earth f rame Ea from
the acceleration components measured in the sensor f rame
Sa as decribed in the following equation:
Ean =

S
q
E est,n

San

S
q
E est,n

Fig. 2. Block diagram of a basic complementary filter.

E
q
S est,n

San

E
q
S est,n

(5)

where is the quaternion product determined by the Hamilton


S
rule,
q
is the estimated orientation of the earth
E est,n 1
f rame relative to the sensor f rame at the (n-1)-th sample
and San is the vector of sensor readings at n-th instant.
Ea is subject to static (gravity) and dynamic (movements)
accelerations when the system is moves with non-constant
speed. Therefore:
Ea = Ed + Eg
(6)
where Ea is the vector of accelerometer, Eg and Ed are
the vectors of the static and dynamic accelerations expressed
in the earth f rame. We only use dynamic acceleration Ed
and specifically the component perpendicular to the terrestrial surface Edz . Therefore, we compute the estimate of
vertical displacement through double integration of dynamic
acceleration perpendicular to the terrestrial surface Edz . This
measure of vertical displacement is mainly influenced by the
low frequency noise.
D. Complementary Filter to Estimate Altitude
In order to achieve a good estimate of a single state
variable, a complementary filter is applied to fuse similar or
redundant data from different types of measurements of the
same signal [10]. This filter acts only on the several kinds of
noise associated with different fequency contents. In a twoinput system (shown in Fig. 2), an input provides information
with high frequency noise which will be low-pass filtered [11].
The other input provides information with low frequency noise
which will be high-pass filtered. If the low-pass and highpass filters are mathematical complements, then the output
of the filter is the complete reconstruction of the variable to
estimate, minus the noise associated to the sensors. Assume
that x1 = z + n1 and x2 = z + n2 are two measurements
of the same signal z, where n1 and n2 are respectively the
noise measurements and z is the estimated signal. Assume
also that n1 is a predominantly low frequency noise and n2 is
a predominantly high frequency noise. The output z is given
by:
b
Z(s)
= G1 (s)X1 (s) + G2 (s)X2 (s) =
= Z(s) + G1 (s)n1 (s) + G2 (s)n2 (s)

(7)

Fig. 3. Block diagram of the complementary filter used to estimate altitude


from dynamic acceleration component perpendicular to the terrestrial surface
and altitude calculated from altimeter sensor.

where G1 (s) is the transfer function of a high-pass filter and


G2 (s) = 1 G1 (s) of a low-pass filter. The output signal
is not distorted while the two noises n1 and n2 are filtered
according to the characteristics of the filters. In the estimation of the altitude the complementary filter is used to fuse
altitude computed from altimeter sensor Hbaro , and vertical
displacement computed from AHRS Edz . The basic scheme
of complementary filter for the estimation of the altitude is
shown in Fig. 3. The term n1 is the noise of the dinamic
acceleration component perpendicular to the terrestrial surface,
then n1 is the noise of vertical displacement estimated that
is predominantly low-frequency. The term n2 is the noise of
the altimeter measurements which is predominantly at highfrequency. The high pass filter G1 (s) is a second order filter
with transfer function equal to:
s2
(8)
s2 + as + b
Consequently G2 (s) is represented by a low pass filter with
transfer function complementary to G1 (s):
G1 =

as + b
(9)
s2 + as + b
The altitude estimated output by complementary filter is:
1
b
H(s)
= 2 G1 (s)Edz (s) + G2 (s)Hbaro (s) =
s
1
= 2 G1 (s)(ha (s) + n1 (s)) + G2 (s)(hb (s) + n2 (s)) =
s
= G1 (s)(ha (s) + n1 (s)) + G2 (s)(hb (s) + n2 (s)) =
G2 =

s2
as + b
n1 (s) + 2
n2 (s)
(10)
s2 + as + b
s + as + b
where a and b are the natural frequency and damping ratio,
respectively. The proposed complementary filter is shown in
b is obtained as follows:
Fig. 4 where the altitude estimated H
b = {(Hbaro H)k
b 1 + 1 [(Hbaro H)k
b 2 + ha + n1 ]} 1 (11)
H
s
s
b
Since Hbaro = hb + n2 and solving for H(s):
= h(s) +

b
b
b + H k1 + H k2 = hb k1 + n2 k1 + hb k2 + n2 k2 + ha + n1
H
2
2
2
s
s
s
s
s
s
s2 s2

Fig. 4. Diagram of the complementary filter used for the altitude estimation.

Fig. 5. Device allocation and relative reference system.

therefore:
s2
k1 s + k2
n1 (s) + 2
n2 (s)
+ k1 s + k2
s + k1 s + k2
(12)
that is equal to (10). The dinamic acceleration component
perpendicular to the terrestrial surface is doubly integrated
to produce a vertical displacement estimate. The altitude
b is differenced with the altitude measurement
estimated H
Hbaro , to produce an error signal which is fed back to produce
corrections in the estimates.
b
H(s)
= h(s) +

s2

III. E XPERIMENTAL VALIDATION AND R ESULTS


The goal of the presented work is the development of a
system for an accurate for real-time estimation of the altitude.
The performance of our device have been compared with
the system proposed by Y.K. Kim et al. [6] according to
their experimental protocol. As shown in Fig. 5 the prototype
of the device was placed on the waist of subjects which
performed the trial. Each subject starts from initial altitude
corresponding to f loor 0 and walks up the stairs up to
f loor 2. From f loor 2 the subject walks down the stairs up
to f loor 0. Therefore, the experimental trial is as follows:

Fig. 6. (a) Comparison of altitude estimation using raw altitude (grey line)
and complementary filter (black line) data in an experimental trial. (b) Zoom
of altitude estimation showing in greater detail the difference between the two
measures.

f loor 0 =) f loor 1 =) f loor 2 =) f loor 1 =) f loor 0.


In detail, the height difference between two consecutive floors
is 4 m. The subject has to stand still 15 s in each floor before
proceeding to the next floor. The study involved 15 volunteer
subjects aged between 24 and 29. Every subject repeats the
experimental trial five times. Altogether a total of 75 trials
were carried out. The result of a performed trial is shown
in Fig. 6. For each floor it was compared the average of the
standard deviation of raw altitude from the altimeter and the
altitude data estimated from complementary filter, calculated
over 75 trials. Table I shows the results of this comparison.
The data clearly points out that the average of the standard
deviation of the altitude obtained by filtering is considerably
lower than the one obtained by altimetric sensor alone. In
fact it goes from the maximum value of standard deviation
equal to 0.138m in the event of use the only altimeter, to a
maximum value equal to 0.049m in the event of use the filter.
Table II compares the results obtained between our device
and the system proposed by Y.K. Kim [6]. In particular, it
is noted that in our device the average standard deviation is

TABLE I
C OMPARISON OF THE ALTITUDE STANDARD DEVIATION AVERAGE VALUES
BETWEEN ALTIMETER RAW SIGNAL AND FILTERED SIGNAL , CALCULATED
OVER 75 TRIALS

Floor
1
2
3
2
1

STD [m]
using Altimeter
using Filter
0.138
0.043
0.123
0.039
0.125
0.049
0.122
0.029
0.127
0.049

TABLE II
C OMPARISON OF THE STANDARD DEVIATION AVERAGE VALUES BETWEEN
OUR DEVICE AND Y.K. K IM S PROPOSED DEVICE .

Floor
1
2
3
2
1

STD [m]
Our device
Kims device
0.043
0.120
0.039
0.130
0.049
0.140
0.029
0.100
0.049
0.150

between 0.049 and 0.29m whereas in the Y.K. Kims system it


is between 0.100 and 0.150m. Table II shows that the proposed
device performs better than one introduced by Y.K. Kim for
the experimental protocol carried out.
IV. C ONCLUSIONS
This paper presents an accurate system to estimate the
altitude of a rigid body on which it is applied, by fusing data
from four low-cost sensors. In particular, raw altitude signals
from an altimeter sensor and data of vertical displacement
computed through AHRS algorithm applied to the signals
coming from accelerometer, gyroscope and magnetometer
sensors, were fused using a complementary filter implemented
in our system. In order to evaluate the performance of the
proposed device, it has been compared with the currently existing system that presents the better performance for the same
technology according to its experimental protocols. During the
trials the protocol requires that each subject walks up and
down two planes of stairs. For each floor it was compared the
average of the standard deviation, calculated over 75 trials,
of altitude signal measured from our device with the altitude
data measured from the others systems present in literature.
The results show that our device performs better than currently
reported in the literature. In conclusion the proposed device
implements data fusion algorithms with low computational
load, and it can be used for all low cost applications that
require accurate real-time information about orientation and
altitude of a rigid body, such as human activity monitoring
systems, motion tracking systems and others. Particularly, the
device is useful in fall detection inertial systems in which the
vertical falls such as syncope are not easily detectable.

R EFERENCES
[1] K. Sagawa, T. Ishihara, A. Ina, and H. Inooka, Classification of
human moving patterns using air pressure and acceleration, in Industrial
Electronics Society, 1998. IECON98. Proceedings of the 24th Annual
Conference of the IEEE, vol. 2. IEEE, 1998, pp. 12141219.
[2] H. Sternberg, M. Fessele, and C. Honniger, Indoor navigation without
infrastructure-based local positioning system, in Proceedings of the 6th
International Symposium on Mobile Mapping Technology MMT, vol. 9,
2009.
[3] F. Bianchi, S. J. Redmond, M. R. Narayanan, S. Cerutti, and N. H.
Lovell, Barometric pressure and triaxial accelerometry-based falls
event detection, Neural Systems and Rehabilitation Engineering, IEEE
Transactions on, vol. 18, no. 6, pp. 619627, 2010.
[4] A. Pande, Y. Zeng, A. K. Das, P. Mohapatra, S. Miyamoto, E. Seto,
E. K. Henricson, and J. J. Han, Energy expenditure estimation with
smartphone body sensors, in Proceedings of the 8th International Conference on Body Area Networks. ICST (Institute for Computer Sciences,
Social-Informatics and Telecommunications Engineering), 2013, pp. 8
14.
[5] A. M. Sabatini and V. Genovese, A stochastic approach to noise
modeling for barometric altimeters, Sensors, vol. 13, no. 11, pp. 15 692
15 707, 2013.
[6] Y.-K. Kim, S.-H. Choi, H.-W. Kim, and J.-M. Lee, Performance
improvement and height estimation of pedestrian dead-reckoning system
using a low cost mems sensor, in Control, Automation and Systems
(ICCAS), 2012 12th International Conference on. IEEE, 2012, pp.
16551660.
[7] M. Tanigawa, H. Luinge, L. Schipper, and P. Slycke, Drift-free dynamic
height sensor using mems imu aided by mems pressure sensor, in
Positioning, Navigation and Communication, 2008. WPNC 2008. 5th
Workshop on. IEEE, 2008, pp. 191196.
[8] J. Parviainen, J. Kantola, and J. Collin, Differential barometry in
personal navigation, in Position, Location and Navigation Symposium,
2008 IEEE/ION. IEEE, 2008, pp. 148152.
[9] S. O. Madgwick, A. J. Harrison, and R. Vaidyanathan, Estimation
of imu and marg orientation using a gradient descent algorithm, in
Rehabilitation Robotics (ICORR), 2011 IEEE International Conference
on. IEEE, 2011, pp. 17.
[10] W. T. Higgins, A comparison of complementary and kalman filtering,
IEEE Transactions on Aerospace and Electronic Systems, vol. 11, no. 3,
pp. 321325, 1975.
[11] R. Mahony, T. Hamel, and J.-M. Pflimlin, Nonlinear complementary
filters on the special orthogonal group, Automatic Control, IEEE
Transactions on, vol. 53, no. 5, pp. 12031218, 2008.