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是从视频中提取的目标坐标值，一个四十个坐标。
图片福利：
