# Direct Computing on Control Capability for Linear
Continuous-time Systems Based on Hurwitz Matrix ††thanks: Work supported by
the National Natural Science Foundation of China (Grant No. 61273005)

Mingwang Zhao  Information Science and Engineering School, Wuhan University of
Science and Technology, Wuhan, Hubei, 430081, China
Tel.: +86-27-68863897
Work supported by the National Natural Science Foundation of China (Grant No.
61273005)

###### Abstract

In this paper, based on the controllable canonical form and the Hurwitz matrix
of the Hurwitz stability criterion, an analytical volume computing method for
the smooth controllability zonotope for the linear continuous-time(LCT)
systems, without of help of the eigenvalue computing of the systems, is
presented. And then, the computing method is generlized to the volume
computing of the controllability ellipsoid of the LCT systems. Because the
controllability zonotope and ellipsoid are directly related to control
capability and their volumes are the main index describing the control
capability, the new volume computing methods proposed in this paper can help
greatly the computing, analysis and optimization of the control capability of
LCT systems.

###### keywords:

control capability , controllability region , smooth geometry , volume
computation , shape factor , continuous-time systems , state controllability ,
state reachability

††journal: Journal Name

##  1 Introduction

In recent years, based on the concepts and criteria about the state
controllability of linear systems, Zhao has done a series of new works on
control ability and efficiency of the input variables to the state variables
and the state space, and then many novel concepts, analysis and computing
methods about that are putforth. In fact, the so-called control ability and
efficiency, named as the control capability, is a very key concept in the
control theory and engineering and can help us to understand, analyze,
optimize and design the open-loop contrlled plants, controller and the closed-
loop control systems in many aspects. The control capability, so to speak, is
a solid foundation of the future delvelopment of the control theory and
engineering.

In Zhao’s series of works, main contributions are:

  1. 1.

The controllability zonotope of the linear discrete-time(LDT) systems, that
is, the controllability region under unit input variables
$\left(\left\|u_{t}\right\|_{\infty}\leq 1\right)$, is defined firstly for
decribing the control capability with the time-optimality, and then the
relations among the control capability of the open-loop controlled plants,
control Strategy Space of the controller, and the performance of the closed-
loop control systems in paper [7];

  2. 2.

The concept on the control capability is generlized to the linear continuous-
time(LCT) systems and then smooth controllability zonotope for the LCT systems
is defined [8].

  3. 3.

In papers [6], [8], and [9], many effective volume computing methods for the
controllability zonotopes are proven, such as,

     * •

two recrusive computing equations for the finite-time controllability zonotope
(FTCZ) for LDT systems which all eigenvelues satisfy that
$\lambda_{i}\in[0,1)$ [6].

     * •

the analytical computing equation for the infinite-time controllability
zonotope (IFTCZ) for LDT systems which all eigenvelues satisfy that
$\lambda_{i}\in[0,1)$ [6].

     * •

the analytical computing equation for the IFTCZ for LCT systems which all
eigenvelues satisfy that $\lambda_{i}\in(-\infty,0)$ [8].

     * •

the analytical computing equation for the FTCZ for LDT systems and LCT systems
which all eigenvelues satisfy that $\lambda_{i}\in[0,1)$ [8].

  4. 4.

The analytical factors descrbing the control capability of the LDT systems are
desconstructed from the analytical volume-computing equation of the
controllability zonotope and can be very helpful for the analyzing and
opyimizing the control capability [5].

  5. 5.

The controllability ellipsoids the LDT systems, that is, the controllability
region under unit-total-energy input variables
$\left(\int_{0}^{T}\left\|u_{t}\right\|_{2}^{2}\leq 1\right)$, is defined for
decribing the control capability with the energy-optimality, and then above
works for the controllability capability with the time-optimality are
generlized to the control capability with the energy-optimality in paper [4].

In this paer, based on the controllability canonical form (CCF) and the
Hurwitz matrix of the Hurwitz stability criterion, a directly analytical
volume-computing method for the IFTCZ for the LCT systems and it will greatly
improve computing efficiency and will be helpful the computing, anlyzing and
opyomizing of the control capability.

##  2 Definition on the Control Capability with Time Attribute

In general, the LCT Systems can be modelled as follows:

$\dot{x}_{t}=Ax_{t}+Bu_{t},\quad x_{t}\in R^{n},u_{t}\in R^{r},$ (1)

where $x_{t}$ and $u_{t}$ are the state variable and input variable,
respectively, and matrices $A\in R^{n\times n}$ and $B\in R^{n\times r}$ are
the state matrix and input matrix, respectively [3] [1]. To investigate the
controllability of the LCT systems (1), the controllability matrix and the
controllability Grammian matrix can be defined as follows

$\displaystyle P_{n}$ $\displaystyle=\left[B,AB,\dots,A^{n-1}B\right]$ (2)
$\displaystyle G_{T}$
$\displaystyle=\int_{0}^{T}e^{At}B\left(e^{At}B\right)^{T}\textnormal{s}t$ (3)

That the rank of the matrix $P_{n}$/$G_{T}$ is $n$, that is, the dimension of
of the state space the systems (1), is the well-known criterion on the state
controllability.

In papers [6], [4] and [8], two smooth geometry generated by the matrix pair
$(A,B)$, named as the controllability zonotope and controllability eiilipsoid,
can be defined as follows

$\displaystyle R_{a}(T)$
$\displaystyle=\left\\{x:x=\int_{0}^{T}e^{At}Bz_{t}\textnormal{d}t,\;\;\forall
z_{t}:\|z_{t}\|_{\infty}\leq 1\right\\}$ (4) $\displaystyle R_{b}(T)$
$\displaystyle=\left\\{x:x=\int_{0}^{T}e^{At}Bz_{t}\textnormal{d}t,\;\;\forall
z_{t}:\|z_{t}\|_{2}\leq 1\right\\}$ (5)

where the smooth geometry $R_{a}(T)$ and $R_{b}(T)$ are a convex
$n$-dimensional geometry with the origin symmetry.

In fact, the controllability regions defined as Eqs. (4) and (5) are the
reachability regions and can be regarded as a general term of the reachability
regions and the narrow-sense controllability regions. The narrow-sense
controllability regions are also named as the recover regions [2] It can be
proven that the two kinds of region can be transformed each other by the
linear transformation. Therefore, without losing generality, the analysis and
volume computing for the broad-sense controllability regions as Eqs. (4) and
(5) are studied, and then the obtained results can be easily generlized to the
narrow controllability regions.

It is pointed that the above controllability regions, include their volumes,
shapes and radii, are directly related to the control capability of the
systems $\Sigma(A,B)$, such as the time-related control capability and the
eanergy-related control capability [4], [7]. Therefore, the volume and shape
factors can be used to defined these control capabilities, and then the
effective analysis and computing on that are the basis of the analysis and
optimizing the these control capabilities for promoting the performance of the
open-loop plants or the closed-loop control system. The analytical computing
of these volume and the shape factors are studied in papers [5] and some
effective computing methods based on the Jordan and diagonal canonical forms,
where all eigenvalues, eigenvectors and the generalized eigenvectors are need
to be solved with a little computing complexity.

Next, the volume computings of the IFTCZ based on the CCF and Hurwitz matrix
are studied respectively. The new computing methods will no longer depend on
the solving of eigenvalues and eigenvectors and are with the lower
computational complexity. Later, the ideas deduced the new computing methods
will also be generalized to the volume computing of the infite-time
controllability ellipsoid (IFTE).

###  2.1 the analytical computing of the volume of the IFTCZ

In papers [6] and [8], an analytic volume-computing methods for the samooth
zonotope $R_{a}(T)$ generated by the matrix pair $(A,B)$ with $n$ real
eigenvalues of matrix $A$ are proven and then quantifying and maximizing of
the controllability region $R_{a}(T)$ for the LCT systems $\Sigma(A,B)$ can be
carried out based on the results in paper [8]. Because that the computing
methods are proven based on the 3 kinds of the Jordan or diagonal canonical
forms, there computing equations are with 3 kinds of expressions.

As we know, any LCT models can be transformed as the Jordan canonical form
(the diagonal canonical form can be regarded as a special case of the
Jordans). Therefore, for the single-input linear systems $\Sigma(A,B)$, the
Jordan canonical forms
$\Sigma\left(A_{J},B_{J}\right)=\Sigma\left(P_{J}^{-1}AP,P_{J}^{-1}B\right)$
with the Jordan transformation matrix $P_{J}$ are as follows

  1. 1.

The diagonal canonical form:
$\Sigma\left(\textnormal{diag}\left\\{\lambda_{1},\cdots,\lambda_{n}\right\\},\left[\beta_{1},\cdots,\beta_{n}\right]^{T}\right)$;
where ’diag’ means the diagonal matrix.

  2. 2.

The Jordan canonical form with one Jordan block:
$\Sigma\left(A_{\lambda},\left[\beta_{1},\cdots,\beta_{n}\right]^{T}\right)$,
where $A_{\lambda}$ is a $n\times n$ Jordan block with the eigenvalue
$\lambda$;

  3. 3.

The Jordan canonical form with multiple Jordan blocks:

$\Sigma\left(\textnormal{block-
diag}\left\\{A_{1},\cdots,A_{q}\right\\},\left[B_{1}^{T},\cdots,B_{q}^{T}\right]^{T}\right)$

where ’block-diag’ means the block diagonal matrix, $A_{i}$ is a $m_{i}\times
m_{i}$ Jordan block with the eigenvalue $\lambda_{i}$ ,
$B_{i}=\left[\beta_{i,1},\cdots,\beta_{i,m_{i}}\right]^{T}$,
$\sum_{i=1}^{q}=n$.

In fact, the 3-th case of the Jordan canonical forms is a general case, and
the 1-st and 2-nd cases are the some special forms of the 3-th case.

###### Theorem 1

If matrix $A\in R^{n\times n}$ is with $n$ different eigenvalues in the
interval $(-\infty,0)$ and $b\in R^{n}$, the volume of the IFTCZ
$R_{a}(\infty)$ generated by matrix pair $(A,B)$ can be computed analytically
by the following equation:

$\displaystyle V_{1}$
$\displaystyle=2^{n}\left|\det\left(P_{J}\right)\left(\prod_{1\leq i<j\leq
n}\frac{\lambda_{j}-\lambda_{i}}{\lambda_{i}+\lambda_{j}}\right)\left(\prod_{i=1}^{n}\frac{\beta_{i}}{\lambda_{i}}\right)\right|$
$\displaystyle=2^{n}\left|\frac{\det\left(P_{J}\right)}{L_{n}\left(\Lambda_{n}\right)}\left[\prod_{1\leq
i<j\leq
n}\left(\lambda_{j}-\lambda_{i}\right)\right]\left(\prod_{i=1}^{n}\beta_{i}\right)\right|$
(6) $\displaystyle V_{2}$
$\displaystyle=2^{n}\left|\frac{\det\left(P_{J}\right)\left(\beta_{n}/\lambda\right)^{n}}{\left(2\lambda\right)^{n(n-1)/2}}\right|=2^{n}\left|\frac{\det\left(P_{J}\right)\beta_{n}^{n}}{L_{n}\left(\Lambda_{n}\right)}\right|$
(7) $\displaystyle V_{3}$
$\displaystyle=2^{n}\left|\det\left(P_{J}\right)\prod_{1\leq i<j\leq
q}\left(\frac{\lambda_{j}-\lambda_{i}}{\lambda_{i}+\lambda_{j}}\right)^{m_{i}\times
m_{j}}\right|\times\left|\prod_{i=1}^{q}\frac{\left(\beta_{i,m_{i}}/\lambda_{i}\right)^{m_{i}}}{\left(2\lambda_{i}\right)^{m_{i}(m_{i}-1)/2}}\right|$
$\displaystyle=2^{n}\left|\frac{\det\left(P_{J}\right)}{L_{n}\left(\Lambda_{n}\right)}\prod_{1\leq
i<j\leq q}\left(\lambda_{j}-\lambda_{i}\right)^{m_{i}\times
m_{j}}\right|\times\left|\prod_{i=1}^{q}\beta_{i,m_{i}}^{m_{i}}\right|$ (8)

where $L_{n}\left(\Lambda_{n}\right)$ is a $[n(n+1)/2]$-order homogeneous
polynomials on all $n$ eigenvalues
$\Lambda_{n}=\left[\lambda_{1},\lambda_{2},\cdots,\lambda_{n}\right]$ defined
as follows

$L_{n}\left(\Lambda_{n}\right)=\left[\prod_{1\leq i<j\leq
n}\left(\lambda_{i}+\lambda_{j}\right)\right]\left(\prod_{i=1}^{n}\lambda_{i}\right)$
(9)

where $\Lambda_{n}$ includes all eigenvalues and their multiplicities.

Based on the above theorems, the volume of the controllability region
$R_{a}(N)$ when $N\rightarrow\infty$ can be computed analytically.
Furthermore, some analytical factors describing the shape of the $R_{a}(N)$
can be got by deconstructing the analytical volume computing equation (6).
These analytical expressions for these volume and shape factors can be
describe quantitatively the control capability of the dynamical systems, and
then optimizing these volume and shape factors is indeed maximizing the
control capability.

##  3 the direct computing of the IFTCZ based on Hurwitz matrix

###  3.1 The analytical volume expression of of the IFTCZ of the systems
$\Sigma\left(A_{c},B_{c}\right)$

Considering the following controllable canonical form (CCF) of the single-
input LCT systems

$\displaystyle\dot{x}_{c}$ $\displaystyle=A_{c}x_{c}+B_{c}u$
$\displaystyle=\left[\begin{array}[]{cccc}0&1&\cdots&0\\\
\vdots&\vdots&\ddots&\vdots\\\ 0&0&\cdots&1\\\
-a_{n}&-a_{n-1}&\cdots&-a_{1}\end{array}\right]x_{c}+\left[\begin{array}[]{c}0\\\
\vdots\\\ 0\\\ 1\end{array}\right]u$ (18)

In fact, by the linear system theory, any state controllable LCT systems

