Extended Kalman Filtering for Robot Joint

Angle Estimation Using MEMS Inertial

Sensors

Yizhou Wang ∗ Wenjie Chen ∗ Masayoshi Tomizuka ∗

∗ Department of Mechanical Engineering, University of California,Berkeley (e-mail: {yzhwang@,wjchen@,tomizuka@me.}berkeley.edu)

Abstract: The possibility of utilizing low-cost MEMS accelerometers and gyroscopes foraccurate joint position estimation of the robot manipulator is investigated in this paper. CascadeKalman filtering formulation is derived from the robot forward kinematics and the stochasticmodels of the joint motion sensors. We validate the accuracy of the proposed algorithm viaexperimentation. We also discuss the effect of the nonlinearity in the kinematic model on twoapproximation methods - the first-order linearization and the unscented transform.

NOMENCLATURE

θi i-th joint angular position

θi i-th joint angular velocity

θi i-th joint angular accelerationδθi i-th joint estimation error angle by gyro integrationωi i-th gyroscope measurementβi i-th gyroscope bias driftai i-th accelerometer measurementFi i-th joint frameSi i-th sensor frame

Rj

irotation matrix of Fi with respect to Fj

ωj

iangular velocity of link i expressed in Fj

αj

iangular acceleration of link i expressed in Fj

pj

ilinear acceleration of Fi’s origin expressed in Fj

(•)i variable related with joint i expressed in Siri

i−1,i vector from Fi−1’s origin to Fi’s origin w.r.t Fi

di vector from Fi−1’s origin to Si’s origin w.r.t Sigi gravity vector w.r.t Fi

1. INTRODUCTION

The utilization of humanoid robots, such as PR2 (Acker-man [2011]) and NAO (Niemuller et al. [2011]), in house-holds and workspaces can be popularized, or this coursemight be expedited if their costs are reduced. For theseapplications, since milimeter order manipulation errorsare less likely to cause drastic performance degradation,low-cost microelectromechanical-systems (MEMS) basedinertial sensors can be used for revolute joint angle esti-mations replacing traditional expensive optical encoders.A representative cost comparison has been made in Roanet al. [2012], where fourfold to tenfold cost improvementcan be achieved by using inertial sensors.

Other advantages of using inertia sensors are (i) the easeand flexibility of installation, (ii) the reduction of thestructural complexity, especially for passive joints, and(iii) the fact that they can work as a redundant sensingsystem together with existing encoders for safety. Theadvancement of MEMS technology further enables theincreased accuracy and reliability and the decreased priceand size of the MEMS sensors.

Prior research has been done on utilizing different com-binations of inertial sensors for the purpose of joint angleestimation. Chen and Tomizuka [2012] developed a two-stage sensor fusion algorithm to estimate the load side jointspace information. The method, however, was targetingat industrial applications with the sensor configurationof motor encoders and end-effector MEMS accelerometer.In consideration of cost improvement and reduction ofstructural complexity, Quigley et al. [2010] proposed anextended Kalman filtering scheme by measuring gravityin the joint frame and relating the joint angles with themeasurements using rotation matrices. The estimationbreaks down when the rotation axis is parallel to thegravity. Roan et al. [2012] developed a complementaryfilter fusing the angle estimate from the difference betweenconsecutive acceleration vectors and the rate measurementfrom a uniaxial gyroscope. The singularity configurationcan be dealt with by the gyroscope when the accelerometercannot provide useful information. Cheng and Oelmann[2010] presented a detailed review on four different jointangle estimation methods specifically for the planar armconfiguration.

Most of the aforementioned works utilize only the gravitycomponent in the measured acceleration while neglectingthe centripetal and centrifugal components. This is likelyto affect the estimation accuracy, especially in the situa-tion where the manipulator undergoes fast motions.

In this paper, this problem is tackled by adopting a fullkinematic model for the acceleration and the angularrate measurements. An extended Kalman filtering (EKF)method is proposed to estimate joint space variables inrobotic manipulators. The measurements are made withlow-cost MEMS tri-axial accelerometers and uni-axial gy-roscopes, in place of traditional optical encoders. Estima-tion accuracies are demonstrated by experiments.

The organization of the paper is as follows. In Section2, some preliminaries are first reviewed, including themathematical notations, the link relative angular rate and

6th IFAC Symposium on Mechatronic SystemsThe International Federation of Automatic ControlApril 10-12, 2013. Hangzhou, China

978-3-902823-31-1/13/$20.00 © 2013 IFAC 406 10.3182/20130410-3-CN-2034.00021

Fig. 1. Definition of the joint frames and the sensor frames.

