% Model-based control systems
% Exercise 9, Problem 1
clear all; clc; close all
%Model
s=tf('s');
G=(s+0.5)/(s^2+2*s+4);
Gss=ss(G);
[A,B,C,D]=ssdata(Gss);
%Cost function
Q=3*C'*C; R=0.5;
%Optimal control u=-Lx, regulator problem
[L,S,E]=lqr(A,B,Q,R);
damp(A-B*L);
%Closed loop
Gclss=ss(A-B*L,zeros(2,1),C,0);
%Simulation
x0=[-1,1]';
T=0:0.01:10;
U=zeros(size(T));
lsim(Gclss,U,T,x0)
%Optimal control u=-Lx+kr, servo problem
k=1/(C*inv(-A+B*L)*B);
G2clss=ss(A-B*L,k*B,C,0);
figure
step(G2clss)