资源简介

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

评论

共有 条评论