Non-Linear ZMP based State Estimation for Humanoid RobotLocomotion*

Stylianos Piperakis and Panos Trahanias

Abstract— This article presents a novel state estimationscheme for humanoid robot locomotion using an ExtendedKalman Filter (EKF) for fusing encoder, inertial and Foot Sensi-tive Resistor (FSR) measurements. The filter’s model is based onthe non-linear Zero Moment Point (ZMP) dynamics and thus,coupling the dynamic behavior in the frontal and the lateralplane. Furthermore, it provides state estimates for variablesthat are commonly used by walking pattern generators andposture balance controllers, such as the Center of Mass (CoM)and the linear time-varying Divergent Component of Motion(DCM) position and velocity, in the 3-D space. Modeling errorsare taken into account as external forces acting on the robot inthe acceleration level. In addition, an observability analysis forthe non-linear system dynamics and the linearized discrete-timeEKF dynamics is presented. Subsequently, by utilizing ground-truth data obtained from a vicon motion capture system with aNAO humanoid robot, we demonstrate the effectiveness androbustness of the proposed scheme contrasted to the linearfilters, even in the case where disturbances are introducedto the system. Finally, the proposed approach is implementedand employed for feedback to a real-time posture controller,rendering a NAO robot able to walk on an outdoors inclinedpavement.

I. INTRODUCTION

Humanoid robot locomotion is a challenging task withmany difficulties. Mainly, due to the non-linear multi-bodydynamics along with the many Degrees of Freedoms (DoFs)the humanoid robots have, the under-actuation which oc-curs during the gait, and the unilateral type of contact therobots experience with the ground. The non-linear multi-body dynamics prohibit exact solutions to be obtained inreal-time. Therefore, many researchers approximated thosedynamics with simplified models that could describe thedynamic behavior of a humanoid while walking. However,those models are based on the assumption that the robot’sdynamics are decoupled in the frontal and the lateral plane,which is not true, especially when the robot exhibits highlydynamic motions. In addition, since the robot does not havea fixed base, it is under-actuated. Nevertheless, when theassumptions that a rigid type contact between the supportleg and the ground along with sufficient friction exist, allthe under-actuated DOFs vanish. Unfortunately, this is notthe case in a realistic environment. Vertical displacementwith respect to the ground can cause acceleration in the

*This work has been partially supported by the EU FET Proactivegrant (GA: 641100) TIMESTORM - Mind and Time: Investigation of theTemporal Traits of Human-Machine Convergence.

The authors are with the Institute of Computer Science, Foundationfor Research and Technology - Hellas (FORTH) and the Departmentof Computer Science, University of Crete, Heraklion, Crete, Greece.spiperakis@ics.forth.gr trahania@ics.forth.gr

same direction which must be taken into consideration whileplanning or controlling the robot in order to avoid undesiredground reaction forces.

In this paper, we propose a novel estimation scheme withan Extended Kalman Filter (EKF) which has its dynamicsbased on the non-linear Zero Moment Point (ZMP) [1]formulation, thus, effectively coupling the dynamic behaviorin the frontal and lateral plane and fusing information fromsensors that are widely available on humanoids, namely,encoders, Inertial Measurement Units (IMU), and Foot Sensi-tive Resistors (FSRs). This filter provides accurate estimatesfor variables that are commonly used by walking patterngenerators and posture stabilization controllers, such as theCenter of Mass (CoM) and the linear time-varying DivergentComponent of Motion (DCM) [2] position and velocity, inthe x, y, and z axes, as experimentally validated with a NAOhumanoid robot under real world conditions.

II. RELATED WORK

Biped state-estimation plays an important role in realizingstable walking motions and in posture balance control [3].Xinjilefu et al. [4] solved a Quadratic Program (QP) utilizingthe robot’s full-body dynamics. The proposed approach wasadvantageous, in the sense that it did not require a state-space model as in the Kalman Filter (KF) case, couldnaturally handle equalities and inequalities as constraints,and consider modeling error in the state vector. However,due to the imposed constraints and the high-dimensionalitythe framework was computationally expensive for real-timeexecution, did not generalize since it was based on the robot’sdynamics and required force/torque sensors on robot’s joints.Stephens [5] used simplified models based on the the LinearInverted Pendulum Model (LIPM) dynamics [6] for state-estimation in order to control the posture of the force-controlled Sarcos Primus humanoid. He was able to estimatemodeling errors as incoming external forces, and possibleCoM biases by fusing CoM and Center of Pressure (CoP)measurements from the joint encoders and the FSRs respec-tively. Nevertheless, he observed that there was a trade-offbetween disturbance estimation and state estimation, sincetime-varying disturbances demanded a carefully tuning ofthe noise covariances. Based on that approach, Xinjilefu andAtkeson [7] compared two KF schemes; one based on theLIPM dynamics and one based on robot’s planar dynamics.They observed that LIPM KF was simple to design andimplement, easy to tune, robust to modeling errors, and cangeneralize to other robots, while, as expected, the Planar KFyielded more accurate estimates since it is based on a more

