function [currentjob,jobq] = getCurrentRadarTask(jobq,current_time) searchq = jobq. searchqueue;trackq = jobq.TrackQueue;searchidx = jobq. searchchindex;num_trackq_items = jobq.NumTrackJobs;%更新搜索队列索引searchqidx = mod(searchidx-1，数字(searchq))+1;查找到期且优先级最高的跟踪任务readyidx = Find ([trackq(1:num_trackq_items).Time]<=current_time);[~，maxpidx] = max([trackq(readyidx).Priority]);Taskqidx = readyidx(maxpidx);如果找到的跟踪作业具有更高的优先级，则将其用作当前作业，并增加下一个搜索作业的优先级，因为它被延迟。%否则，下一个到期的搜索任务就是当前任务。 if ~isempty(taskqidx) % && trackq(taskqidx).Priority >= searchq(searchqidx).Priority currentjob = trackq(taskqidx); for m = taskqidx+1:num_trackq_items trackq(m-1) = trackq(m); end num_trackq_items = num_trackq_items-1; searchq(searchqidx).Priority = searchq(searchqidx).Priority+100; else currentjob = searchq(searchqidx); searchidx = searchqidx+1; end jobq.SearchQueue = searchq; jobq.SearchIndex = searchidx; jobq.TrackQueue = trackq; jobq.NumTrackJobs = num_trackq_items;