$\Sigma(A,B):\,x=Ax+Bu,$

can be transformaed as the above CCF $\Sigma\left(A_{c},B_{c}\right)$ by the
state transformation $x=W_{c}x_{c}$, where the transformation matrix $W_{c}$
[3] [1]

$\displaystyle W_{c}$ $\displaystyle=\left[\begin{array}[]{c}w_{1}\\\
w_{1}A\\\ \vdots\\\ w_{1}A^{n-1}\end{array}\right]^{-1}$ (23) $\displaystyle
w_{1}$ $\displaystyle=[0,0,\dots,1]P_{n}^{-1}$
$\displaystyle=[0,0,\dots,1]\left[B,AB,\dots,A^{n-1}B\right]^{-1}$ (24)

By the Theorem 1 for volume computing of the IFTCZ, we have the following
corollary for volume computing of CCF

###### Corollary 1

If the matrix $A_{c}$ of the CCF (18) is with $n$ eigenvalues in the interval
$(-\infty,0)$ and $B_{c}\in R^{n}$, the volume of the IFTCZ $R_{a}(\infty)$
generated by matrix pair $\Sigma\left(A_{c},B_{c}\right)$ can be computed
analytically by the following equation:

$\textnormal{vol}(R_{\infty})=\left|\frac{2^{n}}{L_{n}\left(\Lambda_{n}\right)}\right|$
(25)

And then, for any controllable LCT systems $\Sigma(A,B)$, the volume of the
correponding IFTCZ can be computed analytically as follows

$\textnormal{vol}(R_{\infty})=\left|\frac{2^{n}\det\left(W_{c}\right)}{L_{n}\left(\Lambda_{n}\right)}\right|$
(26)

Proof The corollary will be prven in 3 cases of the Jordan canonical forms as
follows.

1) Firstly, the case that all $n$ eigenvalues of the matrix $A$ are
differential is considered.

For the matrix $A_{c}$ of the CCF (18), the eigenvector $v_{i}$ corresponding
to the eigenvalue $\lambda_{i}$ can be taken as

$\displaystyle
v_{i}=\left[1\;\lambda_{i}\;\lambda_{i}^{2}\;\cdots\;\lambda_{i}^{n-1}\right]^{T}$
(27)

And then, the diagonalization transformation of the systems
$\Sigma\left(A_{c},B_{c}\right)$ can be choosen as

$\displaystyle P_{J}=\left[v_{1}\;v_{2}\;\cdots\;v_{n}\right]$ (28)

and the corresponding diagonal canonical form is

$\displaystyle\dot{\tilde{x}}$
$\displaystyle=P_{J}^{-1}A_{c}P_{J}\tilde{x}+P_{J}^{-1}B_{c}u$
$\displaystyle=\left[\begin{array}[]{cccc}\lambda_{1}&0&\cdots&0\\\
0&\lambda_{2}&\cdots&0\\\ \vdots&\vdots&\ddots&\vdots\\\
0&0&\cdots&\lambda_{n}\end{array}\right]x+\left[\begin{array}[]{c}\beta_{1}\\\
\beta_{2}\\\ \vdots\\\ \beta_{n}\end{array}\right]u$ (37)

where

$\displaystyle\det\left(P_{J}\right)$ $\displaystyle=\prod_{1\leq i<j\leq
n}\left(\lambda_{j}-\lambda_{i}\right)$ (38) $\displaystyle P_{J}^{-1}$
$\displaystyle=\frac{\textrm{adj}\left(P_{J}\right)}{\left|P_{J}\right|}=\left[\begin{array}[]{cccc}*&\cdots&*&\beta_{1}\\\
&\cdots&*&\beta_{2}\\\ \vdots&\ddots&\vdots&\vdots\\\
&\cdots&*&\beta_{n}\end{array}\right]$ (43) $\displaystyle\beta_{i}$
$\displaystyle=(-1)^{n-1}\prod_{j\in\\{1,n\\}\setminus
i}\frac{1}{\lambda_{j}-\lambda_{i}}\;i=1,2,\cdots,n$ (44)

where ’$\\{1,n\\}\setminus j$’ means the set $\\{1,2,\cdots,n\\}$ but doesn’t
include number $j$.

By Theorem 1, the volume of the IFTCZ zonotope $R_{a}(\infty)$ generated by
matrix pair $\Sigma\left(A_{c},B_{c}\right)$ is

$\displaystyle\textrm{Vol}\left(E_{n}\right)$
$\displaystyle=\left|\det\left(P_{J}\right)\left(\prod_{1\leq i<j\leq
n}\frac{\lambda_{j}-\lambda_{i}}{\lambda_{j}+\lambda_{i}}\right)\left(\prod_{i=1}^{n}\frac{\beta_{i}}{\lambda_{i}}\right)\right|$
$\displaystyle=\left|\prod_{1\leq i<j\leq
n}\left(\lambda_{j}-\lambda_{i}\right)\left(\prod_{1\leq i<j\leq
n}\frac{\lambda_{j}-\lambda_{i}}{\lambda_{j}+\lambda_{i}}\right)\left(\prod_{i=1}^{n}\frac{(-1)^{n-1}}{\lambda_{i}}\prod_{j\in\\{1,n\\}\setminus
i}\frac{1}{\lambda_{j}-\lambda_{i}}\right)\right|$
$\displaystyle=\left|\left(\prod_{1\leq i<j\leq
n}\frac{\left(\lambda_{j}-\lambda_{i}\right)^{2}}{\lambda_{j}+\lambda_{i}}\right)\left(\prod_{i=1}^{n}\frac{1}{\lambda_{i}}\right)\left(\prod_{1\leq
j\leq n}\frac{1}{\left(\lambda_{j}-\lambda_{i}\right)^{2}}\right)\right|$
$\displaystyle=\left|\left(\prod_{1\leq i<j\leq
n}\frac{1}{\lambda_{j}+\lambda_{i}}\right)\left(\prod_{i=1}^{n}\frac{1}{\lambda_{i}}\right)\right|$
(45)

2) Secondly, the case that all $n$ eigenvalues of the matrix $A$ are same as
$\lambda$, that is, the Jordan matrix correponding to the matrix $A$ is with
on Jordan block.

For the matrix $A_{c}$ of the CCF (37), the eigenvector and generalized
eigenvector corresponding to the eigenvalue $\lambda$ can be taken as

$\displaystyle v_{1}$
$\displaystyle=\left[1\;\lambda\;\lambda^{2}\;\cdots\;\lambda^{n-1}\right]^{T}$
$\displaystyle v_{2}$
$\displaystyle=\left[0\;1\;2\lambda\;\cdots\;(n-1)\lambda^{n-2}\right]^{T}$
$\displaystyle v_{3}$
$\displaystyle=\left[0\;0\;1\;\cdots\;\frac{(n-1)!}{2!(n-3)!}\lambda^{n-3}\right]^{T}$
$\displaystyle\cdots\cdots$ $\displaystyle v_{n}$
$\displaystyle=\left[0\;0\;0\;\cdots\;1\right]^{T}$ (46)

And then, the diagonalization transformation of the systems
$\Sigma\left(A_{c},B_{c}\right)$ can be choosen as

$\displaystyle P_{J}$ $\displaystyle=\left[v_{1}\;v_{2}\;\cdots\;v_{n}\right]$
$\displaystyle=\left[\begin{array}[]{cccccc}1&0&0&\cdots&0\\\
\lambda&1&0&\cdots&0\\\ \lambda^{2}&2\lambda&1&\cdots&0\\\
\vdots&\vdots&\vdots&\ddots&\vdots\\\
\lambda^{n-1}&(n-1)\lambda^{n-2}&\frac{(n-1)!}{2!(n-3)!}\lambda^{n-3}&\cdots&1\end{array}\right]$
(52)

and the corresponding Jordan canonical form is

$\displaystyle\dot{\tilde{x}}$
$\displaystyle=\left[\begin{array}[]{cccc}\lambda&1&\cdots&0\\\
0&\lambda&\cdots&0\\\ \vdots&\vdots&\ddots&\vdots\\\
0&0&\cdots&\lambda\end{array}\right]x+\left[\begin{array}[]{c}\beta_{1}\\\
\beta_{2}\\\ \vdots\\\ \beta_{n}\end{array}\right]u$ (61)

where

$\displaystyle\det\left(P_{J}\right)$ $\displaystyle=1$ $\displaystyle
P_{J}^{-1}$
$\displaystyle=\frac{\textrm{adj}\left(P_{J}\right)}{\left|P_{J}\right|}=\left[\begin{array}[]{cccc}*&\cdots&*&\beta_{1}\\\
&\cdots&*&\beta_{2}\\\ \vdots&\ddots&\vdots&\vdots\\\
&\cdots&*&\beta_{n}\end{array}\right]$ (66) $\displaystyle\beta_{i}$
$\displaystyle=\left\\{\begin{array}[]{ll}0&i<n\\\ 1&i=n\end{array}\right.$
(69)

By Theorem 1, the volume of the IFTCZ $R_{a}(\infty)$ generated by matrix pair
$\Sigma\left(A_{c},B_{c}\right)$ is

$\displaystyle\textrm{Vol}\left(E_{n}\right)$
$\displaystyle=2^{n}\left|\frac{\det\left(P_{J}\right)\beta_{n}^{n}}{L_{n}\left(\Lambda_{n}\right)}\right|=\left|\frac{2^{n}}{L_{n}\left(\Lambda_{n}\right)}\right|$
(70)

3) Combining above proving processes for the two kind of Jordan canonical
forms, it can be proven that, Eq. (25) for the CCF (61) which the
corresponding Jordan matrix is with the multiply Jordan blocks is also true.

To sum up, when all eigenvalues the CCF (61) are negative numbers, whether
they are single or repeated eigenvalues, Eq. (25) for the volume computing of
the IFTCZ generated by the matrix pair $\Sigma\left(A_{c},B_{c}\right)$ holds.

###  3.2 The determinant of the Hurwitz matrix

Consider the following $n$-order real polynomial $f^{(n)}(s)$ with the zeros
$\left\\{\lambda_{1},\lambda_{2},\cdots,\lambda_{n}\right\\}$

$\displaystyle f^{(n)}(s)$
$\displaystyle=s^{n}+a^{(n)}_{1}s+\cdots+a^{(n)}_{n-1}s+a^{(n)}_{n}$
$\displaystyle=\prod_{i=1}^{n}\left(s-\lambda_{i}\right)$ (71)

where

$\displaystyle a^{(n)}_{k}=(-1)^{k}\sum_{1\leq i_{1}<\cdots<i_{k}\leq
n}\prod_{j=1}^{k}\lambda_{i_{j}}$ (72)

For the $n$-order real polynomial $f^{(n)}(s)$, the correponding Hurwitz matrx
can be defined as follows

$\displaystyle
H^{(n)}=\left[\begin{array}[]{cccccccc}a_{1}^{(n)}&a_{3}^{(n)}&a_{5}^{(n)}&a_{7}^{(n)}&\cdots&0&0&0\\\
1&a_{2}^{(n)}&a_{4}^{(n)}&a_{6}^{(n)}&\cdots&0&0&0\\\
0&a_{1}^{(n)}&a_{3}^{(n)}&a_{5}^{(n)}&\cdots&0&0&0\\\
0&1&a_{2}^{(n)}&a_{4}^{(n)}&\cdots&0&0&0\\\
\vdots&\vdots&\vdots&\vdots&\ddots&\vdots&\vdots&\vdots\\\
0&0&0&0&\cdots&a_{n-2}^{(n)}&a_{n}^{(n)}&0\\\
0&0&0&0&\cdots&a_{n-3}^{(n)}&a_{n-1}^{(n)}&0\\\
0&0&0&0&\cdots&a_{n-4}^{(n)}&a_{n-2}^{(n)}&a_{n}^{(n)}\end{array}\right]$ (81)

By Eq. (72) for computing coefficients $a^{(n)}_{k}$, we see, the deteminant
value of the Hurwitz matrix $H^{(n)}$ is a $[n(n+1)/2]$-order homogeneous
polynomial about the variabl vector $\Lambda_{n}$.

By the Hurwitz criterion, we know, the conditions that the all zeros of the
polynomial $f^{(n)}(s)$ are with the negative real parts, that is, all poles
of the LCT systems are with the negative real parts, and then systems are
stable is the following order principal minor determinants of the Hurwitz
matrix $H^{(n)}$ are greater than 0, that is,

$\displaystyle H^{(n)}_{i}>0,\,i=1,2,\cdots,n$

where the order principal minor determinant $H^{(n)}_{i}$ is the $i\times i$
matrix composed by the first $i$ rows and first $i$ columns of the matrix
$H^{(n)}$.

For the deteminant of the Hurwitz matrix $H^{(n)}$, we have thw following
lemma.

###### Lemma 1

For the $n$-order real polynomial $f^{(n)}(s)$, the determinat of the
correponding Hurwitz matrix $H^{(n)}$ can be computed as

$\displaystyle\det\left(H^{(n)}\right)$
$\displaystyle=L_{n}\left(\Lambda_{n}\right)=\left(\prod_{1\leq i<j\leq
n}\lambda_{j}+\lambda_{i}\right)\left(\prod_{i=1}^{n}\lambda_{i}\right)$ (82)

Proof. By Eq. (81), we have

$\displaystyle\det\left(H^{(n)}\right)$
$\displaystyle=\det\left(H_{n-1}^{(n)}\right)a_{n}^{(n)}=\det\left(H_{n-1}^{(n)}\right)\left(\prod_{i=1}^{n}\lambda_{i}\right)$

Next, it is proved fistly that for any different $i$ and $j$,
$\lambda_{j}+\lambda_{i}$ is a factor of $\det H_{n-1}^{(n)}$. For that, we
have the following computing result

