控制系统数字仿真与CAD-第二次实验-附完整代码

function [z]=fun(X)
Kn = 26.7;
taon = 0.03;
Ki = 0.269;
taoi = 0.067;
Ks = 76;
Ts = 0.00167;
R = 6.58;
Tl = 0.018;
Tm = 0.25;
Ce = 0.131;
alpha = 0.00337;
beta = 0.4;
Idl = 0;
P = [0, taon, Kn, Kn*taon;
     0, taoi, Ki, Ki*taoi;
     1, Ts, Ks, 0;
     1, Tl, 1/R, 0;
     0, Tm*Ce, R, 0];
A = diag(P(:,1));
B = diag(P(:,2));
C = diag(P(:,3));
D = diag(P(:,4));
WIJ = [1, 0, 1;
       1, 5, -alpha;
       2, 1, 1;
       2, 4, -beta;
       3, 2, 1;
       4, 3, 1;
       4, 5, -Ce;
       5, 4, 1];
m = length(WIJ(:,3));
W0 = zeros(5,1);
W = zeros(5,5);
for k = 1:m;
    if (WIJ(k,2 )==0);
        W0(WIJ(k, 1)) = WIJ(k,3);
    else W(WIJ(k, 1),WIJ(k, 2))=WIJ(k,3);
    end
end      
Q = B-D*W;
Qn = inv(Q);
R = C * W-A;
V1 = C * W0;
Ab = Qn * R;
Bb = Qn * V1;
z = Ab*X+Bb;
end

clear all;
clear;
h = 0.001;
y = [0;0;0;0;0];
x = [];
outputy1 = [];
outputy2 = [];
outputy3 = [];
outputy4 = [];
outputy5 = [];
for i = 0:1:1500
    t = i*h;
    x(i+1) = t;
    k1 = fun(y);
    k2 = fun(y+h*k1/2);
    k3 = fun(y+h*k2/2);
    k4 = fun(y+h*k3);
    y = y + (k1 + 2*k2 +2*k3 + k4)*h/6;
    outputy1(i+1) = y(1,1);
    outputy2(i+1) = y(2,1);
    outputy3(i+1) = y(3,1);
    outputy4(i+1) = y(4,1);
    outputy5(i+1) = y(5,1);
end
plot(x,outputy1,x,outputy2,x,outputy3,x,outputy4,x,outputy5)
legend('y1','y2','y3','y4','y5')