accurate representation of the robot’s dynamics. Anotherapproach based on the LIPM dynamics was presented byKwon and Oh [8], where the current CoP measurement wasthe input of a KF and the output was the CoM position. Thefilter’s state was augmented with a CoM bias and a state forexternal forces. A similar approach but without the CoM biasin the state vector was proposed by Wittmann et al. [9], wherea state estimator for biped robots fusing encoders, IMU, andforce/torque measurements with a KF based on the LIPMdynamics was presented. Nevertheless, by using the LIPMdynamics, one assumes that the CoM is constrained to lie ona constant horizontal plane and furthermore that the motionin the x and y axes are decoupled. Unfortunately, neitherholds in real world conditions and especially when a robotlocomotes on uneven and/or rough terrain.

Other approaches, treat the robot as a floating rigid body,based on Newton-Euler dynamics. Bloesch et al. [10] pro-posed a state estimation scheme for quadrupled robots, fusingleg kinematics and the IMU measurements to estimate notonly the position and the velocity of the floating base butalso the body’s orientation, expressed as a quaternion, andthe foothold positions. This approach was extended in thehumanoid robot case by Rotella et al. [11]. However, sincethose approaches are based on generic dynamics, they arevery sensitive to noise and thus, difficult to tune. To thisend, the authors carried out an Alan variance analysis [12]to carefully identify the noise characteristics of the IMU. Asimilar approach was proposed by Bry et al. [13] for Micro-Aerial-Vehicle (MAV) navigation, where the orientation un-certainty was expressed as a screw in exponential coordinatesaround the body frame. This approach was extended to theAtlas robot by Kuindersma et al. [14], where it yielded drift-free estimates [15] at a cost of incorporating exteroceptiveLIDAR measurements with a Gaussian Particle Filter (GPF).

In our work, the proposed state estimator fuses effectivelythree different kind of sensors, generalizes with little to noeffort to other humanoids, can be easily tuned, and yieldsaccurate 3-D estimates for important quantities in humanoidplanning and control, even in the z-axis contrasted to theLIPM approaches.

This paper is organized as follows, section III, presents theunderlying EKF dynamics. In addition, section IV demon-strates an observability analysis for both the non-linear dy-namics and the EKF linearized dynamics. Next, in section Vexperimental results with a real NAO humanoid robot arepresented. Section VI concludes the paper and discussespossible future work.

III. EXTENDED KALMAN FILTER BASED STATEESTIMATION

In this section, we will present the EKF’s process andmeasurement model which will be used for the state estima-tion task. The dynamics are based on the non-linear ZMPequation, where we treat the ZMP location on the plane andthe vertical Ground Reaction Force (GRF) as the input to thesystem and the output are the position and the accelerationof the CoM in the 3-D space.

The ZMP is defined as the point on the ground at whichthe moments generated by the reaction forces vanish. Byalso considering external forces acting on the robot’s body,the equations of motion are formulated as:

cx =cx − zx

cz(cz + g) +

1

mfx (1)

cy =cy − zy

cz(cz + g) +

1

mfy (2)

where zx, zy, fx, fy are the ZMP coordinates and externalforces/modeling errors in the x and y axes respectively,cx, cy, cz is the position of the CoM with respect to aninertial frame of reference, cx, cy, cz is the correspondingacceleration, g is the gravitational acceleration and m is therobot’s mass. Furthermore, for the z-axis the dynamics are:

cz =1

mfN − g +

1

mfz (3)

where fN is the vertical GRF and fz the externalforce/modeling error in the z direction.

Replacing (3) in (1), (2), yields the following 3-D non-linear dynamics:

cx =cx − zxmcz

(fN + fz) +1

mfx (4)

cy =cy − zymcz

(fN + fz) +1

mfy (5)

cz =1

m(fN + fz)− g (6)

A. Process Model

Assume the following state vector for the process dynam-ics:

xt =[cx cy cz cx cy cz fx fy fz

]>with cx, cy, cz the CoM velocity. Furthermore, assume theinput u to the filter is the ZMP in the x and y axes alongwith the vertical GRF as measured by the FSRs:

ut =[zFSRx zFSRy fFSR

N

]>(7)

Consequently, the process model takes the standard non-linear form:

xt = f(xt,ut, εt) (8)

where

d

dt

cxcyczcxcyczfxfyfz

=

cxcycz

cx−u1

mcz(u3 + fz) + 1

mfxcy−u2

mcz(u3 + fz) + 1

mfy1m (u3 + fz)− g

000

+ εt (9)

and εt is a Gaussian zero-mean additive noise with covari-ance Qt, εt ∼ N (0,Qt).

Taking the appropriate continuous dynamic linearization,yields the following Jacobian matrix of the state vector x:

Gt =∂f

∂x=

