2022年IMMKF算法Matlab程序 .pdf
《2022年IMMKF算法Matlab程序 .pdf》由会员分享,可在线阅读,更多相关《2022年IMMKF算法Matlab程序 .pdf(6页珍藏版)》请在淘文阁 - 分享文档赚钱的网站上搜索。
1、一、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=1,0,0,0;0,0,1,0;%模型量测矩阵G=T2/2,0;T,0;0,T2/2;0,T;%模型过程噪声加权矩阵r=200;%20 2000 R=r,0;0,r;%模型量测噪声协方差矩阵Q=10,0;0,10;%模型过程噪声协方差矩阵F1=1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1;%模
2、型 1状态转移矩阵F2=1,sin(w2*T)/w2,0,(cos(w2*T)-1)/w2;0,cos(w2*T),0,sin(w2*T);0,(1-cos(w2*T)/w2,1,sin(w2*T)/w2;0,-sin(w2*T),0,cos(w2*T);%模型 2状态转移矩阵左转弯F3=1,sin(w3*T)/w3,0,(cos(w3*T)-1)/w3;0,cos(w3*T),0,sin(w3*T);0,(1-cos(w3*T)/w3,1,sin(w3*T)/w3;0,-sin(w3*T),0,cos(w3*T);%模型 3状态转移矩阵右转弯w4=3*2*pi/360;%模型 2转弯率 3度
3、w5=-3*2*pi/360;%模型 3转弯率-3 度F4=1,sin(w4*T)/w4,0,(cos(w4*T)-1)/w4;0,cos(w4*T),0,sin(w4*T);0,(1-cos(w4*T)/w4,1,sin(w4*T)/w4;0,-sin(w4*T),0,cos(w4*T);%模型 2状态转移矩阵F5=1,sin(w5*T)/w5,0,(cos(w5*T)-1)/w5;0,cos(w5*T),0,sin(w5*T);0,(1-cos(w5*T)/w5,1,sin(w5*T)/w5;0,-sin(w5*T),0,cos(w5*T);%模型 3状态转移矩阵x0=1000,200,1
4、000,200;%初始状态名师资料总结-精品资料欢迎下载-名师精心整理-第 1 页,共 6 页 -%产生量测数据%z,z_true=targets();randn(state,sum(100*clock);%Shuffle the pack!x=zeros(4,simTime);z=zeros(2,simTime);%含噪声量测数据z_true=zeros(2,simTime);%真值数据measureNoise=zeros(2,simTime);measureNoise=sqrt(R)*randn(2,simTime);%产生量测噪声x(:,1)=x0;z(:,1)=H*x(:,1)+mea
5、sureNoise(:,1);z_true(:,1)=H*x(:,1);for a=2:simTime if(a=20)&(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(4,1);%模型 1IMM 算法状态估计值x2_IMM=zeros(4,1);%模型 2IMM 算法状态估计值x3_IMM=zeros(4,1);%模型
6、3IMM 算法状态估计值x_pro_IMM=zeros(4,simTime);%IMM算法模型综合状态估计值P_IMM=zeros(4,4,simTime);%IMM算法模型综合状态协方差矩阵P1_IMM=zeros(4,4);P2_IMM=zeros(4,4);P3_IMM=zeros(4,4);%IMM算法各模型协方差矩阵r1_IMM=zeros(2,1);r2_IMM=zeros(2,1);r3_IMM=zeros(2,1);S1_IMM=zeros(2,2);S2_IMM=zeros(2,2);S3_IMM=zeros(2,2);%初始化x_pro_IMM(:,1)=x0;pij=0.
7、9,0.05,0.05;0.1,0.8,0.1;名师资料总结-精品资料欢迎下载-名师精心整理-第 2 页,共 6 页 -0.05,0.15,0.8;%模型转移概率矩阵%pij=0.6,0.2,0.2;0.2,0.6,0.2;0.25,0.15,0.6;%模型转移概率矩阵u_IMM=zeros(3,simTime);u_IMM(:,1)=0.3,0.3,0.4;%IMM算法模型概率x1_IMM=x0;x2_IMM=x0;x3_IMM=x0;%IMM算法各模型初始状态P0=diag(1000,500,1000,500);%初始状态协方差矩阵P1_IMM=P0;P2_IMM=P0;P3_IMM=P0
8、;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);%计算模型混合概率%计算各模型滤波初始化条件 x01=x1_IMM*ui1(1)+x2_IMM*ui1(2)+x3_IMM*ui1(3);x02=x1_IMM*ui2(1)+x2_IMM*ui2(2)+x3
9、_IMM*ui2(3);x03=x1_IMM*ui3(1)+x2_IMM*ui3(2)+x3_IMM*ui3(3);%各模型滤波初始状态 P01=(P1_IMM+x1_IMM-x01*x1_IMM-x01)*ui1(1)+.(P2_IMM+x2_IMM-x01*x2_IMM-x01)*ui1(2)+.(P3_IMM+x3_IMM-x01*x3_IMM-x01)*ui1(3);P02=(P1_IMM+x1_IMM-x02*x1_IMM-x02)*ui2(1)+.(P2_IMM+x2_IMM-x02*x2_IMM-x02)*ui2(2)+.(P3_IMM+x3_IMM-x02*x3_IMM-x02
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 2022年IMMKF算法Matlab程序 2022 IMMKF 算法 Matlab 程序
限制150内