卡尔曼滤波(kalman filter)最简单易懂的解释,对初学者有很大的帮助
13. Rvv=q1.^2;
14. q2=std(x);
15. Rxx=q2.^2;
16. q3=std(w);
17. Rww=q3.^2;
18. c=0.2; %c为方程中H(k)
19. Y=c*x+V; %Y为观察值 20.
21. p(1)=0;
22. s(1)=0;
23. for t=2:N;
24. p1(t)=a.^2*p(t-1)+Rww; %p1为方程中p'
25. b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);
26. s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));
27. p(t)=p1(t)-c*b(t)*p1(t);
28. end
29.
30. t=1:N;
31. plot(t,s,'r',t,Y,'g',t,x,'b');