State Estimation for Quadruped Robots on Non-Stationary Terrain via Invariant Extended Kalman Filter and Disturbance Observer

1. Introduction

Quadruped robots are advanced robotic systems inspired by four-legged animals. Compared to other types of mobile robots, quadruped robots have excellent stability and flexibility, capable of walking on various complex and uneven terrains such as sand, grass, gravel roads, and even slippery surfaces. These capabilities make quadruped robots highly promising for applications in rescue missions [1], environmental monitoring [2], and smart agriculture [3]. With the advancement of technology, quadruped robots are increasingly being used in various demanding scenarios, such as mountainous areas, forests, and post-disaster rubble environments. To ensure the stability and reliability of robots in complex environments, precise state estimation becomes particularly important.

State estimation algorithms based on multi-sensor fusion are widely used in quadruped robots, incorporating external sensors like LiDAR, cameras, and GPS, as well as proprioceptive sensors such as IMUs and joint encoders [4]. While external sensors provide rich environmental information, they have limitations; for example, cameras perform poorly in low-light or low-texture environments, LiDAR's accuracy degrades under smoke or dust, and GPS signals can be disrupted in indoor or urban canyon settings [5]. In contrast, proprioceptive sensors, which rely on joint kinematics and IMU data, are unaffected by such environmental factors and offer higher measurement frequencies, enabling fast and precise control of quadruped robots. Therefore, it is crucial to explore state estimation methods based on proprioceptive sensors such as IMUs and joint encoders. IMUs provide high-frequency acceleration and angular velocity data, while joint encoders offer precise measurements of joint angles and speeds. Combining these sensors enables accurate motion state estimation in complex environments, particularly when external sensors fail or are limited. This approach enhances the autonomy, robustness, and reliability of quadruped robots in diverse scenarios.

State estimation methods for legged robots date back to 2005 when Lin [6] developed an approach based on leg kinematic information for a hexapod robot. This method required the robot to maintain a tripod gait with three legs always in contact with a flat terrain. In 2006, Lin [7] further proposed fusing leg odometry with IMU data to improve state estimation during tripod gait running. Since then, state estimation for legged robots has garnered significant attention. Bloesch [8] introduced a quadruped robot state estimation method using an Extended Kalman Filter (EKF), incorporating leg contact points into the filter's state variables and using forward kinematics to estimate leg contact positions and body pose. Camurri [9] developed a legged odometry method without contact sensors, using ground reaction forces to determine the reliability of leg contact and weight each leg's contribution to body velocity. However, due to inconsistencies between EKF's positive feedback and observation information, the filter may diverge. To address this, Hartley [10] proposed a contact-aided Invariant Extended Kalman Filter (InEKF) based on invariant observer theory, showing better performance compared to the traditional quaternion-based EKF.

The aforementioned methods assume stable, slip-free foot-end contact between legged robots and the ground. However, on slippery or soft terrains such as snow, mud, or sand, foot-end slippage occurs, making stable contact challenging. This introduces non-Gaussian errors that accumulate over time, causing unbounded drift. To address this, Ting [11] treated slippage in foot-end observations as outliers and used a weighted least squares method to mitigate their impact on the EKF. Similarly, Bloesch [12] replaced the EKF with an Unscented Kalman Filter (UKF) and applied an outlier rejection method to manage occasional slippage. Jenelten [13] developed a probabilistic slip detector using a Hidden Markov Model, allowing robots to walk on slippery surfaces through impedance control and friction adjustment. However, this approach does not completely resolve pose estimation drift. To reduce drift from slippage, some methods fuse external sensor observations. Wisth [14] proposed a factor graph optimization-based state estimation method that tightly integrates visual features, inertial data, and kinematic constraints. This method was further extended to incorporate LiDAR observations and an online-estimated linear velocity deviation term to minimize drift in legged odometry [4]. Kim [15] introduced a state estimator for quadruped robots based on a pre-integrated foot velocity factor, which does not rely on precise contact detection or fixed foot positions, showing strong performance on uneven and slippery terrains. However, factor graph-based methods use measurements along the entire trajectory for smoothing, which limits their update rates for real-time control. To address this, Teng [16] integrated camera observations into the Invariant Extended Kalman Filter (InEKF) to enhance state estimation on slippery terrains. Fink [17] combined a Global Exponential Stability (GES) nonlinear attitude observer with legged odometry, ensuring consistently bounded speed estimation and reducing drift in unobservable position estimates.

Recently, advancements in computational hardware (e.g., GPUs) have enabled the training and deployment of complex deep learning models, encouraging researchers to develop learning-based slip detection and state estimation methods. Rotella[18] proposed a method using fuzzy clustering to learn contact probabilities for humanoid robots, which, when integrated into a basic state estimation framework, can significantly reduce estimation errors. Buchanan[19] developed a deep neural network to learn motion patterns from IMU data, and when combined with traditional legged odometry, it substantially reduces drift in proprioceptive state estimation. Yang[20] applied neural networks to train weight functions for foot-end forces and legged odometry states, enhancing observation accuracy. However, learning-based methods face challenges such as poor model interpretability and limited generalization. Supervised models require large amounts of labeled data, which is often difficult to obtain, while unsupervised methods treat slip detection as a classification task, limiting the ability to observe slip velocity.