$\displaystyle\left.a_{k}^{(n)}\right|_{\lambda_{n-1}=-\lambda_{n}}$
$\displaystyle=(-1)^{k}\left.\sum_{1\leq i_{1}<\cdots<i_{k}\leq
n}\prod_{j=1}^{k}\lambda_{i_{j}}\right|_{\lambda_{n-1}=-\lambda_{n}}$
$\displaystyle=\left\\{\begin{array}[]{ll}a_{1}^{(n-2)}&k=1\\\
a_{2}^{(n-2)}-\lambda_{n}^{2}&k=2\\\
a_{k}^{(n-2)}-\lambda_{n}^{2}a_{k-2}^{(n-2)}&k\in\\{3,n-2\\}\\\
-\lambda_{n}^{2}a_{k-2}^{(n-2)}&k=n,n-1\end{array}\right.$ (87)

where $\left\\{a_{k}^{(n-2)},k=1,2,\dots,n-2\right\\}$ are the coefficients of
the $(n-2)$-order real polynomial $f^{(n-2)}(s)$ with the zeros
$\left\\{\lambda_{1},\lambda_{2},\cdots,\lambda_{n-2}\right\\}$. Based on
that, we have

$\displaystyle\left.H_{n-1}^{(n)}\right|_{\lambda_{n-1}=-\lambda_{n}}$
$\displaystyle=\left[\begin{array}[]{cccccc}a_{1}^{(n-2)}&a_{3}^{(n-2)}-\lambda_{n}^{2}a_{1}^{(n-2)}&a_{5}^{(n-2)}-\lambda_{n}^{2}a_{3}^{(n-2)}&\cdots\\\
1&a_{2}^{(n-2)}-\lambda_{n}^{2}&a_{4}^{(n-2)}-\lambda_{n}^{2}a_{2}^{(n-2)}&\cdots\\\
0&a_{1}^{(n-2)}&a_{3}^{(n-2)}-\lambda_{n}^{2}a_{1}^{(n-2)}&\cdots\\\
0&1&a_{2}^{(n-2)}-\lambda_{n}^{2}&\cdots\\\ \vdots&\vdots&\vdots&\ddots\\\
0&0&0&\cdots\\\ 0&0&0&\cdots\end{array}\right.$ (95)
$\displaystyle\;\;\;\;\;\left.\begin{array}[]{cccccc}0&0\\\ 0&0\\\ 0&0\\\
0&0\\\ \vdots&\vdots\\\
a_{n-2}^{(n-2)}-\lambda_{n}^{2}a_{n-4}^{(n-2)}&-\lambda_{n}^{2}a_{n-2}^{(n-2)}\\\
a_{n-3}^{(n-2)}-\lambda_{n}^{2}a_{n-5}^{(n-2)}&-\lambda_{n}^{2}a_{n-3}^{(n-2)}\end{array}\right]$
(103)
$\displaystyle=\left[\begin{array}[]{ccccccc}a_{1}^{(n-2)}&a_{3}^{(n-2)}&a_{5}^{(n-2)}&\cdots&0&0\\\
1&a_{2}^{(n-2)}&a_{4}^{(n-2)}&\cdots&0&0\\\
0&a_{1}^{(n-2)}&a_{3}^{(n-2)}&\cdots&0&0\\\ 0&1&a_{2}^{(n-2)}&\cdots&0&0\\\
\vdots&\vdots&\vdots&\ddots&\vdots&\vdots\\\ 0&0&0&\cdots&a_{n-2}^{(n-2)}&0\\\
0&0&0&\cdots&a_{n-3}^{(n-2)}&0\\\ \end{array}\right]$ (111)

Therefore, we have

$\displaystyle\det\left(\left.H_{n-1}^{(n)}\right|_{\lambda_{n-1}=-\lambda_{n}}\right)=0$
(112)

that is, $\lambda_{n-1}+\lambda_{n}$ is a factor of $\det H_{n-1}^{(n)}$.
Without losing generality, it can be summarized as that for any $i$ and $j$,
$\lambda_{i}+\lambda_{j}$ is a factor of $\det H_{n-1}^{(n)}$. Hence, we have

$\displaystyle\det H_{n-1}^{(n)}$
$\displaystyle=h\left(\Lambda_{n}\right)\left(\prod_{1\leq i<j\leq
n}\lambda_{j}+\lambda_{i}\right)$ $\displaystyle\det H^{(n)}$
$\displaystyle=h\left(\Lambda_{n}\right)\left(\prod_{1\leq i<j\leq
n}\lambda_{j}+\lambda_{i}\right)\left(\prod_{i=1}^{n}\lambda_{i}\right)=h\left(\Lambda_{n}\right)L_{n}\left(\Lambda_{n}\right)$
(113)

where $h\left(\Lambda_{n}\right)$ is an undetermined function on vector
$\Lambda_{n}$. By Eqs. (81) and (9), we know, $\det H^{(n)}$ and
$L_{n}\left(\Lambda_{n}\right)$ are the $[n(n+1)/2]$-order homogeneous
polynomials on the vector $\Lambda_{n}$. After anylysing these two homogeneous
polynomials, we can conclude that
$\lambda_{1}^{n}\lambda_{2}^{n-1}\dots\lambda_{n}$ is a common term of these
two polynomials and correspondingly two coefficients of these terms are samed
as one. Therefore, we have

$h\left(\Lambda_{n}\right)=1,$

and then the lemma is proven true.

Combining the above Corollary 1 and Lemma 1, we have the following result
about the volume of the IFTCZ based on the Hurwita matrix.

###### Theorem 2

For the single-input LCT systems with the state controllability, if all
eigenvalues of the systems are the negative real numbers, the volume of the
IFTCZ can be computed analytically as follows

$\textnormal{vol}(R_{\infty})=2^{n}\left|\frac{\det W_{c}}{\det
H^{(n)}}\right|$ (114)

where the matrix $W_{c}$ is the transformation matrix which can transformate
the system models $\Sigma(A,B)$ as the CCF $\Sigma\left(A_{c},B_{c}\right)$.

To sum up, by now we have 3 kinds of the analytical volume computing methods
of the IFTCZ for the LCT systems $\sigma(A,B)$, presented in Theorem 1,
Corollary 1 and Theorem 2. The difference among the 3 kind computing methods
are summaried as follows:

  * •

The all eigenvalues, eignevectors and generalized eigenvectors of the matrix
$A$ are needed to be solved out in the methods presented in Theorem 1 and is
with slightly larger computational complexity. In addition, some combination
calculation for the matrix $A$ with the corresponding multiply Jordan blocks
is with a little trouble;

  * •

The all eigenvalues of the matrix $A$ are needed to be solved out in the
method in Corollary 1;

  * •

The method in Theorem 2 is only based on the determinant calculation of
Hurwitz matrix and is with less computational complexity.

##  4 the analytical computing of the volume of the IFTCE

The above volume computing methods about the IFTCZ based on the CCF and the
Hurwita matrix will be generlized to the volume computing of the IFTCE.

For the volume computing problem of the IFTCE of the LCT systems, we have the
following theorem about the analytical computing [8].

###### Theorem 3

If the $n$ complex eigenvalues of the matrix $A\in R^{n\times n}$ is with
negative real parts and $b\in R^{n}$, the volume of the IFTCE $R_{b}(\infty)$
generated by matrix pair $(A,B)$ can be computed analytically by the following
equation:

$\displaystyle V_{1}$
$\displaystyle=\Pi_{n}\left|\det\left(P_{J}\right)\left(\prod_{1\leq i<j\leq
n}\frac{\lambda_{j}-\lambda_{i}}{\lambda_{i}+\lambda_{j}}\right)\left(\prod_{i=1}^{n}\frac{\beta_{i}}{\left(2\lambda_{i}\right)^{1/2}}\right)\right|$
(115) $\displaystyle V_{2}$
$\displaystyle=\Pi_{n}\left|\frac{\det\left(P_{J}\right)\beta_{n}^{n}}{\left(2\lambda\right)^{n^{2}/2}}\right|$
(116) $\displaystyle V_{3}$
$\displaystyle=\Pi_{n}\left|\det\left(P_{J}\right)\prod_{1\leq i<j\leq
q}\left(\frac{\lambda_{j}-\lambda_{i}}{\lambda_{i}+\lambda_{j}}\right)^{m_{i}\times
m_{j}}\right|\times\left|\prod_{i=1}^{q}\frac{\beta_{i,m_{i}}^{m_{i}}}{\left(2\lambda_{i}\right)^{m_{i}^{2}/2}}\right|$
(117)

where the hypersphere volume-coefficient $\Pi_{n}$ and the Gamma function
$\varGamma(s)$ can be defined respectively as

$\displaystyle\Pi_{n}$
$\displaystyle=\frac{\pi^{n/2}}{\varGamma\left(\frac{n}{2}+1\right)}$ (118)
$\displaystyle\varGamma(s)$
$\displaystyle=\left\\{\begin{array}[]{ll}(s-1)\varGamma(s-1)&s>1\\\
\sqrt{\pi}&s=1/2\end{array}\right.$ (121)

Based on the above theorems, the volume of the IFTCE $R_{b}(\infty)$ can be
computed analytically. Furthermore, some analytical factors describing the
shape of the $R_{b}(\infty)$ can be got by deconstructing the analytical
volume computing equations (115), (116) and (117). These analytical
expressions for these volume and shape factors can be describe quantitatively
the control capability of the dynamical systems, and then optimizing these
volume and shape factors is indeed maximizing the control capability.

Similar to Corollery 1 and Theorem 2, we have the following analytical
computing corollary and theorem based on the CCF and the Hurwita matrix.

###### Corollary 2

For any controllable single-input LCT systems $\Sigma(A,B)$, if all
eigenvalues of the matrix $A$ are the negative real numbers, the volume of the
correponding IFTCE can be computed analytically as follows

$\textnormal{vol}(R_{\infty})=\Pi_{n}\left(\frac{a_{n}}{2^{n}}\right)^{1/2}\left|\frac{\det\left(W_{c}\right)}{L_{n}\left(\Lambda_{n}\right)}\right|$
(122)

where the matrix $W_{c}$ is the transformation matrix which can transformate
the system models $\Sigma(A,B)$ as the CCF $\Sigma\left(A_{c},B_{c}\right)$;
$a_{n}$ is a constant term of the characteristic polynomial of the matrix $A$
and can be represented as follows

$a_{n}=(-1)^{n}\prod_{i=1}^{n}\lambda_{i}$

###### Theorem 4

For the single-input LCT systems with the state controllability, if all
eigenvalues of the systems are the negative real numbers, the volume of the
IFTCZ can be computed analytically as follows

$\textnormal{vol}(R_{\infty})=\Pi_{n}\left(\frac{a_{n}}{2^{n}}\right)^{1/2}\left|\frac{\det
W_{c}}{\det H^{(n)}}\right|$ (123)

To sum up, by now we have also 3 kinds of the analytical volume computing
methods of the IFTCE for the LCT systems $\Sigma(A,B)$, presented in Theorem
3, Corollary 2 and Theorem 4. The difference among the 3 kind computing
methods are summaried as follows:

  * •

The all eigenvalues, eignevectors and generalized eigenvectors of the matrix
$A$ are needed to be solved out in the methods presented in Theorem 3 and is
with slightly larger computational complexity. In addition, some combination
calculation for the matrix $A$ with the corresponding multiply Jordan blocks
is with a little trouble;

  * •

The all eigenvalues of the matrix $A$ are needed to be solved out in the
method in Corollary 2;

  * •

The method in Theorem 2 is only based on the determinant calculation of
Hurwitz matrix and is with less computational complexity.

##  5 Numerical Experiments (Not available here)

##  6 Conclusions (Not available here)

## References

  * Chen [1998] C.T. Chen, Linear system theory and design, Oxford University Press, Inc. New York, NY, USA, 3rd edition, 1998.
  * Hu and Lin [2001] T. Hu, Z. Lin, Control systems with actuator saturation: analysis and design, BirkhTauser, Boston, 2001\.
  * Kailath [1980] T. Kailath, Linear systems, Prentice-Hall, Englewood Cliffs, NJ, 1980\.
  * Zhao [2020a] M. Zhao, Analytical expression and deconstruction of the volume of the controllability ellipsoid, arXiv:2004.05528 (2020a) 10\.
  * Zhao [2020b] M. Zhao, Analytical factors for describing the control ability of linear discrete-time systems, arXiv:2004.07982 (2020b).
  * Zhao [2020c] M. Zhao, Exact volume of zonotopes generated by a matrix pair, arXiv:2004.05530 (2020c) 20.
  * Zhao [2020d] M. Zhao, Relations among open-loop control ability, control strategy space and closed-loop performance for linear discrte-time systems, arXiv:2004.05619 (2020d) 13.
  * Zhao [2021] M. Zhao, Control capability with time attributy for linear continuous-time systems, arXiv:2103.15038 (2021).
  * Zhao [2022] M. Zhao, Analytical volume analysis for the finite-time controllable region of the linear discrete-time systems, arXiv: (2022).


# Invariant Filtering for Legged Humanoid Locomotion on a Dynamic Rigid
Surface

Yuan Gao, Chengzhi Yuan, Yan Gu  This work was supported by the National
Science Foundation under Grants CMMI-1934280 and CMMI-2046562. Y. Gao is with
the Department of Mechanical Engineering, University of Massachusetts Lowell,
Lowell, MA 01854, U.S.A. Y. Gu is with the School of Mechanical Engineering,
Purdue University, West Lafayette, IN 47907, U.S.A. This work was partly
conducted while Y. Gu was with the University of Massachusetts Lowell. C. Yuan
is with the Department of Mechanical, Industrial and Systems Engineering,
University of Rhode Island, Kingston, RI 02881, U.S.A. Emails:
<EMAIL_ADDRESS><EMAIL_ADDRESS><EMAIL_ADDRESS>
###### Abstract

State estimation for legged locomotion over a dynamic rigid surface (DRS),
which is a rigid surface moving in the world frame (e.g., ships, aircraft, and
trains), remains an under-explored problem. This paper introduces an invariant
extended Kalman filter that estimates the robot’s pose and velocity during DRS
locomotion by using common sensors of legged robots (e.g., inertial
measurement units (IMU), joint encoders, and RDB-D camera). A key feature of
the filter lies in that it explicitly addresses the nonstationary surface-foot
contact point and the hybrid robot behaviors. Another key feature is that, in
the absence of IMU biases, the filter satisfies the attractive group affine
and invariant observation conditions, and is thus provably convergent for the
deterministic continuous phases. The observability analysis is performed to
reveal the effects of DRS movement on the state observability, and the
convergence property of the hybrid, deterministic filter system is examined
for the observable state variables. Experiments of a Digit humanoid robot
walking on a pitching treadmill validate the effectiveness of the proposed
filter under large estimation errors and moderate DRS movement. The video of
the experiments can be found at: https://youtu.be/ScQIBFUSKzo.

###### Index Terms:

State estimation, legged locomotion, dynamic environments.

##  I Introduction

State estimation is essential to providing the estimates of a robot’s movement
state (e.g., pose and velocity) needed for planning and control. While state
estimation for locomotion on static [1] or relatively unstable surfaces [2]
has been extensively studied, state estimation for dynamic rigid surfaces
(DRS), such as ships and aircraft [3], has not been fully investigated. This
paper aims to solve the estimation problem for DRS locomotion. Yet, solving
this problem is challenging due to the nonstationary surface-foot contact
point [3, 4] and the hybrid robot dynamics involving both continuous behaviors
and discrete foot-landing events [5, 6, 7].

Extended Kalman filtering (EKF) [8, 9] has been used to achieve real-time
state estimation of legged locomotion on static surfaces by fusing the data
returned by common on-board sensors such as encoders (which measure the joint
angles) as well as IMUs attached to the robot’s base (which measure the base’s
linear acceleration and angular velocity in the base frame). Recently, EKF-
based estimators have been created to estimate a robot’s base pose and
velocity [1, 10]. These methods can be applied to general legged locomotion
because their formulation is independent of robot dynamics and gait types.
Specifically, the process model includes the IMU motion and bias dynamics, and
the measurement model is based on the the leg odometry formed via the forward
kinematics between the base and the ground contact point. Yet, they may not be
effective for DRS locomotion because they assume the foot-ground contact point
is static in the world frame. Also, similar to standard EKF, they may not
handle large estimation errors well because the underlying system
linearization depends on the true state but is evaluated at the state estimate
[11].

To ensure provable, rapid convergence under large estimation error, the
previous EKF-based design [1] has been transformed into an invariant extended
Kalman filter (InEKF) for legged locomotion on static surfaces [12, 13]. By
the theory of InEKF [14], when the InEKF system meets the group affine and
invariant observation conditions, the system linearization is independent of
the true state and thus is valid even under relatively large errors. Still,
the effectiveness of these methods in handling DRS locomotion is unclear,
especially under a relatively significant surface motion (e.g., ship motion
under sea waves [15]), because of the underlying assumption of stationary
surface-foot contact.

To address hybrid robot behaviors, state estimators for hybrid models of
legged locomotion [16, 17] have been derived. However, the convergence
property of invariant filters for hybrid locomotion models has not been
examined.

This paper introduces an InEKF method to produce real-time, accurate state
estimation for bipedal humanoid walking on a DRS even under relatively large
estimation errors. The main contributions are:

  * (a)

Deriving an InEKF that considers the DRS motion and hybrid robot behaviors and
meets the group affine and invariant observation properties without IMU
biases.

  * (b)

Building a right-invariant measurement model based on the rotational
constraint at the surface-foot contact area, enhancing the convergence rate
and rendering the base yaw angle observable under general DRS movement.

  * (c)

Performing observability analysis that reveals how the DRS pose affects state
observability, and proving the stability for the hybrid, deterministic error
dynamics.

  * (d)

Demonstrating the computational efficiency, accuracy, and robustness of the
proposed filter through experiments of humanoid walking on a rocking
treadmill.

Some results in this paper have been reported [18]. The new, substantial
contributions of this paper are the last three items in the aforementioned
list.

The paper is structured as follows. Section II provides a brief background on
matrix Lie groups. Section III presents the problem formulation. Section IV
introduces the proposed InEKF for the hybrid model of DRS locomotion. Section
V provides observability and convergence analysis. Section VI reports
experiment results. Section VII discusses the capabilities and limitations of
the filter. Section VIII gives the concluding remarks.

##  II PRELIMINARIES

The matrix Lie group, denoted as $\mathcal{G}$, is a subset of $n\times n$
invertible square matrices. The associated Lie algebra $\bm{\mathfrak{g}}$
with a dimension of $\mbox{dim}\bm{\mathfrak{g}}$ is a set of $n\times n$
square matrices. The linear operator $(\cdot)^{\wedge}$ maps any vector
$\bm{\xi}\in\mathbb{R}^{\text{dim}\bm{\mathfrak{g}}}$ onto
$\bm{\mathfrak{g}}$. The exponential map,
$\text{exp}:\mathbb{R}^{\text{dim}\bm{\mathfrak{g}}}\rightarrow\mathcal{G}$,
is defined as: $\text{exp}(\bm{\xi})\triangleq\text{expm}(\bm{\xi}^{\wedge})$,
where expm is the usual exponential of $n\times n$ matrices. The inverse
operator of $(\cdot)^{\wedge}$ is denoted as
$(\cdot)^{\vee}:\bm{\mathfrak{g}}\rightarrow\mathbb{R}^{\text{dim}\bm{\mathfrak{g}}}$.
The adjoint matrix $\mathbf{Ad}_{X}$ at $\mathbf{X}$ for any vector
$\bm{\xi}\in\mathbb{R}^{\text{dim}\bm{\mathfrak{g}}}$ is defined as
$\mathbf{Ad}_{X}\bm{\xi}=(\mathbf{X}\bm{\xi}^{\wedge}\mathbf{X}^{-1})^{\vee}$.
More detailed introductions to matrix Lie groups can be found in [19]. A
nomenclature table is given in supplementary material.

##  III PROBLEM FORMULATION

In the proposed filter design, the state is chosen as variables that are often
needed in locomotion planning and control, including the linear velocity
$\mathbf{v}\in\mathbb{R}^{3}$ and orientation $\mathbf{R}\in SO(3)$ of the
robot’s base (e.g., chest) expressed in the world frame (see Fig. 1). The
state also includes the base position $\mathbf{p}\in\mathbb{R}^{3}$ and
contact-point position $\mathbf{p}^{c}\in\mathbb{R}^{3}$ (see Fig. 1), so as
to exploit the forward kinematics between the base and contact/foot frames in
the filter design [1, 12].

The DRS of interest possesses two common characteristics of real-world DRSes
such as aircraft and vessels. First, when traveling on such surfaces, a robot
can only see landmarks attached to the surface instead of the world frame,
e.g., due to the concealed environment on the surface. Second, the DRS
orientation $\mathbf{R}^{DRS}\in SO(3)$ and linear and angular velocities
$\mathbf{v}^{DRS},\bm{\omega}^{DRS}\in\mathbb{R}^{3}$ (see Fig.1) are
relatively accurately known. This is a reasonable assumption because these
real-world DRSes are typically equipped with high-accuracy motion monitoring
systems [20]. Also, the proposed filter design explicitly treats the
inaccurate knowledge of surface pose and motion as explained in Sec. IV.

Figure 1: Illustration of coordinate frames and key variables. The treadmill
is a DRS that rotates in the world frame.

The sensors considered in this study are common on-board sensors, which are:
an IMU attached to the robot’s base, joint encoders, a RGB-D camera, and a
contact indicator. The RGB-D camera tracks the landmarks attached to the DRS,
which is used to obtain the camera pose in the DRS frame. The contact
indicator detects foot landing events.

The encoders measure the joint angles $\mathbf{q}\in\mathbb{R}^{m}$ with $m$
the number of joints. Corrupted by white zero-mean Gaussian noise
$\mathbf{w}^{q}$, the raw encoder data $\mathbf{\tilde{q}}$ is expressed as:
$\mathbf{\tilde{q}}=\mathbf{q}+\mathbf{w}^{q}$.

The IMU includes a gyroscope and an accelerator that respectively measure the
angular velocity $\bm{\omega}\in\mathbb{R}^{3}$ and linear acceleration
$\mathbf{a}\in\mathbb{R}^{3}$ of the IMU in the base frame. Corrupted by white
Gaussian zero-mean noise
$\mathbf{w}^{a},\mathbf{w}^{\omega}\in\mathbb{R}^{3}$, as well as biases
$\mathbf{b}^{a},\mathbf{b}^{\omega}\in\mathbb{R}^{3}$, the IMU readings
$\tilde{\mathbf{a}}$ and $\tilde{\bm{\omega}}$ are expressed as:
$\tilde{\mathbf{a}}=\mathbf{a}+\mathbf{b}^{a}+\mathbf{w}^{a}$ and
$\tilde{\bm{\omega}}=\bm{\omega}+\mathbf{b}^{\omega}+\mathbf{w}^{\omega}$.

###  III-A Continuous-Phase IMU Motion and Bias Dynamics

To form the process model, we choose to adopt the IMU motion dynamics due to
its accuracy and simplicity [1]. At time $t$, the IMU motion dynamics is given
by:

$\begin{gathered}\dot{\mathbf{R}}_{t}=\mathbf{R}_{t}(\bm{\tilde{\omega}}_{t}-\mathbf{b}_{t}^{\omega}-\mathbf{w}_{t}^{\omega})_{\times},\\\
\dot{\mathbf{v}}_{t}=\mathbf{R}_{t}(\tilde{\mathbf{a}}_{t}-\mathbf{b}^{a}_{t}-\mathbf{w}_{t}^{a})+\mathbf{g},~{}\mbox{and}~{}\dot{\mathbf{p}}_{t}=\mathbf{v}_{t},\end{gathered}$
(1)

where $(\cdot)_{\times}$ is a skew-symmetric matrix and $\mathbf{g}$ is the
gravitational acceleration vector. The IMU bias dynamics is modeled as
Brownian motion [12]:

$\displaystyle\dot{\mathbf{b}}^{a}_{t}=\mathbf{w}_{t}^{ba}~{}\mbox{and}~{}\dot{\mathbf{b}}^{\omega}_{t}=\mathbf{w}_{t}^{b\omega},$
(2)

where $\mathbf{w}_{t}^{ba}$ and $\mathbf{w}_{t}^{b\omega}$ are white zero-mean
Gaussian noise.

###  III-B Continuous-Phase Contact-Point Motion Dynamics

During DRS locomotion, the foot moves in the world frame due to the surface
movement. Thus, the deterministic motion model of the contact point is not
$\dot{\mathbf{p}}^{c}_{t}=\mathbf{0}$ as in previous work [1, 12] on static
surfaces. Instead, we explicitly consider the contact point velocity
$\mathbf{v}^{c}_{t}$ in the model:

$\dot{\mathbf{p}}^{c}_{t}=\mathbf{v}^{c}_{t}.$ (3)

In this study, to inform the model in Eq. (3), we choose to directly measure
the contact point velocity based on the known surface pose and motion and the
measured contact position in the DRS frame through the following kinematics:

$\mathbf{{v}}_{t}^{c}=\mathbf{v}_{t}^{DRS}+\bm{\omega}^{DRS}_{t}\times(\mathbf{R}_{t}^{DRS}{~{}^{DRS}\mathbf{{p}}_{t}^{c})}.$
(4)

Here, ${}^{DRS}\mathbf{{p}}_{t}^{c}$ is the contact point position relative to
the DRS frame, expressed in the DRS frame (see Fig. 1). Note that the
computation of the velocity ${}^{DRS}\mathbf{{p}}_{t}^{c}$ depends on the
robot’s camera data and the joint angle data returned by encoders. Also,
recall that the surface orientation $\mathbf{R}_{t}^{DRS}$ and motion
$\bm{\omega}^{DRS},\mathbf{v}_{t}^{DRS}$ are assumed to be known as explained
earlier. An example of computing $\mathbf{v}^{c}_{t}$ is given in Sec. VI.

The velocity computation inaccuracy is considered as:

$\mathbf{\tilde{v}}_{t}^{c}=\mathbf{v}_{t}^{c}+\mathbf{R}_{t}{\mathbf{w}}_{t}^{c},$
(5)

where $\tilde{\mathbf{v}}^{c}_{t}\in\mathbb{R}^{3}$ is the measured contact
point velocity, and the inaccuracy $\mathbf{w}_{t}^{c}$ is modeled as white
Gaussian zero-mean noise expressed in the base frame.

###  III-C Discrete Jump Dynamics at a Foot Landing

At a foot landing, the swing and support legs switch roles, causing a discrete
jump in the contact point position $\mathbf{p}^{c}_{t}$. To appropriately
propagate the estimate and covariance at foot landings, we choose to
explicitly consider the jump.

The jump map of the contact point position $\mathbf{p}^{c}_{t}$ is:

$\mathbf{p}^{c}_{t^{+}}=\mathbf{p}^{c}_{t}+\mathbf{R}_{t}\mathbf{h}_{c}(\mathbf{q}_{t})$
(6)

where the subscript $t^{+}$ denotes the timing just after the foot landing at
$t$. Here the function $\mathbf{h}_{c}$ is the forward kinematics from the
previous support-foot position to the new one, expressed in the base frame.
Except for $\mathbf{p}^{c}_{t}$, all other state variables remain continuous
at foot switching.

With the first-order Taylor expansion, the nonlinear term in the jump dynamics
(Eq. (6)) can be approximated as:
$\mathbf{R}_{t}\mathbf{h}_{c}(\mathbf{q}_{t})\approx\mathbf{R}_{t}\mathbf{h}_{c}(\tilde{\mathbf{q}}_{t})-\mathbf{R}_{t}\frac{\partial\mathbf{h}_{c}}{\partial\mathbf{q}}(\mathbf{\tilde{q}}_{t})\mathbf{w}^{q}_{t}$.

###  III-D Position based Forward Kinematics Measurement

To connect the contact and the base frames, we adopt the leg odometry
measurement in [1, 12] (see Fig. 2-b):

$\mathbf{R}_{t}^{T}(\mathbf{p}^{c}_{t}-\mathbf{p}_{t})=\mathbf{h}_{p}(\mathbf{{q}}_{t}),$
(7)

where the forward kinematics function $\mathbf{h}_{p}$ is the support foot
position relative to the base expressed in the base frame. Given the
inaccuracy of the encoder reading
$\tilde{\mathbf{q}}_{t}=\mathbf{q}+\mathbf{w}^{q}$ and with the first-order
Taylor expansion, the model in Eq. (7) can be rewritten as:
$\mathbf{h}_{p}(\mathbf{q}_{t})\approx\mathbf{h}_{p}(\tilde{\mathbf{q}}_{t})-\frac{\partial\mathbf{h}_{p}}{\partial{\mathbf{q}}}(\mathbf{\tilde{q}}_{t})\mathbf{w}^{q}_{t}$.

Figure 2: Illustrations of the observations: a) normal vector alignment of the
contact and DRS frames and b) contact point position in the base frame.

