State Feedback

By Prof. Seungchul Lee
http://iai.postech.ac.kr/
Industrial AI Lab at POSTECH

Table of Contents

Reference

• Control of Mobile Robots by Prof. Magnus Egerstedt

1. State Space Representation

1.1. State Space

• Given a point mass on a line whose acceleration is directly controlled:

$$ \ddot{p} = u $$

• We want to write this on a compact/general form

$$ \begin{align*} \dot{x}_1 &= x_2\\ \dot{x}_2 &= u\\ \end{align*} $$

• on a state space form

$$ \begin{align*} \dot{x} &= \left[ {\begin{matrix} \dot{x}_1 \\ \dot{x}_2 \\ \end{matrix} } \right] = \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\\ \\y & = p=x_1= \left[ {\begin{matrix} 1 & 0 \end{matrix} } \right] \left[ {\begin{array}{cc} x_1 \\ x_2 \\ \end{array} } \right] \end{align*} $$

• Block diagram

1.2. The Car Model

$$ \dot{x} = \frac{c}{m}u - \gamma x$$

• If we care about/can measure the velocity:

$$ A = -\gamma, \qquad B = \frac{c}{m}, \qquad C = 1 $$

• If we care about/can measure the position we have the same general equation with different matrices:

$$ A = \left[ {\begin{matrix} 0 & 1 \\ 0 & -\gamma \\ \end{matrix} } \right], \qquad B = \left[ {\begin{matrix} 0 \\ \frac{c}{m} \\ \end{matrix} } \right], \qquad C = \left[ {\begin{matrix} 1 \quad 0 \\ \end{matrix} } \right] $$

% 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

• Control idea: move towards the origin $r=0$

$$ \begin{align*} u &= r-Ky = -KCx \\\\ \dot{x} &= Ax + Bu = Ax - BKCx = (A-BKC)x \end{align*} $$

• Assume $ \gamma = 0$

• Pick, if possible, $K(=1)$ such that

$$\text{Re}\,(\lambda) < 0 \quad \forall \lambda \in \text{eig} \,(A-BKC)$$

$$ \begin{align*} \dot{x} &= \left(\begin{bmatrix} 0 & 1 \\ 0 & 0\end{bmatrix} - \begin{bmatrix} 0 \\ 1\end{bmatrix} 1 \begin{bmatrix} 1 & 0 \end{bmatrix} \right) x \\\\ \dot{x} & = \begin{bmatrix} 0 & 1 \\ -1 & 0\end{bmatrix}x \\ \\ \text{eig} (A-BKC) &= \pm \,j \end{align*} $$

• What's the problem?
  • the problem is that we do not take the velocity into account
  • we need to use the full state information in order to stabilize this system

% 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)

ans =

   0.0000 + 1.0000i
   0.0000 - 1.0000i

2. State Feedback

• To move forwards origin, $r = 0$

$$ \begin{align*} \dot{x} &= Ax + Bu\\ \\ u &= -Kx \\\\ \dot{x} &= Ax + Bu = Ax - BKx = (A-BK)x \end{align*} $$

• Pick, if possible, $K$ such that the closed-loop system is stabilized

$$\text{Re}\, (\text{eig} (A-BK)) < 0 $$

$$ \begin{align*} K &= \begin{bmatrix}k_1 & k_2 \end{bmatrix}\\\\ \dot{x} &= \left(\begin{bmatrix} 0 & 1 \\ 0 & 0\end{bmatrix} - \begin{bmatrix} 0 \\ 1\end{bmatrix} \begin{bmatrix} k_1 & k_2 \end{bmatrix} \right) x \\\\ \dot{x} & = \begin{bmatrix} 0 & 1 \\ -k_1 & -k_2\end{bmatrix}x \\ \\ \end{align*} $$

• Let's try
  • Asymptotically stable
  • Damped oscillations

$$k_1 = k_2 = 1$$

$$ \begin{align*} A-BK &= \begin{bmatrix} 0 & 1 \\ -1 & -1\end{bmatrix}\\ \\ \text{eig} (A-BK) &= -0.5 \pm \,0.866j \end{align*} $$

• Let's do another attempt
  • Asymptotically stable
  • No oscillations

$$k_1 = 0.1, k_2 = 1$$

$$ \begin{align*} A-BK &= \begin{bmatrix} 0 & 1 \\ -0.1 & -1\end{bmatrix}\\\\ \text{eig} (A-BK) &= -0.1127, -0.8873 \end{align*} $$

• Eigenvalues Matter
  • It is clear that some eigenvalues are better than others. Some cause oscillations, some make the system respond too slowly, and so forth ...
  • We will see how to select eigenvalues and how to pick control laws based on the output rather than the state.

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)

ans =

  -0.5000 + 0.8660i
  -0.5000 - 0.8660i

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

• back to the point-mass, again

$$ u = -Kx \quad \rightarrow \quad \dot{x}=(A-BK)x $$

