Real-Time Cascaded State Estimation Framework on Lie Groups for Legged Robots Using Proprioception

Abstract

This paper proposes a cascaded state estimation framework based on proprioception for robots. A generalized-momentum-based Kalman filter (GMKF) estimates the ground reaction forces at the feet through joint torques, which are then input into an error-state Kalman filter (ESKF) to obtain the robot’s prior state estimate. The system’s dynamic equations on the Lie group are parameterized using canonical coordinates of the first kind, and variations in the tangent space are mapped to the Lie algebra via the inverse of the right trivialization. The resulting parameterized system state equations, combined with the prior estimates and a sliding window, are formulated as a moving horizon estimation (MHE) problem, which is ultimately solved using a parallel real-time iteration (Para-RTI) technique. The proposed framework operates on manifolds, providing a tightly coupled estimation with higher accuracy and real-time performance, and is better suited to handle the impact noise during foot–ground contact in legged robots. Experiments were conducted on the BQR3 robot, and comparisons with measurements from a Vicon motion capture system validate the superiority and effectiveness of the proposed method.

1. Introduction

Legged robots have witnessed rapid advancements in recent years. Compared to traditional wheeled and tracked robots, their superior mobility and obstacle-negotiation capabilities exhibit significant potential across various task scenarios such as patrolling, search and rescue, and transportation [1,2,3]. State estimation plays a crucial role in the operations of legged robots. Accurate state estimation not only provides precise coordinates within the global frame but also supplies smooth and accurate signals to the controller. However, because legged robots are complex electromechanical systems, their variable working environments, continuous multi-support movement characteristics, underactuation, and nonlinear system properties pose considerable challenges to state estimation.

Various methods have been applied to the state estimation problem in legged robots. The Extended Kalman Filter (EKF) has been introduced for its lightweight nature and robustness; for instance, Sola et al. developed a quaternion-based QEKF [4]. Hartley, taking into account the right-invariant properties on Lie groups, developed a contact-aided invariant Extended Kalman Filter (IEKF) [5]. Li further incorporated dynamic characteristics into this framework, enhancing the estimator’s adaptability to contact slippage [6]. However, Kalman filter-based methods can only handle Gaussian noise and cannot adequately address constraints. Consequently, optimization-based tightly coupled methods have been introduced. Wisth et al. developed a state estimation framework based on factor graphs, which is often used for processing information from external sensors such as vision [7]. Although integrating multiple sensors brings excellent estimation performance, it also incurs high computational costs, posing challenges to real-time applications [8,9]. In addition, external sensors may be unstable and are prone to being affected by the environment. For example, under adverse weather conditions such as heavy rain, thick fog, and heavy snow, the accuracy of optical sensors will decrease because the propagation of light is hindered, resulting in blurred images or data being obtained. Therefore, proprioceptive estimation remains necessary [10].

Moving horizon estimation is the dual problem of Model Predictive Control (MPC) [11]. It integrates the system model over a finite past time horizon and fuses all observational information within this horizon to obtain the optimal state estimate sequence [12,13]. This method can handle non-Gaussian noise and constraints, making it well-suited for legged robots with frequent contact switches, thereby reducing noise impact at the moments of ground contact. While MHE theory has been extensively studied, its application to legged robots remains limited. For example, Khorshidi combined Dynamic Mode Decomposition (DMD) with MHE to estimate the centroidal states of a quadruped robot, but this method relies on neural networks for estimating centroidal dynamics, making it challenging to apply at high update frequencies on real hardware [14]. Kang proposed a distributed state estimation method combining EKF and MHE, where the nonlinear parts are separately estimated. While this method is fast, it does not fully utilize the advantages of MHE [15].

In previous studies, Euler angles and quaternions were commonly used to describe rigid body motion [4,10]. However, Euler angles suffer from gimbal lock, and quaternions have the issue of double coverage. To overcome these limitations, Lie group representations have been introduced [16,17]. Nevertheless, the over-parameterization of rotation matrices makes it challenging to directly construct the state equations. In optimal control, error dynamics based on variational methods are often employed [18,19], but extending these methods to MHE is difficult. This paper employs canonical coordinates of the first kind on the Special Euclidean Group ($SE\left(3\right)$) to parameterize rigid body motion [20], which naturally avoids tensor usage while preserving structural properties. The Cayley transform, as a third-order approximation of the exponential map, ensures both computational efficiency and sufficient accuracy.

MHE, being an optimization-based tightly coupled method, relies heavily on solving speed and update frequency for its performance. In nonlinear Model Predictive Control (NMPC), methods such as sequential quadratic programming (SQP) [21,22] and real-time iteration (RTI) [23,24,25] have been widely explored for real-time optimization, and these methods can easily be extended to MHE. RTI, known for its strong real-time capability, has already been shown to offer optimality under certain conditions.

The main contribution of this paper is the introduction of a proprioceptive tightly coupled state estimation framework based on Lie group representations. The framework first uses an ESKF to provide prior estimates of the robot’s state, followed by MHE to obtain the final state estimation. Our ESKF incorporates joint torque information, which not only improves the estimation accuracy compared to existing methods but also allows for the estimation of external forces acting on the robot. MHE is formulated using canonical coordinates of the first kind based on the Cayley transform in the right-trivialized tangent space of $SE\left(3\right)$, combining past observations and prior information from the ESKF to achieve tightly coupled estimation. To further reduce the delay caused by optimization, we propose a parallel RTI method, enabling high-speed state updates. Finally, experiments conducted on the BQR3 robot validate the effectiveness and superiority of the proposed method.

