clear;clc;
M=5/10;
m=0.1;
L=1*0.3;
% I=1;
g=9.8;
% d=I+L^2*m-m^2*L^2/(M+m);
A=[0 1 0 0;
    0 0 -m*g/M 0;
    0 0 0 1;
    0 0 -(M+m)*g/L/M 0];
B=[0;
    1/M;
    0;
    1/L/M];
C=[1 0 0 0;0 0 1 0];
D=[0;0];
sys=ss(A,B,C,D);
co=ctrb(sys); %controlability matrix
rank(co)
impulse(sys,5)

%% upward

% aa=(M+m)*(m*L^2+I)/m/L-m*L;
Au=[0 1 0 0;
    0 0 -m*g/M 0;
    0 0 0 1;
    0 0 (M+m)*g/M/L 0];
Bu=[0;
    1/M;
    0;
    -1/M/L];
Cu=[1 0 0 0;0 0 1 0];
Du=[0;0];
sys1=ss(Au,Bu,Cu,Du);
co1=ctrb(sys1);
rank(co1)

impulse(sys1,1)

%% LQR control downward
q1=100;
q2=10;
Q=[q1 0 0 0;
    0 0 0 0;
    0 0 q2 0
    0 0 0 0];
R=1;
K=lqr(A,B,Q,R);
Acd=A-B*K;
eig(Acd)
Bcd=B;
Ccd=C;
Dcd=D;
sysd_cl=ss(Acd,Bcd,Ccd,Dcd);
T=0:0.01:5;
U=0.2*ones(size(T));
[Yd,T,Xd]=lsim(sysd_cl,U,T);
plot(T,Yd)
grid on
legend('cart','pendulum')
%% LQR upward
qu1=1000;
qu2=100;
Qu=[qu1 0 0 0;
    0 0 0 0;
    0 0 qu2 0
    0 0 0 0];
Ru=1;
Ku=lqr(Au,Bu,Qu,Ru);
Acu=Au-Bu*Ku;
eig(Acu)
Bcu=Bu;
Ccu=Cu;
Dcu=Du;
sysu_cl=ss(Acu,Bcu,Ccu,Dcu);
T=0:0.01:5;
U=0.2*ones(size(T));
[Yu,T,Xu]=lsim(sysu_cl,U,T);
figure
plot(T,Yu)
grid on
legend('cart','pendulum')

