THREE DIMENSIONAL ATTITUDE ESTIMATION VIA THE TRIAD ALGORITHM ANDA TIME-VARYING COMPLEMENTARY FILTER

Yizhou Wang, Evan Chang-Siu,Matthew Brown, Masayoshi TomizukaMechanical System Control LaboratoryDepartment of Mechanical Engineering

University of CaliforniaBerkeley, California 94720-1740Email: yzhwang@berkeley.edu

Mohammed I. Almajed,Badr N. Alsuwaidan

National Satellite Technology ProgramKing Abdulaziz City for Science and Technology

Riyadh 11555Saudi Arabia

ABSTRACTAn innovative implementation of attitude estimation in 3 de-

grees of freedom (3-DOF) combining the TRIAD algorithm [1]and a time-varying nonlinear complementary filter (TVCF) isderived. This work is inspired by the good performance of theTVCF in 1-DOF [2] developed for applications limited to smallmobile platforms with low computational power. To demon-strate robust 3-DOF estimation, information from vector andrate-gyroscope measurements are fused. Simulation and exper-imental results demonstrate comparable performance to the ex-tended Kalman filter (EKF) and improved performance over al-ternative methods such as sole gyroscope rate-integration andthe TRIAD algorithm without the TVCF as a pre-filter.

NOMENCLATURE

ri fixed vectors in reference framebi measured vectors in body framebgi vectors via integrating gyro from bibi best estimate of vectors in body frameA rotation matrix from reference to body frameFr reference frameFb body frameωωω angular velocityβββ time-varying bias in gyroscope measurementωc cutoff frequency for TVCFW a triad formed from {b1,b2}V a triad formed from {r1,r2}

INTRODUCTIONOrientation estimation in three degrees of freedom is useful

and necessary for a broad area of applications. Examplesinclude satellites, mobile robots, phones and motion tracking forvideo games, film industry and sports. Many sensors have beenstudied for this purpose; for example, inertial measurement units(IMUs), indoor/outdoor GPS, star cameras, vision, ultrasonicsenors and range finders [3–6]. For terrestrial applications,IMUs, while still computationally limited are attractive sincethey are becoming faster, more compact and affordable. Forspace/satellite applications, a star camera is convenient sinceunbiased vector measurements can be acquired from the sur-rounding stars.

The results of other researchers that have studied 3-DOF orientation estimation [4, 7] are promising, but can beimproved in some form. Nonlinear forms of the Kalman filterhave also been studied for attitude estimation [8], but the heavycomputational complexity can make them less attractive inapplications where only small scale processors are available.

By combining two computationally efficient schemes,the TRIAD algorithm and the time-varying complementaryfilter (TVCF), we present an algorithm that shows comparableperformance to the EKF with less computional burden. It aimsto be implementable on a small portable platform with lowcomputational power in Fig.6(a). This application can benefitfrom complementary filtering because rate and angle sensors

1

(gyroscope versus accelerometer and magnetometer) possessbenefits and drawbacks in different frequency regimes [9–11].The TVCF, which uses a fuzzy logic scheme to adjust trust todifferent sensors, has already been found to be useful for 1-DOFattitude estimation due to its low computational requirement andthe ability to discern stationary and motion states [2]. This papergeneralizes the previous work to 3-DOF attitude estimation. Wewill also show that when the angle measurements are from astar camera, the complementary filtering method with a tunedfixed cutoff frequency also shows comparable performance tothe EKF. The experimental results are made possible by theQuanser 3-DOF gyroscope in Fig.6(b), to provide true attitudeverification and sensor calibration through high resolutionencoders.

This paper is organized as follows: first, two attitudeparameterizations, the TRIAD algorithm and the gyroscopemeasurement model will be briefly reviewed. Second, aninnovative nonlinear complementary filter structure will bepresented along with the fuzzy logic based cutoff frequencyscheduling and the performance analysis. Then the simulationand experimental results will be discussed demonstrating thebenefit of the proposed method compared to others. Finally,conclusions will be drawn.

ATTITUDE PARAMETERIZATIONSRotation matrices, quaternions, and Euler angles are among

the many extensively used mathematical tools developed toparameterize attitude. Since each method has distinct advan-tages over others under specific schemes, the choice of attitudeparameterization is diverse and dependent on application.

