Control:
State Feedback
Table of Contents
Reference
- Control of Mobile Robots by Prof. Magnus Egerstedt
1. State Space Representation¶
1.1. Introduction to State Space¶
The state space representation provides a unified and compact way to model dynamic systems using first-order differential equations.
This approach is particularly useful for analyzing multi-input, multi-output (MIMO) systems and for applying modern control techniques.
Example: A Point Mass on a Line
Consider a point mass moving along a line. Its motion is governed by Newton's second law:
$$ \ddot{p}(t) = u(t) $$
where:
- $p(t)$ is the position of the mass
- $u(t)$ is the input force applied to the mass
To express this as a first-order system, define the state variables:
$$ x_1 = p(t), \quad x_2 = \dot{p}(t) $$
Then the system dynamics become:
$$ \begin{aligned} \dot{x}_1 &= x_2 \\ \dot{x}_2 &= u \end{aligned} $$
State Space Form
This system can be written in the standard state-space form:
$$ \dot{x} = Ax + Bu, \qquad y = Cx + Du $$
where:
- $x = \begin{bmatrix} x_1 \\ x_2 \end{bmatrix}$ is the state vector
- $u$ is the control input
- $y$ is the system output
For our example, we have:
$$ A = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ 1 \end{bmatrix}, \quad C = \begin{bmatrix} 1 & 0 \end{bmatrix}, \quad D = 0 $$
Thus, the complete state-space model becomes:
$$ \begin{aligned} \dot{x} &= \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix} x + \begin{bmatrix} 0 \\ 1 \end{bmatrix} u \\ y &= \begin{bmatrix} 1 & 0 \end{bmatrix} x \end{aligned} $$
This model captures the full dynamics of the system in a form suitable for analysis and controller design.
Block diagram
1.2. The Car Model¶
To illustrate how different modeling choices affect the state-space representation, consider a simple model of a car.
The car’s motion is governed by Newton’s second law, taking into account a control input and a linear damping (friction) force.
Velocity-based Model
Suppose we model the car's velocity $x(t)$, and assume that the force applied to the car is proportional to a control input $u(t)$.
Let $\gamma > 0$ denote a damping coefficient (e.g., due to friction or air resistance), and let $c/m$ represent the control gain, where:
- $m$ is the mass of the car
- $c$ is a control constant related to the input actuator (e.g., throttle gain)
The governing equation is:
$$ \dot{x} = -\gamma x + \frac{c}{m} u $$
This is already in first-order form and can be written in state-space form:
$$ \dot{x} = Ax + Bu, \qquad y = Cx $$
with:
$$ A = -\gamma, \qquad B = \frac{c}{m}, \qquad C = 1 $$
This model is suitable when we care about or can directly measure the velocity of the car.
Position-based Model
Alternatively, suppose we are interested in tracking the position of the car. Define:
$$ x_1 = \text{position}, \quad x_2 = \text{velocity} $$
Then the state equations become:
$$ \begin{aligned} \dot{x}_1 &= x_2 \\ \dot{x}_2 &= -\gamma x_2 + \frac{c}{m} u \end{aligned} $$
This can be written in state-space form as:
$$ \dot{x} = Ax + Bu, \qquad y = Cx $$
with:
$$ A = \begin{bmatrix} 0 & 1 \\ 0 & -\gamma \end{bmatrix}, \qquad B = \begin{bmatrix} 0 \\ \frac{c}{m} \end{bmatrix}, \qquad C = \begin{bmatrix} 1 & 0 \end{bmatrix} $$
This model captures both position and velocity, and is appropriate when we are interested in the car’s trajectory over time.
Both models are valid but serve different purposes depending on the system outputs of interest.
% system in ss
c = 1;
m = 1;
gamma = 1;
A = -gamma;
B = c/m;
C = 1;
D = 0;
Gss = ss(A,B,C,D);
% P controller
k = 5;
C = k;
% close loop
Gcl = feedback(C*Gss,1,-1);
x0 = 0;
t = linspace(0,5,100);
r = 70*ones(size(t));
[y,tout] = lsim(Gcl,r,t,x0);
plot(tout,y,tout,r,'--k'), xlabel('t'), ylim([0,75])
1.3. Output Feedback¶
Consider the control objective of driving the system state to the origin. In other words, we aim for asymptotic convergence of the state $x(t)$ to zero as $t \to \infty$.
To achieve this, we define the control input $u$ as a function of the system output $y$:
Output Feedback Law
Suppose we use the following control strategy:
$$ u = r - Ky = -KCx $$
Here,
- $r = 0$ is the desired reference (origin)
- $K$ is a scalar gain
- $C$ is the output matrix, so $y = Cx$
Substituting into the system dynamics yields the closed-loop system:
$$ \dot{x} = Ax + Bu = Ax - BKCx = (A - BKC)x $$
This system matrix $A - BKC$ governs the closed-loop behavior. The location of its eigenvalues determines the stability of the system.
Example: Output Feedback with Incomplete State
Assume that $\gamma = 0$, and recall the earlier car model with position and velocity states:
$$ A = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ 1 \end{bmatrix}, \quad C = \begin{bmatrix} 1 & 0 \end{bmatrix} $$
Apply output feedback with $K = 1$:
$$ \begin{aligned} \dot{x} &= \left( A - BKC \right) x \\ &= \left( \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix} - \begin{bmatrix} 0 \\ 1 \end{bmatrix} \cdot 1 \cdot \begin{bmatrix} 1 & 0 \end{bmatrix} \right) x \\ &= \begin{bmatrix} 0 & 1 \\ -1 & 0 \end{bmatrix} x \end{aligned} $$
The eigenvalues of the closed-loop system are:
$$ \text{eig}(A - BKC) = \pm j $$
These purely imaginary eigenvalues indicate that the system is marginally stable, but not asymptotically stable.
The system will oscillate indefinitely rather than converge to the origin.
The problem arises because we have used only the output $y = x_1$ (position) in our feedback law, ignoring the velocity $x_2$.
Without knowledge of velocity, we cannot adequately damp the system.
This motivates the use of full-state feedback, where all state variables are available for control design.
Full-state feedback enables precise placement of the closed-loop poles in the left-half plane to ensure asymptotic stability.
% to move towards the origin
% u = -y
A = [0 1;0 0];
B = [0 1]';
C = [1 0];
D = 0;
G = ss(A,B,C,D);
K = 1;
Gcl = feedback(G,K,-1);
x0 = [1 0]';
t = linspace(0,10,100);
r = zeros(size(t));
[y,tout] = lsim(Gcl,r,t,x0);
plot(tout,y), xlabel('t')
eig(Gcl)
2. State Feedback¶
State feedback allows us to modify the dynamics of a system by feeding back the entire state vector.
The goal is to design a controller that drives the system state to the origin over time.
Closed-Loop Dynamics
Suppose the control objective is to stabilize the system at the origin, i.e., $r = 0$.
We define the control law as:
$$ u = -Kx $$
where $K$ is a row vector of appropriate dimension.
Substituting this into the system dynamics:
$$ \dot{x} = Ax + Bu = Ax - BKx = (A - BK)x $$
The stability of the closed-loop system depends on the eigenvalues of the matrix $A - BK$.
Specifically, we seek:
$$ \text{Re}(\lambda) < 0 \quad \text{for all} \quad \lambda \in \text{eig}(A - BK) $$
Example
Let the open-loop system be defined as:
$$ A = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ 1 \end{bmatrix}, \quad K = \begin{bmatrix} k_1 & k_2 \end{bmatrix} $$
Then the closed-loop system becomes:
$$ \dot{x} = \left( A - BK \right)x = \begin{bmatrix} 0 & 1 \\ -k_1 & -k_2 \end{bmatrix}x $$
(1) Case 1: Asymptotically Stable with Damped Oscillations__
Choose:
$$ k_1 = 1, \quad k_2 = 1 $$
Then:
$$ A - BK = \begin{bmatrix} 0 & 1 \\ -1 & -1 \end{bmatrix}, \quad \text{eig}(A - BK) = -0.5 \pm 0.866j $$
These complex conjugate eigenvalues indicate oscillatory behavior with exponential decay.
The system is asymptotically stable.
(2) Case 2: Asymptotically Stable without Oscillations__
Choose:
$$ k_1 = 0.1, \quad k_2 = 1 $$
Then:
$$ A - BK = \begin{bmatrix} 0 & 1 \\ -0.1 & -1 \end{bmatrix}, \quad \text{eig}(A - BK) = -0.1127,\ -0.8873 $$
Both eigenvalues are real and negative.
The system is asymptotically stable with no oscillations.
Summary
Eigenvalues of the closed-loop matrix $A - BK$ govern the system’s dynamic response.
Their location determines:
- Stability
- Speed of convergence
- Oscillatory behavior
In later sections, we will discuss how to select desired eigenvalues systematically and how to implement control laws when full state measurement is not available.
A = [0 1;0 0];
B = [0 1]';
C = [1 0];
D = 0;
G = ss(A,B,C,D);
k1 = 1;
k2 = 1;
K = [k1 k2];
Gcl = ss(A-B*K,B,C,D);
x0 = [1 0]';
t = linspace(0,30,100);
r = zeros(size(t));
[y,tout] = lsim(Gcl,r,t,x0);
plot(tout,y,tout,zeros(size(tout)),'k--'), xlabel('t')
eig(Gcl)
A = [0 1;0 0];
B = [0 1]';
C = [1 0];
D = 0;
G = ss(A,B,C,D);
k1 = 0.1;
k2 = 1;
K = [k1 k2];
Gcl = ss(A-B*K,B,C,D);
x0 = [1 0]';
t = linspace(0,30,100);
r = zeros(size(t));
[y,tout] = lsim(Gcl,r,t,x0);
plot(tout,y,tout,zeros(size(tout)),'k--'), xlabel('t')
2.1. Pole Placement¶
Consider again the point-mass system with full state feedback:
$$ u = -Kx \quad \Rightarrow \quad \dot{x} = (A - BK)x $$
Let
$$ A = \begin{bmatrix} 0 & 1 \\ 0 & 0 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ 1 \end{bmatrix}, \quad K = \begin{bmatrix} k_1 & k_2 \end{bmatrix} $$
Then the closed-loop system becomes:
$$ A - BK = \begin{bmatrix} 0 & 1 \\ -k_1 & -k_2 \end{bmatrix} $$
To find the characteristic polynomial, compute:
$$ \begin{vmatrix} -\lambda & 1 \\ -k_1 & -k_2 - \lambda \end{vmatrix} = \lambda^2 + \lambda k_2 + k_1 $$
Suppose we desire both closed-loop poles at $-1$, i.e., the desired characteristic polynomial is:
$$ (\lambda + 1)^2 = \lambda^2 + 2\lambda + 1 $$
Matching coefficients:
$$ k_2 = 2, \quad k_1 = 1 $$
This procedure is known as pole placement — selecting the feedback gain $K$ to place the eigenvalues of the closed-loop system at desired locations in the complex plane.
Is Pole Placement Always Possible?
No - pole placement is only possible if the system is controllable.
Controllability means that the input can influence all modes (states) of the system.
How to Choose Desired Poles?
There is no single best answer.
The choice is typically based on the following trade-offs:
- The smallest (most slowly decaying) eigenvalue dominates the convergence rate
- Placing poles farther into the left-half plane increases speed, but also requires larger control effort
- Avoid placing poles too far left to prevent actuator saturation or numerical issues
Example: A Non-Controllable Case
Consider the system:
$$ \dot{x} = \begin{bmatrix} 2 & 0 \\ 1 & 1 \end{bmatrix} x + \begin{bmatrix} 1 \\ 1 \end{bmatrix} u $$
We attempt state feedback with $u = -Kx$, and compute:
$$ A - BK = \begin{bmatrix} 2 - k_1 & -k_2 \\ 1 - k_1 & 1 - k_2 \end{bmatrix} $$
The characteristic polynomial is:
$$ \varphi(\lambda) = \lambda^2 + \lambda(-3 + k_1 + k_2) + (2 - k_1 - k_2) $$
Suppose we again desire both poles at $-1$:
$$ (\lambda + 1)^2 = \lambda^2 + 2\lambda + 1 $$
Matching terms:
$$ -3 + k_1 + k_2 = 2, \quad 2 - k_1 - k_2 = 1 $$
This system of equations has no solution.
The failure to place the poles arises from lack of controllability.
The input $u$ cannot influence all directions in the state space sufficiently to arbitrarily assign closed-loop eigenvalues.
A = [2 0;
1 -1];
B = [1 1]';
C = [1 0];
P = [-0.5 + 1j, -0.5 - 1j];
%P = [-0.1 + 1j, -0.1 - 1j];
%P = [-0.5, -1];
%P = [-5, -4];
K = place(A,B,P)
$$ \dot{x} = Ax + Bu = Ax-BKx = (A-BK)x $$
x0 = [1 1]';
Gcl = ss(A-B*K,B,C, 0);
t = linspace(0,5,100);
u = zeros(size(t));
[y, tout] = lsim(Gcl,u,t,x0);
plot(tout,y,tout,zeros(size(tout)),'k--'), xlabel('t')
2.2. Controllability¶
To determine whether we can place the eigenvalues of the closed-loop system arbitrarily using state feedback, we must examine the concept of controllability.
The system
$$ \dot{x} = Ax + Bu $$
is said to be controllable if, for any initial state $x_0$ and any desired final state $x_f$, there exists a control input $u(t)$ that transfers the system from $x_0$ to $x_f$ in finite time.
This definition can be explored more intuitively by considering a discrete-time analog:
$$ x_{k+1} = Ax_k + Bu_k $$
Assume we want to drive the system to a target state $x^*$ in $n$ steps, starting from the origin. The state evolves as:
$$ \begin{align*} x_1 &= Bu_0 \\ x_2 &= ABu_0 + Bu_1 \\ x_3 &= A^2Bu_0 + ABu_1 + Bu_2 \\ &\vdots \\ x_n &= A^{n-1}Bu_0 + A^{n-2}Bu_1 + \cdots + Bu_{n-1} \end{align*} $$
This can be written compactly as:
$$ x^* = \begin{bmatrix} B & AB & \cdots & A^{n-1}B \end{bmatrix} \begin{bmatrix} u_{n-1} \\ \vdots \\ u_1 \\ u_0 \end{bmatrix} $$
Define the matrix
$$ \mathcal{C} = \begin{bmatrix} B & AB & \cdots & A^{n-1}B \end{bmatrix} $$
which is called the controllability matrix.
The system $(A, B)$ is controllable if and only if $\mathcal{C}$ has full row rank:
$$ \text{rank}(\mathcal{C}) = n $$
This condition ensures that every direction in the $n$-dimensional state space can be influenced by the input $u(t)$ through the matrix $B$ and its successive transformations by $A$.
In MATLAB, the controllability matrix is computed using the function:
C = ctrb(A, B)
Note that $A$ must be a square matrix for controllability to be defined in this context.
Example 1
$$ \dot{x} = \left[ {\begin{matrix} 2 & 0 \\ 1 & 1 \\ \end{matrix} } \right] \left[ {\begin{array}{cc} x_1 \\ x_2 \\ \end{array} } \right] + \left[ {\begin{array}{cc} 1 \\ 1 \\ \end{array} } \right]u $$
A = [2 0;
1 1];
B = [1 1]';
G = ctrb(A,B)
rank(G)
Example
$$ \dot{x} = \left[ {\begin{matrix} 0 & 1 \\ 0 & 0 \\ \end{matrix} } \right] \left[ {\begin{array}{cc} x_1 \\ x_2 \\ \end{array} } \right] + \left[ {\begin{array}{cc} 0 \\ 1 \\ \end{array} } \right]u $$
A = [0 1;
0 0];
B = [0 1]';
G = ctrb(A,B)
rank(G)
3. Observer¶
We have seen how to design effective controllers using state feedback. However, in practice, the full state vector $x$ is often not directly measurable. Instead, we observe the system output $y$:
$$ \begin{align*} \dot{x} &= Ax \\ y &= Cx \end{align*} $$
To estimate the unmeasured state $x(t)$, we construct an observer. One widely used approach is the Luenberger observer, which consists of two components: a predictor and a corrector. (For simplicity, we omit the input matrix $B$ and any control input $u$ from the formulation.)
Predictor
We start by creating a copy of the system that evolves according to the same dynamics:
$$ \dot{\hat{x}} = A \hat{x} $$
This equation predicts the state estimate $\hat{x}$, assuming perfect knowledge of the system model and no correction from measurements.
Corrector
To account for errors in the prediction, we introduce a correction term that adjusts the state estimate using the difference between the actual output $y$ and the estimated output $C\hat{x}$:
$$ \dot{\hat{x}} = A \hat{x} + L(y - C\hat{x}) $$
Here, $L$ is the observer gain matrix, and $y - C\hat{x}$ represents the estimation error in the output. This structure is often referred to as a predictor-corrector formulation.
Estimation Error Dynamics
Define the estimation error as:
$$ e = x - \hat{x} $$
Then, the dynamics of the estimation error are:
$$ \begin{align*} \dot{e} &= \dot{x} - \dot{\hat{x}} = Ax - (A\hat{x} + L(y - C\hat{x})) \\ &= A(x - \hat{x}) - L(Cx - C\hat{x}) \\ &= (A - LC)e \end{align*} $$
To ensure the estimation error decays to zero, we require that all eigenvalues of $(A - LC)$ lie in the left-half plane:
$$ \text{Re} \left( \lambda \right) < 0 \quad \forall \lambda \in \text{eig}(A - LC) $$
This condition ensures that the observer is asymptotically stable, and $\hat{x}(t) \to x(t)$ as $t \to \infty$.
We can use techniques such as pole placement to select $L$ so that the observer error dynamics have desired stability and performance characteristics.
Does Observer Design Always Work?
No. The ability to arbitrarily place the eigenvalues of $(A - LC)$ depends on a property known as observability. This will be discussed next.
3.1. Observability¶
In the context of linear systems, observability refers to the ability to reconstruct the full internal state of a system from its external outputs over a finite time interval. Just as controllability characterizes how the input affects the state, observability characterizes how much information the output contains about the state.
Consider a discrete-time, linear time-invariant (LTI) system with no input:
$$ \begin{aligned} x_{k+1} &= A x_k, \\ y_k &= C x_k, \end{aligned} $$
where $x_k \in \mathbb{R}^n$ is the state and $y_k \in \mathbb{R}^p$ is the output at time step $k$. The goal is to determine whether the initial state $x_0$ can be uniquely identified from a finite sequence of output measurements $\{y_0, y_1, \dots, y_{n-1}\}$.
We compute the output sequence as follows:
$$ \begin{aligned} y_0 &= C x_0, \\ y_1 &= C A x_0, \\ y_2 &= C A^2 x_0, \\ &\;\vdots \\ y_{n-1} &= C A^{n-1} x_0. \end{aligned} $$
Stacking these equations into a single matrix expression yields:
$$ \begin{bmatrix} y_0 \\ y_1 \\ \vdots \\ y_{n-1} \end{bmatrix} = \begin{bmatrix} C \\ C A \\ \vdots \\ C A^{n-1} \end{bmatrix} x_0. $$
The matrix
$$ R = \begin{bmatrix} C \\ C A \\ \vdots \\ C A^{n-1} \end{bmatrix} $$
is called the observability matrix. The system $(A, C)$ is said to be observable if and only if $R$ has full column rank, i.e.,
$$ \text{rank}(R) = n. $$
In this case, the system’s initial state $x_0$ can be uniquely determined from the output measurements. If the rank condition is not satisfied, some components of the state are unobservable, and the initial state cannot be uniquely identified.
In MATLAB, the observability matrix can be constructed using the built-in function:
R = obsv(A, C)
Note: The matrix $A$ must be square, and the number of columns in $C$ must match the number of columns in $A$.
Example
$$ \begin{align*} \dot{x} &= \left[ {\begin{matrix} 1 & 1 \\ 4 & -2 \\ \end{matrix} } \right] \left[ {\begin{array}{cc} x_1 \\ x_2 \\ \end{array} } \right] + \left[ {\begin{array}{cc} 1 \\ 1 \\ \end{array} } \right]u \\ y &= \left[ {\begin{matrix} 1 & 0 \\ 0 & 1 \end{matrix} } \right]x \end{align*} $$
A = [1 1;
4 -2];
C = [1 0;
0 1];
ob = obsv(A,C)
rank(ob)
4. The Separation Principle¶
In many practical systems, we cannot directly measure the full state $x$ but only the output $y = Cx$.
To design a controller in such cases, we must estimate the state using an observer and then apply state feedback using the estimate $\hat{x}$.
The separation principle provides a powerful framework to do this.
Step 1: Design the state feedback controller as if the full state $x$ were available
Assume we can apply full-state feedback:
$$ u = -Kx \quad \Rightarrow \quad \dot{x} = (A - BK)x. $$
In practice, since we do not have access to $x$, we implement:
$$ u = -K\hat{x}, $$
where $\hat{x}$ is the estimate of the true state.
Step 2: Estimate the state using an observer
We use an observer to estimate $x$ from $y$ and $u$:
$$ \dot{\hat{x}} = A\hat{x} + Bu + L(y - C\hat{x}). $$
Define the estimation error $e = x - \hat{x}$.
The dynamics of the estimation error become:
$$ \dot{e} = \dot{x} - \dot{\hat{x}} = (A - LC)e. $$
Combined system dynamics
Substitute $u = -K\hat{x}$ into the plant dynamics:
$$ \dot{x} = Ax + Bu = A x - BK \hat{x} = A x - BK(x - e) = (A - BK)x + BK e. $$
Hence, the combined system evolves according to:
$$ \begin{aligned} \dot{x} &= (A - BK)x + BK e, \\ \dot{e} &= (A - LC)e. \end{aligned} $$
This can be written in block matrix form:
$$ \begin{bmatrix} \dot{x} \\ \dot{e} \end{bmatrix} = \begin{bmatrix} A - BK & BK \\ 0 & A - LC \end{bmatrix} \begin{bmatrix} x \\ e \end{bmatrix}. $$
This matrix is upper triangular, and its eigenvalues are simply the union of the eigenvalues of the diagonal blocks.
Separation Principle
To ensure overall stability, we design $K$ and $L$ separately to make both $A - BK$ and $A - LC$ stable:
$$ \text{Re}(\text{eig}(A - BK)) < 0, \quad \text{Re}(\text{eig}(A - LC)) < 0. $$
This result — known as the separation principle — allows us to design the controller and observer independently while still guaranteeing stability of the closed-loop system.
5. Other Tutorials¶
from IPython.display import YouTubeVideo
YouTubeVideo('kQNUpNh6nBc', width = "560", height = "315")
from IPython.display import YouTubeVideo
YouTubeVideo('W6AUOyj5bFA', width = "560", height = "315")
from IPython.display import YouTubeVideo
YouTubeVideo('HmqOnsRH73w', width = "560", height = "315")
from IPython.display import YouTubeVideo
YouTubeVideo('m3-2ppIIWrk', width = "560", height = "315")
from IPython.display import YouTubeVideo
YouTubeVideo('S4WZTmEnbrY', width = "560", height = "315")
from IPython.display import YouTubeVideo
YouTubeVideo('5tWhOK8Klo0', width = "560", height = "315")
%%javascript
$.getScript('https://kmahelona.github.io/ipython_notebook_goodies/ipython_notebook_toc.js')