State Estimation for Force-Controlled Humanoid Balance using Simple
Models in the Presence of Modeling Error
Benjamin J. Stephens
Abstract—This paper considers the design of state estimators
for dynamic balancing systems using a Linear Inverted Pendu-
lum model with unknown modeling errors such as a center
of mass measurement offset or an external force. A variety of
process and output models are constructed and compared. For
a system containing modeling error, it is shown that a naive
estimator (one that doesn’t account for this error) will result in
inaccurate state estimates. These state estimators are evaluated
on a force-controlled humanoid robot for a sinusoidal swaying
task and a forward push recovery task.
I. INTRODUCTION
The goal of this paper is to provide a practical exploration
of the effects of modeling error and unknown forces on state
estimation for dynamic balancing humanoid robots. These
disturbances have the effect that the observed dynamics do
not match the expected dynamics, a discrepancy that cannot
be simply represented as process noise. State estimators, such
as a Kalman filter, are complicated by this because they rely
on a prediction step that uses the expected forward dynamics.
Likewise, model-based control frameworks, such as model
predictive control (MPC), use the expected dynamics to
generate a control policy. This policy is also often in the form
of a state feedback controller which requires estimates of the
full state of the system. This paper will explore how certain
modeling errors can be overcome, either through changes to
the process model in the estimator or the addition of specific
sensing capabilities.
Humanoid balance is often studied using simplified mod-
els. One of the simplest and most widely-used is the Linear
Inverted Pendulum Model (LIPM) [1] which treats the body
as a lumped mass, centered at the center of mass (COM), that
only moves horizontally. These assumptions result in a sim-
ple linear model of the dynamics, opening up the possibility
of using any number of modern linear control and estimation
techniques. One of the most widely used applications of
the LIPM has been walking trajectory optimization using
preview control [2] [3], which outputs a COM trajectory that
satisfies the constraints that the center of pressure (COP) be
within the base of support.
In general, many implementations of balance control on
humanoid robots rely on stiff position-controlled joints and
perfect tracking of the desired COM trajectory. However,
these methods tend to fail in the presence of un-modeled
disturbances such as uneven ground. This paper is focused
on robots with force-controlled joints, such as the Sarcos
B. J. Stephens is a member of the Robotics Institute, Carnegie Mellon
University, 5000 Forbes Ave, Pittsburgh, PA, USA. bstephens@cmu.edu,
http://www.cs.cmu.edu/˜bstephe1
Fig. 1. The Sarcos Primus humanoid is a force-controlled robot that
requires active balance control in the presence of modeling error.
Primus humanoid shown in Figure 1, that can comply to
disturbances but require reactive balance control. Balance for
force-controlled robots is often achieved using force-control
techniques [4] [5] [6], where feedback is required to control
COM state.
The problem being explored in this paper is that the state
(position and velocity) of the COM are not easily determined.
Not only is there no direct measurement of velocity, but
modeling error can be significant. Often, modeling error
has been modeled by augmenting the process and/or output
models with step disturbances [7] [8]. The construction of
these augmented models can have both positive and negative
effects. This paper will consider both types of disturbances
in the context of humanoid balance. First, an error in the
true position of the COM offset can be represented by an
output disturbance. Second, an unknown external force can
be modeled by a state disturbance.
This paper is outlined as follows. In Section II, the
different models for disturbances, sensors and estimators are
presented and their performance is theoretically predicted by
computing the observability of the system. Next, in Section
III, state estimators are designed and compared for different
cases of modeling error. Finally, in Section IV, the estimators
are applied to the state estimation of a force-controlled
humanoid robot performing different balance tasks.

