IMM滤波代码完整版

IMM 滤波算法 多模型 代码

clear all;
clc;
echo off;
%===============================
%建立模型
%===============================
% 仿真参数
simTime=100;      %仿真迭代次数
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];                            %模型量测噪声协方差矩阵
Q=[100;010];                                  %模型过程噪声协方差矩阵

F1=[1T00;0100;001T;0001];     %模型1状态转移矩阵

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状态转移矩阵 左转弯

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状态转移矩阵 右转弯

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状态转移矩阵

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状态转移矩阵

x0=[10002001000200]‘;  % 初始状态

% 产生量测数据
%[zz_true]=targets（）;
randn（‘state‘sum（100*clock））; % Shuffle the pack!
x = zeros（4simTime）;
z = zeros（2simTime）;         %含噪声量测数据
z_true = zeros（2simTime）;    %真值数据
measureNoise = zeros（2simTime）;
measureNoise = sqrt（R）*randn（2simTime）;  %产生量测噪声
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算法状态估计值
x2_IMM = zeros（41）;      %模型2IMM算法状态估计值
x3_IMM = zeros（41）;      %模型3IMM算法状态估计值
x_pro_IMM = zeros（4simTime）;   %IMM算法模型综合状态估计值
P_IMM=zeros（44simTime）;       %IMM算法模型综合状态协方差矩阵
P1_IMM=zeros（44）;
P2_IMM=zeros（44）;
P3_IMM=zeros（44）;              %IMM算法各模型协方差矩阵
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算法各模型初始状态

P0=diag（[10005001000500]）;  %初始状态协方差矩阵

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代码\