Center-of-Mass Estimation for a Polyarticulated System in Contact—A Spectral Approach

This paper discusses the problem of estimating the position of the center of mass for a polyarticulated system (e.g., a humanoid robot or a human body), which makes contact with its environment. The only sensors providing measurements on this point are either interaction force sensors or kinematic reconstruction applied to a dynamic model of the system. We first study the observability of the center-of-mass position using these sensors and we show that the accuracy domain of each measurement can be easily described through a spectral analysis. We finally introduce an original approach based on the theory of complementary filtering to efficiently merge these input measurements and obtain an estimation of the center-of-mass position. This approach is extensively validated in simulations by using a model of a humanoid robot through which we confirm the spectral analysis of the signal errors and show that the complementary filter offers a lower average reconstruction error than the classical Kalman filter. Some experimental applications of this filter on real signals are also presented.

Abstract-This paper deals with the problem of estimating the position of center of mass for a polyarticulated system (e.g. a humanoid robot or a human body), which makes contact with its environment. The only sensors providing measurements on this point are either interaction force sensors or kinematic reconstruction applied to a dynamic model of the system. We first study the observability of the center of mass position using these sensors and we show that the accuracy domain of each measurement can be easily described through a spectral analysis. We finally introduce an original approach based on the theory of complementary filtering to efficiently merge these input measurements and obtain an estimation of the center of mass position. This approach is extensively validated in simulation using a model of humanoid robot where (i) we confirm the spectral analysis of the signal errors and (ii) we show that the complementary filter offers a lower average reconstruction error than the classical Kalman filter. Some experimental applications of this filter on real signals are also presented.

I. MOTIVATION
The comunities of biomechanics and humanoid robotics 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 [2]. Nevertheless, the CoM of humans is at the heart of classical biomechanical studies on equilibrium and locomotion [3]. Indeed, CoM trajectories constitute a synthetic, mechanically and geometrically relevant motion descriptor [4], 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 final robot. Furthermore Justin Carpentier (jcarpent@laas.fr), Mehdi Benallegue (mehdi@benallegue.com), Nicolas Mansard (nmansard@laas.fr) and Jean-Paul Laumond (jpl@laas.fr) are with: 1 CNRS, LAAS, 7 avenue du colonel Roche, F-31400 Toulouse, France 2 Univ de Toulouse, LAAS, F-31400 Toulouse, France Parts of this paper have been submitted to the 15th IEEE-RAS Humanoids Conference [1]. Despite some similarities in the the text, the conference submission presents a different approach based on zero moment point which cannot be applied to non-planar contacts and which is only efficient in horizontal plane. the aging of the robot in addition to material updates and repairs lead the robot inertial parameters to drift from the initial model, and may require a new calibration process [5].
Despite that, the CoM is the main control variable for walking motion generation. For instance, this control aims for example to ensure displacement in space while respecting balance criteria often related to interaction forces [6]. The modeling errors inducing misestimation of the CoM position may then endanger the balance of humanoid robots [7]. There are two kinds of sensors that provide data about the position of center of mass. The first one is the reconstruction of the multibody kinematics using any motion capture technique (optical, IMUs, etc.). The technique requires also the dynamical model representing the inertial parameters of the system. This approach suffers from modeling errors and provides usually biased estimations. The second kind of sensors measures interaction forces and moments with the environment. The forces provide CoM accelerations. Moments are more closely related to the position of the CoM, through a straight line in the space named the central axis of the contact wrench. However this axis is not exactly passing through the center of mass because of the possible variation of angular momentum due to gesticulation [8]. Moreover the position of the CoM along this axis cannot always be known precisely. In addition, all these signals suffer also from measurement noise reducing the estimation quality.
Let us suppose then that we have these three input signals. The first is the biased kinematics reconstruction. The second signal is the acceleration provided by force measurement. And the third signal is the central axis provided by forces and moments. The first contribution of this paper is to study the properties of observability provided by these signals. Then we describe one important property that characterizes these signals : they have different spectral distributions of errors and noises. This means that for a given frequency range of the CoM trajectory, there is a signal providing a better estimation than others. We finally develop a complete method for multi-sensor data fusion to merge all these signals into one estimator (see Figure 1).
We propose to use data fusion based on complementary filtering. Complementary filtering is a common technique which consists in merging input signals that suffer from errors that lie in different bandwidths into one output signal. Furthermore, it is a simple and real-time method that provides theoretically unbiased, noise-free and non-phase-shifted estimation of the CoM position.
In Section II we describe the dynamical system providing the relations between the available signals and the CoM trajectory. Section III analyses the observability conditions of the center of mass position. In section IV we develop our linear complementary filter for the three signals. In Section V we show how our method behaves against noisy measurements in a simulated environment where the ground truth is immediately accessible and we compare the performances to estimation by a Kalman filter fusion. In SectionVI, two scenarios of application of our method on real signals are depicted. And in Section VII, we compare our method to related works.

