ATTITUDE ESTIMATION USING A TWO-STEP UNSCENTED … · 2014. 9. 20. · and stands out for being more e cient in the pro-pagation of states of nonlinear systems. This pro-cedure is

ATTITUDE ESTIMATION USING A TWO-STEP UNSCENTED APPROACH WITHGYRO BIAS ESTIMATION AND ACCELERATION CORRECTION

Guilherme S. Terra∗, Leonardo A.B. Torres∗, Bruno O.S. Teixeira∗

∗Department of Electronic Engineering - Universidade Federal de Minas GeraisBelo Horizonte, MG, Brasil

Emails: [email protected], [email protected], [email protected]

Abstract— In the last years, the interest for research on unmanned aerial vehicles (UAVs) has grown up dueto the vast field of application of these aircrafts. For such aircrafts, it is crucial to have an automatic pilot system,which also demands the knowledge of the location and spatial orientation. Thus, this work implements and testsin simulation environment a sensor fusion algorithm that is able to estimate the attitude of UAVs. The algorithmimplemented is based on the unscented Kalman filter (UKF). It corrects the measurements of accelerometers andestimates the gyro bias. It is assessed in simulation environment by means of simulated flights, considering themodeling of the uncertainties of the sensors. Also a comparison is made with an EKF-based algorithm to verifythe benefits and drawbacks of the unscented approach.

Keywords— UAV, recursive estimation, UKF.

1 Introduction

In the development of UAVs, a basic premise isto know the position and attitude of the vehicle.This information can be obtained from an inertialnavigation system, which, generally, implementssensor fusion algorithms that process the measure-ments from sensor embedded in the vehicle. ForUAV attitude estimation, the algorithm generallycombines measurements from accelerometers, gy-ros, magnetometers and eventually GPS receivers.However, these sensors have some intrinsic errorsources, such as cross sensibility, misalignment,noise and bias, that can compromise the attitudeestimate. Therefore, it is important to considerthese error sources in the estimation algorithm.

In this context, many works have consideredthe use of sensor fusion algorithms for attitudeestimation, in particular EKF-based quaternionattitude estimation algorithms (Bo and Borges,2007) (Hemerly and Schad, 2004) (Ma et al.,2012). In this work, we use the unscented ap-proach to improve the attitude estimation.

A constrained UKF-based (Tang et al., 2012)algorithm is implemented and analyzed in simu-lation environment, which considers sensor mo-dels to treat the measurement errors. Three flightsimulations are tested: a straight leveled flight,a flight with longitudinal accelerated movementand another one with typical maneuvers such asclimbs, turns and descents.

The implementation proposed in this paper isbased on the algorithm described in (Lima, 2013)called mEKF7s, which uses the EKF. This al-gorithm combines vehicle acceleration correctionsand gyro bias estimation in a pre-processing stepusing analytical linearization in the propagationof the sensor measurements. Unlike other pa-pers (Shin, 2004)(Bae and Kim, 2010)(Tang et al.,2012), the main contribution of this work is to ap-ply the unscented transform at this pre-processing

of measurements. Both approaches are analyzedin simulation environment to verify the benefitsand costs of the unscented approach.

The paper is organized as follows. In the nextsection, the problem statement is presented. InSection 3, the algorithms for nonlinear Kalman fil-ters are revised while, in Section 4, the algorithmfor attitude estimation is presented. In Section5, this algorithm is tested in simulation environ-ment. Finally, Section 6 contains the concludingremarks. This paper is based on the results pre-sented in (Terra, 2013).

2 Problem statement

Consider the stochastic discrete-time nonlineardynamic system

xk = f (xk−1, uk−1, wk−1) , (1)

where f :<n ×<p ×<q → <n, with measurementmap

yk = h (xk) + vk, (2)

where h :<n → <m and k denotes discrete time.The process noise wk ∈ <q and the measurementnoise vk ∈ <m are considered to have zero meanand known covariance matrices Qk and Rk, res-pectively. They are also assumed to be mutuallyindependent. Assume that, for all k ≥ 0, the in-put uk ∈ <p is known from sensors and, for allk ≥ 1, the output yk ∈ <m is also measured.

The state-estimation problem is to obtain anestimate for the state vector xk that maximizes aprofit function given by

