matlab练习程序（卡尔曼滤波）

clear all;

close all;

clc;

n=40;



point=load('point.mat');

current_point=point.point;



plot(current_point(:,1),current_point(:,2),'r');



%系统方程：x(k+1)=fi*x(k)+gm*w(k)

%观测方程：z(k)=h*x(k)+v(k)



x=current_point(:,1);

y=current_point(:,2);



fi=1;

h=1;

gm=5;



w=randn(1,n);

v=randn(1,n);



xy_x(1)=x(1);

xy_y(1)=y(1);



p(1)=0;

z_x(1)=x(1)+w(1);

z_y(1)=y(1)+w(1);

R=(std(v)).^2;

Q=(std(w)).^2;

k(1)=fi*p(1)*h'*inv(h*p(1)*h'+R);

pp(1)=fi*p(1)*fi'+gm*Q*gm';



for i=2:n

    xy_x(i)=fi*xy_x(i-1)+k(i-1)*(z_x(i-1)-xy_x(i-1));

    xy_y(i)=fi*xy_y(i-1)+k(i-1)*(z_y(i-1)-xy_y(i-1));

    

    k(i)=pp(i-1)*h'*inv((h*pp(i-1)*h'+R));

    pp(i)=fi*p(i-1)*fi'+gm*Q*gm';

    p(i)=pp(i-1)-k(i)*h*p(i-1);



    z_x(i)=x(i)+w(i);

    z_y(i)=y(i)+w(i);    

end



hold on;

plot(xy_x,xy_y);

grid on;

point.mat是从视频中提取的目标坐标值，一个四十个坐标。

图片福利：

参考：
http://www.ilovematlab.cn/forum.php?mod=viewthread&tid=1453&reltid=159346&pre_thread_id=0&pre_pos=2&ext=CB