基于毫米波雷达目标检测数据仿真的IMM目标跟踪算法matlab下载

% 雷达运动航迹数据仿真 %在车身坐标系下，y指向本车前进方向，x指向本车右侧； %目标航向角是与x轴的夹角，逆时针为正 clc;clear all; % close all FramesLen = 1000; delt = 0.06; EKFflag = 1; %0-UKF,1-EKF,2-AEKF plotfalg = 1; simulate = 0; testflag = 0; FramesLen = 500; filter = 5;%1-CV;11-CA;2-CTRA;3-IMM(CV+CTRA);4(CA+CTRA);5(CA+CTRV);6 if filter == 1 pcolor = 'b'; elseif filter == 11 pcolor = 'c'; elseif filter == 2 pcolor = 'g'; elseif filter == 3 pcolor = 'y'; elseif filter == 4 pcolor = 'm'; elseif filter == 5 pcolor = 'r'; elseif filter == 6 pcolor = 'c'; end % if EKFflag == 1 % pcolor = 'b'; % else % pcolor = 'm'; % end if simulate == 0 load('SV.mat') SV.IMM = []; load('allSV.mat') end error = []; for testTime = 1:300 %% 在雷达车身坐标系下 if simulate == 1 SV.x(1,1) = -10+(10-(-10))*rand(1); SV.y(1,1) = -120+(-1-(-120))*rand(1); SV.head(1,1) = -180+(180-(-180))*rand(1);%90; % ° SV.speed(1,1) = 10+(30-10)*rand(1); SV.acc(1,1) = -2+(2-(-2))*rand(1); SV.yawrate(1,1) = 0; % °/s Aspeedup = 2+0.01*randn(); Aslowdown = -2+0.01*randn(); Wturnright = -1+0.001*randn(); Wturnleft = 1+0.001*randn(); state = 'SPEED-UP'; state_keep_time = 0; state_keep_counts = 0; stateChangeIframe = 1; stateChangeAngle = 30+randn(); state_stop_time = 0; for iFrame = 2:FramesLen SV.x(iFrame,1) = SV.x(iFrame-1,1) + SV.speed(iFrame-1,1) * delt * cosd(SV.head(iFrame-1,1)) ; SV.y(iFrame,1) = SV.y(iFrame-1,1) + SV.speed(iFrame-1,1) * delt * sind(SV.head(iFrame-1,1)) ; SV.head(iFrame,1) = SV.head(iFrame-1,1) + SV.yawrate(iFrame-1,1) * delt; if SV.head(iFrame,1) > 180 SV.head(iFrame,1) = SV.head(iFrame,1) - 360; elseif SV.head(iFrame,1) < -180 SV.head(iFrame,1) = SV.head(iFrame,1) + 360; end if SV.speed(iFrame-1,1) < 40/3.6 && strcmp(state,'SPEED-UP') SV.speed(iFrame,1) = SV.speed(iFrame-1,1) + (Aspeedup + 0.01 * randn()) * delt; SV.yawrate(iFrame,1) = SV.yawrate(iFrame-1,1) + 0.01 * randn(); elseif strcmp(state,'SPEED-UP') || strcmp(state,'KEEP-GOING') SV.speed(iFrame,1) = SV.speed(iFrame-1,1) + 0.01 * randn(); SV.yawrate(iFrame,1) = SV.yawrate(iFrame-1,1)*0.8 + 0.01 * randn(); state_keep_time = state_keep_time + 1; state = 'KEEP-GOING'; if state_keep_time > 3/delt state_keep_counts = state_keep_counts + 1; if SV.speed(iFrame-1,1) > 40/3.6 state = 'SLOW-DOWN'; elseif SV.speed(iFrame-1,1) < 5/3.6 state = 'STOPING'; else state = 'KEEP-GOING'; end state_keep_time = 0; if state_keep_counts == 2 state = 'CIRCLE'; stateChangeIframe = iFrame; elseif state_keep_counts >= 3 state_keep_counts = 0; state = 'STOPING'; end end elseif strcmp(state,'SLOW-DOWN') || strcmp(state,'TURN-RIGHT') || strcmp(state,'TURN-LEFT') SV.speed(iFrame,1) = max([SV.speed(iFrame-1,1) + (Aslowdown + 0.01 * randn()) * delt; 3]); if strcmp(state,'SLOW-DOWN' ) if SV.speed(iFrame-1,1) >= 5/3.6 && abs(SV.head(iFrame,1)-SV.head(iFrame-1,1)) < 10 SV.yawrate(iFrame,1) = SV.yawrate(iFrame-1,1) + Wturnright + 0.01 * randn(); state = 'TURN-RIGHT'; stateChangeIframe = iFrame; else state = 'STOPING'; end elseif strcmp(state,'TURN-RIGHT') && abs(SV.head(iFrame,1)-SV.head(stateChangeIframe,1)) < 30 if abs(SV.yawrate(iFrame-1)) < 10 SV.yawrate(iFrame,1) = SV.yawrate(iFrame-1,1) + Wturnright + 0.01 * randn(); else SV.yawrate(iFrame,1) = SV.yawrate(iFrame-1,1) + 0.01 * randn(); end elseif strcmp(state,'TURN-RIGHT') && abs(SV.head(iFrame,1)-SV.head(stateChangeIframe,1)) >= 30 SV.yawrate(iFrame,1) = SV.yawrate(iFrame-1,1) + Wturnleft + 0.01 * randn(); state = 'TURN-LEFT'; stateChangeIframe = iFrame; elseif strcmp(state,'TURN-LEFT') && abs(SV.head(iFrame,1)-SV.head(stateChangeIframe,1)) < 30 if abs(SV.yawrate(iFrame-1,1)) < 10 SV.yawrate(iFrame,1) = SV.yawrate(iFrame-1,1) + Wturnleft + 0.01 * randn(); else SV.yawrate(iFrame,1) = SV.yawrate(iFrame-1,1) + 0.01 * randn(); end elseif strcmp(state,'TURN-LEFT') && abs(SV.head(iFrame,1)-SV.head(stateChangeIframe,1)) >= 30 state = 'KEEP-GOING'; SV.yawrate(iFrame,1) = SV.yawrate(iFrame-1,1) + 0.01 * randn(); end elseif strcmp(state,'STOPING') || strcmp(state,'STOPED') SV.speed(iFrame,1) = max([SV.speed(iFrame-1,1) + (Aslowdown + 0.01 * randn()) * delt; 0]); SV.yawrate(iFrame,1) = SV.yawrate(iFrame-1,1) + 0.01 * randn(); if SV.speed(iFrame,1) <= 1/3.6 && state_stop_time < 50 state_stop_time = state_stop_time + 1; state = 'STOPED'; elseif state_stop_time >= 50 state = 'SPEED-UP'; end elseif strcmp(state,'CIRCLE') SV.speed(iFrame,1) = SV.speed(iFrame-1,1) + 0.01 * randn(); if abs(SV.head(iFrame,1)-SV.head(stateChangeIframe,1)) < 90 SV.yawrate(iFrame,1) = SV.yawrate(iFrame-1,1) + Wturnright + 0.01 * randn(); else SV.yawrate(iFrame,1) = SV.yawrate(iFrame-1,1) + Wturnleft + 0.01 * randn(); if abs(SV.yawrate(iFrame,1)) < 1 state = 'KEEP-GOING'; end end end end %% simulation actual for iFrame = 1:length(SV.x) SV.Rm(iFrame,1) = sqrt(SV.x(iFrame,1)^2 + SV.y(iFrame,1)^2) + 0.01*randn(); SV.thetaRm(iFrame,1) = atan2d(SV.x(iFrame,1),SV.y(iFrame,1)) + 0.05*randn(); SV.xm(iFrame,1) = SV.Rm(iFrame,1) * sind(SV.thetaRm(iFrame,1)); SV.ym(iFrame,1) = SV.Rm(iFrame,1) * cosd(SV.thetaRm(iFrame,1)); SV.vx(iFrame,1) = SV.speed(iFrame,1)*cosd(SV.head(iFrame,1)); SV.vy(iFrame,1) = SV.speed(iFrame,1)*sind(SV.head(iFrame,1)); SV.vRm(iFrame,1) = SV.vx(iFrame,1) * SV.x(iFrame,1)/sqrt(SV.x(iFrame,1)^2 + SV.y(iFrame,1)^2) + SV.vy(iFrame,1) * SV.y(iFrame,1)/sqrt(SV.x(iFrame,1)^2 + SV.y(iFrame,1)^2) + 0.01*randn(); end save 'SV.mat' SV % allSV(testTime) = SV; elseif testflag == 1 SV = allSV(testTime); fprintf('testTime = %d\n',testTime) end if plotfalg == 1 figure(11) clf subplot(3,2,1) plot(SV.x,SV.y);hold on;plot(SV.x(1),SV.y(1),'s');hold on;plot(0,0,'s');axis equal;title('位置') subplot(3,2,2) plot(SV.thetaRm);title('方位角') subplot(3,2,3) plot(SV.vRm);title('径向速度') subplot(3,2,4)