Multi-Contact Geometric Yaw Correction

• Multi-contact geometric yaw correction is a method that leverages fixed geometric relationships among multiple stance contacts to constrain and correct yaw drift.
• It fuses joint-torque-based contact selection, forward kinematics, and inertial measurements to provide a robust, drift-free heading update mechanism.
• The approach enhances state estimation in challenging environments, proving effective across various robotic platforms in GPS-denied and visually impaired conditions.

Multi-contact geometric yaw correction is a method for constraining and correcting the yaw (heading) estimate of legged robots using geometric relationships between multiple stance contacts, leveraging only proprioceptive sensing. This approach directly counters the unbounded yaw drift that arises in dead-reckoned state estimators relying on IMU integration. By exploiting the fixed geometry imposed by multiple stationary contacts, the estimator fuses information from joint-torque-based contact selection, forward kinematics, and inertial measurements, yielding a robust heading update mechanism that does not require exteroceptive sensing such as cameras or LiDAR (Sun et al., 19 Feb 2026). Additionally, related geometric mechanics formulations provide a theoretical framework for quantifying yaw changes due to discrete contact switching, further informing the design and correctness of yaw correction strategies (Prasad et al., 2023).

1. Motivation and Problem Statement

Legged robot odometry based solely on IMU and joint sensors suffers from unconstrained heading (yaw) drift. While roll and pitch remain observable through gravity alignment in the IMU, the absence of absolute heading information allows the yaw estimate to wander unboundedly, accumulating integration errors over time. During phases where two or more feet are in fixed stance, their world-frame locations, recorded at touchdown, must remain stationary if there is no foot slip. The geometric relationships (relative vectors) between these fixed contact points form a drift-free reference, providing a direct cue to correct heading drift.

In such systems, the availability of multiple, reliable stance contacts creates geometric constraints that can be formalized mathematically and exploited algorithmically to achieve robust, drift-suppressing yaw correction (Sun et al., 19 Feb 2026).

2. Mathematical Formulation of Multi-Contact Geometric Yaw Correction

Let $\mathcal F_W$ denote the world frame and $\mathcal F_B$ the robot's body frame. At time $t_k$, the estimated pose is $(\mathbf p_{WB,k},\,\mathbf R_{WB,k})$, with orientation

$$\mathbf R_{WB,k} = R_z(\psi_k) R_y(\theta_k) R_x(\phi_k),$$

where $\psi_k$ is yaw, $\theta_k$ pitch, $\phi_k$ roll.

Define the contact set at $t_k$:

$$\mathcal C_k = \{ i : \text{contact detected for leg } i \}.$$

For each $i \in \mathcal C_k$,

• $\mathbf p^B_{\mathrm{ee},i,k}$: body-frame foot position via kinematics,
• $\mathbf c^W_i$: world-frame footfall position, fixed during stance.

For all unordered pairs $(i, j) \subset \mathcal C_k,\, i<j$:

• Body-frame vector: $$v^B_{ij,k} = \mathbf p^B_{\mathrm{ee},j,k} - \mathbf p^B_{\mathrm{ee},i,k}$$,
• World-frame vector: $v^W_{ij} = \mathbf c^W_j - \mathbf c^W_i$.

Tilt compensation removes roll/pitch from the body-frame vector:

$R_{rp,k} = R_y(\theta_k) R_x(\phi_k)$

$\bar v_{ij,k} = R_{rp,k} v^B_{ij,k}$

Project both vectors onto the $xy$-plane. The instantaneous yaw aligning $\bar v_{ij,k}$ to $v^W_{ij}$ is

$$\hat \psi_{ij,k} = \text{wrap} \big( \mathrm{atan2}((v^W_{ij})_y, (v^W_{ij})_x) - \mathrm{atan2}((\bar v_{ij,k})_y, (\bar v_{ij,k})_x) \big)$$

Aggregate pairwise yaw estimates using the circular mean:

$$\hat \psi_k = \mathrm{atan2} \left( \sum_{(i,j)} \sin \hat \psi_{ij,k},\; \sum_{(i,j)} \cos \hat \psi_{ij,k} \right)$$

The yaw error is $e_k = \text{wrap}(\hat \psi_k - \psi_k)$, and the correction is applied via a bounded gain $\alpha_k \in (0,1]$:

$$\psi_k \leftarrow \text{wrap}(\psi_k + \alpha_k e_k)$$

The gain $\alpha_k$ increases with contact stability, reaching unity in prolonged four-foot stance and returning to a minimal base value $\alpha_0$ when contacts are intermittent.

3. Contact Detection and Wrench-Based Selection

Reliable detection of stance contacts is critical. For each leg $i$, the contact force in the body frame is estimated from joint torques:

$$\mathbf f^B_{i,k} = \left( J_i(\mathbf q_{i,k}) J_i(\mathbf q_{i,k})^\top \right)^{-1} J_i(\mathbf q_{i,k}) \boldsymbol \tau_{i,k}$$

where $J_i$ is the $3 \times 3$ geometric Jacobian and $\boldsymbol \tau_{i,k}$ the measured joint torques.

