卡尔曼滤波轨迹跟踪

%======卡尔曼算法，状态参数有两个，速度与位置，估计其值======
%=======对匀速运动物体的跟踪：速度和位移================

%%
clc ;
clear;
n=100;                  %100个点
Yk          =zeros（2n）;
Xk_real     =zeros（2n）;
Xk_real（:1） =[5015]‘;%position and speed
Z（:1）=50;%观测值初始化
F=[11;01];%状态转移矩阵2*2
H=[1 0];%观测矩阵1*2
R=1;
% Q=1e-3;%经验取值
Xk（:1）=[3;1];
P0=eye（2）;

%观测值与状态值生成
for i=2:n
    Xk_real（:i）=F*Xk_real（:i-1）;%+sqrt（Q）*randn（21）;%2*2与2*1=2*1
    Yk（i）=H*Xk_real（:i） +sqrt（R）*randn（1）;
end

%卡尔曼迭代
for i=2:n
    Xn     =F*Xk（:i-1）;         %2*1
    Pk     =F*P0*F‘;