Efficient Observer Design for Ambulatory Estimation of Body Centre of Mass Position

Complementary Linear Filter (CLF) is a common techinque employed for estimating the ground projection of body Centre of Mass starting from ground reaction forces. This method fuses centre of pressure position and double integration of horizontal forces, selecting best cut-off frequencies for low-pass and high-pass filters. Classical Kalman filter is a substantially equivalent approach, as both methods rely on an overall quantification of error/noise and don’t analyze its origin and time-dependence. In order to overcome such limitations, a Time-Varying Kalman Filter (TVKF) is proposed in this paper: the effect of unknown variables is directly taken into account by employing a statistical description which is obtained from experimental data. To this end, in this paper we have employed a dataset of 8 walking healthy subjects: beside supplying gait cycles at different speeds, it deals with subjects in age of development and provides a wide range of body sizes, allowing therefore to assess the observers’ behaviour under different conditions. The comparison carried out between CLF and TVKF appears to highlight several advantages of the latter method in terms of better average performance and smaller variability. Results presented in this paper suggest that a strategy which incorporates a statistical description of unknown variables and a time-varying structure can yield a more reliable observer. The demonstrated methodology sets a tool that can undergo a broader investigation to be carried out including more subjects and different walking styles.


Efficient Observer Design for Ambulatory Estimation of Body Centre of Mass Position
Marco Maddalena , Member, IEEE, and Mozafar Saadat , Senior Member, IEEE Abstract-Complementary Linear Filter (CLF) is a common techinque employed for estimating the ground projection of body Centre of Mass starting from ground reaction forces. This method fuses centre of pressure position and double integration of horizontal forces, selecting best cut-off frequencies for low-pass and high-pass filters. Classical Kalman filter is a substantially equivalent approach, as both methods rely on an overall quantification of error/noise and don't analyze its origin and time-dependence. In order to overcome such limitations, a Time-Varying Kalman Filter (TVKF) is proposed in this paper: the effect of unknown variables is directly taken into account by employing a statistical description which is obtained from experimental data. To this end, in this paper we have employed a dataset of 8 walking healthy subjects: beside supplying gait cycles at different speeds, it deals with subjects in age of development and provides a wide range of body sizes, allowing therefore to assess the observers' behaviour under different conditions. The comparison carried out between CLF and TVKF appears to highlight several advantages of the latter method in terms of better average performance and smaller variability. Results presented in this paper suggest that a strategy which incorporates a statistical description of unknown variables and a time-varying structure can yield a more reliable observer. The demonstrated methodology sets a tool that can undergo a broader investigation to be carried out including more subjects and different walking styles.
Index Terms-Locomotion, center of mass, ground projection, observer, estimation, ground reaction forces, complementary linear filter, time-varying Kalman filter.