J(xk) , ρ (xk|(y1, y2, . . . , yk)) . (3)

The profit function J(xk) represents the valueof conditional probability density function ofxk given the past and present measured data

Anais do XX Congresso Brasileiro de Automática Belo Horizonte, MG, 20 a 24 de Setembro de 2014

3421

y1, y2, . . . , yk. However, for nonlinear systems, theconditional probability density function could notbe completely characterized by its mean xk|k andcovariance matrix P xxk|k. Thus we use approxima-

tions based on the classical Kalman filter (KF)for linear systems to obtain suboptimal solutionfor nonlinear systems, specifically the EKF andUKF.

In this paper, the state vector xk representsthe aircraft attitude, in unit quaternion, and gyrobias. The input vector uk is given by the gyromeasurements and the output vector yk is given bythe quaternion vector inferred from sensors mea-surements (accelerometers, gyros, magnetometerand GPS receiver) using the unscented transformduring the pre-processing step.

3 Nonlinear Kalman Filtering

3.1 Extended Kalman Filter (EKF)

For nonlinear systems, an approximate solution tothe state-estimation problem is to linearize analy-tically the nonlinear model around the most cur-rent state estimate and apply the original KFequations (Simon, 2006). This algorithm is knownas the extended Kalman filter (EKF).

In the first-order EKF, the nonlinear func-tions are approximated by their respective first-order Taylor series expansions. The linearizationof the model (1)-(2) is compounded by

F x, k−1 ,∂f

∂xk−1

xk−1|k−1, uk−1, 0q×1

, (4)

Fw, k−1 ,∂f

∂wk−1

xk−1|k−1, uk−1, 0q×1

, (5)

Hx, k ,∂h

∂xk

xk|k−1

. (6)

The EKF propagates the previous estimatesthrough the nonlinear functions defined by (1)-(2).The propagation of the covariance matrices usesthe Jacobian matrices defined in (4)-(6). Thus theforecast step of the EKF has the following equa-tions:

xk|k−1 = f(xk−1|k−1, uk−1, 0q×1

), (7)

P xxk|k−1 = F x, k−1Pxxk−1|k−1F

T

x, k−1

+Fw, k−1Qk−1FT

w, k−1, (8)

yk|k−1 = h(xk|k−1

), (9)

P yyk|k−1 = Hx, kPxxk|k−1H

T

x, k +Rk, (10)

P xyk|k−1 = P xxk|k−1HT

x, k, (11)

where P xxk|k−1 is the forecast error covariance,

P yyk|k−1 is the innovation covariance, P xyk|k−1 is the

cross covariance and P xxk|k is the data-assimilationerror covariance.

With the arrival of new measurements yk, theEKF corrects the estimate propagated in the data-assimilation step, which is given by

Kk = P xyk|k−1

(P yyk|k−1

)−1, (12)

xk|k = xk|k−1 +Kk

(yk − yk|k−1

), (13)

P xxk|k = P xxk|k−1 −KkPyyk|k−1K

Tk , (14)

where Kk ∈ <n×m is the Kalman gain matrix.

3.2 Square-Root Equality-Constrained Unscen-ted Kalman Filter (SR-UKF)

There is another approximate solution that doesnot require analytical or numerical linearizationand stands out for being more efficient in the pro-pagation of states of nonlinear systems. This pro-cedure is known as the unscented Kalman filter(UKF) and it is based on statistical linearization.The UKF is based on the unscented transform(UT)(Julier, 2002), which propagates a randomvector x ∈ <n through a nonlinear function hyielding y = h(x).

A special case of UKF is called cubatureKalman filter (CKF). The CKF assumes that theprior estimate x has a Gaussian probability den-sity function (Simon and Arasaratnam, 2009). Inthis work, we use the cubature approach. Fur-thermore, in order to ensure numeric stabilityof the algorithm, we use the SR-UKF (squareroot) version. That adaptation propagates theCholesky matrix itself instead of the covariancematrix (Merwe and Wan, 2001) and it is used forthe implementation in hardware.

Basically, to perform the UT with the square-root and cubature approaches in the nonlinearpropagation y = h(x) with h :<n → <m, we usethe following equations:

X = x (11×2n) +√n[Sxx −Sxx

], (15)