The paper is structured as follows. The system modeling on Lie group is described in Section 2. The prior estimation method of ESKF incorporating torque information is presented in Section 3. Section 4 discusses the implementation of the tightly coupled MHE framework and the Para-RTI solution method. Furthermore, Section 5 conducts experimental validation of the proposed framework, and finally the paper is concluded in Section 6.

2. System Modeling Using Canonical Coordinates of the First Kind

2.1. Cayley Map and Right-Trivialized Tangent

For dynamical systems, their configuration space can be represented on a Lie group G, with the corresponding tangent space referred to as the Lie algebra $\mathfrak{g}$. The Lie algebra can be mapped from the vector space ${\mathbb{R}}^n$ using the hat operator $\widehat{\left(·\right)}:{\mathbb{R}}^n\to \mathfrak{g}$, and the inverse mapping is represented by the vee map ${\left(·\right)}^{\vee }:\mathfrak{g}\to {\mathbb{R}}^n$. Retraction maps $\phi :\mathfrak{g}\to G$ are used to relate the Lie group to its corresponding Lie algebra and can be written as

$$g=\phi \left(\widehat{\mathit{\rho }}\right)$$

(1)

where $g\in G$ and $\mathit{\rho }\in {\mathbb{R}}^6$.

The exponential map $\exp$ can be used as a kind of retraction map, which is also called canonical coordinates of the first kind. The exponential map provides an exact representation of rigid body motion on a Lie group. However, due to its computational complexity, this paper considers a third-order approximation $cay\left(\widehat{\mathit{\rho }}\right)=exp\left(\widehat{\mathit{\rho }}\right)+O\left(\parallel \widehat{\mathit{\rho }}{\parallel }^3\right)$ (which is only valid for the rigid motion groups like the Special Orthogonal Group ($SO\left(3\right)$) and $SE\left(3\right)$), and the Cayley map is defined as follows:

$$g=cay\left(\widehat{\mathit{\rho }}\right)={\left(\mathbb{I}-\widehat{\mathit{\rho }}/2\right)}^{-1}\left(\mathbb{I}+\widehat{\mathit{\rho }}/2\right)$$

(2)

$$\widehat{\mathit{\rho }}={cay}^{-1}\left(g\right)=2\left(g-\mathbb{I}\right){\left(g+\mathbb{I}\right)}^{-1}$$

(3)

where $\mathbb{I}$ is the identity matrix.

The Cayley map does not involve transcendental functions and is easier to differentiate. For the twist $\mathit{\xi }\in {\mathbb{R}}^n$ on the group, it has the Poisson relation $\dot{g}=g\widehat{\mathit{\xi }}$ which is a differential equation. To solve the differential equation, use right-trivialized tangent $d{\phi }_{\mathit{\rho }}:\mathfrak{g}\to T_{g}G$ (Tangent space on g):

$$\begin{array}{cc}\hfill \widehat{\mathit{\xi }}& =g^{-1}\dot{g}\hfill \\ \hfill & =g^{-1}{\displaystyle \frac{d}{dt}}cay\left(\widehat{\mathit{\rho }}\right)\hfill \\ \hfill & =g^{-1}cay\left(\widehat{\mathit{\rho }}\right){dcay}_{\mathit{\rho }}\left(\dot{\widehat{\mathit{\rho }}}\right)\hfill \\ \hfill & ={cay}^{-1}\left(\widehat{\mathit{\rho }}\right)cay\left(\widehat{\mathit{\rho }}\right){dcay}_{\mathit{\rho }}\left(\dot{\widehat{\mathit{\rho }}}\right)\hfill \\ \hfill & ={dcay}_{\mathit{\rho }}\left(\dot{\widehat{\mathit{\rho }}}\right)\hfill \end{array}$$

(4)

where ${\displaystyle \frac{d}{dt}}cay\left(\widehat{\mathit{\rho }}\right)=cay\left(\widehat{\mathit{\rho }}\right){dcay}_{\mathit{\rho }}\left(\dot{\widehat{\mathit{\rho }}}\right)$ is derived from the chain rule.

Then the local reconstruction equation can be obtained from the inverse of Equation (4):

$$\dot{\widehat{\mathit{\rho }}}={dcay}_{\mathit{\rho }}^{-1}\left(\widehat{\mathit{\xi }}\right)$$

(5)

The system model and mapping relationships on the manifold are illustrated in Figure 1.

2.2. Configuration of Rigid Body on Lie Groups

In our case, the rigid body motion evolves on the Special Euclidean group $SE\left(3\right)$, which represents the set of all rigid body transformations in three-dimensional space. It is a Lie group that combines rotation and translation, and it can be expressed as

$$\mathit{T}=\left[\begin{array}{cc}\mathit{r}& p\\ \mathbf{0}& 1\end{array}\right]\in SE\left(3\right)$$

(6)