II. THE UNDERACTUATED POLYARTICULATED SYSTEMS
In this section, we briefly recall the equations of the dynamics of a free-floating system with an polyarticulated structure like a humanoid robot or the human body. The main idea is to make the link between the measured quantities (i.e. the estimates of the position of the CoM, the central axis of the contact wrench, and the forces) and the under-actuated dynamics, namely the dynamics reduced around the CoM.

A. The under-actuated dynamics
We first consider the Lagrangian dynamics of a n degrees of freedom free-floating based system which makes N contacts with the surrounding environment. We name q ∈ Q def = SE(3) × R n the configuration vector of the system andq,q its first and second time derivatives. The Lagrangian dynamics reads: 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 τ over the joints space, J i is the jacobian of the contact point i and φ i is the vectorial representation of the unilateral contact wrenches [9] acting on the robot and it is composed of a linear f i and angular τ 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): The first row of (2) is the so-called Newton-Euler equation of a moving body, having a mass m, a position c relative to the inertial frame, a linear and angular momenta denoted by p and L respectively. The point c is nothing more than the center of mass of the whole anthropomorphic system.
In a more classical manier, this under-actuated dynamics can be rewritten as: where × denotes the cross product operator, p i is the position of the contact point i relative to the inertial frame and g is the gravity field. In order to simplify the notations, we set down: the resulting wrench of contact forces and moments expressed at the center O of the inertial frame. Finally, knowing that p def = mċ and injecting (3) into (4) leads to:

B. The Zero-Moment Point
We make the hypothesis that all contact points lie on the same plane. Without any loss of generality, we assume this plane corresponds to the flat ground with normal vector n, aligned with the gravity field g. The ZMP (also known as the center of pressure [10]), is then defined as the point on the contact plane where the moment component of the resulting wrench is aligned with the normal axis of the plane. The equation of the ZMP (denoted z) is then given by: We can now inject the two first rows of (6) into (7), which leads to the expression of the ZMP position as a function of c and L and their time derivatives: Numerous works in humanoid robotics use the ZMP as a criterion for balance on flat ground. Indeed, as long as the ZMP remains strictly inside the convex hull of the support polygon, support feet don't tip around their edge and the contact is firmly maintained on the ground [11]. Therefore controlling the position of ZMP enables to generate balanced moving locomotion for humanoid robots.
Most of ZMP-based control make the simplification of considering negligible variations of angular momentum around the CoM (L ≈ 0). This makes the CoM lie on the straight line that passes through ZMP and follows the direction of contact force vector f c . We name this line the ZMP axis.
In addition, most walking pattern generators for robots consider also that height of the CoM is constant. This simplification is named cart-table model [6]. In this context we obtain the linearized version of the ZMP: which is linear in both variables c x,y andc x,y .

