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;

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

图片福利：

参考：

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