accelerations as well as the stochastic measurement modelsof the inertial sensors. Section 3 presents the derivation ofan extended Kalman filter for accurate joint angle estima-tions. In Section 4, the performance of the proposed algo-rithm is validated by experiments, by comparing the es-timates with the high-resolution optical encoder measure-ments. Section 5 discusses the effect of the measurementmodel nonlinearity on the estimation accuracy. Finally, theconclusion is drawn in Section 6.

2. PRELIMINARIES

Consider rigidly attaching a coordinate frame Fi to eachlink i of the n-DOF robot manipulator at the i+1th jointrotation axis, with F0 denoting the world frame. Theoverall description of the manipulator can be obtainedrecursively from the kinematic relationship between con-secutive links. The Denavit-Hartenberg convention (DH)is adopted to further define Fi.

The proposed algorithm uses a uni-axial gyroscope anda tri-axial accelerometer for each joint. To simplify the de-scription of the position and the orientation of the sensors,assumptions are made that:

(1) the sensor frame Si is rigidly attached to link i,i ∈ {1, · · · , n}

(2) the orientation of Si is the same as that of Fi−1 atthe robot home position, i ∈ {1, · · · , n}

(3) the axes of the i-th accelerometer are aligned withthose of the sensor frame Si

(4) the measurement axis of the i-th gyroscope is alignedwith the z axis of Si

(5) the origin of Si is the accelerometer measurementpoint

The proposed joint frames and sensor frames are depictedin Fig. 1.

2.1 Robot forward differential kinematics

This section briefly reviews the relationship between theangular velocities, the angular accelerations and the linearaccelerations of consecutive joints as a function of thevariables in joint space θi, θi, θi, i ∈ {1, ..., n}.

It is assumed that each joint provides the mechanicalstructure with a single DOF, parameterized by the joint

variable. For each revolute connection, the relationship canbe expressed as follows (detailed in Siciliano et al. [2009]),

ω0i = ω

0i−1 + θiz

0i−1 (1)

α0i = α

0i−1 + θiz

0i−1 + θiω

0i−1 × z

0i−1 (2)

p0i = p

0i−1 +α

0i × r

0i−1,i + ω

0i × (ω0

i × r0i−1,i) (3)

where ω,α, and p are the angular velocity, the angularacceleration and the linear acceleration respectively. z0

i−1is the z-axis of the frame Fi−1 expressed in the frame F0.In the expressions above, all vectors are expressed withrespect to the base frame F0.

The recursion is computationally more efficient and suit-able, if the aforementioned vectors are referred to thecurrent frame Fi. Hence, the following forward recursionsare derived,

ωii = (Ri−1

i )T (ωi−1i−1 + θiz

00) (4)

αii = (Ri−1

i )T (αi−1i−1 + θiz

00 + θiω

i−1i−1 × z

00) (5)

pii = (Ri−1

i )T pi−1i−1 +α

ii × r

ii−1,i + ω

ii × (ωi

i × rii−1,i) (6)

where z00 = [0 0 1]T , rii−1,i is the position vector from

the origin of Fi−1 to the origin of Fi expressed in Fi.

Rji denotes the rotation matrix of Fi with respect to

Fj . From this recursive formula, once the joint variables

θj , θj , θj , ∀j ∈ {1, ..., i}, the velocity ω00 and the accelera-

tion (α00, p

00) of the base frame are known, ωi

i ,αii, and p

ii

can be easily computed. The value of p00 is conveniently

set to p00 − g0, where g0 is the gravity expressed in F0, so

that the rotated gravity is included in pii.

2.2 Sensor measurement model

Mathematically, the measurement model of the gyroscopecan be written as,

ωi = ωi,true + βi + ηwi

βi = ηβi(7)

where the subscript i denotes the joint index, ωi,true is thetrue angular velocity, βi is a drifting bias. The noises ηwi

and ηβi in the model are usually referred to as the anglerandom walk (ARW) and the rate random walk (RRW)respectively. Since the z-axes of Si and Fi−1 are aligned,

θi can be measured by the i-th gyroscope. Namely, fromthe z-axis component of Eqn. (4), we have

ωi,true = ωi−1i−1,z + θi (8)

where (•)z denotes the z-axis component of the variable •.The accelerometer measurement can be written as,

ai = pi,true + ηai (9)

where pii,true represents the actual linear acceleration of

the origin of Si in F0 expressed in Si. ηai is the accelerom-eter measurement noise. We assume that the accelerometerbias is accounted for by the sensor calibration. This isnormally valid for constant or slow time-varying bias.The small bias variation will not cause any drift problemlike the gyroscope due to the fact that the accelerometermeasurement will not be integrated in our algorithm. Itis noted that gi, representing the gravity accelerationprojected onto Si, is hidden in the equation as it is al-ready lumped into the first term in the recursive formula