I. INTRODUCTION
T HE evolution of body Centre of Mass (CoM) position throughout locomotion is a major determinant factor to understand and differentiate human gait [1], [2].
Application of markers on individual's body and camera acquisition allow to estimate the position of body segments [3], [4], while inertial parameters are employed to calculate CoM position for any segment and consequently of the whole body. This Geometrical Method (GM) has several drawbacks, in addition to equipment cost: its accuracy relies on kinematic and inertial parameters precision and particular care is required in the trial preparation, which in turn translates to considerable demand in terms of time and skill. During experimental trials for human biomechanical analysis, Ground Reaction Forces (GRF) are often acquired. The force measuring interface is commonly floor-installed [5], [6], yet, in order to allow multiple strides GRF acquisition, it can be alternatively installed either on a treadmill [7], [8] or inside instrumented shoes [9], [10], [11]. Double integration (DI) of GRF allows to straightforwardly calculate the displacement of CoM through a single gait cycle with an accuracy comparable to GM [7], [12], [13]; nonetheless, employing DI for calculating absolute position of CoM would be troublesome, as the estimation is heavily influenced by drift due to initial state error and sensor noise.
This limitation can be mitigated by fusing the outcome of DI with the position of the equivalent GRF application point, called Centre of Pressure (CoP): while DI can provide instantaneous rate of change of CoM evolution, CoP supplies a reference to prevent drift phenomenon [9], [11], [14]. Clearly, this strategy allows to enhance the estimation of CoM position only with regards to its horizontal coordinates, which correspond to its projection on the ground (CoMG). Such information, although incomplete, holds considerable interest in the biomechanical analysis field, in particular it allows the study of balance and dynamic stability during locomotion through the concepts of extrapolated centre of mass position and margin of stability [11], [14], [15], [16], with particular application to neurological disorders [10], [17], [18].
DI and CoP fusion is usually carried out by employing a Complementary Linear Filter (CLF): not considering any statistical description for the noise corrupting the signals, the filter is obtained by a simple analysis in the frequency domain [19]. This technique foresees that a low-pass filter is used to process CoP position while DI outcome is high-pass filtered in order to avoid drift. Complementarity is given as the two filters have the same cut-off frequency.
Kalman filter (KF) [20] can be used as an alternative to CLF: errors introduced by DI and CoP are seen as model and measurement additive noises respectively, characterized by a particular variance. This approach results in a stationary (timeinvariant) filter which balances model-and measurement-based estimations according to different noise variances. It's been proven that CLF and stationary KF are substantially equivalent methods [19], assuming that CLF time constant (corresponding to the chosen cut-off frequency) is set equal to the ratio of standard deviations of measurement and model noises in KF. A legged robot CoM estimation case [21], [22] confirmed that stationary KF is not better than CLF.
Both methods rely on an overall quantification of error/noise and don't analyze its origin and time-dependence. In particular, the goodness of the incorporation of CoP into the estimation is contingent on the hypothesis that angular momentum around CoM is negligible inside the relevant frequency span. Although angular momentum is highly regulated throughout the walking cycle to minimize energy expenditure, substantial segmental momenta do take place (gesticulation), indicating large segment-to-segment cancellations which can cause an important instantaneous variation of whole-body angular momentum [23]. Moreover, the influence of this quantity on estimation is not constant during locomotion, making stationary filters not particularly suitable.
Motivated by these observations, this paper presents a comparison between classical CLF-based observer for CoMG and a novel Time-Varying Kalman Filter (TVKF) which incorporates statistical knowledge of body angular momentum variation in order to refine the estimation.

A. GRF/CoM Correlation Throughout Locomotion
The combination of forces and moments acting on a human body of mass m during locomotion is summarized in figure 1: force f i and moment τ i operate on a generic body-ground contact point p i , while p CoM indicates the 3d-position of CoM. X-axis is taken in the walking direction, y-axis is transverse while z-axis is vertical. Dynamics yield the equations for the time derivative of linear and angular momentum by summing the effects of forces and moments on all the contact points, in addition to the gravity acceleration g: A generic Zero Moment Point (ZMP) p Z M P is defined in such a way that the sum of the moments generated by ground/body contact calculated with respect to that point is null, therefore: The points which satisfy such definition belong to a straight line in the space (ZMP axis), yet, if ZMP is selected to lie on the ground plane, it can be proven that it corresponds to the CoP position p CoP [24]. Hence, all the points belonging to the ZMP axis can be written as p CoP + a i f i for a ∈ R.
Combining equations 1b and 2, the derivative of the angular momentum turns out to be: which corresponds to the net moment about the body's CoM. Developing the cross product limited to xand ycomponents, the following holds: Finally, these equations can be re-arranged in order to express the relationship between CoMG and the other elements taken into consideration: Force plates measure GRF with good precision, therefore i f i and p CoP can be considered as known. Instead, the lack of knowledge of the vertical position of CoM and the derivative of angular momentum due to gesticulation is detrimental to the quantification of CoMG. The variables p z CoM , L x andL y fluctuate during gait and are periodic with a period corresponding to the stride span, while variation of force sum change the impact of these variables on the difference between CoMG and CoP. In the following sections, two kinds of observers are presented, aiming at estimating p x CoM and p y CoM from i f i and p CoP while attenuating the effects of unknown variables.