0 I 0Ct 0 Dt

0 0 0

(10)

with

Ct =

u3+fzmcz

0 − (u3+fz)(cx−u1)mc2z

0 u3+fzmcz

− (u3+fz)(cy−u2)mc2z

0 0 0

(11)

Dt =

1m 0 cx−u1

mcz

0 1m

cy−u2

mcz0 0 1

m

(12)

Although, a more accurate approximation could be used tocompute the discretized matrix Gk, Euler integration is usedfor simplicity:

Gk = I + Gt∆t (13)

where ∆t is the sampling time.To this end, the prediction step of the EKF is readily

formulated as:

xk|k−1 = f(xk−1|k−1,uk,0)∆t + xk−1|k−1 (14)

Pk|k−1 = GkPk−1|k−1G>k + Qk (15)

with P being the estimate error covariance matrix.

B. Measurement Model

For the output dynamics we employ sensors that arecommonly available on humanoid robots nowadays. Weassume that the robot is equipped with encoders on everyjoint and thus we are able to compute the CoM position withrespect to the torso local frame. Moreover, with the IMU wecompute the corresponding CoM accelerations, again in thetorso’s local frame. Notice that all measurements, denoted asyi, need to be transformed to the inertial frame of reference.

y1 = cENCx , y2 = cENC

y , y3 = cENCz ,

y4 = cIMUx , y5 = cIMU

y , y6 = cIMUz ,

Subsequently, since the CoM acceleration is not part ofthe state vector, the output equation is non-linear:

yt = h(xt,ut) + δt (16)

with

h(xt,ut) =

cxcycz

cx−u1

mcz(u3 + fz) + 1

mfxcy−u2

mcz(u3 + fz) + 1

mfy1m (u3 + fz)− g

(17)

and δt be the Gaussian zero-mean measurement noise withcovariance Rt, δt ∼ N (0,Rt). After discretizing, the Jaco-bian matrix Hk = ∂h

∂x can be readily computed, following

the derivation of Gt. Then, the EKF update step is realizedas:

Kk = HkPk|k−1H>k + Rk (18)

xk|k = xk|k−1 + Kk(yk − h(xk|k−1,uk)) (19)Pk|k = (I−KkHk)Pk|k−1 (20)

where Kk is the Kalman gain.

IV. OBSERVABILITY ANALYSIS

A. Non-linear Observability Analysis

Non-linear observability analysis is far from trivial, as itis in the linear time-invariant case, where the observabil-ity properties are excessively studied over the years andthus, well-understood. This is mainly due to the strongdependence the analysis has on the underlying non-lineardynamics and the neighborhood of the current system’s stateand input. Therefore, we have included a brief Appendixshortly describing some important results from the non-lineargeometric control theory that are used in our analysis.

Following the notation introduced in the Appendix, thedimension of the state-space and the measurement’s modelis n = 9 and m = 6 respectively, therefore, by choosingthe following coordinates (h1, ϕ

11, h2, ϕ

12, h3, ϕ

13, h4, h5, h6),

defined on the current operating point (x∗t , u∗t ), we obtainthe following map:

Φ(x∗t ,u∗t ) =

c∗xc∗xc∗yc∗yc∗zc∗z

c∗x−u∗1

mc∗z(u∗3 + f∗z ) + 1

mf∗xc∗y−u

∗2

mc∗z(u∗3 + f∗z ) + 1

mf∗y1m (u∗3 + f∗z )− g

(21)

By re-ordering the quantities to obtain a mathematicallyconvenient form Φ and then taking the Jacobian with respectto x∗t , we get the local non-linear observability matrix:

O =∂Φ(x∗t ,u

∗t )

∂x∗t=

I 0 00 I 0

C∗t 0 D∗t

(22)

and since detO = − 1m3 we have that

rankO = 9 (23)

rendering the non-linear dynamics in (8), (16) to be locallyobservable in all cases.

B. Linear Time-Varying Observability Analysis

Since, we are using a discrete EKF for the state estimationtask, we must explore the observability of the filter which isbased on the following linear time-varying dynamics:

xk+1 = Gkxk + εk (24)yk = Hkxk + δk (25)

We will give the local observability analysis based on thelinear time-varying observability matrix M, as proposed byChen et al. [16]. Notice, in the general case, the observabilityproperties of a discrete linear time-varying system, can differfrom the observability properties of the true underlying non-linear continuous system. This is due to the errors that ariseby the linearization and/or discretization procedure.

For the state-space and output model in (24), (25), thismatrix is defined as:

M =

Hk

Hk+1Gk

...Hk+8Gk+7 . . .Gk

(26)

and the sufficient condition for the local observability is:

rankM = 9 (27)

By examining the first 9× 9 submatrix of M:

M∗ =

I 0 0Ck 0 Dk

0 ∆tI 0

(28)

where Ck and Dk are the matrices in (11), (12) evaluated atthe k-th discrete time instant, it is straightforward to derive:

