LQ Control

Recap

The theory of optimal control is concerned with operating a dynamic system at minimum cost.

The infinity horizon LQ optimal control problem is formulated as:

$$ \min_{u(t),\,t\in [0,\infty )} J(u)=\min_{u(t),\,t\in [0,\infty )} \int_{t=0}^{\infty } \left(x^T (\tau )Qx(\tau )+u^T (\tau )Ru(\tau )\right)d\tau $$

Matrices $Q=Q^T \ge 0,R=R^T >0$ are the design parameters chosen according to the desired performance tradeoff.

Solvability

Necessary conditions to the solution of this problem are related to the reachability and observability of the LTI system

$$ \left\lbrace \begin{array}{ll} \dot{x} (t)=Ax(t)+Bu(t), & x(t)\in {\mathbb{R}}^n \newline y(t)=Cx(t) & \end{array}\right. $$

Reachability matrix:

$$ M_R (A,B)=[B AB A^2 B \cdots A^{n-1} B] $$

Observability matrix:

$$ M_O (A,C)=\left\lbrack \begin{array}{c} C\newline CA\newline CA^2 \newline \vdots \newline CA^{n-1} \end{array}\right\rbrack $$

1
2
3
4


% Reachability
M_R=ctrb(A,B);
% Observability
M_O=obsv(A,C);

$$\rho (M_R )=\rho (M_O )=n$$

then the closed-loop system described by the state equation

$$\dot{x} (t)=(A-BK)x(t)$$

is asymptotically stable.

If their rank

$$\rho (M_R )=n$$

then the optimal solution exists and it’s equal to

$$u^* (t)=-Kx(t)$$$$ K=R^{-1} B^T P $$

Where $P=P^T ,P>0$ is the solution to the Algebraic Riccati Equation (ARE)

$$ Q-PBR^{-1} B^T P^T +PA+A^T P=0 $$

The infinite horizon LQ optimal control control law $u(t)=-Kx(t)$ is realized through a static state feedback control architecture.

Tuning

Tuning LQ regulators implies choosing the weight matrices Q and R.

Q and R are usually chosen as diagonal matrices, so that for a

system with n states and p control inputs we have n+p parameters to choose. The diagonal values $q_{jj} \ge 0$ and $r_{jj} \ge 0$ are chosen according to the relative importance of each state and control variable.

Q is usually expressed as a multiplication of 2 matrixes (cholesky factorization of Q)

$$ Q=C_q^T C_q $$

1

C_q=chol(Q);

Then, it is possible to find K:

1

K=lqr(A,B,Q,R);

Example

The double integrator is a common example in control theory. It models the dynamics of a simple mass in along an axis under the effect of a time-varying force input u.

$$ \left\lbrace \begin{array}{ll} \dot{x} (t)=Ax(t)+Bu(t), & x(t)\in {\mathbb{R}}^n \newline y(t)=Cx(t) & \end{array}\right. $$$$ \ddot{z} (t)=F(t)\Longrightarrow \left\lbrace \begin{array}{l} z(t)=x_1 (t)\textrm{position}\newline \dot{z} (t)=x_2 (t)\textrm{speed}\newline F(t)=u(t)\textrm{force} \end{array}\right. $$$$ A=\left\lbrack \begin{array}{cc} 0 & 1\newline 0 & 0 \end{array}\right\rbrack ,B=\left\lbrack \begin{array}{c} 0\newline 1 \end{array}\right\rbrack ,C=\left\lbrack \begin{array}{cc} 1 & 0 \end{array}\right\rbrack $$$$ x_0 =\left\lbrack \begin{array}{r} -6\newline 0 \end{array}\right\rbrack $$

The problem will be divided in 2 parts:

Calculating K
Testing

Calculating K

The matrix Q will be fixed, and three cases will be considered by varying the parameter R.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27


clear
close all
clc
A=[0 1;0 0];
B=[0;1];
C=[1 0];
D=0;
x0=[-6;0];

M_R=ctrb(A,B);
rho_M_R=rank(M_R);

M_O=obsv(A,C);
rho_M_O=rank(M_O);
if rho_M_R~=rho_M_O
    disp("ERROR");
end

Q=[1 0; 0 0.001];
R1=0.1;
R2=1;
R3=10;

K1=lqr(A,B,Q,R1);
K2=lqr(A,B,Q,R2);
K3=lqr(A,B,Q,R3);
% that's all folks!

Testing

At first there is the needing to calculate the system at steady state.

Then it’s possible to plot the results and doing the valuation.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28


% Here the output must be equal to the input,
% so C_x=eye(2) and D=0 as usual.

% Performing 3 simulations...
close all
t_sim=15;
sys_x=ss(A,B,eye(2),0);
K=K1;
out1=sim("LQone.slx");

K=K2;
out2=sim("LQone.slx");

K=K3;
out3=sim("LQone.slx");

% u(t)
f1=figure(1);
plot(out1.u.time, out1.u.data,'r');
hold on
plot(out2.u.time, out2.u.data,'g');
hold on
plot(out3.u.time, out3.u.data,'b');
grid on
legend(["R = 0.1", "R = 1", "R = 10"]);
xlabel("t [s]");
ylabel("u(t)");
title("u(t)");

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13



%x1(t)
f2=figure(2);
plot(out1.x.time, out1.x.data(:,1),'r');
hold on
plot(out2.x.time, out2.x.data(:,1),'g');
hold on
plot(out3.x.time, out3.x.data(:,1),'b');
grid on
legend(["R = 0.1", "R = 1", "R = 10"]);
xlabel("t [s]");
ylabel("x_1(t)");
title("x_1(t)");

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13



%x2(t)
f3=figure(3);
plot(out1.x.time, out1.x.data(:,2),'r');
hold on
plot(out2.x.time, out2.x.data(:,2),'g');
hold on
plot(out3.x.time, out3.x.data(:,2),'b');
grid on
legend(["R = 0.1", "R = 1", "R = 10"]);
xlabel("t [s]");
ylabel("x_2(t)");
title("x_2(t)");