# A kinematics-dynamics based estimator of the center of mass position for anthropomorphic system - A complementary filtering approach

Justin Carpentier, Mehdi Benallegue, Nicolas Mansard, Jean-Paul Laumond

<figure><img src="image_1.png" /></figure>

## To cite this version:

Justin Carpentier, Mehdi Benallegue, Nicolas Mansard, Jean-Paul Laumond. A kinematics-dynamics based estimator of the center of mass position for anthropomorphic system - A complementary filtering approach. 2015 IEEE-RAS International Conference on Humanoid Robots, Nov 2015, Seoul, South Korea. pp.1121 - 1126, 10.1109/HUMANOIDS.2015.7363493. [hal-01175592v3](https://hal.science/hal-01175592v3)

HAL Id: hal-01175592

https://hal.science/hal-01175592v3

Submitted on 13 Jan 2016

HAL is a multi-disciplinary open access archive for the deposit and dissemination of scientific research documents, whether they are published or not. The documents may come from teaching and research institutions in France or abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est destinée au dépôt et à la diffusion de documents scientifiques de niveau recherche, publiés ou non, émanant des établissements d'enseignement et de recherche français ou étrangers, des laboratoires publics ou privés.

<figure><img src="image_2.png" /></figure>
# A Kinematics-Dynamics based Estimator of the Center of Mass position for Anthropomorphic System - A Complementary Filtering Approach

Justin Carpentier, Mehdi Benallegue, Nicolas Mansard and Jean-Paul Laumond

*Abstract*— This paper presents an original approach to simply and efficiently estimate the center of mass position of a free-floating base system, like a humanoid robot or a human body. This approach relies on the theory of complementary filtering, which is a popular technique in aerial robotics, but rarely implemented in humanoid robotics. The main idea consists in merging input measurements like the zero-moment point position, the contact forces, etc. which are then filtered according to their reliability in their respective spectral bandwidth. We validate this approach in simulation by (i) comparing the estimated center of mass trajectory with its real value and (ii) showing that the complementary filter offers on average a least reconstruction error than the classic Kalman filter.

## I. PROBLEM STATEMENT

Biomechanics and humanoid robotics communities share a common interest in the estimation of Center of Mass (CoM) position. From a biomechanics perspective, it concerns the CoM position of the human body which depends on a very large number of parameters, including soft tissues shapes and densities. These parameters are classically reduced to articular angles coupled to a mass distribution model considering perfectly rigid limbs [1]. Nevertheless, the CoM of humans is at the heart of classical biomechanical studies on equilibrium and locomotion [2]. Indeed, CoM trajectories constitute a synthetic, mechanically and geometrically relevant motion descriptor [3], and its dynamics carries also information about the contact forces necessary to compensate for the gravity and ensure locomotion. The more accurate is the reconstruction of the CoM trajectory, the more precise will be the extraction of features and phenomena from studied motions.

In Robotics, the CoM of a humanoid robot depends on the configuration of the robot and the dynamical model. Although the modeling error is much lower for humanoid robots than for humans, they are usually extracted from CAD data and may contain discrepancies with the real robot. And, the aging of the robot in addition to material updates and repairs lead the robot parameters to drift from the initial model, and may require a new calibration process [4]. Despite that, the CoM is the main control variable for walking motion generation. For instance, on flat ground this control aims mainly to ensure displacement in space while respecting the balance constraints that the center of pressure of the feet, also called Zero Moment Point (ZMP), stays always strictly inside the convex hull

The authors (firstname.surname@laas.fr) are with CNRS, LAAS, 7 avenue du colonel Roche, and Univ. de Toulouse, F-31400 Toulouse, France.

<figure><img src="image_3.png" /><figcaption>Fig. 1: The problem of merging measurements for CoM reconstruction in the presence of noises and modeling errors.</figcaption></figure>

of the contact surface [5]. The modeling errors inducing misestimation of the CoM position which may then endanger the balance of humanoid robots [6]. All these modeling errors are due to forward kinematic estimation. Nevertheless, many experimental setups that study human motion and most humanoid robots are equipped with force and moment sensors at contact points that provide dynamics-based, unbiased, ground-truth data on the CoM kinetics.

We have three input signals. The first one comes from the geometry and contains the modeling errors. This signal also suffers from noise coming from improper estimation of the position of the limbs, especially in the case of motion capture. The second signal is the force measurement, which is proportional to CoM accelerations. The third one is the ZMP measurement that we construct using the forces and the moments. The ZMP can be approximated by a linear combination of horizontal positions and accelerations of the CoM. Both last signals suffer from measurement noise. The linear approximation of ZMP, commonly assumed, also introduces an estimation error.

This paper proposes a simple method for multi-sensor data fusion to merge these three signals into an estimator (see Fig. 1). To do so, we take into account an important property that characterizes these signals : they have different spectral distributions of errors and noises. This means that for almost any frequency range of the CoM trajectory, there is a signal providing a better estimation than others. We propose to use data fusion based complementary filtering. This common technique consists in merging input signals that suffer from errors that lie in different bandwidths into one output signal. It is a simple and real-time method that enables to obtain theoretically unbiased, noise-free and non-phase-shifted estimation of the CoM position.

Section II describes the dynamical system linking the input signals to the trajectory of the CoM. Section III introduces our linear complementary filter for three signals.
<figure><img src="image_4.png" /><figcaption>Fig. 2: A sketch representation of the spectral distribution of errors that would emerge from the naive reconstruction of CoM trajectory if we use only one signal (Geometry, Forces and ZMP). The signal with the lowest error is then selected at each frequency bandwidth to constitute minimal-error fusion of these signals.</figcaption></figure>

In Section IV we show how our method behaves against simulated noisy measurements and we compare the performance to estimation by a Kalman filter. In Section V, we compare our method to existing approaches.

## II. THE ANTHROPOMORPHIC SYSTEM MODEL

In this section, we briefly describe the equations of the dynamics of a free-floating system with an anthropomorphic structure. The main purpose is to emphasize the link between the measured quantities (i.e. the estimates of the positions of the CoM and ZMP, and the forces) and the under-actuated dynamics, namely the dynamics reduced around the CoM.

### A. The under-actuated dynamics

Consider first the Lagrangian dynamics of a $n$ degrees of freedom free-floating based system which makes $N$ contacts with its environment. We name $q \in Q \stackrel{\text{def}}{=} SE(3) \times \mathbb{R}^n$ the configuration vector of the system and $\dot{q}, \ddot{q}$ its first and second time derivatives. The Lagrangian dynamics reads:

$$M(q)\ddot{q} + b(q, \dot{q}) = G(q) + S^{\top}\tau + \sum_{i} J_{i}^{\top}(q)\phi_{i}, \quad (1)$$

where $M$ stands for the mass matrix, $b$ for the centrifugal and Coriolis effects, $G$ for the action of the gravity field. $S$ is a selection matrix which distributes the torque $\tau$ over the joints space, $J_i$ is the jacobian of contact $i$ and $\phi_i = (f_i, v_i)$ is the vectorial representation of the unilateral contact wrenches acting on the robot with linear $f_i$ and angular $v_i$ components.

This dynamical equation can be split into two parts: the under-actuated dynamics, i.e the dynamics of the free-floating base (denoted by $b$) and the dynamics of the actuated segments (denoted by $a$):

$$\begin{bmatrix} M_b \\ M_a \end{bmatrix} \ddot{q} + \begin{bmatrix} b_b \\ b_a \end{bmatrix} = \begin{bmatrix} G_b \\ G_a \end{bmatrix} + \begin{bmatrix} 0_6 \\ \tau \end{bmatrix} + \sum_i \begin{bmatrix} J_{i,b}^\top \\ J_{i,a}^\top \end{bmatrix} \phi_i \quad (2)$$

The first row of (2) is the so-called Newton-Euler equation of a moving body, having a mass $m$, a CoM position $c$ relative to the inertial frame, a linear and angular momenta denoted by $p$ and $\mathcal{L}$ respectively. This under-actuated

dynamics can be rewritten as:

$$\dot{p} = \sum_{i} f_{i} - mg \quad (3)$$

$$\dot{\mathcal{L}} = \sum_{i} (\mathbf{p}_i - \mathbf{c}) \times \mathbf{f}_i + \nu_i, \quad (4)$$

where $\times$ denotes the cross product operator, $\mathbf{p}_i$ is the position of the contact point $i$ relative to the inertial frame and $g$ is the gravity field.

### B. The Zero-Moment Point

Under the hypothesis where all contact points lie on the same plane surface, the ZMP horizontal components are defined by [7]:

$$z^{x,y} = c^{x,y} - \frac{c^z}{\ddot{c}^z + g^z} \ddot{c}^{x,y} + \frac{1}{m(\ddot{c}^z + g^z)} \begin{bmatrix} -\dot{\mathcal{L}}^y \\ \dot{\mathcal{L}}^x \end{bmatrix} \quad (5)$$

And, if we neglect the variation of the angular momentum and consider the vertical CoM position remains constant, we obtain the so-called cart-table model:

$$z^{x,y} = c^{x,y} - \frac{c^z}{g^z} \ddot{c}^{x,y}, \quad (6)$$

which is linear in both variables $c^{x,y}$ and $\ddot{c}^{x,y}$.

## III. THE LINEAR COMPLEMENTARY FILTER

The complementary filter [8] is well known in the field of aerial robotics [9], for example to estimate the attitude of a quad-rotor system by combining the gyroscopic and accelerometer measurements. Unlike the Kalman filter [10] which makes no distinction between the contributions of each measurement in the frequency domain, the complementary filter exploits the influence and the accuracy of each input signal in their respective frequency domain and reconstructs the integrality of the signal by a combination of filtered measurements, with zero-phase-shift. A simple example of complementary filter is derived in Sec. III-A. All along this section, we make use of the following definition:

**Definition 1 (Linear Complementary Filter):** We say that the transfert function $Y$ is the linear complementary filter of the transfert function $X$ if and only if $X(s) + Y(s) = 1$ for any $s \in \mathbb{C}$, $s$ being the Laplace variable.

For our case, we have three signals providing information on the CoM.

[S1] The first one is the geometry-based reconstruction of the CoM. It suffers mainly from biases due to modeling errors of mass distribution. It is also subject to high frequency sensor noise due to motion capture technology or the measurement of the angular position of the joints. The error between this signal and the real position of the CoM lies then in low and high frequency domains.

[S2] The second signal is the CoM acceleration extracted from force measurements. This signal suffers also from sensor noise. The double integration of this signal reduces the high frequency error but generates quadratic drift, visible in low and medium frequencies.
<figure><img src="image_5.png" /><figcaption>Fig. 3: On the left, diagram of the complementary filter for the CoM vertical component. On the right, the diagram for the horizontal components.</figcaption></figure>

[S3] The third signal is the ZMP, which only provides information about the horizontal position of the CoM. It contains high frequency sensor noise, but also carries error due to the linear approximation (6). This approximation assumes a constant CoM height and a negligible variation of the angular momentum. These assumptions are particularly acceptable in low frequency domain, specifically below natural walking rhythm. Similar reasoning can be found in [11], [12].

These three considerations are schematized in Fig. 2.

In the following, we gradually design the complementary filters of the CoM vertical and horizontal components. We designate by $s$ the Laplace variable acting in the frequency domain. The Laplace Transform of a temporal signal $g(t)$ is written $G(s)$ and $sG(s)$ corresponds to the Laplace Transform of its time derivative $\dot{g}(t)$.

### A. Complementary filter of the CoM vertical component

1) **Hypothesis:** The vertical position $c^z$ of the CoM can be observed from two decoupled sources of measurement:

