State Observer for Linear Systems with Explicit Constraints: Orthogonal Decomposition Method

Abstract

In this paper, an orthogonal decomposition-based state observer for systems with explicit constraints is proposed. State observers have been an integral part of robotic systems, reflecting the practicality and effectiveness of the dynamic state feedback control, but the same methods are lacking for the systems with explicit mechanical constraints, where observer designs have been proposed only for special cases of such systems, with relatively restrictive assumptions. This work aims to provide an observer design framework for a general case linear time-invariant system with explicit constraints, by finding lower-dimensional subspaces in the state space, where the system is observable while giving sufficient information for both feedback and feed-forward control. We show that the proposed formulation recovers minimal coordinate representation when it is sufficient for the control law generation and retains non-minimal coordinates when those are required for the feed-forward control law. The proposed observer is tested on a flywheel inverted pendulum and on a quadruped robot Unitree A1.

1. Introduction

State estimation is needed when values of some of the state variables required by the control law are not directly available from the measurements. This can be the result of the lack of sensors, measurement noise, or be the result of the chosen parametrization of the system. In a typical scenario, control law requires full state information, while only some of the states are directly measured. This problem has been addressed in great detail, leading to solutions in the form of, among others, optimal Dynamic Output Feedback, Luenberger State Observers, and Kalman Filters. The related theory has been well developed for linear dynamical systems; however, it has also been extended and actively applied to nonlinear dynamical systems, in particular to mechanical systems and robots, often with the use of local linearization of their nonlinear dynamics.

While many mechanical systems and robots can be effectively described by systems of ordinary differential equations, making it possible to use the first term in Taylor expansion of their dynamics to apply aforementioned linear state estimation methods, some are better described as differential-algebraic equations (DAE); these include systems with explicit mechanical constraints [1]. In particular, walking robots, robot arms in contact with the environment, and many other similar systems are better described with a specific conventional set of coordinates, which can be non-minimal; for walking robots, this description is called floating-base dynamics, combining joint coordinates and the position and orientation of the robot’s “base”, which is usually a robot’s trunk [2,3,4]. There are a number of reasons why this description can be favored: it allows for maintaining the structure of the equations while contacts are being acquired and released, it explicitly includes contact forces, allowing to place them under additional constraints, such as a friction cone, and, in some cases, it is simply difficult to find a singularity-free minimal coordinate representation for multi-body dynamics. However, dynamics represented as a DAE poses a problem when classical control and state estimation methods are to be applied.

Over the last 40 years, there has been a continuous research effort in establishing control and estimation methods, suitable for systems with explicit constraints. Theory and methods have been well developed for a general case when systems with explicit constraints were presented in descriptor form [5,6,7]. Separately, notable progress has been seen in solving an inverse dynamics problem for the second-order systems with constraints [3,4]. Both of those will be discussed in more detail in the next section. A few types of stabilizing control have been proposed: based on second-order error dynamics with suitable projections, and on projecting local linearization of the dynamics equations into the null space of the constraints matrix, allowing for formulating linear-quadratic regulator for the constrained system [8,9]. However, an equivalent observer design (based on orthogonal decomposition) so far has not been proposed. As will be discussed in the following sections, linear dynamics of the systems with explicit constraints naturally takes affine form, due to the fact that part of the state is constrained to be constant. With that in mind, it is important not only to design stabilizing feedback control but also the appropriate feed-forward component, to account for affine terms in the system dynamics.

Additionally, constraints introduce closed kinematics chains, making the systems over-actuated; on the other hand, in the absence of constraints systems such as floating-base walking robots are underactuated and not controllable. This creates additional challenges regarding state estimation, as some of the states in the floating base coordinates are neither observable nor do they influence the dynamics of the system. Moreover, the structure of the dynamics of mechanical systems makes the same variables appear both in the state and in its time derivative (this happens with generalized velocities in Lagrange dynamics or manipulator equations when those are expressed in state-space form). This information can be lost if the constraints are placed on the state derivatives only, as was done in [8]; if accounted for, it additionally simplifies the state estimation task, as we will show in Section 6.

The main contributions of this paper are as follows:

1.

we propose a state observer that converges to the true values of the state for linear dynamical systems with explicit mechanical constraints;

2.

we formulate a lower-dimensional state observer, able to take advantage of the constraints placed explicitly on the state variables, which is a natural scenario for mechanical systems; thus, the obtained observer has demonstrably lower requirements with respect to the available measured outputs, without compromising the control law design;

3.

we propose an additional change of basis, based on the intersection of the row space of the state matrix for static states, and the state space of the previously mentioned observer, leading to an observer with a lower-dimensional state, able to recover minimal coordinate representation, as we demonstrate in Section 9.

The rest of the paper is organized as follows: Section 2 gives an overview of the state of the art and expands on where the proposed method stands relative to the previous ones, Section 3 gives a brief description of the materials and methods used, Section 4 gives a mathematical formulation of the problem and covers preliminaries, Section 5 proposes constrained observer design, Section 6 gives a lower-dimensional version of the observer, overcoming the limitations of the constraint representation, Section 7 proposes an even lower-dimensional version, excluding the static states that do not play in the feedback or feed-forward components of the control law, Section 8 discusses gain design for the proposed observer, Section 9, Section 10 and Section 11 discuss experimental and numerical case-studies: flywheel inverted pendulum, quadruped robot Unitree A1, and a flat quadruped moving in the sagittal plane, Section 12 demonstrates a numerical comparison study of the proposed Observer and an Extended Kalman Filter, and Section 13 discusses the application of the proposed method in control and sensor distribution design for mobile robots.

2. State of the Art

2.1. Orthogonal Projection and Decomposition Methods for Systems with Explicit Constraints

A state estimation method proposed in this paper is based on orthogonal projections. There is a number of existing orthogonal projection and decomposition-based methods developed for systems with explicit constraints, which we review in this subsection. The results closest to the one proposed in the paper are found in papers on control of mechanical systems with explicit constraints, so we focus our discussion on those.

Models of mechanical systems with explicit constraints are often presented as differential-algebraic equations, which prevents direct application of methods, designed for ordinary differential equations (ODE). This motivated a group of work, aimed at replacing the DAE description of the dynamics with an equivalent ODE. In [10], a procedure was proposed for simulation of mechanical systems with explicit constraints, which establishes a connection with earlier works in the field of numerical methods for DAE, such as [11] and others, which tackled a very similar problem. In [10,12], as well as in later works [3,4,13], orthogonal projections have been used. In [10,12], a number of new equivalent models in non-minimal coordinates were proposed, while, in [3,4,13], following earlier research [14,15], the focus was on control law design. However, as was demonstrated in [4], a number of the independently developed projection-based control methods yield the same (up to torque redundancy resolution) control laws.

Another family of projection-based methods have been designed for inverse dynamics problem: the problem of finding such control input that produces desired acceleration for a mechanical system [3,4,16]. Alternative formulations have been proposed; in [17], the DAEs were solved directly, and, in [18,19,20] and others, convex optimization has been used to solve the problem, taking into account contact models (such as friction cone). We should note that the presented methods assume partial or complete knowledge of the state of the system.

The last family of projection-based methods we mention here are schemas for solving the linear-quadratic regulator (LQR) problem for mechanical systems with explicit constraints. The solution to this problem was originally proposed in [8,9] and in a different form in [21]. The method presented in [8] takes advantage of the known structure of the constraint matrix to project the locally linearized dynamics into its null space, resulting in a local linear model in minimal coordinates. With that, LQR can be formulated and optimal feedback control gains can be found. We should note that the method is motivated by practical problems, such as control of bipedal robots, which are naturally nonlinear. Therefore, the methods proposed in [8,9] take locally linearized models, but then use them to design infinite-horizon LQR or a time-variant LQR where the model is obtained by linearization along the nominal trajectory. While guarantees of stability of the nonlinear closed-loop dynamics and optimality of the proposed control law are a subject of further investigation, the practical applicability of the methods has been demonstrated in hardware experiments and its simplicity advocates for its use. Present work has a similar motivation, and the new observer designs we propose for the systems with explicit constraints can be used in a similar way. The same as control design presented in [8], our state estimation method takes advantage of the orthogonal projections, but, in order to construct the state observer, we consider both the null space and the row space components of the linear dynamics and take advantage of the linear relations between components of the state and the state derivative, which allow for writing constraints placed on the system’s state directly and exploit them in the observer design.

2.2. State Estimation for Systems with Explicit Constraints

