卡尔曼正弦信号滤波.m

卡尔曼滤波问题

clc;
clear all
t=1:300;
s1=sin(4*pi*t/300);%有用信号
C=0.01;  %噪音强度
N=3000; %某点的观测次数
A=0.99;
for t=1:300
   z=C*randn(1,N)+sin(4*pi*t/300);%在某个点观察了N次的测量结果
xx(1)=0;
Q=0.00738;
R=1;
p(1)=0.2;
for k=2:1:N
xs(k)=A*xx(k-1);
ps(k)=p(k-1)+Q;
K(k)=0.0098;
xx(k)=xs(k)+K(k)*(z(k)-xs(k));
p(k)=(1-K(k))*ps(k);
end
x(t)=xx(k);
end
 
plot(s1);
pause;
hold on 
plot(x,'k')