where $\mathbf{R}$ is a rotation matrix in the Special Orthogonal Group $SO\left(3\right)$, and $p\in {\mathbb{R}}^3$ is a translation vector. The tangent vector field at the identity element of a Lie group G is defined as the Lie algebra $\mathfrak{g}$.

For $\mathit{T}\in SE\left(3\right)$, the corresponding Lie algebra can be expressed in the following form:

$$\widehat{\mathit{\rho }}=\left[\begin{array}{cc}{\mathit{\theta }}^{\times }& \mathit{r}\\ \mathbf{0}& \mathbf{0}\end{array}\right]\in \mathfrak{se}\left(3\right),{\mathit{\theta }}^{\times }=\left[\begin{array}{ccc}0& -{\theta }_3& {\theta }_2\\ {\theta }_3& 0& -{\theta }_1\\ -{\theta }_2& {\theta }_1& 0\end{array}\right]$$

(7)

where $\mathit{\theta }\in {\mathbb{R}}^3$ and $\mathit{r}\in {\mathbb{R}}^3$, while the operator ${\left(·\right)}^{\times }:{\mathbb{R}}^3\to \mathfrak{so}\left(3\right)$ denotes the skew-symmetric matrix that maps from the Lie algebra to the vector space.

Because $\mathfrak{se}\left(3\right)$ is obviously diffeomorphic to ${\mathbb{R}}^6$, we can get the parameterized equation from Equation (5):

$$\dot{\mathit{\rho }}={dcay}_{\mathit{\rho }}^{-1}\left(\mathit{\xi }\right),\mathit{\rho }=\left[\begin{array}{c}\mathit{\theta }\\ \mathit{r}\end{array}\right]\in {\mathbb{R}}^6,\mathit{\xi }=\left[\begin{array}{c}\mathit{\omega }\\ \mathit{v}\end{array}\right]\in {\mathbb{R}}^6$$

(8)

where $\mathit{\theta }\in {\mathbb{R}}^3$ and $\mathit{r}\in {\mathbb{R}}^3$ are angular and linear velocity, respectively, and the inverse Cayley tangent can be derived simply as [20]

$${dcay}_{(\mathit{\theta },\mathit{r})}^{-1}=\left[\begin{array}{cc}{\mathbb{I}}_3-{\displaystyle \frac{1}{2}}{\mathit{\theta }}^{\times }+{\displaystyle \frac{1}{4}}\mathit{\theta }{\mathit{\theta }}^T& {\mathbf{0}}_3\\ -{\displaystyle \frac{1}{2}}\left({\mathbb{I}}_3-{\displaystyle \frac{1}{2}}{\mathit{\theta }}^{\times }\right){\mathit{r}}^{\times }& {\mathbb{I}}_3-{\displaystyle \frac{1}{2}}{\mathit{\theta }}^{\times }\end{array}\right]$$

(9)

The dynamic equation of a rigid body on $SE\left(3\right)$ can be described by the Euler–Poincaré equation:

$$\dot{\mathit{\xi }}={\mathit{J}}_b^{-1}(-{ad}_{\mathit{\xi }}^{*}{\mathit{J}}_b\mathit{\xi }+\mathit{u})$$

(10)

where ${\mathit{J}}_b$ is the mass–inertia matrix composed by inertia matrix $\mathit{I}$ and mass m

$${\mathit{J}}_b=\left[\begin{array}{cc}{\mathit{I}}_b& \mathbf{0}\\ \mathbf{0}& m{\mathbb{I}}_3\end{array}\right]\in {\mathbb{R}}^{6\times 6}$$

(11)

$\mathit{u}$ is the system input and ${ad}_{\mathit{\xi }}$ is called the adjoint operator and is given by

$${ad}_{\mathit{\xi }}^{*}=-{ad}_{\mathit{\xi }}^T=\left[\begin{array}{cc}-{\mathit{\omega }}^{\times }& -{\mathit{v}}^{\times }\\ \mathbf{0}& -{\mathit{\omega }}^{\times }\end{array}\right]$$

(12)

Combining Equation (8) and Equation (10), we can obtain the complete system equation of state:

$$\dot{\mathit{x}}=\left[\begin{array}{c}\dot{\mathit{\rho }}\\ \dot{\mathit{\xi }}\end{array}\right]=\underset{f(\mathit{x},\mathit{u})}{\underbrace{\left[\begin{array}{c}{dcay}_{\mathit{\rho }}^{-1}\left(\mathit{\xi }\right)\\ {\mathit{J}}_b^{-1}(-{ad}_{\mathit{\xi }}^{\ast }{\mathit{J}}_b\mathit{\xi }+\mathit{u})\end{array}\right]}}$$

(13)

Optimization problems for nonlinear models are often solved by linearizing the system state equations to the first order as follows:

$$\dot{\mathit{x}}=\mathit{A}\mathit{x}+\mathit{B}\mathit{u}$$

(14)

where