II. SYSTEM MODELING
For this paper, the 1D Linear Inverted Pendulum dynamics
[1] are used as the base process dynamics.
x = !2 (x xc) (1)
where ! =
p
g=z0, x is the position of the center of mass
(COM), z0 is the constant height of the COM, and xc is
the position of the center of pressure (COP). The LIPM
dynamics are convenient because they can be written as a
discrete linear system in statespace form,
xk+1 = Axk + Buk (2)
where xk = fx; _x; xcgT is the state vector and uk = _xc is
the input at timestep k. The matrices in (2) are given by
A0 =
2
4
1 T 0
!2T 1 !2T
0 0 1
3
5 (3)
and
B =
2
4
0
0
T
3
5 (4)
where T is the timestep. This simple Euler approximation
is used for clarity, although can be easily implemented for
small T , such as the T = 0:0025s corresponding to the
400Hz control rate on the robot. A0 is the basic dynamics
equation that will be used throughout this paper, though some
modifications will be proposed.
A linear measurement model is also assumed to have the
form,
yk = Cxk (5)
where yk is a vector of measurements. The structure of
C depends on the type of sensor modalities and state
description.
A. Sensing Models
This section will describe various sensing models to to be
used in (5). The sensor models are denoted by Cijj where
i denotes the sensor model (i.e. Si) and j denotes the state
dynamics (i.e. Aj). Combinations of sensor models will be
represented by Ciijj , for example C12jj .
S1) Position - Position sensors are the most common type
of sensor. In this case, the position of the COM is calculated
from a model of the mass distribution in the system. In
general this mass distribution will be incorrect, so
C1j0 =

1 0 0

(6)
S2) Center of Pressure - It is also common for balancing
systems to have contact force sensors that measure the center
of pressure of the system. In this case,
C2j0 =

0 0 1

(7)
S3) Total COM Force - A ground contact force sensor
can provide the total horizontal force on the COM. In this
case, (1) shows up in the output model, multiplied by the
mass of the system,
C3j0 =

m!2 0 m!2

(8)
B. Modeling Errors
Two general types common to humanoid robots are con-
sidered here: an unknown COM offset in the measurement
output and an unknown external force. The modified process
and output models for these will be presented.
D1) Unknown COM Offset - It is often the case in real
systems that the true position of the COM will be unknown.
This error can be represented by an output step disturbance,
xk = x^k +
xk (9)
where x^ is the estimated COM position and
x is an
unknown offset. In this case, a COM offset can be appended
to the state vector, x = fx; _x; xc;
xg and re-write the
process model as
A1 =
2
6
6
4
1 T 0 0
!2T 1 !2T 0
0 0 1 0
0 0 0 1
3
7
7
5 (10)
which has the same dynamics as in (1).
D2) External Force - It also quite possible that an
unmodeled external force could be perturbing the system.
This force could represent an unknown contact with the
world such as leaning against a wall, an interaction force
such as shaking a hand, or an inertial frame force such as
standing on a moving bus. In this case, the dynamics take
the form,
x = !2 (x xc) + ux (11)
where ux is an unknown external force. In this case, an
external force estimate can be appended to the process
model, making the state, x = fx; _x; xc; uxgT , which has
the dynamics of (11) and the state space dynamics,
A2 =
2
6
6
4
1 T 0 0
!2T 1 !2T T
0 0 1 0
0 0 0 1
3
7
7
5 (12)
D12) External Force and COM Offset - If both of the
above modeling errors are assumed, then both an unknown
external force and COM offset are appended to the dynamics
of the system,
A12 =
2
6
6
6
6
4
1 T 0 0 0
!2T 1 !2T T 0
0 0 1 0 0
0 0 0 1 0
0 0 0 0 1
3
7
7
7
7
5
(13)
C. State Estimators
Process and measurement noise models are also con-
structed for the system to form the dynamics needed by the
state estimator,
xk+1 = Axk + Buk + x (14)
yk = Cxk + y (15)

Fig. 2. Table of covariance values used for the various estimators
where x is a vector representing the process noise of each
of the components of xk and y is a vector of measurement
noise on each component of yk. The noise variables are
assumed to be Gaussian processes with
x  N (0;Q) (16)
y  N (0;R) (17)
The covariance values used throughout this paper are
shown in Figure 2.
An estimator will be denoted by Fijj which corresponds
to a process model Aj and a sensor model Cijj . Below are
descriptions of various estimator.
F0) Naive Estimator (Fij0) - This estimator is referred
to as naive because it has no representation of the modeling
error.
F1) COM Offset Estimator (Fij1) - For this estimator,
the A1 process model includes the COM offset as a variable
and the ouput model is modified with a position measurement
derived from (9),
C1j1 =

1 0 0 1

x (18)
F2) External Force Estimator (Fij2) - In this case, the A2
process model is used but the output model is unchanged,
except for adding an additional column for the force, for
example,
C12j1 =

1 0 0 0
0 0 1 0

