@@ -1,55 +1,67 @@ Base Velocity Estimator

Base Velocity Estimator

This is an extended Kalman filter using a measurement and a system model:

Assumptions

When foot is in contact with the ground and not slipping, $ \mathbf{v}_{\text{foot}}^w = 0 $
Known:
- Joint angles $ \boldsymbol{\theta} $, velocities $ \dot{\boldsymbol{\theta}} $
- Angular velocity $ \boldsymbol{\omega}^b $ from IMU
- Foot position relative to base from forward kinematics

Equations

Foot velocity from rigid body motion:

$$ \mathbf{v}_{\text{foot}}^w = \mathbf{v}_{\text{base}}^w + \boldsymbol{\omega}^w \times \mathbf{r}_{\text{foot}}^w + \mathbf{R}_b^w \mathbf{J}(\boldsymbol{\theta}) \dot{\boldsymbol{\theta}} $$

When the foot is on the ground:

$$ \mathbf{v}_{\text{base}}^w = - \boldsymbol{\omega}^w \times \mathbf{r}_{\text{foot}}^w - \mathbf{R}_b^w \mathbf{J}(\boldsymbol{\theta}) \dot{\boldsymbol{\theta}} $$

Transform to base frame:

$$ \mathbf{v}_{\text{base}}^b = \mathbf{R}_w^b \mathbf{v}_{\text{base}}^w $$

Final base velocity in base frame:

$$ \mathbf{v}_{\text{base}}^b = - \boldsymbol{\omega}^b \times \mathbf{r}_{\text{foot}}^b - \mathbf{J}(\boldsymbol{\theta}) \dot{\boldsymbol{\theta}} $$

System Model

Updated using integrated IMU acceleration:

$$ \mathbf{v}_{\text{system}} = \sigma_0^t a_t $$

Assumptions

When the foot is in contact with the ground and there's no slippage:
$$\mathbf{v}_{\text{foot}}^w = \mathbf{0}$$
Known from sensors:
- Joint angles $\boldsymbol{\theta}$ and velocities $\dot{\boldsymbol{\theta}}$ (from motor encoders)
- Angular velocity $\boldsymbol{\omega}^b$ (from IMU)
- Foot position relative to the base $\mathbf{r}_{\text{foot}}^b$ (computed by forward kinematics)

Equations

The velocity of the foot is composed of:
- Velocity due to base motion:
  $$\mathbf{v}_0 = \mathbf{v}_{\text{base}}^w + \boldsymbol{\omega}^w \times \mathbf{r}_{\text{foot}}^w$$
- Velocity from joint motion:
  $$\mathbf{v}_1 = \mathbf{J}(\boldsymbol{\theta}) \dot{\boldsymbol{\theta}}$$
- Total foot velocity in world frame:
  $$\mathbf{v}_{\text{foot}}^w = \mathbf{v}_{\text{base}}^w + \boldsymbol{\omega}^w \times \mathbf{r}_{\text{foot}}^w + \mathbf{R}_{b}^w \mathbf{J}(\boldsymbol{\theta}) \dot{\boldsymbol{\theta}}$$
If the foot is in contact with ground:
$$\mathbf{v}_{\text{foot}}^w = \mathbf{0}$$

Then:

$$\mathbf{v}_{\text{base}}^w = - \boldsymbol{\omega}^w \times \mathbf{r}_{\text{foot}}^w - \mathbf{R}_{b}^w \mathbf{J}(\boldsymbol{\theta}) \dot{\boldsymbol{\theta}}$$
Transform to base frame:
$$\mathbf{v}_{\text{base}}^b = \mathbf{R}_{w}^b \mathbf{v}_{\text{base}}^w$$

Which gives:

$$\mathbf{v}_{\text{base}}^b = - \boldsymbol{\omega}^b \times \mathbf{r}_{\text{foot}}^b - \mathbf{J}(\boldsymbol{\theta}) \dot{\boldsymbol{\theta}}$$

System Model

The EKF system model predicts base velocity by integrating acceleration from the IMU, which unfortunately tends to drift over time:

$$\mathbf{v}_{\text{system}} = \sum_{t=0}^T a_t \Delta t$$

This system model and the measurement model are fused in the EKF to get a more stable and accurate base velocity estimate.

Using Neural Network

It's also possible to estimate the base velocity using a deep neural network. By feeding the network with the past few observations as input and training it to predict the current base velocity, you can often achieve even better accuracy than model-based methods.

No need for complex architectures — even a simple MLP (Multi-Layer Perceptron) can converge quickly and perform well!