{'fraction_non_alphanumeric': 0.03549462416075872, 'fraction_numerical': 0.008685853795952861, 'mean_word_length': 4.667908461947845, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 0, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 6, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
On the Use of Torque Measurement in Centroidal State Estimation

State of the art legged robots are either capable of measuring torque at the output of their drive systems, or have transparent drive systems which enable the computation of joint torques from motor currents. In either case, this sensor modality is seldom used in state estimation. In this paper, we propose to use joint torque measurements to estimate the centroidal states of legged robots. To do so, we project the whole-body dynamics of a legged robot into the nullspace of the contact constraints, allowing expression of the dynamics independent of the contact forces. Using the constrained dynamics and the centroidal momentum matrix, we are able to directly relate joint torques and centroidal states dynamics. Using the resulting model as the process model of an Extended Kalman Filter (EKF), we fuse the torque measurement in the centroidal state estimation problem. Through real-world experiments on a quadruped robot with different gaits, we demonstrate that the estimated centroidal states from our torque-based EKF drastically improve the recovery of these quantities compared to direct computation.


I. INTRODUCTION
State estimation for legged robots plays a crucial role in the successful application of modern state feedback controllers to this domain. The estimation problem is especially difficult for legged robots, as they are inherently underactuated and experience uncertain, intermittent contacts with the environment during motion. Furthermore, their dynamics are highly nonlinear, which makes the design of estimators nontrivial.
The majority of legged robot estimation works to date have focused on base state estimation. This problem is particularly difficult for floating base systems, as the base pose cannot be measured directly. One of the most widely used choices for base state estimation is the Extended Kalman Filter (EKF) [1], [2], [3]. While there have been recent attempts to use more advanced approaches based on the use of factor graphs [4] and invariant Kalman Filters [5], [6], the EKF framework is commonly used due to its compromise between simplicity, efficiency, and performance [7], [8].
For base state estimation, most works have fused Inertial Measurement Unit (IMU) data with leg odometry to estimate the base position and velocity as well as its orientation [1], [2]. In order to compensate for the drift of the unobservable base position while simultaneously mapping the environment, sensor modalities like LiDAR and cameras are often added [3], [7], [9].
Force measurement at the endeffectors of legged robots is another useful source of information that can be used to better estimate [10], [11] and handle contact switch events [12], [13] as well as aid in estimation of the centroidal states [14], [4]. However, since it is highly important for legged robots to have low leg inertia for agile movements, using sensorized feet [13] can drastically degrade the range of dynamic movements they can perform, especially for quadrupeds. Thus, most of works in this area first estimate the contact forces without direct sensing, and then use the estimated forces for state estimation [12], [4].
While legged robots often lack endeffector force sensing, they are commonly equipped with joint torque sensors. In this work, we propose to fuse joint torque measurements with the nonlinear dynamic model in the EKF framework to estimate the centroidal states -namely, the center of mass position and momenta -of a legged robot. We argue that since most state-of-the-art quadrupedal [15], [16], [17] and bipedal [18], [19], [20] platforms have the capability to measure or estimate joint torque, this sensing modality can be used to perform centroidal state estimation regardless of contact force measurement capabilities. Model-based centroidal state computations are usually very noisy due to noise in measured joint velocities, and low-pass filtering can introduce delays on the order of tens of milliseconds; such large delays can destabilize high rate controllers [14]. By using joint torque measurements for centroidal estimation, we can avoid this issue without necessitating contact force sensing.

A. Contributions
The main contributions of this work are as follows: • We propose an EKF-based framework which fuses the whole body dynamics of a legged robot projected into the nullspace of the contact constraints with joint torque measurements in order to perform centroidal state estimation. To the best of our knowledge, this is the first time that this sensing modality is used to estimate the centroidal states of a legged robot. Notably, the proposed framework is general enough to be used on any legged robot, not just a quadruped. • We demonstrate through an extensive set of experiments with different gaits on the real quadruped robot Solo12 [17], that the use of torque measurements provides accurate centroidal state estimation with minimal noise and time delay.

B. Notation
• The ⊕ operator denotes the proper vector composition of SE (3) × R n . • ∇(.) is the derivative operator with respect to (w.r.t.) corresponding variable, and I is the identity matrix with the proper dimension. • Throughout the paper, we use small letters to specify scalars and scalar-valued functions, bold small letters for vectors and vector-valued functions, and capital letters for matrices.
II. FUNDAMENTALS The dynamics of a floating-base system can be written as where M ∈ R (n+6)×(n+6) is the mass-inertia matrix, q ∈ SE (3) × R n denotes the configuration space, v ∈ R n+6 encodes the vector of generalized velocities (or more precisely quasi-velocities), and n ∈ R n+6 is a concatenation of nonlinear terms including centrifugal, Coriolis and gravitational effects. B ∈ R (n+6)×n is a selection matrix that separates the actuated and unactuated Degrees of Freedom (DoFs), τ ∈ R n is the vector of actuated joint torques, J c ∈ R 3m×(n+6) is the Jacobian of m feet in contact, and finally λ ∈ R 3m is the vector of contact forces (here we assume point-contact feet for quadrupeds, however all results hold for humanoids with flat feet as well).

A. Motion and Constraint Dynamics
Assuming rigid non-slipping contact with the environment, and using the orthogonal projection operator N = I − J † c J c (where † is the Moore-Penrose inverse) which projects into the nullspace of the contact Jacobian, we can divide (1) into the following set of equations [21] N M (q)v + N n(q, v) = N Bτ , (2a) Equation (2a) encodes the dynamics of the robot motion independent of the constraint forces, while (2b) yields the dynamics of the system in the contact constraint space [21]. In other words, N Bτ is used to move the system on a desired trajectory without violating the contact constraint, while (I − N )Bτ preserves the stationary contact. Interestingly, since (2a) is independent of contact forces, we can use it to relate the measured joint torques to the joint accelerations without knowledge of the contact forces. This is especially useful for agile legged robots whose endeffectors are often not equipped with contact force/torque sensors.
Since the feet in contact are assumed stationary, we can write down the following constraint and differentiate both sides w.r.t. time, yielding Defining the constraint-consistent mass-inertia matrix as M c = N M + I − N and substituting it along with (3) into (2a), we have Now, we can invert M c and solve the constraint-consistent equations of motion forv to yielḋ

B. Relation between Joint Torques and Centroidal States
Denoting the centroidal momentum vector by h G ∈ R 6 , we can write down its relation with generalized velocities using where A G (q) is the Centroidal Momentum Matrix [22]. Taking the derivative of (6) w.r.t. time yieldṡ Substituting (5) into (7), we havė Assuming that q, v can be measured using joint encoders, and the base state is estimated separately using a base estimator, we can write down the momentum dynamics in the following compact form: where We aim to estimate the centroidal states x ∈ R 9 = c, l, k using joint torque measurements and the above centroidal momentum dynamics. The process model iṡ whereτ is the measured joint torques, m is the total mass of the robot, c ∈ R 3 is the center of mass (CoM) position, and l ∈ R 3 and k ∈ R 3 are the linear and angular components of centroidal momentum respectively. These values are computed using the results of base state estimation, measured generalized joint states, kinematics, and inertial properties of the robot. Specifically, we use (6) to compute the momenta while the CoM is computed using the estimated base state and joint measurements as The measurement model is As shown in [14], although the centroidal states can be computed directly, they are subject to considerable noise and modeling errors. By fusing the computed states with a torque-based process model, we similarly obtain low-noise estimates with minimal delay which is suitable for highbandwidth control. Remark 1: It is important to note that the right hand side of both the process and measurements models (6), (11), (12) are implicit functions of c , l, and k. Hence, we need to compute their Jacobians to be used inside the EKF formulation. We show in Sec. III-C how these Jacobians can be computed.
Remark 2: For both base and centroidal state estimations, a contact detection mechanism for each foot is employed. One might ask since we estimate the contact forces for contact detection, then why not use a centroidal state estimator based on estimated contact forces (for instance as in [14]) which would render the process model much simpler compared to (11)? The answer to this question is threefold: 1) contact detection is normally performed by setting a threshold only on the estimated normal force separately for each leg neglecting the dynamics. This force measurement is not accurate enough to be used in the centroidal state estimation problem. 2) Instead of using the whole-body dynamics to estimate the contact forces and then use it to estimate the centroidal states, our approach estimates those quantities directly from torque measurements. 3) Many legged robots are endowed with binary contact sensors at their endeffectors [17], [18] that can directly detect contact events, hence there is no need to estimate endeffector forces.