detM∗ = −(

∆t

m

)3

(29)

Therefore, the linear time-varying observability matrix Mis full rank and cannot drop rank under any circumstances.

V. EXPERIMENTAL RESULTS

In the current section we outline representative results thatdemonstrate the effectiveness and robustness of the proposedscheme, contrasted to others well-established approaches. Allconducted experiments were performed with a real NAOhumanoid robot. First, the EKF is compared to a KF basedon the LIPM dynamics, on ground-truth data sets obtainedwith a vicon motion capture system. Next, the proposedapproach is employed to feedback a posture stabilizationcontroller based on the DCM, enabling a NAO robot to walkoutdoors on an inclined pavement. The filter’s response iscontrasted to an EKF based on Newton-Euler dynamics. Inall our experiments the covariance matrices were determinedexperimentally and set to:

Q = diag(1e− 6I, 1e− 5I, I) (30)R = diag(5e− 6I, 5e− 4I) (31)

A. Evaluation on Motion Captured Data Sets

A first result regards an estimation accuracy study, in termsof the Root Mean Square Error (RMSE), contrasted to a KFbased on the LIPM, as proposed by Wittman et al. [9]. TheLIPM KF can estimate the following state vector:

xt =[cx cy cx cy fx fy

]>(32)

utilizing the CoM position and velocity, as measured bythe encoders, and transformed to the inertial frame with the

0 5 10 15 20 25

c x(m

)

-2

-1

0

1EKFKFvicon

11.2 11.3 11.4-0.7-0.65-0.6

0 5 10 15 20 25

c y(m

)

-0.4

-0.2

0

0.2EKFKFvicon

11.2 11.6 12

-0.040

Time(s)0 5 10 15 20 25

c z(m

)

0.2

0.25

0.3

0.35

EKFvicon

9 10 11

0.320.34

Fig. 1. CoM trajectories in the 3-D space, blue lines indicate the ground-truth trajectories, red dotted lines the EKF estimated trajectories and greendotted the KF estimated trajectories, notice in the z axis no KF estimate isavailable since cz is assumed constant.

IMU. The employed covariance matrices are given by (30),(31), where this time the matrices-dimensions are obtainedby neglecting the z-axis dynamics.

We’ve selected 10 ground-truth data sets, collected witha vicon motion capture system consisting of 15 infra-redcameras, and a NAO v3.3 humanoid robot [17]. To beginwith, we calibrated the IMU, to remove the biases andcut off unwanted high frequencies with a low-pass filter.Then, we computed the CoM with respect to the local torsoframe using kinematics and the CoP using the FSR in thefeet which was then transformed to the torso local framewith kinematics. Subsequently, all the acquired data weretransformed to the inertial frame of reference using theestimated rotation matrix obtained by the IMU. Notice, sincethe NAO robot is not equipped with a 3-axis gyroscope,namely no gyro rate is available about the z-axis, the viconyaw angle was used instead. This is why no drifting isobserved in all our data, although the robot drifts in manycases. Nevertheless, this does not pose any limitation, sincethe same data are used as input and measurements signalsfor both approaches.

Since we cannot plot the response for every data set,we selected one where the motion is unstable and thusmore dynamic [17]. In Figure 1, the CoM trajectories areshown where the EKF estimated trajectory is overlapped bythe actual signal, notice in the start the robot rises from asitting position, the EKF can accurately capture that motion.In addition, Figure 2 shows that the EKF estimated moreaccurately the corresponding CoM velocites in all axes.

Figure 3 illustrates the external forces/modeling error forthe corresponding motion; notice that there is no delay in theforce estimation as observed in the KF’s case, also reportedin [9]. This is due to the fusion of the acceleration measure-ment which yields a lag free estimation. Furthermore, noticein the z-axis, at time 0–2s where the robot is practically

0 5 10 15 20 25

c x(m

/s)

-0.4

-0.2

0

0.2

0.4EKFKFvicon

9.5 10 10.5 11 11.5-0.2-0.10

0 5 10 15 20 25

c y(m

/s)

-0.5

0

0.5EKFKFvicon

11.5 12 12.5 13 13.5-0.10

0.1

Time(s)0 5 10 15 20 25

c z(m

/s)

-0.1

0

0.1

0.2EKFvicon

3.5 4 4.5 5 5.5-0.0200.020.04

Fig. 2. CoM velocites in the 3-D space, blue lines indicate the ground-truth velocities, red dotted lines the EKF velocty estimates and green dottedthe KF velocty estimates, no KF estimate is available in the z-axis since isassumed zero.

0 5 10 15 20 25

f x(N

)

-20

-10

0

10EKFKF

11.5 12 12.5 13 13.5-404

0 5 10 15 20 25

f y(N

)

-20

-10

0

10

20EKFKF 11 12 13

-8-40

Time(s)0 5 10 15 20 25

f z(N

)

0

10

