%======卡爾曼算法，狀態(tài)參數(shù)有兩個，速度與位置，估計其值======
%=======對勻速運動物體的跟蹤：速度和位移================

%%
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];%狀態(tài)轉(zhuǎn)移矩陣2*2
H=[1?0];%觀測矩陣1*2
R=1;
%?Q=1e-3;%經(jīng)驗取值
Xk（:1）=[3;1];
P0=eye（2）;

%觀測值與狀態(tài)值生成
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‘;?????