An Accurate Device for Real-Time AltitudeEstimation Using Data Fusion Algorithms

Paola Pierleoni, Alberto Belli, Lorenzo Palma, Luca Pernini and Simone ValentiDepartment of Information Engineering

Marche Polytechnic UniversityAncona, Italy

Email: s.valenti@univpm.it

Abstract—This paper presents an accurate system to estimatethe altitude of a rigid body by fusing data from four low-costsensors such as an accelerometer, a gyroscope, a magnetometerand an altimeter. Usually a MEMS altimeter barometric sensorallows to obtain the altitude signal from measures of atmosphericpressure and temperature but these measures are affected bynoise that causes a significant error in the calculated altitudevalues. In order to get an accurate estimation of the altitude,in this work a complementary filter is used to fuse the rawsignal of the altitude obtained from barometer sensor and verticaldisplacement signal calculated through a data fusion algorithmapplied to the signals of the other three sensors. In order toevaluate the performance in human activity monitoring applica-tions, the proposed device has been tested and compared withthe system that currently presents the better performance forthe same technology according to its experimental protocols. Theresults show that our device exceeds the performance providedby the currently systems reported in literature.

I. INTRODUCTION

Information about the altitude of a rigid body is used invarious applications such as in human activity monitoring sys-tems [1], indoor navigation systems [2], fall detection systems[3], smartphones applications [4], bike computers and others.The MEMS altimeter barometric sensor allows to calculatethe altitude signal from measurements of atmospheric pressureand temperature. These measures are affected by noise thatcauses wide and rapid fluctuations of the altitude signal [5]. Inorder to obtain an improved altitude estimation, measures fromaltimeter sensor are fused with additional altitude informationderivated from the GPS [2] or accelerometer signals [6], [7]using Kalman filter. However, Kalman filter is characterizedby a significant complexity and demands large computationalload for its implementation. Moreover in indoor applications,information about the vertical position of a body can not usethe GPS signal because it might be too weak.

II. METHODS & MATERIALS

Our work presents an accurate device for real-time altitudeestimation mainly composed of a triaxial MEMS accelerome-ter, a triaxial MEMS gyroscope, a triaxial magnetometer anda MEMS altimeter sensor. The first three sensors implementan Attitude and Heading Reference System (AHRS) andare used to estimate the orientation of the device. Altime-ter barometric sensor is used to calculate the raw altitudefrom measurements of atmospheric pressure and temperature.

Through complementary filter, the altimeter raw data arefused with vertical displacement measurements obtained bydouble-integration of dynamic acceleration component per-pendicular to the terrestrial surface. In order to derive thisuseful acceleration component we have used the estimationof orientation computed by AHRS algorithm implementedinto the device. Our implementation of the complementaryfilter together with AHRS algorithm is particularly suitablefor real-time applications into low cost microcontrollers and itsignificantly reduces the computational effort associated withthe conventional Kalman-based approaches. Essentially, theproposed system performs data fusion algorithms with lowcomputational load and has the benefit of being low cost andeasy to wear.

A. Hardware of the Proposed DeviceThe device includes an accelerometer, a gyroscope, a mag-

netometer and an altimeter that communicates with the MCUvia I2C bus. A RF Bluetooth module interfaces the board withexternal devices as personal computer, smartphone and tablet.SD card memory allows data storing. The block diagramof the system is shown in Fig. 1 and the main features ofeach component of the device are described in the followingsubsubsections.

1) MCU Module: ATmega328 (ATMEL, USA) is a low-power single chip 8-bit microcontroller belongs to themegaAVR series. This module combines 32-kb ISP flashmemory with read-while-write capabilities, 1-kb EEPROM, 2-kb SRAM, 23 general purpose I/O lines, 32 general purposeworking registers, internal and external interrupts, serial pro-grammable USART, byte-oriented a 2-wire serial interface, aSPI serial port, 6-channel 10-bit A/D converter, programmablewatchdog timer with internal oscillator, and five softwareselectable power saving modes. The device operates in therange 1.8-5.5 V.