x (19)
F12) Dual Estimator (Fij12) - In this case, the A12
process model is used and the output model includes two
additional columns, for example
C12j12 =

1 0 0 0 1
0 0 1 0 0

x (20)
D. Observability
One way to partially predict the performance of different
estimators is to consider the observability of the system
defined by (14) and (15).
Given an estimator, Fijj , observability is determined by
evaluating [9]
O

Fijj


= rank
0
B
B
B
B
B
@
2
6
6
6
6
6
4
Cijj
CijjAj
CijjA2j
...
CijjA
n1
j
3
7
7
7
7
7
5
1
C
C
C
C
C
A
(21)
Fig. 3. Table of observability given different process and output models.
If the observability is equal to the size of the state vector then the state is
fully observable. An (X) signifies that the system is not observable given
these conditions.
where n is the dimension of the state vector (including
any offset parameters). If O

Fijj


= n, then the system is
observable. The table in Figure 3 gives this value for several
different process and sensor model pairs.
Note that the dual estimator (F12) is not observable even
given all three sensors (S123). This is because the S3 sensor
does not provide an independent measure of the dynamics.
III. STATE ESTIMATOR COMPARISONS
In this section, the performance of various estimators in
the presence of different modeling errors is compared. The
estimators take as inputs data from known ground truth state
trajectories generated by sinusoids with an amplitude, a, and
frequency of .
The state trajectories used have the form
x(t) = a sint (22)
_x(t) = a cos!t (23)
xc(t) = x(t) +
1
!2

a2 sint+ ux


(24)
and COM measurements of
y1 = x(t)
x (25)
y2 = xc(t) (26)
A. Naive Estimation
First the need for estimators which account for modeling
error is demonstrated. In this section a naive state estimator
(F0) is implemented. However, when generating the ground
truth trajectories, modeling error in the form of a COM
measurement offset (D1) or an external force (D2) is added.
The performance using position-only (S1) and position-COP
(S12) measurement models is compared.
Figures (4) and (5) show the results using the naive
estimators in the presence of a COM offset and an unknown
external force, respectively. Using S1, the velocity is cor-
rectly estimated but results in a steady state offset of the
COP. For the S12 case, the COP is correctly estimated but
there is a steady state offset in the velocity. Both S1 and S12
fail in the D1 case to correctly estimate the COM position.
Because of this, it is desirable to modify the estimator to
include additional states that correct for these steady state
offsets.

Fig. 4. State estimation performance unknown COM offset (D1) comparing
position-only measurement (S1) with position and COP measurement (S12).
A COM offset of 2cm was used.
Fig. 5. State estimation performance with unknown external force (D2)
comparing position-only measurement (S1) with position and COP mea-
surement (S12). The external force was set such that ux = 0:5.
B. COM Offset Estimation (F1)
Results using a Fij1 state estimator in the presence of an
output error of
x = 2cm are shown in Figure 6. Using
only S1, the system is unable to estimate the state properly,
whereas using S12 the estimator quickly converges.
C. External Force Estimation (F2)
Results using a Fij2 state estimator in the presence of a
state disturbance of ux = 0:5m=s2 are shown in Figure 7.
Both S1 and S12 are able to estimate the COM position and
velocity, but S12 is also able to estimate correctly the COP
and the external force.
IV. EXPERIMENTAL RESULTS
The intended application of these state estimators is to
better estimate the COM state of the Sarcos Primus humanoid
robot shown in Figure 1. It can be expected that both types of
modeling error (D1 and D2) will be present in the system.
Because of the complexity and high dimensionality of the
robot, an offset in the modeled COM and the true COM
is likely. In addition, the hydraulic hoses that provide the
Fig. 6. State estimation performance with unknown COM offset (D1) com-
paring position-only measurement (S1) with position and COP measurement
(S12). The COM offset (with a true value of
x = 0:02m) is added as an
additional state variable in the process model.
Fig. 7. State estimation performance with unknown external force (D2)
comparing position-only measurement (S1) with position and COP mea-
surement (S12). The external force (with a true value of ux = 0:5m=s2)
is added as an additional state variable in the process model.
the offboard power to the robot are known to apply external
forces to the robot that cannot be measured. Below, the model
of the robot used and its relation to the simplified models
presented earlier is shown. Then the experimental setups are
described and results are shown.
A. Robot Model
A rigid body dynamics model of the robot is assumed.
The model has 12 actuated joints (6 in each leg) and a
free-floating root joint (the hip). The torso and upper body
joints are assumed to be rigidly attached to the hip and are
controlled using stiff PD control with constant desireds.
From the robot, the measurements received are joint
angles, q, from potentiometers, and hip orientation, r0,
given by an IMU mounted to the hip. This leaves the hip
position variables, x0, which are not observable. However,
by assuming that a foot is flat on the ground at a known
position these variables can be determined from the forward
kinematics. There are two important features computed by

