06316172

590 IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT, VOL. 62, NO. 3, MARCH 2013

A Double-Stage Kalman Filter for OrientationTracking With an Integrated Processor in 9-D IMU

Simone Sabatelli, Marco Galgani, Luca Fanucci, and Alessandro Rocchi

Abstract—This paper presents an application-specific inte-grated processor for an angular estimation system that workswith 9-D inertial measurement units. The application-specificinstruction-set processor (ASIP) was implemented on field-programmable gate array and interfaced with a gyro-plus-accelerometer 6-D sensor and with a magnetic compass. Outputdata were recorded on a personal computer and also used toperform a live demo. During system modeling and design, it waschosen to represent angular position data with a quaternion and touse an extended Kalman filter as sensor fusion algorithm. For thispurpose, a novel two-stage filter was designed: The first stage usesaccelerometer data, and the second one uses magnetic compassdata for angular position correction. This allows flexibility, lesscomputational requirements, and robustness to magnetic fieldanomalies. The final goal of this work is to realize an upgradedapplication-specified integrated circuit that controls the microelec-tromechanical systems (MEMS) sensor and integrates the ASIP.This will allow the MEMS sensor gyro plus accelerometer and theangular estimation system to be contained in a single package; thissystem might optionally work with an external magnetic compass.

Index Terms—Angular position, application-specific instruc-tion-set processor (ASIP), inertial measurement unit (IMU),Kalman filter, orientation tracking, quaternions, sensor fusion.

I. INTRODUCTION

CURRENTLY, microelectromechanical systems (MEMS)sensors are widely used in a variety of consumer ap-

plications due to their low cost, small size, and low powerconsumption. In industrial research, there is widespread interestin additional features and improved precision from MEMS sys-tems. In scientific literature, a lot of attention is paid to sensorfusion algorithms, because they allow raw data from multiplesensors to be processed to obtain additional information orimproved precision.

A great amount of research is done for improving gyro ac-curacy for the use of inertial measurement units (from now on,IMUs) in high-precision applications, such as dead reckoningand pedestrian indoor navigation. These systems are based on

Manuscript received March 27, 2012; revised May 4, 2012; acceptedJune 18, 2012. Date of publication September 28, 2012; date of current versionFebruary 5, 2013. The Associate Editor coordinating the review process for thispaper was Dr. Deniz Gurkan.

S. Sabatelli and L. Fanucci are with the Department of Information En-gineering, University of Pisa, 56126 Pisa, Italy (e-mail: [email protected]).

M. Galgani is with the On-Board Payload Data Processing Section, EuropeanSpace Research and Technology Centre, European Space Agency, 2200 AGNoordwijk, The Netherlands.

A. Rocchi is with the SensorDynamics AG, 56123 Pisa, Italy.Color versions of one or more of the figures in this paper are available online

at http://ieeexplore.ieee.org.Digital Object Identifier 10.1109/TIM.2012.2218692

IMU units that estimate the angular position. The expectedgravity vector is subtracted from the measured accelerations,and the position of the IMU is calculated with a double inte-gration. The main problem is that errors grow rapidly with thedouble integration, so that the system is not reliable for morethan a few seconds without the aid of GPS signal or withoutspeed or movement constraints. For example, in a navigationapplication, the movements of the sensor could be calculatedwithin the constraints of a building map; in an odometer andfoot counter, if the sensor is placed on a shoe, the accelerometerwill measure a spike when the foot is landed and it can be as-sumed that, at this instant, the speed is zero, so that integrationerrors can be periodically corrected.

In a work dealing with an existing dead reckoning unit, [1]the analysis showed that the main error source in such systemsis not the accelerometer noise but the incorrect gravity vector(because of inaccurate angle estimation) that is subtractedfrom measured accelerations. This work is thus focused on animproved angle estimation system. The main problem of thissystem is that the use of an integrator for gyro data leads tothe sum of gyro errors during time, so that a correction factormust be introduced. In particular, it is known from theory thatgyro angle random walk, the Gaussian white noise, introducesan error in angular estimation that grows with the square rootof time, while bias instability and constant biases on gyro dueto calibration errors or to thermal or soldering effects give anerror that grows linearly with time.

A number of angular estimation systems have been previ-ously published. Most of the papers use quaternions to representthe angular position, because they are more flexible than Eulerangles to represent the angular position; moreover, with theuse of quaternions, there is no problem of angular singularities,and the system can be better linearized. If needed, quaternionscan be easily converted to other rotation representation meth-ods, like a rotational matrix or a sequence of Euler angles.To eliminate the effects of drift in angular estimation dueto gyro errors, all studies used some kind of sensor fusionalgorithm. The Kalman filter is the most commonly used. In [2],an extended Kalman filter is used with three rate gyros andthree accelerometers, and the state equation is composed ofthe quaternion and also of the angular velocity and the gyrodrift. The proposed system has a good estimation of roll andpitch angles but only a low correction on yaw angle. In orderto have a complete correction for the yaw angle, a magneticcompass is needed. Indeed, in [3] and [4], a complete magnetic,angular gate, and gravity sensor is used for angle estimation. Tosimplify the design of the Kalman filter, in these two works, anadditional algorithm is proposed to estimate the quaternion that

