# Invariant Filtering for Bipedal Walking
on Dynamic Rigid Surfaces with Orientation-based Measurement Model

Yuan Gao  Department of Mechanical Engineering
University of Massachusetts Lowell
Lowell, USA
<EMAIL_ADDRESS>Yan Gu  Department of Mechanical Engineering
University of Massachusetts Lowell
Lowell, USA
<EMAIL_ADDRESS>
##  I Introduction

Real-world applications of bipedal robot walking require accurate, real-time
state estimation. State estimation for locomotion over dynamic rigid surfaces
(DRS) [1], such as elevators, ships, public transport vehicles, and aircraft,
remains under-explored, although state estimator designs for stationary rigid
surfaces have been extensively studied [2, 3, 4]. Addressing DRS locomotion in
state estimation is a challenging problem mainly due to the nonlinear, hybrid
nature of walking dynamics [5, 6], the nonstationary surface-foot contact
points [7], and hardware imperfections (e.g., limited availability, noise, and
drift of onboard sensors).

Towards solving this problem, we introduce an Invariant Extended Kalman Filter
(InEKF) whose process and measurement models explicitly consider the DRS
movement and hybrid walking behaviors while respectively satisfying the group-
affine condition and invariant form. Due to these attractive properties, the
estimation error convergence of the filter is provably guaranteed for hybrid
DRS locomotion. The measurement model of the filter also exploits the
holonomic constraint associated with the support-foot and surface
orientations, under which the robot’s yaw angle in the world becomes
observable in the presence of general DRS movement. Experimental results of
bipedal walking on a rocking treadmill demonstrate the proposed filter ensures
the rapid error convergence and observable base yaw angle.

##  II Method

In this study, we provably expand the existing invariant extended Kalman
filter (InEKF) for legged locomotion on stationary surfaces [4] to explicitly
addressing the discrete walking behaviors (e.g., support-foot switching) and
the time-varying DRS movement. We choose to build the proposed filter design
upon the InEKF framework [8] because of its attractive properties in
guaranteeing error convergence. Specifically, the InEKF ensures that the error
dynamics are independent from the estimated state when the process and
measurement models are respectively group-affine and invariant, thus leading
to the provable error convergence even under large initial errors. In
addition, we form a new, invariant measurement model based on the alignment of
the orientations of the support foot and flat DRS. Combined with the position-
based invariant measurement model in [4], which is omitted for brevity, the
proposed orientation-based measurement model can render the base yaw angle
observable as long as the DRS is flat and does not remain perpendicular to the
gravity. The proposed filter design is briefly explained next.

Figure 1: A: Experimental setup. B: Procedure of obtaining the support-foot
velocity with Digit’s RGBD camera, ArUCo tags, and known treadmill motion. C:
Aligned normal vectors to the surface and support foot.

###  II-A Propagation for the Continuous-Time Subsystem

We choose to estimate the following variables: base position
$\mathbf{p}_{t}=[p_{x},p_{y},p_{z}]^{T}$, base velocity
$\mathbf{v}_{t}=[v_{x},v_{y},v_{z}]^{T}$, base orientation $\mathbf{R}_{t}\in
SO(3)$, and support-foot position $\mathbf{d}_{t}=[d_{x},d_{y},d_{z}]^{T}$.
These variables are commonly used in motion planning and controller design for
legged locomotion.

The state is expressed on the matrix Lie group $SE_{3}(3)$ as:

$\displaystyle\mathbf{X}_{t}$
$\displaystyle:=\begin{bmatrix}\mathbf{R}_{t}&\mathbf{v}_{t}&\mathbf{p}_{t}&\mathbf{d}_{t}\\\
\mathbf{0}_{1\times 3}&1&0&0\\\ \mathbf{0}_{1\times 3}&0&1&0\\\
\mathbf{0}_{1\times 3}&0&0&1\end{bmatrix}\in SE_{3}(3)$ (1)

The input $\mathbf{u}_{t}$ is defined as
$\mathbf{u}_{t}:=[\tilde{\boldsymbol{\omega}}_{t},\tilde{\mathbf{a}}_{t}^{T},(\tilde{\mathbf{v}}^{d}_{t})^{T}]^{T}.$
The vectors $\tilde{\boldsymbol{\omega}}_{t}\in\mathbb{R}^{3}$ and
$\tilde{\mathbf{a}}_{t}\in\mathbb{R}^{3}$ are the raw data returned by the
gyroscope and accelerometer of the IMU attached to the robot’s base (i.e.,
trunk), respectively. The vector $\tilde{\mathbf{v}}^{d}_{t}\in\mathbb{R}^{3}$
is the linear velocity of the foot-surface contact area obtained using the
robot’s RGBD camera, the ArUCo tags on the DRS, and the known surface pose in
the world. Here we assume that the surface pose is relatively accurately known
since real-world dynamic rigid platforms (e.g., ships) are commonly equipped
with motion monitoring systems that return such data.