Yi = h (Xi) , (16)

y =2n∑i=1

ωYi, (17)

ΠY =√ω (Y − y (11×2n)) , (18)

Syy =(QR

[ΠY]T)T

, (19)

where x, y, Sxx and Syy are, respectively, the ave-rages and the Cholesky matrices for the randomvectors of input x ∈ <n and output y ∈ <m. Fur-thermore, ω = 1

2n is the weight factor, 11×2n isa row vector of ones and QR· denotes the QRdecomposition on which just the upper triangularmatrix is returned.

The vector propagated by the unscentedtransform in the forecast step is called augmentedstate vector and includes the process noise, wk.The augmented state vector is defined by

x(a)k−1|k−1 ,

[xTk−1|k−1 wTk−1

]T, (20)


3422

with dimension na = n+ q, so that the mean andthe Cholesky decomposition of covariance matrixare, respectively

x(a)k−1|k−1 =

[xTk−1|k−1 0Tq×1

]T, (21)

Sxx(a)k−1|k−1 =

[Sxxk−1|k−1 0

0 SQk−1

], (22)

where SQk−1 is the Cholesky decomposition ofQk−1.

Applying the UT to the augmented state vec-tor, 2na sigma-points are generated. Each oneof these points can be separated into n elements

for the states X (x)i and q elements for the process

noise X (w)i as[X (x)i

TX (w)i

T]T

, X (a)i . (23)

Thus the forecast step of the SR-UKF has thefollowing equations:

X (a)k−1|k−1

= x(a)k−1|k−1

(11×2na )

+√na

[Sxx(a)k−1|k−1

−Sxx(a)k−1|k−1

], (24)

X (x)i,k|k−1

= f(X (x)

i,k−1|k−1, uk−1,X

(w)i,k−1|k−1

).(25)

xk|k−1 =

2na∑i=1

ωX (x)i,k|k−1

, (26)

ΠXk|k−1=√ω(X (x)

k|k−1− xk|k−1 (11×2na )

)(27)

Sxxk|k−1 =

(QR

[ΠXk|k−1

]T)T

, (28)

Xk|k−1 = xk|k−1 (11×2n)

+√n[Sxxk|k−1

−Sxxk|k−1

], (29)

Yi,k|k−1 = h(Xi,k|k−1

), (30)

yk|k−1 =

2n∑i=1

ωYi,k|k−1, (31)

ΠYk|k−1=√ω(Yk|k−1 − yk|k−1 (11×2n)

)(32)

Syyk|k−1

=

(QR

[ΠYk|k−1

SRk

]T)T

. (33)

Pxyk|k−1

= ΠXk|k−1

(ΠYk|k−1

)T, (34)

where SRk is the Cholesky decomposition of Rk.In the data-assimilation step, the Kalman

gain is calculated as

Kk =(P xyk|k−1/

(Syyk|k−1

T))

/Syyk|k−1. (35)

Finally, we have

x(u)k|k = xk|k−1 +Kk

(yk − yk|k−1

), (36)

Sxx(u)k|k =

(QR

[(ΠXk|k−1

−KkΠYk|k−1

)KkS

Rk

]T)T

.

Now, suppose that the state vector must sa-tisfy the nonlinear constraint

c(xk|k) = 0nc×1, (37)

where c :<n → <nc is the nonlinear constraintfunction. In order to ensure that the estimatessatisfy this constraint, we consider the projectionfunction p :<n → <n

c(p(x(u)k|k)) = 0nc×1. (38)

Therefore, the goal of the projection functionis to transform the unconstrained state into theconstrained state (Tang et al., 2012) (Teixeiraet al., 2009), i.e.,

xk|k = p(x(u)k|k). (39)

In the equality-constrained SR-UKF algo-rithm we consider a third step, called projectionstep. It is defined by the following equations:

X (u)k|k = x

(u)k|k (11×2n) +

√n[Sxx(u)k|k −Sxx(u)

k|k

](40)

X (p)i,k|k = p

(X (u)

i,k|k

)(41)

x(p)k|k =

2n∑i=1

ωX (p)i,k|k (42)

ΠX (p)

k|k=√ω(X (p)

k|k − x(p)k|k (11×2n)

)(43)

xk|k = p(x(p)k|k

)(44)