0018-9456/$31.00 © 2012 IEEE

SABATELLI et al.: KALMAN FILTER FOR ORIENTATION TRACKING WITH INTEGRATED PROCESSOR IN IMU 591

Fig. 1. Sensor fusion algorithm principle.

represents the angular position from accelerometer and mag-netic compass data, so that a linear Kalman filter could be used.In particular, a Gauss–Newton method is proposed in [3], and aQUaternion ESTimate (QUEST) algorithm is proposed in [4].An adaptive gain for the Kalman filter is used in [5] to adjust theangular correction when the sensor is in high acceleration modeor in a quiet state. Instead of the extended Kalman filter, [6] usesan unscented Kalman filter because this filter is deemed to bemore accurate and less costly to implement. However, in [7], acomparison between the extended and the unscented filters isdone, and the resulting precision is found comparable, althoughthe unscented filter requires much more computational time.

All the cited previous works use discrete sensor systems,with separate sensors for gyro, accelerometer, and magneticcompass, and the filter algorithm is processed on a microcon-troller or a personal computer (PC). A commercial exampleof this kind of angle estimation system can be found in [8],where the angular estimation is carried out with the use of threeseparate sensors and a microcontroller. In some models, it isalso possible to interface this system with a GPS for a deadreckoning system.

In this paper, we focused on the realization of a completesystem in a single package. For this reason, starting fromthe existing prototype of SensorDynamics SD746 6-D IMU, asystem was developed that can use only gyro and accelerometerdata to run the filter and may be optionally connected with anexternal magnetic compass. The two-stage filter offers greateroperational flexibility, as it is possible to switch on or offthe second stage to include or exclude the magnetic compasscorrection. Using accelerometer data, it is possible to correctlyestimate roll and pitch angles only, while with the magneticcompass, the yaw angle is correctly estimated as well. Thisapproach also offers simplification: The division of the systemin two stages allows operating with smaller matrices, with lesscomputing power required. This two-stage filter also allows theseparation of the effects of a magnetic anomaly on roll andpitch angles’ estimation precision, because, in this case, onlythe yaw angle estimation is affected [11]. Our final goal is todevelop and integrate this angular estimation system within theapplication-specified integrated circuit (ASIC) that controls thegyro and accelerometer sensors; the external magnetic compass

may be connected by an Inter-Integrated Circuit or two-wireinterface (I2C) bus. To realize this, we have an area constraintof 0.5 mm2.

This paper is organized as follows. After this Introduction,in Section II, the mathematical theory used for the angularestimation system model is explained, Section III describesthe design and the architecture of the application-specificinstruction-set processor (ASIP), and finally, in Section IV,the field-programmable gate array (FPGA) prototype and testresults are presented.

II. THEORY OF SYSTEM MODELING

The system model was developed using MathWorksSimulink. This allows fast system development and testing bothwith artificial data sets and real acquired data.

The system has three different types of input data, as shownin Fig. 1: the gyro data measuring the angular rate in degreesper second on each of the three axes, the accelerometer datameasuring acceleration in gram units, and the magnetic com-pass data measuring the magnetic field in gauss. It will besubsequently explained how the gyro data are used as input toupdate the angular position; meanwhile, the accelerometer andmagnetic compass data are used as fixed references to correctthe errors in the angular position estimation.

According to the Kalman filter theory (see [14]), it is neces-sary to define a discrete-time state equation that describes theevolution of the system state xk, starting from the system stateat the previous step xk−1 with the evolution law described bythe matrix Ak and responding to some system inputs uk withthe evolution law Bk

x−k = Akxk−1 +Bkuk. (1)

Equation (1) will give the so-called “a priori” system stateestimation. For this reason, the system state xk has the super-script “minus,” while the hat represents the fact that the realsystem state is unknown and the Kalman filter is providing anestimation.

Only the quaternion representing the angular position assystem state was used in the state equation. The addition of


other variables does not significantly increase the precisionof the angular estimation, although it requires significantlygreater processing power. The inclusion of extra variables inthe system state leads to bigger matrix operations. Using themcould possibly account for the gyro bias, but according to oursimulations, the resulting increase of precision is less than 1◦.

The angular position is represented using a quaternion q

q = [q0, q1, q2, q3]T. (2)

