Matlab--采用低通滤波器的PID控制

  被控对象的传递函数：
        
  低通滤波器为：
        
采样时间为1ms,噪声信号加在对象的输出端。

输出模型的变换参考下面：
%%参考给出网站
然后得到

$$y(k)+a_{1}y(k-1)+\cdot \cdot \cdot +a_{n}y(n-k)=b_{0}u(k)+b_{1}u(k-1)+\cdot \cdot \cdot +a_{n}u(n-k)$$
$$y(k)+de{n}_{1}y(k-1)+\cdot \cdot \cdot +de{n}_{n}y(n-k)=nu{m}_{0}u(k)+nu{m}_{1}u(k-1)+\cdot \cdot \cdot +nu{m}_{n}u(n-k)$$
$$y(k)=-de{n}_{1}y(k-1)-\cdot \cdot \cdot -de{n}_{n}y(n-k)+nu{m}_{0}u(k)+nu{m}_{1}u(k-1)+\cdot \cdot \cdot +nu{m}_{n}u(n-k)$$
通常分子的系数要大于分母的系数，即m<n

%PID Controller with Partial differential
clear all;
close all;

ts=0.001;
sys=tf(5.235e005,[1,87.35,1.047e004,0]);  %%连续系统的模型
dsys=c2d(sys,ts,'z');                     %%离散化
[num,den]=tfdata(dsys,'v');               %%得到离散化后的分子分母

u_1=0;u_2=0;u_3=0;u_4=0;u_5=0;
y_1=0;y_2=0;y_3=0;
yn_1=0;
error_1=0;error_2=0;ei=0;

kp=0.20;ki=0.05;

sys1=tf([1],[0.04,1]);   %Low Freq Signal Filter
dsys1=c2d(sys1,ts,'tustin');
[num1,den1]=tfdata(dsys1,'v');
f_1=0;

M=3;                    %%%选择的方式
for k=1:1:1000
time(k)=k*ts;           %%输出的时间控制

yd(k)=20;  %Step Signal            %%希望的理想输出为20的阶跃信号

%Linear model         
y(k)=-den(2)*y_1-den(3)*y_2-den(4)*y_3+num(2)*u_1+num(3)*u_2+num(4)*u_3;  %%线性模型

if M==1                 %没有噪声信号
   error(k)=yd(k)-y(k); 
   filty(k)=y(k);
end

n(k)=5.0*rands(1);      %Noisy signal
yn(k)=y(k)+n(k);

if M==2                 %No filter
   filty(k)=yn(k);
   error(k)=yd(k)-filty(k);
end
if M==3                 %使用低通滤波器呀
   filty(k)=-den1(2)*f_1+num1(1)*(yn(k)+yn_1);
   error(k)=yd(k)-filty(k);
end

%I separation            %%积分的隔离
if abs(error(k))<=0.8
   ei=ei+error(k)*ts;
else
   ei=0;
end
   u(k)=kp*error(k)+ki*ei;               %%计算输出
%----------Return of PID parameters------------
yd_1=yd(k);
u_5=u_4;u_4=u_3;u_3=u_2;u_2=u_1;u_1=u(k);  
y_3=y_2;y_2=y_1;y_1=y(k);

f_1=filty(k);
yn_1=yn(k);

error_2=error_1;
error_1=error(k);
end
figure(1);
subplot(211);
plot(time,yd,'r',time,filty,'k:','linewidth',2);
xlabel('time(s)');ylabel('yd,y');
legend('Ideal position signal','Position tracking');
subplot(212);
plot(time,u,'r','linewidth',2);
xlabel('time(s)');ylabel('u');
figure(2);
plot(time,n,'r','linewidth',2);
xlabel('time(s)');ylabel('Noisy signal');