Transform the force to the world frame:

$\mathbf f^W_{i,k} = R_{WB,k} \mathbf f^B_{i,k}$

A leg is declared in contact if its vertical force surpasses a threshold: $f^W_{i,k,z} \leq f_{\mathrm{th}}$. An entering contact updates the touchdown world location and triggers height correction.

This judicious selection of contacts ensures geometric constraints are anchored to genuine, load-bearing stance events, avoiding the corruption of the geometry by transient or unreliable contacts (Sun et al., 19 Feb 2026).

4. Algorithmic Implementation and Gain Scheduling

The method's workflow is summarized in the following steps:

1. IMU and Joint Data Acquisition: Read roll $\phi$, pitch $\theta$, gyro yaw rate; read joint angles $\mathbf q_i$, torques $\boldsymbol \tau_i$.
2. Contact Detection: For each leg, compute $\mathbf f^B_{i,k}$ and $\mathbf f^W_{i,k}$; if $f^W_{i,k,z} \leq f_{\mathrm{th}}$, register contact and, upon touchdown, record $\mathbf c^W_i$ with height correction.
3. Yaw Correction (when $|\mathcal C_k| \geq 2$):
   • If all legs are in contact, ramp $\alpha_k$ from a small value $\alpha_0$ to $1$ over $T_\psi$;
   • Otherwise, reset the ramp and set $\alpha_k = \alpha_0$.
   • Compute all unordered pairwise yaw estimates $\hat \psi_{ij,k}$ and aggregate by the circular mean to obtain $\hat \psi_k$.
   • Update yaw: $$\psi_k \leftarrow \mathrm{wrap}(\psi_k + \alpha_k e_k)$$.
4. Fallback: When less than two contacts, retain $\alpha_k = \alpha_0$, and no update is applied.
5. Proceed with Standard Estimation: Use $\psi_k$ as the current yaw for the rest of the estimator update (Sun et al., 19 Feb 2026).

This procedure achieves a dynamic tradeoff: conservatism during intermittent contacts (limit overcorrection) and aggressive correction in stable, full support.

5. Relation to Geometric Mechanics of Contact-Switching Gaits

The geometric mechanics formalism for contact-switching systems generalizes yaw correction to hybrid shape-spaces and discrete contact transitions. A hybrid configuration space $(\alpha, \beta)$, with $\alpha$ a continuous shape and $\beta$ the set of active contacts, yields a local connection $A_\beta(\alpha)$ mapping shape velocities to body twist. Upon switching from contact mode $\beta$ to $\beta'$, the yaw difference is encoded by:

$$dz^\theta_{\beta \rightarrow \beta'}(\alpha) = A^\theta_{\beta'}(\alpha) - A^\theta_\beta(\alpha)$$

Accrued yaw over a cycle of $N$ switches is then:

$$\Theta_{\rm net} = \sum_{k=1}^N \int_{\alpha_k^-}^{\alpha_k^+} dz^\theta_{\beta_k \rightarrow \beta_{k+1}}(\alpha)$$

This framework, detailed in (Prasad et al., 2023), provides theoretical guarantees for aggregate rotational effects due to discrete gait transitions, offering planning and analysis tools for optimizing net yaw or analyzing residual drift in gaits involving nonholonomic contact switching.

6. Quantitative Performance and Efficacy

Empirical evaluation on four robot platforms demonstrates the efficacy of multi-contact geometric yaw correction in proprioceptive odometry (Sun et al., 19 Feb 2026). Key results include:

Platform	Loop Length (m)	Loop-Closure Error (m; %)	Vertical Error (m)
Astrall A	200	0.1638 (0.08%)	0.219
Astrall B	200	0.2264 (0.11%)	0.199
Astrall C	700	7.68 (1.1%)	0.540
Unitree Go2 EDU	120	2.2138 (1.8%)	< 0.1

During prolonged static stance, IMU-based yaw drift is effectively arrested, and heading remains locked. When IMU yaw integration is disabled entirely, kinematics-only heading retains residual errors of approximately $10^\circ$ per closed turn (short steps) and $30^\circ$ (long steps), demonstrating graceful degradation. This confirms the robustness of the multi-contact geometric principle for maintaining heading consistency under realistic proprioceptive-only conditions.

7. Significance and Applications

Multi-contact geometric yaw correction provides a robust solution for purely-proprioceptive legged state estimation. Its reliance on internal sensing enables drift suppression without exteroceptive aids, increasing reliability in visually degraded or GPS-denied environments. The method demonstrates compatibility with a range of platforms, including bipeds, quadrupeds, and wheel-legged robots, and is lightweight enough for real-time deployment. The fusion of wrench-based contact selection, kinematics, and geometric averaging further ensures resilience to transient contact fluctuations and offers a modular ingredient for advanced SLAM and navigation systems in legged robots (Sun et al., 19 Feb 2026). The theoretical connection to geometric mechanics yields pathways for systematic gait optimization targeting precise rotational outcomes (Prasad et al., 2023).