%************交互多模算法*****************
%IMM算法??clear;clc
%function?IMM（N）
N=10;
T=2;
sigma=100;?%驅動噪聲標準差
R=[1?0;0?1]*sigma^2;
Fai1=[1?T?0?0
????0?1?0?0
????0?0?1?T
????0?0?0?1];
H1=[1?0?0?0
????0?0?1?0];
G1?=?[T/2?0;?1?0;?0?T/2;?0?1];

Fai2=[1?T?0?0?T^2/2?0
????0?1?0?0?T?????0
????0?0?1?T?0?T^2/2
????0?0?0?1?0?????T
????0?0?0?0?1?????0
????0?0?0?0?0?????1];
H2=[1?0?0?0?0?0;0?0?1?0?0?0];
G2=[T^2/4?0;T^2/2?0
????0?T^2/4;0?T^2/2
????1??0;?0?1];

q1=0;q2=0.001;q3=0.0144;?????%P208?劉福聲，劉鵬飛??科大出社??
Q1=[q1?0;0?q1];
Q2=[q2?0;0?q2];
Q3=[q3?0;0?q3];
P=[0.95?0.025?0.025
???0.025?0.95?0.025
???0.025?0.025?0.95];?%轉移概率
U=[1?0?0]‘;???%??????

%模擬目標真實運動軌跡
x0=2000;y0=10000;v0=-15;
a1x=0.075;a1y=0.075;a2x=-0.3;a2y=-0.3;
t1=400;K1=t1/T;
t2=600;K2=（t2-t1）/T;
t3=610;K3=（t3-t2）/T;
t4=660;K4=（t4-t3）/T;
t5=1500;K5=（t5-t4）/T;
%0---400s
k1=1:K1;
x1=x0*ones（1length（k1））;
y1=y0+v0*k1*T;
%400---600
k2=1:K2;
x2=x1（K1）+a1x*（k2*T）.^2/2;
y2=y1（K1）+v0*k2*T+a1y*（k2*T）.^2/2;
%600----610
k3=1:K3;
x3=x2（K2）+a1x*（t2-t1）*（k3*T）;
y3=y2（K2）*ones（1length（k3））;
%610---660
k4=1:K4;
x4=x3（K3）+a1x*（t2-t1）*（k4*T）+a1x*（k4*T）.^2/2;
y4=y3（K3）+a2y*（k4*T）.^2/2;
%660---1500
k5=1:K5;
x5=x4（K4）*ones（1length（k5））;
y5=y4（K4）+a2y*（t4-t3）*（k5*T）;

x=[x1?x2?x3?x4?x5];
y=[y1?y2?y3?y4?y5];
K=K1+K2+K3+K4+K5;
%K=t5/T;

%輸出初始化
xx=zeros（1K）;yy=zeros（1K）;
ex1=zeros（1K）;ey1=zeros（1K）;
ex2=zeros（1K）;ey2=zeros（1K）;

%進行N次仿真
for?i=1:N
????????%產生觀測數據，加噪
????????zx=randn（1K）*sigma+x;
????????zy=randn（1K）*sigma+y;
????????Z=[zx;zy];
????????%兩點起始法確定初值?
????????LP=[1???1/T???????0?0
????????????1/T?2/（T*T）?0?0
????????????0???0????1????1/T
????????????0???0?1/T?2/（T*T）]*sigma^2;??%初始協方差????
????LX=[zx（2）?（zx（2）-zx（1））/T?zy（2）?（zy（2）-zy（1））/T]‘;?%狀態估計值初始化
????xx（1）=xx（1）+zx（1）;yy（1）=yy（1）+zy（1）;
????ex1（1）=ex1（1）+x（1）-zx（1）;??????%X方向誤差均值的初始化
????ex2（1）=ex2（1）+（x（1）-zx（1））^2;???%X方向濾波誤差標準差的初始化
????ey1（1）=ey1（1）+y（1）-zy（1）;
????ey2（1）=ey2（1）+（y（1）-zy（1））^2;
????%從M+1次采樣開始采用具有3個模型IMM算法
????%濾波起始值
????LX1=[LX‘00]‘;???%位數擴充到6維???因為Fai是6*6矩陣??kfilter_IMM算法中，有Fai*X01
????LX2=[LX‘00]‘;
????LX3=[LX‘00]‘;
????%濾波協方差起始值
????LP1=[LPzeros（42）;zeros（26）];??%為什么要擴大維列數???因為Fai是6*6矩陣??kfilter_IMM算法中，有Fai*X01
????LP2=[LPzeros（42）;zeros（26）];
????LP3=[LPzeros（42）;zeros（26）];
????
????for?k=2:K???????????????????????
????????????%形成混合初始條件????
????????????%Ps:具體算法參看P209劉福聲科大版???和?何友?P161?電子工業出版社?
????????????C_=P‘*U;
????????????U=P.*（U*ones（13））./（ones（31）*C_‘）;
????????????X01=LX1*U（11）+LX2*U（21）+LX3*U（31）;
????????????X02=LX1*U（12）+LX2*U（22）+LX3*U（32）;
????????????X03=LX1*U（13）+LX2*U（23）+LX3*U（33）;%它們為LX（i）交互作用的結果，作為濾波器的輸入
????????????P01=U（11）*（LP1+（LX1-X01）*（LX1-X01）‘）+U（21）*（LP2+（LX2-X01）*（LX2-X01）‘）+U（31）*（LP3+（LX3-X01）*（LX3-X01）‘）;
????????????P02=U（12）*（LP1+（LX1-X02）*（LX1-X02）‘）+U（22）*（LP2+（LX2-X02）*（LX2-X02）‘）+U（32）*（LP3+（LX3-X02）*（LX3-X02）‘）;
????????????P03=U（13）*（LP1+（LX1-X03）

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2013-12-22?19:37??IMM\
?????文件????????4888??2013-11-10?10:55??IMM\IMM.asv
?????文件????????4881??2013-11-30?22:56??IMM\IMM.m
?????文件???????51712??2013-11-10?12:27??IMM\IMM數據分析.doc
?????文件?????????255??2013-11-10?09:41??IMM\kfilter_IMM.m