clear
clc
P=[-4+3i,-4-3i,-30+6i,-30-6i];
P1=[-20+3i,-20-3i,-80-1i,-80+1i];
tf=10;
r=[-0.1;0.05;0;0];
A=[     0       0         1         0
         0       0         0         1 
     65.8751  -16.8751    -3.7062     0.2760
    -82.2122   82.2122    4.6254     -1.3444] ; % System matrix
B=[0;0;5.2184;-6.5125]; % Control Matrix
C=[1,0,0,0;0,1,0,0]; % output matrix
disp(['r(Qc)=',num2str(rank (ctrb (A, B)))]);
disp(['r(Qo)=',num2str(rank (obsv (A, C)))]);
disp('λ=');
disp(num2str(eig (A))); % stability
[K,L,t,x]=dlb(r,P,P1,tf,A,B,C);
disp(['K=',num2str(K)]);
disp('L=');
disp(num2str(L));
figure;
plot(t,x(:,1)*180/pi,'r',t,x(:,2)*180/pi,'b',t,x(:,5),'g');
legend('spiral arm angle','the pendulum angle','control value');
function [K,L,t,x]=dlb(r,P,P1,tf,A,B,C)
global K;
K=place(A,B,P); % state feedback gain matrix
L=place(A',C',P1)';
t0=0;
u0=-K*r; 
x0=[r;u0];%initial value 
[t,x]=ode45(@dlfun,[t0,tf],x0);% solve differential equations
function xdot=dlfun(~,x)
m1=0.200; m2=0.052; L1=0.10; L2=0.12; r1=0.20; km=0.0236; ke=0.2865; 
g=9.8; J1=0.004; J2=0.001; f1=0.01; f2=0.001; % value of parameter 
a=J1+m2*r1*r1; b=m2*r1*L2; c=J2; d=f1+km*ke; e=(m1*L1+m2*r1)*g;  f=f2;  h=m2*L2*g;
x(1)=x(1,:);
x(2)=x(2,:);
x(3)=x(3,:);
x(4)=x(4,:);
x(5)=x(5,:);
u=-K*[x(1);x(2);x(3);x(4)]; % control variable 
xdot=zeros(5,1); %derivative of x 
xdot(1)=x(3); 
xdot(2)=x(4); 
xdot(3)=((-d*c).*x(3)+(f*b*cos(x(2)-x(1))).*x(4)+b*b*sin(x(2)-x(1)).*cos(x(2)-x(1)).*x(3).*x(3)-b*c*sin(x(1)-x(2)).*x(4).*x(4)+e*c*sin(x(1))-h*b*sin(x(2)).*cos(x(2)-x(1))+km*c*u)/(a*c-b*b.*cos(x(1)-x(2)).*cos(x(2)-x(1))); 
xdot(4)=((d*b*cos(x(1)-x(2))).*x(3)-(a*f).*x(4)-a*b*sin(x(2)-x(1)).*x(3).*x(3)+b*b*sin(x(1)-x(2)).*cos(x(1)-x(2)).*x(4).*x(4)-e*b*sin(x(1)).*cos(x(1)-x(2))+a*h*sin(x(2))-b*cos(x(1)-x(2))*km*u)/(a*c-b*b.*cos(x(1)-x(2)).*cos(x(2)-x(1))); % differential equations to describe the nonlinear model
xdot(5)=-K*[xdot(1);xdot(2);xdot(3);xdot(4)]; % derivative of control variable
end
end