clear all, close all, clc;

%% Define the system

A = [1 0 2;
2 3 0;
1 2 3]
% the state matrix

B = [1; 0; 0]
% the input vector

eig(A)
% the eigenvalues are
% 4.7511 + 0.0000i
% 1.1244 + 1.0251i
% 1.1244 - 1.0251i
% it is not stable

%% LQR design

Q = eye(size(A))

R=1

K = lqr(A,B,Q,R)
%K=
% 14.5461 24.4004 41.9475

%% State feedback controller

sys = ss(A-B*K, eye(size(A)), eye(size(A)), eye(size(A)))

eig(sys.a)
% the eigenvalues are
% -4.7742 + 0.0000i
% -1.3860 + 1.1477i
% -1.3860 - 1.1477i
% it is stable

x0 = [5; 5; 5]
% initial state

t = 0:0.01:5;
% the time duration

x = initial(sys,x0,t);
% response of the state-space
% with initial condition x0
% and time duration t

x1 = [1 0 0]*x';
x2 = [0 1 0]*x';
x3 = [0 0 1]*x';
% the state variables

%% Plot the state variables

subplot(3,1,1); plot(t,x1), grid

title('Response to initial condition')
ylabel('x1')

subplot(3,1,2); plot(t,x2), grid

ylabel('x2')

subplot(3,1,3); plot(t,x3), grid

ylabel('x3')
xlabel('t (seconds)')

2. state space to transfer function

clear all, close all, clc;

%% State space representation

A = [-11 -18;
1 0]
% the state matrix

B = [1; 0]
% the input vector

C = [0 1]
% the output vector

D = zeros(size(C,1),size(B,2))
% the feedforward

%% Verify the controllability and observability

Cm = ctrb(A,B)
% Controllability matrix

Om = obsv(A,C)
% Observability matrix

if rank(Cm) == size(A,1)
'It is controllable'
else
'It is not controllable'
end

if rank(Om) == size(A,1)
'It is observable'
else
'It is not observable'
end

%% Transform into transfer function

[num,den] = ss2tf(A,B,C,D)
sys = tf(num,den)
pole(sys)
% yields
% s = -9
% s = -2

step(sys)
% plot the step response
3. POLE PLACEMENT

clear all, close all, clc;

%% Define the system

A = [1 0 2;
2 3 0;
1 2 3]
% the state matrix

B = [1; 0; 0]
% the input vector

eig(A)
% the eigenvalues are
% 4.7511 + 0.0000i
% 1.1244 + 1.0251i
% 1.1244 - 1.0251i
% it is not stable

%% The pole-placement

desiredPoles = -0.5*ones(3,1)
% desired poles = [-0.5; -0.5; -0.5]

K = acker(A,B,desiredPoles)

% determine the gain K

% based on the Ackermann's formula
% yields
% K = [8.5000 13.0156 12.7188]

%% State feedback controller

sys = ss(A-B*K, eye(size(A)), eye(size(A)), eye(size(A)))

eig(sys.a)
% the eigenvalues are
% -0.5000 + 0.0000i
% -0.5000 + 0.0000i
% -0.5000 - 0.0000i

x0 = [0.1; 0.1; 0.1]

% initial state

t = 0:0.01:25;
% the time duration

x = initial(sys,x0,t);
% response of the state-space
% with initial condition x0
% and time duration t

x1 = [1 0 0]*x';
x2 = [0 1 0]*x';
x3 = [0 0 1]*x';
% the state variables
%% Plot the state variables

subplot(3,1,1); plot(t,x1), grid

title('Response to initial condition')
ylabel('x1')

subplot(3,1,2); plot(t,x2), grid

ylabel('x2')

subplot(3,1,3); plot(t,x3), grid

ylabel('x3')
xlabel('t (seconds)')

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