Fig. 8. Comparing the COM position estimation of multiple estimators,
two of which estimate COM offset errors
the forward kinematics, including the foot centers,
xL = fL (0; r0;q) (27)
xR = fR (0; r0;q) (28)
and the COM position,
x = fC (0; r0;q) (29)
where fX(x; r;q) represents a forward kinematics function.
In the above equations, x0 is set to zero. Therefore, assuming
for example that the left foot is flat on the ground, the COM
measurement is given by
x^ = x xL (30)
The COP measurement is computed by combining the mea-
sured COP under each foot using a 6-axis force/torque
sensor. If xpL and xpR are the COPs and FzL and FzR are
the vertical forces under the left and right foot, respectively,
then
x^c =
(xL + xpL)FzL + (xR + xpR)FzR
FzL + FzR
xL (31)
if the total COP, again relative to the left foot.
B. Experiments
Two types of experiments were performed on the Sarcos
Primus humanoid robot. In both experiments, the robot
data was generated from a fixed balance controller using
only low-pass filtered COM measurements and no state
estimators. The state estimators were applied to this data and
performance was compared. For verification, the robot was
standing on a force-measuring platform. Though it is pre-
dicted to fail, a F123j12 estimator is included for comparison
which uses the force measurements from the force platform
and the output model in (8). The lower covariance for the
COM offset is used for the F123j12 estimator because this
variable is likely to change much slower than the external
force.
Coronal Swaying - For this experiment, the robot is
standing on a force-measuring platform and swaying side to
Fig. 9. Zoomed in view of the COM velocity estimation for multiple
estimators
Fig. 10. Velocity estimation error of multiple estimators compared to a
computed velocity
side in the coronal plane in a sinusoidal pattern. Using the
measured COM and COP positions, several state estimators
were applied to the data. Position estimates are shown in
Figure 8. All estimators perform similarly, with Fij1 and
Fij12 additionally estimating a significant COM offset of
approximately 2:4cm. Figure 9 shows a zoomed-in view
of the velocity estimates after some time has passed. The
velocities are compared to a computed velocity that is
calculated by differentiating and applying a zero-phase low-
pass filter. The velocity estimation is generally improved by
the more advanced estimators. The table in Figure 10 shows
compares the error between the estimates and the computed
velocity.
Sagittal Pushes - For this experiment, the robot is standing
on a force-measuring platform and pushed from the front in
the sagittal plane. The push is performed using a stick with
a 6-axis force/torque sensor mounted at the end point. There
are actually two types of external forces in this experiment.
There is a small, nearly-constant external force caused by the
hydraulic hoses in addition to the large momentary pushes
from the experimenter. The F2 estimator was applied to this
data with varying covariances for the external force variable
and the results are shown in Figure 11. As the process
noise covariance is increased, the estimator more closely
estimates the push force, but also begins to incorrectly
estimate external forces during the transient recovery period
between pushes.

Fig. 11. External force estimation while undergoing pushes in the sagittal
plane for various process noise models
V. DISCUSSION AND FUTURE WORK
This paper has presented through several examples, from
both simulation and hardware, motivational evidence for
the use of disturbance-modeling state estimators on force-
controlled balance systems. Some of the key insights gained
are summarized:
 Naive estimation in the presence of modeling error
results in inaccurate results
 COM position only sensing (S1) almost always per-
forms worse than when combined with COP sensing
(S12)
 The presented models will not allow the estimation of
both COM offset and external force disturbances (D12)
simultaneously
 There is a trade-off between precisely estimating the