* (1) the mass distribution of the anthropomorphic system coupled with the current measure of the positions of links provide an estimation $\tilde{c}^z$

* (2) vertical contact force measurements divided by the mass $m$ and subtracted from gravity (cf. Newton equation (3)) provide the estimation $\tilde{c}^z$ of the vertical acceleration of the CoM.

The complementary filter diagram corresponding to those two measurements is shown in Fig. 3. $H_1^z$ and $H_2^z$ are the linear filters of $\tilde{c}^z$ and $\tilde{c}^z$ respectively.

2) **Design:** For the first signal [S1], the noise is mostly located in the high frequency domain. While for the second measurement [S2], due to the double integration process, the noise is mainly concentrated in the low frequency domain. Then $s^2H_2^z$ is made a high-pass filter<fnref n="1" /> in order to filter out the low frequency noise in the double integration processus. We can now set:

$$s^2 H_2^z(s) = \frac{(s\tau_1)^2}{(1 + s\tau_1)^2}, \qquad (7)$$

with $\tau_1 \stackrel{\text{def}}{=} \frac{1}{2\pi f_1}$ the time constant and $f_1$ the cut-off frequency of the high-pass filter $s^2H_2$. Therefore (7) reads:

$$H_2^z(s) = \frac{\tau_1^2}{(1 + s\tau_1)^2} \qquad (8)$$

