[louyu1984]

下面是我编的一段程序，自己编写的龙格库塔算法。。。主要是一个控制算法对一个非线性对象进行控制。。。但是仿真总是出错，找了一个下午也不知道错误在哪里。。。
哪位大侠帮帮忙。。。



clear;
v=10;u=1;f=10;kq1=0.1820;kq2=0.1886;v01=30.48;v02=55.33;b=4000e2;m=2200;A1=3.1416;A2=1.6567;t=0.01;kv=1;be=3.9e5;p0=0.5;ps=315000000;f=100000;%parameters
x1(1)=0; x2(1)=0;x3(1)=p0;x4(1)=0;f(1)=0;g(1)=0;df(1)=0;dg(1)=0;%initial value
h=0.01;%stepsize
for i=1:500

x1n=x1(i);x2n=x2(i);x3n=x3(i);x4n=x4(i);fn=f(i);gn=g(i);dfn=df(i);dgn=g(i);

i=i+1;

k1=x2n;
l1=(A1*x3n-b*x2n-f)/m;
p1=(-A1*x2n+sqrt(ps-x3n)*x4n*kq1)*be/(v01+A1*x1n);
n1=(-x4n+kv*u)/t;

k2=x2n+0.5*l1*h;
l2=(-b*(x2n+0.5*l1*h)+A1*(x3n+0.5*p1*h)-f)/m;
p2=(-A1*(x2n+0.5*l1*h)+sqrt(ps-(x3n+0.5*h*p1))*(x4n+0.5*n1*h)*kq1)*be/(v01+A1*(x1n+0.5*k1*h));
n2=(-(x4n+0.5*n1*h)+kv*u)/t;

k3=x2n+0.5*l2*h;
l3=(-b*(x2n+0.5*l2*h)+A1*(x3n+0.5*p2*h)-f)/m;
p3=(-A1*(x2n+0.5*l2*h)+sqrt(ps-(x3n+0.5*h*p2))*(x4n+0.5*n2*h)*kq1)*be/(v01+A1*(x1n+0.5*k2*h));
n3=(-(x4n+0.5*n2*h)+kv*u)/t;

k4=x2n+l3*h;
l4=(-b*(x2n+l3*h)+A1*(x3n+p3*h)-f)/m;
p4=(-A1*(x2n+l3*h)+sqrt(ps-(x3n+h*p3))*(x4n+n3*h)*kq1)*be/(v01+A1*(x1n+k3*h));
n4=(-(x4n+0.5*n3*h)+kv*u)/t;

x1(i)=x1n+h*(k1+2*k2+2*k3+k4)/6;
x2(i)=x2n+h*(l1+2*l2+2*l3+l4)/6;
x3(i)=x3n+h*(p1+2*p2+2*p3+p4)/6;
x4(i)=x4n+h*(n1+2*n2+2*n3+n4)/6;

f(i)=(m*v+(b-m)*x2(i)+f)/A1;
df(i)=(f(i)-fn)/h;
g(i)=((v01+A1*x1(i))*(df(i)+f(i)-x3(i))+A1*be*x2(i))/(kq1*be*sqrt(ps-x3(i)));
dg(i)=(g(i)-gn)/h;
u=(t*dg(i)+t*g(i)+(1-t)*x4(i))/kv;

%u=0.1*(10-x2(i));
%%x7(i)=u;u=15000*(10-x2(i))+20000*(x2(i)-x2n)/h;
%x7(i)=u;
if x3(i)>314000000;
x3(i)=314000000;
end

%u=15*(10-x2(i));
end