IFAC Mechatronics '13April 10-12, 2013. Hangzhou, China

407

discussed in the forward kinematics.

Utilizing Eqns. (5) and (6), we set the rotation matrixR

i−1i to I3×3 and replace r

ii−1,i by di, which is the vector

from the origin of Fi−1 to the origin of Si expressed in Si.Then, the true measurement of the i-th link accelerationcan be written explicitly as a function of the angular ve-locity ω

i−1i−1 , the angular acceleration α

i−1i−1, and the linear

acceleration pi−1i−1 of the previous joint, and the current

joint variables {θi, θi, θi} 1 ,

pi,true = h(ωi−1i−1 ,α

i−1i−1, p

i−1i−1, θi, θi, θi,di) (10)

The noises ηwi, ηβi, and ηai, i = {1, · · · , n}, are assumedto be independent, zero-mean, white, and Gaussian dis-tributed, i.e.

E

{[ηwi(t)ηβi(t)

] [ηwi(τ)ηβi(τ)

]T }=

[σ2wi 00 σ2

βi

]δ(t− τ)

E{ηai(t)ηTai(τ)} = diag(σ2

axi, σ2ayi, σ

2azi)δ(t− τ)

(11)

where δ(t − τ) is the Dirac-delta function. Using thesestochastic models of the gyroscope and the accelerometermeasurements, an extended Kalman filtering formulationis derived in the next section.

3. EXTENDED KALMAN FILTERING

This section derives a cascade third-order extended Kalmanfilter for each joint. Namely, during the processing of the i-th EKF, the a-posteriori estimates of the states associatedwith the previous i − 1 filters are utilized. The compu-tational burden is reduced by this sequential structurebecause n 3×3 matrices are much easier to process than a3n×3nmatrix (if a single EKF is used to include all joints).However, the estimation accuracy may be compromised inthe cascade configuration.

The three states of the i-th filter are defined to be thejoint error angle δθi, the bias βi, and the joint angularacceleration θi. The error angle by integration of the gy-roscope measurement is defined as,

δθi(t) =

∫ t

0

ωidτ −∫ t

0

ωi−1i−1,zdτ − θi(t)

=

∫ t

0

(βi + ηwi)dτ

(12)

The filter dynamics for the i-th joint can thus be expressedas,

d

dt

δθiβi

θi

︸ ︷︷ ︸x(t)

= F

δθiβi

θi

︸ ︷︷ ︸x(t)

+G

[ηwi

ηβiηγi

]

︸ ︷︷ ︸υ(t)

where F =

[0 1 00 0 00 0 0

], G =

[1 0 00 1 00 0 1

] (13)

and a fictitious noise ηγi with standard deviation σγi isused to represent the unknown jerk.

1 The expression of the measurement model h can be found in theappendix.

Initialization

δθ−

i(0) = δθi,o θ

−

i(0) = θi,o

β−

i(0) = βi,o

ˆθ−

i(0) = θi,o

P−

i(0) = Pi,o

Measurement measure ωi(k) and ai(k)

Known θ+j(k),

ˆθ+j(k),

ˆθ+j(k), j = 1, ..., i− 1

Kinematicscompute ω

i−1i−1(k), α

i−1i−1(k),

ˆpi−1i−1(k)

using Eqns (4)–(6)

Filter Gain

ˆθ−

i(k) = ωi(k)− ω

i−1i−1,z(k)− β

−

i(k)

Hi(k) = H(ωi−1i−1 , α

i−1i−1,

ˆpi−1i−1, θ

−

i,ˆθ−

i,ˆθ−

i,di)

Ki(k) = P−

i(k)HT

i(k)

·

[Hi(k)P

−

i(k)HT

i(k) +Rd

]−1

Correction

[ δθ+

i(k)

β+

i(k)

ˆθ+

i(k)

]=

[ δθ−

i(k)

β−

i(k)

ˆθ−

i(k)

]+Ki(k)

[ai(k)

−h(ωi−1i−1 , α

i−1i−1,

ˆpi−1i−1, θ

−

i,ˆθ−

i,ˆθ−

i,di)

]P

+i(k) = [I3×3 −Ki(k)Hi(k)]P

−

i(k)

θ+i(k) = θ

−

i(k) + δθ

−

i(k)− δθ

+i(k)

ˆθ+i(k) = ωi(k)− ω

i−1i−1,z(k)− β

+i(k)

Propagation

δθ−

i(k + 1) = β

+i(k)∆t, i.e., reset δθ