Inspired by [4], we consider modeling slip velocity as a deviation term of speed and explicitly incorporating it into the state equations to reduce pose drift caused by slip through slip velocity estimation. To meet real-time requirements, we opted for a filtering-based method. The Invariant Extended Kalman Filter (InEKF), developed based on invariant observer theory, has been successfully applied in simultaneous localization and mapping (SLAM) [21] and aided inertial navigation systems [22] in recent years. Its symmetry ensures that the estimation error satisfies the "log-linear" autonomous differential equation on the Lie algebra of the corresponding Lie group in the system dynamics. Thus, it is possible to design a nonlinear observer or state estimator that exhibits strong convergence within an attraction domain independent of the system trajectory. Yu [23] presented a similar approach for wheeled platforms, mainly using speed observations from encoders. We have extended this to legged robot platforms. Unlike wheeled platforms, speed observations from encoders at the foot-end are less reliable than those from wheel encoders. To address this issue, we incorporate the contact point positions of each leg into the filter state variables and consider both foot-end position and velocity observations in the observation equation, making the filter more suitable for legged robots.

The main contributions of this paper are as follows:

1. Model foot-end slippage caused by legged robot motion on unstable terrain as a deviation term of body velocity, reducing drift caused by foot-end slippage through velocity deviation estimation.

2. Develop a real-time RI-EKF state and slip estimator for quadruped robots by fusing foot-end velocity and position observations.

3. Validate the mathematical derivation and the proposed state estimator's effectiveness through experimental results using the Jueying Mini robot on multiple unstable terrains.

2. Theoretical Backround

The Assume a matrix Lie group denoted as $\mathcal{G}$, with its corresponding Lie algebra denoted as $\mathfrak{g}$. Let ${(\cdot )}^{\wedge }：{ℝ}^{\dim \mathfrak{g}}\to \mathfrak{g}$ represent the isomorphism that maps elements in the tangent space at the identity element to their corresponding matrix representations in $\mathcal{G}$. The exponential mapping of the Lie group is denoted by $\exp ：{ℝ}^{\dim \mathfrak{g}}\to \mathcal{G}$ and is represented as $\exp \left(\xi \right)={\exp }_{\mathrm{m}}\left({\xi }^{\wedge }\right)$, where ${\exp }_{\mathrm{m}}(\cdot )$ is the usual exponential mapping of $n\times n$ matrices. A system that evolves over time $t$ on a Lie group can be represented by the dynamics: ${\displaystyle \frac{d}{dt}}{X}_t=f_{u_t}\left(X_t\right)$,$X_t\in \mathcal{G}$represents the system state, and ${\widehat{X}}_t$ is usually used to denote the estimated state.

Definition 1.

(Right-invariant and left-invariant errors) The right-invariant and left-invariant errors between the two trajectories $X_t$ and ${\widehat{X}}_t$ are defined as follows:

$${\eta }_t^r={\widehat{X}}_t{X}_t^{-1}=\left({\widehat{X}}_{t}L\right){\left(X_{t}L\right)}^{-1}(\mathrm{right}-\mathrm{invariant}\mathrm{error})$$

(1)

$${\eta }_t^l=X_t^{-1}{\widehat{X}}_t={\left(L{\widehat{X}}_t\right)}^{-1}\left(L{X}_t\right)(\mathrm{left}-\mathrm{invariant}\mathrm{error})$$

(2)

where $L$ is an arbitrary element of the group $\mathcal{G}$.

Definition 2.

(Adjoint map) Let $\mathcal{G}$ be a matrix Lie group with its Lie algebra denoted as $\mathfrak{g}$. For any $X\in \mathcal{G}$, the adjoint map ${\mathrm{Ad}}_X：\mathfrak{g}\to \mathfrak{g}$ is a linear map that satisfies ${\mathrm{Ad}}_X\left({\xi }^{\wedge }\right)=X{\xi }^{\wedge }{X}^{-1}$. Furthermore, the matrix representation of the adjoint map is denoted by ${\mathrm{Ad}}_X$.

Theorem 1.

A system is group-affine, if for all $t>0$,$X_1,X_2\in \mathcal{G}$ its dynamics $f_{u_t}\left(X_t\right)$ satisfies:

$$f_{u_t}\left(X_1{X}_2\right)=f_{u_t}\left(X_1\right)X_2+X_1{f}_{u_t}\left(X_2\right)-X_1{f}_{u_t}\left(I_d\right)X_2$$

(3)

where is the identity matrix. If a system is group-affine, then its right-invariant and left-invariant error dynamics are independent of the trajectory and satisfy:

$${\displaystyle \frac{d}{dt}}{\eta }_t^r=g_{u_t}^r\left({\eta }_t^r\right),\mathrm{where}{g}_{u_t}\left(\eta \right)=f_{u_t}\left(\eta \right)-\eta f_{u_t}\left(I_d\right)$$