To eliminate Euler angle singularities and the doublecover addition issues with quaternions, we choose the rotationmatrix for our computation. The rotation matrix, A ∈ SO(3)(special orthogonal group) is subject to a unity determinantconstraint, detA = 1, because it is a non-minimal non-singularattitude representation. It is usually used as a map from thereference frame Fr to the body frame Fb. For example, if b isa vector expressed in Fb and r is the same vector expressed inFr, then

b = Ar (1)

The rotation matrix kinematic equation can be derived as,

A =−[ωωω×]A (2)

Figure 1. THE DIAGRAM OF THE TRIAD METHOD. THE ROTATIONMATRIX A REPRESENTS THE TRANSFORMATION BETWEEN THEFRAMES Fr AND Fb.

where the cross product matrix of the angular velocity, ωωω =[ω1,ω2,ω3]

T , of Fb with respect to Fr, is given by

[ωωω×] =

0 −ω3 ω2ω3 0 −ω1−ω2 ω1 0

(3)

Our proposed algorithm outputs a rotation matrix, rep-resenting the current orientation with respect to a predefinedreference frame. Other parameterizations can be easily con-verted from this if necessary.

For performance validation and data communicationbetween the IMU and an external computer, we use thequaternion, because it requires the least number of variables innon-singular parameterizations. It is defined by both the Euleraxis e and the rotation angle θ ,

q =

[ρρρ

q4

]=

[esin(θ/2)cos(θ/2)

](4)

THE TRIAD ALGORITHM: ATTITUDE DETERMINATIONFROM VECTOR MEASUREMENTS

The full attitude of a rigid body can be constructed from theknowledge of at least two vector measurements with respect to abody frame Fb and a reference frame Fr. Many attitude deter-mination algorithms have been well studied [1,12], but a popularone is the TRIAD algorithm due to its simplicity. This algorithmprovides a deterministic, non-optimal solution for the attitude pa-rameterized by a rotation matrix. We denote the vector measuredin Fb as bi and the one measured in Fr as ri. A rotation relat-ing those vectors may not exist in general due to sensor noises.

2

The TRIAD algorithm constructs triads W = {w1,w2,w3} andV = {v1,v2,v3} from the vector measurements ({b1,b2} and{r1,r2}), as shown in Fig. 1. Then the rotation matrix A, whichis considered to be the attitude estimate, can be represented bythe two triads as,

A =3

∑i=1

wivTi (5)

and is guaranteed to exist [1]. To construct the triad V frommeasurements {r1,r2}, first set v1 equal to r1, then calculatev2 from the normalized cross product of v1 and r2 and finallycalculate v3 from the cross product of v1 and v2. The triad Wcan be constructed similarly from measurements {b1,b2}.

Presently we are interested in two applications, (a) whenthe two vector measurements are the normalized gravitationalvector and the normalized magnetic flux, and (b) when they arefrom a star camera.

We assume that the environmental magnetic field doesnot change its intensity and direction significantly, so that it isreliable to use magnetic flux as a vector measurement. Also weassume large magnetic distortion is not present for long timedurations.

Vector measurements from the accelerometer and themagnetometer, in contrast to a star camera in the attitudeestimation in space mission, are sometimes problematic becausethey also measure motion accelerations and magnetic distortionsthat inhibit the performance of the sole TRIAD algorithm.Another inertia sensor, the rate-integrating gyroscope, canprovide additional information to enhance accuracy of theattitude estimation.

GYROSCOPE MEASUREMENT MODELA gyroscope is a commonly used sensor that measures the

angular rate. Its measurement model is mathematically ex-pressed as,

ωωω = ωωω true +βββ +ηηηv

βββ = ηηηu(6)

where ωωω true and βββ denote the true angular rate and the bias drift,ηηηv and ηηηu are two independent zero-mean Gaussian white-noiseprocesses with covariances σ2

v I3×3 and σ2u I3×3. Those random

processes are usually referred to as the angle-random walk(ARW) and the rate-random walk (RRW) respectively. It isreadily seen that the attitude estimate from direct integration of

the measured angular rate using the rotation matrix kinematicequation in Eqn. (2) is inaccurate, especially in applicationswhere long duration of operation is required. The inaccuracyis due to the accumulations of both ARW and RRW. However,those noises when appearing in the attitude estimate are identi-fied to have low-frequency content.