Systems with explicit constraints can be represented as so-called descriptor systems, which can be thought of as a form of a DAE or as a mix of static and dynamic equations with control inputs [5]. Concerning state estimation, the descriptor system theory presented a wide range of results: state observers for general-case linear descriptor systems were proposed and conditions for their existence were established [6,7,22,23,24,25], extensions for some classes on nonlinear descriptor systems were proposed [26,27] and adaptive and robust observers were designed [28,29], among other contributions. Additionally, it was demonstrated that mechanical systems with constraints can be written in the descriptor form [1] and Luenberger observers for such systems were proposed [30,31]. This body of work might be unduly overlooked in modern legged robotics and similar fields. However, one of the limitations of the methods developed for descriptor systems is the aim to estimate the whole descriptor state, which in the case of mechanical systems includes position and velocity variables, as well as aggregate reaction forces [1]; it is easy to find examples where such a system is not observable, while estimation of only a subset of the state variables is sufficient for stabilizing feedback control design (we provide one such example in the paper). Our contribution compared to results in descriptor systems theory is the systematic way to produce a lower-dimensional state observer based on orthogonal projections, which can be seen as automatically recovering a minimal representation of the static component on the descriptor state.

We continue by briefly discussing state-of-the-art techniques in state estimation of walking robots, since walking robot models are among the most prominent examples of systems with explicit constraints. In [32], an observer design for a flat bipedal robot is considered, and reaction forces multiplied by constraints Jacobian are viewed as an additive uncertain term in the dynamics model, allowing for freeing the problem formulation from explicit constraints. In [33], an observer is designed to estimate the state of the so-called ’cart on the table’ model, which represents a simplified model of the motion of the center of mass of the walking robot. In [34], the motion of a humanoid robot was divided into phases based on the contact interaction scenario, and then the unknown reaction forces were expressed out of the dynamics equation, leading to a formulation with no explicit constraints, which allowed the application of the classic Luenberger observer. A similar approach is shown in [35]. In [36], the same was done but an Extended Kalman Filter was used. In [37], the sliding-mode observer was used, with no explicit constraints in the observer formulation.

Overall, one of the more prevailing approaches is the use of extended Extended Kalman Filter to estimate the state of the robot, taking advantage of the information about the gait and expected nature of the contact interaction with the environments and the kinematic structure of the robot, while making some assumptions about mentioned contact interactions and the availability of specific sensors [38,39,40,41]. These methods have been successfully used for legged robots in a number of experiments and practical tasks. In relation to the method proposed in this paper, we can see them as effective use of platform-specific properties to build a highly effective estimator, whereas the proposed method enjoys the advantage of being general, potentially requiring less work to be adapted to a new platform. This relation is similar to the one between linear-quadratic regulators and platform-specific Model-Predictive Control (MPC) algorithms developed for specific types of legged robots.

Finally, we note that, in some cases, the walking robot dynamics can be formulated in a hybrid constraint-free form, which allows the design of hybrid dynamics state observers and disturbance observers [42,43]. However, their use is limited to the cases when a hybrid description of the system’s dynamics is available.

Figure 1 presents a diagram, sorting the aforementioned multitude of methods with respect to the plant model types they use (linear: with explicit constraints and in descriptor form, and nonlinear: in floating-base form and in hybrid dynamics form with minimal coordinates), and the type of feedback control they implement: full state feedback (static feedback) and dynamic output feedback (in Luenberger form and in general form).

Analyzing the diagram, we note that, while feedback control design is well-developed for hybrid systems in minimal coordinates, the corresponding hybrid dynamics models are not easy to work with for high-dimensional systems with numerous and varied contact interactions, such as quadrupeds and humanoid robots. Using a nonlinear floating-base description of the dynamics leads to non-minimal coordinates, and allows the use of a number of platform-specific solutions. A more general framework has been built for linear dynamical systems expressed in descriptor form, where some solutions have been found for both static and dynamic output feedback, subject to conditions on admissibility and observability. Depending on the formulation, the descriptor vector includes both state variables and reaction force variables; it can be shown that, for some cases, a lower-dimensional observer would be sufficient for control design, whereas observing the full state might not be possible (as demonstrated in Section 9). This is where linear controller and observer designs based on subspace projections have an advantage. The method proposed in this paper gives an observer design based on null and row space projections for the constraint matrix, taking additional advantage of the constraints placed directly on the state variables and of the analysis of the effect static states have on the dynamics of the system, automatically excluding those that have none from the observer state.

The proposed method can be seen as a combination of a Luenberger observer for the space of non-static states and a disturbance observer for the rest. If the control input that turns the system into a node is exactly known (via Inverse dynamics, for example), the observation of the static component of the state might not be needed. However, as we mentioned before, algorithms that solve this problem usually assume that information about the full state of the robot is available [3,4]. Thus, the proposed observer fills the gap in the family of methods for systems with explicit constraints based on orthogonal projections.

3. Materials and Methods

This paper presents a new type of state observer based on the orthogonal projections of the system’s dynamics. The bulk of the work is done via analytic derivations, verified in simulations and experimental studies. For verification, a table-top flywheel inverted pendulum was used, described in Section 9. Details of the experiment, measurements, and the model of the robot are presented in the section. For further analysis, a quadruped robot Unitree A1 was used; the parameters of this robot are publicly available.

4. Problem Statement

4.1. Dynamics with Explicit Constraints: Model

A linear time-invariant dynamical system with explicit constraints can be presented as follows:

$$\left\{\begin{array}{c}\dot{\mathbf{x}}=\mathbf{A}\mathbf{x}+\mathbf{B}\mathbf{u}+\mathbf{F}\lambda \hfill \\ \mathbf{G}\dot{\mathbf{x}}=\mathbf{0}\hfill \\ \mathbf{y}=\mathbf{C}\mathbf{x},\hfill \end{array}\right.$$

(1)

where $\mathbf{x}\in {\mathbb{R}}^n$, $\mathbf{u}\in {\mathbb{R}}^m$ and $\mathbf{y}\in {\mathbb{R}}^h$ are state, control input, and measured output, $\mathbf{A}$, $\mathbf{B}$, and $\mathbf{C}$ are state, control, and observation matrices, respectively, $\mathbf{F}$ and $\mathbf{G}$ are constraint Jacobian and constraint matrix and $\lambda \in {\mathbb{R}}^k$ are reaction forces (or Lagrange multipliers). From the given formulation, it follows that the rate of change for the state of the system lies in the subspace of ${\mathbb{R}}^n$. We can treat $\lambda$ as input generated by the environment, acting on the plant, which would allow us to use linear-fractional transformation on it (see Figure 2). Expressing out $\lambda$ leads to the following matrix representation:

$$\dot{\mathbf{x}}=(\mathbf{I}-\mathbf{F}{(\mathbf{G}\mathbf{F})}^{-1}\mathbf{G})(\mathbf{A}\mathbf{x}+\mathbf{B}\mathbf{u})$$

(2)

System (2) represents dynamics (5). Note that, while (5) has explicit DAE form, (2) appears to be an ODE; however, the values of $\dot{\mathbf{x}}$ still belong to the null space of constraint matrix $\mathbf{G}$; the transformation (2) only rids the equations of the reaction forces $\lambda$. We introduce short-hand notation for the matrices of this system:

$$\begin{array}{cc}\hfill & {\mathbf{A}}_c=(\mathbf{I}-\mathbf{F}{(\mathbf{G}\mathbf{F})}^{-1}\mathbf{G})\mathbf{A}\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill & {\mathbf{B}}_c=(\mathbf{I}-\mathbf{F}{(\mathbf{G}\mathbf{F})}^{-1}\mathbf{G})\mathbf{B}.\hfill \end{array}$$

(4)

where ${\mathbf{A}}_c$ and ${\mathbf{B}}_c$ are state and control matrices of the system (2), which now takes the form:

$$\left\{\begin{array}{c}\dot{\mathbf{x}}={\mathbf{A}}_c\mathbf{x}+{\mathbf{B}}_c\mathbf{u}\hfill \\ \mathbf{G}\dot{\mathbf{x}}=0\hfill \\ \mathbf{y}=\mathbf{C}\mathbf{x},\hfill \end{array}\right.$$

(5)

With that, we can use some of the standard results known from the literature on systems with explicit constraints, such as the linear feedback method from [8] that we will introduce later in this section. Note that matrices ${\mathbf{A}}_c$ and ${\mathbf{B}}_c$ can also be obtained as a linearization of nonlinear dynamics, solved for the higher derivatives, as illustrated by the next example.

Example 1.

Consider a mechanical system with explicit constraints:

$$\left\{\begin{array}{c}{\mathbf{H}}_n\ddot{\mathbf{q}}+{\mathbf{C}}_n\dot{\mathbf{q}}+{\mathbf{g}}_n={\mathbf{T}}_n\mathbf{u}+{\mathbf{F}}_n^{\top }\lambda ,\hfill \\ {\mathbf{F}}_n\ddot{\mathbf{q}}+{\dot{\mathbf{F}}}_n\dot{\mathbf{q}}=0\hfill \end{array}\right.$$

where $\mathbf{q}$ are generalized coordinates, ${\mathbf{H}}_n$, ${\mathbf{C}}_n$, and ${\mathbf{T}}_n$ are generalized inertia, Coriolis and control matrices, ${\mathbf{g}}_n$ is the generalized gravity vector and ${\mathbf{F}}_n$ is a constraint Jacobian; all mentioned quantities are nonlinear functions of $\mathbf{q}$. We can solve for $\ddot{\mathbf{q}}$:

$$\ddot{\mathbf{q}}={\mathbf{f}}_n(\mathbf{q},\dot{\mathbf{q}},\mathbf{u})=(\mathbf{I}-{\mathbf{M}}_n{\mathbf{F}}_n){\mathbf{H}}_n^{-1}({\mathbf{T}}_n\mathbf{u}-{\mathbf{C}}_n\dot{\mathbf{q}}-{\mathbf{g}}_n)+{\mathbf{M}}_n{\dot{\mathbf{F}}}_n\dot{\mathbf{q}},$$

where ${\mathbf{M}}_n={\mathbf{H}}_n^{-1}{\mathbf{F}}_n^{\top }{\left({\mathbf{F}}_n{\mathbf{H}}_n^{-1}{\mathbf{F}}_n^{\top }\right)}^{-1}$. Defining state as $\mathbf{x}={[{\mathbf{q}}^{\top },{\dot{\mathbf{q}}}^{\top }]}^{\top }$, matrices ${\mathbf{A}}_c$, ${\mathbf{B}}_c$ and $\mathbf{G}$ for this system can be found as:

$${\mathbf{A}}_c=\frac{\partial {\mathbf{f}}_n}{\partial \mathbf{x}},{\mathbf{B}}_c=\frac{\partial {\mathbf{f}}_n}{\partial \mathbf{u}},\mathbf{G}=\left[\begin{array}{cc}{\mathbf{F}}_n& \mathbf{0}\\ {\dot{\mathbf{F}}}_n& {\mathbf{F}}_n\end{array}\right].$$

(6)

As we can see from the example, the expression (5) can be generated directly from the nonlinear dynamics, bypassing the form (1). Other approaches to generate expression (5) are shown in [9,21].

Remark 1.

From (3) and (4), or from (6), it is easy to observe that the first equation in the system (5) will always satisfy constraints $\mathbf{G}\dot{\mathbf{x}}=0$; in fact, there are no reaction forces to enforce these constraints. That begs the conclusion that explicit constraints can be dropped out of the expression (5). However, these explicit constraints contain information about the subspace to which the state derivative $\dot{\mathbf{x}}$ is bound, which is instrumental in designing control law for the system with the use of appropriate projections, as will be shown in the next sections.

4.2. Dynamics with Explicit Constraints: Control

Let us denote $\mathbf{N}=\mathrm{null}\left(\mathbf{G}\right)$ and $\mathbf{R}=\mathrm{col}\left({\mathbf{G}}^{\top }\right)$, where $\mathrm{null}\left(·\right)$ and $\mathrm{col}\left(·\right)$ are operators, returning orthonormal bases in, respectively, null space and column space of the input matrix. Then, we can represent $\dot{\mathbf{x}}$ and $\mathbf{x}$ in terms of the bases $\mathbf{N}$ and $\mathbf{R}$:

$$\begin{array}{cc}\hfill & \dot{\mathbf{x}}=\mathbf{N}\dot{\mathbf{z}}\hfill \end{array}$$

(7)

$$\begin{array}{cc}\hfill & \mathbf{x}=\mathbf{N}\mathbf{z}+\mathbf{R}\zeta ,\hfill \end{array}$$

(8)

where $\mathbf{z}={\mathbf{N}}^{\top }\mathbf{x}\in {\mathbb{R}}^{n_z}$ and $\zeta ={\mathbf{R}}^{\top }\mathbf{x}\in {\mathbb{R}}^{n_{\zeta }}$ are coordinate representation of the projection of $\mathbf{x}$ onto the span of $\mathbf{N}$ and $\mathbf{R}$. Vector $\zeta$ represents states (expressed in basis $\mathbf{R}$) that are static: their values cannot change under given constraints; $\mathbf{z}$ represents linearly independent non-static states (expressed in basis $\mathbf{N}$).

Following [8,9], we project (5) into the span of $\mathbf{N}$, arriving at the following zero dynamics formulation:

$$\dot{\mathbf{z}}={\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{N}\mathbf{z}+{\mathbf{N}}^{\top }{\mathbf{B}}_c\mathbf{u}+{\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{R}\zeta .$$

(9)

We note that, in [8,9], the constant component ${\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{R}\zeta$ is not mentioned explicitly. It can be seen as one of the many sources of constant terms in affine dynamics, affecting the steady-state solution, but not the stability of the system. However, as we will demonstrate in the rest of the paper, a systematic approach to identifying both constant and time-varying terms in the system dynamics leads to effective state estimation and control algorithms.

In [8,9], it is proposed to design feedback control for system (9) by stabilizing the following closed-loop dynamics by finding control gains ${\mathbf{K}}_z$:

$${\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{N}-{\mathbf{N}}^{\top }{\mathbf{B}}_c{\mathbf{K}}_z\in \mathbb{H}$$

(10)

where $\mathbb{H}$ is the set of all matrices whose all eigenvalues have negative real parts; $\mathbf{M}\in \mathbb{H}$ means that the matrix $\mathbf{M}$ is Hurwitz. As an example, control gains ${\mathbf{K}}_z$ can be found by solving the LQR problem.

Resulting control law takes the form $\mathbf{u}=-{\mathbf{K}}_z\mathbf{z}=-{\mathbf{K}}_z\mathbf{N}\mathbf{x}$, leading to closed loop dynamics $\dot{\mathbf{z}}={\mathbf{N}}^{\top }({\mathbf{A}}_c-{\mathbf{B}}_c{\mathbf{K}}_z)\mathbf{N}\mathbf{z}+{\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{R}\zeta$. This makes it clear that state matrix ${\mathbf{A}}_c-{\mathbf{B}}_c{\mathbf{K}}_z$ does not have to be stable, only its projection onto the span of $\mathbf{N}$ does. It is also clear that the steady-state solution will lie at the origin only if ${\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{R}\zeta =0$.

We can propose an affine control law that stabilizes the origin: $\mathbf{u}=-{\mathbf{K}}_z\mathbf{z}+{\mathbf{u}}_{\zeta }$. In order for $\dot{\mathbf{z}}=0$ when $\mathbf{z}=0$, the following needs to hold:

$${\mathbf{N}}^{\top }{\mathbf{B}}_c{\mathbf{u}}_{\zeta }=-{\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{R}\zeta ,$$

(11)

which means that the origin can become a node as long as vector ${\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{R}\zeta$ lies in the column space of ${\mathbf{N}}^{\top }{\mathbf{B}}_c$, which can be represented as the following condition:

$$(\mathbf{I}-{\mathbf{N}}^{\top }{\mathbf{B}}_c{({\mathbf{N}}^{\top }{\mathbf{B}}_c)}^{+}){\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{R}\zeta =0,$$

(12)

where $\mathbf{I}\in {\mathbb{R}}^{n\times n}$ is an identity matrix, and ${(·)}^{+}$ is a Moore–Penrose pseudo-inverse. If condition (12) holds, the value of ${\mathbf{u}}_{\zeta }$ can be calculated as follows:

$${\mathbf{u}}_{\zeta }=-{\left({\mathbf{N}}^{\top }{\mathbf{B}}_c\right)}^{+}{\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{R}\zeta ,$$

(13)

Denoting ${\mathbf{K}}_{\zeta }={\left({\mathbf{N}}^{\top }{\mathbf{B}}_c\right)}^{+}{\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{R}$, we get control law in the form:

$$\mathbf{u}=-{\mathbf{K}}_z\mathbf{z}-{\mathbf{K}}_{\zeta }\zeta .$$

(14)

We would like to note here that variable $\zeta$ comes into the dynamics equations only as input to the operator $\mathbf{R}$. This will be used in the following sections when the pair $\zeta$, $\mathbf{R}$ will be replaced with lower-dimensional projections, without changing the structure of the equations.

As was said previously, the use of state estimation is justified by the need to use the full state in the control law, as was exemplified in this section. The next section provides the proposed formulation of a state estimator for constrained LTI systems.

5. State Estimation for LTI Systems with Explicit Constraints

In this section, we propose a method for estimating the state of an LTI system with explicit constraints. We start by observing that, in previously introduced notation $\dot{\zeta }=0$, and therefore we can write the following description of the dynamics:

$$\left\{\begin{array}{c}\left[\begin{array}{c}\dot{\mathbf{z}}\\ \dot{\zeta }\end{array}\right]=\left[\begin{array}{cc}{\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{N}& {\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{R}\\ \mathbf{0}& \mathbf{0}\end{array}\right]\left[\begin{array}{c}\mathbf{z}\\ \zeta \end{array}\right]+\left[\begin{array}{c}{\mathbf{N}}^{\top }{\mathbf{B}}_c\mathbf{u}\\ \mathbf{0}\end{array}\right]\hfill \\ \mathbf{y}=\mathbf{C}\left[\begin{array}{cc}\mathbf{N}& \mathbf{R}\end{array}\right]\left[\begin{array}{c}\mathbf{z}\\ \zeta \end{array}\right]\hfill \end{array}\right.$$

(15)

We will refer to this representation of the dynamics as subspace representation. Using this representation, we can propose the following state observer:

$$\left[\begin{array}{c}\dot{\widehat{\mathbf{z}}}\\ \dot{\widehat{\zeta }}\end{array}\right]=\left[\begin{array}{cc}{\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{N}& {\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{R}\\ \mathbf{0}& \mathbf{0}\end{array}\right]\left[\begin{array}{c}\widehat{\mathbf{z}}\\ \widehat{\zeta }\end{array}\right]+\left[\begin{array}{c}{\mathbf{N}}^{\top }{\mathbf{B}}_c\mathbf{u}\\ \mathbf{0}\end{array}\right]+\mathbf{L}\left(\mathbf{y}-\mathbf{C}\left[\begin{array}{cc}\mathbf{N}& \mathbf{R}\end{array}\right]\left[\begin{array}{c}\widehat{\mathbf{z}}\\ \widehat{\zeta }\end{array}\right]\right),$$

(16)

where $\widehat{\mathbf{z}}$ and $\widehat{\zeta }$ are estimates of $\mathbf{z}$ and $\zeta$.

As long as the system in the proposed subspace representation is observable, we can ensure the stability of the resulting system by choosing correct observer gains $\mathbf{L}$. We stress the fact that chosen representation does play a role, as we will demonstrate in the following sections. The correct choice of observer gains $\mathbf{L}$ is given by the following theorem:

Theorem 1.

Observer (16) and dynamics (9), with control law $\mathbf{u}=-{\mathbf{K}}_z\widehat{\mathbf{z}}-{\mathbf{K}}_{\zeta }\widehat{\zeta }$ form a stable system as long as (12) holds, control gains ${\mathbf{K}}_z$ are chosen to satisfy condition (10) and observer gains are chosen to satisfy the next condition:

$$\left[\begin{array}{c}{\mathbf{N}}^{\top }\\ {\mathbf{R}}^{\top }\end{array}\right]\left(\left[\begin{array}{c}{\mathbf{A}}_c^{\top }\mathbf{N}\\ \mathbf{0}\end{array}\right]-{\mathbf{C}}^{\top }{\mathbf{L}}^{\top }\right)\in \mathbb{H}$$

(17)

Note that condition (17) is written dual form, making the observer gains appear on the right side of the observation matrix, allowing for finding stabilizing gains via pole placement or LQR, as will be shown in Section 8.2.

Proof of Theorem 1.

Defining observation error $\mathbf{e}=\left[\begin{array}{c}\mathbf{z}-\widehat{\mathbf{z}}\\ \zeta -\widehat{\zeta }\end{array}\right]$ and subtracting (16) from (15), we get observer error dynamics:

$$\dot{\mathbf{e}}=\left(\left[\begin{array}{cc}{\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{N}& {\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{R}\\ \mathbf{0}& \mathbf{0}\end{array}\right]-\mathbf{L}\mathbf{C}\left[\begin{array}{cc}\mathbf{N}& \mathbf{R}\end{array}\right]\right)\mathbf{e}$$

(18)

Substituting control law into (9), we find state dynamics as:

$$\dot{\mathbf{z}}=({\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{N}-{\mathbf{N}}^{\top }{\mathbf{B}}_c{\mathbf{K}}_z)\mathbf{z}+{\mathbf{N}}^{\top }{\mathbf{B}}_c\mathbf{K}\mathbf{e}+({\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{R}-{\mathbf{N}}^{\top }{\mathbf{B}}_c{\mathbf{K}}_{\zeta })\zeta .$$

(19)

where $\mathbf{K}=\left[\begin{array}{cc}{\mathbf{K}}_z& {\mathbf{K}}_{\zeta }\end{array}\right]$. With that, we can write the combined state and observer dynamics:

$$\left[\begin{array}{c}\dot{\mathbf{z}}\\ \dot{\mathbf{e}}\end{array}\right]=\left[\begin{array}{cc}({\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{N}-{\mathbf{N}}^{\top }{\mathbf{B}}_c{\mathbf{K}}_z)& {\mathbf{N}}^{\top }{\mathbf{B}}_c\mathbf{K}\\ \mathbf{0}& ({\overline{\mathbf{N}}}^{\top }{\mathbf{A}}_c-\mathbf{L}\mathbf{C})\mathbf{E}\end{array}\right]\left[\begin{array}{c}\mathbf{z}\\ \mathbf{e}\end{array}\right]+\left[\begin{array}{c}({\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{R}-{\mathbf{N}}^{\top }{\mathbf{B}}_c{\mathbf{K}}_{\zeta })\zeta \\ \mathbf{0}\end{array}\right]$$

(20)

where $\overline{\mathbf{N}}=\left[\begin{array}{cc}\mathbf{N}& {\mathbf{0}}_{n\times n}\end{array}\right]$, and $\mathbf{E}=\left[\begin{array}{cc}\mathbf{N}& \mathbf{R}\end{array}\right]$. Since the state matrix of the obtained closed-loop system is upper triangular, it is sufficient for matrices $({\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{N}-{\mathbf{N}}^{\top }{\mathbf{B}}_c{\mathbf{K}}_z)$ and $({\overline{\mathbf{N}}}^{\top }{\mathbf{A}}_c-\mathbf{L}\mathbf{C})\mathbf{E}$ to be stable for the system to be stable. Condition (10) implies the stability of the first one, and condition (17) implies the stability of the second, as eigenvalues of matrix are equal to those of its transpose. □

Theorem 1 gives Separation Principle for LTI systems with explicit constraints in subspace representation. The proof follows the same principle as the proof in the analogous theorem for LTI systems.

A proposed state observer can be seen as a combination of a state observer in variable $\mathbf{z}$ and disturbance observer in variable $\zeta$; indeed, a disturbance observer is an instrument for determining unknown constant components of the dynamics equations for linear systems, and $\zeta$ acts like one. A diagram of the proposed method as a Dynamic Output Feedback controller is shown in Figure 3.

While observer (16) works for a large class of linear systems, we can show that there are LTI systems for which it would not work, as will be demonstrated in Section 9. However, a different parameterization of state variables would allow us to construct a lower-dimensional stabilizable observer. This is the subject of the next section.

6. State Constraints in Observer Design

System (5) has its constraints expressed in terms of state derivative $\dot{\mathbf{x}}$. However, in a number of cases, these constraints on $\dot{\mathbf{x}}$ imply a constraint on $\mathbf{x}$. For example, for mechanical systems, generalized velocities appear both in $\dot{\mathbf{x}}$ and $\mathbf{x}$, and some of the constraints are placed only on generalized velocities; thus, an equivalent constraint on $\mathbf{x}$ can be formed. Let us denote this state constraint as:

$${\mathbf{G}}_x\mathbf{x}=0,$$

(21)

where ${\mathbf{G}}_x$ is a state constraint matrix. The intersection of the row space of ${\mathbf{G}}_x$ and the span of $\mathbf{R}$ represent the space of static state values that are necessarily equal to zero and therefore do not need to be estimated. Therefore, all static state values that require estimation lie in the intersection of the null space of ${\mathbf{G}}_x$ and the span of $\mathbf{R}$. We will call orthonormal basis in this intersection ${\mathbf{R}}_{SC}$. Remembering that $\mathbf{N}$ is the orthogonal complement of $\mathbf{R}$ and that intersection of subspaces can be found as the orthogonal complement of the union of their respective orthogonal complements, we produce the following formula for ${\mathbf{R}}_{SC}$:

$${\mathbf{R}}_{SC}={\left[\begin{array}{cc}\mathrm{col}\left({\mathbf{G}}_x^{\top }\right)& \mathbf{N}\end{array}\right]}^{\perp },$$

(22)

where operator ${(·)}^{\perp }$ returns an orthonormal basis in the left null space of the matrix, or, equivalently, an orthonormal basis in the orthogonal compliment of the range (column space) of the operator.

Example 2.

For mechanical systems with state $\mathbf{x}$ and constraint matrix $\mathbf{G}=\left[\begin{array}{cc}{\mathbf{F}}_n& \mathbf{0}\\ {\dot{\mathbf{F}}}_n& {\mathbf{F}}_n\end{array}\right]$ defined as in the example 1, the state constraint matrix ${\mathbf{G}}_x$ takes the form: ${\mathbf{G}}_x=\left[\begin{array}{cc}\mathbf{0}& {\mathbf{F}}_n\end{array}\right]$.

Defining $\zeta ={\mathbf{R}}_{SC}^{\top }\mathbf{x}$ and replacing row-space basis $\mathbf{R}$ with new basis ${\mathbf{R}}_{SC}$ in the proposed observer design, we obtain SC observer (where SC stands for State Constraint):

$$\left[\begin{array}{c}\dot{\widehat{\mathbf{z}}}\\ \dot{\widehat{\zeta }}\end{array}\right]=\left[\begin{array}{cc}{\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{N}& {\mathbf{N}}^{\top }{\mathbf{A}}_c{\mathbf{R}}_{SC}\\ \mathbf{0}& \mathbf{0}\end{array}\right]\left[\begin{array}{c}\widehat{\mathbf{z}}\\ \widehat{\zeta }\end{array}\right]+\left[\begin{array}{c}{\mathbf{N}}^{\top }{\mathbf{B}}_c\mathbf{u}\\ \mathbf{0}\end{array}\right]+\mathbf{L}\left(\mathbf{y}-\mathbf{C}\left[\begin{array}{cc}\mathbf{N}& {\mathbf{R}}_{SC}\end{array}\right]\left[\begin{array}{c}\widehat{\mathbf{z}}\\ \widehat{\zeta }\end{array}\right]\right)$$

(23)

To stabilize the SC observer, we find gains $\mathbf{L}$, such that the following condition holds:

$$\left[\begin{array}{c}{\mathbf{N}}^{\top }\\ {\mathbf{R}}_{SC}^{\top }\end{array}\right]\left(\left[\begin{array}{c}{\mathbf{A}}_c^{\top }\mathbf{N}\\ \mathbf{0}\end{array}\right]-{\mathbf{C}}^{\top }{\mathbf{L}}^{\top }\right)\in \mathbb{H}$$

(24)

Both $\mathbf{R}$ and ${\mathbf{R}}_{SC}$ are orthonormal; $\mathrm{rank}\left({\mathbf{R}}_{SC}\right)\le \mathrm{rank}\left(\mathbf{R}\right)$, meaning the former has the same number or fewer columns than the latter. The difference between projections of $\mathbf{x}$ on those two linear spaces is given by the following Theorem.

Theorem 2.

For system (5), the only admissible value of the state $\mathbf{x}$ that lies in the range of $\mathbf{R}$ and is orthogonal to columns of ${\mathbf{R}}_{SC}$ is $\mathbf{x}=0$: ${\mathbf{R}}_{SC}^{\top }\mathbf{x}=0\iff {\mathbf{R}}^{\top }\mathbf{x}=0$.

Proof Theorem 2.

Since the span of ${\mathbf{R}}_{SC}$ is the intersection of the span of $\mathbf{R}$ and null space of ${\mathbf{G}}_x$, a vector ${\mathbf{R}}^{\top }\mathbf{x}$, orthogonal to ${\mathbf{R}}_{SC}$ has to lie in the row space of ${\mathbf{G}}_x$, and according to (21) is equal to 0. Conversely, $\mathrm{col}\left({\mathbf{R}}_{SC}\right)\subset \mathrm{col}\left(\mathbf{R}\right)$ implies that ${\mathbf{R}}^{\top }\mathbf{x}=0\Rightarrow {\mathbf{R}}_{SC}^{\top }\mathbf{x}=0$. □

Thus, the main difference between the observer (16) and (23) is that the latter has fewer state variables, making it easier to find gains $\mathbf{L}$ that satisfy the condition (24). In fact, it is possible to show systems where observer (16) cannot be stabilized because part of the state variables are not observed and cannot be inferred from the dynamics of the system, but have to be equal to zero due to the constraints. In particular, this is often true with unmeasured velocities for mechanical systems. Section 9 illustrates this.

7. The Effective States in Observer Design

From (9), we observe that the component of static state $\mathbf{R}\zeta$ that lies in the null space of matrix ${\mathbf{N}}^{\top }{\mathbf{A}}_c$ has no influence on the dynamics of the system. Hence, these states can be ignored from the control and observer design. Projecting row space of ${\mathbf{N}}^{\top }{\mathbf{A}}_c$ onto the column space of ${\mathbf{R}}_{SC}$, we find space of all values of ${\mathbf{R}}_{SC}\zeta$ (and equivalently $\mathbf{R}\zeta$, according to Theorem 2) that influence control design ${\mathbf{S}}_{ES}$, which we will call effective row space of the constrained system:

$${\mathbf{R}}_{ES}=\mathrm{col}\left({\mathbf{R}}_{SC}{\mathbf{R}}_{SC}^{\top }{\mathbf{A}}^{\top }\mathbf{N}\right),$$

(25)

where ${\mathbf{R}}_{ES}$ is a basis in the effective row space of the constrained system.

Defining $\zeta ={\mathbf{R}}_{ES}^{\top }\mathbf{x}$ and replacing row-space basis $\mathbf{R}$ with new basis ${\mathbf{R}}_{ES}$ in the proposed observer design, we obtain ES observer (where ES stands for Effective States):

$$\left[\begin{array}{c}\dot{\widehat{\mathbf{z}}}\\ \dot{\widehat{\zeta }}\end{array}\right]=\left[\begin{array}{cc}{\mathbf{N}}^{\top }{\mathbf{A}}_c\mathbf{N}& {\mathbf{N}}^{\top }{\mathbf{A}}_c{\mathbf{R}}_{ES}\\ \mathbf{0}& \mathbf{0}\end{array}\right]\left[\begin{array}{c}\widehat{\mathbf{z}}\\ \widehat{\zeta }\end{array}\right]+\left[\begin{array}{c}{\mathbf{N}}^{\top }{\mathbf{B}}_c\mathbf{u}\\ \mathbf{0}\end{array}\right]+\mathbf{L}\left(\mathbf{y}-\mathbf{C}\left[\begin{array}{cc}\mathbf{N}& {\mathbf{R}}_{ES}\end{array}\right]\left[\begin{array}{c}\widehat{\mathbf{z}}\\ \widehat{\zeta }\end{array}\right]\right)$$

(26)

To stabilize the ES observer, we find gains $\mathbf{L}$, such that the following condition holds:

$$\left[\begin{array}{c}{\mathbf{N}}^{\top }\\ {\mathbf{R}}_{ES}^{\top }\end{array}\right]\left(\left[\begin{array}{c}{\mathbf{A}}_c^{\top }\mathbf{N}\\ \mathbf{0}\end{array}\right]-{\mathbf{C}}^{\top }{\mathbf{L}}^{\top }\right)\in \mathbb{H}$$

(27)

In Section 9, we will show that the proposed ES observer (26) requires fewer independent measurements (counted as the rank of the observation matrix $\mathbf{C}$), compared with any other observer structure discussed in this paper.

8. Observer Gains Tuning

8.1. Compact Notation

Thus far, we have used an explicit notation, resulting in relatively long but clear expressions. It is in fact possible to make the notation more compact, by reintroducing matrices $\overline{\mathbf{N}}=\left[\begin{array}{cc}\mathbf{N}& {\mathbf{0}}_{n\times n^{*}}\end{array}\right]$, and $\mathbf{E}=\left[\begin{array}{cc}\mathbf{N}& {\mathbf{R}}^{*}\end{array}\right]$, where $n^{*}$ equals the number of rows in $\mathbf{E}$, and proposing to use a single variable $\widehat{\chi }=\left[\begin{array}{c}\widehat{\mathbf{z}}\\ \widehat{\zeta }\end{array}\right]$ to denote the state of the observer. Then, the expression for the observer becomes:

$$\dot{\widehat{\chi }}=({\overline{\mathbf{N}}}^{\top }{\mathbf{A}}_c-\mathbf{L}\mathbf{C})\mathbf{E}\widehat{\chi }+{\overline{\mathbf{N}}}^{\top }{\mathbf{B}}_c\mathbf{u}+\mathbf{L}\mathbf{y}$$

(28)

In compact notation, if ${\mathbf{R}}^{*}=\mathbf{R}$ or ${\mathbf{R}}^{*}={\mathbf{R}}_{SC}$, it follows that $\mathbf{x}=\mathbf{E}\chi$ and, conversely, $\chi ={\mathbf{E}}^{\top }\mathbf{x}$, where $\chi$ stands for the true value of $\widehat{\chi }$. Control law then can be written as follows:

$$\mathbf{u}=-\mathbf{K}\widehat{\chi }$$

(29)

where $\mathbf{K}=\left[\begin{array}{cc}{\mathbf{K}}_z& {\mathbf{K}}_{\zeta }\end{array}\right]$. Conditions (17), (24) and (27) can be written in compact notation as:

$${\mathbf{E}}^{\top }{\mathbf{A}}_c^{\top }\overline{\mathbf{N}}-{\mathbf{E}}^{\top }{\mathbf{C}}^{\top }{\mathbf{L}}^{\top }\in \mathbb{H}$$

(30)

This notation makes it obvious how the observer gains can be designed using standard techniques developed for the control of LTI systems, which will be shown in the next subsection.

8.2. Observer Gain Design

Using notation, introduced in the last subsection, we note that expression (30) is the closed-loop state matrix of the dual form of the observer error dynamics, and can be directly used to compute control gains using, for example, pole placement or LQR procedure. As a pseudo-code, it can be expressed as follows:

$${\mathbf{L}}^{\top }=\mathrm{place}(\left({\mathbf{E}}^{\top }{\mathbf{A}}_c^{\top }\overline{\mathbf{N}}\right),\left({\mathbf{E}}^{\top }{\mathbf{C}}^{\top }\right),\mathbf{p})$$

(31)

where $\mathbf{p}\in {\mathbb{R}}^{n^{*}}$ is the vector of poles, desired for the closed loop system and $\mathrm{place}(·,·,·)$ is a pole placement algorithm, taking state matrix as the first input, control matrix as the second and desired poles as the third. Similarly, we can solve LQR problem:

$${\mathbf{L}}^{\top }=\mathrm{lqr}(\left({\mathbf{E}}^{\top }{\mathbf{A}}_c^{\top }\overline{\mathbf{N}}\right),\left({\mathbf{E}}^{\top }{\mathbf{C}}^{\top }\right),\mathbf{Q},\mathbf{S})$$

(32)

where positive semidefinite $\mathbf{Q}\in {\mathbb{R}}^{n^{*}\times n^{*}}$ and positive definite $\mathbf{S}\in {\mathbb{R}}^{h\times h}$ are state error cost and observation correction cost matrices and $\mathrm{lqr}(·,·,·,·)$ is an lqr solver, taking state matrix as the first input, control matrix as the second, and cost matrices as the last two inputs.

9. Case Study: Table-Top Flywheel Inverted Pendulum

Consider a planar flywheel inverted pendulum: an underactuated system consisting of a shaft connected to the ground via an un-actuated pin joint, and to a symmetrical load via a motor [44,45], as shown in Figure 4. The system can be described with four state variables: orientations of the shaft ${\phi }_1$ and the load ${\phi }_2$, and the rates of change of those orientations ${\dot{\phi }}_1$, ${\dot{\phi }}_2$. However, following floating-base dynamics formalism, actively used in bipedal robotics, we can also describe the system using eight state variables, adding the position of the tip of the shaft $r_x$, $r_y$ (2 variables for the planar case) and their rates of change ${\dot{r}}_x$, ${\dot{r}}_y$. This representation is convenient when the tip of the shaft can move along the supporting surface or leave it altogether, for example, due to reaction forces being limited to the friction cone. We will refer to this system as a table-top flywheel inverted pendulum.

A nonlinear model of the table-top flywheel inverted pendulum has the following general form:

$$\left\{\begin{array}{c}\mathbf{H}\ddot{\mathbf{q}}+\mathbf{c}=\mathbf{T}u+{\mathbf{F}}^{\top }\lambda \hfill \\ \mathbf{F}\ddot{\mathbf{q}}+\dot{\mathbf{F}}\dot{\mathbf{q}}=0\hfill \end{array}\right.$$

(33)

where $\mathbf{q}={[r_x,r_y,{\phi }_1,{\phi }_2]}^{\top }$, u is the control input (current applied to the motor), $\lambda$ is the vector of reaction forces, is a vector of generalized coordinates, $\mathbf{H}$ is a generalized inertia matrix, $\mathbf{c}$ is a sum of generalized inertial, gravitational and dissipative forces, $\mathbf{T}$ is the control input Jacobian, and $\mathbf{F}$ is the reaction force Jacobian. The values those quantities have are presented below, given here up to two significant figures, values taken from least-squares-based parameter estimation; constant coefficients in the expressions below have appropriate units, not shown for conciseness (e.g., the coefficient 0.67 in (34) has units of kg, and 0.0017—units of $\mathrm{kg}·{\mathrm{m}}^2$):

$$\mathbf{H}=\left[\begin{array}{cccc}0.67& 0& -0.21cos\left({\phi }_1\right)& 0\\ 0& 0.67& -0.21sin\left({\phi }_1\right)& 0\\ -0.21cos\left({\phi }_1\right)& -0.21sin\left({\phi }_1\right)& 0.071& -0.0017\\ 0& 0& -0.0017& 0.0017\end{array}\right],$$

(34)

$$\mathbf{c}=\left[\begin{array}{c}0.21{\dot{{\phi }_1}}^{2}sin\left({\phi }_1\right)\\ -0.21cos\left({\phi }_1\right){\dot{{\phi }_1}}^2+6.6\\ -2.1sin\left({\phi }_1\right)\\ 1.5·{10}^{-4}\dot{{\phi }_2}\end{array}\right],{\mathbf{F}}^{\top }=\left[\begin{array}{cc}1& 0\\ 0& 1\\ 0& 0\\ 0& 0\end{array}\right],\mathbf{T}=\left[\begin{array}{c}0\\ 0\\ 0\\ 0.061\end{array}\right]$$

(35)

One of the big advantages of this system for our purposes is the simplicity its dynamics attains when the equations (33) are solved with respect to the highest order derivatives:

$$\left\{\begin{array}{c}{\ddot{r}}_x=0\hfill \\ {\ddot{r}}_y=0\hfill \\ {\ddot{\phi }}_1=0.88u-0.0022\dot{{\phi }_2}+29.6sin\left({\phi }_1\right)\hfill \\ {\ddot{\phi }}_2=36.8u-0.09\dot{{\phi }_2}+29.6sin\left({\phi }_1\right)\hfill \end{array}\right.$$

(36)

We can linearize the model around the unstable equilibrium, obtaining the following values for the state, control, and constraint matrices:

$${\mathbf{A}}_c=\left[\begin{array}{cccccccc}0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 29.6& 0& 0& 0& 0& -2.2\times {10}^{-3}\\ 0& 0& 29.6& 0& 0& 0& 0& -0.09\end{array}\right],{\mathbf{B}}_c=\left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0.88\\ 36.8\end{array}\right]$$

(37)

Let us consider five cases, related to the availability of sensors: (1) when all states are measured (note that an observer is not actually needed in this case), (2) when all states except ${\dot{\phi }}_1$ and ${\dot{\phi }}_2$ are measured, (3) when all states except ${\dot{r}}_x$ and ${\dot{r}}_y$ are measured, (4) when only ${\phi }_1$, ${\phi }_2$, ${\dot{\phi }}_1$ and ${\dot{\phi }}_2$ are measured, and (5) when only ${\phi }_1$ and ${\phi }_2$ are measured. In all of these cases, matrix ${\mathbf{E}}^{\top }{\mathbf{A}}^{\top }\overline{\mathbf{N}}$ has the form:

$${\mathbf{E}}^{\top }{\mathbf{A}}^{\top }\overline{\mathbf{N}}=\left[\begin{array}{cc}\begin{array}{cccc}0& 0& 29.6& 29.6\\ 0& 0& 0& 0\\ 1& 0& 0& 0\\ 0& 1& -2.2\times {10}^{-3}& -0.09\end{array}& {\mathbf{0}}_{z\times (n^{*}-z)}\\ {\mathbf{0}}_{(n^{*}-z)\times z}& {\mathbf{0}}_{(n^{*}-z)\times (n^{*}-z)}\end{array}\right]$$

(38)

where $z=4$ is the number of columns of $\mathbf{N}$ (number of independent non-static states). The value of $n^{*}$ depends on the choice of the observer. For observer (16) $n^{*}=8$, for SC observer (23) $n^{*}=6$ and for ES observer (26) $n^{*}=4$. Matrix $\left({\mathbf{E}}^{\top }{\mathbf{C}}^{\top }\right)$ is orthonormal; moreover, its elements are either 0 or 1. For conciseness, we only mention the rank of this matrix for each considered case. This information is found in

Table 1. Analysis results for different observer types.

States Measured	Rank C	Observer Stabilizable			Descriptor Observable
		Full State (16)	SC (23)	ES (26)
all	8	yes	yes	yes	yes
${\phi }_1$, ${\phi }_2$, $r_x$, $r_y$, ${\dot{r}}_x$, ${\dot{r}}_y$	6	yes	yes	yes	yes
${\phi }_1$, ${\phi }_2$, $r_x$, $r_y$, ${\dot{\phi }}_1$, ${\dot{\phi }}_2$	6	no	yes	yes	no
${\phi }_1$, ${\phi }_2$, ${\dot{\phi }}_1$, ${\dot{\phi }}_2$	4	no	no	yes	no
${\phi }_1,{\phi }_2$	2	no	no	yes	no

.

Table 1 shows which of the observers (16), (23) and (26) is stabilizable for which measurement availability scenario. We also show which case allows the corresponding descriptor system to be stabilizable (using the definition from [1]).

As we can see from the Table 1, both the observer (16) and (23) work when all position variables and velocities ${\dot{r}}_x$${\dot{r}}_y$ are measured; it also satisfies the observably criterion for descriptor systems [1]. However, if instead of ${\dot{r}}_x$${\dot{r}}_y$ we measure ${\dot{\phi }}_1$, ${\dot{\phi }}_2$, the observer (16) no longer can be stabilized (and the observably criterion for descriptor systems would not be satisfied in this case either), while SC observer (23) and ES observer (26) both can be stabilized, being lower-dimensional systems. Moreover, the SC observer cannot be stabilized when $r_x$ and $r_y$ are not measured directly. The ES observer, on the other hand, excludes those from the list of estimated variables, as they do not affect the dynamics of the system (as discussed in Section 7); thus, it can be stabilized without measuring these states.

We can show that the method works not only on the LTI systems (as is proven by stability analysis), but can also be employed on nonlinear systems, linearized around a given trajectory. Figure 5 and Figure 6 show behavior of the ES Observer used on the table-top flywheel inverted pendulum discussed in this section. The mechanism follows a cyclic trajectory around the upper unstable equilibrium point. The motion of the system (shown as a blue graph on all figures) is placed far from the nominal trajectory, used for linearization (shown as a dotted orange graph), affecting the accuracy of the model used by the observer. The experimental setup was designed to allow two types of measurements: (1) change in orientation of both links are measured with a 12-bit magnetic encoder (with the resolution of 0.088 degrees), (2) change in orientation of the load is measured with a 12-bit magnetic encoder, while the orientation of the shaft is measured with the use of two IMU modules MPU6050, whose readings from gyroscopes and accelerometers are fused to compute an estimate of the link’s orientation.

We can see that the observer drives down the initial estimation error. Since the motion of the system was taking place far from the linearization trajectory, the model available to the observer was not accurate; together with limited sensor accuracy, this illustrates a typical use-case for observers in mobile robotics, where the availability of high-quality sensors and exact models are limited, while exact tracking of a trajectory is not always possible or necessary. With this experiment, we wanted to demonstrate that these circumstances do not prevent the use of the proposed Observer, subject to proper tuning. For experiments shown in Figure 5 and Figure 6, the tuning method (32) was used, with matrices $\mathbf{Q}=\mathrm{diag}(100,100,10,10)$ and $\mathbf{S}=\mathrm{diag}(0.05,1)$. This, however, is only a qualitative result, showing the possibility of using such observers. The study of the analytic conditions guaranteeing stability of the proposed observers for nonlinear systems in the vicinity of the linearization point would be of interest.

Figure 7 and Figure 8 show the behavior of the ES observer when used for the same system, but this time around its lower equilibrium point. Studying Figure 7 and Figure 8, we note that initial estimation error quickly tends to zero both for the position and the velocity variables. However, the velocity variable, which is not measured directly and relies on the imprecise dynamics model to be estimated, is not tracked perfectly. These experiments were performed using the same observer gains as previously.

10. Case Study: Quadruped Robot

In this section, we explore the influence contact scenarios have on the performance and structure of the proposed observer, using as an example an actual quadruped robot Unitree A1. As was highlighted previously, proposed SC and ES observers have equal or lower-dimensional state space compared with the original robot described in floating base coordinates (joint angles and position and orientation of the trunk), but an equal or higher number of coordinates than the same robot described in minimal coordinate representation. In this section, we demonstrate a number of contact scenarios where values of the static part of the state space ${\mathbf{R}}^{*}\zeta$ are needed to solve inverse dynamics (or, equivalently, when ${\mathbf{R}}_{ES}$ has a non-zero number of columns).

Figure 9 shows a scheme of the Unitree A1 quadruped with four points of contact with the ground ${\mathbf{r}}_{Ki}$, with contact forces lying in the interior of the respective friction cones: we do not consider scenarios, where reaction force lies on the boundary of the friction cone, and the constraint is violated by slipping.

A robot’s leg consists of a hip link connected to the trunk via two motors and a shin link connected to the hip link via a single motor, installed at the hinge ${\mathbf{r}}_{Oi}$; we will refer to the later points as “knees”. Contact points ${\mathbf{r}}_{Ki}$ lie on the contact pads installed on the shin links. In the following, we will consider two types of constraints: (1) constraints on the position of contact points ${\mathbf{r}}_{Ki}=\mathrm{const}$, arising from the foot being in contact with the environment, and (2) constraints on the positions of the knee hinges ${\mathbf{r}}_{Oi}=\mathrm{const}$, arising from accidental contacts with the environment, for example during blind stair ascent or descent, or from the robot attempting to "stay on its knees" for vertical stability.

Table 2. Analysis results for different observer types.

#	Constrained (Constant) Values	Rank
		$\mathbf{R}$	${\mathbf{R}}_{\mathit{SC}}$	${\mathbf{R}}_{\mathit{ES}}$
1	${\mathbf{r}}_{K1}$, ${\mathbf{r}}_{K2}$, ${\mathbf{r}}_{K3}$, ${\mathbf{r}}_{K4}$	24	12	12
2	${\mathbf{r}}_{K1}$, ${\mathbf{r}}_{K2}$, ${\mathbf{r}}_{K3}$	18	9	9
3	${\mathbf{r}}_{K3}$, ${\mathbf{r}}_{K4}$, ${\mathbf{r}}_{O3}$, ${\mathbf{r}}_{O4}$	20	10	8–9
4	${\mathbf{r}}_{K1}$, ${\mathbf{r}}_{K2}$, ${\mathbf{r}}_{K3}$, ${\mathbf{r}}_{O4}$	24	12	10–11

shows ranks (which is equivalent to the number of columns, as these matrices are orthonormal) of matrices $\mathbf{R}$, ${\mathbf{R}}_{SC}$ and ${\mathbf{R}}_{ES}$, which define the number of static state components that are required to be observed for the full state observer (16), SC observer (23) and ES observer (26) respectively, in different contact scenarios.

We can note that the first contact scenario in Table 2 is the stance on all legs, and the second is the stance on only three legs. In both cases, the number of static states that require estimation is half of the total number of static states, which is the effect of state constraints and the use of an SC observer. The use of ES Observer does not produce a benefit here. The third scenario in Table 2 represents the robot standing on its rear shins, with its front legs free for manipulation tasks. Here, the use of ES observer allows us to drop one or two dimensions from the observer state, depending on the robot configuration. The last case represents the robot standing on three feet, while the last one (the rear left one) contacts the environment via its knee, which can happen during staircase descent. Here, again, the ES observer obtained one or two dimensions fewer than the SC observer, and 13–14 dimensions fewer than a full state observer, showing a clear benefit of using this Observer design.

11. Case Study: Motion of a Quadruped in the Sagittal Plane

In this section, we demonstrate the performance of the proposed observer on a quadruped robot moving in the sagittal plane. Following [46], we represent it as a five-link structure. This allows us to describe the robot’s position with seven generalized coordinates: the position and orientation of the robot’s trunk and its four joint angles. The flat model pairs up the legs—the configuration of both front legs is described by the same two coordinates; the same is true about both rear legs. We denote as ${\varphi }_1$ and ${\varphi }_2$ angles associated with the joints connecting the front and rear legs, respectively, and ${\varphi }_3$ and ${\varphi }_4$ are angles in the knee joints in the front and rear legs. Parameters of the robot used in the simulation are shown in

Table 3. Parameters of the flat quadruped.

Link	Parameters
	Mass, kg	Length, m
Trunk	10	0.5
Front Thigh	2	0.3
Rear Thigh	2	0.3
Front Shin	2	0.3
Rear Shin	2	0.3

.

The simulation is set up as follows. The model of the robot is linearized along a nominal trajectory, and stabilizing affine control law is found for the resulting linear model at each time step by solving the Riccati equation. In the same way, observer gains are found at each time step. That represents a gain scheduling approach for control and observer design [47]. We log both the evolution of the state of the robot and the state of the observer; for conciseness, we only present the evolution of the observer error with respect to joint angles ${\varphi }_i$ and their estimates ${\widehat{\varphi }}_i$.

Figure 10 and Figure 11 demonstrate observer error dynamics when we measure joint angles ${\varphi }_i$, as well as orientation of the trunk, as well as the rate of change of that orientation. In Figure 10 and Figure 11, $e_i={\varphi }_i-{\widehat{\varphi }}_i$.

Let us note that observer error graphs on both Figure 10 and Figure 11 demonstrate convergence to zero; however, as a numerical result, this only suggests practical applicability of the methods. Formal stability guarantees for the closed-loop nonlinear system using this observer would have to be given in a similar manner as for other gain scheduling approaches [47].

12. Comparative Analysis of the Effective States Observer and Extended Kalman Filter

It is of practical interest to better understand the relations of the proposed observer and the existing state estimation methods. A broad outline of these relations was given in Section 2.2 and illustrated in Figure 1. Here, we illustrate the relation between the proposed ES observer and Extended Kalman Filter (EKF) [48,49,50], as one of the most widely used state estimation methods.

We implement both ES Observer and EKF on the flat quadruped model, presented in the previous section, with the same dynamics and measurement model, but with added noise. We model process noise and sensor noise as random signals with covariance matrices ${\mathbf{Q}}_k=9\times {10}^{-8}\mathbf{I}$ and ${\mathbf{R}}_k={10}^{-4}\mathbf{I}$, respectively. The ES Observer is tuned using LQR procedure (32), with cost matrices $\mathbf{Q}={10}^3\mathbf{I}$ and $\mathbf{S}=\mathbf{I}$. Note that the ES filter does not account for noise, and it also uses a linear model rather than the nonlinear dynamics available to EKF. At the same time, EKF is not designed to take advantage of the structure of the constraints.

To access the quality of the observers, we introduce a quality criterion as a cost function $J_c$:

$$J_c=\sum _i\left(\left|\left|{\mathbf{x}}_i-{\widehat{\mathbf{x}}}_i\right|\right|\Delta t_i\right)$$

(39)

where ${\mathbf{x}}_i$ is the true value of the state on the i-th time step, ${\widehat{\mathbf{x}}}_i$ is the estimated value of the state, and $\Delta t_i$ is the length of the i-th time step.

Using a quality criterion (39), we can simulate the behavior of ES Observer and EKF from the same initial conditions, and compare the results. Since both are model-based methods and rely on the exact model information, we perform two experiments: (1) where the initial state of the system lies near the nominal trajectory and (2) where the initial state of the system lies far from it. The initial state of the system is denoted as ${\mathbf{x}}_0$ and the initial point on the nominal trajectory is given as ${\mathbf{x}}_0^{*}$.

Table 4. Comparison results for ES Observer and Extended Kalman Filter on a flat quadruped robot.

Observer	Cost ${\mathit{J}}_{\mathit{c}}$
	$\left|\left|{\mathbf{x}}_0-{\mathbf{x}}_0^{*}\right|\right|$ = 0.0027	$\left|\left|{\mathit{x}}_0-{\mathit{x}}_0^{*}\right|\right|$ = 0.027
Extended Kalman Filter	0.0440	1.0171
Effective States Obsever	1.0171	1.3714

shows the simulation result.

As we can see, near the nominal trajectory EKF demonstrates a much smaller cost than the ES Observer, which should be expected since it had access to the exact nonlinear model of the system and ES Observer did not have access to the information about noise. However, further from the nominal trajectory, EKF’s performance decreases much faster than that of the ES Observer.

We should note that the ES Observer is designed for systems without either process or sensor noise; at the same time, EKF is designed for systems without explicit constraints. That limits the conclusions that can be inferred from the results presented here. One of the main conclusions that have to be drawn is that gain design for ES Observer should be studied further, focusing on the inclusion of different optimality criteria in the gain design. This is a possible path because ES Observer imposes the observer structure, but not the design method for the observer gains.

13. Application of the Proposed Method

As has been stated previously, the main field where the proposed observer design method can be applied is state estimation of robots with explicit constraints. These include walking robots of various kind, and especially new types of robots for which the platform-specific observers have not yet been developed. The motivation here is the simplicity of the observer structure and the ease of gain design.

The second potential application is benchmarking other observers, including platform-specific ones. This is again linked with the ease of implementation of the proposed observer.

Finally, but possibly the most interesting area of application, is sensor distribution design. Since the proposed observer automatically finds minimal dimension of the space of the static state variables in each contact situation, it can be used to assess which sensors (and which sensor placement) would be sufficient to extract all necessary information about the state of the robot. This might be of practical value for rapid design and prototyping of new types of legged robots and other contact-driven mobile platforms. If the robot is designed to perform motions that are known in advance, and the contact interaction scenarios are also known, the ability to stabilize the ES observer can be used as a criterion in sensor placement decisions.

14. Conclusions

In this work, a novel observer design for linear systems with explicit constraints has been proposed. The proposed observer takes advantage of the structure of the constraints equations, making it possible to either recover a minimal coordinate representation or a representation with fewer coordinates (compared to, e.g., floating base representation for walking robots), making lower requirements on the measured outputs (hence, requiring fewer sensors) needed to make the observer stabilizable while keeping the observer state sufficient for both the feedback and feed-forward control laws design. The paper showed the observer structure, stability condition, gain design method, and demonstrated separation principle for the proposed observer.

The observer was tested on the table-top flywheel inverted pendulum (a hardware experiment), a Unitree A1 quadruped robot in a number of contact scenarios, as well as a quadruped robot model moving in the sagittal plane (where simulation results were demonstrated). The paper showed that the observer recovers minimal coordinate representation for a table-top flywheel inverted pendulum. Using Unitree A1 as an example, we showed that the observer automatically takes advantage of the given contact interaction scenario, lowering its state-space dimensions when possible. Simulations with a quadruped robot model moving in the sagittal plane demonstrated the convergence of the joint angle estimation errors and joint angle rate of change estimation errors to zero. Comparative analysis with Extended Kalman Filter showed that the proposed observer can perform better than EKF for a system with explicit constraints when the motion takes place further from the nominal trajectory, even in the presence of process and sensor noise; however, when the motion takes place on the nominal trajectory, EKF performs better, as the proposed observer gain design does not take into account the properties of un-modelled random inputs.

The proposed observer can be used in control design for linear systems with explicit constraints, or in gain-scheduling methods, where a nonlinear model is replaced with its local linearizations. The method fills the gap in orthogonal projection-based control design methods with respect to dynamic output feedback design. It requires the use of SVD decomposition and stabilizing feedback design algorithms (such as LQR or pole-placement), and is platform-independent, which makes it interesting in practical applications. The questions of stability verification for the closed-loop dynamics of nonlinear systems using the proposed observer are a subject of further study. The questions related to optimal gain design for such observers with respect to various metrics can be of further interest.