$$\begin{array}{cc}\hfill \mathbf{A}& ={\displaystyle \frac{\mathit{\partial }f(\mathit{x},\mathit{u})}{\mathit{\partial }\mathit{x}}}=\left[\begin{array}{cc}{\displaystyle \frac{\mathit{\partial }\left({dcay}_{\mathit{\rho }}^{-1}\mathit{\xi }\right)}{\mathit{\partial }\mathit{\rho }}}& {dcay}_{\mathit{\rho }}^{-1}\\ \mathbf{0}& {\mathit{J}}_b^{-1}{\displaystyle \frac{\mathit{\partial }\left(\mathit{A}{\mathbf{d}}_{\mathit{\xi }}^T{\mathit{J}}_b\mathit{\xi }\right)}{\partial \mathit{\xi }}}\end{array}\right]\hfill \\ \hfill \mathit{B}& ={\displaystyle \frac{\partial f(\mathit{x},\mathit{u})}{\partial \mathit{u}}}=\left[\begin{array}{c}\mathbf{0}\\ {\mathit{J}}_b^{-1}\end{array}\right]\hfill \end{array}$$

(15)

3. Error-State Kalman Filter with Joint Torque

In the previous section, we derived the state equations of the system. However, when modeling actual dynamical systems, it is essential to account for additional factors such as system noise. Within the framework proposed in this paper, it is necessary to model not only the basic state information but also the sensor noise and biases. Moreover, the identification of inputs to the dynamical system becomes crucial with the incorporation of dynamics. This section presents an estimator based on the ESKF to estimate the biases of the robot’s sensors and the external wrench acting on it.

3.1. GRF Reconstruction

Dynamical systems are typically complex and nonlinear, making it crucial to accurately account for system dynamics to achieve precise state estimation. In this paper, we simplify the dynamical system of a legged robot to a single rigid body model. The input $\mathit{u}$ is a six-dimensional force vector ${[M_x,M_y,M_z,F_x,F_y,F_z]}^T$ acting on the torso, which generally consists of contact forces and gravitational forces. Contact forces arise from interactions with the environment, including the ground reaction forces (GRF) ${\mathit{f}}_c={[f_{cx},f_{cy},f_{cz}]}^T$ at the foot and external interaction wrench ${\mathit{u}}_e={[M_{ex},M_{ey},M_{ez},F_{ex},F_{ey},F_{ez}]}^T$. Therefore, $\mathit{u}$ in Equation (14) can be rewritten as

$$\mathit{u}=\sum _{i=1}^n\left[\begin{array}{c}{\mathit{r}}_i^{\times }\\ \mathbb{I}\end{array}\right]{\mathit{f}}_{c,i}+{\mathit{u}}_e+\left[\begin{array}{c}\mathbf{0}\\ mg\end{array}\right]$$

(16)

where $g={[0,0,-9.8]}^T$ is the gravity vector and n is the number of stance legs.

The reconstruction of GRF can be achieved through a Kalman filter using the generalized momentum equation for a single leg, which can be expressed as

$$\dot{\mathit{h}}-\mathit{C}{(\mathit{q},\dot{\mathit{q}})}^T\dot{\mathit{q}}+\mathit{G}\left(\mathit{q}\right)=\mathit{\tau }+{\mathit{J}}_c^T{\mathit{f}}_c$$

(17)

where $\mathit{h}=\mathit{M}\left(\mathit{q}\right)\dot{\mathit{q}}$ is the generalized momentum, and $\mathit{M}\left(\mathit{q}\right)$, $\mathit{C}(\mathit{q},\dot{\mathit{q}})$, and $g\left(\mathit{q}\right)$ are the inertia matrix, Coriolis matrix, and gravity matrix, respectively. $\mathit{q}$ and $\dot{\mathit{q}}$ are the joint position and joint velocity. $\mathit{\tau }$ is the vector of the actuated joint torques, and ${\mathit{J}}_c$ is the corresponding Jacobian.

The GRF can be modeled as random walk process $\dot{{\mathit{f}}_c}={\mathit{n}}_f$ and ${\mathit{n}}_f$ is the Gaussian noise. The leg model can be obtained as

$$\left[\begin{array}{c}\dot{\mathit{h}}\\ \dot{{\mathit{f}}_c}\end{array}\right]=\underset{{\mathit{A}}_c}{\underbrace{\left[\begin{array}{cc}\mathbf{0}& {\mathit{J}}_c^T\\ \mathbf{0}& \mathbf{0}\end{array}\right]}}\left[\begin{array}{c}\mathit{h}\\ {\mathit{f}}_c\end{array}\right]+\underset{{\mathit{B}}_c}{\underbrace{\left[\begin{array}{c}\mathbb{I}\\ \mathbf{0}\end{array}\right]}}\mathit{u}+{\mathit{n}}_h$$

(18)

where $\mathit{u}=\mathit{C}{(\mathit{q},\dot{\mathit{q}})}^T\dot{\mathit{q}}-\mathit{G}\left(\mathit{q}\right)+\mathit{\tau }$ includes all the nonlinear dynamics.

As the generalized momentum $\mathit{h}$ can be measured from joint encoders and with the high transparency joint, the GRF measurement can be obtained from joint torque. The measurement model can be defined as

$$\underset{\mathit{z}}{\underbrace{\left[\begin{array}{c}{\mathit{h}}_m\\ {\mathit{f}}_m\end{array}\right]}}=\mathit{H}\mathit{x}+{\mathit{n}}_m$$

(19)