2) Accelerometer and Gyroscope: MPU-6050 (InvenSenseInc., USA) combines a MEMS 3-axis gyroscope and a MEMS3-axis accelerometer on the same silicon die together. Thegyroscope is a tri-axis angular rate sensor with a sensitivityup to 131 LSBs/dps and a full-scale ranges of ±250, ±500,±1000, and ±2000dps. The accelerometer is a tri-axis sensorwith a programmable full scale range of ±2g, ±4g, ±8g and±16g. MPU-6050 communicates with MCU via I2C bus.

978-1-4799-2280-2/14/$31.00 ©2014 IEEE

Fig. 1. Block diagram of the proposed device.

3) Magnetometer: HMC5883L (Honeywell, USA) is a 3-axis magnetoresistive sensor designed for low-field magneticsensing. It has an embedded 12-bit ADC achieving 4 mGfield resolution in ±8 G fields. The module measures boththe direction and the magnitude of Earth’s 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 sensorswith SPI and I2C bus interface. It provides a precise digital24 bit pressure and temperature values and different operationmodes that allow the user to optimize the conversion speedand the current consumption.

5) Wireless Module: HC-05 module (Honeywell, USA) isan easy to use Bluetooth SPP (Serial Port Protocol) module,designed for transparent wireless serial connection setup. It isa small low-power transceiver ideal for embedded applications.

6) Storage Module: MicroSD card is used to providethe device with mass-storage capability. Communication withmicroSD card is achieved over SPI interface.

The data from sensors are sampled at 50 Hz. A calibrationprocedure of the device was performed offline to compensatesystematic errors in the AHRS.

B. Altimeter to Calculate AltitudeThe altimeter sensor allows to calculate altutitude H

baro

,from temperature and pressure measurements [8]:

Hbaro

=(T + 273.15)

0.0065(1� (P/P0)

(0.19)) (1)

where Hbaro

is measured in meters, T is the temperaturemeasured in �C, P is the pressure measured in Pa and P0

is the pressure at sea level. P0 is approximately 101.325kPa in normal conditions, but varies according to the time.The altitude measure is affected by high frequency noise thatcauses a measurement error. For this reason, the raw values ofaltitude calculated by the altimeter are unusable in all systemsthat require great accuracy. In order to reduce the error about

this measure, it is therefore necessary an appropriate filteringoperation of the raw altitude signal.

C. AHRS to Estimate Vertical DisplacementThe vertical displacement signal is obtained by a double in-

tegration of the dynamic acceleration component perpendicularto the terrestrial surface. This acceleration must be referredto the terrestrial reference system, called earth frame, inwhich the Z-axis is the vertical component of the Earth’ssurface while X-axis and Y-axis are the parallel components.Therefore, to estimate vertical displacement we use the Zcomponent of the vector of acceleration referred to earthframe E

az . A 3-axis accelerometer measures the accelerationin three dimensions in space expressed in the coordinate sys-tem aligned with the sensor board and called sensor frame.It is necessary to obtain the acceleration components in theearth frame from the acceleration components measured inthe sensor frame. This is possible rotating the accelerationcomponents S

a

to the earth frame by means of the rotationmatrix ESR [9]. In general, the rotation matrix of a rigid bodymay be represented by Yaw, Pitch and Roll angles. Yaw isa rotation around the Z-axis of the earth frame, Pitch is arotation around the Y-axis of the earth frame and Roll is arotation around the X-axis of the earth frame. The sensorfusion algorithm was implemented to compute the rotationmatrix to obtain an absolute measurement of orientation.The AHRS filtering procedure combines information acquiredfrom a 3-axis accelerometer, a 3-axis gyroscope and a 3-axismagnetometer in order to obtain an unique estimate of a rigidbody orientation.