C. The Central Axis of the Contact Wrench
The notion of central axis of the contact wrench have been extensively used in robotics, either to justify the concept of zero-moment point [10] or to extend this concept for multi-contacts scenarios as depicted in [12], [13] or more recently in [14]. In the following, we recall the notion of central axis and use it as a descriptor of movement. Definition 1. The central axis W c of the contact wrench φ c is defined as the set of points where the torque of the wrench τ Wc is aligned with the resulting force f c . Relatively to the inertial frame center in O, this axis is uniquely defined by: where · denotes the dot product operator, E 3 is the euclidian space centered in O and n c is the direction cosine of f c .
For each point P of this axis, the value of the torque τ P is equal to (τ c · n c ) n c . We may also interpret the central axis as the set of points where the moment has a minimal norm of value τ c · n c . This trait is due to the orthogonality property of the cross product operator and to the equiprojectivity property of the wrench field.
1) Approximation of the CoM position: As in the case of the ZMP, if we neglect the variation of angular momentum around the center of mass (sayL ≈ 0) and we inject (3) into (6), we obtain: In other words, under this approximation the center of mass c belongs to the central axis W c . We now introduce an other point c p which is the orthogonal projection of c onto the central axis W c . The expression of c p is then given by: The projection c p is nothing more than a good approximation of c as soon as the variations of angular momentum around the center of mass become negligible relatively to τ c .
D. The zero moment point versus the projection on central axis of contact wrench Figure 2 illustrates the difference between the zero-moment point and the central axis of the contact wrench.
We can also mention the following property linking the central axis of the wrench contact to the zero-moment point concept: force vector is equal to n or (ii-b) the contact torque vector is orthogonal to the contact forces, i.e. τ c · n c = 0.
The demonstration is left to Appendix A.

III. OBSERVABILITY OF CENTER OF MASS POSITION
We aim at observing the trajectory of the center of mass online using the available measurements. We consider that the position of the CoM together with its second order derivatives can be set as a dynamical system of the form: where x = c ċ c is the state vector, u ∈ R 3 is the jerk (third time derivative) of the center of mass, and the matrices A and B defined as following: where each 0 and I is 3 × 3 zero and identity matrices respectively.
In this section, we study the observability of the center of mass position given the signals we described earlier. First, we consider the variations of angular momentum around the CoM are negligible. In this context, we show that when we have the force and moment measurements only, the center of mass position is not generally fully observable, but only the components orthogonal to the contact forces vector. We show then that the reconstruction of the CoM does not improve the observability but enables to bound the estimation error along forces vector. We discuss then the conditions and domains of validity of the assumption of negligible variation of angular momentum around CoM, introducing the spectral approach that we propose in the following section.
A. Observability with force/moment signals Eq. (5) provides the expression of force and moment measurements. By considering the variations of the angular momentumL negligible we can rewrite this signal as: We first see that the moments measurement y 2 is nonlinear with regard to the state vector. This is due to the bilinear property of the cross product. It appears clearly that the measurement is invariant for CoM position modifications along the contact force vector f c , i.e. ∀λ ∈ R: This implies that for certain trajectories, for example when f c is constant (u = 0), the state is indistinguishable along one axis, which assesses the non-observability of the full CoM position in that case. Moreover, this non-observability property remains even whenL is non-negligible. Particularly, this situation happens when the polyarticulated system is static with f c = mg.
Of course, this indistinguishability problem does not appear for all possible CoM trajectories. Indeed there exist theoretically inputs u which guarantee the distinguishability of all the state space. However, first, we have no control on the input u which drives the motion we observe. Second, for the majority of humans and robots motions the most important part of contact forces tend to be used to compensate the gravity. This means the forces are mostly vertical during all the time. This leads us to conclude that it is unlikely that any estimation of the altitude c z based on these measurements will reach high precision compared to other components, except for very dynamic motions. This theoretical assertion is validated in Section V.
In order to assess the observability of other axes, let's consider the worst case u = 0 and study it in details using the observability matrix obtained by successive Lie derivatives of h by the vector field generated by matrix A [15]: where [·] × is the skew symmetric matrix operator that enables to perform cross product. The rank of this matrix M is 7 for all states such thatc + g = 0. More importantly, we can see that the components of the CoM position and velocities which lie in the span space of [c + g] × are observable. In other words, the axes of c andċ which are orthogonal to the contact force vector m(c + g) are always observable.
The equalityc + g = 0 corresponds to the case of free falling of the CoM, the force measurement is null and unsurprisingly only the CoM acceleration is observable. This situation happens in particular during jumps and flight phases of running.
We conclude from this observability analysis that CoM estimations based on the force and moment measurements alone may obtain precise results in horizontal position within the limitations of the assumption thatL = 0. Regarding CoM height, the observation is likely to drift from the real value, especially with the double integration of a noisy force signal.
This leads us to introduce the other measurement of the CoM position, which is the geometry-based reconstruction.