###  III-E Contact Orientation based Measurement

When the support foot and the surface have a full area contact, their normal
vectors are parallel, whether the surface is stationary or moving (see Fig.
2-a)). In this study, we utilize this rotational kinematic relationship to
form a measurement model. Suppose that the $z$-axes of the contact and surface
frames are aligned and normal to the DRS. Then,

$\mathbf{R}^{DRS}_{t}\begin{bmatrix}0~{}~{}0~{}1\end{bmatrix}^{T}={\mathbf{R}}^{c}_{t}\begin{bmatrix}0~{}0~{}1\end{bmatrix}^{T}={\mathbf{R}}_{t}\mathbf{h}_{R}(\mathbf{q}_{t})\begin{bmatrix}0~{}0~{}1\end{bmatrix}^{T}$
(8)

holds, where $\mathbf{R}^{c}_{t}\in SO(3)$ is the contact frame orientation
and the forward kinematics matrix function $\mathbf{h}_{R}$ is the support
foot orientation with respect to the base frame (see Fig.1).

To address the inaccuracy of the known surface orientation
$\mathbf{\tilde{R}}^{DRS}_{t}$, we assume the true orientation is corrupted by
white Gaussian zero-mean uncertainty $\mathbf{w}^{DRS}_{t}$ as:
${\mathbf{R}}^{DRS}_{t}=\text{exp}(-\mathbf{w}^{DRS}_{t}){\mathbf{\tilde{R}}}^{DRS}_{t}\approx(\mathbf{I}_{3}-{(\mathbf{w}^{DRS}_{t})_{\times})}\tilde{\mathbf{R}}^{DRS}_{t}$,
where $\mathbf{I}_{n}$ is an $n\times n$ identity matrix.