Other sources of noises can be temperature variation, scalefactor identification, sensor misalignment etc. The latter two canbe accounted for by proper calibration and use of the sensor. weassume the temperature variation is not significant so its effect isnegligible.

Therefore, we are motivated to develop a nonlinear com-plementary filter with a time-varying cutoff frequency, presentedin the next section, to (a) attenuate the high-frequency noise inthe vector measurements, (b) attenuate the low-frequency biasin the rate measurement, and (c) identify motion accelerationsand distortions in the environmental magnetic field.

TIME-VARYING COMPLEMENTARY FILTERINGSince rate and angle sensors possess noises in different fre-

quency regimes, the complementary filtering method can be uti-lized to produce a more accurate attitude estimation.

Filter structureIn a first order complementary filter, we extract the useful

low frequency information from the vector measurement bypassing it through a low pass filter and the useful high frequencyinformation from the rate measurements by passing it througha high pass filter (Fig. 2(a)) where both filters have the samecutoff frequency, ωc. On Earth, the vector measurements b1and b2 in Fb could be taken by the on-board accelerometer andmagnetometer respectively; in space they could be two vectormeasurements from a star camera.

In actuality, the components of the rate signal, bg arepassed through a slightly modified high-pass filter (lowerchannel of Fig. 2(a)) that incorporates an integrator,

Y (s) =s

s+ωcU(s) =

1s+ωc

L {u} (7)

It is desirable to directly use the measurements from a gyro-scope, ωωω , as the rate signal, however the nonlinearity in the at-titude kinematics requires further treatment. We denote bg1 andbg2 as two vector measurements estimated by integrating angularvelocity. The rate of change of these two vector measurementscan be estimated by the gyroscope measurement using the rota-

3

(a)

acc

gyro

mag

gyro

(b)

Figure 2. (a) DETAILS OF THE TVCF BLOCK AND (b) THE NONLINEAR COMPLEMENTARY FILTER USED AS A PREFILTER TO THE TRIADMETHOD USED IN 3-DOF ATTITUDE ESTIMATION.

tion matrix kinematic relationship,

bgi(k) = A(k)ri

=−[ωωω(k)×]A(k)ri

≈−[ωωω(k)×]bi(k), i = 1,2

(8)

Note that the ri’s are constant vectors in the reference frameand bi’s are the best estimates of the vectors from the TVCF. Adiscrete-time complementary filter is obtained by applying thebilinear transformation.

The block diagram of the entire 3-DOF estimationscheme is shown in Fig. 2(b), which can be summarized asfollows: the initial measurements of the earth gravitationalvector r1 and the magnetic flux r2 are recorded during the setupinitialization. Namely, the initial attitude is chosen to be thereference frame. Also, the IMU is assumed to be initializedproperly so that no bias is present initially. At time step k,the best estimate of attitude is available, or equivalently thebi(k)’s are available. The vectors b1(k + 1), b2(k + 1), andωωω(k+ 1) are measured by the accelerometer and magnetometeror star camera, and gyroscope respectively. Their derivativesbg1(k + 1) and bg2(k + 1) are calculated from Eqn. (8). Twoparallel time-varying complementary filters then fuse the sensormeasurements to obtain the best estimates of the normalizedvector measurements,

bi(k+1) =ωci(k+1)∆t

2+ωci(k+1)∆t(bi(k)+bi(k+1))+

2−ωci(k+1)∆t2+ωci(k+1)∆t

bi(k)+∆t

2+ωci(k+1)∆t(bgi(k)+ bgi(k+1))

i = 1,2(9)

where ∆t denotes the sampling period. With b1(k+1) and b2(k+

1) obtained from the TVCF, the attitude estimate at time stepk+1, parameterized by the rotation matrix A, is then calculatedby the TRIAD algorithm.

Fuzzy Logic Based Time-Varying Cutoff FrequencyScheduling

When using an accelerometer and a magnetometer, we cantake advantage of their physical properties to adapt the cutofffrequencies between a low and high value based on a fuzzy logicapproach proposed by Chang-Siu et al [2]. In 1-DOF pitch angleestimation, the magnetometer is not required; for the accelerom-eter, a fuzzy logic scheme is used to discern the stationary state.In 3-DOF the magnetometer provides true heading information,but the scheme will need to account for disturbances in the mag-netic field. The output of the fuzzy logic variable µ , which isvalued in the range of [0,1], signifies how the cutoff frequencyshifts between ωhigh and ωlow as,