A. Extended Kalman Filtering
We choose to implement our estimator as an Extended Kalman Filter (EKF). The EKF estimates the mean µ and the error covariance matrix P over the state x k , where both process and measurement models are corrupted by additive zero-mean Gaussian noise. The discrete-time stochastic system evolves as: where the input u k = τ k is the vector of joint torques and z k is the measurement. The process noise w k ∼ N (0, Q k ) and measurement noise v k ∼ N (0, R k ) are parameterized by the corresponding noise covariance matrices Q k and R k . These matrices constitute the main tuning parameters of the filter. Note that the process noise is chosen to be purely additive (rather than based on the uncertainty in torque sensing) for the sake of simplicity in filter implementation. In other words, we represent the effects of joint torque measurements and dynamic model errors as additive noise in the process and measurement models respectively. In the prediction step, having determined the set of feet in contact using estimated contact forces, the mean is propagated using the discrete-time nonlinear process model, and the process model Jacobian F k = ∂f (x) ∂x x=µ − k is used to propagate the error covariance matrix as where the minus superscript denotes the a priori (before measurement update) estimate, and the plus superscript denotes the a posteriori (after measurement update).
In the update step, the measurements z k are integrated into the EKF using the a priori estimates and the measurement noise covariance as follows: where K k is the Kalman gain, and H k = ∂h(x) ∂x x=µ − k is the measurement Jacobian matrix.