Accordingly, $H_1$ can be directly computed as the complement of $s^2H_2$, i.e. $H_1 \stackrel{\text{def}}{=} 1 - s^2H_2$. So $H_1$ is of the following form:

$$H_1^z(s) = \frac{1 + 2s\tau_1}{(1 + s\tau_1)^2}, \qquad (9)$$

which is the combination of a low-pass filter and a bandpass filter, both with a unique cutting frequency of value $f_1$. As expected, $H_1$ acts in the low frequency domain. Fig. 4 represents the bode diagram of the two designed filters $H_1$ and $H_2$. One can remark that both filters depends on the same free variable $f_1$, which corresponds to the unique cutting frequency of the low-pass and high-pass filters. The choice of $f_1$ corresponds to the frequency of sensor noise in the force measurement and link position reconstruction.

**Remark:** The double integrator corresponding to acceleration measurements imposes the order of the filter to be at least 2; otherwise, the filter $H_1^z$ would be non-causal.

### B. Complementary filter of the CoM horizontal components

In the case of horizontal components estimation, we add the measure of the ZMP position [S3] to the input of the estimator. The complete diagram of the complementary filter of horizontal components is depicted in Fig. 3.

1) **Hypothesis:** The hypotheses of the CoM vertical component still hold for the case of the horizontal ones. We make the hypothesis the ZMP is a good approximation of the CoM position for very low frequencies.