$$ \begin{align*} A-BK = \left[ {\begin{matrix} 0 & 1 \\ 0 & 0 \\ \end{matrix} } \right]- \left[ {\begin{array}{cc} 0 \\ 1 \\ \end{array} } \right] \left[ {\begin{array}{cc} k_1 \,\, k_2 \end{array} } \right]&= \left[ {\begin{matrix} 0 & 1 \\ -k_1 & -k_2 \\ \end{matrix} } \right] \end{align*} $$

$$ \left| { \begin{matrix} -\lambda & 1 \\ -k_1 & -k_2 -\lambda\\ \end{matrix} } \right|= \lambda^2 + \lambda k_2 + k_1 $$

• Desired eigenvalues: let's pick both eigenvalues at $-1$

$$ (\lambda+1)(\lambda+1) = \lambda^2 + 2 \lambda + 1 $$

$$k_1 = 2, k_2 = 1 $$

• Pick the control gains such that the eigenvalues (poles) of the closed loop system match the desired eigenvalues
  • Questions: is this always possible? (No)

• How should we pick the eigenvalues? (Mix of art and science)

  • No clear-cut answer
  • The "smallest" eigenvalue dominates the convergence rage
  • The bigger eigenvalues, the bigger control gains/signals

• Example

$$ \begin{align*} \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 \end{align*} $$

$$ \begin{align*} A-BK = \left[ {\begin{matrix} 2-k_1 & -k_2 \\ 1-k_1 & 1-k_2 \\ \end{matrix} } \right] \end{align*} $$

$$ \varphi = \lambda^2 + \lambda (-3 + k_1 + k_2) + 2 - k_1 - k_2$$

• Let's pick both eigenvalues at $-1$

$$ \varphi = (\lambda+1)^2 = \lambda^2 + \lambda (-3 + k_1 + k_2) + 2 - k_1 - k_2$$

$$-3 + k_1 + k_2 = 2 \quad \text{and} \quad 2 - k_1 - k_2 = 1$$

$\quad\;\rightarrow$ no $k_1$ and $k_2$ exist

• What's at play here is a lack of controllability, i.e., the effect of the input is not sufficiently rich to influence the system enough

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)

K =

    2.6250   -0.6250

$$ \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

• When can we place the eigenvalues using state feedback?

• When is $B$ matrix (the actuator configuration) rich enough so that we can make the system do whatever we want it to do?

• The answer revolves around the concept of controllability

• The system $\dot{x} = Ax + Bu$ is controllable if there exists a control $u(t)$ that will take the state of the system from any initial state $x_0$ to any desired final state $x_f$ in a finite time interval

• Given a discrete-time system

$$x_{k+1} = Ax_k + Bu_k$$

• We would like to drive this system in $n$ steps to a particular target state $x^{*}$

$$ \begin{align*} x_1 &= Ax_0 + Bu_0 = Bu_0\\\\ x_2 &= Ax_1 + Bu_1 = ABu_0 + Bu_1\\\\ x_3 &= Ax_2 + Bu_2 = A^2Bu_0 + ABu_1 + Bu_2\\\\ &\vdots\\\\ x_n &= A^{n-1}Bu_0 + \cdots + Bu_{n-1}\\\\ \end{align*} $$

• We want to solve

$$ \begin{align*} x^{*} &= \left[ {\begin{matrix} B & AB & \cdots & A^{n-1}B\\ \end{matrix} } \right] \left[ {\begin{array}{cc} u_{n-1} \\ \vdots \\ u_1\\ u_0 \end{array} } \right] \end{align*} $$

• $ \left[ \begin{matrix} B & AB & \cdots & A^{n-1}B \end{matrix}\right] $ is controllability matrix written as $C$

• The system ($A, B$) is controllable if and only if $C$ has full row rank

$$ \text{rank}\left(\left[ {\begin{matrix} B & AB & \cdots & A^{n-1}B\\ \end{matrix} } \right]\right) = n $$

• ctrb(A, B) is the MATLAB function to form a controllability matrix, $C$

• The $A$ matrix must be square

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)

G =

     1     2
     1     2


ans =

     1

Example 2

$$ \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)

G =

     0     1
     1     0


ans =

     2

3. Observer

• We now know how to design rather effective controllers using state feedback.

• But what about $y$ ?

• The predictor-corrector ('Luenberger' observer)

$$ \begin{align*} \dot x &= A x \\ y &= C x \end{align*} $$

$\quad \;$ 1) Make a copy of the system

$$\dot{\hat{x}} = A \hat x \quad \text{predictor}$$

$\quad \;$ 2) Add a notion of how wrong your estimate is to the model

$$ \dot{\hat{x}} = A \hat x + \underbrace{L \left(y - C \hat x \right)}_{\text{corrector}} $$

• What we want to stabilize (drive to zero) is the estimation error, i.e., the difference between the actual state and the estimated state $e = x - \hat x$

$$ \begin{align*} \dot e &= \dot x - \dot{\hat{x}} = Ax - A \hat x - L \left(y - C \hat x \right)\\\\ & = A\left( x - \hat x\right) - LC \left(x - \hat x \right) = (A-LC) \; e \end{align*} $$

