%功能說(shuō)明:Kalman濾波用于自由落體運(yùn)動(dòng)目標(biāo)跟蹤問(wèn)題
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function?main
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=1000;%仿真時(shí)間，時(shí)間序列總數(shù)

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

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

%初始化
X=zeros（2N）;?%物體真實(shí)狀態(tài)
X（:1）=[95;1];?%初始位移和速度
P0=[100;01];?%初始誤差
Z=zeros（1N）;
Z（1）=H*X（:1）;?%初始化觀測(cè)值
Xkf=zeros（2N）;?%Kalman估計(jì)狀態(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)方程的驅(qū)動(dòng)
????X（:k）=A*X（:k-1）+B*U+W（k）;
????
????%位移傳感器對(duì)目標(biāo)進(jìn)行觀測(cè)
????Z（k）=H*X（:k）+V（k）;
????
????%Kalman濾波
????X_pre=A*Xkf（:k

?屬性????????????大小?????日期????時(shí)間???名稱(chēng)
-----------?---------??----------?-----??----

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

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

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