B. Discrete-time Nonlinear Process Model
In the prediction step, the mean of the state is propagated using the discretized nonlinear process model. We discretize (11) using a first-order explicit Euler integration as follows: where ∆t is the discretization time step, andτ are the measured joint torques.

C. Discrete-time Linearized Process Model
In order to propagate the state covariance in the prediction step (15b), we first linearize then discretize the dynamics at the filtering frequency. Since the nonlinear model of the momentum states [l, k] is expressed implicitly as a function of the generalized vector positions and velocities, we resort to finite differencing for the computation of the continuoustime process model Jacobian F c in (9).
To compute the partial derivative ofḣ G w.r.t. c x , we first find the deviation of δq corresponding to the variation of CoM position in the x-direction. By considering the first three rows (linear momentum part) of A G (q) as A G (q), we can write down the variation of the CoM position as a function of variation of generalized coordinates as Finding the variation of q only in the x-direction of the CoM position simply becomes Using q + = q ⊕ δq, we can find the part of the Jacobian matrix corresponding to c x as where ∆c x ∈ R is the x component of (20b). Following the same procedure above, we compute the derivative w.r.t. c y and c z by considering small variation as [0, δc y , 0] and [0, 0, δc z ] respectively. Similarly, to find the derivative ofḣ G w.r.t. linear momentum l x , we compute the deviation of δv corresponding to the variation of linear momentum only in the x-direction. To do so, we rewrite (7) as By considering A G (q) constant for small variations (i.e. δA G (q) = 0), we can find a small variation of v corresponding to the small variation of linear momentum in the x-direction being Given v + = v + δv, we compute the part of Jacobian matrix corresponding to l x as where ∆l x ∈ R is the x component of (23b). Following the same procedure above, we compute the rest of the remaining partial derivatives of the linear and angular momenta w.r.t. l y , l z , k x , k y , and k z . Finally, we assemble the continuoustime process model Jacobian matrix F c ∈ R 9×9 as where Ω In the discrete-time form, the Jacobian is truncated at its first-order Taylor approximation for simplicity yielding F k ≈ I 9×9 +F c ∆t, and the discretized process noise covariance matrix is likewise approximated as Q k ≈ F k Q c F T k ∆t. Similarly, we can find the measurement Jacobian in the update step, by taking corresponding derivatives w.r.t. our state. By following the procedure described for computing prediction Jacobian, and defining proper variations for q, and v we get H k = I 9×9 , which is consistent with the result of [14]. We highlight that the measurement Jacobian is the identity matrix since the state is directly measured. This also validates the finite difference method proposed in this work for computing the process and measurement Jacobians.

D. Observability Analysis
The process model of the proposed estimator is a highlynonlinear function of the states; in general, we may investigate the observability of a nonlinear estimator by forming the nonlinear observability matrix [23]. Given a system with nonlinear process modelẋ = f (x) and measurement model y = h(x), the nonlinear observability matrix is . . . The state of the estimator is observable if O has full rank. In our case, the process model cannot be expressed explicitly in terms of the states, preventing us from computing Jacobians of f (x). However, because we directly measure the full state, we have ∇h(x) = H k = I 9×9 . This already means that O has full column rank, so the state is observable. This is in agreement with the results of [14], where O is computed for a similar momentum estimator. The difference in this case is that higher-order terms would depend not on measured contact wrenches, but instead on properties of the constrained dynamics model and on measured generalized positions and velocities. As in those results, since we directly measure the state, higher-order terms do not affect observability (O cannot lose rank).

