%% Generate Visual Detections of Multiple Vehicles
% Generate detections using a forward-facing automotive vision sensor mounted on an ego vehicle. Assume that there are two target vehicles:
% Vehicle 1 is directly in front of the ego vehicle and moving at the same speed.
% Vehicle 2 vehicle is driving faster than the ego vehicle by 12 kph in the left lane.
% All positions, velocities, and measurements are relative to the ego vehicle. Run the simulation for ten steps.

dt = 0.1;
car1 = struct('ActorID',1,'Position',[100 0 0],'Velocity', [5*1000/3600 0 0]);
car2 = struct('ActorID',2,'Position',[150 10 0],'Velocity',[12*1000/3600 0 0]);

%
% Create an automotive vision sensor having a location offset from the ego vehicle. By default, the sensor location is at (3.4,0) meters from the vehicle center and 1.1 meters above the ground plane.

sensor = visionDetectionGenerator('DetectionProbability',1, ...
    'MinObjectImageSize',[5 5],'MaxRange',200,'DetectionCoordinates','Sensor Cartesian');
tracker = multiObjectTracker('FilterInitializationFcn',@initcvkf, ...
    'ConfirmationParameters',[3 4],'NumCoastingUpdates',6);

%
% Generate visual detections for the non-ego actors as they move. The output detections form a cell array. Extract only position information from the detections to pass to the multiObjectTracker, which expects only position information. Then update the tracker for each new set of detections.

simTime = 0;
nsteps = 10;
for k = 1:nsteps
    dets = sensor([car1 car2],simTime);
    n = size(dets,1);
    for k = 1:n
        meas = dets{k}.Measurement(1:3);
        dets{k}.Measurement = meas;
        measmtx = dets{k}.MeasurementNoise(1:3,1:3);
        dets{k}.MeasurementNoise = measmtx;
    end
    [confirmedTracks,tentativeTracks,allTracks] = updateTracks(tracker,dets,simTime);
    simTime = simTime + dt;
    car1.Position = car1.Position + dt*car1.Velocity;
    car2.Position = car2.Position + dt*car2.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 into a MATLAB® array. Then, plot the detections using birdsEyePlot functions.

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

% Copyright 2017 The MathWorks, Inc.