Madgwick et al. [9] proposed an efficient algorithm appli-cable to AHRS which employs a three-dimension representa-tion of orientation referring to earth frame. In this algorithmthe rotation matrix ESR is described directly from quaternion

data ES

q [9]. Madgwick’s algorithm uses a quaternion

representation to prevent problems such as gimbal lock thatmay occur when using Euler angles. A quaternion q, is afour-element vector constituted of one real part, q0, and threecomplex parts, q1 q2 and q3. The representation of the Eulerangles starting from the components of a quaternion q can beexpressed by the following equations:

Y aw = atan2(2(q2q3 � q0q1), 2q2o

� 1 + 2q23) (2)

Pitch = �arctan(2(q1q3 � q0q2)p1� (q2q3 � q0q2)2

) (3)

Roll = atan2(2(q1q2 � q0q3), 2q2o

� 1 + 2q21) (4)

Our AHRS implementation is based on Madgwick’s sensorfusion algorithm in order to obtain the quaternions represen-tation of the device orientation. The quaternions are used toderive acceleration components in the earth frame E

a

fromthe acceleration components measured in the sensor frameSa

as decribed in the following equation:

Ean =

SE

qest,n�1 ⌦ S

an ⌦ SE

q⇤est,n�1 =

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

=ES

q⇤est,n�1 ⌦ S

an ⌦ ES

qest,n�1 (5)

where ⌦ is the quaternion product determined by the Hamilton

rule, SE

qest,n�1 is the estimated orientation of the earth

frame relative to the sensor frame at the (n-1)-th sampleand S

an is the vector of sensor readings at n-th instant.E

a

is subject to static (gravity) and dynamic (movements)accelerations when the system is moves with non-constantspeed. Therefore:

Ea

= Ed

+ Eg

(6)

where Ea

is the vector of accelerometer, Eg

and Ed

arethe vectors of the static and dynamic accelerations expressedin the earth frame. We only use dynamic acceleration E

d

and specifically the component perpendicular to the terres-trial surface E

dz . Therefore, we compute the estimate ofvertical displacement through double integration of dynamicacceleration perpendicular to the terrestrial surface E

dz . Thismeasure of vertical displacement is mainly influenced by thelow frequency noise.

D. Complementary Filter to Estimate AltitudeIn order to achieve a good estimate of a single state

variable, a complementary filter is applied to fuse similar orredundant data from different types of measurements of thesame signal [10]. This filter acts only on the several kinds ofnoise associated with different fequency contents. In a two-input system (shown in Fig. 2), an input provides informationwith high frequency noise which will be low-pass filtered [11].The other input provides information with low frequency noisewhich will be high-pass filtered. If the low-pass and high-pass filters are mathematical complements, then the outputof the filter is the complete reconstruction of the variable toestimate, minus the noise associated to the sensors. Assumethat x1 = z + n1 and x2 = z + n2 are two measurementsof the same signal z, where n1 and n2 are respectively thenoise measurements and z is the estimated signal. Assumealso that n1 is a predominantly low frequency noise and n2 isa predominantly high frequency noise. The output z is givenby:

