The estimator is inside the legged_estimation package.
It is a linear Kalman filter assuming the quaternion comes from the IMU, and the IMU has no bias.
The measurement updates are feet positions, feet velocities, and feet height.
This means that the estimator works only on flat ground for short experiments, because the IMU bias build up will become unbearable
Equations
Given the following:
-
numContacts_ = $n$
-
dimContacts_$=3n$
-
numState_$=6+3n$
-
numObserve_$=6n+n=7n$
The state is:
$$x = \begin{bmatrix}
p\ v\ f
\end{bmatrix}
\in \mathbb{R}^{6+3n},
\quad
p\in\mathbb{R}^3 v\in\mathbb{R}^3,\; f=\begin{bmatrix}f_1\\ \vdots \\ f_n\end{bmatrix} f_i\in\mathbb{R}^3$$
Here $f_i$ is the world position of foot/contact $i$ (a contact point state).
The measurement is:
$$y = \begin{bmatrix}
p_s\ v_s\ h
\end{bmatrix}
\in \mathbb{R}^{7n},
\quad
p_s=\begin{bmatrix}p_{s,1}\\ \vdots\\ p_{s,n}\end{bmatrix}\in\mathbb{R}^{3n},\;
v_s=\begin{bmatrix}v_{s,1}\\ \vdots\\ v_{s,n}\end{bmatrix}\in\mathbb{R}^{3n},\;
h=\begin{bmatrix}h_1\\ \vdots\\ h_n\end{bmatrix}\in\mathbb{R}^{n}$$
Dynamics
From a_ identity plus a_.block(0,3)=dt I and b_ blocks:
$$A=
\begin{bmatrix}
I_3 & dt,I_3 & 0\\
0 & I_3 & 0\\
0 & 0 & I_{3n}
\end{bmatrix},
\qquad
B=
\begin{bmatrix}
\frac12 dt^2 I_3\\
dt, I_3\\
0
\end{bmatrix}$$
Prediction:
$$\begin{aligned}
p_{k|k-1} &= p_{k-1|k-1} + dt, v_{k-1|k-1} + \tfrac12 dt^2,a_k\\
v_{k|k-1} &= v_{k-1|k-1} + dt,a_k\\
f_{i,k|k-1} &= f_{i,k-1|k-1}\quad \forall i
\end{aligned}$$
Covariance prediction:
$$P_{k|k-1}=A P_{k-1|k-1} A^\top + Q$$
Measurement matrix $C$
It is build from two 3×6 blocks:
$$c_1=\begin{bmatrix}I_3 & 0\end{bmatrix},\qquad
c_2=\begin{bmatrix}0 & I_3\end{bmatrix}$$
Then for each contact $i$:
- position rows (first $3n$ rows):
c_.block(3*i, 0, 3, 6) = c1
- velocity rows (next $3n$ rows):
c_.block(3*(n+i),0,3,6)=c2
- height rows (last $n$ rows):
c_(2*dimContacts_+i, 6+3*i+2)=1
Then
c_.block(0, 6, dimContacts_, dimContacts_) = -I, i.e. only the position measurement depends on the foot states with a $-I$.
So $C$ implies the per-foot measurement equations.
Foot position constraint rows
For each foot $i$:
$$y^{(p)}_i = p - f_i + \nu^{(p)}_i$$
Stacked:
$$p_s = \mathbf{1}_n\otimes p \;-\; f \;+\; \nu_p$$
where $\mathbf{1}_n\otimes p$ means $p$ repeated $n$ times in a $3n$-vector.
Foot velocity constraint rows
For each foot $i$:
$$y^{(v)}_i = v + \nu^{(v)}_i$$
Stacked:
$$v_s = \mathbf{1}_n\otimes v + \nu_v$$
Because there is no $-I$ in the velocity rows on the foot states, the model is not $v - \dot{f_i}$. It literally measures base velocity replicated $n$ times, with contact gating controlling whether those rows matter.
Foot height rows
For each foot $i$, the last measurement row picks out the z-component of $f_i$:
$$h_i = (f_i)_z + \nu^{(h)}_i$$
Equivalently:
$$h = \begin{bmatrix}e_z^\top f_1 \ \vdots \ e_z^\top f_n\end{bmatrix} + \nu_h
\quad \text{with } e_z=[0,0,1]^\top$$
Putting it together:
$$y=
\begin{bmatrix}
p_s\ v_s\ h
\end{bmatrix}
C
\begin{bmatrix}
p\ v\ f
\end{bmatrix}
+
\nu$$
Kalman update equations
Innovation:
$${\tilde y_k = y_k - C x_{k|k-1}}$$
Innovation covariance:
$${S_k = C P_{k|k-1} C^\top + R}$$
Gain (implicit; code uses LU solvers):
$$K_k = P_{k|k-1} C^\top S_k^{-1}$$
State update:
$${x_{k|k}=x_{k|k-1}+K_k\tilde y_k}$$
Covariance update (the exact algebra in code):
$${P_{k|k} = (I - P_{k|k-1} C^\top S_k^{-1} C),P_{k|k-1}}$$
then:
$${P_{k|k}\leftarrow \tfrac12(P_{k|k}+P_{k|k}^\top)}$$
The estimator is inside the legged_estimation package.
It is a linear Kalman filter assuming the quaternion comes from the IMU, and the IMU has no bias.
The measurement updates are feet positions, feet velocities, and feet height.
This means that the estimator works only on flat ground for short experiments, because the IMU bias build up will become unbearable
Equations
Given the following:
numContacts_=dimContacts_numState_numObserve_The state is:
Here$f_i$ is the world position of foot/contact $i$ (a contact point state).
The measurement is:
Dynamics
From
a_identity plusa_.block(0,3)=dt Iandb_blocks:Prediction:
Covariance prediction:
Measurement matrix$C$
It is build from two 3×6 blocks:
Then for each contact$i$ :
c_.block(3*i, 0, 3, 6) = c1c_.block(3*(n+i),0,3,6)=c2c_(2*dimContacts_+i, 6+3*i+2)=1Then
$-I$ .
c_.block(0, 6, dimContacts_, dimContacts_) = -I, i.e. only the position measurement depends on the foot states with aSo$C$ implies the per-foot measurement equations.
Foot position constraint rows
For each foot$i$ :
Stacked:
where$\mathbf{1}_n\otimes p$ means $p$ repeated $n$ times in a $3n$ -vector.
Foot velocity constraint rows
For each foot$i$ :
Stacked:
Because there is no$-I$ in the velocity rows on the foot states, the model is not $v - \dot{f_i}$ . It literally measures base velocity replicated $n$ times, with contact gating controlling whether those rows matter.
Foot height rows
For each foot$i$ , the last measurement row picks out the z-component of $f_i$ :
Equivalently:
Putting it together:
Kalman update equations
Innovation:
Innovation covariance:
Gain (implicit; code uses LU solvers):
State update:
Covariance update (the exact algebra in code):
then: