function?[?MMX0?]?=?IMM_f（sigmarPtu0TMX0?）
%%
%UNtitleD3?Summary?of?this?function?goes?here
%???該函數的目的是利用交互多模型算法對機動目標進行跟蹤
%%%參數意義如下：
%%%??MMX0，輸出目標狀態
%%%??sigmar，目標位置誤差標準差
%%%??Pt，markov模型轉移概率，3*3矩陣
%%%??u0，模型先驗概率
%%%??T，雷達采樣時間間隔
%%%??MX0，目標位置量測值
%%
NN=length（MX0（1:））;
%%%%%%參數初始化%%%%%%%
MMX0=zeros（6NN）;
%%%%利用前三次量測值初始化%%%
MMX0（11）=MX0（11）;
MMX0（12）=MX0（12）;
MMX0（13）=MX0（13）;
MMX0（22）=（MX0（12）-MX0（11））/T;
MMX0（23）=（MX0（13）-MX0（12））/T;
MMX0（33）=（MX0（13）-2*MX0（12）+MX0（11））/T;
MMX0（41）=MX0（21）;
MMX0（42）=MX0（22）;
MMX0（43）=MX0（23）;
MMX0（52）=（MX0（22）-MX0（21））/T;
MMX0（53）=（MX0（23）-MX0（22））/T;
MMX0（63）=（MX0（23）-2*MX0（22）+MX0（21））/T;
MMXj=[MMX0（:3）MMX0（:3）MMX0（:3）];%%%三個模型的初始狀態
%%%%%%初始化各模型新息協方差%%%%%%%%
R=sigmar^2*eye（2）;
P11=R（11）;
P12=R（11）/T;
P13=R（11）/T^2;
P22=2*R（11）/T^2;
P23=3*R（11）/T^3;
P33=6*R（11）/T^4;
P44=R（22）;
P45=R（22）/T;
P46=R（22）/T^2;
P55=2*R（22）/T^2;
P56=3*R（22）/T^3;
P66=6*R（22）/T^4;
Pj1=[P11?P12?P13?0???0???0;
????P12?P22?P23?0???0???0;
????P13?P23?P33?0???0???0;
????0???0???0???P44?P45?P46;
????0???0???0???P45?P55?P56;
????0???0???0???P46?P56?P66];
Pj2=Pj1;
Pj3=Pj1;
%%%%%初始化過程噪聲協方差%%%%%%
QQQ=[T^5/20?T^4/8?T^3/6;
????T^4/8?T^3/3?T^2/2;
????T^3/6?T^2/2?T];
QQ=[QQQ?zeros（3）;zeros（3）?QQQ];
Q1=10*QQ;%%%模型1，q=10
Q2=QQ;%%%模型2，q=1
Q3=0.1*QQ;%%%模型3，q=0.1
F1=[1?T?0.5*T^2;0?1?T;0?0?1];
F=[F1zeros（3）;zeros（3）F1];%%%狀態矩陣
H=[1?0?0?0?0?0;0?0?0?1?0?0?];%%%測量矩陣
X1=MMXj（:1）;
X2=MMXj（:2）;
X3=MMXj（:3）;%%%迭代初始值
%%
%%%%%%%迭代%%%%%%%
for?k=4:NN
????%%%%%%%交互作用%%%%%%
????Cj=u0*Pt;
????for?i=1:3
????????for?j=1:3
????????????ukk（ij）=Pt（ij）*u0（i）/Cj（j）;
????????end
????end
????MMXoj=MMXj*ukk;%%%%交互狀態輸出
????Poj11=（Pj1+（MMXj（:1）-MMXoj（:1））*（MMXj（:1）-MMXoj（:1））‘）*ukk（11）;
????Poj12=（Pj2+（MMXj（:2）-MMXoj（:1））*（MMXj（:2）-MMXoj（:1））‘）*ukk（21）;
????Poj13=（Pj3+（MMXj（:3）-MMXoj（:1））*（MMXj（:3）-MMXoj（:1））‘）*ukk（31）;
????Poj1=Poj11+Poj12+Poj13;
????Poj21=（Pj1+（MMXj（:1）-MMXoj（:2））*（MMXj（:1）-MMXoj（:2））‘）*ukk（12）;
????Poj22=（Pj2+（MMXj（:2）-MMXoj（:2））*（MMXj（:2）-MMXoj（:2））‘）*ukk（22）;
????Poj23=（Pj3+（MMXj（:3）-MMXoj（:2））*（MMXj（:3）-MMXoj（:2））‘）*ukk（32）;
????Poj2=Poj21+Poj22+Poj23;???
????Poj31=（Pj1+（MMXj（:1）-MMXoj（:3））*（MMXj（:1）-MMXoj（:3））‘）*ukk（13）;
????Poj32=（Pj2+（MMXj（:2）-MMXoj（:3））*（MMXj（:2）-MMXoj（:3））‘）*ukk（23）;
????Poj33=（Pj3+（MMXj（:3）-MMXoj（:3））*（MMXj（:3）-MMXoj（:3））‘）*ukk（33）;
????Poj3=Poj31+Poj32+Poj33;%%%%交互后狀態方差輸出
????%%%%%%%%模型修正，卡爾曼濾波%%%%%%%%
????XX1=F*X1;
????ZZ1=H*XX1;
????PP1=F*Poj1*F‘+Q1;
????SS1=H*PP1*H‘+R;
????VV1=MX0（:k）-ZZ1;
????WW1=PP1*H‘*inv（SS1）;
????X1=XX1+WW1*VV1;
????Pj1=PP1-WW1*SS1*WW1‘;
????MMXj（:1）=X1;
????XX2=F*X2;
????ZZ2=H*XX2;
????PP2=F*Poj2*F‘+Q2;
????SS2=H*PP2*H‘+R;
????VV2=MX0（:k）-ZZ2;
????WW2=PP2*H‘*inv（SS2）;
????X2=XX2+WW2*VV2;
????Pj2=PP2-WW2*SS2*WW2‘;
????MMXj（:2）=X2;
????XX3=F*X3;
????ZZ3=H*XX3;
????PP3=F*Poj3*F‘+Q3;
????SS3=H*PP3*

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????3770??2015-03-12?20:01??IMM_f.m