IMM交互多模型算法

采用交互多模型算法（IMM）对机动目标进行跟踪的实现

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