LIPM estimator

LIPM DCM Bias Estimator

1 One dimension: the basics

1.1 Definitions

Symbol	Meaning	Expression
ξ	divergent component of motion (DCM)
z	The zero moment point
b	the bias that we want to observe
ξb	biased DCM	ξ + b
ẑ	Measured z	z + wz
ξ̂b	Measured ξb	ξb + wξ
ω0	Natural frequency of the DCM
T	sampling time
wb	Brownian motion noise in the bias
wz	measurement noise in ẑ [A]  [A]  This covariance could be overestimated to take into account the modeling error due to centroidal angular momentum and variations of CoM height
wξ	measurement noise in ξ̂b
κb	standard deviation of wb
κz	standard deviation of wz
κξ	standard deviation of wξ

Table 1.1 Variable definition

State Variables	Inputs	Measurements	Standard deviations	Parameters
ξ, b	ẑ	ξ̂b	κb, κz, κξ	ω0, T

Table 1.2 State definion and use of available signals in the Kalman filter

1.2 Dynamics

The dynamics of the DCM is

(1.1) ξ̇  = ω₀(ξ − z)

This can be discetized to

(1.2) ξ_k + 1  = ξ_k + ω₀T(ξ_k − z_k)      = (1 + ω₀T)ξ_k − ω₀Tz_k      = (1 + ω₀T)ξ_k − ω₀T(ẑ_k − w^z)      = (1 + ω₀T)ξ_k − ω₀Tẑ_k + ω₀Tw^z

And the dynamics of b is

(1.3) b_k + 1  = b_k + v_k

Giving this dynamics

(1.4) ⎛⎜⎝ ξ_k + 1   b_k + 1 ⎞⎟⎠  = ⎛⎜⎝ 1 + ω₀T 0     0 1 ⎞⎟⎠⎛⎜⎝ ξ_k   b_k ⎞⎟⎠ + ⎛⎜⎝  − ω₀T   0 ⎞⎟⎠ẑ_k + ⎛⎜⎝ ω₀Tw^z   v_k ⎞⎟⎠

This means that the process covariance matrix is of the form

(1.5) Q  = ⎛⎜⎝ (ω₀Tκ^z)² 0     0 κ²_v ⎞⎟⎠

The measurement dynamics model is simply

(1.6) ξ̂^b_k  = ξ_k + b_k + w^ξ      = ( 1 1 )⎛⎜⎝ ξ_k   b_k ⎞⎟⎠ + w^ξ

Giving this covariance matrix

(1.7) R =  ( (κ^ξ)² )

1.3 Observability

Let’s analyze the observability

(1.8) O =  ⎛⎜⎜⎜⎝ ( 1 1 )   ( 1 1 )⎛⎜⎝ 1 + ω₀T 0     0 1 ⎞⎟⎠   ⋮ ⎞⎟⎟⎟⎠ = ⎛⎜⎜⎜⎝ 1 1     1 + ω₀T 1     ⋮ ⋮ ⎞⎟⎟⎟⎠

Which is full rank.

1.4 Initializing ξ̂ with measurement

if we initialize ξ̂₀ = ξ̂^b_k − b̂₀ the we have the state

(1.9) ⎛⎜⎝ ξ̂₀   b̂₀ ⎞⎟⎠  = ⎛⎜⎝ ξ₀   b₀ ⎞⎟⎠ + ⎛⎜⎝ b₀ − b̂₀ + w^ξ    − b₀ + b̂₀ ⎞⎟⎠      = ⎛⎜⎝ ξ₀   b₀ ⎞⎟⎠ + ⎛⎜⎝ e^b₀ + w^ξ    − e^b₀ ⎞⎟⎠

Giving the initial error covariance matrix

(2.1) P₀ =  ⎛⎜⎝ (κ^ξ)² + (κ^b₀)²  − (κ^b₀)²      − (κ^b₀)² (κ^b₀)² ⎞⎟⎠

