Inverse-Kinematics CKF for Legged Robot Odometry

• The method employs a third-degree spherical–radial cubature rule to integrate nonlinear dynamics and map latent Cartesian states from joint encoder data.
• It leverages an analytic inverse-kinematics model to mitigate encoder quantization noise, reducing velocity spikes from ~200% to ~25% of nominal values.
• Implemented in quadruped and wheel-legged systems, IKVel-CKF ensures stable proprioceptive odometry and accurate elevation tracking on challenging terrains.

Inverse-Kinematics Cubature Kalman Filtering (IKVel-CKF) is a nonlinear filtering approach designed to estimate foot-end velocities in legged robots by directly filtering the latent Cartesian foot state from joint encoder measurements, mitigating the impact of encoder quantization and noise. Deployed in contact-anchored proprioceptive odometry frameworks for quadruped and wheel-legged robots, IKVel-CKF enables robust stance contact detection and stable odometry without reliance on exteroceptive sensors, leveraging an exact analytic inverse-kinematics and differential kinematics measurement model (Sun et al., 19 Feb 2026).

1. State and Measurement Representation

For each robot leg $i$, the IKVel-CKF tracks a six-dimensional latent Cartesian state vector at time-step $k$,

$$x_{i,k} = \begin{bmatrix} r_{i,k} \ \dot r_{i,k} \end{bmatrix} = \begin{bmatrix} x_{i,k} \ y_{i,k} \ z_{i,k} \ v_{x,i,k} \ v_{y,i,k} \ v_{z,i,k} \end{bmatrix} \in \mathbb{R}^6,$$

where $r_{i,k}$ is the hip-to-foot position vector in the robot body frame, and $\dot r_{i,k}$ its derivative.

The measurement vector is the vector of motor encoder readings, comprising joint angles and joint angular rates,

$$z_{i,k} = \begin{bmatrix} q_{i,k} \ \dot{q}_{i,k} \end{bmatrix} \in \mathbb{R}^6,$$

with $q_{i,k} \in \mathbb{R}^3$ (three joint angles per leg) and $\dot{q}_{i,k}$ their time-derivatives.

2. Process and Measurement Models

IKVel-CKF applies a constant-velocity process model with additive Gaussian process noise,

$$x_{i,k} = f(x_{i,k-1}) + w_{i,k-1}, \quad w_{i,k-1} \sim \mathcal{N}(0, Q),$$

with time-update

$$f(x) = \begin{bmatrix} I_3 & \Delta t_k I_3 \ 0 & I_3 \end{bmatrix} x,$$

where $\Delta t_k = t_k - t_{k-1}$, clamped in implementation to prevent large integration steps.

The nonlinear measurement model maps the Cartesian foot state to the observable joint space using an analytic inverse-kinematics (IK) and differential kinematics procedure: $$z_{i,k} = h(x_{i,k}) + v_{i,k}, \quad v_{i,k} \sim \mathcal{N}(0, R),$$ where $h(\cdot)$ implements the following steps:

• Compute joint angles $\theta = (\theta_1, \theta_2, \theta_3)$ from $r$ via closed-form IK equations involving link lengths $L_{\text{hip}}, L_{\text{thigh}}, L_{\text{calf}}$.
• Compute the leg Jacobian $J(\theta)$ and map Cartesian velocities $v$ to joint rate predictions via $\dot{\theta} = J(\theta)^{-1} v$.
• Output $$h(x) = [\theta_1, \theta_2, \theta_3, \dot{\theta}_1, \dot{\theta}_2, \dot{\theta}_3]^\top$$.

3. Cubature Integration and Filtering Recursion

IKVel-CKF employs the third-degree spherical–radial cubature rule to perform Gaussian integrals through the nonlinear dynamics and measurement models. For state dimension $n = 6$, and covariance $P = S S^\top$ (Cholesky factorization), cubature points are constructed as: $$\chi^{(j)} = \hat{x} + \sqrt{n} S e_j, \quad \chi^{(j+n)} = \hat{x} - \sqrt{n} S e_j, \quad j = 1, \ldots, n,$$ with equal weights $w^{(m)} = 1/(2n)$.

The time-update and measurement-update recursion follows:

• Time-update:

$\chi^{(m)-}_k = f(\chi^{(m)}_{k-1})$

$$\bar{x}^-_k = \sum_{m=1}^{2n} w^{(m)} \chi^{(m)-}_k$$