To handle the inaccuracy of the encoder reading $\tilde{\mathbf{q}}_{t}$, the
support foot orientation $\mathbf{R}_{t}\mathbf{h}_{R}(\mathbf{q}_{t})$ is
approximated as:
$\mathbf{R}_{t}\mathbf{h}_{R}(\mathbf{q}_{t})\approx\mathbf{R}_{t}\mathbf{h}_{R}(\mathbf{\tilde{q}}_{t})-\mathbf{R}_{t}\mathbf{J}_{h_{R}}(\tilde{\mathbf{q}}_{t},\mathbf{w}^{q}_{t})$.
Here the matrix
$\mathbf{J}_{h_{R}}(\tilde{\mathbf{q}}_{t},\mathbf{w}^{q}_{t})$ is obtained
based on the Jacobian of each column of
$\mathbf{h}_{R}\triangleq[\mathbf{h}_{R,1},~{}\mathbf{h}_{R,2},~{}\mathbf{h}_{R,3}]$
as:
$\mathbf{J}_{h_{R}}\triangleq\begin{bmatrix}\frac{\partial\mathbf{h}_{R,1}}{\partial{\mathbf{q}}_{t}}(\tilde{\mathbf{q}}_{t_{n}})\mathbf{w}^{q}_{t},~{}\frac{\partial\mathbf{h}_{R,2}}{\partial{\mathbf{q}}_{t}}(\tilde{\mathbf{q}}_{t})\mathbf{w}^{q}_{t},~{}\frac{\partial\mathbf{h}_{R,3}}{\partial{\mathbf{q}}_{t}}(\tilde{\mathbf{q}}_{t})\mathbf{w}^{q}_{t}\end{bmatrix}.$

Combining these equations yields:

$\displaystyle\mathbf{R}_{t}^{T}\mathbf{\tilde{R}}^{DRS}_{t}\begin{bmatrix}0~{}0~{}1\end{bmatrix}^{T}+\mathbf{R}^{T}_{t}(-\mathbf{w}^{DRS}_{t})_{\times}\mathbf{\tilde{R}}^{DRS}_{t}\begin{bmatrix}0~{}0~{}1\end{bmatrix}^{T}$
(9) $\displaystyle\approx$
$\displaystyle\mathbf{h}_{R}(\mathbf{\tilde{q}}_{t}){\begin{bmatrix}0~{}0~{}1\end{bmatrix}^{T}}-\tfrac{\partial\mathbf{h}_{R,3}}{\partial\tilde{\mathbf{q}}_{t}}(\tilde{\mathbf{q}}_{t})\mathbf{w}^{q}_{t}.$

##  IV FILTER DESIGN

This section introduces the proposed InEKF design based on the models
formulated in Sec. III.

The proposed filter derivation begins with proper state representation. We
adopt the representation in [12] since our filters estimate the same state.
First, the state variables $\mathbf{R}_{t}$, $\mathbf{v}_{t}$,
$\mathbf{p}_{t}$, and $\mathbf{p}^{c}_{t}$ are expressed on the matrix Lie
group $\mathcal{G}$ as:

$\mathbf{X}_{t}\triangleq\begin{bmatrix}\mathbf{R}_{t}&\left[\mathbf{v}_{t},~{}\mathbf{p}_{t},~{}\mathbf{p}^{c}_{t}\right]\\\
\mathbf{0}_{3\times 3}&\mathbf{I}_{3}\end{bmatrix}\in\mathcal{G},$ (10)

where $\mathbf{0}_{m\times n}$ is an $m\times n$ zero matrix. The Lie group
$\mathcal{G}$ is $SE_{3}(3)$, an extension of the special Euclidean group
$SE(3)$.

To explicitly handle IMU biases, they are also chosen as state variables.
These biases are typically expressed on the vector space instead of
$\mathcal{G}$ [14]; that is,
$\bm{\theta}_{t}\triangleq[(\mathbf{b}^{\omega}_{t})^{T},(\mathbf{b}^{a}_{t})^{T}]^{T}$.

Let $\mathbf{\bar{(\cdot)}}$ denote the estimate of the variable
$\mathbf{{(\cdot)}}$. Based on the InEKF framework [11], we use the right-
invariant error $\bm{\eta}_{t}$ to represent the estimation error of
$\mathbf{X}_{t}$ on $\mathcal{G}$:

$\bm{\eta}_{t}\triangleq\bar{\mathbf{{X}}}_{t}\mathbf{X}_{t}^{-1}\in\mathcal{G}.$
(11)

The log of the invariant error, denoted as $\bm{\xi}_{t}$, is a vector on
$\mathbb{R}^{\text{dim}\bm{\mathfrak{g}}}$ defined via
$\bm{\eta}_{t}\triangleq\text{exp}(\bm{\xi}_{t})=\text{expm}(\bm{\xi}_{t}^{\wedge})$.
The expressions of $\bm{\eta}_{t}$, $\bm{\xi}_{t}$, and
$\bm{\xi}_{t}^{\wedge}$ are in Sec. 2 of supplementary document. The IMU bias
error $\bm{\zeta}_{t}$ is defined as:
$\bm{\zeta}_{t}\triangleq\bm{\bar{\theta}}_{t}-\bm{\theta}.$

###  IV-A Continuous-Phase Process Model and Propagation Step

This subsection introduces the process model and propagation step of the
proposed filter for the continuous phases.

####  IV-A1 Process model

Based on the IMU motion and bias dynamics and the contact point motion in Eqs.
(1)-(3), the process model is expressed as:

$\displaystyle\dot{\mathbf{X}}_{t}$
$\displaystyle=\begin{bmatrix}\mathbf{R}_{t}(\tilde{\bm{\omega}}_{t}-\mathbf{b}_{t}^{\omega})_{\times}&\left[\mathbf{R}_{t}(\tilde{\mathbf{a}}_{t}-\mathbf{b}_{t}^{a})+\mathbf{g},~{}\mathbf{v}_{t},~{}\tilde{\mathbf{v}}^{c}_{t}\right]\\\
\mathbf{0}_{3\times 3}&\mathbf{0}_{3\times 3}\end{bmatrix}$ (12)
$\displaystyle~{}~{}~{}~{}-\mathbf{X}_{t}(\mathbf{w}^{X}_{t})^{\wedge}\triangleq\mathbf{f}_{u_{t}}(\mathbf{X}_{t},\bm{\theta}_{t})-\mathbf{X}_{t}(\mathbf{w}^{X}_{t})^{\wedge},$

with the noise vector
$\mathbf{w}^{X}_{t}\triangleq\begin{bmatrix}(\mathbf{w}^{\omega}_{t})^{T},(\mathbf{w}^{a}_{t})^{T},\mathbf{0}_{1\times
3},(\mathbf{w}^{c}_{t})^{T}\end{bmatrix}^{T}.$ Here we define the input
$\mathbf{u}_{t}$ to consist of the IMU and encoder readings and the measured
contact point velocity, i.e.,
$\mathbf{u}_{t}=\begin{bmatrix}\bm{\tilde{\omega}}_{t}^{T},~{}\mathbf{\tilde{a}}_{t}^{T},~{}(\mathbf{\tilde{v}}_{t}^{c})^{T},~{}\tilde{\mathbf{q}}_{t}\end{bmatrix}^{T}$.
Note that the encoder reading $\tilde{\mathbf{q}}_{t}$ is not an input to the
continuous-phase process model in Eq. (12) but is used later in the jump
process model.

####  IV-A2 Linearized error model

By using the first-order Taylor expansion
$\bm{\eta}_{t}\approx\mathbf{I}+\bm{\xi}_{t}^{\wedge}$ and by applying the
chain rule to express $\dot{\bm{\eta}}_{t}$, we obtain the linearized error
equation:

$\begin{bmatrix}\dot{\bm{\xi}}_{t}\\\
\dot{\bm{\zeta}}_{t}\end{bmatrix}=\mathbf{A}_{t}\begin{bmatrix}\bm{\xi}_{t}\\\
\bm{\zeta}_{t}\end{bmatrix}+\begin{bmatrix}\mathbf{Ad}_{\mathbf{\bar{X}}_{t}}&\mathbf{0}_{12\times
6}\\\ \mathbf{0}_{6\times 12}&\mathbf{I}_{6}\end{bmatrix}\mathbf{w}_{t}.$ (13)

Here, the noise term $\mathbf{w}_{t}$ is
$\mathbf{w}_{t}\triangleq[(\mathbf{w}_{t}^{X})^{T},(\mathbf{w}_{t}^{b\omega})^{T},(\mathbf{w}_{t}^{ba})^{T}]^{T}$,
the adjoint matrix $\mathbf{Ad}_{\bar{\mathbf{X}}_{t}}$ is given in Sec. 3 of
supplementary material, and the matrix $\mathbf{A}_{t}$ is:

$\small\mathbf{A}_{t}=\begin{bmatrix}\mathbf{0}_{3\times
3}&\mathbf{0}_{3\times 3}&\mathbf{0}_{3\times 3}&\mathbf{0}_{3\times
3}&-\mathbf{\bar{R}}_{t}&\mathbf{0}_{3\times 3}\\\
(\mathbf{g})_{\times}&\mathbf{0}_{3\times 3}&\mathbf{0}_{3\times
3}&\mathbf{0}_{3\times
3}&-(\mathbf{\bar{v}}_{t})_{\times}\mathbf{\bar{R}}_{t}&-\mathbf{\bar{R}}_{t}\\\
\mathbf{0}_{3\times 3}&\mathbf{I}_{3}&\mathbf{0}_{3\times
3}&\mathbf{0}_{3\times
3}&-(\mathbf{\bar{p}}_{t})_{\times}\mathbf{\bar{R}}_{t}&\mathbf{0}_{3\times
3}\\\ (\tilde{\mathbf{v}}_{t}^{c})_{\times}&\mathbf{0}_{3\times
3}&\mathbf{0}_{3\times 3}&\mathbf{0}_{3\times
3}&-(\mathbf{\bar{p}}^{c}_{t})_{\times}\mathbf{\bar{R}}_{t}&\mathbf{0}_{3\times
3}\\\ \mathbf{0}_{6\times 3}&\mathbf{0}_{6\times 3}&\mathbf{0}_{6\times
3}&\mathbf{0}_{6\times 3}&\mathbf{0}_{6\times 3}&\mathbf{0}_{6\times
3}\end{bmatrix}.$ (14)

Note that $\mathbf{A}_{t}$ contains the contact point velocity
$\mathbf{\tilde{v}}_{t}^{c}$ because the process model explicitly considers
it. Derivation of Eqs. (13) and (14) is in Sec. 3 of supplementary material.

####  IV-A3 Propagation

Let $t_{n}$ ($n\in\\{1,2,...\\}$) denote the time when sensors return data for
estimation error correction. Then, during the propagation step on
$t\in[t_{n-1},t_{n})$, the estimates $\bar{\mathbf{X}}_{t}$ and
$\bar{\bm{\theta}}_{t}$ are obtained via
$\dot{\bar{\mathbf{X}}}_{t}=\mathbf{f}_{u_{t}}(\bar{\mathbf{X}}_{t},\bar{\bm{\theta}}_{t})$
and $\dot{\bar{\bm{\theta}}}_{t}=\mathbf{0}$ based on the process models in
Eqs. (12) and (2).