Sxxk|k =

(QR

[ΠX (p)

k|k

(xk|k − x

(p)k|k

)]T)T

.(45)

In (40)-(43), the projection is applied to theunconstrained estimate across the entire distribu-tion, while in (44)-(45) the mean is translated sothat it lies on the constraint surface.

4 The attitude estimator algorithm

The main characteristics of the EKF-based atti-tude estimator algorithm of (Lima, 2013) are theacceleration correction and the gyro bias estima-tion. This algorithm is based on two others: thefirst one considers the bias estimation of gyros(Jang and Liccardo, 2006) and the second one ischaracterized by the acceleration correction (Maet al., 2012). All of them use the unit quaternionto represent the attitude.

In this paper we use another algorithm keep-ing these characteristics, but considering the uns-cented approach. The attitude estimator dis-cussed in this work is based on the SR-UKF. Adiagram that illustrates this algorithm is shownin Figure 1. This algorithm uses the informa-tion of accelerometers, gyros, magnetometers andGPS receiver. These measurements undergo apre-processing step, in which an attitude estimateis inferred. This open loop estimate is later usedas indirect measurements in the data-assimilationstep of the SR-UKF (second step).


3423

Figure 1: Diagram of the attitude estimator

4.1 Dynamic modeling

The attitude estimation algorithm needs a dy-namic model for the SR-UKF to perform its fore-cast step. The model can be characterized by theequations of evolution (1) and observation (2) ofthe states. Where, x is compounded by attitudevector, represented by the quaternion

e =[e0 e1 e2 e3

]T,

and also estimates of the gyros bias, representedby

β =[bp bq br

]T.

Therefore, the model used by the algorithm is a7-state vector, represented by

x =[e0 e1 e2 e3 bp bq br

]T. (46)

Because of that, this algorithm is called SR-UKF7s. The input vector of the model is com-posed by the gyro measurements

u =[p q r

]T. (47)

The model used by the algorithm considersthe attitude representation by a unit quaternion,which is subject to the equality constraint

e20 + e21 + e22 + e23 = 1. (48)

In this case, the unconstrained state estimated byequations (36) must be projected onto the cons-trained surface by using (40)-(45), with

p(xk|k

)=[

ek|k||ek|k||

bp bq br]T

, (49)

where ek|k ∈ <4×1 is the quaternion vector.The relation between the unit quaternion and

the Euler angles (φ, θ e ψ), in the rotation se-quence used in aeronautics (Z → Y → X) is de-fined in (Diebel, 2006). Unlike the Euler angles,the unit quaternion has no singularities and thereexists a discrete time equation that is exact for theattitude propagation (Teixeira et al., 2009). How-ever, the attitude estimated can be better ana-lyzed and understood by the Euler angles. So,

the quaternion estimated is converted to the Eu-ler angles in the end of the algorithm using theunscented transform.

The model used in both EKF and SR-UKFconsiders the process noise as an input of the stateevolution function. In this case, the process noiseis associated to the gyro measurements, w(g), andto the bias estimation, w(β). They are signals as-sumedly Gaussian, with zero mean and the devi-ations represented by σp, σq and σr for the gyromeasurements, and σbp , σbq and σbr for the biasestimation. Therefore, the state evolution func-tion, f(x, u, w), of the equation (1) is defined by[

ekβk

]=

[Ak−1 04×303×4 I3×3

] [ek−1βk−1

]+

[04×1

w(β)k−1

], (50)

where ek−1 ∈ <4×1 is the quaternion vector andAk−1 is the matrix given by

Ak−1 = cos (sk−1) I4×4 − 0,5Tssin (sk−1)

sk−1Ω∗k−1 (51)

where sk−1 is the norm of the angular velocity,given by

sk−1 = 0,5Ts√p2 + q2 + r2, (52)

in which Ts is the sampling period. Finally, thematrix Ω∗k−1 is given by

Ω∗k−1 =

0 p∗ q∗ r∗

−p∗ 0 −r∗ q∗

−q∗ r∗ 0 −p∗−r∗ −q∗ p∗ 0

, (53)

where p∗, q∗ e r∗ are the components of the angu-lar velocity, u∗k−1, after inserting the process noiseand correcting with the estimated gyro bias, i.e.,