where $\mathit{H}$ is an identity matrix and ${\mathit{n}}_m$ is the Gaussian noise.

With Equations (18) and (19), the GRF ${\mathit{f}}_c$ can be obtained from a standard Kalman filter.

In practical applications, most mainstream legged robots today are equipped with high-performance planetary gearboxes, typically with a reduction ratio below 30. Extensive experimental evidence has shown that such gearboxes, when operating at low reduction ratios, can effectively reduce gearbox friction and non-ideal actuator effects, thus achieving high torque control transparency. Therefore, in the process of joint torque estimation and GRF reconstruction, it is reasonable to approximate that the output joint torque closely corresponds to the actual force applied.

3.2. Error-State Kalman Filter

Consider the equations of motion of a rigid body on the $SE\left(3\right)$ group, which, unlike the modeling in vector space in the previous section, can be written as

$$\begin{array}{cc}\hfill \dot{g}& =g\widehat{\mathit{\xi }}\hfill \\ \hfill \dot{\mathit{\xi }}& ={\mathit{J}}_b^{-1}(-{ad}_{\mathit{\xi }}^{*}{\mathit{J}}_b\mathit{\xi }+\mathit{u})\hfill \end{array}$$

(20)

To incorporate the biases of the accelerometer and gyroscope in the IMU, as well as the external wrench acting on the system, the true state variables of the system can be expanded as

$$\mathit{s}:=\left(g,\mathit{\xi },\mathit{B},{\mathit{u}}_e\right)$$

(21)

By modeling the biases and external wrench as random walk processes, the system’s true state equations can be formulated to reflect these stochastic components as follows:

$$\begin{array}{cc}\hfill \dot{g}& =g\widehat{\mathit{\xi }}\hfill \\ \hfill \dot{\mathit{\xi }}& ={\mathit{J}}_b^{-1}(-{ad}_{\mathit{\xi }}^{*}{\mathit{J}}_b\mathit{\xi }+{\mathit{u}}_c+{\mathit{u}}_e+{\mathit{u}}_g)+{\mathit{n}}_{uc}\hfill \\ \hfill \dot{\mathit{b}}& ={\mathit{n}}_b\hfill \\ \hfill {\dot{\mathit{u}}}_e& ={\mathit{n}}_{ue}\hfill \end{array}$$

(22)

where ${\mathit{n}}_{uc}$, ${\mathit{n}}_b$, and ${\mathit{n}}_{ue}$ are the corresponding Gaussian noise.

Considering that the prediction in Kalman filter occurs only within a single time step, we approximate the infinitesimal transformation on the Lie group to linearize the system state equations:

$$\delta g={\left.{\displaystyle \frac{d}{dϵ}}\right|}_{ϵ=0}gexp\left(ϵ\delta \widehat{\zeta }\right)=g\delta \widehat{\zeta }$$

(23)

Using the feature of skew-symmetric matrix, ${\mathit{a}}^{\times }\mathit{b}=\mathit{a}\times \mathit{b}=-\mathit{b}\times \mathit{a}=-{\mathit{b}}^{\times }\mathit{a},\forall \mathit{a},\mathit{b}\in {\mathbb{R}}^3$, the error-state model can be derived as

$$\left[\begin{array}{c}\delta \dot{\mathit{\zeta }}\\ \delta \dot{\mathit{\xi }}\\ \delta \dot{\mathit{b}}\\ \delta {\dot{\mathit{u}}}_e\end{array}\right]=\left[\begin{array}{cccc}-{\mathrm{ad}}_{\mathit{\xi }}& \mathbb{I}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& {\mathit{J}}_b^{-1}(\mathit{C}-{\mathrm{ad}}_{\mathit{\xi }}^{*}{\mathit{J}}_b)& \mathbf{0}& {\mathit{J}}_b^{-1}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\end{array}\right]\left[\begin{array}{c}\delta \mathit{\zeta }\\ \delta \mathit{\xi }\\ \delta \mathit{b}\\ \delta {\mathit{u}}_e\end{array}\right]+\left[\begin{array}{c}\mathbf{0}\\ {\mathit{n}}_{uc}\\ {\mathit{n}}_b\\ {\mathit{n}}_{ue}\end{array}\right]$$

(24)

where

$$\mathit{C}=\left[\begin{array}{cc}{\left({\mathit{I}}_b\omega \right)}^{\times }& m{\mathit{v}}^{\times }\\ m{\mathit{v}}^{\times }& \mathbf{0}\end{array}\right]$$

(25)

Equation (24) can be easily rewritten to the discrete linearized form by using the forward Euler method. Then the error-state estimation is

$$\delta {\mathit{s}}_k={\mathit{K}}_k\left({\mathit{z}}_k-h\left({\mathit{s}}_k\right)\right)$$

(26)

where

$${\mathit{z}}_k-h\left({\mathit{s}}_k\right)=\left[\begin{array}{c}{\mathit{a}}_m-{\displaystyle \frac{{\mathit{u}}_c+{\mathit{u}}_g+{\mathit{u}}_e}{m}}+{\mathit{b}}_a\\ {\omega }_m-\omega +{\mathit{b}}_g\\ {\mathit{v}}_m-\mathit{v}\\ log{\left(g^{-1}{g}_m\right)}^{\vee }\end{array}\right]$$

