【求助】程序有毛病，请大家帮忙改改

[liuxin5913364]

最近我编辑了一个ＭＡＴＬＡＢ仿真程序，这是一个关于小车倒立摆的ＰＩＤ仿真程序，但是运行仿真时得到的曲线极其不好，先是请大家帮忙指出毛病．以下是这个程序
　　%倒立摆-状态反馈；小车一阶 调用dlb_k_fun.m
%数值积分方法-四阶龙格库塔法
clear;
M=1.096; %小车质量
m=0.109;%摆杆质量
b=0.1; % 小车磨擦系数
l=0.25; %摆杆转动轴心到杆质心的长度
I=0.0034; %摆杆惯量
f=0; %摆杆所受的磨擦阻力矩系数
g=9.8; %重力

p=I*(M+m)+M*m*l^2;
A=[0 1 0 0;
0 -(I+m*l^2)*b/p (m^2*g*l^2)/p -m*l*f/p;
0 0 0 1;
0 -(m*l*b)/p m*g*l*(M+m)/p -(M+m)*f/p;];
B=[0;-(I+m*l^2)/p;0;-m*l/p];
C=[1 0 0 0;0 0 1 0];
D=[0;0];

%系统参数
x=[10;0;1.2;0];y10=0;y20=0;u=0;t=0; %初始值
tf=10;h=0.02; N=tf/h; %仿真总时间,仿真步长,仿真循环次数
T=h;
%td=0.02;Nd=td/T;
%N=500;
R=0*ones(1,N); %参数输入
%PID
Nd=2;
e=0;de=0;ie=0;
kp=0.1;ki=0.20;kd=150;
for k=1:N

if k<=Nd
u=0;
else
u=(kp*e+ki*de+kd*ie);
end

%利用格－塔法进行仿真
k1=A*x+B*u;
k2=A*(x+h*k1/2)+B*u;
k3=A*(x+h*k2/2)+B*u;
k4=A*(x+h*k3)+B*u;
x=x+h*(k1+2*k2+2*k3+k4)/6;
y10=[y10,C(1,*x]; %位移值
y20=[y20,C(2,*x] %摆杆角度值
t=[t,t(k)+h]; %时间
%计算系统输出误差和误差导数
e1=e;
%e=y(1,1)-R(1,k);
e=y20(1,k)-R(1,k);
de=(e-e1)/T;
ie=e*T+ie;
%yy1(1,k)=y;
end

%典型二阶环节的模糊控制输出曲线
%plot(t,y10,'r',t,y20,'b'); %画出响应曲线
plot(t,y20,'b'); %画出响应曲线
grid on
title('输出曲线');

%plot(kk,R,'k',kk,yy1,'b');

%xlabel('time(0.01)');
%ylabel('output');
%gtext('PID control');