u∗k−1 = uk−1 + w(g)k−1 − βk−1. (54)

The state observer equation, h(x), of theequation (2) returns the own attitude in quater-nion representation, i.e,

yk =[em0,k em1,k em2,k em3,k

]T. (55)

Despite the quaternion not to be measured di-rectly by the sensors, it is calculated from the sen-sors by the pre-processing, as showed in Figure 1and described in Section 4.2.

4.2 Pre-Processing of Measurements

Before running the SR-UKF, the algorithm SR-UKF7s performs a pre-processing of measure-ments. The main goal of this pre-processing stepis, from the sensor measurements, to estimatethe quaternion vector (55). The sensor measure-ments present uncertainties associated, and be-cause of that, they are here represented by Gaus-sian statistics, whose variances are showed in Ta-


3424

ble 1. Therefore, the pre-processing of measure-ments performs a propagation of the measure-ments through nonlinear functions and, to accom-plish that, we use the unscented transform. A di-agram that illustrate the calculations realized inthis processing is showed in Figure 2.

Figure 2: Diagram of the pre-processing step ofthe attitude estimator

Table 1: Variances used in algorithmSensor Variable Variance

Accelerometer liner acceleration 223,59 × 10−9[m2/s4]

Gyro angular velocity 4,1 · 10−3[rad2/s2]

Gyro bias 2,02 · 10−8[rad2/s2]

Magnetometer magnetic field 24,1 · 10−3[Ga2]

GPS velocity 1,6 · 10−3[m2/s2]

GPS acceleration 3,2 · 10−3[m2/s4]

One of the main characteristic of this pre-processing step is the compensation of the com-ponents of gravity acceleration read from accele-rometers. Those components are disturbed whenthe aircraft performs dynamic maneuvers. In thisway, the measurements of the accelerometers (ax,ay and az) are compensated by the linear veloci-ties (vx, vy and vz) and accelerations (vx, vy andvz) and also by the angular velocities (p, q andr) measured from the gyros, all those variables inthe vehicle reference frame. Therefore, the com-ponents of the gravity acceleration in the aircraftreference frame are given by

gx = ax − (vx + vz(q − bq)− vy(r − br))gy = ay − (vy + vx(r − br)− vz(p− bp)) (56)

gz = az − (vz + vy(p− bp)− vx(q − bq)).

The linear velocities and accelerations in the air-craft reference frame are calculated from the esti-mates of velocity (vN , vE and vD) and accelera-tion (vN , vE and vD) in NED1 reference frameobtained from the GPS. It is done using the

1North East Down reference frame

most current attitude estimate, as described in(Diebel, 2006).

The roll (φ) and pitch (θ) angles can be calcu-lated from the components of the gravity accele-ration computed in equations (56), as

φm = tan−1(gygz

),

(57)

θm = tan−1(−gx cosφm

gz

).

The yaw (ψ) angle is calculated from the mea-surements of the magnetometers (hx, hy and hz).For that, it is done a decomposition of the mag-netic field with the angles φm and θm calculatedin equations (57). So,

H1 = hx cos θm + hysinφmsinθm

+hz cosφmsinθm,

(58)

H2 = hzsinφm − hy cosφm.

Therefore, disconsidering the inclination angle ofthe Earth magnetic field, the yaw angle, ψm, isgiven by

ψm = tan−1(H2

H1

). (59)

The Euler angles calculated in equations (57)and (59) are converted into the unit quaternionrepresentation. Finally, the quaternion repre-sents the measurement vector used in the data-assimilation step of SR-UKF. The square-root co-variance matrix SRk , which characterizes the un-certainty of measurements in terms of SR-UKF, iscalculated at each iteration from the propagatedrepresentations in this pre-processing according tothe unscented transform.

5 Simulated Results

5.1 Modeling Sensors

In the implementation of the sensor fusion algo-rithm, it is necessary to know models that repre-sent the relations between the variables measuredby the sensors. In this work, the algorithm usesmeasurements from accelerometers, gyros, magne-tometers and GPS receiver. Basically, from idealmeasurements, the model must simulate the ac-tual measurements with the main characteristicerrors of the sensors.