A quaternion is composed of a real number and a vector: q0represents the real number, and v = [q1, q2, q3]

T represents thevector.

A rotation is defined by a rotation axis and by the angle ofrotation α around this axis. The vector part v represents the axisof rotation, while the real part represents the angle of rotation αaccording to

q = cos(α2

)+ sin

(α2

)∗ v. (3)

In order to correctly represent a rotation, the quaternion musthave unit norm.

The quaternion norm is calculated with

|q| =√

q20 + q21 + q22 + q23 . (4)

Standard formulas were used to convert the angular rotationrepresentation from the quaternion to the Euler angle form. TheXY Z sequence of Euler angles was chosen, and the roll, pitch,and yaw angles are given by the following formulas:

φ = atan2(2(q2q3 + q0q1),

(q20 − q21 − q22 + q23

))(5)

θ = asin

(2(q0q2 − q1q3)

(q20 + q21 + q22 + q23)

)(6)

ψ = atan2(2(q1q2 + q0q3),

(q20 + q21 − q22 − q23

)). (7)

The state equation in the time-continuous form is given by

qbn =1

2Ωn

nbqbn (8)

where qbn is the quaternion representing the rotation of the bodyframe, united to the IMU sensor, with respect to the inertialn-frame, and Ωn

nb is the rotational matrix, derived from thequaternion’s properties (see [12] and [13])

Ωnnb =

⎡⎢⎣

0 −ωx −ωy −ωz

ωx 0 ωz −ωy

ωy −ωz 0 ωx

ωz ωy −ωx 0

⎤⎥⎦ . (9)

The angular velocities ωx, ωy , and ωz measured by the gyroare used to fill the matrix (9). In this particular system, there isno explicit external input uk with the evolution law B [see (1)].The gyro data are implicit inputs because they are used to deter-mine the matrix deriving the evolution of the system from theprevious state. A time-varying ATC = (1/2)Ωn

nb matrix wasthus obtained. For the digital Kalman filter implementation, it isnecessary to transform (8) from this time-continuous version to

a time-discrete one. By making explicit the derivative operation,the continuous-time system can be written as

qbn = limT→0qbn(t+ T )− qbn(t)

T= ATCq

bn(t). (10)

In order to transform this equation to a discrete-time one,the time step T between each execution of the algorithm in thedigital system is used. In this manner, we obtain

qbn(t+ T ) = qbn(t) +ATCqbn(t)T = (I +ATCT )q

bn(t). (11)

The discrete-time matrix Ak has the form shown in

Ak = (I +ATCT ) =

(I +

1

2Ωn

nbT

). (12)

The quaternion norm is not preserved in the state equation.As the angle position can be correctly represented only withunit quaternions, a normalization unit was introduced to ensuredata integrity.

This first part, described earlier in (1), is the predictive, or“a priori,” equation that updates the angular position usinggyro data only. It is now necessary to define the correctionequation of the Kalman filter. In our system, there are twocorrection equations, one for each stage: the first operating withaccelerometer data and the second operating with magneticcompass data. Using the Kalman filter theory, the correctionequation results as in

xk = x−k +Kk

(zk −Hx−

k

)(13)

where zk represents the actual measurement, which, in theproposed system, is the accelerometer or the magnetic compassdata. Hx−

k is the expected measurement that is calculated fromthe “a priori” system state. The difference between the real andthe expected measurements is called the residual. The residualmust be weighted with the Kk gain to obtain the correctionfactor and to calculate the “a posteriori” estimation of thesystem state xk, which is the least square solution to thisprobabilistic problem. In our system, it is not possible to use thelinear relationship Hx−

k to calculate the estimated gravity or themagnetic field, but it is necessary to use a nonlinear relationshiph(x−

k ) and the extended Kalman filter. This is because we havesecond-degree polynomials [see (17) and (21)].

The calculation of the Kalman gain Kk is a quite complextask, involving many matrix operations. First, it is necessary tocalculate the “a priori” error covariance P−

k that represents theerror in the state estimation using the “a priori” state equationonly

P−k = AkPk−1A

Tk +Qk−1. (14)

In (14), the Pk−1 matrix is the “a posteriori” error covariancematrix at the previous filter iteration [see (16)], while Qk−1 isthe process noise covariance matrix. In this system, the Qk−1

covariance matrix directly depends on gyro noise and othergyro error sources, because the system evolution is describedby the Ak matrix that contains the gyro data terms. However,the Qk−1 value does not represent the gyro noise but is relatedto the noise during the system evolution.


In the extended Kalman filter, the Kalman gain is calculatedwith

Kk = P−k HT

k

(HkP

−k HT

k + VkRkVTk

)−1(15)

