untitled2.asv

来自「卡尔曼滤波问题」· ASV 代码 · 共 27 行

clc;
clear all
t=1:300;
s1=sin(4*pi*t/300);%有用信号
C=1;  %噪音强度
N=3000; %某点的观测次数
for t=1:300
   z=C*randn(1,N)+sin(4*pi*t/300);%在某个点观察了N次的测量结果
xx(1)=0;
Q=【0.09 0；0.09 0】;
R=1;
p(1)=0.2;
for k=2:1:N
xs(k)=xx(k-1);
ps(k)=p(k-1)+Q;
K(k)=ps(k)/(ps(k)+R);
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')