(4)

$${\displaystyle \frac{d}{dt}}{\eta }_t^l=g_{u_t}^l\left({\eta }_t^l\right),\mathrm{where}{g}_{u_t}\left(\eta \right)=f_{u_t}\left(\eta \right)-f_{u_t}\left(I_d\right)\eta$$

(5)

Define a matrix $A_t$ of size $\dim \mathfrak{g}\times \dim \mathfrak{g}$ such that for any $t\ge 0$ the function $g_{u_t}\left(\exp (\xi )\right):={\left(A_t\xi \right)}^{\wedge }+\mathcal{O}\left({\Vert \xi \Vert }^2\right)$ holds. Let ${\xi }_t$ be the solution to the following linear differential equation:

$${\displaystyle \frac{d}{dt}}{\xi }_t=A{\xi }_t$$

(6)

Theorem 2.

Consider any two trajectories defined by (4) and (5) with a left-invariant and right-invariant error, respectively. For any initial error ${\xi }_0\in {ℝ}^{\dim \mathfrak{g}}$, if ${\eta }_0=\exp ({\xi }_0)$ , then for all $t\ge 0$,the nonlinear estimation error ${\eta }_t$ can be accurately recovered by the linear differential equation (6), where ${\eta }_t=\exp ({\xi }_t)$.

3. System Model

3.1. State Equation

Our state estimator can estimate the state variables of the robot's motion in the world coordinate system $\left\{w\right\}$, which include the robot's orientation $R_{WB}$, robot's position ${}_{W}p_{WB}$, robot's velocity ${}_{W}v_{WB}$, and the positions of the four feet ${}_{W}p_{Wf1}$, ${}_{W}p_{Wf2}$, ${}_{W}p_{Wf3}$, ${}_{W}p_{Wf4}$.To consider the impact of end-effector slippage on state estimation, we use an anti-slippage observer idea, introducing a bias term $u$ modeled as a drift velocity and explicitly incorporating it into the state equations. These state variables form a group in $S{E}_7\left(3\right)$, hence we represent the state variables of our InEKF model as:

$$X_t=\left[\begin{array}{cc}{R}_{WB}(t)& \begin{array}{ccccccc}{}_{W}v_{WB}(t)& {}_{W}p_{WB}(t)& {}_{W}p_{Wf1}(t)& {}_{W}p_{Wf2}(t)& {}_{W}p_{Wf3}(t)& {}_{W}p_{Wf4}(t)& {}_{W}b_{WB}^v(t)\end{array}\\ 0_{3,7}& I\end{array}\right]$$

(7)

To better understand the content, we adopt the following simplified notation:

$$X_t=\left[\begin{array}{cc}{R}_t& \begin{array}{ccccccc}{v}_t& p_t& p_t^{f_1}& p_t^{f_2}& p_t^{f_3}& p_t^{f_4}& b_t^v\end{array}\\ 0_{3,7}& I\end{array}\right]$$

(8)

Similar to [23], we describe the slippage velocity using an autoregressive model, modeling the slippage velocity as an exponentially decaying variable:

$${\dot{b}}_t^v(t)=-\alpha b_t^v+n_{bv}(t),n_{bv}(t)\sim \mathcal{N}\left(0_3,{\sum }^{b_t^v}\right)$$

(9)

The IMU provides the following measurements

$${\tilde{w}}_t=w_t+n_t^w,{\tilde{a}}_t=a_t+n_t^a$$

(10)

The measurement of foot position primarily relies on the angular measurements from joint encoders, and the joint encoder measurements are

$${\tilde{\theta }}_t^{f_i}={\theta }_t^{f_i}+n_t^{f_i}$$

(11)

Based on the IMU measurement model and the foot-end contact model, the system's dynamic model is:

$${\dot{R}}_t=R_t{\left({\tilde{w}}_t-n_t^w\right)}_{\times }$$

(12)

$${\dot{v}}_t=R_t\left({\tilde{a}}_t-n_t^a\right)+g$$

(13)

$${\dot{p}}_t=v_t$$

(14)

$${\dot{p}}_t^{f_i}=R_t{h}_{f_i}({\tilde{\theta }}_t^{f_i})n_t^{f_i}$$

(15)

Rewrite the above model in matrix form,

$$\begin{array}{c}{\displaystyle \frac{d}{dt}}{X}_t=\left[\begin{array}{cc}{R}_t{({\tilde{w}}_t)}_{\times }& \begin{array}{ccccccc}{R}_t{\tilde{a}}_t+g& v_t& 0_{3,1}& 0_{3,1}& 0_{3,1}& 0_{3,1}& -\alpha b_t^v\end{array}\\ 0_{3,1}& 0_{3,7}\end{array}\right]-X_t{n}_t^{\wedge }\\ \triangleq f_{u_t}\left(X_t\right)-X_t{n}_t^{\wedge }\end{array}$$

(16)