2) **Design:** We first write the Laplace Transform of (6):

$$Z^{x,y}(s) = (1 - \tau_z^2 s^2) C^{x,y}(s), \qquad (10)$$

with $\tau_z \stackrel{\text{def}}{=} \sqrt{\frac{c^z}{g^z}}$ the time constant of the linearized ZMP model. (10) can be rewritten as:

$$Z^{x,y}(s) = (1 + \tau_z s)(1 - \tau_z s) C^{x,y}(s) \qquad (11)$$

The ZMP dynamics (11) is a second order differential equation whose integration reveals one stable $(1 + \tau_z s)$ and one unstable $(1 - \tau_z s)$ component [13]. So, we need to reject this unstable dynamic in order to make the complementary filter stable. Finally, we want that:

$$H_3^{x,y}(s)(1 + \tau_z s)(1 - \tau_z s) = \frac{1 - \tau_z s}{(1 + s\tau_2)^2}, \qquad (12)$$

with $\tau_2 \stackrel{\text{def}}{=} \frac{1}{2\pi f_2}$ the time constant and $f_2$ the cutting frequency of the low-pass filter of the ZMP. $H_3^{x,y}$ has the following expression:

$$H_3^{x,y}(s) = \frac{1}{(1 + s\tau_2)^2(1 + \tau_z s)} \qquad (13)$$

While $H_2^{x,y}$ remains unchanged and equal to $H_2^z$, $H_1^{x,y}$ is the complementary filter of both $s^2H_2^{x,y}$ and $(1 + \tau_z s)(1 - \tau_z s)H_3^{x,y}$ transfert functions, leading to:

$$\begin{aligned} H_1^{x,y}(s) &= 1 - s^2 H_2^{x,y}(s) - (1 + \tau_z s)(1 - \tau_z s) H_3^{x,y}(s) \\ &= 1 - \frac{(s\tau_1)^2}{(1 + s\tau_1)^2} - \frac{1 - \tau_z s}{(1 + s\tau_2)^2} \end{aligned} \qquad (14)$$

<footnote n="1">¹the $s^2$ term before $H_2^z$ comes directly from the fact that $s^2C^z(s)$ is the Laplace Transform of $\tilde{c}^z$.</footnote>
<figure><img src="image_6.png" /><figcaption>Fig. 4: Bode diagram of filters: $H_1^z$ (a), $H_2^{x,y,z}$ (b), $H_1^{x,y}$ (c) and $H_3^{x,y}$ (d) with $f_1 = 10$ Hz, $f_2 = 0.4$ Hz and $c^z = 0.6$ m.</figcaption></figure>

Fig. 4 (right) illustrates the bode diagrams of the designed filter $H_1^{x,y}$ and $H_3^{x,y}$. $H_1^{x,y}$ acts as a bandpass filter in a bandwidth around $[f_2; f_1]$, while $H_3^{x,y}$ is mainly a third order low-pass filter. Choosing the frequency $f_2$ is a little bit more subtile than in the case of $f_1$. We consider that the linearized ZMP model is only valid for the frequencies below the fondamental frequency of the locomotion pattern. In general, this fondamental frequency is around 1 or 2 Hz. Therefore, we might consider a value of $f_2$ below 1 Hz.

## IV. EXPERIMENTAL RESULTS

In this section, we apply the proposed complementary filter to the case of a simulated humanoid robot walking in straight line. The simulation framework allows: (i) to obtain ground truth measurements, that will be used for the evaluation of the performance of the complementary filter and (ii) to generate noisy model and measurements which will serve as inputs of the filter. We also compare the performance of the designed complementary filter to a more classic Kalman filtering approach, which uses the same kind of measures while assuming that those sensor measures are affected by a white noise.

### A. Generation of noisy data

1) **Motion generation:** We use standard techniques in humanoid robotics to generate the motion of the robot. We first plan a CoM trajectory according to the given foot placements and ZMP reference trajectory [5]. Then we generate a whole body trajectory using a second-order generalized inverse kinematics [14]; the following tasks where combined using a strict hierarchy: the feet positions (first priority), the CoM trajectory and a fixed orientation of the pelvis (second priority) and finally a posture task to avoid the drift of actuated joints (third and lowest priority).

2) **Generation of noisy measurements:** The second-order kinematics produces a control based on $\dot{q}$ from which we obtain by integration $\dot{q}$ and $q$.

These three quantities injected in the right hand side of the unactuated part of the dynamical equation (2) give us the resulting wrench $\phi_c$ of contact forces. The linear and angular part of the measurement of $\phi_c$ are then perturbed by a Gaussian colored noise in the high frequency domain with standard deviation $\sigma_{\text{linear}} = 10$ N and $\sigma_{\text{angular}} = 10$ Nm, leading to a noisy measurements $\dot{\phi}_c$. From $\dot{\phi}_c$ we obtain the perturbed ZMP position $\tilde{z}$.

The measurement of the configuration vector $q$ is disturbed by another Gaussian colored noise in the high frequency domain too, with a standard deviation $\sigma_{\text{configuration}} = 0.05\pi$. This noise replicates the effects of errors due to motion capture techniques. In addition, we generate an error at the level of the dynamical model of the robot. We add a Gaussian perturbation to the mass distribution of the body and position of the CoM of each link. We make the hypothesis that we know the mass and CoM position of each limb with a precision of 20%. This process aims at generating modeling error for a humanoid robot or for humans due to anthropometric tables. The new dynamical model and the noisy measurements of $q$ enable to generate the geometry-based CoM measurement $\tilde{c}$.