B. Geometry-based CoM reconstruction
A polyarticulated system with rigid limbs evolves in the configuration space Q. And the current CoM position depends on the current configuration. In fact, if we have an accurate model of the kinematic tree and mass distribution of the multibody system, the configuration q is sufficient to rebuild the CoM position. In this context, the observability of the CoM position is complete, and the estimation rather easy. This is why the vast majority of robots just use this method not only for reconstruction but also for planning and closed-loop control of CoM trjectories.
However this reconstruction relies entirely on the accuracy of the dynamic model. In particular this means that for humans, it requires either to use anthropomorphic tables with important modeling errors [2] or to estimate inertial parameters using relatively long and tedious identification techniques [16]. Robots also suffer from a drift between the initial model and the actual multibody system due to ageing, maintenance and upgrades which may require also inertial identification [5]. These considerations lead to write this CoM position measurement as: where b ∈ R 3 represents biases due to modeling error. The value of b depends nonlinearly on the joint configuration with an unknown function. So we have no choice but considering that it evolves following its own unknown dynamics. Therefore we have to concatenate the vector b to the state vector x. Nevertheless, most studied motions for robots and humans evolve in a small subset of the configuration space. For example during walking, a human remains upright with legs and arms broadly to the bottom. In this case, we may consider that the bias b is relatively constant. This assumption gives us the new state dynamics:ẋ =Āx +Bu, wherex = x b is the augmented state vector and the matricesĀ andB are matrices of appropriate dimensions defined as following: The first thing we see is that the response of this dynamical system is still invariant for any modifications of the CoM position along f c . Specifically, the vector To see more clearly what modifications to observability this addition provides, let's study the observability matrix for the case u = 0 provided by this model (with removed zero lines): The rank of the matrix is 11 ifc + g = 0 for a 12 dimensional state. Indeed, this new model does obviously not enable the CoM position to be fully observable, but it provides full observability of the velocityċ. This improvement is due to the assumption of a constant b. That means that even if biased, the geometry-based estimation of the CoM remains relatively a reliable measurement for velocity estimations.
Of course another guarantee can be provided if we assume that the bias is bounded b < b max , where . is any real norm and b max is a positive scalar, which implies that we can build an estimation with less than b max error by ignoring the biases.
All this observability study until now does not take into account multiple sources of error. Indeed, the estimation relies also on the actual rigidity of multibody limbs and the precision of the configuration estimation. Indeed, regarding the estimation of the joint angles, if robots have usually precise and reliable joint encoders, no technique is available to obtain such precise joint angles for humans, due to the presence of soft tissues and to the motion capture technique. Furthermore, the sensors themselves may generate errors due to measurement noises and disturbances. Finally, the force and moment measurements were studied with the hypothesis that variations of angular momentum around the CoM are negligible. We see next in which context these assumptions are valid and which part of each signal is the most trustable.