where Rk is the measurement noise covariance matrix, di-rectly depending from the noise of the accelerometer and of themagnetic sensor, plus other error sources that are consideredas noise. Hk and Vk are the Jacobian matrices of the partialderivatives with respect to the quaternion and to the noise of thenonlinear equations h1 and h2 [(17) and (21)]. These equationsrelate the quaternion to the estimated gravity and the estimatedmagnetic field. Because of the nonlinearity of these equations,the use of an extended Kalman filter is needed.

At the end, it is necessary to calculate the “a posteriori” errorcovariance matrix Pk to be used in the subsequent filter iteration

Pk = (I −KkHk)P−k (16)

Now that the Kalman filter theory is explained, it is possibleto see how it is applied to the system described in this paper.The first correction stage uses data from the accelerometersto correct the system state: The expected gravity vector iscalculated from the estimated angular position and is subtractedfrom the value measured from the accelerometer, obtaining theso-called residual. The estimated gravity vector g is calculatedusing the direction cosine matrix Rb

n, assuming the g-forceacceleration |g| constant

h1(qk)=g=Rbn

⎡⎣ 0

0|g|

⎤⎦= |g|

⎡⎣ 2q1q3−2q0q2

2q0q1+2q2q3q20−q21−q22+q23

⎤⎦ (17)

Rbn=

⎡⎣q20+q21−q22−q23 2q1q2+2q0q3 2q1q3−2q0q2

2q1q2−2q0q3 q20−q21+q22−q23 2q2q3+2q0q12q1q3+2q0q2 2q2q3−2q0q1 q20−q21−q22+q23

⎤⎦.

(18)

The measured gravity suffers from significant errors becausethe accelerometers measure not only gravity but also the ex-ternal accelerations if the system is in a dynamic state. Forthis reason, the residual must be weighted with the Kalmangain Kk1, a coefficient that is calculated from the statistics ofthe noise covariance matrices of the system. To calculate theKalman gain, it is necessary to calculate the Jacobian matrixHk1

Hk1 =∂h1[i]

∂q[j]=

⎡⎣−2q2 2q3 −2q0 2q1

2q1 2q0 2q3 2q22q0 −2q1 −2q2 2q3

⎤⎦ . (19)

The noise of the accelerometer (and also of the magneticcompass) is considered noncorrelated with the current angularposition. Thus, the Jacobian matrix Vk is an identity matrix.

With the gravity vector measurement, it is possible to correctroll and pitch angles only. In order to ensure that the yaw angleis not affected by in applicable corrections, the third vectorialpart of the correction quaternion qε1 is set equal to zero

qε1 = qε1,0 + qε1,1 + qε1,2 + 0 · qε1,3. (20)

Fig. 2. Kalman filter algorithm.

The second stage of the filter is similar to the first one: Asecond Kalman gain Kk2 is calculated with the same algorithmbut using different noise covariance matrices. The magneticfield, normalized to one, is considered directed along the y-axisonly, and the vertical component is neglected, as the Earth’smagnetic field has a variable intensity from 0.25 to 0.65 Gand the vertical component is geographically dependent. Inthe filter, the magnetic field is estimated with the followingequation:

h2(qk) = m = Rbn

⎡⎣ 010

⎤⎦ =

⎡⎣ 2q1q2 + 2q0q3q20 − q21 − q22 − q232q2q3 − 2q0q1

⎤⎦ (21)

The Jacobian matrix Hk2 is the following:

Hk2 =∂h2[i]

∂q[j]=

⎡⎣ 2q3 2q2 2q1 2q0

2q0 −2q1 −2q2 −2q3−2q1 −2q0 2q3 2q2

⎤⎦ . (22)

In this case, to ensure that magnetic anomalies do not influ-ence roll and pitch estimation, but yaw angle only, the first twovectorial parts of the correction quaternion qε2 are set equal tozero

qε2 = qε2,0 + 0 · qε2,1 + 0 · qε2,2 + qε2,3. (23)

The complete algorithm is summarized as follows (see alsoFig. 2 for a schematic outline).

Start of “a priori” system estimation:1) reading of the gyro sensor and obtaining angular veloci-

ties ωx, ωy, and ωz;2) calculation of the discrete-time state transition matrix

Ak = I + (1/2)ΩnnbT ;

3) calculation of the “a priori” system state estimation q−k =Ak qk−1;

4) calculation of the “a priori” noise covariance matrixP−k = AkPk−1A

Tk +Qk.

Start of the correction stage 1:1) calculation of the Jacobian matrix Hk1 =⎡

⎣−2q2 2q3 −2q0 2q12q1 2q0 2q3 2q22q0 −2q1 −2q2 2q3

⎤⎦;


2) calculation of the Kalman gain Kk1 =

P−k HT

k1(Hk1P−k HT

k1 + Vk1Rk1VTk1)