3) **Identification of the mass of the anthropomorphic system:** The total mass of the system is directly measurable. It suffices to exploit the forces measurement in static equilibrium (half-sitting position for a humanoid robot or standing rest position for humans), and, by taking the average value of the vertical forces divided by the gravity value, we obtain a good estimate of the total mass.

### B. Spectral analysis of measurement errors

Fig. 5 shows the Fourier transform of the errors. The simplest spectral distribution is the error of the force measurement $\tilde{c}$ at the middle of the figure. It is simply the Fourier transform of the noise we added initially, which lies in high frequencies that are partly canceled by our $H_2$ low-pass filter. At the top of the figure we see the error of geometry-based estimation of the CoM. As expected, the error mainly lies in low and high frequencies. The medium frequencies bandwidth shows a very clean estimation of the CoM position. The bottom part of the figure shows the spectral distribution of the error between the noisy measurement of the ZMP and the perfect linear ZMP of the model. We see clearly that this measurement is reliable only in low frequencies and grows very fast with frequency increase. This is why we fixed the cut-off frequency at 0.4 Hz. We see then that these figures confirm clearly the hypotheses of Fig. 2.

### C. Estimation and comparison with KF

The three measurements of the walking trajectory were fed to our complementary filter and to a Kalman filter. The estimation of the complementary filter compared with real values is shown on the top of Fig. 6. We see that the
<figure><img src="image_7.png" /><figcaption>Fig. 5: Fourier transform of the error of each signal. On the right, the transform of the error between the real CoM position $c$ and geometry-based estimation $\tilde{c}$. In the middle, the error between the second CoM time-derivative $\dot{\tilde{c}}$ and its estimation $\tilde{\tilde{c}}$. On the left, the transform of the error between the linearized ZMP model (6) and the measured values $\tilde{z}$, defined only in $x$ and $y$.</figcaption></figure>

tracking in $x$ and $y$ axes is accurate. However, the tracking in $z$ is subject to bias. This is due to the absence of bias correction of ZMP along that axis. On the bottom, the estimation error is displayed for the complementary filter and Kalman filter along the three axes. We see that the error of our complementary filter is always inferior or equivalent to the Kalman filter. We see also that the signal of the complementary filter contains more high frequency noises, that partly due to our choice to take the lowest possible orders for the band-pass filters to keep the simplest possible formulation. We believe that more sophisticated filters can get reduce significantly these artifacts without introducing phase shift. This phenomenon is also due to the fact that there is certainly a small frequency bandwidth where we have no perfectly clean signal. This can be tackled by applying model-based filtering to the estimation, which can also enable to avoid phase-shift, but may be subject to other modeling errors. Finally about this figure, along $z$ direction, we see that Kalman filtering estimation is drifted away from the estimation. This is because the Kalman filter integrates the acceleration signal at all frequencies, introducing drift in medium and low frequency ranges.

We also validate the robustness of our filter with respect to the hypothesis of constant COM altitude, by generating a second walking trajectory whose COM height is constrained: it therefore freely derived, mostly in low frequency ranges, which introduces a small error in the frequency range of our ZMP signal. We see in Fig. 6 (right) that along $x$, this modification only affected the estimation in low frequencies, as expected, whereas along $y$ and $z$ this modification had minor effects. Kalman filter suffered more from this modification, especially along the $z$ direction.

We see in the next section how former studies also considered the sensors fusion for CoM estimation in humanoid robotics and human bio-mechanics communities. We compare the existing approaches with the complementary filtering technique in terms of theoretical guarantees and actual performance.

## V. DISCUSSIONS

Our CoM estimation approach is part of an active topic both in research on human motion and in humanoid robotics [15]. For humanoids, the corrections on the CoM provided by forward kinematics is achieved mainly using various measurement system [16] including force sensors [17], [18]. These solutions use mostly Kalman

filtering techniques which is agnostic of the frequency domains of each signal. On the other hand, the CoM reconstruction has a longer history in the field of biomechanics [19]. Moreover, since few decades, force platforms were already considered for CoM position estimation [20]. Most of the methods did not consider the fusion of Force sensors with direct kinematics reconstruction of the CoM [21], [22].