C. Validity of hypotheses, the spectral viewpoint
The variation of angular momentum around the center of massL is due to gesticulation. It is a nonholonomic phenomenon which depends on the configuration and joint velocities and accelerations [8]. In general the motions of humans and robots have relatively lowL compared to the moment of contact forces c × f c alone, especially in the case of locomotion where the center of mass moves away from the origin. However, this gesticulation can be sufficient to deviate the CoM position from the central axis of the contact wrench by up to several centimeters. This imprecision can be tackled in two methods. The first one is to estimateL and subtract it from the contact torque τ c . The only way to do it is by using the dynamic model of the polyarticulated system and applying forward dynamics, which leads to errors due to modeling and double derivation of joint angles. We suggest here to resort to a second easier method that enables to avoid errors related toL. The solution is to only consider the frequency bandwidth where there is few gesticulation and therefore negligibleL: the low frequency range, below the fundamental frequencies of the studied motion.  such as walking, this frequency range contains almost no gesticulation. In the case of non periodic motions, for these frequencies ofL to be significant it likely requires very wide and slow joint trajectories, which is implausible in general. Therefore, the moments signal reduced to this domain provides important low-bias estimations of the CoM position, especially when there are slow and large CoM displacements like for locomotion.
Regarding the forward kinematics (geometry-based) estimation, it is subject to biases which occupy the lowest frequency ranges. These frequencies have to be removed from this signal. Nevertheless we have seen that this measurement may provide reliable estimation of the CoM velocity. Velocities can be seen as amplifications of higher frequencies of CoM trajectory. Therefore there should be a frequency range of the trajectory which can be efficiently reconstructed using this signal.
Finally, the contact forces provide direct measurements of the acceleration of the CoM. A double integration of this signal leads usually to a diverging quadratic drift. This drift lies in low and middle frequencies, but the sensor is much more sound in the very high frequency ranges which are amplified in the accelerations.
Consequently we propose to merge in one signal, the low frequencies of moments, the middle frequencies of forward kinematics and the high frequencies of an acceleration-based CoM reconstruction. Similar reasoning concerning these measurements can be found in [17], [18]. All these considerations are summarized and schematized in Figure 3.
The sensors are often subject to errors partly due to electronic noise and sampling. These errors usually lie in higher frequencies than the desired signals. Classical filtering techniques enable to get rid of the high frequency noises, but if they are applied online they introduce phase shift and delays in the signal. In the next section, we suggest a complementary filtering solution which enables to perform online the desired distribution of the frequency domains on different signals and to avoid high frequency sensor noise without getting theoretically any phase shift.