ωc = µωhigh +(1−µ)ωlow (10)

Note that if µ = 1 then ωc = ωhigh, which indicates whenthe accelerometer or magnetometer signal is trusted more thanthe gyroscope. Conversely, if µ = 0 and ωc = ωlow, the gyro-scope is trusted more. The cutoff frequency decreases when themagnitude of the accelerometer or magnetometer signals deviatefrom their initial value (indicating rotational or translationalacceleration, and magnetic disturbances), and increases whenthese signals are close to their initial values. The extension from1-DOF to 3-DOF is simple; just one additional TVCF filterneeds to be added in parallel. Therefore, two independent fuzzylogic variables, µi, i = 1,2, need to be computed. The details ofcalculating µ1 of the accelerometer channel are the same as the1-DOF case in [2].

For the magnetometer, the signal ym(t) =[ymx(t),ymy(t),ymz(t)]T is considered trustworthy if there

4

0

0.5

1

Figure 3. THE SMOOTH SATURATION FUNCTION THAT COMPUTESTHE FUZZY LOGIC VARIABLE. THE SLOPE si AND THE 50%THRESHOLD xoi CAN BE TUNED.

Table 1. FUZZY LOGIC FOR MAGNETOMETER.Condition x1 x2 µ2(t)no magnetic disturbance low low 1constant disturbance high low 0moving disturbance high high 0

are minimal magnetic disturbances. This is operationalized byexamining x1(t) = ||ym(t)|| − ||ym(0)|| and x2(t) = d

dt ||ym(t)||and decreasing µ2 when these signals have a high value, where||ym(t)|| =

√ymx(t)2 + ymy(t)2 + ymz(t)2. The IMU is assumed

to be initialized in an environment free of magnetic disturbance.If there is a constant disturbance, such as a large piece of iron,it can be detected by comparing the magnitude of the currentmagnetometer signal ||ym(t)|| to its initial value ||ym(0)||. Fluc-tuations in the magnetic field, evidence of a moving disturbance,are detected by examining d

dt ||ym(t)||. This logic is depicted inTab. 1.

More formally, µ2 in the magnetometer chan-nel is calculated by µ2(t) = ∏

2i=1(1 − xi(t)), where

x1(t) = f1(||ym(t)|| − ||ym(0)||) and x2(t) = f2(ddt ||ym(t)||).

As detailed in [2], fi is a smooth saturation function shownin Fig. 3 that converts physical units to logical units boundedby [0,1] and distinguishes between high and low based onparameters dependent on the stationary signals.

Performance analysisThe high-frequency noises, present in the accelerometer and

magnetometer measurements, are attenuated by the low-passfilters in the algorithm. Consequently, less oscillatory behavioris observed in the attitude estimation.

For the bias present in the gyroscope measurement,we will analytically show that it does not cause the accu-mulation of error in this estimation scheme unlike the pure

rate-integration. However, the filtered vector measurement doesmaintain small steady-state error. To simplify the analysis, wesuppose that the body stays steady. Hence, any signals measuredby the gyroscope are just bias βββ . We also neglect high-frequencynoise in the accelerometer and magnetometer in this analysis.Eqn. (9) becomes

αbss =2ωc∆t

2+ωc∆tbtrue +

2−ωc∆t2+ωc∆t

bss−2∆t

2+ωc∆t[βββ×]bss (11)

where bss is the steady-state estimate from the TVCF. Note thatthe output from the TVCF does not necessarily have unity normbecause of discretization and noises in the accelerometer andmagnetometer. However, α ≈ 1 if fast sampling rate is assumedand noises are neglected. Thus bss has a closed-form solution,

bss ≈ (I3×3 +[βββ

ωc×])−1btrue (12)

The term in the parenthesis is invertible because its deter-minant is always greater than or equal to 1. If ωc is chosen tobe much greater than βββ component-wise, the matrix inverse willbe close to the identity. Hence the bias in the gyroscope mea-surement results in only a small steady-state error in the TVCFestimate. Furthermore, there exists a tradeoff in selecting high orlow cut-off frequency. In order to attenuate the high-frequencynoise using the low-pass filter, ωc should be chosen significantlylower than noise frequencies. However, it should be noted thatthe proposed filter during motion or magnetic disturbances islimited by the quality of the gyroscope.