bZ(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 altitudefrom dynamic acceleration component perpendicular to the terrestrial surfaceand altitude calculated from altimeter sensor.

where G1(s) is the transfer function of a high-pass filter andG2(s) = 1 � G1(s) of a low-pass filter. The output signalis not distorted while the two noises n1 and n2 are filteredaccording to the characteristics of the filters. In the estima-tion of the altitude the complementary filter is used to fusealtitude computed from altimeter sensor H

baro

, and verticaldisplacement computed from AHRS E

dz . The basic schemeof complementary filter for the estimation of the altitude isshown in Fig. 3. The term n1 is the noise of the dinamicacceleration component perpendicular to the terrestrial surface,then n1 is the noise of vertical displacement estimated thatis predominantly low-frequency. The term n2 is the noise ofthe altimeter measurements which is predominantly at high-frequency. The high pass filter G1(s) is a second order filterwith transfer function equal to:

G1 =s2

s2 + as+ b(8)

Consequently G2(s) is represented by a low pass filter withtransfer function complementary to G1(s):

G2 =as+ b

s2 + as+ b(9)

The altitude estimated output by complementary filter is:

bH(s) =1

s2G1(s)Edz (s) +G2(s)Hbaro

(s) =

=1

s2G1(s)(ha

(s) + n1(s)) +G2(s)(hb

(s) + n2(s)) =

= G1(s)(ha

(s) + n1(s)) +G2(s)(hb

(s) + n2(s)) =

= h(s) +s2

s2 + as+ bn1(s) +

as+ b

s2 + as+ bn2(s) (10)

where a and b are the natural frequency and damping ratio,respectively. The proposed complementary filter is shown inFig. 4 where the altitude estimated bH is obtained as follows:

bH = {(Hbaro

� bH)k1+1

s[(H

baro

� bH)k2+ ha

+ n1]}1

s(11)

Since Hbaro

= hb

+ n2 and solving for bH(s):

bH+bHsk1+

bHs2

k2 =hb

sk1+

n2

sk1+

hb

s2k2+

n2

s2k2+

ha

s2+

n1

s2

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

Fig. 5. Device allocation and relative reference system.

therefore:

bH(s) = h(s) +s2

s2 + k1s+ k2n1(s) +

k1s+ k2s2 + k1s+ k2

n2(s)

(12)that is equal to (10). The dinamic acceleration componentperpendicular to the terrestrial surface is doubly integratedto produce a vertical displacement estimate. The altitudeestimated bH is differenced with the altitude measurementH

baro

, to produce an error signal which is fed back to producecorrections in the estimates.

III. EXPERIMENTAL VALIDATION AND RESULTS

The goal of the presented work is the development of asystem for an accurate for real-time estimation of the altitude.The performance of our device have been compared withthe system proposed by Y.K. Kim et al. [6] according totheir experimental protocol. As shown in Fig. 5 the prototypeof the device was placed on the waist of subjects whichperformed the trial. Each subject starts from initial altitudecorresponding to floor 0 and walks up the stairs up tofloor 2. From floor 2 the subject walks down the stairs upto floor 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) Zoomof altitude estimation showing in greater detail the difference between the twomeasures.

floor 0 =) floor 1 =) floor 2 =) floor 1 =) floor 0.In detail, the height difference between two consecutive floorsis 4 m. The subject has to stand still 15 s in each floor beforeproceeding to the next floor. The study involved 15 volunteersubjects aged between 24 and 29. Every subject repeats theexperimental trial five times. Altogether a total of 75 trialswere carried out. The result of a performed trial is shownin Fig. 6. For each floor it was compared the average of thestandard deviation of raw altitude from the altimeter and thealtitude data estimated from complementary filter, calculatedover 75 trials. Table I shows the results of this comparison.The data clearly points out that the average of the standarddeviation of the altitude obtained by filtering is considerablylower than the one obtained by altimetric sensor alone. Infact it goes from the maximum value of standard deviationequal to 0.138m in the event of use the only altimeter, to amaximum value equal to 0.049m in the event of use the filter.Table II compares the results obtained between our deviceand the system proposed by Y.K. Kim [6]. In particular, itis noted that in our device the average standard deviation is

TABLE ICOMPARISON OF THE ALTITUDE STANDARD DEVIATION AVERAGE VALUESBETWEEN ALTIMETER RAW SIGNAL AND FILTERED SIGNAL, CALCULATED

OVER 75 TRIALS

STD [m]Floor using Altimeter using Filter

1 0.138 0.0432 0.123 0.0393 0.125 0.0492 0.122 0.0291 0.127 0.049

TABLE IICOMPARISON OF THE STANDARD DEVIATION AVERAGE VALUES BETWEEN