where $n_t\triangleq \mathrm{vec}\left(n_t^w,n_t^a,0,h_{f_1}({\tilde{\theta }}_t^{f_1})n_t^{f_1},h_{f_2}({\tilde{\theta }}_t^{f_2})n_t^{f_2},h_{f_3}({\tilde{\theta }}_t^{f_3})n_t^{f_3},h_{f_4}({\tilde{\theta }}_t^{f_4})n_t^{f_4}\right)$, It can be proven that the deterministic system dynamics $f_{u_t}(\bullet )$ satisfy the group affine property (3). According to Theorem 1, the left-invariant and right-invariant error dynamics are autonomous and evolve independently of the system state. The system's right-invariant error is:

$$\begin{array}{c}{\displaystyle \frac{d}{dt}}{\eta }_t^r=f_{u_t}\left({\eta }_t^r\right)-{\eta }_t^r{f}_{u_t}\left(I_d\right)+\left({\widehat{X}}_t{n}_t^{\wedge }{\widehat{X}}_t^{-1}\right){\eta }_t^r\\ :=g_{u_t}^r\left({\eta }_t^r\right)+{\left({\mathrm{Ad}}_{\widehat{X}}{n}_t\right)}^{\wedge }{\eta }_t^r\end{array}$$

(17)

Theorem 2 further explains the logarithmic linear property of invariant errors. If $A_t$is defined as

$$g_{u_t}^r\left(\exp (\xi )\right)\triangleq {\left(A_t\xi \right)}^{\wedge }+\mathcal{O}\left({\Vert \xi \Vert }^2\right)$$

(18)

Then the invariant error $\xi$ satisfies the following linear system:

$${\displaystyle \frac{d}{dt}}{\xi }_t=A_t{\xi }_t+{\mathrm{Ad}}_{\widehat{X}}{n}_t$$

(19)

Using first-order approximation to linearize $g_{u_t}^r(\bullet )$ yields:

$${\eta }_t^r=\exp \left({\xi }_t\right)\approx I_d+{\xi }_t^{\wedge }$$

(20)

Substituting into the expression for $g_{u_t}^r\left({\eta }_t^r\right)$, we drive:

$$g_{u_t}^r\left({\eta }_t^r\right)={\left(\left[\begin{array}{cccc}{0}_{3,3}& 0_{3,3}& 0_{3,15}& 0_{3,3}\\ {[g]}_{\times }& 0_{3,3}& 0_{3,15}& 0_{3,3}\\ 0_{3,3}& I_3& 0_{3,15}& 0_{3,3}\\ 0_{12,3}& 0_{12,3}& 0_{12,15}& 0_{3,3}\\ 0_{3,3}& 0_{3,3}& 0_{3,3}& -\alpha I_3\end{array}\right]{\xi }_t\right)}^{\wedge }$$

(21)

Based on these computations, the prediction step of the right-invariant extended Kalman filter (EKF) can be derived. The state estimate is denoted as ${\overline{X}}_t$, and the covariance matrix $P_t$ is obtained using the Riccati equation:

$${\displaystyle \frac{d}{dt}}{\widehat{X}}_t=f_{u_t}\left({\widehat{X}}_t\right)$$

(22)

$${\displaystyle \frac{d}{dt}}{P}_t=A_t{P}_t+P_t{A}_t^{{\rm T}}+{\overline{Q}}_t$$

(23)

where ${\overline{Q}}_t={\mathrm{Ad}}_{\widehat{X}}\mathrm{Cov}(n_t){\mathrm{Ad}}_{\widehat{X}}$.

3.2. Measurement Model

According to the forward kinematics of the quadruped robot, the position of the foot relative to the body coordinate system is given by:

$${}_{B}p_{f_i}=h_{f_i}({\theta }^{f_i})=R_{WB}^{{\rm T}}\left({}_{W}p_{f_i}-{}_{W}p\right)$$

(24)

The velocity of the foot is composed of the velocity generated by the joint rotation $v_{bj}^{f_i}$ and the velocity of the body motion $v_{be}^{f_i}$

$${}_{B}v_{bj}^{f_i}=J{\dot{\theta }}^{f_i}$$

(25)

$${}_{B}v_{be}^{f_i}={}_{B}v+{[{}_{B}w]}_{\times }{}_{B}p_{f_i}$$

(26)

Transform $v_{bj}^{f_i}$ and $v_{be}^{f_i}$ into the world coordinate system $\left\{w\right\}$, we can obtain the foot velocity in the world coordinate system:

$${}_{W}v_{f_i}=R_t\left({}_{B}v_{bj}^{f_i}+{}_{B}v_{be}^{f_i}\right)=v+R_t\left(J{\dot{\theta }}^{f_i}+[{}_{B}w]\times {}_{B}p_{f_i}\right)$$

(27)

When the foot is in stable contact with the ground without slipping, ${}_{W}v_{f_i}=0$, that is:

$$v=-R_t\left(J{\dot{\theta }}^{f_i}+[{}_{B}w]\times {}_{B}p_{f_i}\right)$$