(27)

Then the measurement matrix ${\mathit{H}}_k$ can be derived by using the chain rule. The implementation details of the ESKF are given in Algorithm 1.

Algorithm 1 Error-state Kalman filter with joint torque.

Initialize the state variable and covariance matrix

${\tilde{\mathit{s}}}_0=\mathbf{0}$, ${\tilde{P}}_0=\mathbb{I}$

repeat

Compute state

$\delta {\mathit{s}}_k={\mathit{A}}_{e,k}\delta {\tilde{\mathit{s}}}_{k-1}$

Compute covariance estimation

${\mathit{P}}_k={\mathit{A}}_{e,k}{\tilde{\mathit{P}}}_{k-1}{\mathit{A}}_{e,k}^T+{\mathit{Q}}_k$

Compute optimal Kalman gain

${\mathit{K}}_k={\mathit{P}}_k{\mathit{H}}_k^T{\left({\mathit{H}}_k{\mathit{P}}_k{\mathit{H}}_k^T+{\mathit{R}}_k\right)}^{-1}$

Update state estimation with measurement

$\delta {\tilde{\mathit{s}}}_k={\mathit{K}}_k\left({\mathit{z}}_k-h\left({\mathit{s}}_k\right)\right)$

Update covariance estimation

${\tilde{\mathit{P}}}_k=\left(\mathit{I}-{\mathit{K}}_k{\mathit{H}}_k\right){\mathit{P}}_k$

until stop

4. MHE with Para-RTI

To enable accurate and real-time state estimation for nonlinear systems under constraints, MHE has emerged as a powerful optimization-based framework. However, its practical deployment is often hindered by high computational demands, especially in fast-sampling or resource-limited scenarios. To address this issue, we adopt an efficient implementation of MHE based on the parallelizable real-time iteration (Para-RTI) scheme, which exploits parallel computation across the estimation horizon to significantly reduce latency while preserving estimation accuracy. The whole state estimation framework is shown in Figure 2.

4.1. MHE

Although Kalman filter is simple and effective, with the advancement of onboard computational power, state estimation problems are no longer satisfied with using only the Kalman filter. Optimization-based methods can better handle complex nonlinear systems, constraints, and non-Gaussian noise, and they also facilitate multi-sensor fusion. Optimization-based methods can be seen as solving the Maximum A Posteriori (MAP) problem, where MHE achieves state estimation by minimizing a cost function over a finite time window. MHE is a recursive implementation of MAP estimation and can be written as

$$\begin{array}{cc}\hfill \underset{\begin{array}{c}\mathit{X}[0:n]\end{array}}{min}& {\displaystyle \frac{1}{2}}{∥{\mathit{x}}_0-{\tilde{\mathit{x}}}_0∥}_{\mathit{Q}}^2+\sum _{k=0}^{n-1}{\displaystyle \frac{1}{2}}{∥{\mathit{e}}_{x,k}∥}_{{\mathit{P}}_x}^2+\sum _{k=0}^n{\displaystyle \frac{1}{2}}{∥{\mathit{e}}_{y,k}∥}_{{\mathit{P}}_y}^2\hfill \end{array}$$

(28a)

$$\begin{array}{cc}\hfill \mathrm{s}.\mathrm{t}.& {\mathit{x}}_{k+1}=\mathit{f}\left({\mathit{x}}_k,{\mathit{u}}_k\right)+{\mathit{e}}_{x,k}\hfill \end{array}$$

(28b)

$$\begin{array}{cc}\hfill & {\mathit{y}}_k=\mathit{h}\left({\mathit{x}}_k,{\mathit{u}}_k\right)+{\mathit{e}}_{y,k}\hfill \end{array}$$

(28c)

$$\begin{array}{cc}\hfill & \mathit{g}\left({\mathit{x}}_k,{\mathit{u}}_k\right)\le 0,k=0,\dots ,n\hfill \end{array}$$

(28d)

where ${\mathit{X}}_{0:n}=[{\mathit{x}}_{0:n},{\mathit{e}}_{x,0:n},{\mathit{e}}_{y,0:n}]$ is a sequence that contains all state variables and error terms within the finite horizon, n is the number of time steps, $\mathit{Q}$, ${\mathit{P}}_x$, and ${\mathit{P}}_y$ are corresponding weight matrices. Equations (28b) and (28c) represent the state equation and observation equation as constraints, respectively, while Equation (28d) represents the inequality constraints. The first term of the cost function is typically referred to as the arrival cost. Here, ${\tilde{\mathit{x}}}_0$ is the prior estimate, which, in this paper, is updated using the state estimate from the ESKF designed in the previous section. The other terms are the running cost, which quantify the process error and measurement error.

Unlike the weighted matrices in MPC, which can be freely adjusted, the weighting matrices in MHE have a significant impact on the estimation results. They are typically inversely proportional to the noise covariance of the system. The noise covariance can be obtained from the Kalman filter to ensure that the final estimation result approximates the maximum likelihood estimate.

4.2. Para-RTI