−1;

3) reading of the accelerometer data zk1 = axay, az;

4) calculation of h1(q−k ) =

⎡⎣ 2q1q3 − 2q0q2

2q0q1 + 2q2q3q20 − q21 − q22 + q23

⎤⎦;

5) calculation of the correction factor qε1 = Kk1(zk1 −h1(q

−k )), qε1,3 is put equal to zero;

6) calculation of the “a posteriori” state estimation qk1 =q−k + qε1;

7) calculation of the “a posteriori” error covariance matrixPk1 = (I −Kk1Hk1)P

−k .

Start of the correction stage 2:

1) calculation of the Jacobian matrix Hk2 =⎡⎣ 2q3 2q2 2q1 2q0

2q0 −2q1 −2q2 −2q3−2q1 −2q0 2q3 2q2

⎤⎦;

2) calculation of the Kalman gain Kk2 =

P−k HT

k2(Hk2P−k HT

k2 + Vk2Rk2VTk2)

−1;

3) reading of the magnetic compass data zk2 = mxmy,mz;

4) calculation of h2(q−k ) =

⎡⎣ 2q1q2 + 2q0q3q20 − q21 − q22 − q232q2q3 − 2q0q1

⎤⎦;

5) calculation of the correction factor qε2 = Kk2(zk2 −h2(q

−k )), qε2,1 and qε2,2 are put equal to zero;

6) calculation of the “a posteriori” state estimation qk =qk1 + qε2;

7) calculation of the “a posteriori” error covariance matrixPk = (I −Kk2Hk2)Pk1.

III. ALGORITHM SIMPLIFICATION TOWARDS

HARDWARE IMPLEMENTATION

To create an integrated system with low area occupation andlow power consumption, the Kalman filter algorithm presentedin Section II was simplified. The objective was to process thefilter on a simplified arithmetic logic unit (ALU) capable ofonly multiplication and sum operations.

The first simplification consists of approximating the“a priori” error covariance P−

k with the “a posteriori” errorcovariance Pk. This is a logical simplification if the angularposition estimated before the correction can be estimated withthe angular position after the correction. This assumption isvalid if the filter is iterated at high frequency, with very lowcorrections at each iteration.

To reduce the number of matrix multiplications, a precom-puted value of Pk is used. Instead of calculating this parameteronline from the noise statistics of the sensor, its value isassigned offline as a tuning parameter of the filter, assuming thatsimilar sensors will have similar noise statistics. During normalfilter operation, the initial value of Pk is greater because of theuncertain initial angular position. It then converges to lowervalues. The final value of P was calculated experimentally withdifferent simulations on Matlab Simulink system model. Tosimulate the fact that the initial value of P is higher and to allowa fast initial position correction even if the system starts upsidedown, it was chosen to initialize the filter with a higher value of

the Pk matrix and to scale down this value with a step functionto the nominal value after the first 128 filter iterations. The samevalue of Pk is used for the two filter stages.

The following simplifications are important, because theyallow eliminating all the division operations in the filter algo-rithm. As the matrix Pk is now a fixed parameter, it is possibleto use a fixed value as the matrix determinant during matrixinversion. Moreover, it is possible to eliminate the normal-ization operation on the quaternion when the Kalman filter isworking, because it operates as a self-normalization algorithmon the quaternion. This property was experimentally verifiedwith many simulations on the simplified Simulink model of thesystem, but it does not apply, in general, for the full Kalmanalgorithm. With these simplifications, further matrix algebramanipulation, and a fine tuning of the operations computedfrom the hardware, it was possible to reduce the number ofmultiplications and additions by up to 60% with negligibleperformance degradation compared to the initial filter algorithm[9].

In this simplified system, the error covariance matrices Q,R1, and R2 are considered constant and have the followingvalues:

Q =10−6 ∗

⎡⎢⎣1 0 0 00 1 0 00 0 1 00 0 0 1

⎤⎥⎦ (24)

R1 =

⎡⎣ 2 0 00 2 00 0 2

⎤⎦ (25)

R2 =

⎡⎣ 1 0 00 1 00 0 1

⎤⎦ . (26)

The error covariance matrix Pk has the value P0 at thefilter start-up and for the first 128 iterations, while its value isdecreased to P1 with a step response for normal filter operation

P0 =

⎡⎢⎣

0.125 0.0003 0.0003 0.00030.0003 0.125 0.0003 0.00030.0003 0.0003 0.125 0.00030.0003 0.0003 0.0003 0.125

⎤⎥⎦ (27)

P1 =

⎡⎢⎣

0.001 0.0002 0.0002 0.00020.0002 0.001 0.0002 0.00020.0002 0.0002 0.001 0.00020.0002 0.0002 0.0002 0.001