(28)

Assuming that the joint encoder is affected by Gaussian white noise with a covariance matrix of $R_{\theta }$, i.e. ${\tilde{\theta }}_t={\theta }_t+n_t^{\theta }$ we can derive the following observation model:

$$h_{f_i}({\tilde{\theta }}^{f_i})=R_t^{{\rm T}}\left({}_{W}p_{f_i}-{}_{W}p\right)+J({\tilde{\theta }}^{f_i})n_t^{\theta }$$

(29)

$$v=-R_t\left(J{\dot{\tilde{\theta }}}^{f_i}+{}_{B}w\times h_{f_i}({\tilde{\theta }}^{f_i})\right)+{\dot{R}}_{t}J({\tilde{\theta }}^{f_i})n_t^{\theta }$$

(30)

Therefore, when in stable contact, there exist the following two observations:

$$\left[\begin{array}{c}{h}_{f_i}({\tilde{\theta }}^{f_i})\\ 0\\ 1\\ \begin{array}{l}{0}_{i-1,1}\\ -1\\ 0_{5-i,1}\end{array}\end{array}\right]=X_t^{-1}\left[\begin{array}{l}{0}_{3,1}\\ 0\\ 1\\ 0_{i-1,1}\\ -1\\ 0_{5-i,1}\end{array}\right]+\left[\begin{array}{c}J({\tilde{\theta }}^{f_1})n_t^{\theta }\\ 0_{7,1}\end{array}\right],\left[\begin{array}{c}-\left(J{\dot{\tilde{\theta }}}^{f_i}+{}_{B}w\times h_{f_i}({\tilde{\theta }}^{f_i})\right)\\ -1\\ 0_{5,1}\end{array}\right]=X_t^{-1}\left[\begin{array}{c}{0}_{3,1}\\ -1\\ 0_{5,1}\end{array}\right]+\left[\begin{array}{c}{\dot{R}}_{t}J({\tilde{\theta }}^{f_i})n_t^{\theta }\\ 0_{7,1}\end{array}\right],i=1,2,\mathrm{...},4$$

(31)

The first equation represents the observation of the foot position of the quadruped robot, denoted as $Y_t^{p_{f_i}}=X_t^{-1}{c}_t^{p_{f_i}}+N_t^{p_{f_i}}$ where $Y_t^{p_{f_i}}={\left[\begin{array}{cccccc}{h}_{f_i}^{{\rm T}}({\tilde{\theta }}^{f_i})& 0& 1& 0_{1,i-1}& -1& 0_{1,5-i}\end{array}\right]}^{{\rm T}}$,$c^{p_{f_i}}={\left[\begin{array}{cccccc}{0}_{1,3}& 0& 1& 0_{1,i-1}& -1& 0_{1,5-i}\end{array}\right]}^{{\rm T}}$,$N_t^{p_{f_i}}={\left[\begin{array}{cc}{\left(J({\tilde{\theta }}^{f_1})n_t^{\theta }\right)}^{{\rm T}}& 0_{1,7}\end{array}\right]}^{{\rm T}}$,$i=1,2,\mathrm{...},4$.The second equation represents the observation of the body velocity of the quadruped robot, denoted as $Y_t^{v_i}=X_t^{-1}{c}^{v_i}+N_t^{v_{f_i}}$, where $Y_t^{v_i}={\left[\begin{array}{ccc}-\left(J{\dot{\tilde{\theta }}}^{f_i}+{}_{B}w\times h_{f_i}({\tilde{\theta }}^{f_i})\right)& -1& 0_{1,5}\end{array}\right]}^{{\rm T}}$,$c^v={\left[\begin{array}{ccc}{0}_{1,3}& -1& 0_{1,5}\end{array}\right]}^{{\rm T}}$, $N_t^{v_{f_i}}={\left[\begin{array}{cc}{\left({\dot{R}}_{t}J({\tilde{\theta }}^{f_i})n_t^{\theta }\right)}^{{\rm T}}& 0_{1,7}\end{array}\right]}^{{\rm T}}$, $i=1,2,\mathrm{...},4$. Hartley [10] used position as the observation in bipedal robots, while Teng [16] used velocity as the observation. In practice, these two observations can be considered together. Using the moving pose as an example, the observation error is defined as:

$$Z_t^{p_{f_i}}={\widehat{X}}_t{Y}_t^{p_{f_i}}-c_t^{p_{f_i}}$$

(32)

Substituting $Y_t^{p_{f_i}}=X_t^{-1}{c}_t^{p_{f_i}}+N_t^{p_{f_i}}$ into (32) , we have

$$\begin{array}{c}{Z}_t^{p_{f_i}}={\widehat{X}}_t\left(X_t^{-1}{c}_t^{p_{f_i}}+N_t^{p_{f_i}}\right)-c_t^{p_{f_i}}\\ ={\eta }_t{c}_t^{p_{f_i}}-c_t^{p_{f_i}}+{\widehat{X}}_t{N}_t^{p_{f_i}}\\ =\left[\begin{array}{c}{\xi }_t^p-{\xi }_t^{p^{f_i}}\\ 0_{7,1}\end{array}\right]+\left[\begin{array}{c}{\widehat{R}}_{t}J({\tilde{\theta }}^{f_i})n_t^{\theta }\\ 0_{7,1}\end{array}\right]\end{array}$$