• Just pick $L$ such that the eigenvalues to $A-LC$ have negative real part !!!

$$ \text{Re}\left( \text{eig} (A-LC)\right) < 0$$

• We already know how to do this $\rightarrow$ Pole-placement

• Does this always work?
  • No

3.1. Observability

• Need to redo what we did for control design to understand when we can recover the state from the output

• The system is observable if, for any $x(0)$, there is a finite time $\tau$ such that $x(0)$ can be determined from $u(t)$ and $y(t)$ for $0 \leq t \leq \tau$

• Given a discrete time system without inputs

$$ \begin{align*} x_{k+1} &= Ax_k \\ y_k &= C x_k \end{align*} $$

• Can we recover the initial condition by collecting $n$ output values?

$$ \begin{align*} y_0 &= Cx_0 \\\\ y_1 &= Cx_1 = CAx_0\\\\ &\;\vdots\\\\ y_{n-1} &= CA^{n-1}x_0 \\\\ \end{align*} $$

• The Observability Matrix $R$
  • The system $(A, C)$ is observable if and only if $R$ has full column rank

$$ \begin{align*} \left[ {\begin{array}{cc} y_{0} \\ y_1\\ y_2\\ \vdots \\ y_{n-1} \end{array} } \right] = \left[ {\begin{array}{cc} C \\ CA\\ CA^2\\ \vdots \\ CA^{n-1} \end{array} } \right] x_0 \end{align*} $$

• The initial condition can be recovered from the outputs when the so-called observability matrix has full rank.

• obsv(A, C) is the MATLAB function to form a observability matrix

• The $A$ matrix must be square with as many columns as $C$

Example 1

$$ \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)

ob =

     1     0
     0     1
     1     1
     4    -2


ans =

     2

4. The Separation Principle

• Now, how do we put everything together ?

$\quad\;$ Step 1) Design the stat feedback controller as if we had $x$ (which we don't)

$$ \begin{align*} u &= -Kx \quad \implies \dot x = (A-BK) x \quad \text{what we design for}\\\\ u &= -K \hat x \quad \text{what we actually have} \end{align*} $$

$\quad\;$ Step 2) Estimate $x$ using an observer (that now also contains $u$)

$$ \begin{align*} \dot{\hat{x}} &= A \hat x + Bu + L \left(y - C \hat x \right)\\\\ &\implies \dot e = (A-LC)\, e, \qquad (e = x - \hat x) \end{align*} $$

• Want both $x$ and $e$ to be stabilized ($r=0$)

$$ \begin{align*} \dot x &= Ax-BK \hat x = Ax - BK(x-e) = (A-BK)x + BK e\\\\ \dot e &= (A-LC)\,e \end{align*} $$

$\quad\;$ or

$$ \begin{bmatrix} \dot x \\ \dot e \end{bmatrix}= \underbrace{\begin{bmatrix} A-BK & BK \\ 0 & A-LC \end{bmatrix}}_{M} \begin{bmatrix} x \\ e \end{bmatrix} $$

• This is an (upper) triangular block matrix
  • Its eigenvalues are given by the eigenvalues of the diagonal blocks !

• (The Separation Principle) Design $K$ and $L$ independently to satisfy

$$ \text{Re}\left( \text{eig} (A-BK)\right) < 0, \quad \text{Re}\left( \text{eig} (A-LC)\right) < 0$$

5. Other Tutorials

%%html
<center><iframe src="https://www.youtube.com/embed/kQNUpNh6nBc?list=PLp8ijpvp8iCvFDYdcXqqYU5Ibl_aOqwjr?rel=0" 
width="560" height="315" frameborder="0" allowfullscreen></iframe></center>

%%html
<center><iframe src="https://www.youtube.com/embed/W6AUOyj5bFA?list=PLp8ijpvp8iCvFDYdcXqqYU5Ibl_aOqwjr?rel=0" 
width="560" height="315" frameborder="0" allowfullscreen></iframe></center>

%%html
<center><iframe src="https://www.youtube.com/embed/HmqOnsRH73w?list=PLp8ijpvp8iCvFDYdcXqqYU5Ibl_aOqwjr?rel=0" 
width="560" height="315" frameborder="0" allowfullscreen></iframe></center>

%%html
<center><iframe src="https://www.youtube.com/embed/m3-2ppIIWrk?rel=0" 
width="560" height="315" frameborder="0" allowfullscreen></iframe></center>

%%html
<center><iframe src="https://www.youtube.com/embed/S4WZTmEnbrY?list=PLp8ijpvp8iCvFDYdcXqqYU5Ibl_aOqwjr?rell=0" 
width="560" height="315" frameborder="0" allowfullscreen></iframe></center>

%%html
<center><iframe src="https://www.youtube.com/embed/5tWhOK8Klo0?list=PLp8ijpvp8iCvFDYdcXqqYU5Ibl_aOqwjr?rel=0" 
width="560" height="315" frameborder="0" allowfullscreen></iframe></center>

%%javascript
$.getScript('https://kmahelona.github.io/ipython_notebook_goodies/ipython_notebook_toc.js')