⎤⎥⎦ . (28)

IV. ASIP DESIGN AND SYNTHESIS

Before starting with the digital design, it was necessary todefine the number of bits necessary to represent the data andto derive a bit-true model in fixed-point arithmetic from thesimplified model in floating-point arithmetic. The objective wasto use 16-b arithmetic to match the input data from gyro andaccelerometer; however, the use of a tailored number of bits forspecified operations is not a problem in our custom design.

Data were acquired with a repeatable test and were processedoffline with a Matlab Simulink model of the system. The model


TABLE IRMS ERROR WITH DIFFERENT BIT NUMBERS USED IN THE

FIXED-POINT ARITHMETIC MODEL

allows simulating the bit-true arithmetic that will be used inthe integrated processor, to be implemented in FPGA or CMOSstandard-cell technologies. The Simulink model allows a fullsimulation of the behavior of the system and fine tuning of keyparameters of the processor architecture. In this case, it wasused to simulate the effects of the fixed-point arithmetic usingdifferent bit numbers to represent the internal data. Using thedata acquired during the test, the RMS error was calculated forthe roll angle. The results are reported in Table I for the fixed-point arithmetic. For comparison, the floating-point arithmeticmodel leads to an RMS error of 0.17◦.

As shown in Table I, the use of 16 b for the fixed-pointarithmetic leads to poor precision in the angular estimation.While the best result is obtained with the use of the greaternumber of bits, it is useful to evaluate the best value in terms ofthe ratio precision/cost. The cost of implementing a processorwith a processing unit is a function of n2, where n is thenumber of bits needed. The best tradeoff between precisionand cost is obtained using 20 b for the fixed-point arithmetic.To further optimize the system, a different number of bits wasused to represent different kinds of data during the simulations.The data were divided into groups, each with a parameterizednumber of bits of precision: the quaternion, the state updatematrix A, the K matrix, the estimated gravity and magneticfield, and the other internal variables of the simplified Kalmanfilter. Simulations showed that the critical data for the precisionwere the quaternion and the A matrix. Reducing the numberof bits used in the fixed-point arithmetic to 16 did not causea strong increase in RMS error. Using 20 b to represent thequaternion and state update matrix A data and 16 b to representall other data resulted in an RMS error of 0.27 compared to 0.26resulting from using all data as 20 b.

The second issue to be considered is the architecture toimplement. A fully parallel implementation was considered butwas deemed unsuitable because of its area occupation. It waspreferred to have a flexible system, so that the current designcould be easily updated for future system expansions or foralgorithm improvements. An ASIP architecture with a generalALU and reduced instruction set computer (RISC) architecturewas chosen. It was possible to implement an RISC architecturebecause the number of instructions to be executed is muchlower than the available number of clock cycles: The estimatedASIP frequency is 1 MHz, and a suitable filter update rateis 1 kHz.

A mixed approach between the hardwired and the pro-grammed solution was chosen for the control unit, defining acustom instruction set architecture and storing the instructions

Fig. 3. Integrated processor architecture.

to be executed on a ROM. In the future, the system could beupdated just by changing the ROM.

The system was divided into three main blocks: the controlunit, the computation unit with the ALU, and the memorycomposed of the regbank (see Fig. 3). During normal operation,the input data from the IMU sensor, the gyro rate, and the accel-erations are acquired in the regbank; the filter algorithm is thenperformed, and the output quaternion is stored in the regbank.

The computation unit is capable of sum, multiplication, andmultiplier and accumulator operations on 20-b input data. Eachinstruction of the computation unit is performed in two steps:the fetch-and-decode operation and the execute-and-write op-eration. The regbank is made up of two register groups. Thecomputation unit reads one register from each group. The out-put data of the computation unit are on 20 b. For the noncriticaldata, a truncation unit allows the data to be stored on a 16-bregister with consequent area and power savings.

The control unit allows setting a specific filter frequency andsets a low-power state for the system when the elaboration isfinished (i.e., before starting a new filter iteration). Most ofthe time, the integrated processor is in the low-power state,so that clock gating reduces both the switching activity andthe power consumption. The ROM containing the instructionsis read sequentially, but it is also possible to set some jumpconditions for future upgrades.

The design was carried out using VHSIC hardware descrip-tion language (VHDL) and Cadence NCSim tool for simula-tion and synthesis. The synthesis was done with BuildGateson Taiwan Semiconductor Manufacturing Company (TSMC)0.18-μm CMOS standard-cell technology, which is cur-rently used for mixed analog and digital ASIC design inSensorDynamics.

After the synthesis with TSMC 0.18-μm technology, theASIP occupied an area of 0.36 mm2, which is less than the ini-tial constraint of 0.5 mm2. The estimated current consumptionis 75 μA in active mode and 2 μA of leakage current wheninactive.