Nonlinear models typically impose significant computational burdens on the solution of MHE. As the dual problem of MPC, the solution methodologies for NMPC have become relatively mature, and the efficacy of RTI techniques has been validated. In this paper, we extend the application of RTI to the solution of MHE. RTI can be viewed as a specialized application of SQP. By performing a first-order linearization of the nonlinear system model and compressing the problem into a quadratic programming (QP) subproblem, RTI executes a single Newton step at each sampling time. Each individual RTI step is divided into a preparation phase and a feedback phase.

Algorithm 2 Para-RTI: Parallel real-time iteration for nonlinear MHE.
1:	Initialization: Initialize Kalman Filter (KF), Preparation Thread, and Estimation Thread
2:	Main Thread:
3:	while true do
4:	Run Kalman Filters to obtain prior estimate ${\widehat{\mathit{x}}}_0$, bias $\mathit{b}$, and external wrench ${\mathit{u}}_e$
5:	Send KF results to Preparation Thread
6:	Wait for sensor measurements ${\mathit{y}}_k$ and send sensor measurements to Estimation Thread
7:	Receive estimated values from Estimation Thread
8:	Update system state with estimated values
9:	end while
10:	Preparation Thread:
11:	while true do
12:	Wait for prior results ${\tilde{\mathit{x}}}_0$ from Main Thread
13:	Evaluate objective in Equation (28a), constraints in Equations (28b)–(28d), and sensitive matrices $A,B$ in Equation (15)
14:	Store evaluation results for Estimation Thread
15:	end while
16:	Estimation Thread:
17:	while true do
18:	Wait for sensor measurements ${\mathit{y}}_k$ from Main Thread
19:	Retrieve evaluation results from Preparation Thread
20:	Solve QP problem using evaluation results
21:	Send estimated values to Main Thread
22:	end while

During the preparation phase, which is executed before new measurement data is acquired, the system model is linearized, and the solution from the previous time step is used as the initial guess. The entire Nonlinear Programming (NLP) problem is then constructed into a QP subproblem. The feedback phase, which occurs immediately after new measurement data is available, involves solving the previously constructed QP problem. The last element in the resulting solution sequence is taken as the current state estimate to be sent to the controller.

To further accelerate the update frequency of MHE, we propose a Para-RTI method. Para-RTI is also divided into a preparation phase and an estimation phase. However, unlike standard RTI, the preparation and estimation phases are executed in two separate threads outside the main thread. A high-real-time Kalman Filter runs in the main thread, calculating the prior estimate, bias, and external wrench in each cycle. The preparation thread obtains the Kalman Filter results from the main thread in each cycle, and evaluates the objective, constraints, and corresponding sensitivities. The estimation thread then solves the QP problem using the results from the preparation thread each time sensor measurements are received from the main thread, and the estimated values are sent back to the main thread. The continuous execution of Para-RTI compresses the estimation time to the QP problem-solving time, thereby enhancing the estimation speed of NMHE. The structure of Para-RTI is shown in Figure 3.

In this paper, Equation (28b) adopts the linearized form presented in Equation (14), with the input $\mathit{u}$ being computed within ESKF. Additionally, the observation equation incorporates the computed biases as

$$\left[\begin{array}{c}{cay}^{-1}{\left(g_{m,k}\right)}^{\vee }\\ {\mathit{\xi }}_{m,k}+{\mathit{b}}_{g,k}\end{array}\right]=\left[\begin{array}{c}{\mathit{\rho }}_k\\ {\mathit{\xi }}_k\end{array}\right]$$

(29)

Without inequality constraints, the QP subproblem has the standard form as

$$\begin{array}{cc}\hfill min& {\displaystyle \frac{1}{2}}{\mathit{X}}^{\top }\mathbf{H}\mathit{X}+{\mathbf{h}}^{\top }\mathit{X}\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \mathbf{C}\mathit{X}=\mathbf{D}.\hfill \end{array}$$

(30)

5. Experiments

Experiments were conducted on a quadruped robot named BQR3, which weighs 50 kg and measures 0.8 m in length and 0.5 m in height. It is equipped with 12 joints, each integrated with absolute encoders, and an MTi-300 IMU mounted at the center of the torso. Thanks to the high transmission transparency of the joints, joint torques can be computed in real time using actuator current feedback. The proposed framework was deployed on a mini PC (SER07) powered by an AMD Ryzen R7 7840HS CPU (P-core 5.10 GHz, E-core 3.80 GHz).

To evaluate estimation accuracy, a Vicon motion capture system was used to record the absolute position and orientation of the robot’s center of mass.

Two sets of experiments were conducted: circular walking without disturbance (Figure 4) and with external disturbances (Figure 5). All experiments were carried out on a flat terrain within a confined area. The robot was teleoperated using velocity commands, including sudden stops and acceleration/deceleration. In disturbance experiments, random external perturbations were applied via a rope attached to the torso. The robot was controlled by an NMPC controller running at 200 Hz, the ESKF operated at 1000 Hz, and, enabled by the proposed parallel computing framework, and the MHE state estimator was updated at 333 Hz with a prediction horizon of 25 steps.