To our best knowledge, the closest published work to our method is the technique by Maus et al [12]. The Kinematic CoM estimation was low-pass filtered and the double time-integral of forces was high-pass filtered, before adding the two signals. However, the use of non-complementary filtering removes the guarantee to obtain the totality of the initial signal with zero-phase-shift. Moreover, the double integration of acceleration is not a stable process and this method requires to reset regularly the integral to zero. Instead, our method works for arbitrary durations thanks to the stability of all our filters. Schepers et al [11] developed the same approach as Maus et al, but with ZMP and force measurements. In addition to theoretical guarantees and integration stability issues, this method neglected the horizontal accelerations for the ZMP, which increases the approximation errors.

## VI. CONCLUSION AND FUTURE WORKS

We have seen through this paper the design and the implementation of an estimator of the position of the CoM of humans and humanoid robots based on multi-sensor data fusion. This estimator takes profit from the most common sensors available both on-board robots and for human motion recording: the CoM reconstruction provided by the geometrical reconstruction together with a model of mass distribution, the forces that give CoM accelerations, and the ZMP leading to an approximated linear link between horizontal positions and accelerations. The key idea of our method is to consider that these measurements carry noises and errors, but with separated frequency bandwidths for each error. This statement directs our choice to use a complementary filtering technique to merge these signals, specifically because of its particular suitability to merge different bandwidths of signals.

The direct conclusion is that this complementarity in frequency should be exploited when fusing these three measurements. Complementarity filters are very good at that, while it is not straightforward to achieve with common
<figure><img src="image_8.png" /><figcaption>Fig. 6: On the left, the reconstructed trajectory thanks to the complementary filter. In the middle, error between the ground truth measure of the CoM position and its reconstruction with Kalman and complementary filters. On the right, error between the ground truth measure of the CoM position and its reconstruction with both Kalman and complementary filters, when allowing variations of the CoM vertical position.</figcaption></figure>

Kalman filters. Despite the solid expertise of our team in using Kalman filtering for this kind of measurements, we were not able to obtain qualitative results with such filter while it was immediate to obtain when implementing a complementarity filter.

The main limitations of our approach lies in the non correction of biases in the vertical component of the CoM, because the ZMP measure [S3] provides only a 2 dimensional information. Actually, this limitation takes its root from the Newton-Euler dynamics: the moment of a force is obtained by a cross product, and the cross product is not an invertible operator. Therefore, one dimension of the CoM position is not observable using the force/torque signal. However, some CoM trajectories may still make the observation possible if we do not ignore the CoM vertical displacement. An interesting perspective is to build an estimator that aims to rebuild the full body dynamics by estimating variation of the angular momentum and considering CoM height variations. This estimator would also exploit a nonlinear version of the complementary filtering techniques in order to increase its accuracy on the parts to keep in each of the input signals.

## ACKNOWLEDGMENT

The work is supported by the European Research Council through the Actanthrope project (ERC Grant Agreement 340050), the European project KOROIBOT (FP7 Grant Agreement 611909) and the French National Research Agency project ENTRACTE (ANR Grant Agreement 13-CORD-002-01).

## REFERENCES

[1] P. de Leva, "Adjustments to zatsiorsky-seluyanov's segment inertia parameters," *Journal of Biomechanics*, vol. 29, no. 9, 1996.

[2] C. T. Farley and D. P. Ferris, "Biomechanics of walking and running: Center of mass movements to muscle action," *Exercise and sport sciences reviews*, vol. 26, no. 1, 1998.

[3] J.-P. Laumond, M. Benallegue, J. Carpentier, and A. Berthoz, "The yoyo-man," in *International Symposium on Robotics Research*, 2015.

[4] K. Ayusawa, G. Venture, and Y. Nakamura, "Identification of the inertial parameters of a humanoid robot using unactuated dynamics of the base link," in *2008 IEEE-RAS International Conference on Humanoid Robots*, 2008.