During the design, both Simulink and VHDL models werefed with the same random input vectors and real acquired data.The output data verified a perfect match.


Fig. 4. FPGA board with SensorDynamics SD746 gyro plus accelerometerand Honeywell HMC5843 magnetic compass.

Fig. 5. Roll angle position estimation with still sensor.

V. RESULTS: FPGA PROTOTYPING AND TESTING

To verify the correct functionality in a practical scenario, thesystem was synthesized on an FPGA Xilinx Spartan 3A (seeFig. 4) using the Avnet design kit. A serial-to-parallel interface(SPI) was developed to connect directly the FPGA with Sen-sorDynamics SD746 sensor, a three-axis gyro plus three-axisaccelerometer integrated sensor. The Honeywell HMC5843magnetic compass connected with I2C bus. Gyro output datawere available with an update rate of 10 kHz, but the filter wasset to run at a frequency of 1 kHz. The FPGA was also con-nected via universal asynchronous receiver/transmitter (UART)to a PC, so that the output data formed by the quaternion couldbe acquired and processed. To allow a first system verification,a simple 3-D graphical animation was developed using theVPython library, so that the position of the sensor could beshown on a PC with a live demo.

The system was tested in static position to ensure that thefilter was able to contrast the drift due to the gyro bias in angleestimation. As shown in Figs. 5–7, the static error results in lessthan 1◦ for roll and pitch angles, and less than 3◦ for the yawangle.

The system was also tested to ensure that the initial positionis correctly estimated even if the sensor starts upside down (seeFigs. 8–10).

Finally, the system was tested with an RMS test machinethat allows precise and repeatable rotations of the board. Themagnetic compass was not used in this test.

Fig. 6. Pitch angle position estimation with still sensor.

Fig. 7. Yaw angle position estimation with still sensor.

Fig. 8. Upside-down start-up for roll angle.

Fig. 9. Upside-down start-up for pitch angle.

For roll and pitch axes, the absolute error in static positionis less than 1◦ across the entire acquisition time (see Figs. 11and 12).


Fig. 10. Upside-down start-up for yaw angle.

Fig. 11. Roll angle estimation.

Fig. 12. Pitch angle estimation.

VI. CONCLUSION

During this work, a sensor fusion algorithm based on a noveltwo-stage Kalman filter has been developed. The two-stagefilter has many advantages in terms of flexibility, computingrequirements, system simplification, and also for robustness tomagnetic anomalies. An ASIP to process the algorithm wasspecifically designed to be integrated within a 6-D sensor logic.The estimation system is flexible and can also process data froman external magnetic compass.

The filter was developed using MathWorks Simulink, severalalgorithm simplifications were tested and implemented, and aproof of concept was created on a FPGA for full testing. TheASIP was designed paying special attention to area and powerconsumption tradeoffs.

The FPGA was interfaced via SPI with SD746 gyro-plus-accelerometer sensor and with Honeywell magnetic compass

and connected to a PC, so that it was possible to run a livedemo and to record data. Tests showed errors of less than 1◦ forroll and pitch angles and less than 3◦ for yaw angle.

The future goal is to integrate the processing unit within themixed analog/digital ASIC that controls the 6-D sensor, withthe option to interface with an external magnetic compass.

REFERENCES

[1] O. J. Woodman, “An introduction to inertial navigation,” Univ.Cambridge, Cambridge, U.K., Tech. Rep., 2007.

[2] A. Kim and M. F. Golnaraghi, “A quaternion-based orientation estimationalgorithm using an inertial measurement unit,” in Proc. Int. Conf. Intell.Robots Syst., May 2001, pp. 268–272.

[3] X. Yun, M. Lizarraga, E. R. Bachmann, and R. B. McGhee, “An im-proved quaternion-based Kalman filter for real-time tracking of rigidbody orientation,” in Proc. Int. Conf. Intell. Robots Syst., Dec. 2003,pp. 1074–1079.

[4] X. Yun and E. R. Bachmann, “Design, implementation, and experimen-tal results of a quaternion-based Kalman filter for human body motiontracking,” IEEE Trans. Robot., vol. 22, no. 6, pp. 1216–1227, Dec. 2006.

[5] M. Wang, Y. Yang, R. R. Hatch, and Y. Zhang, “Adaptive filter for aminiature MEMS based attitude and heading reference system,” in Proc.Position Location Navig. Symp., Jul. 2004, pp. 193–200.

[6] E. Kraft, “A quaternion-based unscented Kalman filter for orientationtracking,” in Proc. 6th Int. Conf. Inf. Fusion, 2003, pp. 47–54.