+i(k) = 0

θ−

i(k + 1) = θ

+i(k) +

ˆθ+i(k)∆t

β−

i(k + 1) = β

+i(k)

ˆθ−

i(k + 1) =

ˆθ+i(k)

P−

i(k + 1) = FdP

+i(k)FT

d+GdQdG

T

d

Table 1. Cascade extended Kalman filteringprocedure for the i-th joint angle estimation.

The system matrices and the process noise covariancematrix associated with the discretized system Fd, Gd, Qd

are derived below with the sampling time ∆t,

xd(k + 1) = Fdxd(k) +Gdυd(k)

Fd =

[1 ∆t 00 1 00 0 1

], Gd =

∆t

1

2∆t2 0

0 ∆t 00 0 ∆t

Qd =

σ2wi∆t+

1

3σ2βi∆t3

1

2σ2βi∆t2 0

1

2σ2βi∆t2 σ2

βi∆t 0

0 0 σ2γi∆t

(14)

The measurement model of the Kalman filter, which is thecomposition of Eqns. (9) and (10), is a nonlinear functionof the state variables

ai = h(ωi−1i−1 ,α

i−1i−1, p

i−1i−1, θi, θi, θi,di) + ηai (15)

The Jacobian matrix of h with respect to the states,H = ∇h, when evaluated at the a-priori estimate of the

IFAC Mechatronics '13April 10-12, 2013. Hangzhou, China

408

current filter state {θ−i ,ˆθ−i ,

ˆθ−i }, is written as 2 ,

H = H(ωi−1i−1 , α

i−1i−1,

ˆp

i−1

i−1, θ−

i ,ˆθ−i ,

ˆθ−i ,di) (16)

where ωi−1i−1 , α

i−1i−1,

ˆp

i−1

i−1 are obtained from the a-posteriori

estimates of the previous joint variables {θ+j ,ˆθ+j ,

ˆθ+j | j =

1, . . . , i− 1}. The measurement noise covariance matrix inthe discrete-time formulation is

Rd = R/∆t = diag(σ2axi, σ

2ayi, σ

2azi)/∆t (17)

The detailed procedure of the proposed extended Kalmanfilter for the i-th joint is summarized in Tab. 1. Byincorporating the acceleration information, the proposedalgorithm is drift-free, as long as the axis of rotation isnot always parallel to the gravity. Also, the algorithm isexpected to work for general motions, due to the inclusionof the motion acceleration in the model.

4. EXPERIMENTAL RESULTS

The performance of the proposed extended Kalman fil-tering method is evaluated via experiments. Figure 2(a)shows the 9-DOF IMU developed in the Mechanical Sys-tems Control Laboratory at UC Berkeley. It has an Ar-duino Pro Mini microprocessor and a Sparkfun 9-DOF sen-sor stick (ADXL345 accelerometer, HMC5843 magnetome-ter, ITG-3200 gyroscope) onboard. In the experiments,the tri-axial accelerometers and only one axis of eachgyroscope are actually utilized. The sampling frequency isset to 75Hz due to hardware limitation since the clock rateof the processor is only 8MHz. The raw data is transferedto the external computer and recorded. The joint angleestimates are calculated offline using the same samplingrate of 75 Hz.

During experiments, two IMUs are rigidly attached to thered and blue gimbals of the Quanser 3-DOF gyroscope inFig.2(b), which serve as two consecutive revolute jointsand are moved manually in arbitrary trajectories. Theouter rectangular gimbal is fixed. The estimation accu-racy is evaluated by comparing the estimates with themeasurements from 5000 pulses per revolution quadratureencoders.

Two other estimation schemes are also performed andcompared. One is simply from integrating the gyroscopemeasurements (abbr. GYRO), which suffers from biasdrift. The other (abbr. ACC) is to treat the accelerometermeasurements as the gravity vector related with the jointangle as [

gi,xgi,ygi,z

]=

[cos θi − sin θi 0sin θi cos θi 00 0 1

][gi−1,x

gi−1,y

gi−1,z

](18)

where gi,• denotes the gravity component on the •-axis ofthe frame Fi. By doing this, it is assumed that the motionacceleration is negligible. The above equation provides twotranscendental equations in θi which can be solved for thejoint angle as follows

θi = atan2(gi,ygi−1,x − gi,xgi−1,y, gi,xgi−1,x + gi,ygi−1,y)(19)

2 The expression of the Jacobian matrix H can be found in theappendix.

9-DOF Sensor Stick

(a) (b)

Fig. 2. (a) The 9-DOF inertial measurement unit and (b)The quanser 3-DOF gyroscope (size: 0.7m × 0.5m ×0.5m).

