交互多模IMM 卡尔曼滤波

基于交互多模（IMM）卡尔曼滤波算法，很好的实现机动跟踪效果！

%************交互多模算法*****************
%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