交互式多模型滤波IMM

交互式多模型算法(IMM)程序包 包括详细说明文档

clear all
close all
clc
%%%加载数据
load Measure.mat%加载量测数据
load Real.mat%加载真实值数据
Z=TimeRPhi（2:3:）;%量测值
Xr=TXYdXdYAxAyTPhi;%真值
%%%设置初值
T=0.1;%采样时间
m=10;%斜距标准差
n=0.1/57.3;%方位角标准差
X=[10000;20;0;0;0;0];%状态变量初值
P=diag（[101010101010]）;%误差协方差初值
R=diag（[m^2n^2]）;%量测噪声方差

F=[1 0 T 0 T^2/2 0;%状态转移矩阵
   0 1 0 T 0 T^2/2;
   0 0 1 0 T 0;
   0 0 0 1 0 T;
   0 0 0 0 1 0;
   0 0 0 0 0 1];
Ta=[T^2/2 0 T 0 1 0;
    0 T^2/2 0 T 0 1];
Ta=Ta‘;%噪声阵
% sigam=60;
sigam=20;
Q=Ta*Ta‘*sigam^2;%系统噪声方差

for k=1:501
    Xk=F*X;
    Pk=F*P*F‘+Q;
    Zk=[（（Xk（1））^2+（Xk（2））^2）^0.5;atan（（Xk（2））/（Xk（1）））];
    H11=Xk（1）/（（Xk（1））^2+（Xk（2））^2）^0.5;
    H12=Xk（2）/（（Xk（1））^2+（Xk（2））^2）^0.5;
    H21=-Xk（2）/（（Xk（1））^2+（Xk（2））^2）;
    H22=Xk（1）/（（Xk（1））^2+（Xk（2））^2）;
    H=[H11 H12 0 0 0 0;
       H21 H22 0 0 0 0];
   
    K=Pk*H‘*inv（H*Pk*H‘+R）;
    X=Xk+K*（Z（:k）-Zk）;
    P=Pk-K*H*Pk;
    Xe（:k）=X;
    Re=（（X（1））^2+（X（2））^2）^0.5;
    Te=atan（X（2）/X（1））;
    Ze（:k）=[Re;Te];
    
end

figure（1）
plot（Xe（1:）Xe（2:）Xr（2:）Xr（3:）‘r‘）
figure（2）
plot（Xr（7:）-Xe（6:）‘r‘）

% figure（2）
% plot（Xr（9:）-Ze（2:）‘r‘）
% hold on
% plot（Xr（9:）-Z（2:））

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       1265  2009-09-12 16:01  IMM0902_20090916\CAEKF.m

     文件        175  2009-09-05 08:53  IMM0902_20090916\FX1.m

     文件        176  2009-09-05 08:53  IMM0902_20090916\FX2.m

     文件        142  2009-09-05 08:53  IMM0902_20090916\FZ.m

     文件        555  2009-11-20 12:51  IMM0902_20090916\IMM0902程序说明.txt

     文件       5626  2009-09-16 10:52  IMM0902_20090916\IMMUKF.m

     文件      12053  2009-09-05 08:53  IMM0902_20090916\Measure.mat

     文件      36108  2009-09-05 08:53  IMM0902_20090916\Real.mat

     文件     286277  2009-11-20 12:47  IMM0902_20090916\机动目标跟踪.pdf

     目录          0  2009-11-20 12:51  IMM0902_20090916

----------- ---------  ---------- -----  ----

               342377                    10