kalman.m

利用Kalman预测对信号进行预测,信号源为《现代数字信号处理导论》上册

clear;

N=1000;
sigma_1=0.2;
sigma_2=2;
xminus3=0;
xminus2=0;
xminus1=0;
xzero=0;

v1=sigma_1*randn(1,N);
v2=sigma_2*randn(1,N);

x(1)=1.352*xzero-1.338*xminus1+0.662*xminus2-0.24*xminus3+v1(1);
x(2)=1.352*x(1)-1.338*xzero+0.662*xminus1-0.24*xminus2+v1(2);
x(3)=1.352*x(2)-1.338*x(1)+0.662*xzero-0.24*xminus1+v1(3);
x(4)=1.352*x(3)-1.338*x(2)+0.662*x(1)-0.24*xzero+v1(4);
for n=5:N
    x(n)=1.352*x(n-1)-1.338*x(n-2)+0.662*x(n-3)-0.24*x(n-4)+v1(n);
end

for n=1:N
    y(n)=x(n)+v2(n);
end

F=[0 1 0 0;0 0 1 0; 0 0 0 1;-0.24 0.662 -1.338 1.352];
q1=[0 0 0 0; 0 0 0 0 ; 0 0 0 0 ; 0 0 0 sigma_1^2];
Q1=sparse(q1);
C=[0 0 0 1];
Q2=sigma_2^2;

e_x(1,1)=0;
E_x=[0,0,0,e_x(1,1)]';
k(1,1)=sigma_1^2;
K=[0 0 0 0; 0 0 0 0 ; 0 0 0 0; 0 0 0 k(1,1)];

for n=1:N
    R=C*K*C'+Q2;
    G=F*K*C'*inv(R);
    a=y(n)-C*E_x;
    E_x=F*E_x+G*a;
    e_x(n+1)=E_x(4);
    P=K-F*G*C*K;
    K=F*P*F'+Q1;
end


s=N-100:N;
plot(s,x(N-100:N),'r-',s,y(N-100:N),'g--',s,e_x(N-100:N),'b-.');
xlabel('n');
ylabel('test');
title('this is my homework');
legend('x(n)','y(n)','^x(n)');