2 2D case

This chapter is essentially a copy paste of the pervious one but

• changing to 2D
• adding the support for rotations

2.1 Definitions

These are the same except that 2d vectors replace states, inputs and measurements and 2 × 2 matrices replace the standard deviations. parameters remain scalars

Symbol	Meaning	Expression
ξ	divergent component of motion (DCM)
z	The zero moment point
b	the bias that we want to observe
ξb	biased DCM	ξ + b
ẑ	Measured z	z + wz
ξ̂b	Measured ξb	ξb + wξ
ω0	Natural frequency of the DCM
T	sampling time
wb	Brownian motion noise in the bias
wz	measurement noise in ẑ [B]  [B]  This covariance could be overestimated to take into account the modeling error due to centroidal angular momentum and variations of CoM height
wξ	measurement noise in ξ̂b
κb	standard deviation of wb
κz	standard deviation of wz
κξ	standard deviation of wξ

Table 2.1 Variable definition

State Variables	Inputs	Measurements	Variances	Parameters
ξ, b	ẑ	ξ̂b	κb, κz, κξ	ω0, T

Table 2.2 State definion and use of available signals in the Kalman filter

2.2 Dynamics

The dynamics of the DCM is

(2.2) ξ̇  = ω₀(ξ − z)

This can be discetized to

(2.3) ξ_k + 1  = ξ_k + ω₀T(ξ_k − z_k)      = (1 + ω₀T)ξ_k − ω₀Tz_k      = (1 + ω₀T)ξ_k − ω₀T(z_k − w^z)      = (1 + ω₀T)ξ_k − ω₀Tz_k + ω₀Tw^z

And the dynamics of b is

(2.4) b_k + 1  = b_k + v_k

Giving this dynamics

(2.5) ⎛⎜⎝ ξ_k + 1   b_k + 1 ⎞⎟⎠  = ⎛⎜⎝ (1 + ω₀T)I_2 × 2 0     0 I_2 × 2 ⎞⎟⎠⎛⎜⎝ ξ_k   b_k ⎞⎟⎠ + ⎛⎜⎝  − ω₀TI_2 × 2   0 ⎞⎟⎠ẑ_k + ⎛⎜⎝ ω₀Tw^z   v_k ⎞⎟⎠

This means that the process covariance matrix is of the form

(2.6) Q  = ⎛⎜⎝ (ω₀Tκ^z)(ω₀Tκ^z)^T 0     0 κ_vκ^T_v ⎞⎟⎠

The measurement dynamics model is simply

(2.7) ξ̂^b_k  = ξ_k + b_k + w^ξ      = ( I_2 × 2 I_2 × 2 )⎛⎜⎝ ξ_k   b_k ⎞⎟⎠ + w^ξ

Giving this covariance matrix

(2.8) R =  ( κ^ξκ^ξT )

2.3 Observability

Let’s analyze the observability

(2.9) O =  ⎛⎜⎜⎜⎝ ( I_2 × 2 I_2 × 2 )   ( I_2 × 2 I_2 × 2 )⎛⎜⎝ (1 + ω₀T)I_2 × 2 0     0 I_2 × 2 ⎞⎟⎠   ⋮ ⎞⎟⎟⎟⎠ = ⎛⎜⎜⎜⎝ I_2 × 2 I_2 × 2     (1 + ω₀T)I_2 × 2 I_2 × 2     ⋮ ⋮ ⎞⎟⎟⎟⎠

Which is full rank.

2.4 Initializing ξ̂ with measurement

if we initialize ξ̂₀ = ξ̂^b_k − b̂₀ the we have the state