Comparison with the EKFSince the above filters only require simple algebraic ma-

nipulations, the proposed algorithm does not demand muchcomputational power; the required computations can be handledby low power processors. This is a major reason why the pro-posed estimation scheme should be found attractive comparedwith some known optimal Kalman filter formulations when theimplementation is on mobile platforms with low computationalpower. For example, several high-dimensional matrix operations(addition, multiplication and inversion) need to be performedin the multiplicative quaternion EKF. In practice, it takes an8MHz Arduino Pro Mini processor on average 9 millisecondsto perform either a 6× 6 matrix inversion or a multiplicationof two 6× 6 matrices. Since the EKF requires more than fouroperations, it cannot be processed within an acceptable samplingrate (i.e. 25Hz in our case).

5

90 91 92 93 94 95 96 97 98 99 100−1

0

1

φ(deg)

90 91 92 93 94 95 96 97 98 99 100−1

0

1

θ(deg)

90 91 92 93 94 95 96 97 98 99 100−1

0

1

ψ(deg)

time (sec)

EKF CF+TRIAD

Figure 4. PLOT OF ERROR EULER ANGLES FROM THE EKF ANDCF, SHOWING SIMILAR PERFORMANCES BETWEEN THE TWOMETHODS. ωc IS FIXED AT 2rad/s.

TRIAD Rate integration CF+TRIAD EKF0

2

4

6

8

10

12

meanerrorand

standard

deviation(deg)

Figure 5. PLOT OF THE ERROR AND STANDARD DEVIATION BYTRIAD, RATE INTEGRATION, CF+TRIAD AND EKF. SIGNIFICANT IM-PROVEMENTS ARE SEEN USING THE LATTER TWO ALGORITHMS.

In terms of filter performances, for terrestrial applica-tions the proposed algorithm significantly simplifies the designof the fuzzy logic law that is used to shift trustworthinessbetween available sensors, and thus make it possible to easilyidentify distortions in vector measurements. Although theo-retically the EKF perfectly captures the stochastic propertiesof the system and should produce more accurate estimation,it is extremely difficult in practice to develop some equivalenttime-varying noise covariance scheduling in order to account fordistortions measured in inertial vector sensors.

RESULTSSimulation results:3-DOF spacecraft attitude estimation simulation

Via a simulated space mission, the proposed combina-tion of the complementary filter and the TRIAD algorithm areshown to estimate attitude equally well compared with the mul-

tiplicative quaternion EKF formulation [13]. The gyroscopemodel remains the same but the vector measurements b1 andb2 are assumed from a star camera that do not possess non-zero bias but only zero-mean Gaussian noises. Hence, a prop-erly tuned fixed cutoff frequency is used in the complemen-tary filter approach. A random true attitude profile is used.Based on the literature [13], the other parameters are set tobe: the sampling rate f = 100Hz, the gyro noise parametersσu = 0.003rad1/2/s,σv = 0.003(rad/s)1/2, the star camera mea-surement noise covariance R = 0.03I6×6, and the initial state er-ror covariance P0 = 0.001I6×6. No initial quaternion and biasestimate errors are present. To evaluate performance, the errorEuler angles, {φ ,θ ,ψ} are computed by the following equationwhen they are small,

Aerr ≈

1 ψ −θ

−ψ 1 φ

θ −φ 1

(13)

From Fig. 4, it can be seen that the errors from the two methodsare quite similar.

The total error angle, regardless of rotation axis, canbe calculated from the fourth component of Eqn. (4) as,

θtot = 2cos−1(qerr,4) (14)

Figure 5 shows the comparison of the mean error and standarddeviation from the four different methods. Compared with thesole TRIAD and rate integration methods, the proposed algo-rithm and the EKF significantly improve the accuracy of the esti-mation. The proposed algorithm retains good performance whilebeing less demanding in terms of computations.

Experimental resultsThe performance of the proposed TVCF+TRIAD in 3-DOF

attitude estimation is evaluated via experiments. Figure 6(a)shows the 9-DOF IMU developed in the Mechanical SystemControl Laboratory at UC Berkeley. It has an Arduino Pro Minimicroprocessor and a Sparkfun 9-DOF sensor stick (ADXL345accelerometer, HMC5843 magnetometer, ITG-3200 gyroscope)onboard. The sampling frequency is set to 25Hz. The clockrate of the processor is only 8MHz, but is capable of handlingall the required computations including three estimation algo-rithms in real time. This shows the computational friendlinessof the proposed TVCF algorithm. The three estimation schemesare (i) TVCF+TRIAD, (ii) angular rate integration and (iii) pureTRIAD. In the performance validations shown below, when dif-ferent cutoff frequencies are compared, the data are collected inreal time but the attitude estimates are calculated offline.

6

9-DOF Sensor Stick

(a) (b)

Figure 6. (a) THE 9-DOF INERTIAL MEASUREMENT UNIT AND (b)THE QUANSER 3-DOF GYROSCOPE (SIZE: 0.7m× 0.5m× 0.5m).BOTH FIGURES ARE NOT THE SAME SCALE.

0 5 10 15 20 25 300

0.5

1

1.5

time (sec)

errorangle

(deg)

TRIAD CF+TRIAD(ωc = 3rad/s) CF+TRIAD(ωc = 0.3rad/s)

Figure 7. EFFECT OF THE LOW-PASS FILTER. THE NOISE ATTEN-UATION INCREASES AFTER THE PROPOSED TVCF PREFILTER ISADDED TO THE TRIAD AND INCREASES MORE AFTER THE CUTOFFFREQUENCY IS DECREASED FROM 3rad/s TO 0.3rad/s.

Effect of the low-pass filter. The low-pass filter at-tenuates the high-frequency noise in the accelerometer and themagnetometer. Figure 7 shows the comparison of the attitudeestimates when the IMU is kept steady. The error angles arecomputed from the quaternions estimated by the two methodsusing Eqn. (14). The oscillatory behavior is significantly atten-uated when the cutoff frequency decreases from infinity (i.e. CFis not utilized) to 0.3 rad/s, which shows the benefit of choosinga small cutoff frequency. Due to the unity magnitude constraintof the quaternion parameterization, noisy measurement signalsresult in large mean errors. The mean error is thus effectivelyreduced by the proposed algorithm.

Effect of the high-pass filter. Direct rate integration ofthe gyroscope measurements is not desirable for applications re-

0 2 4 6 8 10 12 14 16 18 200

10

20

30

40

50

60

time (min)

errorangle

(deg) rate integration

CF+TRIAD(ωc = 0.003rad/s)

CF+TRIAD(ωc = 3rad/s)

Figure 8. EFFECT OF THE HIGH-PASS FILTER. AS THE CUT-OFF FREQUENCY IN THE CF ESTIMATE IS INCREASED FROM0.003rad/s TO 3rad/s, THE ESTIMATE DOES NOT DRIFT AWAYWHEN NONZERO BIAS EXISTS.

quiring long period of operation because nonzero bias and noiseskeep accumulating during the integration. The high-pass fil-ter prevents the accumulation of errors in the gyroscope mea-surements. The information from angle sensors are utilized tocorrect the estimate errors. Figure 8 shows that the attitude es-timate from the rate integration method drifts away, while theCF+TRIAD estimate maintains a bounded error when the IMUis kept steady for a long period of time. This shows the benefit ofchoosing large cutoff frequencies. Note that the average bias inthis particular test is calculated to be 8.3×10−4rad/s hence theβ/ωc ratio is of the order -4 when ωc is selected to be 3rad/s.The result confirms our analysis in Eqn. (12).

Selection of the cutoff frequencies. An admissiblecutoff frequency range can then be designed. The lower and up-per limits of the range are suggested by the test results shownin Fig. 7 and Fig. 8 respectively. Namely, the design of thelower value depends on the desired noise attenuation and theupper value depends on the tolerable drift due to the existenceof bias. A more rigorous way of choosing cutoff frequency isdiscussed in [2] where rms estimation errors are experimentallyobtained over a grid of cutoff frequencies.

