clc; clear all; close all;

%Khai bao cac tham so he thong

m = 0.078; l0 = 0.157;
l1=0.157; kt=0.07;
J0 = 4.4*10^-3;
J1 = 5.5026*10^-4;
b0 =5.8*10^-4;
b1 =1.4142*10^-3;
g =9.8;
% Xay dung cac ma tran cua phuong trinh trang thai
% Xd = A*X +B*i
% Y = D*X
A = [0 0 1 0;0 0 0 1; 0 (m^2*l1^2*l0*g)/(J0*J1+J0*m*l1^2+J1*m*l0^2) (-
b0*(m*l1^2+J1))/(J0*J1+J0*m*l1^2+J1*m*l0^2)
(-m*l1*l0*b1)/(J0*J1+J0*m*l1^2+J1*m*l0^2);0
(m*l1*g*(m*l0^2+J0)/(J0*J1+J0*m*l1^2+J1*m*l0^2))
(-m*l1*l0*b0)/(J0*J1+J0*m*l1^2+J1*m*l0^2)
(-b1*(m*l0^2+J0))/(J0*J1+J0*m*l1^2+J1*m*l0^2)];
B =
[0;0;(m*l1^2+J1)/(J0*J1+J0*m*l1^2+J1*m*l0^2);(m*l1*l0)/(J0*J1+J0*m*l1^2+J1*m*l
0^2)];
D = [0 0 0 0];
C = [1 0 0 0];
C1 = eye(4);
D1 = zeros(4,1);
% % Kiem tra tinh quan sat duoc
L=[C;C*A;C*A^2;C*A^3];
disp ('L ='),disp(L)
disp ('Gia tr cua det(L)='),disp(det(L))
if(rank(L)==4)
disp('He thong quan sat duoc');
else
disp('He thong khong quan sat duoc');
end
% % Kiem tra tinh dieu khien duoc
P = [B A*B A^2*B A^3*B];
disp ('P =');disp(P)
disp ('Gia tr cua det(P)=');disp(det(P))
if(rank(P)==4)
disp('He thong dieu khien duoc');
else
disp('He thong khong dieu khien duoc');
end
% %Tim cac tri rieng cua ma tran A
sys = ss(A,B,C1,D1) %Lap he tuyen tinh
zeros = zero(sys) %Tim tap hop cac diem khong
poles = pole(sys) %Tim cac diem cuc

%phýõng tr?nh ð?ng l?c h?c

xd1=x2;
xd2 =((m*l0*l1*cos(x1)*(0.5*m*l1^2*sin(2*x1)*x4^2+m*g*l1*sin(x1)-b1*x2))/
(J0*J1+m^2*l1^4*(sin(x1))^2+m^2*l0^2*l1^2+J0*m*l0^2+J0*m*l1^2+J0*m*l1^2*(sin(x
1))^2-m^2*l0^2*l1^2*(cos(x1))^2))-
(((m*l1^2+J1)*(m*l1^2*sin(2*x1)*x2*x4+m*l0*l1*sin(x1)*x2^2-u+b0*x4))/
(J0*J1+m^2*l1^4*(sin(x1))^2+m^2*l0^2*l1^2+J0*m*l0^2+J0*m*l1^2+J0*m*l1^2*(sin(x
1))^2-m^2*l0^2*l1^2*(cos(x1))^2)) ;
xd3=x4;
xd4
=((m*l0^2+m*l1^2*(sin(x1))^2+J0)*(0.5*m*l1^2*sin(2*x1)*x4^2+m*g*l1*sin(x1)-
b1*x2))/
(J0*J1+m^2*l1^4*(sin(x1))^2+m^2*l0^2*l1^2+J0*m*l0^2+J0*m*l1^2+J0*m*l1^2*(sin(x
1))^2-m^2*l0^2*l1^2*(cos(x1))^2)-
(m*l0*l1*cos(x1)*(m*l1^2*sin(2*x1)*x2*x4+m*l0*l1*sin(x1)*x2^2-u+b0*x4))/
((J0*J1+m^2*l1^4*(sin(x1))^2+m^2*l0^2*l1^2+J0*m*l0^2+J0*m*l1^2+J0*m*l1^2*(sin(
x1))^2-m^2*l0^2*l1^2*(cos(x1))^2));
xd_linear = [xd1; xd2; xd3; xd4];