clear all;
close all;
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];
p2 = 4*M + m
    
C = [1 0 0 0;
     0 0 1 0];
D = [0;0];
E = eye(4);
 
Q = eye(4);
Q(1,1) = 500;
Q(2,2) = 1;
Q(3,3) = 50000;
Q(4,4) = 1000;

R = eye(1);
R(1,1) = 10;
states = {'x' 'x_dot' 'phi' 'phi_dot'};
inputs = {'u'};
outputs = {'x'};

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

% Discretize
Tsamp = 0.01;
prop_sys_d = c2d(sys_ss, Tsamp);
Ad = prop_sys_d.a;
Bd = prop_sys_d.b;
Cd = prop_sys_d.c;
Dd = prop_sys_d.d;

Kd = dlqr(E\Ad,E\Bd,Q,R);
Cn = [1 0 0 0];
sys_ss = ss(A,B,Cn,0);
Nbar = rscale(sys_ss,Kd)

Ac = (Ad-Bd*Kd);

D_plot = [0;0;0;0]

Kud =(Cd*inv(Ad-Bd*Kd)*Bd);
disp('Ku');
disp(Kud);
% I ended up using the calculation for Nbar from the Michigan website
% whcih uses rscale.m
%sys_cl = dss(Ac,Bd*Nbar,Cd,Dd,eye(4),Tsamp,'statename',states);
sys_cl = dss(Ac,Bd*Nbar,eye(4),D_plot,eye(4),Tsamp,'statename',states);

title('Step Response with LQR Control')
disp('Kd')
disp(Kd)

t = 0:Tsamp:5;
r =0.2*ones(size(t));
[y,t,x]=lsim(sys_cl,r,t);
[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)')
title('Step Response with LQR Control')
figure;
impulse(sys_cl);