The noisy continuous-time process model is given by:

$\displaystyle\frac{d}{dt}\mathbf{X}_{t}$
$\displaystyle=\begin{bmatrix}\mathbf{R}_{t}(\tilde{{\omega}}_{t})_{\times}&\mathbf{R}_{t}\tilde{\mathbf{a}}_{t}+\mathbf{g}&\mathbf{v}_{t}&\tilde{\mathbf{v}}^{d}_{t}\\\
\mathbf{0}_{3\times 3}&\mathbf{0}_{3\times 1}&\mathbf{0}_{3\times
1}&\mathbf{0}_{3\times 1}\end{bmatrix}+\mathbf{X}_{t}\boldsymbol{w}_{t},$ (2)
$\displaystyle=\mathbf{f}_{u_{t}}(\mathbf{X}_{t})+\mathbf{X}_{t}\boldsymbol{w}_{t}$

with
$\boldsymbol{w}_{t}:=([\mathbf{w}^{g}_{t},(\boldsymbol{w}^{a}_{t})^{T},\mathbf{0}_{1\times
3},(\boldsymbol{w}^{d}_{t})^{T}]^{T})^{\wedge}$. The vectors
$\mathbf{w}^{g}_{t}$, $\boldsymbol{w}^{a}_{t}$, and $\boldsymbol{w}^{d}_{t}$
are continuous Gaussian white noises associated with the measured angular
velocity and linear acceleration of the base as well as the contact-velocity
measurement, respectively. The vector $\mathbf{g}$ is the gravitational
acceleration. It can be proved that $\mathbf{f}_{u_{t}}$ meets the group-
affine property [8].

###  II-B Measurement Update for the Continuous-Time Subsystem

When the support foot remains a secured contact with the DRS, the normal
vector to the surface and that to the support foot are parallel (see Fig. 1C).
Based on this kinematic relationship, we introduce a new measurement model in
the right-invariant form as follows:

$\displaystyle\underbrace{\begin{bmatrix}{}^{b}\mathbf{R}_{f}\begin{bmatrix}\mathbf{0}_{2\times
1}\\\ 1\end{bmatrix}\\\ \mathbf{0}_{3\times
1}\end{bmatrix}}_{\mathbf{Y}_{t}}=\mathbf{X}_{t}^{-1}\underbrace{\begin{bmatrix}\mathbf{R}_{s}\begin{bmatrix}\mathbf{0}_{2\times
1}\\\ 1\end{bmatrix}\\\ \mathbf{0}_{3\times
1}\end{bmatrix}}_{\mathbf{d}_{t}}+\mathbf{V}_{t},$ (3)

where $\mathbf{R}_{s}\in SO(3)$ is the orientation of DRS in the world frame,
${}^{b}\mathbf{R}_{f}\in SO(3)$ is the orientation of the support foot in the
base frame, and $\mathbf{V}_{t}$ is the noise term accounting for the
uncertainty in the knowledge of the surface orientation.

Besides the orientation-based measurement model, the proposed filter also
includes the right-invariant, position-based measurment model as in [4].
Following the standard steps of the InEKF methodology [8], we obtain the
equations for the update step of the filter, which are omitted for brevity.

Figure 2: Estimation results of the existing [4] and proposed InEKF designs.

###  II-C Propagation at Jumps

When the swing foot strikes the DRS, the support and swing legs switch roles,
causing a sudden jump in the support-foot position $\mathbf{d}$. We express
the process model of the jump as:

$\displaystyle{\mathbf{X}_{t^{+}}}$
$\displaystyle=\begin{bmatrix}\mathbf{R}_{t}&\mathbf{v}_{t}&\mathbf{p}_{t}&{\mathbf{d}_{t}+\mathbf{R}_{t}\mathbf{h}_{d}(\mathbf{\tilde{q}}_{t})}\\\
\mathbf{0}_{1\times 2}&1&0&0\\\ \mathbf{0}_{1\times 2}&0&1&0\\\
\mathbf{0}_{1\times
2}&0&0&1\end{bmatrix}\text{exp}(\boldsymbol{w}^{\Delta}_{t}),$ (4)