[5] S. Kajita, F. Kanehiro, K. Kaneko, K. Fujiwara, K. Harada, K. Yokoi, and H. Hirukawa, "Biped walking pattern generation by using preview control of zero-moment point," in *2003 IEEE International Conference on Robotics and Automation*, 2003.

[6] M. Benallegue and F. Lamiraux, "Estimation and stabilization of humanoid flexibility deformation using only inertial measurement units and contact information," *International Journal of Humanoid Robotics*, vol. 12, no. 3, 2015.

[7] P.-B. Wieber, R. Tedrake, and S. Kuindersma, *Handbook of Robotics*, ch. Modeling and Control of Legged Robots. Springer, B. Siciliano & O. Khatib editors, 2nd ed., 2015.

[8] W. T. Higgins, "A comparison of complementary and kalman filtering," *IEEE Transactions on Aerospace and Electronic Systems*, vol. 11, no. 3, 1975.

[9] M. Euston, P. Coote, R. Mahony, J. Kim, and T. Hamel, "A complementary filter for attitude estimation of a fixed-wing uav," in *2008 IEEE/RSJ International Conference on Intelligent Robots and Systems*, 2008.

[10] R. E. Kalman, "A new approach to linear filtering and prediction problems," *Journal of Fluids Engineering*, vol. 82, no. 1, 1960.

[11] H. M. Schepers, E. H. Van Asseldonk, J. H. Buurke, and P. H. Veltink, "Ambulatory estimation of center of mass displacement during walking," *IEEE Transactions on Biomedical Engineering*, vol. 56, no. 4, 2009.

[12] H.-M. Maus, A. Seyfarth, and S. Grimmer, "Combining forces and kinematics for calculating consistent centre of mass trajectories," *The Journal of experimental biology*, vol. 214, no. 21, 2011.

[13] J. Pratt, T. Koolen, T. De Boer, J. Rebula, S. Cotton, J. Carff, M. Johnson, and P. Neuhaus, "Captuability-based analysis and control of legged locomotion, part 2: Application to m2v2, a lower-body humanoid," *International Journal of Robotics Research*, vol. 31, no. 10, 2012.

[14] L. Saab, O. E. Ramos, F. Keith, N. Mansard, P. Soueres, and J. Fourquet, "Dynamic whole-body motion generation under rigid contacts and other unilateral constraints," *IEEE Transactions on Robotics*, vol. 29, no. 2, 2013.

[15] S. Cotton, A. Murray, and P. Fraisse, "Estimation of the center of mass: From humanoid robots to human beings," *IEEE/ASME Transactions on Mechatronics*, vol. 14, no. 6, 2009.

[16] M. F. Fallon, M. Antone, N. Roy, and S. Teller, "Drift-free humanoid state estimation fusing kinematic, inertial and lidar sensing," in *IEEE-RAS International Conference on Humanoid Robots*, 2014.

[17] B. J. Stephens, "State estimation for force-controlled humanoid balance using simple models in the presence of modeling error," in *2011 IEEE International Conference on Robotics and Automation*, 2011.

[18] Xinjilefu and C. Atkeson, "State estimation of a walking humanoid robot," in *2012 IEEE/RSJ International Conference on Intelligent Robots and Systems*, 2012.

[19] J. Eng and D. Winter, "Estimations of the horizontal displacement of the total body centre of mass: considerations during standing activities," *Gait & Posture*, vol. 1, no. 3, 1993.

[20] T. Shimba, "An estimation of center of gravity from force platform data," *Journal of Biomechanics*, vol. 17, no. 1, 1984.

[21] O. Caron, B. Faure, and Y. Brenière, "Estimating the centre of gravity of the body on the basis of the centre of pressure in standing posture," *Journal of biomechanics*, vol. 30, no. 11, 1997.

[22] F. Barbier, P. Allard, K. Guelton, B. Colobert, and A.-P. Godillon-Maquinghen, "Estimation of the 3-d center of mass excursion from force-plate data during standing," *IEEE Transactions on Neural Systems and Rehabilitation Engineering*, vol. 11, no. 1, 2003.