IV. THE LINEAR COMPLEMENTARY FILTER
The complementary filter [19] is well known in the field of aerial robotics [20], for example to estimate the attitude of a quad-rotor system by combining the gyroscopic and accelerometer measurements. Unlike the Kalman filter [21] 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. All along this section, we exploit the following definition: Definition 2 (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 ∈ C, s being the Laplace variable.
In the following, we gradually design the complementary filters of the CoM position. We designate by s the Laplace variable acting in the frequency domain. The Laplace Transform of a temporal signal g(t), t being the time variable, is written G(s) and sG(s) corresponds to the Laplace Transform of its time derivativeġ(t).

A. The input signals
In Section III-C, we discussed with a spectral viewpoint the validity domain to each input measurement.
We have three different signals conveying information related to the CoM: • (i) The first signal is the geometry-based reconstruction of the CoMc. It suffers mainly from biases due to modeling errors of mass distribution. It is also subject to the 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. • (ii) The second signal is the CoM accelerationc 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.
The third signal provides the data carried by the central axis of the contact wrench. But since the force and moments signals alone don't allow to deduce the CoM position on this line we take the orthogonal projectioñ c p of the geometrical CoMc coming from the first measurement onto the central axis. It contains high frequency sensor noise, but also carries error due to the hypothesis about the weak variation of the angular momentum around the center of mass (eq. 11). This assumption is particularly acceptable in the low frequency domain, specifically below natural locomotion rhythm. The complementary filter diagram related to these measurements is shown in Figure 4.

B. The design of complementary filters
In the previous paragraph, we established that the forces measurement is mainly affected by a low and medium frequencies noise. Therefore, s 2 H 2 1 must be made of a high-pass filter. We can now set: with τ 1 def = 1 2πf1 2 the inverse pulsation and f 1 the cut-off frequency of the high-pass filter. Therefore the transfer function (22) is equivalent to: and H 2 corresponds to second order low-pass filter of cutting frequency f 1 . At this stage, it is worth mentioning that s 2 H 2 must be at least a second order high-pass filter to get the transfer function H 2 stable, i.e. all its poles have a strictly negative real part. Previously, we also established that the third signal is mainly valid in a low frequency domain, forcing H 3 to be a low-pass filter too. The expression of H 3 is then given by: with τ 2 def = 1 2πf2 the inverse pulsation and f 2 the cut-off frequency of the low-pass filter.
Accordingly, H 1 can be directly computed as the complement of both s 2 H 2 and H 3 filters, i.e. H 1 def = 1−s 2 H 2 − H 3 . So H 1 is of the following form: Figure 5 illustrates the bode diagrams of the designed filters H 1 , H 2 and H 3 . We can remark that H 1 acts as a bandpass filter in a bandwidth around [f 2 ; f 1 ]. The bandpass filter characteristics of H 1 may be also deduced from an asymptotic study of the transfer function (25).
Bode diagram of H 1 -A band pass filter.
Bode diagram of H 2 -A low pass filter.
Bode diagram of H 3 -A low pass filter.

V. VALIDATION STUDY
In this section, we apply the complementary filter developed in Section IV 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 performances 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 [6]. Then we generate a whole body trajectory using a second-order generalized inverse kinematics [22]; 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 the second derivative of q, from which we obtain by integrationq 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 φ c of contact forces (5). The linear and angular part of the measurement of φ c are then perturbed by a Gaussian colored noise in the high frequency domain with standard deviation σ linear = 10 N and σ angular = 10 Nm, leading to a noisy measurementsφ c .
The measurement of the configuration vector q is disturbed by another Gaussian colored noise in the high frequency domain too, with a standard deviation σ configuration = 0.05π. 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 measurementc.
Fromφ c combined with the geometry-based CoM and both injected in equation (12), we obtain the perturbed CoM projection onto the central axis of the contact wrenchc p .
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
Before going further and applying the two filtering methods to our simulated motion, we assess our assumptions on the frequency bandwidth where the reliability of each measurement holds. To do so, we study the Fourier Transform of the error between the noisy signals and ground-truth values. Figure 6 shows the Fourier transform of the errors. The simplest spectral distribution is the error of the force measurementc 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 center of mass. 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 projection of the geometry-based CoM estimation onto the noisy central axis of the contact wrench and the real CoM. We see clearly that this measurement is reliable only in a low frequency domain and grows very fast with increasing frequencies. This is why we fixed the cut-off frequency at 0.4 Hz. We see then that these figures confirm clearly the hypotheses of Figure 3.

C. Estimation and comparison with Kalman Filter
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 Figure 7. We see that the tracking in x and y axes is accurate. However, the tracking in z is subject to bias. This is due to the estimation error of the first signal along central axis. On the middle, we see a detailed description of the reconstructed trajectory along x axis where every output signal is displayed separately together with their sum. 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.
In the next section we see how this method behaves against real measurements coming from experimental setting involving human motion.

VI. APPLICATIONS
The method describe in the two former Sections IV and V is directly applicable to robots, as soon as they are equipped with force/torque sensors at contacts. However, the problem of estimating accurately the position of the center of mass for humans represents a more difficult challenge than for robots, because there is no easy access to a fine dynamical model and no precise method to reconstruct joint trajectories. Therefore, in this section, we show two applications of the proposed method on human motions: steady walking and running on a treadmill.

A. Walking
A 26 years old healthy male of 1.80 m height and 64 kg weight was asked to walk on a force platform in the most natural way. The subject was wearing optical markers recorded using VICON motion capture system and following the marker placement suggested by the International Society of Biomechanics [23], [24]. A CoM trajectory was then reconstructed using an anthropometric table providing inertial parameters [25]. The central axis was reconstructed using force and moment measurement and our sensor data fusion technique was applied. The results are displayed on Figure 8.
We see that the estimation of the CoM position provided by our method is slightly different from the trajectory obtained by the geometry, especially in horizontal position. Since we have no ground truth value, we cannot show that our estimation is more accurate, but this difference could be a correction of biases due to errors of the anthropometric table, similarly to what happens for our simulated model of the previous section.

B. Running on a treadmill
A healthy male of 1.72 m height and 71 kg weight was asked to run on a treadmill at a constant speed of 16 km/h (about 4.4 m/s). The treadmill was located on a force platform and the subject was wearing also optical markers for VICON motion capture system, using the same marker placement as in Section VI-A. The experimental setting is described in [26]. The geometry-based CoM trajectory was generated using the same anthropometric table. The same reconstruction process was executed on the recorded signals. Figure 9 illustrates the results. The motion is much more dynamic than walking, as we see in the force measurement on the bottom. Here, a special care has to be considered for the flight phases. Since the central axis is not defined in this case, the projectionc p of the geometry-based reconstructioñ c was set toc p =c itself when contact force norm is below a threshold of 100 N. This does not jeopardize our method since only the frequencies under 0.4 Hz were considered for this signal. We see that there is a difference of few centimeters for each dimension between the geometric reconstruction and the estimation of our sensor data fusion. Similarly to the case of walking, there is no ground truth value for the center of mass. However, since the difference converges after 1.5 s to a value and seems stable for several seconds after, our explanation is that our method succeeded to correct a bias due to anthropometric table.
Of course, a difference in the estimation could also arise when the force and moments sensors are themselves biased. But this magnitude of discrepancy requires an important bias, and usually these high-end force platforms are reliable and their calibration is a relatively easy process.
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.

VII. RELATED WORK
Our CoM estimation approach is part of an active topic both in research on human motion and in humanoid robotics [27]. For humanoids, the corrections on the CoM provided by forward kinematics is achieved mainly using various measurement systems [28] including force sensors [29], [30]. 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 [31]. Moreover, since few decades, force platforms were already considered for CoM position estimation [32], but most of the methods did not consider the fusion of Force sensors with direct kinematics reconstruction of the CoM [33], [34].
To our best knowledge, the closest published work to our method is the technique by Maus et al [18]. The Kinematic CoM estimation was low-pass filtered and the double time-integral of forces was high-pass filtered, then the two signals were added. 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. In another work, Schepers et al [17] 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 accelerations when using the ZMP, which increases again the approximation errors.

VIII. CONCLUSION
We have seen through this paper the analysis and comparison of the observability provided by all sensing devices to reconstruct the center of mass trajectory for humans and for robots. These sensors can be classified by three categories: the CoM reconstruction provided by the geometrical reconstruction together with a model of mass distribution, the forces that give CoM accelerations, and the moments which provide an approximation of the CoM position. We have established the conditions wherein we can trust every signal the most. One key idea is to consider that these measurements carry noises and errors, but with separated but complementary frequency bandwidths for each signal.
Afterwards, we have shown the design and the implementation of an estimator of the the CoM position for humans and robots based on multi-sensor data fusion. Our choice was to use a complementary filtering technique to merge these signals, specifically because of its particular suitability to merge different bandwidths of signals.
We have seen also the simulation results showing that the complementary filtering successfully got rid of estimation error by removing their appropriate frequency bandwidths, whereas Kalman filtering technique could not reject fully these errors.
It is worth to note that this method is not reduced only to the case of walking motions. The considerations that are the Fig. 8: CoM position reconstruction for natural walking (blue for x, green for y and red for z). On the left, the reconstructed CoM in plain line and the CoM coming from geometry in dotted line. On the right the force measurement. Fig. 9: CoM reconstruction for running on a treadmill (blue for x, green for y and red for z). On the left, the reconstructed CoM in plain line and the CoM coming from geometry in dotted line. On the right, one second of force measurement basis of our approach are valid for any kind of trajectory, even for non-planar contacts, as soon as we have all the required measurements. The only detail that has to be taken into account and possibly modified is the frequency range of the error of each signal.
Finally, one limitation to our approach is to neglect the variations of angular momentum around the center of mass. These variations depend on the gesticulation of the system and they introduce errors in the estimation provided by sensors of contact force and moment. We believe that the precision of our method would be be improved if this parameter is explicitly taken into consideration. Obviously, these approaches may complexify dramatically the observation process, but they still should be relevant and interesting to study.