(33)

Note that for each foot's data observation, except for the first row, all other elements are 0. Let $z_t^{p_{f_i}}$ be the term $Z_t^{p_{f_i}}$ where all 0 elements are removed:

$$z_t^{p_{f_i}}=\left[\begin{array}{cccccc}{0}_{3,3}& 0_{3,3}& I_3& 0_{3,3*(i-1)}& -I_3& 0_{3,3*(5-i)}\end{array}\right]{\xi }_t-{\widehat{R}}_{t}J({\tilde{\theta }}^{f_i})n_t^{\theta }$$

(34)

Combining all foot position observations, we have:

$$\left[\begin{array}{c}{z}_t^{p_{f_1}}\\ z_t^{p_{f_2}}\\ z_t^{p_{f_3}}\\ z_t^{p_{f_4}}\end{array}\right]=\left[\begin{array}{cccccccc}{0}_{3,3}& 0_{3,3}& I_3& -I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}\\ 0_{3,3}& 0_{3,3}& I_3& 0_{3,3}& -I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}\\ 0_{3,3}& 0_{3,3}& I_3& 0_{3,3}& 0_{3,3}& -I_3& 0_{3,3}& 0_{3,3}\\ 0_{3,3}& 0_{3,3}& I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}& -I_3& 0_{3,3}\end{array}\right]{\xi }_t+\left[\begin{array}{c}{\widehat{R}}_{t}J({\tilde{\theta }}^{f_1})n_t^{\theta }\\ {\widehat{R}}_{t}J({\tilde{\theta }}^{f_2})n_t^{\theta }\\ {\widehat{R}}_{t}J({\tilde{\theta }}^{f_3})n_t^{\theta }\\ {\widehat{R}}_{t}J({\tilde{\theta }}^{f_4})n_t^{\theta }\end{array}\right]$$

(35)

Similarly, for velocity observations, we have:

$$\left[\begin{array}{c}{z}_t^{v_1}\\ z_t^{v_2}\\ z_t^{v_3}\\ z_t^{v_4}\end{array}\right]=\left[\begin{array}{cccccccc}{0}_{1,3}& I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}\\ 0_{1,3}& I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}\\ 0_{1,3}& I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}\\ 0_{1,3}& I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}\end{array}\right]{\xi }_t+\left[\begin{array}{c}{[{\tilde{w}}_t]}_{\times }J({\tilde{\theta }}^{f_1})n_t^{\theta }\\ {[{\tilde{w}}_t]}_{\times }J({\tilde{\theta }}^{f_2})n_t^{\theta }\\ {[{\tilde{w}}_t]}_{\times }J({\tilde{\theta }}^{f_3})n_t^{\theta }\\ {[{\tilde{w}}_t]}_{\times }J({\tilde{\theta }}^{f_4})n_t^{\theta }\end{array}\right]$$

(36)

By merging velocity observations with position observations, we get:

$$\left[\begin{array}{c}{z}_t^{p_{f_1}}\\ z_t^{p_{f_2}}\\ z_t^{p_{f_3}}\\ \begin{array}{l}{z}_t^{p_{f_4}}\\ z_t^{v_1}\\ z_t^{v_2}\\ z_t^{v_3}\\ z_t^{v_4}\end{array}\end{array}\right]=\left[\begin{array}{cccccccc}{0}_{1,3}& 0_{3,3}& I_3& -I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}\\ 0_{1,3}& 0_{3,3}& I_3& 0_{3,3}& -I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}\\ 0_{1,3}& 0_{3,3}& I_3& 0_{3,3}& 0_{3,3}& -I_3& 0_{3,3}& 0_{3,3}\\ 0_{1,3}& 0_{3,3}& I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}& -I_3& 0_{3,3}\\ 0_{1,3}& I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}\\ 0_{1,3}& I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}\\ 0_{1,3}& I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}\\ 0_{1,3}& I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}\end{array}\right]{\xi }_t+\left[\begin{array}{c}{\widehat{R}}_{t}J({\tilde{\theta }}^{f_1})n_t^{\theta }\\ {\widehat{R}}_{t}J({\tilde{\theta }}^{f_2})n_t^{\theta }\\ {\widehat{R}}_{t}J({\tilde{\theta }}^{f_3})n_t^{\theta }\\ {\widehat{R}}_{t}J({\tilde{\theta }}^{f_4})n_t^{\theta }\\ {[{\tilde{w}}_t]}_{\times }J({\tilde{\theta }}^{f_1})n_t^{\theta }\\ {[{\tilde{w}}_t]}_{\times }J({\tilde{\theta }}^{f_2})n_t^{\theta }\\ {[{\tilde{w}}_t]}_{\times }J({\tilde{\theta }}^{f_3})n_t^{\theta }\\ {[{\tilde{w}}_t]}_{\times }J({\tilde{\theta }}^{f_4})n_t^{\theta }\end{array}\right]$$

