clear?all;
clc;
echo?off;
%===============================
%建立模型
%===============================
%?仿真參數(shù)
simTime=100;??????%仿真迭代次數(shù)
T=1;?????????????????????%采樣時間
w2=3*2*pi/360;?????%模型2轉彎率3度
w3=-3*2*pi/360;????%模型3轉彎率-3度
H=[1000;0010];??????????????????????%模型量測矩陣
G=[T^2/20;T0;0T^2/2;0T];??????????????%模型過程噪聲加權矩陣
r=200;?????????????????????????????????%20?2000
R=[r0;0r];????????????????????????????%模型量測噪聲協(xié)方差矩陣
Q=[100;010];??????????????????????????????????%模型過程噪聲協(xié)方差矩陣

F1=[1T00;0100;001T;0001];?????%模型1狀態(tài)轉移矩陣

F2=[1sin（w2*T）/w20（cos（w2*T）-1）/w2;
????0cos（w2*T）0sin（w2*T）;
????0（1-cos（w2*T））/w21sin（w2*T）/w2;
????0-sin（w2*T）0cos（w2*T）];????????????%模型2狀態(tài)轉移矩陣?左轉彎

F3=[1sin（w3*T）/w30（cos（w3*T）-1）/w3;
????0cos（w3*T）0sin（w3*T）;
????0（1-cos（w3*T））/w31sin（w3*T）/w3;
????0-sin（w3*T）0cos（w3*T）];????????????%模型3狀態(tài)轉移矩陣?右轉彎

w4=3*2*pi/360;?????%模型2轉彎率3度
w5=-3*2*pi/360;????%模型3轉彎率-3度

F4=[1sin（w4*T）/w40（cos（w4*T）-1）/w4;
????0cos（w4*T）0sin（w4*T）;
????0（1-cos（w4*T））/w41sin（w4*T）/w4;
????0-sin（w4*T）0cos（w4*T）];????????????%模型2狀態(tài)轉移矩陣

F5=[1sin（w5*T）/w50（cos（w5*T）-1）/w5;
????0cos（w5*T）0sin（w5*T）;
????0（1-cos（w5*T））/w51sin（w5*T）/w5;
????0-sin（w5*T）0cos（w5*T）];????????????%模型3狀態(tài)轉移矩陣

x0=[10002001000200]‘;??%?初始狀態(tài)

%?產(chǎn)生量測數(shù)據(jù)
%[zz_true]=targets（）;
randn（‘state‘sum（100*clock））;?%?Shuffle?the?pack!
x?=?zeros（4simTime）;
z?=?zeros（2simTime）;?????????%含噪聲量測數(shù)據(jù)
z_true?=?zeros（2simTime）;????%真值數(shù)據(jù)
measureNoise?=?zeros（2simTime）;
measureNoise?=?sqrt（R）*randn（2simTime）;??%產(chǎn)生量測噪聲
x（:1）=x0;
z（:1）=H*x（:1）+measureNoise（:1）;
z_true（:1）=H*x（:1）;
for?a=2:simTime
????if?（a>=20）&&（a<=40）?
????????x（:a）=F4*x（:a-1）;??????%20--40s左轉?
????elseif?（a>=60）&&（a<=80）?
???????x（:a）=F5*x（:a-1）;????????%60--80s右轉?
????else
????????x（:a）=F1*x（:a-1）;??????%勻速直線運動
????end;?
z（:a）=H*x（:a）+measureNoise（:a）;
z_true（:a）=H*x（:a）;
end;
%===============================
%?????IMM
%===============================

%初始化
x1_IMM?=?zeros（41）;??????%模型1IMM算法狀態(tài)估計值
x2_IMM?=?zeros（41）;??????%模型2IMM算法狀態(tài)估計值
x3_IMM?=?zeros（41）;??????%模型3IMM算法狀態(tài)估計值
x_pro_IMM?=?zeros（4simTime）;???%IMM算法模型綜合狀態(tài)估計值
P_IMM=zeros（44simTime）;???????%IMM算法模型綜合狀態(tài)協(xié)方差矩陣
P1_IMM=zeros（44）;
P2_IMM=zeros（44）;
P3_IMM=zeros（44）;??????????????%IMM算法各模型協(xié)方差矩陣
r1_IMM=zeros（21）;
r2_IMM=zeros（21）;
r3_IMM=zeros（21）;
S1_IMM=zeros（22）;
S2_IMM=zeros（22）;
S3_IMM=zeros（22）;


%初始化
x_pro_IMM（:1）=x0;

pij=[0.90.050.05;
????0.10.80.1;
????0.050.150.8];????%模型轉移概率矩陣
%pij=[0.60.20.2;0.20.60.2;0.250.150.6];????%模型轉移概率矩陣
u_IMM=zeros（3simTime）;
u_IMM（:1）=[0.30.30.4]‘;??%IMM算法模型概率

x1_IMM=x0;x2_IMM=x0;x3_IMM=x0;??%IMM算法各模型初始狀態(tài)

P0=diag（[10005001000500]）;??%初始狀態(tài)協(xié)方差矩陣

P1_IMM=P0;P2_IMM=P0;P3_IMM=P0;

P_IMM（::1）=P0;

%main?loop

for?t=1:simTime-1
????
????%第一步Interacting（只針對IMM算法）
????c_j=pij‘*u_IMM（:t）;
????
????ui1=（1/c_j（1））*pij（:1）.*u_IMM（:t）;
????ui2=（1/c_j（2））*pij（:2）.*u_IMM（:t）;
????ui3=（1/c_j（3））*pij（:3）.*u_IMM（:t）;????%計算模型混合概率
????

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????5751??2012-04-30?15:03??IMM代碼\IMMKF_Matlab.m
?????文件?????????985??2012-04-30?14:49??IMM代碼\Kalman.m
?????文件?????????796??2012-04-30?14:48??IMM代碼\Model_P_up.m
?????文件?????????472??2012-04-30?14:49??IMM代碼\Model_mix.m
?????目錄???????????0??2018-08-29?16:01??IMM代碼\