function allocationType = helperAdaptiveTrackingSim(scenario, thp, rax, tracker, resetJobQ, managerPreferences) %初始化变量radar = scenario. platforms {end}.Sensors{1};updateRate = radar.UpdateRate;resourceAllocation = nan(1,updateRate);h =直方图(rax,resourceAllocation，'BinMethod'，'integers');包含(h.Parent TrackID) ylabel (h。父母,“Num梁”);numSteps = update * 185;allocationType = nan(1,numSteps);currentStep = 1;重启(场景);重置(跟踪); % Return to a reset state of jobq jobq = resetJobQ; % Plotters and axes plp = thp.Plotters(1); dtp = thp.Plotters(3); cvp = thp.Plotters(4); trp = thp.Plotters(5); ax = gca; colors = ax.ColorOrder; % For repeatable results, set the random seed and revert it when done s = rng(2020); oc = onCleanup(@() rng(s)); % Main loop tracks = {}; while advance(scenario) time = scenario.SimulationTime; % Update ground truth display poses = platformPoses(scenario); plotPlatform(plp, reshape([poses.Position],3,[])'); % Point the radar based on the scheduler current job [currentJob,jobq] = getCurrentRadarTask(jobq,time); currentStep = currentStep + 1; if currentStep > updateRate resourceAllocation(1:end-1) = resourceAllocation(2:updateRate); end if strcmpi(currentJob.JobType,'Search') detectableTracks = zeros(0,1,'uint32'); resourceAllocation(min([currentStep,updateRate])) = 0; allocationType(currentStep) = 1; cvp.Color = colors(1, :); else detectableTracks = currentJob.TrackID; resourceAllocation(min([currentStep,updateRate])) = currentJob.TrackID; if strcmpi(currentJob.JobType,'Confirm') allocationType(currentStep) = 2; cvp.Color = colors(2, :); else allocationType(currentStep) = 3; cvp.Color = colors(3, :); end end ra = resourceAllocation(~isnan(resourceAllocation)); h.Data = ra; h.Parent.YLim = [0 updateRate]; h.Parent.XTick = 0:max(ra); radar.LookAngle = currentJob.BeamDirection; plotCoverage(cvp, coverageConfig(scenario)); % Collect detections and plot them detections = detect(scenario); if isempty(detections) meas = zeros(0,3); else dets = [detections{:}]; meassph = reshape([dets.Measurement],3,[])'; [x,y,z] = sph2cart(deg2rad(meassph(1)),deg2rad(meassph(2)),meassph(3)); meas = (detections{1}.MeasurementParameters.Orientation*[x;y;z]+detections{1}.MeasurementParameters.OriginPosition)'; end plotDetection(dtp, meas); % Track and plot tracks if isLocked(tracker) || ~isempty(detections) [confirmedTracks,tentativeTracks,~,analysisInformation] = tracker(detections, time, detectableTracks); pos = getTrackPositions(confirmedTracks,jobq.PositionSelector); plotTrack(trp,pos,string([confirmedTracks.TrackID])); tracks.confirmedTracks = confirmedTracks; tracks.tentativeTracks = tentativeTracks; tracks.analysisInformation = analysisInformation; tracks.PositionSelector = jobq.PositionSelector; end % Manage resources for next jobs jobq = manageResource(detections,jobq,tracker,tracks,currentJob,time,managerPreferences); end

函数imm = initMPARIMM(检测)cvekf = initcvekf(检测);cvekf.StateCovariance(2,2) = 1e6;cvekf.StateCovariance(4,4) = 1e6;cvekf.StateCovariance(6,6) = 1e6;Ctekf = initctekf(检测);ctekf.StateCovariance(2,2) = 1e6;ctekf.StateCovariance(4,4) = 1e6;ctekf.StateCovariance(7,7) = 1e6;ctekf.ProcessNoise(3,3) = 1e6;%未知角加速度的大噪声imm = trackingIMM('TrackingFilters'， {cvekf;ctekf}， ' transitionprobability '， [0.99, 0.99]);

函数jobq = manageResource(检测，jobq,tracker,tracks,current_job,current_time,managerPreferences)num_trackq_items = jobq.NumTrackJobs;如果~为空(检测)检测=检测{1};Else检测= [];执行当前作业切换current_job. end %JobTypecase 'Search' % For search job, if there is a detection, establish tentative % track and schedule a confirmation job if ~isempty(detection) % A search task can still find a track we already have. Define % a confirmation task only if it's a tentative track. There % could be more than one if there are false alarms. Create % confirm jobs for tentative tracks created at this update. numTentative = numel(tracks.tentativeTracks); for i = 1:numTentative if tracks.tentativeTracks(i).Age == 1 && tracks.tentativeTracks(i).IsCoasted == 0 trackid = tracks.tentativeTracks(i).TrackID; job = revisitTrackJob(tracker, trackid, current_time, managerPreferences, 'Confirm', tracks.PositionSelector); num_trackq_items = num_trackq_items+1; trackq(num_trackq_items) = job; end end end case 'Confirm' % For a confirm job, if the track ID is within the tentative % tracks, it means that we need to run another confirmation job % regardless of having a detection. If the track ID is within the % confirmed tracks, it means that we must have gotten a detection, % and the track passed the confirmation logic test. In this case we % need to schedule a track revisit job. trackid = current_job.TrackID; tentativeTrackIDs = [tracks.tentativeTracks.TrackID]; confirmedTrackIDs = [tracks.confirmedTracks.TrackID]; if any(trackid == tentativeTrackIDs) job = revisitTrackJob(tracker, trackid, current_time, managerPreferences, 'Confirm', tracks.PositionSelector); num_trackq_items = num_trackq_items+1; trackq(num_trackq_items) = job; elseif any(trackid == confirmedTrackIDs) job = revisitTrackJob(tracker, trackid, current_time, managerPreferences, 'TrackNonManeuvering', tracks.PositionSelector); num_trackq_items = num_trackq_items+1; trackq(num_trackq_items) = job; end otherwise % Covers both types of track jobs % For track job, if the track hasn't been dropped, update the track % and schedule a track job corresponding to the revisit time % regardless of having a detection. In the case when there is no % detection, we could also predict and schedule a track job sooner % so the target is not lost. This would require defining another % job type to control the revisit rate for this case. trackid = current_job.TrackID; confirmedTrackIDs = [tracks.confirmedTracks.TrackID]; if any(trackid == confirmedTrackIDs) jobType = 'TrackNonManeuvering'; mdlProbs = getTrackFilterProperties(tracker, trackid, 'ModelProbabilities'); if mdlProbs{1}(2) > 0.6 jobType = 'TrackManeuvering'; end job = revisitTrackJob(tracker, trackid, current_time, managerPreferences, jobType, tracks.PositionSelector); num_trackq_items = num_trackq_items+1; trackq(num_trackq_items) = job; end end jobq.TrackQueue = trackq; jobq.NumTrackJobs = num_trackq_items; end function job = revisitTrackJob(tracker, trackID, currentTime, managerPreferences, jobType, positionSelector) types = [managerPreferences.Type]; inTypes = strcmpi(jobType,types); revisitTime = 1/managerPreferences(inTypes).RevisitRate + currentTime; predictedTrack = predictTracksToTime(tracker,trackID,revisitTime); xpred = getTrackPositions(predictedTrack,positionSelector); [phipred,thetapred,rpred] = cart2sph(xpred(1),xpred(2),xpred(3)); job = struct('JobType',jobType,'Priority',managerPreferences(inTypes).Priority,... 'BeamDirection',rad2deg([phipred thetapred]),'WaveformIndex',1,'Time',revisitTime,... 'Range',rpred,'TrackID',trackID); end