[7] J. J. LaViola, Jr, “A comparison of unscented and extended Kalman fil-tering for estimating quaternion motion,” in Proc. Amer. Control Conf.,Jun. 2003, pp. 2435–2440.

[8] XSense MTx Product Datasheet. [Online]. Available: http://www.xsens.com/en/general/mtx

[9] S. Sabatelli, F. Sechi, A. Rocchi, and L. Fanucci, “A sensor fusionalgorithm for an integrated angular position estimation with inertialmeasurement units,” in Proc. DATE, 2011, pp. 1–4.

[10] S. Sabatelli, M. Galgani, A. Rocchi, and L. Fanucci, “An applicationspecific instruction set processor for angular position estimation withinertial measurement units,” in Proc. IEEE Sens. Conf., Oct. 2011,pp. 886–888.

[11] S. Sabatelli, M. Galgani, A. Rocchi, and L. Fanucci, “A double stageKalman filter for sensor fusion and orientation tracking in 9D IMU,” inProc. IEEE SAS Conf., Feb. 2012, pp. 1–5.

[12] L. Vicci, “Quaternions and rotations in 3-space: The algebra and itsgeometric interpretation,” Microelectronic Syst. Lab., Dept. Compu. Sci.,Univ. North Carolina, Chapel Hill, NC, 2001.

[13] J. B. Kuipers, Quaternions and Rotations Sequences: A Primer WithApplications to Orbits, Aerospace and Virtual Reality. Princeton, NJ:Princeton Univ. Press, 1999.

[14] G. Welch and G. Bishop, “An introduction to the Kalman filter,” Dept.Comput. Sci., Univ. North Carolina, Chapel Hill, NJ, 2001.

[15] M. Keating and P. Bricaud, Reuse Methodoogy Manual for System on aChip Design, 3rd ed. New York: Springer-Verlag, 2002.

[16] R. R. Eckert, Micro-Programmed Versus Hardwired Control Units: Howa Computer Really Work, Dept. Comput. Sci., Binghmanton, NY, 1988.

[17] N. S. Matlo, Introduction of a Microcode Implementation of a CPU Ar-chitecure, 1997.

Simone Sabatelli received the M.S. degree in elec-tronic engineering from the University of Pisa, Pisa,Italy, in 2010, where he is currently working towardthe Ph.D. degree in information engineering.

Since 2010, he has been collaborating with Sen-sorDynamics AG, Graz, Austria, on the design, de-velopment, and testing of sensor fusion algorithmsfor 6-D inertial measurement units.


Marco Galgani received the M.S. degree in elec-tronic engineering from the University of Pisa, Pisa,Italy, in 2011.

After the degree, he collaborated with SensorDy-namics AG, Graz, Austria, as a Digital Designerfor field-programmable gate array prototyping andtesting to allow the integration of sensor fusion al-gorithms. He is currently with the On-Board PayloadData Processing Section, European Space Researchand Technology Centre, European Space Agency,Noordwijk, The Netherlands, developing a new test

system for avionics applications.

Luca Fanucci received the Laurea and Ph.D. degreesin electronic engineering from the University of Pisa,Pisa, Italy, in 1992 and 1996, respectively.

From 1992 to 1996, he was with the EuropeanSpace Research and Technology Centre, EuropeanSpace Agency, Noordwijk, The Netherlands, as aResearch Fellow. From 1996 to 2004, he was aSenior Researcher with the Italian National ResearchCouncil, Pisa. He is a Professor of microelectronicsystems with the Faculty of Engineering, Universityof Pisa. His research interests include several aspects

of design technologies for integrated circuits and electronic systems, withparticular emphasis on system-level design, hardware/software codesign, andsensor conditioning and data fusion. The main application areas are in the fieldsof wireless communications, low-power multimedia, automotive, healthcare,ambient assisted living, and technical aids for independent living. He is thecoauthor of more than 300 journal and conference papers and coinventor ofmore than 30 patents. He is a Member of the Editorial Board of ElsevierMicroprocessors and Microsystems Journal and IOS Press Technology andDisability Journal.

Dr. Fanucci served in several technical program committees of internationalconferences. He was the Program Chair of Euromicro Conference on DigitalSystem Design (DSD) 2008, the Application Track Chair at Design, Automa-tion & Test in Europe (DATE) from 2006 to 2010, and the Tutorial Chair atDATE in 2011 and 2012. He is currently serving as Vice Program Chair atDATE 2013.

Alessandro Rocchi received the B.E. degree intelecommunication engineering from the Universityof Pisa, Pisa, Italy, in 1995.

He was with the R&D, STMicroelectronics, andwas a Senior Designer with AustriaMicroSystems.He is currently the R&D Director of Inertial andMicro Sensor Systems Business Unit (BU), Sensor-Dynamics AG, Pisa.

Documents

06316172