B. Complementary Linear Filter (CLF)
The following development deals with the estimation of x-component of CoM, while the corresponding value for y-component can be found in an equivalent way.
Analysing the two elements of CLF separately, CoP and DI methods for CoM position estimation raise the errors ε x CoP and ε x D I , respectively: X-component of equation 5 shows that ε x CoP is composed of signals mostly periodical with periods multiple of gait cycle. Therefore, CoP should be processed by a low-pass filter which eliminates the error exceeding a suitable cutoff frequency ω c,x , smaller than step frequency. As proposed by Carpentier et al. [21], a double pole is taken in order to increase the rate of roll-off of the filter: where time constant τ c,x = 1/ω c,x . As for DI, a high-pass filter can eliminate drift which is error at zero frequency. This filter is taken as complementary to the low-pass filter, so as to have the same cut-off frequency: Therefore, in the s-domain of Laplace transform, the filter can be written as: Cut-off frequency should be chosen in such a way to minimize estimation error. Filters allow useful signal passing but also error signal in their respective frequency range. Approximating the two filters gain beyond cut-off frequency to zero, we have the following cumulative error power spectral distributions: where E x CoP ( jω) and E x D I ( jω) are Fourier transforms of ε x CoP and ε x D I , respectively. Finally, ω c,x can be taken in order to minimize the total error:

C. Time-Varying Kalman Filter (TVKF)
Re-writing x-component of equation 5 in a state-space form, we have: where the state, the output and the input are defined as: and the matrices are: The additive signalũ is the input uncertainty due to force measure noise, which can be seen as the main contributor to DI drift. It is assumed to be a white Gaussian process with variance σ 2 u . The additive signalṽ, instead, is the output uncertainty:ṽ The unknown p z CoM andL y are assumed to be white Gaussian noises as well, so that p z CoM ∼ N (µ z , σ 2 z ) anḋ L y ∼ N (0, σ 2 L ,y ). This can be justified as CoM vertical position oscillates throughout the gait cycle around an average value, while time integral ofL over a gait cycle is zero as the angular momentum goes back to its initial value. Moreover, it is assumed, by simplicity, that p z CoM andL y are mutually independent. With this definition,ṽ turns out to be a noise characterized by time-varying stochastic properties.
Hereafter the TVKF development proceeds as a discretetime system. We assume sample time t is sufficiently small so that, at k- The discretized version of the system in equation 12 is therefore: where the matrices are defined as: Noise w k accounts for the input uncertainties and its covariance matrix is equal to Q = σ 2 u B d B T d . Re-arranging the measurement noise of the system in equation 12, the noise signal can be described as v k ∼ N (0, r x k ), whose variance is given by: The variablev k , instead, is at any instant a determined value The matrices relating to the TVKF can be subsequently calculated [20], starting from a priori error covariance, Kalman filter gain and update of the a posteriori error covariance matrix: so that the state observer can be written as: The quantity y k −v k is the x-axis projection of a particular point p * Z M P belonging to ZMP axis whose z-component is µ z (see figure 2): Such projection p * x Z M P can be seen as a first attempt at estimating p x CoM by refining the rough approximation made by using simply p x CoP . As a matter of fact, if p z CoM anḋ L y were known with probability 1, they would coincide with their averages µ z and 0 respectively, and equation 5 would yield p * x Z M P = p x CoM . Since the variances σ 2 z and σ 2 L ,y are non-zero, an uncertainty around p * x Z M P is introduced which is described by value r x k . As dynamic model uncertainty is given by matrix Q, TVKF gain K k provides for fusing the two separate estimations according to the different uncertainty levels.
Two different situations are noteworthy. When | i f x i | is maximal, usually at early and late stance phase of gait cycle, distance between p * x Z M P and p x CoP is also maximal: this means that p x CoP would be a rather imprecise estimation by itself. At the same time, CoM vertical position variance σ 2 z has highest influence on r x k . Instead, when i f x i ≈ 0, usually at mid-stance phase of gait cycle, p * x Z M P and p x CoP coincide and r x k is influenced by σ 2 L ,y only. TVKF development and observations are totally analogous when considering y-component, where for calculating gain we have specific matrix Q, while equation 18 becomes:

