1 clear all;
 2 close all;
 3 clc;
 4 n=40;
 5 
 6 point=load('point.mat');
 7 current_point=point.point;
 8 
 9 plot(current_point(:,1),current_point(:,2),'r');
10 
11 %系统方程：x(k+1)=fi*x(k)+gm*w(k)
12 %观测方程：z(k)=h*x(k)+v(k)
13 
14 x=current_point(:,1);
15 y=current_point(:,2);
16 
17 fi=1;
18 h=1;
19 gm=5;
20 
21 w=randn(1,n);
22 v=randn(1,n);
23 
24 xy_x(1)=x(1);
25 xy_y(1)=y(1);
26 
27 p(1)=0;
28 z_x(1)=x(1)+w(1);
29 z_y(1)=y(1)+w(1);
30 R=(std(v)).^2;
31 Q=(std(w)).^2;
32 k(1)=fi*p(1)*h'*inv(h*p(1)*h'+R);
33 pp(1)=fi*p(1)*fi'+gm*Q*gm';
34 
35 for i=2:n
36     xy_x(i)=fi*xy_x(i-1)+k(i-1)*(z_x(i-1)-xy_x(i-1));
37     xy_y(i)=fi*xy_y(i-1)+k(i-1)*(z_y(i-1)-xy_y(i-1));
38     
39     k(i)=pp(i-1)*h'*inv((h*pp(i-1)*h'+R));
40     pp(i)=fi*p(i-1)*fi'+gm*Q*gm';
41     p(i)=pp(i-1)-k(i)*h*p(i-1);
42 
43     z_x(i)=x(i)+w(i);
44     z_y(i)=y(i)+w(i);    
45 end
46 
47 hold on;
48 plot(xy_x,xy_y);
49 grid on;

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

图片福利：