where the noise vector
$\boldsymbol{w}^{\Delta}_{t}\in\mathbb{R}^{\text{dim}\boldsymbol{\mathfrak{g}}}$
is induced by the robot’s encoder noise. The variable $\mathbf{h}_{d}$ is the
new support-foot position just after a jump relative to the previous support-
foot position with respect to the base frame. The input to the jump map is the
joint angle data $\mathbf{\tilde{q}}_{t}$ provided by encoders.

Since the jump map meets the identity jump map condition for right-invariant
errors in [9], the right-invariant error does not jump. Hence, the Jacobian
matrix of the jump map is identity. If we use the Jacobian to propagate the
estimated covariance, then the estimated covariance does not jump either.

##  III Results

Figure 1A shows the overall experiment setup, which comprises a) a Motek
instrumented treadmill with a time-varying pitch angle
$\theta_{DRS}=3^{\circ}sin(1.5\pi t)$, b) the Digit bipedal humanoid robot
(developed by Agility Robotics), c) four ArUCo tags attached to the treadmill
surface, and d) a motion capture system that records the ground truth. Digit’s
on-board sensors used by the proposed filter are: a) IMU at the base, b) joint
encoders, and c) RGBD camera at the neck. The filter also utilizes Digit’s
default contact estimator to detect foot-landing events. The procedure to
obtain the support-foot velocity $\tilde{\mathbf{v}}^{d}_{t}$ is shown in Fig.
1B.

Figure 2 shows the estimation results from 100 trials of the previous
position-based InEKF [4] and our proposed filter. The results show that both
filter achieve rapid error convergence for the linear velocity and roll and
pitch angles of the base, even in the presence of large initial errors.
Although the base position is unobservable under both filters, the proposed
filter seems to maintain a relatively smaller error within the tested walking
period (i.e., 30 s). More importantly, thanks to the orientation-based
measurement model, the proposed filter ensures that the base yaw angles is
observable under the rocking DRS movement. In contrast, the yaw angle remains
unobservable under the previous InEKF design, as indicated by its large error
throughout the entire tested period.

## References

  * [1] A. Iqbal, Y. Gao, and Y. Gu, “Provably stabilizing controllers for quadrupedal robot locomotion on dynamic rigid platforms,” IEEE/ASME Transactions on Mechatronics, vol. 25, no. 4, pp. 2035–2044, 2020.
  * [2] M. Bloesch, M. Hutter, M. A. Hoepflinger, S. Leutenegger, C. Gehring, C. D. Remy, and R. Siegwart, “State estimation for legged robots-consistent fusion of leg kinematics and IMU,” Robotics, vol. 17, pp. 17–24, 2013.
  * [3] G. Bledt, M. J. Powell, B. Katz, J. Di Carlo, P. M. Wensing, and S. Kim, “MIT cheetah 3: Design and control of a robust, dynamic quadruped robot,” in Proc. of IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 2245–2252, 2018.
  * [4] R. Hartley, M. Ghaffari, R. M. Eustice, and J. W. Grizzle, “Contact-aided invariant extended kalman filtering for robot state estimation,” The International Journal of Robotics Research, vol. 39, no. 4, pp. 402–430, 2020\.
  * [5] Y. Gao and Y. Gu, “Global-position tracking control of multi-domain planar bipedal robotic walking,” in ASME Dynamic Systems and Control Conference, vol. 59148, p. V001T03A009, 2019.
  * [6] N. J. Kong, J. J. Payne, G. Council, and A. M. Johnson, “The salted kalman filter: Kalman filtering on hybrid dynamical systems,” Automatica, vol. 131, p. 109752, 2021.
  * [7] A. Iqbal, Z. Mao, and Y. Gu, “Modeling, analysis, and control of slip running on dynamic platforms,” ASME Letters in Dynamic Systems and Control, vol. 1, no. 2, p. 021007, 2021.
  * [8] A. Barrau and S. Bonnabel, “The invariant extended kalman filter as a stable observer,” IEEE Transactions on Automatic Control, vol. 62, no. 4, pp. 1797–1812, 2017.
  * [9] Y. Gao and Y. Gu, “Invariant extended kalman filtering for hybrid models of bipedal robot walking,” in Proc. of IFAC Modeling, Estimation, and Control Conference, 2021.