IV. RESULTS
In this section, we present the experimental results of performing estimation of the centroidal states both with and without the use of torque measurements, for a variety of different gaits on the quadruped robot Solo12 [17]. Specifically, we consider three different behaviors: moving the base while balancing, trotting, and jumping. The trajectories are generated using the trajectory optimization framework in [24], and the wholebody controller in [17] is used to track these trajectories while satisfying friction cone constraints. The process and measurement models covariance matrices are tuned as Q k = Diag(10 −7 , 10 −7 , 10 −7 , 10 −5 , 10 −5 , 10 −5 , 10 −4 , 10 −4 , 10 −4 ) and R K = (10 −5 ) * I 9 respectively. Note that we do not close the control loop around the estimated centroidal states; this is left to future work. Note also that the measured torques are computed by simply multiplying the measured current to the motor constant. While this does not give a precise torque measurement at the output of the joints due to the un-modeled drive system, we show in the following experiments that this is not an issue for the estimator.

A. Motion #1: Moving the Base while Balancing
In the first scenario we consider a wobbling motion of the base without contact switching (Fig. 1). The main goal of this test is to evaluate the quality of the estimated states in isolation from uncertainties introduced by contact switching as well as contact detection. Fig. 2 shows the centroidal states computed from (6) and forward kinematics (blue) compared against the estimated states from our proposed torque measurement-based EKF (red). The use of torque measurements in the proposed EKF is seen to significantly filter the centroidal momentum without adding delays.

B. Motion #2: Forward Trotting
In the second scenario, we consider a more complex motion involving intermittent contact switching. In this test, the robot moves forward approximately one meter in a trotting gait (Fig. 3). Contact detection is performed by projecting the joint torques of each leg through the Jacobian into the space of endeffector forces, and applying a force threshold to determine whether each foot is in contact. As shown in Fig. 4, in this case there are large discontinuities in the computed states (blue) due to the contact switches; in practice, these anomalies could destabilize a control loop if these states are used for feedback. However, the proposed torque measurement-based EKF (red) reduces the magnitude of these discontinuities, resulting in state estimates which are more suitable to use in a control loop.

C. Motion #3: Jump in Place
The third motion we consider is a jump in place (Fig.  5). This motion is particularly challenging due to the large impact induced during landing that could be problematic if a direct force/torque sensor at the endeffector is used to estimate the centroidal states as in [14]. However, since we are instead using torque measurements in the proposed estimator, we benefit from the fact that Solo12's structural and drive system damping filters out the effect of the impact from measured torque considerably. This helps prevent the estimator from diverging in the presence of large impacts. As demonstrated by the results shown in Fig. 6, the proposed estimator is not significantly affected by the impact force during landing, and can thus filter out the measurement noise even in the presence of large impact forces during dynamic maneuvers.

V. CONCLUSIONS AND FUTURE WORK
In this paper, we proposed to use joint torque measurements for the centroidal momentum state estimation using an EKF for legged robots. First, we exploited the relationship between the joint torques and centroidal states by projecting the whole-body dynamics of the robot into the nullspace of the contact constraints. Then, we used the resulting dynamics as the process model of an EKF with joint torque input to estimate the centroidal states. One of the key advantages of our approach is that we can estimate the required quantities without relying on Force/Torque sensors as direct measurements. We evaluated the performance of our centroidal momentum estimator for three different gaits on the quadruped robot Solo12. The results of these experiments showed that the estimated states from our approach have considerably less noise when compared to the naive computation of the centroidal states from the measured encoder values and estimated base states while maintaining minimal delay.
In the future, we would like to test the performance of closed-loop momentum control using the estimated centroidal states from the proposed EKF. Furthermore, we would like to mitigate the effects of dynamic model uncertainties by simultaneously estimating the centroidal states and performing online model identification. We are also interested in extending the estimation problem to enable identification of the (non-rigid) contact model parameters, which could be used to adapt the controller when the surface properties are changed.