请教卡尔曼滤波跟踪角频率

pebl

有信号s(t)=a*sin(w*t)+n(t),现在想利用卡尔曼滤波从s(t)中估计出w,选取
X=[a theta w]'为目标量，其中theta为相位，w为角频率，
建立状态方程：
X(k)=[1 0 0;0 1 ts;0 0 1]*X(k-1)+V(k-1),其中V(k-1)是噪声
观测方程：
Y(k)=a(k)*sin(theta(k))+n(k),其中n(k)是噪声
下面是代码，运行下来估计值是发散的，哪位大侠帮我看看问题在哪里？或者说扩展
卡尔曼滤波就不适合这种用途，需要用别的类型的卡尔曼滤波？先谢谢了！
clc
clear all
f=200;
fs=1000;
ts=1/fs;
a=1;
T=1;
w=2*pi*f;
t=0:ts:T-ts;
noise=randn(1,length(t));
s=a*sin(w*t)+noise;
%%--目标初始值
A=a;
Theta=2*pi*f*ts;
w=2*pi*f;
X=[A Theta w]';
%%----卡尔曼滤波初值
N=500;%迭代次数
Xstore=zeros(3,N);%用来存储每次迭代后状态变量值
F=[1 0 0;0 1 ts;0 0 1];%状态转移矩阵
H=[sin(Theta) A*cos(Theta) ts*A*cos(Theta)];%观测矩阵
K=[1;1;1];%增益初值
p=500*eye(3);%状态初始方差
P=zeros(3,N);%用来存储每次迭代后的方差值
tao=eye(3);%噪声驱动矩阵
Q=5*eye(3);%状态过程的噪声方差
R=var(s);%观测噪声方差
%%-----扩展卡尔曼滤波---
for i=1:N
   X=F*X;%状态一步预测
   p=F*p*F'+tao*Q*tao';%一步预测方差
   K=p*H'*inv(H*p*H'+R);%滤波增益更新
   X=X+K*(s(i)-X(1)*sin(X(2)));%状态更新
   p=(eye(3)-K*H)*p;%方差更新
   P(:,i)=[p(1,1);p(2,2);p(3,3)];
   Xstore(:,i)=[X(1);mod(X(2),2*pi);X(3)];
end
i=1:N;
figure(1)
plot(i*ts,P(1,:),i*ts,P(2,:),i*ts,P(3,:))
title('估计方差')
legend('幅度估计方差'，'相位估计方差'，'角频率估计方差')
figure(2)
plot(i*ts,a,'r')
hold on
plot(i*ts,Xstore(1,:))
hold off
title('幅度估计曲线')
legend('实际值'，'估计值')
figure(3)
plot(i*ts,2*pi*f,'r')
hold on
plot(i*ts,Xstore(2,:))
title('角频率估计曲线')
legend('实际值','估计值')