扩展卡尔曼滤波的编程求助

程序感觉没错，但是运行出现警告，产生的数据也不对为NaN
In D:\共享\ekfm.m at line 47
Warning: Matrix is singular to working precision.
(Type "warning off MATLAB:singularMatrix" to suppress this warning.)
附件文m文件和数据文件，达人帮忙调下啊
我的QQ：27234351，可适当收费

1. close all
   
2. clear all
   
3. %读取数据，从中取出电流电压等值
   
4. load shuju.mat
   
5. t1=shuju(1,:);
   
6. Ua=shuju(2,:);
   
7. Ub=shuju(3,:);
   
8. Ia=shuju(4,:);
   
9. Ib=shuju(5,:);
   
10. v=shuju(6,:);
    
11. s=shuju(7,:);
    
12. %电机定值设定
    
13. Rs=2.65;L=0.0267;Ke=59.5;tao=0.016;
    
14. ts=t1(2)-t1(1)
    
15. Maxk=floor(max(size(t1)))
    
16. B=[1/L,0;0,1/L;0,0;0,0];
    
17. H=[1,0,0,0;0,1,0,0];
    
18. Q=[0.3,0,0,0;0,0.3,0,0;0,0,0.3,0;0,0,0,3];
    
19. R=[2e-5,0;0,2e-5];
    
20. XX=zeros(4,1);
    
21. PP=[1e-6,0,0,0;0,1e-6,0,0;0,0,1e-6,0;0,0,0,2.5];
    
22. U=zeros(2,1);Y=zeros(2,1);
    
23. IaE=zeros(Maxk,1);IbE=IaE;vE=IaE;sE=IaE;
    
24. 
    
25. %计算矩阵
    
26. F=zeros(4,4);
    
27. f=zeros(4,1);
    
28. a1=-Rs/L;a2=Ke/L;a3=pi/tao
    
29. F(1,1)=a1;F(1,2)=0;
    
30. F(2,1)=0;F(2,2)=a1;
    
31. F(3,1)=0;F(3,2)=0;F(3,3)=0;F(3,4)=0;
    
32. F(4,1)=0;F(4,2)=0;F(4,3)=1;F(4,4)=0;
    
33. %开始循环计算
    
34. for k=1:Maxk
    
35. U(1)=Ua(k);U(2)=Ub(k);Y(1)=Ia(k);Y(2)=Ib(k);
    
36. f(1)=a1*XX(1)+a2*XX(3)*sin(a3*XX(4));
    
37. f(2)=a1*XX(2)+a2*XX(3)*cos(a3*XX(4));
    
38. f(3)=0;
    
39. f(4)=XX(3);
    
40. F(1,3)=a2*sin(a3*XX(4));
    
41. F(1,4)=a2*a3*cos(a3*XX(4));
    
42. F(2,3)=-a2*cos(a3*XX(4));
    
43. F(2,4)=a2*a3*sin(a3*XX(4));
    
44. %预测
    
45. X=XX+f*ts+B*U*ts;
    
46. P=PP+F*PP*ts+PP*F'*ts+Q;
    
47. %更新
    
48. K=P*H'*inv(H*P*H'+R);
    
49. XX=X+K*Y-K*H*X;
    
50. PP=P-K*H*P;
    
51. %保存变量
    
52. IaE(k)=XX(1);
    
53. IbE(k)=XX(2);
    
54. vE(k)=XX(3);
    
55. sE(k)=XX(4);
    
56. end

复制代码

[ 本帖最后由 PMLSM 于 2008-5-28 18:24 编辑 ]

大侠们，帮帮忙啊，关系到毕业的问题啊

什么东东呀？你要实现什么功能嘛？把你的数学模型发上来看看，希望我能帮到你