%% Generate Radar Detections of Multiple Vehicles
% Generate detections using a forward-facing automotive radar mounted on an ego vehicle. Assume that there are three targets:
% Vehicle 1 is in the center lane, directly in front of the ego vehicle, and driving at the same speed.
% Vehicle 2 is in the left lane and driving faster than the ego vehicle by 12 kilometers per hour.
% Vehicle 3 is in the right lane and driving slower than the ego vehicle by 5 kilometers per hour.
% All positions, velocities, and measurements are relative to the ego vehicle. Run the simulation for ten steps.

dt = 0.1;
pos1 = [150 0 0];
pos2 = [160 10 0];
pos3 = [130 -10 0];
vel1 = [0 0 0];
vel2 = [12*1000/3600 0 0];
vel3 = [-5*1000/3600 0 0];
car1 = struct('ActorID',1,'Position',pos1,'Velocity',vel1);
car2 = struct('ActorID',2,'Position',pos2,'Velocity',vel2);
car3 = struct('ActorID',3,'Position',pos3,'Velocity',vel3);

%
% Create an automotive radar sensor that is offset from the ego vehicle. By default, the sensor location is at (3.4,0) meters from the vehicle center and 0.2 meters above the ground plane. Turn off the range rate computation so that the radar sensor measures position only.

radar = radarDetectionGenerator('DetectionCoordinates','Sensor Cartesian', ...
    'MaxRange',200,'RangeResolution',10,'AzimuthResolution',10, ...
    'FieldOfView',[40 15],'UpdateInterval',dt,'HasRangeRate',false);
tracker = multiObjectTracker('FilterInitializationFcn',@initcvkf, ...
    'ConfirmationThreshold',[3 4],'DeletionThreshold',6);

%
% Generate detections with the radar from the non-ego vehicles. The output detections form a cell array and can be passed directly in to the multiObjectTracker.

simTime = 0;
nsteps = 10;
for k = 1:nsteps
    dets = radar([car1 car2 car3],simTime);
    [confirmedTracks,tentativeTracks,allTracks] = tracker(dets,simTime);

%
% Move the cars one time step and update the multi-object tracker.

    simTime = simTime + dt;
    car1.Position = car1.Position + dt*car1.Velocity;
    car2.Position = car2.Position + dt*car2.Velocity;
    car3.Position = car3.Position + dt*car3.Velocity;
end

%
% Use birdsEyePlot to create an overhead view of the detections. Plot the sensor coverage area. Extract the X and Y positions of the targets by converting the Measurement fields of the cell array into a MATLAB array. Display the detections on the bird's-eye plot.

BEplot = birdsEyePlot('XLim',[0 220],'YLim',[-75 75]);
caPlotter = coverageAreaPlotter(BEplot,'DisplayName','Radar coverage area');
plotCoverageArea(caPlotter,radar.SensorLocation,radar.MaxRange, ...
    radar.Yaw,radar.FieldOfView(1))
detPlotter = detectionPlotter(BEplot,'DisplayName','Radar detections');
detPos = cellfun(@(d)d.Measurement(1:2),dets,'UniformOutput',false);
detPos = cell2mat(detPos')';
if ~isempty(detPos)
    plotDetection(detPlotter,detPos)
end

% Copyright 2012 The MathWorks, Inc.