IMM滤波算法

IMM滤波算法 Matlab可运行 目标跟踪

function [XXEp]=immkf（uyTtotalTime）
%五个模型的概率转移矩阵
s=0.001/4;
k=0.001/2;
P=[0.999      s        s           s           s        
    k      0.433       0.333       0.233       k      
    k      0.233       0.433       0.333       k       
    k      0.233       0.333       0.433       k     
    s      s              s       s       0.999 ];
%初始模型概率
U=[1 0 0 0 0];
modelNum=length（U）;

%参数
Mc=43100/2;
Mb=2280;
Mw=1650*2;
Cs=1.2e5;
Ks=0.4e6;
Cp=1.5e4*2;
Kp=1.2e6*2;
n=totalTime/T+1;
t=0:T:totalTime;

%Mode1 No malfunction in vehicle
G1=[（Mc-Cs*T）/Mc  -Ks*T/Mc  Cs*T/Mc  Ks*T/Mc  0   0
    0    1     0     0     0    0
    Cs*T/Mb   Ks*T/Mb   （Mb-Cs*T-Cp*T）/Mb   （-Ks*T-Kp*T）/Mb   Cp*T/Mb  Kp*T/Mb
    0    0     0     1     0    0
    0    0    Cp*T/Mw   Kp*T/Mw   （Mw-Cp*T）/Mw   -Kp*T/Mw
    0    0     0     0     0    1];
H1=[0 0 0 0 1 0]‘;
C1=[-Cs/Mc  -Ks/Mc  Cs/Mc  Ks/Mc  0   0];
Q1=1e-8*eye（66）;
R1=1e-2;
XE1=zeros（61）;
PE1=Q1;

%Mode2 A secondary vetical spring failure（20% reduction in spring rate）
Ks2=（1-0.2）*Ks;
G2=[（Mc-Cs*T）/Mc  -Ks2*T/Mc  Cs*T/Mc  Ks2*T/Mc  0   0
    0    1     0     0     0    0
    Cs*T/Mb   Ks2*T/Mb   （Mb-Cs*T-Cp*T）/Mb   （-Ks2*T-Kp*T）/Mb   Cp*T/Mb  Kp*T/Mb
    0    0     0     1     0    0
    0    0    Cp*T/Mw   Kp*T/Mw   （Mw-Cp*T）/Mw   -Kp*T/Mw
    0    0     0     0     0    1];
H2=H1;
C2=[-Cs/Mc  -Ks2/Mc  Cs/Mc  Ks2/Mc  0   0];
Q2=Q1;
R2=4*R1;
XE2=XE1;
PE2=Q2;

%Mode3 A secondary vetical spring failure（40% reduction in spring rate）
Ks3=（1-0.4）*Ks;
G3=[（Mc-Cs*T）/Mc  -Ks3*T/Mc  Cs*T/Mc  Ks3*T/Mc  0   0
    0    1     0     0     0    0
    Cs*T/Mb   Ks3*T/Mb   （Mb-Cs*T-Cp*T）/Mb   （-Ks3*T-Kp*T）/Mb   Cp*T/Mb  Kp*T/Mb
    0    0     0     1     0    0
    0    0    Cp*T/Mw   Kp*T/Mw   （Mw-Cp*T）/Mw   -Kp*T/Mw
    0    0     0     0     0    1];
H3=H1;
C3=[-Cs/Mc  -Ks3/Mc  Cs/Mc  Ks3/Mc  0   0];
Q3=Q1;
R3=6*R1;
XE3=XE1;
PE3=Q3;

%Mode4 A secondary vetical spring failure（60% reduction in spring rate）
Ks4=（1-0.6）*Ks;
G4=[（Mc-Cs*T）/Mc  -Ks4*T/Mc  Cs*T/Mc  Ks4*T/Mc  0   0
    0    1     0     0     0    0
    Cs*T/Mb   Ks4*T/Mb   （Mb-Cs*T-Cp*T）/Mb   （-Ks4*T-Kp*T）/Mb   Cp*T/Mb  Kp*T/Mb
    0    0     0     1     0    0
    0    0    Cp*T/Mw   Kp*T/Mw   （Mw-Cp*T）/Mw   -Kp*T/Mw
    0    0     0     0     0    1];
H4=H1;
C4=[-Cs/Mc  -Ks4/Mc  Cs/Mc  Ks4/Mc  0   0];
Q4=Q1;
R4=8*R1;
XE4=XE1;
PE4=Q4;


%Mode5 A primary lateral damper failure（100% reduciton）
Cp5=0*Cp;
G5=[（Mc-Cs*T）/Mc  -Ks*T/Mc  Cs*T/Mc  Ks*T/Mc  0   0
    0    1     0     0     0    0
    Cs*T/Mb   Ks*T/Mb   （Mb-Cs*T-Cp5*T）/Mb   （-Ks*T-Kp*T）/Mb   Cp5*T/Mb  Kp*T/Mb
    0    0     0     1     0    0
    0    0    Cp5*T/Mw   Kp*T/Mw   （Mw-Cp5*T）/Mw   -Kp*T/Mw
    0    0     0     0     0    1];
H5=H1;
C5=[-Cs/Mc  -Ks/Mc  Cs/Mc  Ks/Mc  0   0];
Q5=Q1;
R5=70*R1;
XE5=XE1;
PE5=Q5;


% 输入交互
N=length（u）;
%循环

for k=1:N
c=zeros（1modelNum）;
UU=zeros（modelNummodelNum）;
for

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        8312  2010-04-12 10:23  IMM目标跟踪\immkf.m
     文件         608  2010-04-12 10:22  IMM目标跟踪\mode.m
     文件        2809  2010-04-12 10:21  IMM目标跟踪\realTrack.m
     文件         148  2010-04-23 22:25  IMM目标跟踪\vehicleParameters.m
     文件          40  2010-05-12 09:33  IMM目标跟踪\说明.txt
     目录           0  2018-08-29 15:52  IMM目标跟踪\