kaerman.m

卡尔曼滤波器的仿真实现 其中a,b两个参数由自己设定

ts=0.001;
A1=[0,1;0,-a]; 
B1=[0;b]; 
C1=[1,0]; 
D1=0; [A,B,C,D]=c2dm(A1,B1,C1,D1,ts,'z'); %变连续系统为离散系统
x=[-0.5;-0.5]; 
r_1=0;
r_2=0; 
Q=10; %Covariances of w 
Rm=10; %Covariances of v 
P=B*Q*B'; %Initial error covariance 
for k=1:1:2000 
    time(k)=k*ts; 
    r(k)=0.5*sin(1*2*pi*k*ts); 
    c=30;eq=150;q=300;
    Ce=[c,1]; 
    %Using Waitui method 
    dr(k)=(r(k)-r_1)/ts; 
    dr_1=(r_1-r_2)/ts; 
    r1(k)=2*r(k)-r_1; 
    dr1(k)=2*dr(k)-dr_1;
    
    R=[r(k);dr(k)]; 
    R1=[r1(k);dr1(k)]; 
    E=R-x; e(k)=E(1); 
    de(k)=E(2); 
    s(k)=Ce*E; 
    ds(k)=-eq*ts*sign(s(k))-q*ts*s(k); 
    u(k)=inv(Ce*B)*(Ce*R1-Ce*A*x-s(k)-ds(k)); %inv距阵求逆 
    wn(k)=0.1*rands(1); %Process noise on u 
    u(k)=u(k)+wn(k); 
    x=A*x+B*u(k); 
    v(k)=0.3*rands(1); %Measurment noise on y 
    yv(k)=C*x+v(k); 
    
    M=1 
      if M==1 % kalman filter 
          Mn=P*C'/(C*P*C'+Rm); 
          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; 
          errcov(k)=C*P*C'; %Covariance of estimation error 
      elseif M==2 %No filter 
          ye(k)=yv(k); 
          x(1)=ye(k); 
      end
      %Update Parameters 
      r_2=r_1; 
      r_1=r(k);
  end