基于kalman滤波的直流伺服电机模糊PID位置控制仿真研究(6)

time(k)=k*ts; %后面用到，当k=300时，time（k）=k*ts=300*0.001=0.3s % r(k)=sin(2*pi*k*ts); %位置指令为幅值为1的正弦信号 r(k)=1.0; %位置指令为幅值为1的阶跃信号

%Using fuzzy inference to tunning PID

k_pid=evalfis([e_1,ec_1],a); %evalfis命令是进行模糊推理的计算 kp(k)=kp0+k_pid(1); ki(k)=ki0+k_pid(2); kd(k)=kd0+k_pid(3);

u(k)=kp(k)*rin(1)+kd(k)*rin(2)+ki(k)*rin(3); % PID Controller

w(k)=rands(1); %Process noise on u u(k)=u(k)+w(k);

y(k)=-den(2)*y_1-den(3)*y_2+num(1)*u(k)+num(2)*u_1+num(3)*u_2; %离散化后的被控对象 x=A*x+B*u(k);

v(k)=0.03*rands(1); %Measurement noise on y yv(k)=I*y(k)+v(k);

%%%%%%%%%%%%%%Kalmanfiltering%%%%%%%%%%%%%%%%%%%% M=1;

if M==1 %Kalman Filter G=P*C'/(C*P*C'+Rn); P=A*P+A'+B*Q*B'; P=(eye(2)-G*C)*P; x=A*x+G*(yv(k)-C*A*x); ye(k)=C*x;

elseif M==2 %No Filter ye(k)=yv(k); x(1)=ye(k); end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% e(k)=r(k)-ye(k);

25

%%%%%%%%%%%%%%Return of PID parameters%%%%%%%%%%%%%%% u_3=u_2; u_2=u_1; u_1=u(k); y_3=y_2; y_2=y_1; y_1=ye(k);

rin(1)=e(k); % Calculating P rin(2)=(e(k)-e_1)/ts; % Calculating D rin(3)=rin(3)+e(k)*ts; % Calculating I

e1(k)=r(k)-yv(k); %无Kalman的误差 e2(k)=r(k)-ye(k); %有Kalman的误差

ec_1=rin(2); %这样下次的ec_1就更新了。 %e_2=e_1; %可以没有。

e_1=e(k); %这样下次的e(k)-e_1=下次的e(k)-上次的e(k)=下次的rin(2) end figure(1); subplot(211);

plot(time,yv,'b',time,r,'k');

xlabel('time(s)');ylabel('yv无Kalman,r(k)'); subplot(212);

plot(time,ye,'r',time,r,'k');

xlabel('time(s)');ylabel('ye有Kalman,r(k)'); figure(2); subplot(211); plot(time,e1,'b');

xlabel('time(s)');ylabel('e1无Kalman的误差'); subplot(212); plot(time,e2,'r');

xlabel('time(s)');ylabel('e2有Kalman的误差');

26