clear?all
close?all
clc
%%%加載數據
load?Measure.mat%加載量測數據
load?Real.mat%加載真實值數據
Z=TimeRPhi（2:3:）;%量測值
Xr=TXYdXdYAxAyTPhi;%真值
%%%設置初值
T=0.1;%采樣時間
m=10;%斜距標準差
n=0.1/57.3;%方位角標準差
X=[10000;20;0;0;0;0];%狀態變量初值
P=diag（[101010101010]）;%誤差協方差初值
R=diag（[m^2n^2]）;%量測噪聲方差

F=[1?0?T?0?T^2/2?0;%狀態轉移矩陣
???0?1?0?T?0?T^2/2;
???0?0?1?0?T?0;
???0?0?0?1?0?T;
???0?0?0?0?1?0;
???0?0?0?0?0?1];
Ta=[T^2/2?0?T?0?1?0;
????0?T^2/2?0?T?0?1];
Ta=Ta‘;%噪聲陣
%?sigam=60;
sigam=20;
Q=Ta*Ta‘*sigam^2;%系統噪聲方差

for?k=1:501
????Xk=F*X;
????Pk=F*P*F‘+Q;
????Zk=[（（Xk（1））^2+（Xk（2））^2）^0.5;atan（（Xk（2））/（Xk（1）））];
????H11=Xk（1）/（（Xk（1））^2+（Xk（2））^2）^0.5;
????H12=Xk（2）/（（Xk（1））^2+（Xk（2））^2）^0.5;
????H21=-Xk（2）/（（Xk（1））^2+（Xk（2））^2）;
????H22=Xk（1）/（（Xk（1））^2+（Xk（2））^2）;
????H=[H11?H12?0?0?0?0;
???????H21?H22?0?0?0?0];
???
????K=Pk*H‘*inv（H*Pk*H‘+R）;
????X=Xk+K*（Z（:k）-Zk）;
????P=Pk-K*H*Pk;
????Xe（:k）=X;
????Re=（（X（1））^2+（X（2））^2）^0.5;
????Te=atan（X（2）/X（1））;
????Ze（:k）=[Re;Te];
????
end

figure（1）
plot（Xe（1:）Xe（2:）Xr（2:）Xr（3:）‘r‘）
figure（2）
plot（Xr（7:）-Xe（6:）‘r‘）

%?figure（2）
%?plot（Xr（9:）-Ze（2:）‘r‘）
%?hold?on
%?plot（Xr（9:）-Z（2:））

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

?????文件???????1265??2009-09-12?16:01??IMM0902_20090916\CAEKF.m

?????文件????????175??2009-09-05?08:53??IMM0902_20090916\FX1.m

?????文件????????176??2009-09-05?08:53??IMM0902_20090916\FX2.m

?????文件????????142??2009-09-05?08:53??IMM0902_20090916\FZ.m

?????文件????????555??2009-11-20?12:51??IMM0902_20090916\IMM0902程序說明.txt

?????文件???????5626??2009-09-16?10:52??IMM0902_20090916\IMMUKF.m

?????文件??????12053??2009-09-05?08:53??IMM0902_20090916\Measure.mat

?????文件??????36108??2009-09-05?08:53??IMM0902_20090916\Real.mat

?????文件?????286277??2009-11-20?12:47??IMM0902_20090916\機動目標跟蹤.pdf

?????目錄??????????0??2009-11-20?12:51??IMM0902_20090916

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

???????????????342377????????????????????10