function?[?X_XS_XH_P?]?=?kal（?A0A1A2?）
%KAL?此處顯示有關此函數的摘要
%???此處顯示詳細說明

%%?初始化
ORDER?=3;
PHIS=0.;
TS=.1;
%?A0=400000;
%?A1=-6000.;
%?A2=-16.1;
XH=0;
XDH=0;
XDDH=0;
SIGNOISE=100.;
%?P
P=99999999999999.*eye（ORDERORDER）;
%?PHI
PHI=eye（ORDERORDER）;
PHI（12）=TS;
PHI（13）=.5*TS*TS;
PHI（23）=TS;
%?H
H=eye（1ORDER）;
%?R
R=SIGNOISE^2;
%?Q
Q=zeros（ORDERORDER）;
Q（11）=PHIS*TS^5/20;
Q（12）=PHIS*TS^4/8;
Q（13）=PHIS*TS^3/6;
Q（21）=Q（12）;
Q（22）=PHIS*TS^3/3;
Q（23）=PHIS*TS^2/2;
Q（31）=Q（13）;
Q（32）=Q（23）;
Q（33）=PHIS*TS;
%?IDN
IDN=eye（ORDER）;


TT=[];
X_=[];XD_=[];XDD_=[];
XS_=[];
XH_=[];XDH_=[];XDDH_=[];
SP11_=[];SP22_=[];SP33_=[];
XHERR_=[];XDHERR_=[];XDDHERR_=[];

%%?KF濾波循環
for?T=0:TS:30
????%?Riccati方程計算:濾波器增益K
????M=PHI*P*PHI‘+Q;
????K=M*H‘*inv（H*M*H‘+R）;
????P=（IDN-K*H）*M;
????%?信號真實值
????X=A0+A1*T+A2*T*T;
????XD=A1+2*A2*T;
????XDD=2*A2;
????%信號測量值
????XS=X+normrnd（0SIGNOISE）;
????%?KF估計
????????%?殘差RES_k
????RES=XS-XH-TS*XDH-.5*TS*TS*XDDH;
????????%?KF方程
????XH=XH+XDH*TS+.5*TS*TS*XDDH+K（11）*RES;
????XDH=XDH+XDDH*TS+K（21）*RES;
????XDDH=XDDH+K（31）*RES;
????%?估計結果分析
????SP11=sqrt（P（11））;
????SP22=sqrt（P（22））;
????SP33=sqrt（P（33））;
????XHERR=X-XH;
????XDHERR=XD-XDH;
????XDDHERR=XDD-XDDH;
????%?存儲濾波計算結果
????TT=[TT;T];
????X_=[X_;X];XD_=[XD_;XD];XDD_=[XDD_;XDD];
????XS_=[XS_;XS];
????XH_=[XH_;XH];XDH_=[XDH_;XDH];XDDH_=[XDDH_;XDDH];
????SP11_=[SP11_;SP11];SP22_=[SP22_;SP22];SP33_=[SP33_;SP33];
????XHERR_=[XHERR_;XHERR];XDHERR_=[XDHERR_;XDHERR];XDDHERR_=[XDDHERR_;XDDHERR];
end

end

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2017-10-19?15:58??tracking_fusion\
?????文件????????1658??2017-05-20?15:20??tracking_fusion\kalman.m
?????文件?????????369??2017-05-20?15:17??tracking_fusion\measuretrack.m
?????文件?????????330??2017-05-20?15:17??tracking_fusion\realtrack.m
?????文件?????????357??2017-05-18?14:14??tracking_fusion\single_track.m
?????文件???????46361??2017-05-18?13:39??tracking_fusion\track.fig
?????文件????????6168??2017-05-18?14:31??tracking_fusion\track.m
?????文件?????????390??2017-05-20?15:17??tracking_fusion\trackfusion.m
?????文件?????????672??2017-05-12?18:03??tracking_fusion\trackfusion2.m
?????文件?????????799??2017-05-20?23:59??tracking_fusion\trackfusion3.m