Effect of the fuzzy logic. Without the TVCF as a pre-filter, the motion acceleration and the magnetic distortion causethe pure TRIAD method to predict incorrect attitude, which isproblematic. Figure 9 shows that when the IMU undergoes y-axis translational motion, the TVCF correctly identifies the mo-tion acceleration and thus predicts much smaller attitude changecompared with the estimate from the sole TRIAD. The lowerplot shows the time-varying scheduling of the cutoff frequencyin the accelerometer channel. The gyroscope is more trustworthy

7

0 1 2 3 4 5 6 7 8 9 100

20

40

60

80

errorangle

(deg)

TVCF + TRIAD, TRIAD

0 1 2 3 4 5 6 7 8 9 10

0

2

4

6

8

time (sec)

cutofffreq

uen

cy(rad/sec)

Figure 9. THE EFFECT OF THE FUZZY LOGIC TO REJECT MOTIONACCELERATIONS. THE IMU UNDERGOES PURE TRANSLATIONALMOTION IN THE y-AXIS. THE ERRORS ARE SIGNIFICANTLY RE-DUCED WHEN THE TVCF IS ADDED. THE UPPER AND LOWER LIM-ITS OF THE CUTOFF FREQUENCY ARE 8rad/s AND 0.1rad/s.

when the translational motion is detected. Similar behaviors areobserved in the presence of temporary magnetic disturbances.

3-DOF attitude estimation experiment. The IMUundergoes a random rotational motion and is attached to theQuanser 3-DOF gyroscope shown in Fig. 6(b), which isequipped with the 5000 lines/rev quadrature optical encoder oneach gimbal. The encoder readings can be converted to thequaternion parameterization, providing a true attitude referencefor comparison. Magnetic shielding is applied to motors on thetestbed to minimize the change of magnetic field due to gim-bal movements. However, the magnetic distortion is still largeenough to cause problems if the TVCF is not applied. All attitudeestimates from the IMU are processed in real time and the quater-nions are transmitted to the computer. Figure 10(a) compares thefirst three components of the quaternions from the encoders, theTVCF+TRIAD algorithm, the rate integration method and thesole TRIAD method. The upper and lower limits of the cutofffrequency are 5rad/s and 0.1rad/s. It can be easily observedthat (i) in the q1 plot, the rate integration drifts in 2 minutes of op-eration, (ii) in the time intervals where the fuzzy logic shifts thecutoff frequency to its lower limit in the magnetometer channel,the TRIAD method gives inaccurate estimates due to the changeof magnetic field (at around 103s and 107s). The estimate fromthe proposed algorithm gives the most accurate attitude estimatesamong the three methods. Figure 11 shows the mean errors cal-culated from Eqn. (14) and the standard deviation correspondingto the same experiment, where improvements using the proposedalgorithm can be concluded. It should also be noted that the ef-fectiveness of the proposed algorithm is underestimated in thisexperiment because larger errors from the sole TRIAD and therate integration method will be obtained if the motion accelera-

75 80 85 90 95 100 105 110 115 120−0.5

0

0.5

q 1

Encoder TVCF+TRIAD Rate integration TRIAD

75 80 85 90 95 100 105 110 115 120−0.5

0

0.5

q 2

75 80 85 90 95 100 105 110 115 120−0.5

0

0.5

q 3

75 80 85 90 95 100 105 110 115 120

2

4

ωc,acc

75 80 85 90 95 100 105 110 115 120

2

4

ωc,m

ag

time(sec)

(a)

100 101 102 103 104 105 106 107 108 109 110

−0.5

−0.4

−0.3

−0.2

−0.1

0

0.1

magnetic distortion

gyroscope drift

(b)

Figure 10. (a) PLOT OF THE ATTITUDE ESTIMATES FROM THE DIF-FERENT METHODS AND THE TRANSITIONS OF THE FUZZY LOGICBASED CUTOFF FREQUENCIES. (b) ZOOMED-IN PLOT OF q1

tions get larger (the centripetal accelerations in this experimentare small due to the small radius of curvature) and if long-termoperation is performed, as already demonstrated in Fig. 8 and 9.

CONCLUSIONIn this paper, a computationally efficient and tractable

method for estimating orientation in 3-DOF is presented. Thismethod can be implemented on many mobile applications andhas been shown to be well suited for small, low-cost platforms.The proposed algorithm shows comparable performance to theextended Kalman filter in the simulated space missions and im-

8