20

30EKF 0 0.5 1

1416

Fig. 3. External Force/Modeling error in the x, y and z-axis respectively,KF estimates are delayed contrasted to the EKF ones.

still, the estimation is almost 15N , this is reasonable sincethe NAO’s FSR have a reliable working range up to 25Nand when the robot is still they measure approximately 35N ,therefore since the robot weights 4.789kg the modeling errorneeds to be approximately 1.5kg.

Furthermore, we computed the position and velocity ofthe DCM. In the KF’s case, we are forced to compute theLinear Time-Invariant (LTI) DCM since the assumption thatthe CoM lies on constant horizontal plane during the motionis made. The LTI-DCM is given by:

ξLTI = c +1

ω0c (33)

where ω0 =√

gl and l is the constant CoM height. The

corresponding LTI-DCM velocity is thus:

0 5 10 15 20 25

ξ x(m

)

-2

-1

0

1EKFKFvicon

11.5 11.6 11.7-0.64-0.6-0.56

0 5 10 15 20 25

ξ y(m

)

-0.4

-0.2

0

0.2EKFKFvicon

10.5 11 11.5 12 12.5-0.1-0.05

00.05

Time(s)0 5 10 15 20 25

ξ z(m

)

0.2

0.25

0.3

0.35

EKFvicon8.5 9 9.5 10 10.5

0.320.34

Fig. 4. DCM trajectories in the 3-D space, blue lines indicate the ground-truth LTV-DCM, red dotted lines the EKF estimated LTV-DCM trajectories,and green dotted lines the KF estimated LTI-DCM trajectories, no KFestimate is available in the z-axis since is assumed constant.

0 5 10 15 20 25

ξ x(m

/s)

-0.4

-0.2

0

0.2

0.4EKFKFvicon4.5 5 5.5 6 6.5

-0.15-0.1-0.050

0 5 10 15 20 25

ξ y(m

/s)

-1

-0.5

0

0.5

EKFKFvicon

8 9 1000.1

0.20.3

Time(s)0 5 10 15 20 25

ξ z(m

/s)

-0.2

-0.1

0

0.1

0.2EKFvicon11.5 12 12.5 13 13.5

-0.040

0.04

Fig. 5. DCM velocities in the 3-D space, blue lines indicate the ground-truth LTV-DCM velocities, red dotted lines the EKF estimated LTV-DCM,and green dotted lines the KF estimated LTI-DCM velocities, no KF estimateis available in the z-axis since is assumed zero.

RMSE

0

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

0.18

0.2KFEKFstd

ξx ξy ξzcy czcxcx cy cz ξx ξzξy

Fig. 6. Average RMSE for all quantities of interest for the 10 data setstudied; red bars indicate the EKF’s accuracy, green bars the KF’s accuracy,and black lines the standard deviation from the corresponding mean values.

Fig. 7. NAO humanoid robot walking diagonally on a 7◦ inclined pavement (from left to right).

0 5 10 15 20 25

cx(m

)

0

0.5

1

1.5ZMP-EKFIMU-EKF

11.5 12 12.5 13 13.50.40.6

0 5 10 15 20 25

cy(m

)

-0.3

-0.2

-0.1

0

0.1ZMP-EKFIMU-EKF

11.5 12 12.5 13 13.5-0.15-0.1-0.05

Time(s)0 5 10 15 20 25

cz(m

)

0.2

0.25

0.3

0.35

0.4ZMP-EKFIMU-EKF

11.5 12 12.5 13 13.50.30.320.34

Fig. 8. CoM trajectories in the 3-D space, red dotted lines indicate theZMP based EKF estimated trajectories while the black lines indicate theIMU based EKF estimated trajectories.

ξLTI = ω0(ξLTI − c) +1

ω0c (34)

On the other hand, in the EKF’s case we can computethe Linear Time-Varying (LTV) DCM to approximate thetrue non-linear DCM more effectively. The LTV-DCM isformulated as:

ξLTV = c +1

ωtc (35)

where ωt =√

gcz

. Since ωt is now time-depended, thecorresponding LTV-DCM velocity is given by:

ξLTV =

(ωt −

ωt

ωt

)(ξLTV − c) +

1

ωtc (36)

with ωt = − g1/2

2c3/2z

cz .In both DCM velocity cases, we used the calibrated low-

pass filtered acceleration by the IMU, and the correspondingCoM position and velocity estimate by each filter respec-tively.

Figure 4 shows the corresponding DCM trajectories forthis case of study, while Figure 5 demonstrates the DCMvelocities. Finally, in Figure 6 the average RMSE for allquantities of interest for the 10 data sets used in our study ispresented. Notice that the EKF not only yields more accurateestimates in the RMSE sense, but also more certain ones,especially when the estimated quantities are the velocities.

0 5 10 15 20 25

_cx(m

=s)

-0.1

0

0.1

0.2

0.3ZMP-EKFIMU-EKF8.5 9 9.5 10 10.5

