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;