limkalman.m

自适应滤波器程序

%kalman滤波器。参考书目：近代信号处理理论与方法，张明友等著，国防工业出版社。67页
clc;clear all;close all;
s(1)=0;N=1000;a=1;
w=randn(1,N);
for k=2:N
s(k)=a*s(k-1)+w(k-1);
end
figure
plot(s)
c=0.2;n=randn(1,N);
for k=1:N
    x(k)=c*s(k)+n(k);
end
figure
plot(x)
p(1)=0;s1(1)=0;
gw=(std(w))^2;
gn=(std(n))^2;
for k=2:N
    p1(k)=a^2*p(k-1)+gw;
    b(k)=c*p1(k)*(c^2*p1(k)+gn)^(-1);
    s1(k)=a*s1(k-1)+b(k)*(x(k)-c*a*s1(k-1));
    p(k)=p1(k)-c*b(k)*p1(k);
end
figure
plot(s1)