20 30 40 50 60 70 80 90 100−100

−50

0

50

100

Angle(deg) EKF Encoder

20 30 40 50 60 70 80 90 100−100

−50

0

50

100

velocity(deg/s)

t ime (sec)

86.8 87 87.2 87.4 87.6 87.8

42

44

46

48

50

52

54

Fig. 3. The EKF estimates of the joint angle position andvelocity of the first joint compared with the encodermeasurements

30 40 50 60 70 80 90 100−8

−6

−4

−2

0

2

4

6

8

Error

angle(deg)

time (sec)

ACC GYRO EKF

Fig. 4. First joint estimation errors from various methods.The RMS error of the EKF is 1.52◦

Figure 3 and 5 show that the EKF estimates of the jointangle position and velocity follow the encoder measure-ments very closely. Some delays can be observed in thezoom-in plots. The estimation errors are plotted in Fig. 4and 6. The RMS errors and peak errors are tabulated forvarious methods in Tab.2.

As expected, the ACC estimates tend to be noisy andtheir accuracy is sensitive to how well the slow motionassumption is justified. On the other hand, the GYROestimates are accurate for a short-time duration (Joint 1)but their drifts are accumulated and cannot be corrected(Joint 2).

IFAC Mechatronics '13April 10-12, 2013. Hangzhou, China

409

50 60 70 80 90 100

−100

−50

0

Angle(deg) EKF Encoder

50 60 70 80 90 100

−100

−50

0

50

100

velocity

(deg/s)

t ime (sec)

71 71.5 72 72.5 73

−40

−20

0

20

Fig. 5. The EKF estimates of the angle position andvelocity of the second joint compared with the encodermeasurements

50 60 70 80 90 100−15

−10

−5

0

5

10

15

Error

angle(deg)

time (sec)

ACC GYRO EKF

Fig. 6. Second joint estimation errors from various meth-ods. The RMS error of the EKF is 1.66◦

Method rms error [◦] peak error [◦]

Joint 1

EKF 1.52 4.41GYRO 1.45 4.04ACC 1.88 19.45

Joint 2

EKF 1.66 6.93GYRO 3.14 9.21ACC 2.16 45.22

Table 2. Estimation accuracy

5. DISCUSSION

It is well known that the EKF algorithm only providesan approximation to optimal nonlinear system state esti-mation. Namely, the state distribution is approximated bya Gaussian random variable, which is propagated throughthe first-order linearization of the nonlinear system. There-fore, the approximated posterior mean and covarianceagree with the true ones only up to the first order. It isnatural to ask how well the EKF deals with the nonlinearmeasurement model in this application. Do alternativeforms of Kalman filter, such as the unscented Kalmanfilter, likely improve the estimation accuracies?

Julier and Uhlmann [1997] proposed the unscented trans-form (UT) and rigorously proved that it preserves theposterior mean and covariance up to the second order.However, it is not guaranteed that the conditional expec-tation of a random variable, conditioned on the realizedvalue of a nonlinear measurement and calculated from theunscented transform, is more accurate than that from thefirst-order linearization (FOL). We demonstrate this for

our application using the simulated distribution of therandom state variable X and the measurement output Yat one time step.

5.1 Accuracy of the approximated posterior mean andcovariance (a-priori estimate of the output in KF context)

We consider a three-dimensional Gaussian random vari-

able X = [θ, θ, θ]T with mean x = [θ,ˆθ,

ˆθ] = 0 and

covariance Λxx = diag(0.001, 0.001, 0.001) (SI units). Themeasurement Y , which is a nonlinear function of X, isgiven by,

Y = h(ω,α, p, θ, θ, θ,d) (20)

where ω = [0, 1.2498, 0]T (rad/sec), α = [0,−0.2279, 0]T

(rad/sec2), p = [−6.6696, 0,−7.2059]T (m/sec2), d =[−0.029, 0.003, 0.051]T (m) which are from the real data.To see the effect of the nonlinearity only, we assume nomeasurement noise in Y for simplicity. In order to evalu-ate the accuracies of the posterior mean and covariance,N = 10000 samples of X are generated. They are thenpropagated through h to obtain the distribution of Y .

The first-order linearization and the unscented transformare utilized to approximate the posterior mean and co-variance. The actual sample mean and covariance arecalculated by,

y =1

N

N∑i=1

yi (21)

Λyy =1

N − 1

N∑i=1

(yi − y)(yi − y)T (22)

The first-order linearization leads to (Welch and Bishop[1995]),

yFOL = h(ω,α, p, θ,

ˆθ,

ˆθ,d)

ΛFOLyy

