请教KALMAN滤波

花瓣雨

目地：从一个包含噪声的正弦扫频信号中，将扫频信号提取出来。
具体如下，那位帮我分析一下。万分感谢！
clear
N=1024;%定义数据个数%
%w(1)=0;
w=0.5*randn(1,N)+0; %此为获得方差为o，均值为u的白噪声，o*randn(M,N)+u%
x(1)=0;
x(2)=0;
a=[2,-1;1,0];%状态矩阵
for k=3:N;
    x(k)=1;
    x(k)=2*x(k-1)-x(k-2)+w(k-2);
    x(k-1)=x(k-1);
end
%
V=randn(1,N);
q1=std(V);
Rvv=q1.^2;%定义观测误差
q2=std(x);%求取矩阵方差 %
Rxx=q2.^2;%观测协方差%对应R
q3=std(w);
Rww=[q3.^2,0;0,0;]%定义系统协方差%对应Q
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
for i=0:N-1
    fai(i+1)=2*pi*i/10;
end
fPhasor=0;
%%Vibration signal
for i=1:N
    fPhasor=fPhasor+fai(i)/N;
    theta(i)=fPhasor;
    signal(i)=sin(fPhasor);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%量测矩阵
%plot(signal);

%c=[1,0];%观测矩阵
p=[1 0;0 1];%p(1)=0,定义初始方差为5%
s(1)=0;
s(2)=0;
for k=3:N
    c(1)=theta(k);
    c(2)=theta(k-1);
    Y=c*[x(k) x(k-1)]'+V;%观测方程
    p1=a*p*a'+Rww;
    b=p1*c'/(c*p1*c'+Rvv);
    s(k:-1:k-1)=a*[s(k-1),s(k-2)].'+b*(Y(k)-c*a*[s(k-1),s(k-2)].');
    p=p1-b*c*p1;   
end
t=1:N;
%plot(t,Y,'g');
hold on;
t=1:N;
plot(t,s,'r');
hold on;
t=1:N;
plot(t,signal,'b');