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

%噪聲
Q=[00;00];%過程方差為0，即下落過程忽略空氣阻力
R=1;%觀測噪聲方差
W=sqrt（Q）*randn（2N）;%既然Q為0，則W=0;
V=sqrt（R）*randn（1N）;%測量噪聲V（k）

%系統(tǒng)矩陣
A=[11;01];%狀態(tài)轉移矩陣
B=[0.5;1];%控制量
U=-1;
H=[10];%觀測矩陣

%初始化
X=zeros（2N）;?%物體真實狀態(tài)
X（:1）=[95;1];?%初始位移和速度
P0=[100;01];?%初始誤差
Z=zeros（1N）;
Z（1）=H*X（:1）;?%初始化觀測值
Xkf=zeros（2N）;?%Kalman估計狀態(tài)初始化
Xkf（:1）=X（:1）;
err_P=zeros（N2）;
err_P（11）=P0（11）;
err_P（12）=P0（22）;
I=eye（2）;?%二維系統(tǒng)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for?k=2:N
????%物體下落，受狀態(tài)方程的驅動
????X（:k）=A*X（:k-1）+B*U+W（k）;
????
????%位移傳感器對目標進行觀測
????Z（k）=H*X（:k）+V（k）;
????
????%Kalman濾波
????X_pre=A*Xkf（:k

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????2227??2016-09-07?11:24??main.m

-----------?---------??----------?-----??----

?????????????????2227????????????????????1