卡尔曼滤波器MATLAB仿真问题

二硫碘化钾

1. clear;
   
2. FS=100;
   
3. T=1/FS;
   
4. t=0:T:5;
   
5. wv=randn(1,(5-0)/T+1)*0.2;
   
6. wa=randn(1,(5-0)/T+1);
   
7. vi=sin(5*t)+wv;
   
8. ai=cos(5*t)-10+wa;
   
9. 
   
10. A=[1,-T;0,1];
    
11. B=[T;0];
    
12. H=[1,0];
    
13. Q=[0.0001,0;0,0];
    
14. R=[0.04];
    
15. I=[1,1;1,1];
    
16. kg=[0;0];
    
17. x=[0;0];
    
18. p=[1,0;0,1];
    
19. xx=[0;0];
    
20. pp=[0,0;0,0];
    
21. for k=2:501
    
22.     xx=A*x+B*ai(k-1);
    
23.     pp=A*p*A'+Q;
    
24.     kg=pp*H'/(H*pp*H'+R);
    
25.     x=xx+kg*(vi(k)-H*xx);
    
26.     p=(I-kg*H)*pp;
    
27.     vo(k)=x(1)-5;
    
28.     b(k)=x(2);
    
29. end
    
30. plot(t,vi,'r',t,ai,'g',t,vo,'b',t,b,'y');

复制代码

以上是将光电码盘与加速度计融合得到速度的卡尔曼滤波程序（有错。。。），是根据一篇论文编写的，论文见链接。
仿真时出错，1、b不收敛。2、卡尔曼滤波似乎没起作用。。。请高手指点一二三四五六。。。。。。看看程序哪里有问题，在此谢过，此致敬礼。。。
还有程序的仿真图。。。与论文中的相比。。。差好远啊
红色是vi，绿色是ai，蓝色是滤波输出vo，黄色是b

二硫碘化钾

I is fault.

二硫碘化钾

thank me.

JJ哥

kiss

lowweihang123

x(1)即x的横坐标代表什么？为什么要减去5？？？还有x(2)代表什么？

ccyehuo

第一眼看过去，I就不对了兄弟

wen123

ccyehuo 发表于 2013-11-30 20:39
第一眼看过去，I就不对了兄弟

你好，，，，，，你是上海大学的吧，，找你有急事，，希望能帮帮忙！！感激不尽！麻烦加qq863331018私聊！！