While many modern embedded platforms (such as those running Linux, RTOS, or equipped with multi-core processors) do support multi-threading, their computational resources and memory are usually much more limited compared to desktop systems. Multi-threading may introduce overhead from context switching and resource contention, and careful scheduling is needed to ensure that real-time performance is not compromised. For resource-constrained embedded platforms, it is often preferable to adopt state estimation methods with lower computational requirements, such as Kalman filter-based approaches, or to use the original RTI method instead of the parallelized Para-RTI scheme. These alternatives can help ensure reliable and timely state estimation without overloading the system.

To validate the superiority and effectiveness of the proposed method, comparisons were made not only with Vicon-based ground truth but also with ESKF and the IEKF method from [5]. Figure 6 presents the absolute position estimation results from different estimators under both experimental conditions. It is noteworthy that the ESKF output serves as prior input to the MHE, and the MHE weighting is set as the inverse of the ESKF noise covariance.

Figure 6a shows results under undisturbed conditions, including time-series data of the x and y positions and the 2D trajectory. The z-direction is not shown, as vertical drift is unacceptable for quadruped robots and is typically compensated using a weighted average of kinematic foot positions. The vertical position of the robot’s base is recalibrated at each step by computing a weighted average of the positions of the stance foot ends, ensuring that the base height is consistent with the ground contact constraints. The corresponding equation is as follows:

$$z_{\mathrm{base}}={\displaystyle \frac{1}{n}}\sum _{i=1}^n·z_{\mathrm{foot},i}$$

(31)

where $w_i$ is the weight for the i-th stance foot (with $\sum w_i=1$), and $z_{\mathrm{foot},i}$ is the vertical position of the i-th stance foot end. This description has been integrated into the methodology for reproducibility.

Compared with the Vicon ground truth, the proposed method demonstrates superior tracking accuracy. ESKF and IEKF, due to their single-step optimal estimation nature, struggle to handle impact-induced noise during foot contact. Since the proposed state estimation framework relies entirely on proprioceptive sensors, drift is inevitable. Nevertheless, the MHE-based method achieves significantly lower drift levels.

Figure 6b presents the position estimation results under external disturbances, with the perturbed periods highlighted in pink. Compared to the undisturbed case, drift is more pronounced, but the proposed method still maintains higher accuracy and stronger robustness against disturbances.

Figure 7 compares the velocity estimation in three directions. As Vicon cannot directly measure velocity, ground truth was obtained by differentiating position data. Under disturbance conditions, the ground truth exhibits substantial noise. All three estimators effectively track velocity and filter noise, with the proposed method showing significantly lower noise levels.

Figure 8 compares rotational estimation. Vicon represents orientation using quaternions, while our estimator is based on Lie group representations. For clearer comparison and interpretation, all results were converted into Euler angles. Since the experiments were performed on flat terrain, the roll and pitch angles remained nearly constant and are omitted. At the beginning of the motion, both estimators closely match the ground truth yaw angle. However, with extended operation, drift becomes inevitable, although the proposed method exhibits slightly lower drift.

Figure 9 shows the computation time. With a rolling horizon of 25 steps, matrix preparation takes approximately 1.5 ms. The quadratic programming solver used is Proxsuite, with a typical solving time of around 2.5 milliseconds. To maintain a stable update rate for the state estimator, the computation is forcibly terminated if it exceeds 3 ms. In the original RTI method, the period is the sum of the preparation time and the solving time. In para-RTI, the period only depends on the solving time.

Table 1. RMSE comparison.

	Proposed	ESKF	IEKF
Position RMSE w/o disturbance (m)	0.0292	0.0472	0.0539
Velocity RMSE w/o disturbance (m/s)	0.0565	0.0592	0.0594
Angle RMSE w/o disturbance (rad)	0.0376	0.0684	0.0687
Position RMSE w/ disturbance (m)	0.1278	0.1601	0.1841
Velocity RMSE w/ disturbance (m/s)	0.1232	0.1268	0.1262
Angle RMSE w/ disturbance (rad)	0.0677	0.1165	0.1085

presents the RMSE values of position, velocity, and orientation for the three methods across two different experiments. It can be seen that the proposed method achieves the lowest RMSE values in all cases.

6. Discussion

This paper presents a real-time proprioceptive state estimation framework for rigid body dynamics, formulated in the right trivialized tangent space of the $SE\left(3\right)$ group using the Cayley transform. By integrating historical observation data with prior estimates from the ESKF, a receding horizon estimator is constructed, and the Para-RTI method is employed to efficiently update the nonlinear state estimates. Experimental results on a quadruped robot demonstrate that the proposed estimator effectively mitigates impact noise from ground contact and significantly reduces estimation drift. Furthermore, the framework exhibits strong potential for fusing multiple sensor inputs and incorporating physical constraints, providing a solid foundation for future research.

Nevertheless, long-term drift remains an inherent limitation of proprioceptive state estimation, primarily due to IMU integration errors and the cumulative effects of minor slippages during foot–ground impacts. The proposed method offers stability in scenarios where external sensing is limited, enabling the robot to maintain autonomous perception even in challenging environments. However, all current experiments are conducted indoors within a fixed area due to motion capture system constraints. In future work, we will further validate the effectiveness of the proposed approach in outdoor environments and incorporate neural network-based terrain estimation algorithms to enhance the robustness of the system. Overall, the results indicate that the proposed framework is a promising solution for robust and accurate state estimation in legged robots, especially under conditions with limited external sensing.