= HΛxxHT

ΛFOLxy

= ΛxxHT

(23)

where H = H(ω,α, p, θ,ˆθ,

ˆθ,d) is the Jacobian of h with

respect to the states. The computations of yUT , ΛUT

yy,

and ΛUTxy

are detailed in Wan and Van Der Merwe [2000].

Figure 7 compares yFOL and y

UT with y, plotted withthe parabolic distribution of Y . As expected, yUT approx-imates y more accurately than y

FOL. Figure 8 comparesthe three covariance ellipsoids centered at the correspond-ing means. Any point v on each ellipsoid satisfies,

(v − y(•))TΛ(•)−1

yy(v − y

(•)) = 1 (24)

with the corresponding mean and covariance where (•)is UT or FOL. It can be seen that ΛUT

yyalso has better

accuracy than ΛFOLyy

, which is a much thinner ellipsoid. In

fact, as seen in Fig. 9, ΛFOLyy

is tangent to the distribution

of Y , projected on ax-ay plane. On the other hand, ΛUTyy

better captures the parabolic shape of the distribution ofY open in the +ax direction, while ΛFOL

yyfails.

5.2 Accuracy of the conditional expectation (a-posterioricorrection of the state)

The Kalman filter is derived based on the conditionalexpectation result. Namely, given two jointly Gaussian

IFAC Mechatronics '13April 10-12, 2013. Hangzhou, China

410

−6.63−6.62

−6.61−6.6

−6.59−0.5

0

0.5

−7.293

−7.2925

−7.292

−7.2915

−7.291

ay (m/s2)ax (m/s2)

az(m

/s2)

−6.64

−6.638

−6.636

−6.634

−6.632

−6.63

−6.628

−6.626

−6.624

−6.622

−6.62

−0.4−0.200.20.4

ay (m/s2)

ax(m

/s2)

samples

sample mean

UT mean

FOL mean

Fig. 7. The UT approximates the posterior mean through a nonlinear transformation more accurately than the FOL.

−6.64

−6.63

−6.62

−0.50

0.5

−7.2926

−7.2924

−7.2922

−7.292

−7.2918

ax (m/s2)

UT covariance

ay (m/s2)

az(m

/s2)

−6.64

−6.63

−6.62

−0.50

0.5

−7.2926

−7.2924

−7.2922

−7.292

−7.2918

ax (m/s2)

Sample covariance

ay (m/s2)

az(m

/s2)

−6.637

−6.636

−6.635

−0.50

0.5

−7.2926

−7.2924

−7.2922

−7.292

−7.2918

ax (m/s2)

FOL covariance

ay (m/s2)az(m

/s2)

Fig. 8. The UT approximates the posterior covariance through a nonlinear transformation more accurately than theFOL.

−0.8−0.6−0.4−0.200.20.40.60.8−7.293

−7.2925

−7.292

−7.2915

−7.291

Sample covariance

ay (m/s2)

az(m

/s2)

−0.8−0.6−0.4−0.200.20.40.60.8−7.293

−7.2925

−7.292

−7.2915

−7.291FOL covariance

ay (m/s2)

az(m

/s2)

−6.64

−6.62

−6.6

−6.58

−0.8−0.6−0.4−0.200.20.40.60.8

ay (m/s2)

ax(m

/s2)

−6.64

−6.62

−6.6

−6.58

−0.8−0.6−0.4−0.200.20.40.60.8

ay (m/s2)

ax(m

/s2)

Fig. 9. Detailed comparison between the FOL-predicted and the sample posterior covariances. The UT predictedposterior covariance is not plotted because it looks very similar to the sample covariance. The FOL is not able tocapture the parabolic shape of the distribution.

random variables X and Y , the conditional mean X|yof X, conditioned on the realized value of Y , is thebest estimator of X in the sense of least squares. Thecomputation of X|y explicitly depends on y, Λyy, andthe cross covariance Λxy as

X|y = x+ΛxyΛ−1yy

(y − y) (25)

When Y is non-Gaussian due to the nonlinear measure-ment model, as we already see the advantage of the un-scented transform in the previous section, we expect betteraccuracy of the approximated conditional mean from theUT than the FOL. However, this is not generally true, asillustrated by the following simulation results.

To estimate an unknown sample of X, of which the meanand covariance are known a-priori, we calculate the condi-tional mean, denoted as XFOL

i |yiand X

UTi |yi

using Eqn.(25) with y,Λyy, andΛxy approximated by the first-order

linearization and UT respectively. We compare XFOLi |yi

,

XUTi |yi

with xi for the following four cases, with allΛxx =