By the InEKF methodology, the covariance matrix $\mathbf{P}_{t}$ is propagated
via the Riccati equation associated with the linearized error model in Eq.
(13):
$\dot{\mathbf{P}}_{t}=\mathbf{A}_{t}\mathbf{P}_{t}+\mathbf{P}\mathbf{A}_{t}^{T}+\mathbf{\bar{Q}}_{t}$,
where
$\mathbf{\bar{Q}}_{t}\triangleq\begin{bmatrix}\mathbf{Ad}_{\mathbf{\bar{X}}_{t}}&\mathbf{0}_{12\times
6}\\\ \mathbf{0}_{6\times
12}&\mathbf{I}_{6}\end{bmatrix}\text{Cov}(\mathbf{w}_{t})\begin{bmatrix}\mathbf{Ad}_{\mathbf{\bar{X}}_{t}}&\mathbf{0}_{12\times
6}\\\ \mathbf{0}_{6\times 12}&\mathbf{I}_{6}\end{bmatrix}^{T}.$

Remark 1 (Group affine property): Without IMU biases, the continuous process
model in Eq. (12) is group affine as defined in [11]. Thus, without biases and
in the deterministic case, the linear error dynamics in Eq. (13) is exact and
independent of the true state, and the covariance propagation is exact. Such
features are different from the standard EKF whose linearization accuracy
relies on estimation error.

###  IV-B Continuous-Phase Measurement Models and Update Step

This subsection formulates the two measurements in Eqs. (7) and (9) into the
right-invariant observation form defined in [11] and introduces the update
step of the proposed InEKF at time $t_{n}$. These treatments result in an
error update equation that is independent of the true state.

####  IV-B1 Right-invariant orientation based measurement

The orientation based measurement in Eq. (9) can be rewritten into the
following right-invariant observation form:

$\displaystyle\underbrace{\begin{bmatrix}\mathbf{h}_{R}(\mathbf{\tilde{q}})_{t_{n}}\scriptsize{\begin{bmatrix}0\\\
0\\\ 1\end{bmatrix}}\\\ \mathbf{0}_{3\times
1}\end{bmatrix}}_{\mathbf{Y}_{1,t_{n}}}=\mathbf{X}_{t_{n}}^{-1}\underbrace{\begin{bmatrix}\mathbf{\tilde{R}}^{DRS}_{t_{n}}\scriptsize{\begin{bmatrix}0\\\
0\\\ 1\end{bmatrix}}\\\ \mathbf{0}_{3\times
1}\end{bmatrix}}_{\mathbf{d}_{1,t_{n}}}+\begin{bmatrix}\mathbf{V}_{1,t_{n}}\\\
\mathbf{0}_{3\times 1}\end{bmatrix}$ (15)

with
$\mathbf{V}_{1,t_{n}}=\mathbf{{R}}_{t_{n}}^{T}(\mathbf{\tilde{R}}^{DRS}_{t_{n}}\begin{bmatrix}0~{}0~{}1\end{bmatrix}^{T})_{\times}\mathbf{w}_{t_{n}}^{DRS}+\tfrac{\partial\mathbf{h}_{R,3}}{\partial\mathbf{q}}(\tilde{\mathbf{q}}_{t_{n}})\mathbf{w}_{t_{n}}^{q}$.

####  IV-B2 Right-invariant position measurement

The position measurement in Eq. (7) can be expressed as [12]:

$\displaystyle\underbrace{\begin{bmatrix}{\mathbf{h}_{p}}(\tilde{\mathbf{q}}_{t_{n}})\\\
0\\\ 1\\\
-1\end{bmatrix}}_{\mathbf{Y}_{2,t_{n}}}=\mathbf{X}^{-1}_{t_{n}}\underbrace{\begin{bmatrix}\mathbf{0}_{3\times
1}\\\ 0\\\ 1\\\
-1\end{bmatrix}}_{\mathbf{d}_{2,t_{n}}}+\begin{bmatrix}\frac{\partial\mathbf{h}_{p}}{\partial\tilde{\mathbf{q}}}(\tilde{\mathbf{{q}}}_{t_{n}})\mathbf{w}^{q}_{t_{n}}\\\
\mathbf{0}_{3\times 1}\end{bmatrix}.$ (16)

####  IV-B3 Update

At time $t_{n}$, the updated estimates and covariance, denoted as
($\mathbf{\bar{X}}_{t_{n}}^{\dagger},\bm{\bar{\theta}}_{t_{n}}^{\dagger})$ and
$\mathbf{P}_{t_{n}}^{\dagger}$, are given by [11]:

$\small\begin{gathered}\mathbf{\bar{X}}_{t_{n}}^{\dagger}=\text{exp}\left(\mathbf{L}_{t_{n}}^{\xi}\mathbf{z}_{t_{n}}\right)\mathbf{\bar{X}}_{t_{n}},~{}\bm{\bar{\theta}}_{t_{n}}^{\dagger}=\bm{\bar{\theta}}_{t_{n}}+\mathbf{L}_{t_{n}}^{\zeta}\mathbf{z}_{t_{n}},~{}\mathbf{P}_{t_{n}}^{\dagger}=(\mathbf{I}-\mathbf{L}_{t_{n}}\mathbf{H}_{t_{n}})\mathbf{P}_{t_{n}},\end{gathered}$
(17)

where
$\mathbf{L}_{t_{n}}\triangleq\begin{bmatrix}(\mathbf{L}_{t_{n}}^{\xi})^{T},(\mathbf{L}_{t_{n}}^{\zeta})^{T}\end{bmatrix}^{T}$
is filter gain, $\mathbf{H}_{t_{n}}$ is the observation matrix, and
$\mathbf{z}_{t_{n}}\triangleq\begin{bmatrix}(\mathbf{\bar{X}}_{t_{n}}\mathbf{Y}_{1,t_{n}}-\mathbf{d}_{1,t_{n}})^{T},~{}(\mathbf{\bar{X}}_{t_{n}}\mathbf{Y}_{2,t_{n}}-\mathbf{d}_{2,t_{n}})^{T}\end{bmatrix}^{T}$.

To derive the observation matrix $\mathbf{H}_{t_{n}}$, we first decompose it
into
$\mathbf{H}_{t_{n}}=\begin{bmatrix}\mathbf{H}_{1,t_{n}}^{T},~{}\mathbf{H}_{2,t_{n}}^{T}\end{bmatrix}^{T}$,
where $\mathbf{H}_{1,t_{n}}\in\mathbb{R}^{6\times 12}$ and
$\mathbf{H}_{2,t_{n}}\in\mathbb{R}^{6\times 12}$ are respectively associated
with the measurement models in (15) and (16). Since the measurement models are
not explicitly dependent on biases, the matrix $\mathbf{H}_{i,t_{n}}$
($i=1,2$) can be further decomposed as
$\mathbf{H}_{i,t_{n}}\triangleq\begin{bmatrix}\tilde{\mathbf{H}}_{i,t_{n}},~{}\mathbf{0}_{3\times
6}~{};\mathbf{0}_{3\times 12},~{}\mathbf{0}_{3\times 6}\end{bmatrix}$, where
the element $\mathbf{0}_{3\times 6}$ correspond to the bias terms and the
element $\mathbf{0}_{3\times 12}$ could be removed if a reduced-dimensional
filter gain is instead used as in [12]. Based on the right-InEKF methodology
[11], we obtain the submatrix $\tilde{\mathbf{H}}_{i,t_{n}}$ via
$\begin{aligned}
\tilde{\mathbf{H}}_{i,t_{n}}\bm{\xi}_{t_{n}}=-(\bm{\xi}_{t_{n}})^{\wedge}\mathbf{d}_{i,t_{n}}\end{aligned}$:
$\tilde{\mathbf{H}}_{1,t_{n}}\triangleq[(\mathbf{R}^{DRS}_{t_{n}}~{}[0,0,1]^{T})_{\times},~{}\mathbf{0}_{3\times
9}]$ and $\tilde{\mathbf{H}}_{2,t_{n}}\triangleq[\mathbf{0}_{3\times
6},~{}-\mathbf{I}_{3},~{}\mathbf{I}_{3}]$.

To compute $\mathbf{L}_{t_{n}}$, the linearized error update equation is
obtained based on the update equation (Eq. (17)) as:

$\small\begin{bmatrix}\bm{\xi}_{t_{n}}^{\dagger}\\\
\bm{\zeta}_{t_{n}}^{\dagger}\end{bmatrix}=(\mathbf{I}{-}\mathbf{L}_{t_{n}}\mathbf{H}_{t_{n}})\begin{bmatrix}\bm{\xi}_{t_{n}}\\\
\bm{\zeta}_{t_{n}}\end{bmatrix}+{\mathbf{L}_{t_{n}}\begin{bmatrix}\mathbf{\bar{R}}_{t_{n}}\frac{\partial\mathbf{h}_{R,3}}{\partial{\mathbf{q}}_{t}}(\tilde{\mathbf{{q}}}_{t_{n}})\\\
\mathbf{0}_{3\times 1}\\\
\mathbf{\bar{R}}_{t_{n}}\frac{\partial\mathbf{h}_{p}}{\partial{\mathbf{q}}_{t}}(\tilde{\mathbf{{q}}}_{t_{n}})\\\
\mathbf{0}_{3\times 1}\end{bmatrix}\mathbf{w}^{q}_{t_{n}}},$ (18)

with derivation given in Sec. 4 of supplementary material.

Then, applying the standard Kalman filtering methodology to this linear error
update equation, we obtain the filter gain: $\begin{aligned}
\mathbf{L}_{t_{n}}=\mathbf{P}_{t_{n}}\mathbf{H}_{t_{n}}^{T}\mathbf{S}_{t_{n}}^{-1}\end{aligned},$
where
$\mathbf{S}_{t_{n}}=\mathbf{H}_{t_{n}}\mathbf{P}_{t_{n}}\mathbf{H}_{t_{n}}^{T}+\mathbf{\bar{N}}_{t_{n}}$
,
$\mathbf{\bar{N}}_{t_{n}}\triangleq\text{diag}(\mathbf{\bar{N}}_{1,t_{n}},\mathbf{\bar{N}}_{2,t_{n}})$,
$\mathbf{\bar{N}}_{1,t_{n}}\triangleq\mathbf{\bar{R}}_{t_{n}}\tfrac{\partial\mathbf{h}_{R,3}}{\partial\mathbf{q}_{t}}(\mathbf{\tilde{q}}_{t_{n}})\text{Cov}(\mathbf{w}_{t_{n}}^{q})(\tfrac{\partial\mathbf{h}_{R,3}}{\partial\mathbf{q}_{t}}(\mathbf{\tilde{q}}_{t_{n}}))^{T}\mathbf{\bar{R}}_{t_{n}}^{T}$,
and
$\mathbf{\bar{N}}_{2,t_{n}}\triangleq\mathbf{\bar{R}}_{t_{n}}\tfrac{\partial\mathbf{h}_{p}}{\partial\mathbf{q}_{t}}(\mathbf{\tilde{q}}_{t_{n}})\text{Cov}(\mathbf{w}_{t_{n}}^{q})(\tfrac{\partial\mathbf{h}_{p}}{\partial\mathbf{q}_{t}}(\mathbf{\tilde{q}}_{t_{n}}))^{T}\mathbf{\bar{R}}_{t_{n}}^{T}$.

Remark 2 (Independence of true state): The linearized error update equation
(Eq. (18)) is independent of the true state $\mathbf{X}_{t}$ and
$\bm{\theta}_{t}$ in the deterministic case. This is because both measurement
models satisfy the right-invariant observation form with respect to
$\mathbf{X}_{t}$ and are independent of $\bm{\theta}_{t}$ and because the
update equation of $\bar{\mathbf{X}}_{t}$ is in the exponential form as
prescribed by the InEKF methodology [11].

###  IV-C Discrete Process Model and Propagation Step

Without loss of generality and for simplicity, suppose that the foot-landing
events and the updates do not coincide. Thus, the proposed filtering for the
state jump focuses on estimate and covariance propagation without update.
Except for the true contact point position $\mathbf{p}^{c}_{t}$, the rest of
the true state is continuous across a foot landing, as explained in Sec. III.

####  IV-C1 Process model

From the proposed jump dynamics in Sec. III, the stochastic jump dynamics of
$\mathbf{X}_{t}$ can be approximately expressed as:

$\displaystyle{\mathbf{{X}}_{t^{+}}}$
$\displaystyle={\mathbf{{X}}_{t}}\begin{bmatrix}\mathbf{I}_{3}&[\mathbf{0}_{3\times
1},~{}\mathbf{0}_{3\times 1},~{}\mathbf{h}_{c}(\tilde{\mathbf{q}}_{t})]\\\
\mathbf{0}_{3\times
3}&\mathbf{I}_{3}\end{bmatrix}-{{\mathbf{{X}}_{t}}\begin{bmatrix}\frac{\partial\mathbf{h}_{c}}{\partial\mathbf{q}}(\mathbf{\tilde{q}}_{t})\mathbf{w}_{t}^{q}\\\
\mathbf{0}_{3\times 1}\end{bmatrix}^{\wedge}}$ (19)
$\displaystyle\triangleq\bm{\Delta}_{u_{t}}({\mathbf{{X}}_{t}})-{{\mathbf{{X}}_{t}}(\mathbf{w}^{\Delta}_{t})^{\wedge}},$

where the function $\bm{\Delta}_{u_{t}}({\mathbf{{X}}_{t}})$ is the
deterministic jump dynamics, and the encoder data $\mathbf{\tilde{q}}_{t}$
serves as the input. As the biases are continuous under a jump event,
${\bm{\theta}}_{t^{+}}={\bm{\theta}}_{t}$ holds.

Remark 3 (Group affine property): The jump map $\bm{\Delta}_{u_{t}}$ of the
state $\mathbf{X}_{t}$ possesses the discrete-time group affine property
defined in [21], and is independent of IMU biases $\bm{\theta}_{t}$. Thus, the
jump dynamics of the error $\bm{\xi}_{t}$ is independent of the true state and
is exactly linear. Moreover, from the expression of $\bm{\Delta}_{u_{t}}$ in
Eq. (19), we can see that $\bm{\Delta}_{u_{t}}$ is a group action on
$SE_{3}(3)$, under which the error =$\bm{\xi}_{t}$ naturally does not change.

####  IV-C2 Error equation