A model that analyses the triaxial sensors(accelerometers, gyros and magnetometer) is des-cribed in (Lima, 2013). The model considers thefollowing error sources: misalignment and cross-sensitivity of the axes, scale factor, noise, biasand quantization during the A/D conversion. So,the measure obtained from the model of sensors,


3425

m ∈ <3×1, can be written from ideal measure-ment, m ∈ <3×1, by

m = fA/D (S (Rm) +B +N) , (60)

where S ∈ <3×3 is the matrix that characterizesthe scale factor and also the cross-sensitivity error;R ∈ <3×3 is the matrix that models the misalign-ment between the axes; B ∈ <3×1 is the vectorthat describes the bias error; N ∈ <3×1 is thevector with the measurement noise, and finally,the function fA/D characterizes the quantificationin the A/D conversion.

The values for the parameters used in themodeling of the sensors approached in this workare obtained from (Lima, 2013). Some of themare found experimentally and others are obtainedfrom sensor datasheet. The information about thescale factor, cross sensibility, misalignment andquantification are obtained from the datasheets ofthe sensors: accelerometers 2, gyros 3 and mag-netometer 4. These values are shown in Table 2.The information about the noise and bias wereobtained experimentally by the Allan’s variancemethod (Iozan et al., 2010). The estimated noiselevel and bias are shown in the Table 3.

Table 2: Information from Sensor datasheetsParameter Accelerometer Gyro Magnetometer

Scale Factor 10% 6% 5%

Cross sensibility 1% 2% 0,2%

Misalignment 0,1 0,1 -

A/D Resolution 10 bits 16 bits 12 bits

Quantification 0,0039 g 0,0092 /s 0,0012 Ga

Table 3: Information from experiments and Al-lan’s variance method

Sensor Axis Noise variance bias

Accelerometeraxayaz

223,59 × 10−9

207,80 × 10−9

266,29 × 10−9(m/s2)2

−28,95−20,1013,69

mg

Gyrobxbybz

32,93 × 10−6

68,81 × 10−6

30,67 × 10−6(/s)2

−2,8354,345−0,728

/s

Magnetometerhxhyhz

48,12 × 10−6

44,04 × 10−6

39,08 × 10−6Ga2

000

Ga

The GPS receiver provides an estimate for thevelocity and acceleration in the NED referential.In this case, our modeling considers only the noiseas source of errors. The noise variance for the ve-locity and acceleration are obtained from (Zhouet al., 2012) and shown in Table 1. However, weneed to consider that the data acquisition rate forthe GPS receiver is lower than others sensors. Be-cause of that, the model for the GPS makes adecimation of 10 of the output signal to representit with more fidelity.

5.2 Simulation environment

The ideal values for the measurements are ob-tained by an aircraft dynamic model. The data

2Analog Devices: 3-Axis Digital Accel ADXL345.3Inven Sense: 3-Axis MEMS Digital Gyro ITG-3200.4Honeywell: 3-Axis Digital Compass IC HMC5883L.

are generated with flight simulations, in whicha small aircraft is modeled as an rigid bodywith six degrees of freedom (6DOF) (Stevens andLewis, 2003). Beyond the ideal measurements ofthe sensors, the flight simulation also returns theideal values of attitude (φ, θ e ψ) for comparisonwith the estimates of the algorithms analyzed, asillustrated in the Figure 3. The simulated datawere generated with the software MATLAB at therate of 50 Hz (20 ms).

Figure 3: Simulation flowchart.Adapted from (Lima, 2013).

The algorithm is analyzed in three flight simu-lations with different maneuvers. The first flighttest simulates leveled straight flight, a simple con-dition just to verify the operation of the algorithm.In the second flight simulated, there is a commandin the elevator which characterizes a longitudinalacceleration in the aircraft capable of distortingthe accelerometer measures. Finally, the thirdflight is composed by typical maneuvers, such asrise, curve and descent.

The parameters used by the algorithm SR-UKF7s are the initial state estimate and the valuesof uncertainties. These parameters were tunedfrom the noise variances observed in the Table 3.The values were slightly tuned until the errors be-tween the estimated and ideal attitude were con-sistent with the covariance. The mean and thecovariance matrix for the initial estimate are

x0|0 =[1 0 0 0 0 0 0

]T, (61)

P0|0 =

