function pend_inv

M = 0.5;
m = 0.2;
b = 0.1;
I = 0.006;
g = 9.8;
l = 0.3;

p = I*(M+m)+M*m*l^2; %denominator for the A and B matrices

A = [0      1              0           0;
     0 -(I+m*l^2)*b/p  (m^2*g*l^2)/p   0;
     0      0              0           1;
     0 -(m*l*b)/p       m*g*l*(M+m)/p  0];
B = [     0;
     (I+m*l^2)/p;
          0;
        m*l/p];
C = [1 0 0 0;
     0 0 1 0];
D = [0;
     0];

states = {'x' 'x_dot' 'phi' 'phi_dot'};
inputs = {'u'};
outputs = {'x'; 'phi'}; % cart's displacement and pendulum deviation angle from pi

sys_ss = ss(A,B,C,D,'statename',states,'inputname',inputs,'outputname',outputs);

poles = eig(A)

co = ctrb(sys_ss);
controllability = rank(co) % controllabe when rank = n

Q = C'*C; %
R = 1;    % equal weights for u and y (feedback states)

lqr_ctr(sys_ss,Q,R);

% LQR with precompensation to eliminate steady-state error
%lqr_ctrn(sys_ss,Q,R);

Q2 = Q;   % retuning to meet the design requirements
Q2(1,1) = 5000;
Q2(3,3) = 100;

lqr_ctr(sys_ss,Q2,R);

% LQR with precompensation to eliminate steady-state error
lqr_ctrn(sys_ss,Q2,R)

ob = obsv(sys_ss);
observability = rank(ob)  % observable when rank = n

end

function lqr_ctr(sys,Q,R)

K = lqr(sys.A,sys.B,Q,R)

% closed-loop system
Ac = [(sys.A-sys.B*K)];
Bc = [sys.B];
Cc = [sys.C];
Dc = [sys.D];

CL_inputs = {'r'};

sys_cl = ss(Ac,Bc,Cc,Dc,'statename',sys.statename,'inputname',CL_inputs,'outputname',sys.outputname);

tspan = 0:0.01:5;
r =0.2*ones(size(tspan)); % step change in the setpoint of cart's displacement
[y,t,x]=lsim(sys_cl,r,tspan);
u = -(K*x')';
figure;
[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
set(get(AX(1),'Ylabel'),'String','cart position (m)')
set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')
xlabel('time (s)');
title('Step Response with LQR Control')
figure;
plot(t,u);
xlabel('time (s)'); ylabel('Force');
title('Step Response with LQR Control')
end

% LQR with precompensation
function lqr_ctrn(sys,Q,R)

K = lqr(sys.A,sys.B,Q,R)

% closed-loop system
Ac = [(sys.A-sys.B*K)];
Bc = [sys.B];
Cc = [sys.C];
Dc = [sys.D];

CL_inputs = {'r'};

% rescale the reference value to eliminate steady-state error
Cn = [1 0 0 0];
sys_ssn = ss(sys.A,sys.B,Cn,0);
Nbar = rscale(sys_ssn,K)

sys_cl = ss(Ac,Bc*Nbar,Cc,Dc,'statename',sys.statename,'inputname',CL_inputs,'outputname',sys.outputname);

tspan = 0:0.01:5;
r =0.2*ones(size(tspan)); % step change in the setpoint of cart's displacement
[y,t,x]=lsim(sys_cl,r,tspan);
u = -(K*x')';
figure;
[AX,H1,H2] = plotyy(t,y(:,1),t,y(:,2),'plot');
set(get(AX(1),'Ylabel'),'String','cart position (m)')
set(get(AX(2),'Ylabel'),'String','pendulum angle (radians)')
xlabel('time (s)');
title('Step Response with LQR Control')
figure;
plot(t,u);
xlabel('time (s)'); ylabel('Force');
title('Step Response with LQR Control')
end