diag(0.001, 0.001, 0.001). The residuals (X(•)i |yi

− xi) areplotted in Fig. 10 through Fig. 13.

(1) x = [0, 0, 0]T (Fig. 10)

The FOL causes errors in θ and θ dimensions whilethe UT causes small errors in θ dimension.

IFAC Mechatronics '13April 10-12, 2013. Hangzhou, China

411

(2) x = [π/2, 0, 0]T (Fig. 11)

The FOL causes errors in θ and θ dimensions whilethe UT causes errors in θ dimension.

(3) x = [0, 0.5, 0]T (Fig. 12)

The FOL causes errors in θ dimension and smallerrors in θ dimension while the UT causes errors in θdimension.

(4) x = [π/2, 0.3, 0.3]T (Fig. 13)

The FOL causes errors in θ dimension and smallerrors in θ dimension while the UT causes errors in θand θ dimensions.

It should be noted that the accuracy is sensitive tothe numerical values that we choose for the parameters.However, these four cases provide the observations thatthe estimation performance by these two approximations(FOL and UT) is controversial, i.e., there is no guaranteethat one would always perform better than the other. Thechoice of the approximation method would reside in thenature of the application. In this application of estimatingthe joint angle θ, the FOL is likely to perform better thanthe UT (as illustrated by the above four cases). Thus, theEKF method is chosen for the purpose of this work.

6. CONCLUSION

This paper presented a cascade extended Kalman filter-ing method for robotic joint angle estimation. The filterwas derived from the robotic forward kinematics and thestochastic model of the MEMS inertial sensors. Via experi-mentation, the estimation errors of the proposed algorithmwere shown to be within around 2◦ which is unlikely todrastically degrade the performance of humanoid robots.Future works include the investigation of the real-time im-plementation on robots such as PR2, and the developmentof a systematic way for sensor alignment and calibration.

ACKNOWLEDGEMENTS

This research has been conducted in close connectionwith Dr. Philip Roan, in Robert Bosch LLC. Researchand Technology Center. The authors would like to thankKing Abdulaziz City for Science and Technology (KACST)for the financial support, Mr. Evan Chang-Siu and Mr.Matthew Brown for their early efforts on the developmentof the IMU, and Dr. Jacob Apkarian from Quanser for hishelp in the acquisition of the Quanser 3-DOF gyroscope.

REFERENCES

Ackerman. Willow garages introduces pr2 se, halfthe arms at half the price, 2011. Available:http://spectrum.ieee.org/automaton/robotics/robotics-software/willow-garage-pr2-se-now-available-half-the-arms-at-half-the-price.

W. Chen and M. Tomizuka. Load side state estimationin robot with joint elasticity. In Advanced IntelligentMechatronics 2012, IEEE/ASME International Confer-ence on, pages 598–603, 2012.

P. Cheng and B. Oelmann. Joint-angle measurement usingaccelerometers and gyroscopesa survey. Instrumentationand Measurement, IEEE Transactions on, 59(2):404–414, 2010.

S.J. Julier and J.K. Uhlmann. A new extension of thekalman filter to nonlinear systems. In Int. Symp.Aerospace/Defense Sensing, Simul. and Controls, vol-ume 3, page 26. Spie Bellingham, WA, 1997.

Tim Niemuller, Alexander Ferrein, Gerhard Eckel, DavidPirro, Patrick Podbregar, Tobias Kellner, Christof Rath,and Gerald Steinbauer. Providing ground-truth data forthe nao robot platform. RoboCup 2010: Robot SoccerWorld Cup XIV, pages 133–144, 2011.

M. Quigley, R. Brewer, S.P. Soundararaj, V. Pradeep,Q. Le, and A.Y. Ng. Low-cost accelerometers forrobotic manipulator perception. In Intelligent Robotsand Systems (IROS), 2010 IEEE/RSJ InternationalConference on, pages 6168–6174, 2010.

P. Roan, N. Deshpande, Y. Wang, and B Pitzer. Manipu-lator state estimation with low-cost accelerometers andgyroscopes. In Intelligent Robots and Systems (IROS),2012 IEEE/RSJ International Conference on, 2012.

B. Siciliano, L. Sciavicco, and L. Villani. Robotics: mod-elling, planning and control. Springer Verlag, 2009.

E.A. Wan and R. Van Der Merwe. The unscented kalmanfilter for nonlinear estimation. In Adaptive Systemsfor Signal Processing, Communications, and ControlSymposium 2000. AS-SPCC. The IEEE 2000, pages153–158, 2000.

G. Welch and G. Bishop. An introduction to the kalmanfilter. University of North Carolina at Chapel Hill,Chapel Hill, NC, 7(1), 1995.