From Eq. (19), we obtain the dynamics of the logarithmic error $\bm{\xi}_{t}$
as:
$\bm{\xi}_{t^{+}}=\bm{\xi}_{t}-\mathbf{Ad}_{\bar{\mathbf{X}}}\mathbf{w}^{\Delta}_{t}$.
Indeed, as analyzed in Remark 3, the error does not jump under
$\bm{\Delta}_{u_{t}}$. Also, $\bm{\zeta}_{t^{+}}=\bm{\zeta}_{t^{-}}$ holds
since the IMU biases are continuous.

####  IV-C3 Propagation

Based on the deterministic portion of the jump model in Eq. (19), the
propagation of the state estimate at a jump event is:
$\bar{\mathbf{{X}}}_{t^{+}}=\bm{\Delta}_{u_{t}}(\bar{\mathbf{{X}}}_{t})$ and
$\bar{\bm{\theta}}_{t^{+}}=\bar{\bm{\theta}}_{t}$.

With the linear error equation of $\bm{\xi}$ and $\bm{\zeta}$ across a jump,
the propagation of the covariance matrix is expressed as:
$\mathbf{\mathbf{P}}_{t^{+}}=\mathbf{\mathbf{P}}_{t}+\bar{\mathbf{Q}}^{\Delta}_{t}$,
where
$\bar{\mathbf{Q}}^{\Delta}_{t}=\begin{bmatrix}\mathbf{Ad}_{\bar{\mathbf{X}}}\text{Cov}(\mathbf{w}^{\Delta}_{t})\mathbf{Ad}_{\bar{\mathbf{X}}}^{T}&\mathbf{0}_{3\times
3}\\\ \mathbf{0}_{3\times 3}&\mathbf{0}_{3\times 3}\end{bmatrix}$.

The complete algorithm of the proposed right-InEKF is summarized as Algorithm
1 in supplementary material.

Remark 4 (Imperfect InEKF): In the presence of IMU biases, the proposed filter
is no longer a “perfect” InEKF in the sense that the group affine and
invariant form properties no longer hold for continuous phases. Although the
linear equation in Eq. (13) is no longer independent of the true state, it
depends on the true state only through the bias terms while the remaining part
of the Jacobian matrix $\mathbf{A}_{t}$ is still independent of the true
state. Also, the measurement models are still independent of the true state
$\mathbf{X}_{t}$ and $\bm{\theta}_{t}$ as highlighted in Remark 2. For these
reasons, the linearization inaccuracy induced by the biases has a limited
impact on the continuous-phase propagation and update. Thus, the “imperfect
InEKF” with biases considered can still ensure rapid and accurate convergence
under large errors, which is experimentally confirmed on DRS locomotion as
reported in Sec. VI.

##  V Observability and Convergence Analysis

###  V-A Observability Analysis for Continuous Phases

As measurement update is performed during continuous phases, we only analyze
the continuous-phase observability.

Recall that the deterministic continuous-phase dynamics in Eq. (12) is group
affine in the absence of IMU biases $\bm{\theta}_{t}$ (Remark 1). Also, recall
that the measurement models in Eqs. (15) and (16) are in the right-invariant
observation form with respect to $\mathbf{X}_{t}$, regardless of the presence
of biases (Remark 2). Then, by Theorem 20 in [14], the observability of
$\mathbf{X}_{t}$ for the complete continuous-phase system, which has both
$\mathbf{X}_{t}$ and $\bm{\theta}_{t}$ as its state, is the same as that of
the simplified continuous-phase system without IMU biases. Thus, by Theorem 5
in [11], the local observability of $\mathbf{X}_{t}$ for the complete system
can be determined by the couple ($\mathbf{A}$, $\mathbf{H}$), with
$\mathbf{A}$ and $\mathbf{H}$ updated with bias-related terms removed (see
Sec. 6 of supplementary material).

With $\Delta t$ the duration of one propagation step, the discrete state
transition matrix $\bm{\Phi}$ is given by
$\bm{\Phi}=\text{expm}(\mathbf{A}_{t}\Delta t)$ [11] (see Sec. 6 of
supplementary material for the expression of $\bm{\Phi}$). Then, from
$\mathfrak{O}=\begin{bmatrix}(\mathbf{H})^{T},~{}(\mathbf{H}\bm{\Phi})^{T},~{}(\mathbf{H}\bm{\Phi}^{2})^{T},~{}\ldots\end{bmatrix}^{T}$,
we have:

$\displaystyle\mathfrak{O}$
$\displaystyle={\begin{bmatrix}(\mathbf{R}^{DRS}~{}[0,0,1]^{T})_{\times}&\mathbf{0}_{3\times
3}&\mathbf{0}_{3\times 3}&\mathbf{0}_{3\times 3}\\\ \mathbf{0}_{3\times
3}&\mathbf{0}_{3\times 3}&-\mathbf{I}_{3}&\mathbf{I}_{3}\\\
(\mathbf{R}^{DRS}~{}[0,0,1]^{T})_{\times}&\mathbf{0}_{3\times
3}&\mathbf{0}_{3\times 3}&\mathbf{0}_{3\times 3}\\\
-\frac{1}{2}(\mathbf{g})_{\times}\Delta t^{2}&-\mathbf{I}_{3}\Delta
t&-\mathbf{I}_{3}&\mathbf{I}_{3}\\\
\ldots&\ldots&\ldots&\ldots\end{bmatrix}.}$ (20)

As the first two columns of $(\mathbf{g})_{\times}$ are linearly independent,
the base roll and pitch angles are observable. Because all columns in the
second column block of $\mathfrak{O}$ are linear independent, the base
velocity $\mathbf{v}_{t}$ is observable. Yet, as the last two column blocks
are linearly dependant, the base position $\mathbf{p}_{t}$ and contact point
position $\mathbf{p}^{c}_{t}$ are unobservable.

The third column of $(\mathbf{g})_{\times}$ is always zero because only its
$z$-component is nonzero. Then, if the surface is non-horizontal, (i.e., the
third column of $(\mathbf{R}^{DRS}~{}[0,0,1]^{T})_{\times}$ is not all zero),
the yaw will be observable; otherwise, it is unobservable.

From the expression of $\mathfrak{O}$, we also know that: a) the contact
velocity $\mathbf{v}^{c}_{t}$ does not affect observability; b) either
measurement model ensures observable base roll and pitch; c) the proposed
measurement in Eq. (15) renders base yaw observable; and d) the previous
measurement in Eq. (16) makes base velocity observable.

###  V-B Convergence Property for Hybrid Error System

The proposed convergence analysis for the hybrid error system is built upon
previous analysis of the InEKF as a deterministic observer for systems without
state-triggered jumps [11]. Different from the previous work, this subsection
analyzes the effects of the jumps on the error convergence for the overall
hybrid error system.

We first analyze the error evolution across the deterministic discrete jump of
the system. Analyzing the state evolution across discrete, state-triggered
jumps (e.g., foot-landing impacts) is typically complex [3]. Yet, since the
jump map $\bm{\Delta}_{u_{t}}$ is a group action, the error $\bm{\xi}_{t}$
does not jump under $\bm{\Delta}_{u_{t}}$ despite the jump of the true state
$\mathbf{X}_{t}$. Also, the bias error $\bm{\zeta}_{t}$ is continuous across a
jump event. Thus, the hybrid, deterministic error dynamics is essentially
continuous for all time, and its error convergence is equivalent to that of
the deterministic continuous phases.

For continuous phases, the proposed filter meets the group affine condition
and invariant observation form without biases, as discussed in Sec. IV. Thus,
by the theory of InEKF [11], the proposed filter is locally asymptotically
convergent for the observable variables of the deterministic continuous phases
without biases. Accordingly, the local asymptotic convergence of the hybrid,
deterministic filter system is guaranteed in the absence of biases.

This analysis also supports the local asymptotic convergence of the existing
InEKF [12] designed for static surface locomotion, because the jump model in
[12] is a group action and its continuous-phase design also satisfies the
group affine and invariant observation conditions without biases.

##  VI EXPERIMENTS

###  VI-A Experimental setup

Figure 3: Experimental setup that includes a Digit bipedal humanoid robot and
a pitching treadmill (i.e., DRS). Figure 4: Profiles (TM1)-(TM3) of the
treadmill pitch angle $\theta^{DRS}(t)$.

The setup for experimental data collection (Fig. 3) is:

Treadmill (i.e, the tested DRS). A split-belt Motek M-gait treadmill is used
as a DRS. Its dimension is 2.3 m$\times$1.82 m$\times$0.5 m. To emulate a
rocking ship in sea waves, it performs a whole-body pitching motion without
belt translation.

Robot. The Digit robot is $1.6$ m tall, and each leg’s kinematic chain used by
the filter has $12$ joints. Different robot movements are tested: (RM1)
stepping and (RM2) standing. The robot is about 0.8 m away from the treadmill
center.

Treadmill motion profiles. To test filter performance under different DRS
motions, two different profiles of the treadmill’s pitch angle $\theta^{DRS}$
(Fig. 3) are tested: (TM1) a non-periodic trapezoidal wave, $f_{trap}(t)$, and
(TM2) a sine wave $2.5^{\circ}\sin(\pi t)$. Under (TM1) and (TM2), the maximum
contact point speeds $\|\mathbf{v}^{c}_{t}\|$ are respectively $0.41$ m/s and
$0.11$ m/s. To test the filter’s robustness under surface motion inaccuracy, a
fictitious profile is considered: (TM3)
$\theta^{DRS}(t)=f_{trap}(t)+5.1^{\circ}+1.7^{\circ}\sin(\pi t)$ with the
actual profile (TM1) used in experiments. Figure 4 shows all profiles.

Figure 5: Time-lapse figures of Digit walking on a rocking treadmill. The black arrow indicates the treadmill’s direction of rotation. Figure 6: Procedure of obtaining the 3-D contact point position in the DRS frame using the ArUco markers and the robot’s on-board RGB-D camera. Figure 7: Validation results of the proposed method for obtaining the contact point velocity $\mathbf{\tilde{v}}_{t}^{c}\triangleq[\tilde{v}^{c}_{x},\tilde{v}^{c}_{y},\tilde{v}^{c}_{z}]^{T}$ under Case C. The velocity along the y-direction, $\tilde{v}^{c}_{y}$, is zero because the treadmill does not move in that direction. TABLE I: Noise standard deviation for inekf-srs and inekf-drs. Measurement type |  InEKF-SRS |  InEKF-DRS
---|---|---
Linear acc. (m/s2) |  $0.4$ |  $0.4$
Angular vel. (rad/s) |  $0.01$ |  $0.01$
Acc. bias (m/s3) |  $0.001$ |  $0.001$
Gyroscope bias (rad/s2) |  $0.0001$ |  $0.0001$
Contact vel. (m/s) |  $0.01$ |  $0.01$
Encoder $(^{\circ})$ |  $1$ |  $1$
DRS orientation (∘) |  N/A |  $1$

On-board sensors used. Digit’s on-board sensors used (Fig. 3) are: an IMU,
joint encoders, a RealSense RGB-D camera, and the robot’s proprietary contact
detector. The camera returns data at 15 Hz, and the remaining sensors stream
data at the same rate within $60$-$90$ Hz. Cortex motion capture cameras
provide the ground truth. ArUco markers are attached to the treadmill,
emulating the real-world scenario where legged robots that navigate within a
DRS (e.g., a vessel at sea) can only see landmarks attached to the DRS but not
any landmarks on the earth’s ground. The markers are sensed by the camera to
obtain the camera pose in the treadmill frame, which is then used to compute
contact point velocity as explained in the next subsection.

Data collection cases. Figure 5 shows screenshots of experiments. The filter
is simulated in MATLAB using four experimentally collected data sets under
different robot and treadmill motions: Case A: Combination of (RM1) and (TM1);
Case B: Combination of (RM1) and (TM2); Case C: Combination of (RM2) and
(TM1); and Case D: Combination of (RM1) and (TM3), where the actual profile is
(TM1) but the filter uses the inaccurate data (TM3). The experiment video is
available at: https://youtu.be/ScQIBFUSKzo.

###  VI-B Filter Setting

Filters compared. The proposed filter (denoted as “InEKF-DRS”) is compared
with an InEKF designed for locomotion on a static rigid surface [12] (denoted
as “InEKF-SRS”). The InEKF-SRS models the deterministic contact point motion
as $\dot{\mathbf{p}}^{c}=\mathbf{0}$, and uses the position measurement in Eq.
(16) alone. It renders the base orientation (except for yaw) and velocity
observable. It has realized substantially faster convergence under large
errors during stationary surface locomotion, as compared with EKF-based method
[1]. Also, the proposed InEKF-DRS is compared with an EKF-based filter, which
we formulate by augmenting the existing EKF designed for static surfaces [1]
to explicitly handle nonstationary surfaces. Details of the augmentation and
comparison results are in Sec. 8.3 of supplementary material. The rest of this
section focuses on comparing InEKF-DRS and InEKF-SRS.

Contact point velocity computation. The contact point velocity
$\tilde{\mathbf{v}}^{c}$ serves as an input to the continuous-phase process
model of the proposed InEKF-DRS. To obtain the contact point velocity
$\tilde{\mathbf{v}}^{c}$ (see Fig. 6), we first obtain the camera pose in the
DRS frame by processing the features of the ArUco markers in the camera
images, which we then use to compute the 3-D contact point position in the DRS
frame (${}^{DRS}\mathbf{p}^{c}$) through forward kinematics. Next, we estimate
the contact point velocity $\mathbf{\tilde{v}}^{c}$ based on Eq. (4) using the
known treadmill motion data. Details of this procedure are in Sec. 7 of
supplementary material. Results in Fig. 7 validate the accuracy of the
proposed contact point velocity sensing.

Covariance settings. Table I shows the noise standard deviation (SD) of both
filters. The SD for the accelerometer, gyroscope, and their corresponding
biases are obtained from the manufacturer’s manual with a slight adjustment
for better performance. The SD for the encoder readings is adopted from the
previous filter [12] designed for a similar robot. The SD for the contact-
point velocity and orientation-based measurement are tuned for a reasonable
performance. The initial value of the covariance $\mathbf{P}$ is set as an
identity matrix.