TRIAD Rate integration TVCF+TRIAD−5

0

5

10

15

meanerrorand

standard

dev

iation(deg)

Figure 11. COMPARISON OF THE ESTIMATE ERRORS BY THETHREE METHODS. AMONG THEM, THE PROPOSED ALGORITHMGIVES THE MOST ACCURATE ATTITUDE ESTIMATES.

proved performance over alternative methods, such as the rateintegration and the TRIAD without TVCF as a prefilter, in iner-tial measurement applications on Earth. It is capable of not onlyattenuating noises in different frequency ranges, but also detect-ing motion acceleration and change of magnetic field, when usedin 3-DOF IMUs.

ACKNOWLEDGMENTThe authors would like to thank the NSF CiBER-IGERT un-

der Award DGE-0903711 for partial financial support, Dr. JacobApkarian from Quanser for his generous contribution of hard-ware, Dr. Wenjie Chen for his valuable technical advice and Mr.Veresano (http://www.varesano.net/) for providing foundationalcode.

REFERENCES[1] Shuster, M. D., and Oh, S. D., 1981. “Three-axis atti-

tude determination from vector observations”. Journal ofGuidance and Control, vol. 4, Jan.-Feb. 1981, p. 70-77., 4,pp. 70–77.

[2] Chang-Siu, E., Tomizuka, M., and Kong, K. “Time-varyingcomplementary filtering for attitude estimation”. In Intelli-gent Robots and Systems (IROS), 2011 IEEE/RSJ Interna-tional Conference on, pp. 2474–2480.

[3] Roetenberg, D., Luinge, H., and Slycke, P., 2009. “Xsensmvn: Full 6dof human motion tracking using miniature in-ertial sensors”. Xsens Motion Technologies BV, Tech. Rep.

[4] Bachmann, E. R., Xiaoping, Y., McKinney, D., McGhee,R. B., and Zyda, M. J., 2003. “Design and implementa-tion of marg sensors for 3-dof orientation measurement ofrigid bodies”. In Robotics and Automation, 2003. Proceed-

ings. ICRA ’03. IEEE International Conference on, Vol. 1,pp. 1171–1178 vol.1.

[5] Rencken, W. D., 1993. “Concurrent localisation and mapbuilding for mobile robots using ultrasonic sensors”. InIntelligent Robots and Systems ’93, IROS ’93. Proceedingsof the 1993 IEEE/RSJ International Conference on, Vol. 3,pp. 2192–2197 vol.3.

[6] Surmann, H., Nchter, A., and Hertzberg, J., 2003. “Anautonomous mobile robot with a 3d laser range finder for3d exploration and digitalization of indoor environments”.Robotics and Autonomous Systems, 45(3-4), pp. 181–198.doi: DOI: 10.1016/j.robot.2003.09.004.

[7] Madgwick, S. O. H., Harrison, A. J. L., and Vaidyanathan,R. “Estimation of imu and marg orientation using agradient descent algorithm”. In Rehabilitation Robotics(ICORR), 2011 IEEE International Conference on, pp. 1–7.

[8] Crassidis, J. L., Markley, F. L., and Cheng, Y., 2007. “Sur-vey of nonlinear attitude estimation methods”. Journal ofGuidance Control and Dynamics, 30(1), p. 12.

[9] Baerveldt, A. J., and Klang, R., 1997. “A low-cost and low-weight attitude estimation system for an autonomous heli-copter”. In Intelligent Engineering Systems, 1997. INES’97. Proceedings., 1997 IEEE International Conference on,pp. 391–395.

[10] El Hadri, A., and Benallegue, A., 2009. “Attitude estima-tion with gyros-bias compensation using low-cost sensors”.In Decision and Control, 2009 held jointly with the 200928th Chinese Control Conference. CDC/CCC 2009. Pro-ceedings of the 48th IEEE Conference on, pp. 8077–8082.

[11] Brown, R. G., 1983. Introduction to random signal analysisand Kalman filtering. Wiley, New York.

[12] Shuster, M., 1989. “Maximum likelihood estimation ofspacecraft attitude”. Journal of the Astronautical Sciences,37, pp. 79–88.

[13] Crassidis, J. L., and Junkins, J. L. Optimal estimation ofdynamic systems, Vol. 24. Chapman & Hall.

9