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')
评论6