D. Experimental Data and Observer Simulation
Real gait data is needed to find proper values for the observer parameters and to provide a comparison in terms of performance, and it would be ideal to make use of data from several subjects walking at different speeds in order to create a complete analysis.
In this paper we have employed a dataset of walking healthy subjects made available at [25] (see table I for sub-TABLE I  SUBJECTS DESCRIPTION jects' data, [26] and [27] for details about methodology): the participants were given general instructions to walk straight at different speeds during a single testing session. Motion data was collected using a 12-camera Vicon MX system (Vicon, Oxford, UK) operating at 120 Hz, while GRF were recorded using four force plates (AMTI, Watertown, MA), sampled at 1080 Hz. Beside supplying gait cycles at different speeds, the dataset deals with subjects in age of development and provides a wide range of body sizes, allowing therefore to assess the observers' behaviour under different conditions. We have extracted the evolution of GRF (force sum and CoP) and CoM along a single gait cycle for all the 8 subjects at "slow" and "free" speeds and we have utilized MATLAB software (r2021a, the MathWorks Inc., Natick, MA, USA) for processing the data, identifying the filter parameters and calculating the estimation output.
1) Observer Parameters: For the CLF parameters, errors ε x CoP and ε x D I are sampled using the extracted data across all subjects and speeds, and their spectral distributions E x CoP ( jω) and E x D I ( jω) are found through Fast Fourier Transform. Afterwards, the value of ω c,x is chosen to minimize the sum of cumulative error spectral powers J x ε,CoP (ω c,x ) and J x ε,D I (ω c,x ). The procedure is repeated for y-component and ω c,y is obtained. For TVKF, the same data is employed to find the stochastic description of force x-component measurement noise in equation 16. Dynamic model error, sampled as w k = x k − A d x k−1 − B d u k−1 , is used together with the meanw to obtain the sample covariance matrix: Covariance matrix pertaining to force y-component measurement noise is found in the same way. For any subject/speed configuration the following sample mean and variances are found separately: (24) where M x and M y are x-and y-components of right side of equation 3 (net moment of contact forces around CoM). As these values vary according to subject and speed, it is assumed that they roughly depend on body size, in particular a first-order polynomial approximation is proposed: where L leg is the subject's leg length, while a i and b i leastsquare fitting from sample values in equation 24.
2) Observer Simulation and Performance: For any subject and speed taken in consideration, the gait cycle data is repeated 20 times to provide force sum, CoP and CoM evolutions which are employed as input and ground truth for the observers. For performance comparison, the difference between the estimation and the real value of CoMG is taken into consideration at the end of the transient phase. Therefore, for both observers the root mean square error (RMSE) is calculated in the N instants of the interval I corresponding to the 10-th gait cycle. For x-component we have: and analogously for y-component (RMSEy).

A. Observer Parameters Identification
Error frequency-domain analysis for CLF parameters identification is displayed in figure 3. Error power spectral density highlights larger low-frequency error for DI-based estimation and larger high-frequency error for CoP-based estimation in both directions, as expected. In particular, CoP-based estimation shows two peaks at frequencies corresponding to "slow" and "free" speed (figures 3A and 3C): for x-component the peaks take place at step frequency (half gait cycle period) and for y-component at stride frequency (whole gait cycle period). The sum of error power spectral distributions shows an area where the minimum value can be picked, between low frequencies and step/stride frequency (figures 3B and 3D). Cut-off frequency ω x c is taken equal to 4 [rad/s] while ω y c is chosen as 3 [rad/s]. Figure 4 shows sample values and linear fitting for identification of TVKF parameters. It can be observed that fitted line predicts mean CoM vertical position rather precisely, while in the other three cases the linear approximation is less definite. Even for the same subject the two sample values can differ considerably when they are acquired at different speeds. In three out of four cases the fitted lines display a positive slope, while standard deviation of CoM vertical position σ z decreases with square root of leg length. This might seem counter-intuitive, as one could expect the oscillation of CoM to grow, just like the average value does. Yet, as trials have pointed out [28], displacement of CoM throughout locomotion tends to decrease with age due to neural maturation, which means that, when dealing with subjects in the age of development, it is not surprising that a taller individual, which happens to be also older, could show a smaller vertical oscillation.