disturbance and the state
The disturbance-modeling estimators presented use para-
metric models of the modeling error. However, the COM
offset and external force disturbances are lumped-mass gen-
eralizations of real errors in the highly complex full-body
robot. For example, perhaps there are small modeling errors
in the COM position of every body in the rigid body
dynamics model of the robot. Active estimation of a COM
offset allows for online correction of these small errors
without explicitly relying on the full model. This is the major
motivation for applying simple models such as the LIPM to
humanoid robots.
One of the limitations, however, of the LIPM is that it does
not model the effects of angular momentum and external
torques on the system. Models have been suggested to model
these effects, often through the addition of a flywheel with
a rotational inertia to the LIPM model. In this case the
dynamics become
x = !2 (x xc)

mh
(32)
I  =  (33)
where a torque on the flywheel accelerates both the flywheel
and the COM. The angle of the flywheel, , can be roughly
approximated by the angle of the torso. The added inertia
increases the size of perturbations from which balance can
be recovered [10].
Finally, time varying disturbances were only addressed
briefly in Section IV. There are two approaches to handling
this special class of disturbances. The first approach, sug-
gested here, is careful tuning of the process model to change
the rate at which disturbances are estimated. If the rate is
too high, it is likely the system could go unstable. Another
approach is to assume parametric models of the disturbances.
If system expects to experience sinusoidal disturbances, for
example if it were standing on boat, it might be worthwhile
to model the disturbances in that way instead of a constant.
VI. CONCLUSION
This paper presented the theory and design of state estima-
tors that perform state estimation in the presence of modeling
error by considering different disturbance models and sensor
modalities. A variety of process and output models are
constructed and compared. For a system containing modeling
error, it is shown that a naive estimator (one that doesn’t
account for this error) will result in inaccurate state estimates.
Finally, estimators are applied to the state estimation on a
force-controlled humanoid robot for a sinusoidal swaying
task and a push recovery task.
VII. ACKNOWLEDGEMENTS
This material is based upon work supported in part by
the National Science Foundation under grants DGE-0333420,
ECCS-0325383, EEC-0540865, ECCS-0824077, and IIS-
0964581.
REFERENCES
[1] S. Kajita and K. Tani, “Study of dynamic biped locomotion on
rugged terrain-derivation andapplication of the linear inverted pendu-
lum mode,” in Proceedings of the IEEE International Conference on
Robotics and Automation, vol. 2, Apr. 1991, pp. 1405–1411.
[2] S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K. Harada, K. Yokoi,
and H. Hirukawa, “Biped Walking Pattern Generation by using Pre-
view Control of Zero-Moment Point,” in Proceedings fo the IEEE
International Conference on Robotics and Automation, Taipei, Taiwan,
Sep. 2003, pp. 1620–1626.
[3] P.-B. Wieber, “Trajectory Free Linear Model Predictive Control for
Stable Walking in the Presence of Strong Perturbations,” in Proceed-
ings of the IEEE-RAS International Conference on Humanoid Robots,
vol. 25, no. 4, 2006, pp. 137–142.
[4] J. Pratt, P. Dilworth, and G. Pratt, “Virtual model control of a bipedal
walking robot,” Proceedings of the IEEE International Conference on
Robotics and Automation, vol. 1, pp. 193–198, Apr. 1997.
[5] S.-H. Hyon, J. G. Hale, and G. Cheng, “Full-Body Compliant Human
Humanoid Interaction: Balancing in the Presence of Unknown External
Forces,” IEEE Transactions on Robotics, vol. 23, pp. 884–898, Oct.
2007.
[6] B. J. Stephens and C. G. Atkeson, “Dynamic Balance Force Control
for Compliant Humanoid Robots,” in Proceedings of the International
Conference on Intelligent Robots and Systems, 2010.
[7] K. Muske and T. Badgwell, “Disturbance modeling for offset-free
linear model predictive control,” Journal of Process Control, vol. 12,
no. 5, pp. 617–632, Aug. 2002.
[8] G. Pannocchia and J. B. Rawlings, “Disturbance models for offset-free
model-predictive control,” AIChE Journal, vol. 49, no. 2, pp. 426–437,
Feb. 2003.
[9] C. I. Byrnes and C. F. Martin, “Global observability and detectability:
An overview,” Modelling and Adaptive Control, 1988.
[10] B. Stephens, “Humanoid Push Recovery,” in Proceedings of the IEEE-
RAS International Conference on Humanoid Robots, 2007.