Initial estimation errors. For a fair comparison, the two filters are
simulated under the same large range of initial estimation errors. The initial
velocity and orientation errors in each direction are respectively uniformly
distributed within [$-1.5$,$1.5$] m/s and [$-1$,$1$] rad.

Figure 8: Base velocity and orientation estimation results of the two filters,
InEKF-DRS and InEKF-SRS, for Cases A and D. The red, shaded area indicates the
range of the state estimates for 10 runs. The red, solid line is the ground
truth. The blue, dashed line is the treadmill angle.

###  VI-C Computation Time Comparison

In MATLAB, both filters take less than $1$ ms to compute one estimation cycle
(i.e., one propagation and one update step), confirming their validity for
real-time estimation.

###  VI-D Convergence Rate and Yaw Observability Comparison

Figure 8-a) displays the estimation results of InEKF-DRS (proposed) and InEKF-
SRS under Case A where the treadmill stays at a pitch angle of $-8^{\circ}$
for approximately 2.8 sec and then begins to pitch until reaching $+8^{\circ}$
in 0.5 sec.

Both filters drive the error of base roll, pitch, and velocity closer to zero,
indicating their observability as predicted in Sec. V and previous work [1,
12]. In terms of the convergence rates for these variables, subplot a) shows
that the proposed InEKF-DRS is faster than InEKF-SRS, driving the error close
to zero within 1 sec. This is because InEKF-DRS considers the surface motion
and has an additional measurement (Eq. (15)) that corrects estimates.

Under InEKF-DRS, the yaw estimate converges close to the ground truth in
approximately 3 sec, which supports the observability analysis in Sec. V that
the yaw angle is observable if the DRS/treadmill is not horizontal. Yet, the
yaw convergence is slower than pitch and roll, possibly because both
observations in Eqs. (15) and (16) help correct the roll and pitch estimates
whereas only the former corrects the yaw estimate. Finally, as previously
revealed [1], the yaw error divergence under InEKF-SRS confirms that the base
yaw is indeed non-observable with InEKF-SRS.

Figure 9: Accuracy comparison of InEKF-SRS and InEKF-DRS (proposed) for the
estimation of base velocity and roll and pitch angles under Case A.

###  VI-E Accuracy Comparison

Table II shows the comparison of the root-mean-square (RMS) estimation errors
for base orientation (including yaw) and velocity under Case A. Figure 9 shows
the corresponding time evolution of the errors for base roll, pitch, and
velocity under Case A. The table and the figure show that the proposed InEKF-
DRS is more accurate in velocity and orientation estimation compared with
InEKF-SRS.

###  VI-F Performance under Different DRS and Robot Movements

Figures 1-a) and 1-b) in supplementary material respectively show the
estimation results of the two filters under Case B (where the treadmill motion
is different from Case A) and Case C (where the robot stands on the treadmill
instead of walking as in Case A). The plots show that the performance
comparison of the two filters under Cases B and C are similar to Case A (i.e.,
Fig. 8-a)), in terms of convergence rate and accuracy, indicating the InEKF-
DRS can effectively handle different DRS and robot movements.

Comparing the yaw estimate under the InEKF-DRS in Cases A-C, we notice that
the yaw estimate in Case C converges faster than Cases A and B. In Case C, the
treadmill remains horizontal for the first 10 sec, during which the yaw
estimate does not converge. Yet, once the treadmill begins to rock at $t=10$
sec, the yaw estimate converges close to the ground truth within 1 sec,
whereas it takes about 3 sec for the yaw estimate to enter a similar
neighborhood under Cases A and B. This might be due to the fact that in Case
C, by the time the treadmill begins to pitch, the estimates of the rest
observable state are already sufficiently accurate, making the yaw error
correction faster than Cases A and B.

TABLE II: RMS error comparison under Case A. State variables |  InEKF-SRS |  InEKF-DRS
---|---|---
$v_{x}$ (m/s) |  $0.3320$ |  $0.2051$
$v_{y}$ (m/s) |  $0.2488$ |  $0.1955$
$v_{z}$ (m/s) |  $0.1438$ |  $0.1025$
yaw (rad) |  $0.9294$ |  $0.2516$
pitch (rad) |  $0.0897$ |  $0.0413$
roll (rad) |  $0.1365$ |  $0.0318$

###  VI-G Robustness Assessment

Results from Cases A (Fig. 8 a) and D (Fig. 8 b) confirm the robustness of the
proposed InEKF-DRS under inaccurate surface pose knowledge. Case D emulates
the scenario where the DRSes motion monitoring system fails to provide
accurate DRS pose. Subplots a and b show that the filter performance (e.g.,
convergence rate, accuracy, and yaw observability) under Case D is similar to
that under Case A. Specifically, the velocity estimate under InEKF-DRS
converges to the ground truth in all directions within 1 sec. Also, the
orientation convergence rates are similar: the roll and pitch estimates
converge close to the ground truth within 0.3 sec, and the yaw angle converges
within 3 sec. Longer periods (10-30 sec) of estimation results for Cases A-D
are shown and discussed in supplementary material.

##  VII DISCUSSION

This study has designed an InEKF that estimates the orientation and velocity
of a bipedal robot that walks on a DRS with a known, relatively significant
motion, by fusing the known surface pose and the leg, visual, and inertial
odometries. Similar to the InEKF [12] and EKF [1] designed for stationary
surfaces, the filter uses the IMU motion dynamics as the process model, and
the 3-D contact point position and leg kinematics to form a measurement model.
Different from the previous work, the proposed contact-point process model
does not assume that the contact point is static, but instead explicitly
considers its movement in the world. Also, we have introduced a right-
invariant measurement model based on the rotational kinematic relationship
between the surface and support foot. Thanks to these features, the filter
ensures accurate estimation under relatively large surface motion and
estimation errors, as shown by the RMS errors in Table II and the state
trajectories in Figs. 8 and 9.

The proposed filter is suitable for a DRS with a relatively accurately known
surface pose profile, but may not be effective under overly inaccurate or
unknown profiles. One potential solution is to extend this filter to estimate
the surface pose, by constructing a matrix Lie group that includes the surface
pose in the state and formulating an InEKF with fundamental benefits.

This study also assumes the robot’s feet do not persistently and significantly
slip on the surface. When the surface is slippery [22], the support foot may
move relative to the surface, causing discrepancy between the actual robot
movement and the models. Yet, the proposed method could be extended to address
slippage during DRS locomotion by incorporating existing techniques [23, 2]
such as using an RGB-D sensor to measure the base velocity [23].

##  VIII CONCLUSION

This paper has introduced a right-invariant extended Kalman filter for bipedal
humanoid walking on a moving surface. The filter design explicitly considered
the known surface movement and hybrid robot behaviors while enjoying the
fundamental benefits of satisfying the attractive group-affine condition and
invariant observation form in the absence of IMU biases. Observability
analysis for the continuous locomotion phases showed that the robot’s base
velocity and roll and pitch angles are observable, and the base yaw angle
becomes observable when the DRS is not horizontal. Stability analysis proved
the asymptotic error convergence of these observable states for the hybrid
deterministic system. Experimental results of humanoid walking on a pitching
treadmill validated the enhanced accuracy and convergence rate of the proposed
filter over existing work, in the presence of large estimation errors and
moderate DRS movement.

## Acknowledgment

The authors would like to thank M. Ghaffari and A. Saccon for constructive
feedback on theoretical derivation.

## References

  * [1] 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,” Rob., vol. 17, pp. 17–24, 2013.
  * [2] J.-H. Kim, S. Hong, G. Ji, S. Jeon, J. Hwangbo, J.-H. Oh, and H.-W. Park, “Legged robot state estimation with dynamic contact event information,” IEEE Rob. Autom. L., vol. 6, no. 4, pp. 6733–6740, 2021.
  * [3] A. Iqbal, Y. Gao, and Y. Gu, “Provably stabilizing controllers for quadrupedal robot locomotion on dynamic rigid platforms,” IEEE/ASME Trans. Mechatron., vol. 25, no. 4, pp. 2035–2044, 2020.
  * [4] A. Iqbal, Z. Mao, and Y. Gu, “Modeling, analysis, and control of slip running on dynamic platforms,” ASME L. Dyn. Syst. Contr., vol. 1, no. 2, 2021.
  * [5] Y. Gao and Y. Gu, “Global-position tracking control of multi-domain planar bipedal robotic walking,” in Proc. ASME Dyn. Syst. Contr. Conf., vol. 59148, p. V001T03A009, 2019.
  * [6] Y. Gu, B. Yao, and C. S. G. Lee, “Exponential stabilization of fully actuated planar bipedal robotic walking with global position tracking capabilities,” ASME J. Dyn. Syst. Meas. Contr., vol. 140, no. 5, 2018.
  * [7] Y. Gao and Y. Gu, “Global-position tracking control of a fully actuated NAO bipedal walking robot,” in Proc. Amer. Contr. Conf., pp. 4596–4601, 2019.
  * [8] P.-C. Lin, H. Komsuoglu, and D. E. Koditschek, “Sensor data fusion for body state estimation in a hexapod robot with dynamical gaits,” IEEE Trans. Rob., vol. 22, no. 5, pp. 932–943, 2006.
  * [9] M. F. Fallon, M. Antone, N. Roy, and S. Teller, “Drift-free humanoid state estimation fusing kinematic, inertial and Lidar sensing,” in Proc. IEEE-RAS Int. Conf. Humanoid Rob., pp. 112–119, 2014.
  * [10] P. Ramadoss, G. Romualdi, S. Dafarra, F. J. A. Chavez, S. Traversaro, and D. Pucci, “DILIGENT-KIO: A proprioceptive base estimator for humanoid robots using extended Kalman filtering on matrix Lie groups,” arXiv preprint:2105.14914, 2021.
  * [11] A. Barrau and S. Bonnabel, “The invariant extended Kalman filter as a stable observer,” IEEE Trans. Autom. Contr., vol. 62, no. 4, pp. 1797–1812, 2017.
  * [12] R. Hartley, M. Ghaffari, R. M. Eustice, and J. W. Grizzle, “Contact-aided invariant extended Kalman filtering for robot state estimation,” Int. J. Rob. Res., vol. 39, no. 4, pp. 402–430, 2020.
  * [13] T.-Y. Lin, R. Zhang, J. Yu, and M. Ghaffari, “Deep multi-modal contact estimation for invariant observer design on quadruped robots,” arXiv preprint:2106.15713, 2021.
  * [14] A. Barrau, Non-linear state error based extended Kalman filters with applications to navigation.  PhD thesis, Mines Paristech, 2015.
  * [15] S. Küchler, J. K. Eberharter, K. Langer, K. Schneider, and O. Sawodny, “Heave motion estimation of a vessel using acceleration measurements,” IFAC Proc. World Congress, vol. 44, no. 1, pp. 14742–14747, 2011.
  * [16] K. A. Hamed, A. D. Ames, and R. D. Gregg, “Observer-based feedback controllers for exponential stabilization of hybrid periodic orbits: Application to underactuated bipedal walking,” in Proc. Amer. Contr. Conf., pp. 1438–1445, 2018.
  * [17] N. J. Kong, J. J. Payne, G. Council, and A. M. Johnson, “The salted Kalman filter: Kalman filtering on hybrid dynamical systems,” Autom., vol. 131, p. 109752, 2021.
  * [18] Y. Gao, C. Yuan, and Y. Gu, “Invariant extended Kalman filtering for hybrid models of bipedal robot walking,” in Proc. of IFAC Mod., Est., Contr. Conf., vol. 54, pp. 290–297, 2021.
  * [19] J. Sola, J. Deray, and D. Atchuthan, “A micro Lie theory for state estimation in robotics,” arXiv preprint:1812.01537, 2018.
  * [20] “VectorNav,” https://www.vectornav.com/, Accessed: 2021-08-08.
  * [21] A. Barrau and S. Bonnabel, “Invariant Kalman filtering,” An. Rev. Con., Rob., Aut. Sys., vol. 1, pp. 237–257, 2018.
  * [22] M. Trkov, K. Chen, J. Yi, and T. Liu, “Inertial sensor-based slip detection in human walking,” IEEE Trans. Autom. Sc. Eng., vol. 16, no. 3, pp. 1399–1411, 2019.
  * [23] S. Teng, M. W. Mueller, and K. Sreenath, “Legged robot state estimation in slippery environments using invariant extended Kalman filter with velocity update,” arXiv preprint:2104.04238, 2021.

|  Yuan Gao received his B.S. degree in Mechanical Engineering from China
Jiliang University, Hangzhou, China in 2014, and the M.S. degree in Mechanical
Engineering from Arizona State University in 2016. He is currently a Ph.D.
student in the Department of Mechanical Engineering at the University of
Massachusetts Lowell, Lowell, MA, U.S.A..
---|---
|  Chengzhi Yuan received the B.S. and M.S. degrees from the South China
University of Technology, Guangzhou, China, in 2009 and 2012, respectively,
and the Ph.D. degree in mechanical engineering from North Carolina State
University, Raleigh, NC, U.S.A., in 2016. He is currently an Assistant
Professor with the Mechanical, Industrial and Systems Engineering Department
and the Director of the Intelligent Control and Robotics Laboratory,
University of Rhode Island, Kingston, RI, U.S.A.. His research interests
include adaptive learning and control, hybrid systems, and multirobot-
distributed control.
---|---
|  Yan Gu received the B.S. degree in Mechanical Engineering from Zhejiang
University, Hangzhou, China, in 2011 and the Ph.D. degree in Mechanical
Engineering from Purdue University, West Lafayette, IN, U.S.A., in 2017. She
joined the faculty of the School of Mechanical Engineering at Purdue
University in 2022. Prior to joining Purdue, she was with the Department of
Mechanical Engineering at the University of Massachusetts Lowell. Her research
interests include nonlinear control, hybrid systems, legged locomotion, and
wearable robots. She received the NSF CAREER Award in 2021.
---|---