(37)

3.3. Considering Unstable Contact and Slipping

The above model assumes that all four feet are in stable contact with the ground. In practice, however, the quadruped robot’s feet may be airborne during walking, and on many uneven or slippery terrains, the feet may make unstable contact. When the feet are airborne or in unstable contact, the foot positions calculated by the state-space model do not conform to this assumption. To address this issue, we use the foot contact probability model proposed in [9] to determine the probability of stable contact between the feet and the ground.

$$P_k\left(S_l=1|{\mathbf{f}}_k^l\right)={\displaystyle \frac{1}{1+\exp \left(-{\beta }_1{f}_{z,l}^k-{\beta }_2\right)}}$$

(38)

Using this contact probability, we can combine the basic velocities generated by each leg to obtain the body velocity and use it as an observation of the body velocity.

$${}_{B}v={\displaystyle \frac{{\displaystyle {\sum }_{l\in C}{P}_k\left(S_l=1|{\mathbf{f}}_k^l\right){}_B\dot{p}_{f_i}}}{{\displaystyle {\sum }_{l\in C}{P}_k\left(S_l=1|{\mathbf{f}}_k^l\right)}}}$$

(39)

Considering the sliding velocity of the foot, we have

$$v_t=R_t{}_{B}v_t+b_t^v+n_t^v$$

(40)

where $n_t^v$ represents the error in the body velocity, which includes the error generated by each foot velocity observation. To correctly compute the covariance matrix ${\sum }_t^v$ of $n_t^v$ consider the consistency between each foot velocity ${}_B\dot{p}_{f_i}$ and the ground impact. For each axis $r\in \left\{x,y,z\right\}$, the variance at a given moment can be computed as:

$${\sigma }_r^2(k)={\sigma }_0^2+{\left({\gamma }_1\mathrm{std}{\left[{}_B\dot{p}_{f_i\in C}(k)\right]}_C+(1-{\gamma }_1){\gamma }_2\left|\Delta {\overline{f}}_z^k\right|\right)}^2$$

(41)

where $\left|\Delta {\overline{f}}_z^k\right|={\displaystyle \frac{1}{\dim (C)}}{\displaystyle \sum _{l\in C}\left|f_{z,l}^k-f_{z,l}^{k-1}\right|}$ is the average absolute difference in the ground reaction force in the z axis between the current and previous contact times. ${\sigma }_0^2$ is the baseline standard deviation of velocity, $\mathrm{std}\left[{}_B\dot{p}_{f_i\in C}\right]$ is the standard deviation of stance phase foot velocity contribution for the r-th elements. ${\gamma }_1$ is a factor that balances consistency and the impact of collision forces, while ${\gamma }_2$ is a normalization factor to normalize the typical velocity error difference $\left|\Delta {\overline{f}}_z\right|$ at the same moment.

For the observation of foot position, since the foot may be in a hovering state or in unstable contact, the estimated foot position obtained through the state-space model might be unreliable. By dynamically adjusting the noise covariance of the foot contact using the formula below, the impact of unstable or non-contact states on state estimation can be mitigated:

$${\sum }_t^{p_{f_i}}\left(k\right)=\left(1+L\left(1-P_k\left(S_l=1|{\mathrm{f}}_k^l\right)\right)\right){\sum }_t^{p_{f_i}}\left(k\right)$$

(42)

where L is a sufficiently large scalar. The final observation model accounting for unstable contact and sliding is as follows:

$$\left[\begin{array}{c}{z}_t^{p_{f_1}}\\ z_t^{p_{f_2}}\\ z_t^{p_{f_3}}\\ \begin{array}{l}{z}_t^{p_{f_4}}\\ z_t^v\end{array}\end{array}\right]=\left[\begin{array}{cccccccc}{0}_{1,3}& 0_{3,3}& I_3& -I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}\\ 0_{1,3}& 0_{3,3}& I_3& 0_{3,3}& -I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}\\ 0_{1,3}& 0_{3,3}& I_3& 0_{3,3}& 0_{3,3}& -I_3& 0_{3,3}& 0_{3,3}\\ 0_{1,3}& 0_{3,3}& I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}& -I_3& 0_{3,3}\\ 0_{1,3}& I_3& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& 0_{3,3}& I_3\end{array}\right]{\xi }_t+\left[\begin{array}{c}{n}_t^{p_{f_1}}\\ n_t^{p_{f_2}}\\ n_t^{p_{f_3}}\\ n_t^{p_{f_4}}\\ n_t^v\end{array}\right]$$

(43)

An observation equation of the form $z_t=C{\xi }_t+V_t$ is obtained. Based on the definition of right-invariant error, the update steps for InEKF are as follows:

$$K_t=P_t{C}_t^{{\rm T}}\left(C_t{P}_t{C}_t^{{\rm T}}+N_t\right)$$

(44)