0.020.060.1

0 5 10 15 20 25

_cy(m

=s)

-0.2

-0.1

0

0.1

0.2ZMP-EKFIMU-EKF8.5 9 9.5 10 10.5

0.060.10.14

Time(s)0 5 10 15 20 25

_cz(m

=s)

-0.1

-0.05

0

0.05

0.1ZMP-EKFIMU-EKF6.5 7 7.5 8 8.5

-0.020

0.02

Fig. 9. CoM velocites in the 3-D space, red dotted lines indicate the ZMPbased EKF velocity estimates and black lines the IMU based EKF velocityestimates.

B. Walking outdoors on an inclined pavement

In this section, the proposed estimation scheme is ex-perimentally validated on a v4.0 NAO robot. The estimatesobtained by our ZMP based EKF are used to feedback a real-time posture stabilizer based on the DCM [2], rendering therobot able to keep the balance even while walking outdoorson a 7◦ inclined pavement, as shown in Figure 7. Althoughthe slope of the ground is mild, please note that for a robot ofthe size of NAO it represents a rather significant challenge.

Since no ground truth data are available in outdoor envi-ronments, to verify the estimation task we implemented anEKF based on the IMU, as proposed by Rotella et.al [11],but modified in such a way to estimate the CoM quantitiesinstead of the rigid base ones. This IMU based EKF canestimate the following state vector:

xt =[c c q bf bω

]>(37)

where q, bf , and bω , are the torso’s attitude quaternion, theacceleration and the gyroscope biases respectively, utilizingthe inertial CoM position as measured by the encoders.Notice, we’ve collected raw IMU data for 48 hours in orderto perform an Alan variance analysis [12] and carefullytune the process noise covariance, as also suggested by theauthors, to maximize in such a way the filter’s efficiency.

Figure 8 and Figure 9 illustrate the CoM position andvelocity in the 3-D space as estimated by the two fil-ters, for a diagonally forward gait on a 7◦ inclined pave-

0 5 10 15 20 25

f x(N

)

-10

-5

0

5ZMP-EKF

0 5 10 15 20 25

f y(N

)

-20

-10

0

10

20ZMP-EKF

Time(s)0 5 10 15 20 25

f z(N

)

-20

0

20

40

60ZMP-EKF

Fig. 10. External Force/Modeling error in the x, y and z-axis respectively,IMU based EKF estimates are not available.

ment for approximately 25s. Figure 10 shows the externalforce/modeling errors during the gait, notice only the ZMPbased EKF can estimate those quantities. In addition, notethat the magnitude of the external forces can be justifiedby considering that when the robot locomotes on an unevenand rough terrain, early ground contact can commonly occur,giving rise to larger external forces. Moreover, Figure 11 andFigure 12 demonstrate the LTV-DCM position and velocityas estimated by the two filters for the corresponding gait.