OUR DEVICE AND Y.K. KIM’S PROPOSED DEVICE.

STD [m]Floor Our device Kim’s device

1 0.043 0.1202 0.039 0.1303 0.049 0.1402 0.029 0.1001 0.049 0.150

between 0.049 and 0.29m whereas in the Y.K. Kim’s system itis between 0.100 and 0.150m. Table II shows that the proposeddevice performs better than one introduced by Y.K. Kim forthe experimental protocol carried out.

IV. CONCLUSIONS

This paper presents an accurate system to estimate thealtitude of a rigid body on which it is applied, by fusing datafrom four low-cost sensors. In particular, raw altitude signalsfrom an altimeter sensor and data of vertical displacementcomputed through AHRS algorithm applied to the signalscoming from accelerometer, gyroscope and magnetometersensors, were fused using a complementary filter implementedin our system. In order to evaluate the performance of theproposed device, it has been compared with the currently ex-isting system that presents the better performance for the sametechnology according to its experimental protocols. During thetrials the protocol requires that each subject walks up anddown two planes of stairs. For each floor it was compared theaverage of the standard deviation, calculated over 75 trials,of altitude signal measured from our device with the altitudedata measured from the others systems present in literature.The results show that our device performs better than currentlyreported in the literature. In conclusion the proposed deviceimplements data fusion algorithms with low computationalload, and it can be used for all low cost applications thatrequire accurate real-time information about orientation andaltitude of a rigid body, such as human activity monitoringsystems, motion tracking systems and others. Particularly, thedevice is useful in fall detection inertial systems in which thevertical falls such as syncope are not easily detectable.

REFERENCES

[1] K. Sagawa, T. Ishihara, A. Ina, and H. Inooka, “Classification ofhuman moving patterns using air pressure and acceleration,” in IndustrialElectronics Society, 1998. IECON’98. Proceedings of the 24th AnnualConference of the IEEE, vol. 2. IEEE, 1998, pp. 1214–1219.

[2] H. Sternberg, M. Fessele, and C. Honniger, “Indoor navigation withoutinfrastructure-based local positioning system,” in Proceedings of the 6thInternational 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 fallsevent detection,” Neural Systems and Rehabilitation Engineering, IEEETransactions on, vol. 18, no. 6, pp. 619–627, 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 withsmartphone body sensors,” in Proceedings of the 8th International Con-ference 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 noisemodeling 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, “Performanceimprovement and height estimation of pedestrian dead-reckoning systemusing a low cost mems sensor,” in Control, Automation and Systems(ICCAS), 2012 12th International Conference on. IEEE, 2012, pp.1655–1660.

[7] M. Tanigawa, H. Luinge, L. Schipper, and P. Slycke, “Drift-free dynamicheight sensor using mems imu aided by mems pressure sensor,” inPositioning, Navigation and Communication, 2008. WPNC 2008. 5thWorkshop on. IEEE, 2008, pp. 191–196.

[8] J. Parviainen, J. Kantola, and J. Collin, “Differential barometry inpersonal navigation,” in Position, Location and Navigation Symposium,2008 IEEE/ION. IEEE, 2008, pp. 148–152.

[9] S. O. Madgwick, A. J. Harrison, and R. Vaidyanathan, “Estimationof imu and marg orientation using a gradient descent algorithm,” inRehabilitation Robotics (ICORR), 2011 IEEE International Conferenceon. IEEE, 2011, pp. 1–7.

[10] W. T. Higgins, “A comparison of complementary and kalman filtering,”IEEE Transactions on Aerospace and Electronic Systems, vol. 11, no. 3,pp. 321–325, 1975.

[11] R. Mahony, T. Hamel, and J.-M. Pflimlin, “Nonlinear complementaryfilters on the special orthogonal group,” Automatic Control, IEEETransactions on, vol. 53, no. 5, pp. 1203–1218, 2008.