$$X_t^{+}=\mathrm{Exp}\left(K_t{z}_t\right){\widehat{X}}_t$$

(45)

$$P_t^{+}=\left(I-K_t{C}_t\right)P_t$$

(46)

4. Experimental Results and Analysis

To validate the effectiveness of the proposed algorithm, experiments were conducted on three different terrains using the Jueying Mini quadruped robot. The Jueying Mini quadruped robot is equipped with an IMU and 12 joint encoders, all with a measurement frequency of 166 Hz. The IMU provides measurements of angular velocity and linear acceleration, while the joint encoders provide measurements of joint angles, joint torques, and joint angular velocities. Table 1 presents the corresponding noise parameters. The experimental terrains include typical outdoor rugged slopes, shallow grass, and deep grass, as shown in the Figure 1.

4.1. Contact Probability Calculation

We use the phase information of the foot-end as the true value of the contact state between the foot-end and the ground. The parameters ${\beta }_1$, ${\beta }_2$ in Equation (38) are used to compute the contact force between the foot-end and the ground by minimizing the energy:

$${\mathrm{f}}_k^l=-\left(J^{{\rm T}}({\theta }^{f_l})\left({\tau }_l-h_l({\theta }^{f_l},{\dot{\theta }}^{f_l},{}_{b}g)\right)\right)$$

(47)

Considering that the foot-end starts to touch the ground and prepares to leave the ground, the position of the foot-end always undergoes some displacement. An appropriate adjustment of the parameter $\beta$ reduces the sensitivity of the model to input variation, thereby making the fluctuation of the foot-end contact probability smoother. Figure 2 illustrates the scenario where the quadruped robot’s foot slips during contact with the ground，and Figure 3 presents the final results, showing the probability model for the right front foot slipping

4.2. Algorithm Evaluation

Due to the absence of a motion capture system in outdoor areas, we used the LiDAR SLAM method, which performs relatively well in outdoor environments, with LIO-SAM’s computed pose information serving as the ground truth. The experimental results are shown in the figures. Figure 4 presents the trajectory estimates results of various methods for the quadruped robot across three different terrains. We compared a total of five methods: the traditional quaternion-based Kalman filter method (Q-EKF)[8], InEKF with only velocity updates[16], InEKF with only position updates, and the proposed method considering and not considering velocity bias. For the methods [16] and [10], when the foot is in the contact phase, it is determined to be in stable contact with the ground, while the remaining methods employ the proposed contact probability judgment and covariance adjustment strategy. The results from the figure show that the proposed method is closer to the true trajectory.

The trajectories of different methods under various terrains were evaluated using the EVO tool, with the results shown in Table 2. The evaluation metrics include Absolute Trajectory Error (ATE) and Relative Pose Error (RPE), where ATE and RPE measure position error (in meters) and rotation error (in radians), respectively.

Table 2 shows that proposed method (PM with Vel Bias) outperforms other methods in ATE and RPE across all terrains, especially on rugged slopes and shallow grass, where its position error (ATE) and attitude error (RPE) are the lowest, at 0.4572 meters and 0.0345 radians (for rugged slopes position error) and 0.3431 meters and 0.0367 radians (for shallow grass position error), respectively. This indicates that this method offers higher accuracy and robustness in scenarios with velocity bias compensation. In contrast, InEKF with Vel update shows larger errors in most terrains, particularly in deep grass, where the ATE reaches 3.3818 meters and RPE is 1.3146 radians, demonstrating its sensitivity to errors in complex terrains. However, the method shows good stability and accuracy in RPE rotational error performance across various terrains, particularly on shallow grass and rugged slopes. These results indicate that velocity observation plays a crucial role in maintaining low rotational errors. Combining velocity observations with other methods (such as position observation) holds promise for achieving more robust attitude estimation. PM without Vel Bias also shows relatively stable performance in shallow grass and deep grass, especially in position error, suggesting that the strategy of removing velocity bias can be beneficial for trajectory estimation accuracy under certain terrain conditions. The QEKF method performs between other methods but has the lowest rotation error (RPE) on shallow grass, at 0.0367 radians, indicating its advantage in handling certain types of attitude changes. Overall, the experimental results show that the proposed method provides the best performance in most cases.

5. Conclusions

To address the state estimation drift issue caused by unstable foot contact in non-stationary terrains for quadruped robots, we propose an improved Invariant Extended Kalman Filter (InEKF) method. This method models foot-end sliding as a bias term in body velocity and integrates both foot-end velocity and position observations into the observation equation. By using foot-end contact probability assessment and adaptive covariance adjustment strategies, it effectively improves the state estimation accuracy of quadruped robots in complex outdoor environments. Experimental results show that, compared to several existing methods(such as Q-EKF and InEKF with single observation update), our method exhibits lower Root Mean Square Error (RMSE) across multiple unstable terrains and demonstrates significant advantages in position and attitude estimation. This method offers a new approach to the state estimation problem of quadruped robots on non-stationary terrains and suggests further optimization of the filtering algorithm for application in more complex environments in the future.