Appendix A. LONG EXPRESSIONS

The measurement model h and its Jacobian H are givenas follows,

ptrue = h(ω,α, p, θ, θ, θ,d)

= [px py pz]T

(A.1)

px = − αzdy + αycidz + cipx − αxdzsi + pysi − cidysiω2x

− dxsi2ω

2x

+ ci2dyωxωy + 2cidxsiωxωy − dysi

2ωxωy − ci

2dxω

2y

+ cidysiω2y

+ cidzωxωz + dzsiωyωz − dxω2z− dy θ − 2dxωz θ − dxθ

2

py =αzdx − αxcidz + cipy − αydzsi − pxsi − ci2dyω

2x

− cidxsiω2x

+ ci2dxωxωy − 2cidysiωxωy − dxsi

2ωxωy + cidxsiω

2y

− dysi2ω

2y

− dzsiωxωz + cidzωyωz − dyω2z+ dxθ − 2dyωz θ − dy θ

2

pz = − αycidx + αxcidy + pz + αxdxsi + αydysi − ci2dzω

2x

− dzsi2ω

2x

− ci2dzω

2y

− dzsi2ω

2y

+ cidxωxωz − dysiωxωz + cidyωyωz

+ dxsiωyωz + 2cidxωxθ − 2dysiωxθ + 2cidyωy θ + 2dxsiωy θ

H = H(ω,α, p, θ, θ, θ,d)

=

[H11 H12 H13

H21 H22 H23

H31 H32 H33

](A.2)

H11 = αxdzci − pyci − dzωyωzci + dyω2xci

2− 2dxωxωyci

2− dyω

2yci

2

+ αydzsi + pxsi + dzωxωzsi + 2dxω2xcisi + 4dyωxωycisi

− 2dxω2ycisi − dyω

2xsi

2+ 2dxωxωysi

2+ dyω

2ysi

2

H12 = 2dxωz + 2dxθ

H13 = −dy

H21 = αydzci + pxci + dzωxωzci + dxω2xci

2+ 2dyωxωyci

2− dxω

2yci

2

− αxdzsi + pysi + dzωyωzsi − 2dyω2xcisi + 4dxωxωycisi

+ 2dyω2ycisi − dxω

2xsi

2− 2dyωxωysi

2+ dxω

2ysi

2

H22 = 2dyωz + 2dy θ

H23 = dx

H31 = 2θdyωxci − αxdxci − αydyci − 2θdxωyci + dyωxωzci − dxωyωzci

− αydxsi + αxdysi + 2θdxωxsi + 2θdyωysi + dxωxωzsi + dyωyωzsi

H32 = −2cidxωx + 2dysiωx − 2cidyωy − 2dxsiωy

H33 = 0

IFAC Mechatronics '13April 10-12, 2013. Hangzhou, China

412

−0.2

0

0.2

0.4

0.6

−0.4

−0.2

0

0.2

0.4−15

−10

−5

0

5

θ radθ rad/s

θra

d/s2

FOL residualUT residual

−0.2−0.100.10.20.3−14

−12

−10

−8

−6

−4

−2

0

2

θ rad/s

θra

d/s2

Fig. 10. The FOL causes errors in θ and θ dimensions while the UT causes small errors in θ dimension.

−30

−20

−10

0

10

−0.2

0

0.2

0.4

0.6−0.5

0

0.5

1

1.5

θ radθ rad/s

θra

d/s2

FOL residualUT residual

−25 −20 −15 −10 −5 0 5−0.2

0

0.2

0.4

0.6

0.8

1

1.2

θ rad

θra

d/s2

Fig. 11. The FOL causes errors in θ and θ dimensions while the UT causes errors in θ dimension.

−0.2

0

0.2

0.4

0.6

−10

−5

0

5−20

−15

−10

−5

0

5

θ radθ rad/s

θra

d/s2

FOL residualUT residual

−10 −8 −6 −4 −2 0 2−20

−15

−10

−5

0

5

θ rad/s

θra

d/s2

Fig. 12. The FOL causes errors in θ dimension and small errors in θ dimension while the UT causes errors in θ dimension.

−15

−10

−5

0

5

−4−3

−2−1

01

−4

−2

0

2

θ radθ rad/s

θra

d/s2

FOL residual

UT residual

−8 −6 −4 −2 0 2−3

−2.5

−2

−1.5

−1

−0.5

0

0.5

1

1.5

θ rad

θra

d/s2

Fig. 13. The FOL causes errors in θ dimension and small errors in θ dimension while the UT causes errors in θ and θdimensions.

IFAC Mechatronics '13April 10-12, 2013. Hangzhou, China

413