卡尔曼滤波器MATLAB编程实现

fs=11025;f0=1312; A=[cos(2*pi*f0/fs) -sin(2*pi*f0/fs);sin(2*pi*f0/fs) cos(2*pi*f0/fs)]; H=[1 0]; [m,n]=size(H); T=800; w=wgn(n,T,1,5); v=wgn(m,T+1,1,5); q1=std(w(1,:)).^2; q2=std(w(2,:)).^2; Q=[q1 0;0 q2]; r1=std(v(1,:)).^2; R=r1; x2c=zeros([n,1]); Jc=10*eye(n); x=[]; Z=[]; x1=[]; P=[]; x2=[]; J=[]; x(:,1)=zeros([n,1]); for i=1:T-1 x(:,i+1)=A*x(:,i)+w(:,i); Z(:,i+1)=H*x(:,i+1)+v(:,i+1); end for k=1:T x1(:,k)=A*x2c; P(:,[1:n],k)=A*Jc*A'+Q; G(:,[1:m],k)=P(:,[1:n],k)*H'*((H*P(:,[1:n],k)*H'+R).^(-1)); x2(:,k)=x1(:,k)+G(:,[1:m],k)*(Z(:,k)-H*x1(:,k)); J(:,[1:n],k)=(eye(n)-G(:,[1:m],k)*H)*P(:,[1:n],k); end bc=x(1,:)-x2(1,:) t=1:T; plot(t,x(1,:),'r',t,x2(1,:),'b',t,bc,'g')