(2.10) ⎛⎜⎝ ξ̂₀   b̂₀ ⎞⎟⎠  = ⎛⎜⎝ ξ₀   b₀ ⎞⎟⎠ + ⎛⎜⎝ b₀ − b̂₀ + w^ξ    − b₀ + b̂₀ ⎞⎟⎠      = ⎛⎜⎝ ξ₀   b₀ ⎞⎟⎠ + ⎛⎜⎝ e^b₀ + w^ξ    − e^b₀ ⎞⎟⎠

Giving the initial error covariance matrix

(2.11) P₀ =  ⎛⎜⎝ κ^ξκ^ξT + κ^b₀κ^b₀T  − κ^b₀κ^b₀T      − κ^b₀κ^b₀T κ^b₀κ^b₀T ⎞⎟⎠

2.5 Include rotations

What if the bias is actually linked to the orientation of the base? In other words,

(2.12) b_k  = R_kb_l, k

where R is the orientation of the robot in 2D amd b_l is the local drift. We model the local drift to be constant, i.e.

b_{l, k + 1}  = b_l, k + v_k

In this case the update is then

(2.13) b_k + 1  = R_k + 1R^T_kb_k + R_k + 1R^T_kv_k

By assuming that v_k is an isotropic noise we end up with this model

b_k + 1  = R_kR^T_k − 1b_k + v_k

Which gives

(2.14) ⎛⎜⎝ ξ_k + 1   b_k + 1 ⎞⎟⎠  = ⎛⎜⎝ (1 + ω₀T)I_2 × 2 0     0 R_k + 1R^T_k ⎞⎟⎠⎛⎜⎝ ξ_k   b_k ⎞⎟⎠ + ⎛⎜⎝  − ω₀TI_2 × 2   0 ⎞⎟⎠ẑ_k + ⎛⎜⎝ ω₀Tw^z   v_k ⎞⎟⎠

Note that in this case we consider that the rotation signal R is noiseless.

2.6 Consider CoM offset and ZMP coefficient

What if we can measure in the DCM dynamics an offset on the CoM and a coefficient to the ZMP ? In other words the dynamics of the DCM becomes

(2.15) ξ̇  = ω₀(ξ − κz + γ)

wherev α is the CoM offset and \strikeout off\xout off\uuline off\uwave offκ is the ZMP coefficient, they are considered known. This may happen in the presence of planned or measured contact forces (see equation (8) in [1]).

This can be discretized as

(2.16) ξ_k + 1  = ξ_k + ω₀T(ξ_k − κz_k + γ)

Which gives

(2.17) ⎛⎜⎝ ξ_k + 1   b_k + 1 ⎞⎟⎠  = ⎛⎜⎝ (1 + ω₀T)I_2 × 2 0     0 R_k + 1R^T_k ⎞⎟⎠⎛⎜⎝ ξ_k   b_k ⎞⎟⎠ + ⎛⎜⎝  − ω₀TκI_2 × 2 ω₀TI_2 × 2       0 0 ⎞⎟⎠⎛⎜⎝ ẑ_k   γ ⎞⎟⎠ + ⎛⎜⎝ ω₀Tw^z   v_k ⎞⎟⎠

Which is implemented in the code

\strikeout off\xout off\uuline off\uwave off

Bibliography

[1] Masaki Murooka, Kevin Chappellet, Arnaud Tanguy, Mehdi Benallegue, Iori Kumagai, Mitsuharu Morisawa, Fumio Kanehiro, Abderrahmane Kheddar. Humanoid Loco-Manipulations Pattern Generation and Stabilization Control. IEEE Robotics and Automation Letters, 6(3):5597—5604, 2021.

A Unit-testing code

We wish to generate a random trajectory for the dcm but with bounded values. Lets compute a stable desired derivative for the dcm and the corresponding desired zmp

(A.1) ξ̇^d  =  − λξ     ω₀(ξ − z^d)  =  − λξ     z^d  = ⎛⎝(λ)/(ω₀) + 1⎞⎠ξ

---

Document generated by eLyXer 1.2.5 (2013-03-10) on 2022-10-22T18:01:32.718000