%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%功能說明：Kalman濾波用于自由落體運動目標的跟蹤問題
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function?main?
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=1000;?????%仿真時間，時間序列總數

%噪聲
Q=[00;00];??????%過程噪聲方差為0，即下落過程忽略空氣阻力
R=1;??????????????%觀測噪聲方差
W=sqrt（Q）*randn（2N）;????????%sqrt（）為求平方根函數；Q為0，則W=0，在此寫出，方便對照理解
V=sqrt（R）*randn（1N）;????????%測量噪聲V（k）

%系統矩陣
A=[11;01];??????????%狀態轉移矩陣
B=[0.5;1];????????????%控制量
U=-1;
H=[10];??????????????%觀測矩陣

%初始化
X=zeros（2N）;????????%物體真實狀態
X（:1）=[95;1];???????%初始位移和速度
P0=[100;01];???????%初始誤差
Z=zeros（1N）;???????
Z（1）=H*X（:1）;???????%初始觀測值
Xkf=zeros（2N）;??????%Kalman估計狀態初始化
Xkf（:1）=X（:1）;
err_P=zeros（N2）;
err_P（11）=P0（11）;
err_P（12）=P0（22）;
I=eye（2）;???????????%二維狀態
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for?k=2:N
????%物體下落，受狀態方程驅動
????X（:k）=A*X（:k-1）+B*U+W（k）;
????
????%位移傳感器對目標進行觀測
????Z（k）=H*X（:k）+V（k）;
????
????%Kalman濾波
???X_pre=A*Xkf（:k-1）+B*U;???%狀態預測
???P_pre=A*P0*A‘+Q;??????????%協方差預測
???Kg=P_pre*H‘*inv（H*P_pre*H‘+R）;??%計算Kalman增益