B. Comparison of Observers
Performance of CLF-and TVKF-based observers in terms of RMSE is reported in table II with quartiles and significance p-value of inter-observer comparison.
Results are presented also graphically in figure 5, where box and whisker plot is added in order to show quartile distribution of the two methods. Values highlight that TVKF has lower average RMSE in both components, with a more remarkable difference for x-component. MATLAB software has been employed to perform a paired t-test to evaluate significance of RMSE comparison, resulting in p-values p < 0.01 for x-component and p < 0.001 for y-component. Moreover, CLF features a significantly higher variability, as in several cases it performs just as good as TVKF, while it may show large RMSE up to 5 cm. It is noteworthy that CLF outcome for the same subject may differ depending on the speed; also, for the same subject/speed configuration x-and y-component can show significantly different effectiveness.
This inconstancy in performance is detailed in figures 6 and 7 which provide graphical rendering of data pertaining to subject 1 walking at slow and free speed. They show the evolution through the 10-th gait cycle with regards to x-and y-component of CoM, CoP and estimations (A and B). Also estimation errors in the first 10 seconds are shown (C and D). At slow speed x-component displays a considerable difference between the two observers, while along y-component they are almost identical. At free speed, conversely, x-component behaviour features a strong similarity between the two methods

IV. DISCUSSION
The comparison carried out between CLF and TVKF appears to highlight several advantages of the latter method in terms of better average performance and smaller variability. Statistical significance of the comparison confirms that TVKF can be considered a more reliable tool.
CLF can achieve satisfactory estimation error but seems susceptible to walking pattern alteration. This might be partially due to neglecting the effect of variationL of angular momentum around the center of mass: in fact, the method relies onL being sufficiently small, or anyway its impact is expected to be described by the error frequency analysis and therefore attenuated through filtering. However, results suggest that even the choice of suitable cut-off frequency is not sufficient to ensure a good performance in any presented condition.
In the development of TVKF, instead, the effect ofL is directly taken into account by employing a statistical description which is obtained from experimental data. On-line measurement of GRF is exploited to update gain matrix at any instant, therefore making the observer fit to the particular conditions of any phase of gait cycle. This method relies on several assumed simplifications on random variables descrip-tion, however these don't seem to prevent the observer from estimating CoMG with bounded RMSE in any analyzed case.
The developed strategy allows TVKF to overcome the limitations featured by "classical" KF which foresees a timeinvariant structure. For example, KF developed in [22] is based on a discrete-time dynamical model similar to the one expressed by equation 16; however, measurement noise variances are assumed as constant over time and are taken in a rather arbitrary fashion, without incorporation of experimental data, resulting in performance equal to or worse than CLF.
When analyzing the CLF versus TVKF comparison results presented in this paper, however, it should be acknowledged that the range of involved subjects and speeds is rather limited. The comparison could be extended to a larger sample of configurations and walking patterns in order to verify TVKF performance and weigh it against CLF method. Moreover, the additional amount of data would allow to better understand the relations between subject's and TVKF parameters: for example, age could be included as an independent variable to build a more reliable fitting for CoM z-component standard deviation. Generally, a more detailed work could be carried out to achieve a more precise regression of the parameters starting from basic subject's features.
V. CONCLUSION CLF is commonly considered a valid tool for estimating ground projection of CoM; nonetheless, neglected gait dynamics-related variables can have a significant impact on estimation error. Results presented in this paper suggest that a strategy which incorporates a statistical description of such variables and a time-varying structure can yield a more reliable observer. Indeed, experimental data has been employed to identify TVKF parameters and to compare the performance. The demonstrated methodology sets a tool that can undergo a broader investigation to be carried out including more subjects and different walking styles.