ball.m

xnsy算法的matlab实现

figure('name','校正前系统阶跃响应曲线','color',[0.5,0.5,0.7]);
m=0.111;
R=0.015;
g=-9.8;
J=9.99e-6;
H=-m*g/(J/(R^2)+m);
A=[0 1 0 0;0 0 H 0; 0 0 0 1;0 0 0 0];
B=[0;0;0;1];
C=[1 0 0 0];
D=[0];
[z,p,k]=ss2zp(A,B,C,D,1)
T=0:0.01:5;
U=0.25*ones(size(T));
[Y,X]=lsim(A,B,C,D,U,T);
plot(T,Y);
title('ball-beam System step response');
xlabel('Time-sec');
ylabel('response-value');

n=length(A);
Q=zeros(n);
Q(:,1)=B;
for i=2:n
    Q(:,i)=A*Q(:,i-1);
end
m=rank(Q);
if m==n
    disp('System state Variables can be totally contronlled');
else
    disp('System state Variables cannot be totally contronlled');
    disp('The rank of System controllable Matrix is:');
    m
end