kalman_filter_pid_3.m

基于KALMAN滤波的PID整定 有文字说明 是应用对象

%discrete kalman filter for PID control
%x=Fx+G(u+w(k))
%y=Hx+v(k)
clear all
close all
 
ts=0.001;
M=1000;%仿真运行的次数；

%continuous plant
a=20;b=119;
sys=tf(b,[1,a,8]);%连续系统的传递函数      % ％×××××××改变应用对象××××需要改动的地方
dsys=c2d(sys,ts,'z');
[num,den]=tfdata(dsys,'v');%连续 系统的传递函数  转换为   离散 系统的传递函数

%A1=[0 1;0 -a];
%B1=[0;b];
%C1=[1 0];
%D1=[0];
%[A,B,C,D]=c2dm(A1,B1,C1,D1,ts,'z');
num1=[0 0 119];
den1=[1 20 8];%连续系统的传递函数         %％×××××××改变应用对象××××需要改动的地方
[a,b,c,d]=tf2ss(num1,den1);
[A,B,C,D]=c2dm(a,b,c,d,ts,'z');%连续 系统的状态矩阵 转换为 离散 系统的状态矩阵


Q=1;R=1;

P=B*Q*B';
x=zeros(2,1);                            %％×××××××改变应用对象××××需要改动的地方 

y_2=0;y_1=0;
 u_2=0;u_1=0; %％×××××××改变应用对象××××需要改动的地方
 ei=0;
 error_1=0;
   
for k=1:1:M
    time(k)=k*ts;
    
    rin(k)=1;
    kp=8;ki=0.8;kd=0.2;
    
    
    w(k)=0.002*rands(1);
    v(k)=0.002*rands(1);

   
  
  %y(k)=-y_2*den(3)-y_1*den(2)+u_1*num(2)+u_2*num(3);
  y(k)=-den(2)*y_1-den(3)*y_2+num(2)*u_1+num(3)*u_2; %％×××××××改变应用对象××××需要改动的地方
    yv(k)=y(k)+v(k);
    
    %measurement update
    Mn=P*C'/(C*P*C'+R);
    P=A*P*A'+B*Q*B';
    P=(eye(2)-Mn*C)*P;                     %％×××××××改变应用对象××××需要改动的地方
   
    x=A*x+Mn*(yv(k)-C*A*x);
    ye(k)=C*x+D;
    
   tr=2;
  if tr==1;
      yout(k)=yv(k);
   elseif tr==2
     yout(k)=ye(k);
   end 
 
    
    error(k)=rin(k)-yout(k);
    ei=ei+error(k)*ts;
    
    u(k)=kp*error(k)+ki*ei+kd*(error(k)-error_1)/ts;
    u(k)=u(k)+w(k);
       
    errcov(k)=C*P*C';
     
    %TIME update
    x=A*x+B*u(k);
 
u_2=u_1;u_1=u(k); %％×××××××改变应用对象××××需要改动的地方
y_2=y_1;y_1=yout(k);%把滤波后的输出信号反馈到输入端    %％×××××××改变应用对象××××需要改动的地方
error_1=error(k);

end
    figure(1);
    plot(time,rin,'k',time,yout,'k');
   
    xlabel('time(s)');
   % ylabel('y-ideal signal;ye-filtered signal');
   if tr==1;
     ylabel('rin;ye-noised signal'); 
   elseif tr==2
     ylabel('rin;ye-filtered signal');
   end 
   
    
    figure(2);
    plot(time,errcov,'k');
    xlabel('time(s)');
    ylabel('covariance of estimation error');