[10−3 · I4×4 03×4

04×3 10−2 · I3×3

]. (62)

The variances used in the algorithm are shown inthe Table 1.

5.3 Analysis of the simulated results

The attitude estimated by SR-UKF7s andmEKF7s algorithms and the ideal attitude areshown in the Figures 4,5 and 6. Furthermore,the ideal and estimated values for the gyro biasfor the third flight simulation are shown in theFigure 7. The quality of the attitude estimate ismeasured by the RMSE index (root mean squareerror), given by

RMSEi =

√√√√ 1

N

N∑k=1

(xi,k − xi,k)2, (63)


3426

where N is the number of samples, xi,k is the idealvalue and xi,k is the estimated value of the i-thentry of the state vector. For this analysis, thealgorithms SR-UKF7s and mEKF7s use the sameparameters values described in Section 5.2. TheRMSE index for both algorithms in the three flightsimulations are shown in the Table 4.

Table 4: RMSE for attitude estimatesFlight Algorithm φ θ ψ

1mEKF7s 1,00 0,52 0,94

SR-UKF7s 0,51 0,64 0,51

2mEKF7s 1,43 1,07 1,49

SR-UKF7s 0,62 0,80 0,57

3mEKF7s 4,15 2,41 3,91

SR-UKF7s 1,09 0,90 1,00

Figure 4: Attitude estimation for flight 1



In general, the attitude estimated by the SR-UKF7s yielded lower values for the RMSE index.The sum of indices for the three angles with SR-UKF7s is about 32% lower than mEKF7s for thefirst flight simulation, 50% for the second and 71%for the third. It shows that the SR-UKF7s main-tained a good estimation even when increasing the

Figure 7: Bias estimation for flight 3

complexity of the maneuvers.However, the unscented approach usually is

more computationally expensive than EKF. So, itis expected that the SR-UKF7s takes more pro-cessing time than mEKF7s. In fact, in the threeflight simulations, the mean of processing timespent in each sampling time is 5.6ms and 1.0ms forSR-UKF7s and mEKF7s, respectively. These arethe processing times in MATLAB/Simulink withprocessor Intel Core 2 Duo 1.83GHz and 2GB ofRAM memory. The processing time is an impor-tant point for implementation in hardware.

The consistence of the Kalman algorithm canbe verified by the graph that confronts the error ofeach state variable with their respective deviations(σ), also estimated by the algorithm. For the thirdflight simulation, the errors of the Euler anglesand their respective limits of ±3σ are shown inFigure 8. Note that, the errors of the estimate arewithin the range of ±3σ, which indicates that theestimates are consistent.

Figure 8: SR-UKF7s: Attitude error for flight 3

One of the main contributions of the imple-mentation proposed in this work is the use of theunscented approach in the pre-processing step ofmeasurements, as described in Section 4.2. In thiscase, the covariance matrix of measurements fromthe viewpoint of the SR-UKF, Rk, is recalculatedat each iteration using the unscented approach.The elements of the main diagonal of the matrixRk, along the third flight simulation, can be ob-served in the innovation graph, which is shownin Figure 9. Note that, the innovation remainedwithin the range of ±3σ, which indicates that the


3427

result of the pre-processing step is consistent.

Figure 9: SR-UKF7s: Innovation for flight 3

6 Concluding Remarks

In this paper, we propose the implementationof an attitude estimator algorithm that uses theunscented approach. The algorithm, called SR-UKF7s, was analyzed in simulation environmentand compared with the EKF-based algorithm im-plemented at (Lima, 2013), called mEKF7s. Bothalgorithms use measurements from accelerome-ters, gyros, magnetometers and GPS receivers.These sensors address errors sources, such as mis-alignment, cross sensibility, bias and noise, whichwere modeled according to their datasheets andexperimental tests. Furthermore, one of the maincontributions of this work is to apply the unscen-ted transform to the pre-processing of measure-ments.

Three flight simulations were tested and theattitude estimated by the two algorithms is com-pared with the true values. The SR-UKF7syielded better RMSE indexes for attitude and biasestimations. For SR-UKF7s, the sum of RMSEindexes for the three Euler angles was 32%, 50%and 71% lower than mEKF7s at first, second andthird flight simulations, respectively. However,the average processing time spent for SR-UKF7sis about five times larger than it is for mEKF7s.Therefore, from simulation environment, it is sug-gested that the unscented approach can improvethe attitude estimation, even if it costs a largerprocessing time compared to EKF.