Furthermore, we’ve conducted a variety of indoors andoutdoors experiments, namely, walking indoors on a hallway,walking outdoors on an even pavement, walking in placeon grass while heavily disturbing the robot and, as alsoillustrated in the supplementary material (for a higher qualityvideo, please check https://goo.gl/by3cB5), all theZMP based EKF estimates were pretty similar, within noisemargins, to the IMU based EKF ones, validating in such away the proposed estimation scheme.

Notice that in all experiments reported above, the esti-mated z-axis components contain higher noise compared tothe x and y-axis ones. This is due to the fact that the noisyFSR measurements are employed in the z-axis dynamics.

VI. CONCLUSION & FUTURE WORK

In this paper, a novel state estimation scheme for hu-manoid robot locomotion was presented, fusing effectivelythree different sensor sources, namely the joint encoders, theIMU, and the FSRs. We utilized the nonlinear ZMP equationwith an EKF to surpass the limitation of the constant CoMheight and the planar dynamic decouple, as assumed bythe LIPM, and readily estimate control variables commonlyused by walking pattern generators and posture stabilizationcontrollers. In addition, modeling errors were considered asexternal forces acting on the CoM in the acceleration level.

Someone would assume that the observability would belost when the robot experience accelerations in the z-axisequal to g, e.g. the robot is in free fall. As proved by our

0 5 10 15 20 25

9 x(m

)

0

0.5

1

1.5ZMP-EKFIMU-EKF

11.5 12 12.5 13 13.50.40.6

0 5 10 15 20 25

9 y(m

)

-0.4

-0.2

0

0.2ZMP-EKFIMU-EKF

11.5 12 12.5 13 13.5-0.15-0.1-0.05

Time(s)0 5 10 15 20 25

9 z(m

)

0.2

0.25

0.3

0.35

0.4ZMP-EKFIMU-EKF

11.5 12 12.5 13 13.50.280.30.32

Fig. 11. LTV-DCM trajectories in the 3-D space, red dotted lines indicatethe ZMP based EKF estimates and black lines the IMU based estimates.

0 5 10 15 20 25

_ 9 x(m

=s)

-0.1

0

0.1

0.2

0.3ZMP-EKFIMU-EKF 10 11 12

0.020.060.1

0 5 10 15 20 25

_ 9 y(m

=s)

-0.4

-0.2

0

0.2

0.4ZMP-EKFIMU-EKF12 13 14

-0.2-0.15-0.1-0.05

Time(s)0 5 10 15 20 25

_ 9 z(m

)

-0.1

-0.05

0

0.05

0.1ZMP-EKFIMU-EKF 12 13 14

-0.020

0.02

Fig. 12. LTV-DCM velocities in the 3-D space, red dotted lines indicate theZMP based EKF velocity estimates and black lines the IMU based velocityestimates.

observability analysis for both the non-linear dynamics andthe EKF this is not the case, since the local-observabilitymatrix is full rank under all circumstances. Nevertheless, theZMP is not well-defined when the robot is in flight.

Our experimental result demonstrated that the filtershowed robustness to perturbations, quick convergence prop-erties and provided more accurate estimates contrasted toa KF based on the LIPM. In addition, when incorporatedwith a real-time stabilization controller, a NAO robot wasable to walk on an outdoors inclined pavement and ongrass, effectively sensing and negotiating the incoming dis-turbances. Moreover, the filter’s estimates were pretty similarto the ones obtained by an EKF based on generic rigid bodydynamics and the IMU, validating the proposed approach.

In future work, we aim in including the torque that actsabout the robot’s CoM in the design. Thus, we will be able

to estimate rotational variables of interest (as it is the case incontemporary IMU-based estimation schemes), such as theangular momentum and the rate of angular momentum.

REFERENCES

[1] M. Vukobratovic and B. Borovac, “Zero-Moment Point – Thirty FiveYears of its Life,” International Journal of Humanoid Robotics, vol. 1,no. 01, pp. 157–173, 2004.

[2] M. A. Hopkins, D. W. Hong, and A. Leonessa, “Humanoid Locomo-tion on Uneven Terrain using the Time-varying Divergent Componentof Motion,” in IEEE-RAS International Conference on HumanoidRobots, Nov 2014, pp. 266–272.

[3] S. Piperakis, E. Orfanoudakis, and M. G. Lagoudakis, “PredictiveControl for Dynamic Locomotion of Real Humanoid Robots,” inIEEE/RSJ International Conference on Intelligent Robots and Systems,2014, pp. 4036–4043.

[4] X. Xinjilefu, S. Feng, and C. G. Atkeson, “Dynamic State Estimationusing Quadratic Programming,” in IEEE/RSJ International Conferenceon Intelligent Robots and Systems,, 2014, pp. 989–994.

[5] B. J. Stephens, “State Estimation for Force-controlled HumanoidBalance using Simple Models in the Presence of Modeling Error,”in IEEE International Conference on Robotics and Automation, 2011,pp. 3994–3999.

[6] S. Kajita and K. Tani, “Study of Dynamic Biped Locomotion onRugged Terrain-derivation and Application of the Linear InvertedPendulum Mode,” in IEEE International Conference on Robotics andAutomation, 1991, pp. 1405–1411.

[7] Xinjilefu and C. G. Atkeson, “State Estimation of a Walking Hu-manoid Robot,” in IEEE/RSJ International Conference on IntelligentRobots and Systems, 2012, pp. 3693–3699.

[8] S. Kwon and Y. Oh, “Estimation of the Center of Mass of HumanoidRobot,” in International Conference on Control, Automation andSystems, 2007, pp. 2705–2709.

[9] R. Wittmann, A. C. Hildebrandt, D. Wahrmann, D. Rixen, andT. Buschmann, “State Estimation for Biped Robots using Multi-body Dynamics,” in IEEE/RSJ International Conference on IntelligentRobots and Systems, Sept 2015, pp. 2166–2172.

[10] M. Bloesch, M. Hutter, M. Hoepflinger, S. Leutenegger, C. Gehring,C. D. Remy, and R. Siegwart, “State Estimation for Legged Robots–Consistent Fusion of Leg Kinematics and IMU ,” RSS Robotics Scienceand Systems, 2012.

[11] N. Rotella, M. Blosch, L. Righetti, and S. Schaal, “State Estimationfor a Humanoid Robot,” in IEEE/RSJ International Conference onIntelligent Robots and Systems, Chicago, IL, USA, September 14-18,2014, 2014, pp. 952–958.

[12] N. El-Sheimy, H. Hou, and X. Niu, “Analysis and Modeling of InertialSensors Using Allan Variance,” IEEE Transactions on Instrumentationand Measurement, vol. 57, no. 1, pp. 140–149, 2008.

[13] A. Bry, A. Bachrach, and N. Roy, “State estimation for aggressiveflight in gps-denied environments using onboard sensing,” in IEEEInternational Conference on Robotics and Automation, 2012, pp. 1–8.

[14] S. Kuindersma, R. Deits, M. Fallon, A. Valenzuela, H. Dai, F. Per-menter, T. Koolen, P. Marion, and R. Tedrake, “Optimization-basedLocomotion Planning, Estimation, and Control Design for the AtlasHumanoid Robot,” Autonomous Robots, vol. 40, pp. 429–455, 2016.

[15] M. F. Fallon, M. Antone, N. Roy, and S. Teller, “Drift-free HumanoidState Estimation Fusing Kinematic, Inertial and LIDAR Sensing,” inIEEE-RAS International Conference on Humanoid Robots, 2014, pp.112–119.

[16] Z. Chen, K. Jiang, and J. C. Hung, “Local Observability Matrix andits Application to Observability Analyses,” in 16th Annual Conferenceof IEEE Industrial Electronics Society, 1990, pp. 100–103 vol.1.

[17] T. Niemuller, A. Ferrein, G. Eckel, D. Pirro, P. Podbregar, T. Kellner,C. Rath, and G. Steinbauer, “Robocup 2010,” J. Ruiz-del Solar,E. Chown, and P. G. Ploger, Eds. Springer-Verlag, 2011, ch. ProvidingGround-truth Data for the Nao Robot Platform, pp. 133–144.

[18] R. Hermann and A. Krener, “Nonlinear Controllability and Observ-ability,” IEEE Transactions on Automatic Control, vol. 22, no. 5, pp.728–740, 1977.

[19] A. Isidori, Nonlinear Control Systems, 3rd ed., M. Thoma, E. D.Sontag, B. W. Dickinson, A. Fettweis, J. L. Massey, and J. W.Modestino, Eds. Springer-Verlag New York, Inc., 1995.

[20] E. D. Sontag, Mathematical Control Theory: Deterministic FiniteDimensional Systems. New York, NY, USA: Springer-Verlag NewYork, Inc., 1998.

[21] Y. Kawano and T. Ohtsuka, “Observability analysis of nonlinearsystems using pseudo-linear transformation,” in IFAC Symposiumon Nonlinear Control Systems, NOLCOS 2013, Toulouse, France,September 4-6, 2013., 2013, pp. 606–611.

[22] D. D. Vecchio and R. M. Murray, “Observability and Local ObserverConstruction for Unknown Parameters in Linearly and NonlinearlyParameterized Systems,” in American Control Conference, 2003. Pro-ceedings of the 2003, vol. 6, 2003, pp. 4748–4753 vol.6.

[23] M. Travers and H. Choset, “Use of the Nonlinear ObservabilityRank Condition for Improved Parametric Estimation,” in 2015 IEEEInternational Conference on Robotics and Automation, 2015, pp.1029–1035.

APPENDIXNON-LINEAR OBSERVABILITY

Consider the following non-linear dynamical system:

x = f(x,u) (38)y = h(x,u) (39)

with x ∈ Rn, u ∈ Rl is the input vector and y ∈ Rm is themeasured output. In addition, without the loss of generalitywe assume that f and h are smooth functions. The generalquestion is under which conditions we are able to reconstructthe state vector x by observing the system’s output y. Thereare many results on non-linear observability i.e [18], [19],[20], [21], nevertheless, we will follow the results presentedby Del Vecchio and Murray [22], and effectively used in [23],since our output dynamics are depended on the input u.

Let h(x,u) =[h1(x,u), . . . , hm(x,u)

]>, in addi-

tion, let u =[u1, . . . , u

(n1−1)1 , . . . , ul, . . . , u

(nl−1)l

]>with∑l

i=1 ni = nu, and define the functions:

ϕ0i = hi (40)

ϕji = Lfϕ

j−1i =

∂ϕj−1i

∂xf +

j−1∑k=0

∂ϕj−1

∂u(k)u(k+1) = y

(j)i (41)

where Lfϕj−1i is the Lie derivative of ϕj−1

i in the directionof the vector field f and coincides with the j-th derivativeof the i-th output, y(j)i .

Next, define the map Φ(x, u) : Rn × Rnu → Rn to be:

Φ(x, u) =[h1, ϕ

11, . . . , ϕ

k1−11 , . . . , hm, ϕ1

m, . . . , ϕkm−1m

]>∀ ki |

∑mi=1 ki = n.

Then, the system in (38), (39) is locally observable if thereexits a non-empty set X ×U ⊂ Rn × Rnu , such that the mapΦ(x, u), for some ki, is invertible with respect to x and itsinverse is smooth ∀ (x, u) ∈ X × U , in other words:

rankO = rank

(∂Φ(x, u)

∂x

)= n (42)

where O is the local non-linear observability matrix.Notice, the choice of coordinates needed to define the map

Φ depend on the dynamics and are not unique, since thereare many combination of ki’s that sum up to n. To this end,it suffices to find a map that satisfies the condition in (42).