$$P^-_k = Q + \sum_{m=1}^{2n} w^{(m)} (\chi^{(m)-}_k - \bar{x}^-_k)(\chi^{(m)-}_k - \bar{x}^-_k)^\top$$

• Measurement-update:

$Z^{(m)}_k = h(\chi^{(m)-}_k)$

$\bar{z}_k = \sum_{m=1}^{2n} w^{(m)} Z^{(m)}_k$

$$P_{zz} = R + \sum_{m=1}^{2n} w^{(m)} (Z^{(m)}_k - \bar{z}_k)(Z^{(m)}_k - \bar{z}_k)^\top$$

$$P_{xz} = \sum_{m=1}^{2n} w^{(m)} (\chi^{(m)-}_k - \bar{x}^-_k) (Z^{(m)}_k - \bar{z}_k)^\top$$

$K_k = P_{xz} P_{zz}^{-1}$

$\hat{x}^+_k = \bar{x}^-_k + K_k (z_k - \bar{z}_k)$

$P^+_k = P^-_k - K_k P_{zz} K_k^\top$

4. Noise Covariances and Filter Tuning

The process noise covariance $Q$ models the expected variability of foot-end acceleration, while the measurement noise covariance $R$ is selected based on the quantization and noise properties of the joint encoders, transformed through the IK Jacobian. The values are empirically chosen to trade off filter responsiveness for smoothing: $$Q = \mathrm{diag}(\sigma_r^2 I_3, \sigma_v^2 I_3), \quad R = \mathrm{diag}(\sigma_q^2 I_3, \sigma_{\dot{q}}^2 I_3)$$ with $\sigma_r, \sigma_v$ on the order of a few millimeters and centimeters/second, and $\sigma_q, \sigma_{\dot{q}}$ matching hardware encoder resolution and noise.

5. Implementation Considerations

To ensure computational reliability and filter stability in real-time robot deployments, several implementation features are integrated:

• Side-constraint: After each prediction/update, the lateral coordinate is re-signed so that $y \leftarrow s|y|$, reflecting known left/right leg assignment ($s \in \{\pm1\}$).
• Bounded $\Delta t$: Time increments exceeding a threshold cause $\Delta t$ to be clamped to zero, preventing large state jumps.
• Cholesky factorization: Covariance matrices remain symmetric positive definite, and Cholesky factors are used for numerical cubature point generation.
• Per-leg CKF reuse: A single CKF instance is reused by swapping cached $(x_i, P_i, t_i)$ for each leg, minimizing memory allocations.
• Foot contact selection and fusion: The CKF’s filtered Cartesian foot velocity replaces the raw forward-kinematics velocities used elsewhere in the odometry pipeline.

Pseudocode for one CKF cycle per leg outlines the above steps, ensuring consistency across prediction and correction phases, and outputting the velocity estimate to the higher-level odometry module.

6. Impact on Proprioceptive Odometry

IKVel-CKF significantly suppresses impulsive noise in hip–foot velocity estimates induced by encoder quantization and differentiation (spikes reduced from up to 200% of nominal magnitude to within ~25%). The short smoothing lag (milliseconds) is adequate for real-time stance detection. In experimental evaluations—across platforms including Astrall point-foot, wheel-legged, and Unitree Go2 EDU quadrupeds—the filter maintained odometry stability. In stair-climbing, it prevented elevation drift caused by erroneous footfall records (flat-ground simulation horizontal closure error changed marginally, e.g., 0.623 m → 0.616 m) and consistently suppressed spurious vertical bias during repeated impacts.

A summary of IKVel-CKF’s functional contributions is provided below:

Aspect	Effect (as reported)	(Sun et al., 19 Feb 2026)
Spike suppression	Reduces hip–foot velocity spikes from ~200% to ~25% nominal	Figs. 5–6
Odometry stability	Prevents persistent elevation bias due to false contact detection	Figs. 8–9
Horizontal closure error	Marginal impact (e.g., 0.623m → 0.616m flat-ground)	sec. 7
Height consistency	Preserves correct elevation within millimeters over long trajectories	sec. 7

IKVel-CKF thus operationalizes a compact, numerically stable, third-degree CKF under an analytic inverse-kinematics measurement model, directly addressing quantization effects and providing robust proprioceptive odometry on challenging terrain (Sun et al., 19 Feb 2026).