Acknowledgements

This work has been partially supported by CNPq,FINEP and FAPEMIG. We also thank to thePDVA research group for helpful assistance.

References

Bae, J. and Kim, Y. (2010). Attitude Estimation for Satel-lite Fault Tolerant System Using Federated Unscen-ted Kalman Filter, Int J. of Aeronautical e Space Sci.11(2):80-86.

Bo, A. P. L. and Borges, G. A. (2007). Sistema de baixocusto para determinacao da atitude com aplicacaoem VANTs, VIII Simposio Brasileiro de AutomacaoInteligente (SBAI), 2007(1):1-6.

Diebel, J. (2006). Representing Attitude: Euler Angles,Unit Quaternions, and Rotation Vectors, Matrix,Citeseer.

Hemerly, E. M. and Schad, V. R. (2004). Sistema de nave-gacao de baixo custo baseado na fusao INS/GPS us-ando filtro de Kalman, XV Congresso Brasileiro deAutomatica.

Iozan, L., Rusu, C., Collin, J. and Takala, J. (2010). AStudy of the External Factors that Affect the Mea-surement Data of a MEMS Gyroscope Sensor - To-wards an Inertial Navigation System, Electronicsand Telecommunications (ISETC), 9th InternationalSymposium, p. 81-84. IEEE.

Jang, J. and Liccardo, D. (2006). Automation of SmallUAVs using a Low Cost MEMS Sensor and Embed-ded Computing Platform, IEEE/AIAA 25th DigitalAvionics Systems Conference. p. 1-9.

Julier, S. (2002). The scaled unscented transformation,Proceedings of the American Control Conference,6(1):4555-4559.

Lima, R. R. (2013). Desenvolvimento de uma Cabeca Sen-sora para Veıculos Aereos Nao-Tripulados, Disser-tacao de Mestrado. Programa de Pos Graduacao emEngenharia Eletrica, UFMG.

Ma, D. M., Shiau, J. K., Wang, I. C. and Lin, Y. H. (2012).Attitude Determination Using a MEMS-Based FlightInformation Measurement Unit, Sensors 12(1):1-23.

Merwe, R. v. d. and Wan, E. A. (2001). The Square-RootUnscented Kalman Filter for State and Parameter-Estimation, Oregon Graduate Institute of Scienceand Technology. 6(1):3461-3464.

Shin, E. (2004). A Quaternion-Based Unscented KalmanFilter for the Integration of GPS and MEMS INS,Proceedings of the 17th International TechnicalMeeting of the Satellite Division of The Institute ofNavigation.

Simon, D. (2006). Optimal State Estimation - Kalman,H∞ and nonlinear approaches, John Wiley & Sons,Inc.

Simon, H. and Arasaratnam, I. (2009). Cubature KalmanFilters, IEEE Trans. Automat. Contr. 54(6):1254-1269.

Stevens, B. L. and Lewis, F. L. (2003). Aircraft Controland Simulation, John Wiley & Sons, Inc. 2rd Edition,Canada.

Tang, X., Liu, Z. and Zhang, L. (2012). Square-root quater-nion cubature Kalman filtering for spacecraft attitudeestimation, Acta Astronautica, 76(1):84-94.

Teixeira, B. O., Chandrasekar, J., Torres, L. A. andAguirre, L. A. (2009). State estimation for linearand non-linear equality-constrained systems, Interna-tional Journal of Control, 82(5):918-936.

Terra, G. (2013). Fusao Sensorial para um Mini-VeıculoAereo Nao-Tripulado, Trabalho Final de Graduacaoem Engenharia de Controle e Automacao - Escola deEngenharia - UFMG.

Zhou, J., Knedlik, S. and Loffeld, O. (2012). INS/GPSfor High-Dynamic UAV-Based Applications, Inter-national Journal of Navigation and Observation,2012(1):1-11.


3428

Documents

ATTITUDE ESTIMATION USING A TWO-STEP UNSCENTED … · 2014. 9. 20. · and stands out for being more e cient in the pro-pagation of states of nonlinear systems. This pro-cedure is