资源简介
一个简单的交互式多模型滤波程序,跟踪一个平面内运动目标点,包括卡尔曼滤波程序
代码片段和文件信息
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); %计算模型混合概率
属性 大小 日期 时间 名称
----------- --------- ---------- ----- ----
目录 0 2013-12-11 11:21 IMM代码\
文件 5751 2012-04-30 15:02 IMM代码\IMMKF_Matlab.asv
文件 5751 2012-04-30 15:03 IMM代码\IMMKF_Matlab.m
文件 985 2012-04-30 14:49 IMM代码\Kalman.m
文件 472 2012-04-30 14:49 IMM代码\Model_mix.m
文件 796 2012-04-30 14:48 IMM代码\Model_P_up.m
- 上一篇:图像采集处理
- 下一篇:基 于OpenCV 的动态背景下的运动目标检测
相关资源
- DDR3-SO-DIMM 封装库 AD Cadece
- gimms ndvi3g数据提取——R语言.txt
- SO DIMM200 DDR
- 交互式多模型滤波IMM
- IMM滤波代码完整版
- 雷达目标跟踪程序合集.zip
- so-dimm204 DDR3 封装
- 基于SimMechanics的三自由度并联机器人
- 交互式多模型 IMMCT/CV混合及代码实现
- so-dimm pcb封装
- dimm 200pin pcb 封装
- so-dimm 200pin封装尺寸
- 多机动目标跟踪的IMM_GMPHD滤波算法_严
- IMM交互多模型算法
- IMM多模型滤波目标跟踪
- CACV交互滤波IMM
- Rapid detection of virulent factors of Aeromon
- 3650M3升级uEFI_IMM微码
- Isolation Molecules characterization and Immun
- 交互式多模型 IMMCA/CV混合及代码实现
- IMM_CA_CT.zip
- react-reacthooksreduximmutablejs仿网易云音乐
- GIMMS 3g convertion
- GIMMS 3g v1 convert
- NetCDF格式转换TIFF-以全球GIMMS3g-NDVI数据
- 多模式卡尔曼滤波算法(IMMKF)
- IMM算法例程与实验报告
- IMM交互模型跟踪算法
评论
共有 条评论