% Setup the display[videoReader,videoDisplayHandle, bepPlotters, sensor] = helperCreateFCWDemoDisplay('01_city_c2s_fcw_10s.mp4', 'SensorConfigurationData.mat'); % Readthe recorded detections file[visionObjects,radarObjects, inertialMeasurementUnit, laneReports, ...    timeStep, numSteps] =readSensorRecordingsFile('01_city_c2s_fcw_10s_sensor.mat'); % Aninitial ego lane is calculated. If the recorded lane information is%invalid, define the lane boundaries as straight lines half a lane%distance on each side of the carlaneWidth =3.6; % metersegoLane =struct('left', [0 0 laneWidth/2], 'right', [0 0-laneWidth/2]); %Prepare some time variablestime =0;           % Time since thebeginning of the recordingcurrentStep= 0;    % Current timestepsnapTime =9.3;     % The time to capture asnapshot of the display %Initialize the tracker[tracker,positionSelector, velocitySelector] = setupTracker(); whilecurrentStep < numSteps && ishghandle(videoDisplayHandle)    % Update scenariocounters    currentStep = currentStep + 1;    time = time + timeStep;     % Process the sensordetections as objectDetection inputs to the tracker    [detections, laneBoundaries, egoLane] = processDetections(...        visionObjects(currentStep),radarObjects(currentStep), ...        inertialMeasurementUnit(currentStep),laneReports(currentStep), ...        egoLane, time);     % Using the list ofobjectDetections, return the tracks, updated to time    confirmedTracks = updateTracks(tracker,detections, time);     % Find the most importantobject and calculate the forward collision    % warning    mostImportantObject =findMostImportantObject(confirmedTracks, egoLane, positionSelector,velocitySelector);     % Update video andbirds-eye plot displays    frame = readFrame(videoReader);     % Read video frame    helperUpdateFCWDemoDisplay(frame,videoDisplayHandle, bepPlotters, ...        laneBoundaries, sensor, confirmedTracks,mostImportantObject, positionSelector, ...        velocitySelector,visionObjects(currentStep), radarObjects(currentStep));     % Capture a snapshot    if time >= snapTime&& time < snapTime + timeStep        snapnow;    endend

function [tracker, positionSelector, velocitySelector] = setupTracker()        tracker = multiObjectTracker(...            'FilterInitializationFcn', @initConstantAccelerationFilter, ...            'AssignmentThreshold', 35, 'ConfirmationThreshold', [2 3], ...            'DeletionThreshold', 5);         % The State vector is:        %   In constant velocity:     State = [x;vx;y;vy]        %   In constant acceleration: State = [x;vx;ax;y;vy;ay]         % Define which part of the State is the position. For example:        %   In constant velocity:     [x;y] = [1 0 0 0; 0 0 1 0] * State        %   In constant acceleration: [x;y] = [1 0 0 0 0 0; 0 0 0 1 0 0] * State        positionSelector = [1 0 0 0 0 0; 0 0 0 1 0 0];         % Define which part of the State is the velocity. For example:        %   In constant velocity:     [x;y] = [0 1 0 0; 0 0 0 1] * State        %   In constant acceleration: [x;y] = [0 1 0 0 0 0; 0 0 0 0 1 0] * State        velocitySelector = [0 1 0 0 0 0; 0 0 0 0 1 0];    end

function filter = initConstantAccelerationFilter(detection)% This function shows how to configure a constant acceleration filter. The% input is an objectDetection and the output is a tracking filter.% For clarity, this function shows how to configure a trackingKF,% trackingEKF, or trackingUKF for constant acceleration.%% Steps for creating a filter:%   1. Define the motion model and state%   2. Define the process noise%   3. Define the measurement model%   4. Initialize the state vector based on the measurement%   5. Initialize the state covariance based on the measurement noise%   6. Create the correct filter     % Step 1: Define the motion model and state    % This example uses a constant acceleration model, so:    STF = @constacc;     % State-transition function, for EKF and UKF    STFJ = @constaccjac; % State-transition function Jacobian, only for EKF    % The motion model implies that the state is [x;vx;ax;y;vy;ay]    % You can also use constvel and constveljac to set up a constant    % velocity model, constturn and constturnjac to set up a constant turn    % rate model, or write your own models.     % Step 2: Define the process noise    dt = 0.05; % Known timestep size    sigma = 1; % Magnitude of the unknown acceleration change rate    % The process noise along one dimension    Q1d = [dt^4/4, dt^3/2, dt^2/2; dt^3/2, dt^2, dt; dt^2/2, dt, 1] * sigma^2;    Q = blkdiag(Q1d, Q1d); % 2-D process noise     % Step 3: Define the measurement model    MF = @fcwmeas;       % Measurement function, for EKF and UKF    MJF = @fcwmeasjac;   % Measurement Jacobian function, only for EKF     % Step 4: Initialize a state vector based on the measurement    % The sensors measure [x;vx;y;vy] and the constant acceleration model's    % state is [x;vx;ax;y;vy;ay], so the third and sixth elements of the    % state vector are initialized to zero.    state = [detection.Measurement(1); detection.Measurement(2); 0; detection.Measurement(3); detection.Measurement(4); 0];     % Step 5: Initialize the state covariance based on the measurement    % noise. The parts of the state that are not directly measured are    % assigned a large measurement noise value to account for that.    L = 100; % A large number relative to the measurement noise    stateCov = blkdiag(detection.MeasurementNoise(1:2,1:2), L, detection.MeasurementNoise(3:4,3:4), L);     % Step 6: Create the correct filter.    % Use 'KF' for trackingKF, 'EKF' for trackingEKF, or 'UKF' for trackingUKF    FilterType = 'EKF';     % Creating the filter:    switch FilterType        case'EKF'            filter = trackingEKF(STF, MF, state,...                'StateCovariance', stateCov, ...                'MeasurementNoise', detection.MeasurementNoise(1:4,1:4), ...                'StateTransitionJacobianFcn', STFJ, ...                'MeasurementJacobianFcn', MJF, ...                'ProcessNoise', Q ...                );        case'UKF'            filter = trackingUKF(STF, MF, state, ...                'StateCovariance', stateCov, ...                'MeasurementNoise', detection.MeasurementNoise(1:4,1:4), ...                'Alpha', 1e-1, ...                'ProcessNoise', Q ...                );        case'KF'% The ConstantAcceleration model is linear and KF can be used            % Define the measurement model: measurement = H * state            % In this case:            %   measurement = [x;vx;y;vy] = H * [x;vx;ax;y;vy;ay]            % So, H = [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0]            %            % Note that ProcessNoise is automatically calculated by the            % ConstantAcceleration motion model            H = [1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0];            filter = trackingKF('MotionModel', '2D Constant Acceleration', ...                'MeasurementModel', H, 'State', state, ...                'MeasurementNoise', detection.MeasurementNoise(1:4,1:4), ...                'StateCovariance', stateCov);    endend

  function [detections,laneBoundaries, egoLane] = processDetections...            (visionFrame, radarFrame, IMUFrame, laneFrame, egoLane, time)        % Inputs:        %   visionFrame - objects reported by the vision sensor for this time frame        %   radarFrame  - objects reported by the radar sensor for this time frame        %   IMUFrame    - inertial measurement unit data for this time frame        %   laneFrame   - lane reports for this time frame        %   egoLane     - the estimated ego lane        %   time        - the time corresponding to the time frame         % Remove clutter radar objects        [laneBoundaries, egoLane] = processLanes(laneFrame, egoLane);        realRadarObjects = findNonClutterRadarObjects(radarFrame.object,...            radarFrame.numObjects, IMUFrame.velocity, laneBoundaries);         % Return an empty list if no objects are reported         % Counting the total number of objects        detections = {};        if (visionFrame.numObjects + numel(realRadarObjects)) == 0            return;        end         % Process the remaining radar objects        detections = processRadar(detections, realRadarObjects, time);         % Process video objects        detections = processVideo(detections, visionFrame, time);    end

function mostImportantObject = findMostImportantObject(confirmedTracks,egoLane,positionSelector,velocitySelector)         % Initialize outputs and parameters        MIO = [];               % By default, there is no MIO        trackID = [];           % By default, there is no trackID associated with an MIO        FCW = 3;                % By default, if there is no MIO, then FCW is 'safe'        threatColor = 'green';  % By default, the threat color is green        maxX = 1000;  % Far enough forward so that no track is expected to exceed this distance        gAccel = 9.8; % Constant gravity acceleration, in m/s^2        maxDeceleration = 0.4 * gAccel; % Euro NCAP AEB definition        delayTime = 1.2; % Delay time for a driver before starting to brake, in seconds         positions = getTrackPositions(confirmedTracks, positionSelector);        velocities = getTrackVelocities(confirmedTracks, velocitySelector);         for i = 1:numel(confirmedTracks)            x = positions(i,1);            y = positions(i,2);             relSpeed = velocities(i,1); % The relative speed between the cars, along the lane             if x < maxX && x > 0 % No point checking otherwise                yleftLane  = polyval(egoLane.left,  x);                yrightLane = polyval(egoLane.right, x);                if (yrightLane <= y) && (y <= yleftLane)                    maxX = x;                    trackID = i;                    MIO = confirmedTracks(i).TrackID;                    if relSpeed < 0 % Relative speed indicates object is getting closer                        % Calculate expected braking distance according to                        % Euro NCAP AEB Test Protocol                        d = abs(relSpeed) * delayTime + relSpeed^2 / 2 / maxDeceleration;                        if x <= d % 'warn'                            FCW = 1;                            threatColor = 'red';                        else% 'caution'                            FCW = 2;                            threatColor = 'yellow';                        end                    end                end            end        end        mostImportantObject = struct('ObjectID', MIO, 'TrackIndex', trackID, 'Warning', FCW, 'ThreatColor', threatColor);

function [visionObjects, radarObjects, inertialMeasurementUnit, laneReports, ...    timeStep, numSteps] = readSensorRecordingsFile(sensorRecordingFileName)% Read Sensor Recordings% The |ReadDetectionsFile| function reads the recorded sensor data file.% The recorded data is a single structure that is divided into the% following substructures:%% # |inertialMeasurementUnit|, a struct array with fields: timeStamp,%   velocity, and yawRate. Each element of the array corresponds to a%   different timestep.% # |laneReports|, a struct array with fields: left and right. Each element%   of the array corresponds to a different timestep.%   Both left and right are structures with fields: isValid, confidence,%   boundaryType, offset, headingAngle, and curvature.% # |radarObjects|, a struct array with fields: timeStamp (see below),%   numObjects (integer) and object (struct). Each element of the array%   corresponds to a different timestep.%   |object| is a struct array, where each element is a separate object,%   with the fields: id, status, position(x;y;z), velocity(vx,vy,vz),%   amplitude, and rangeMode.%   Note: z is always constant and vz=0.% # |visionObjects|, a struct array with fields: timeStamp (see below),%   numObjects (integer) and object (struct). Each element of the array%   corresponds to a different timestep.%   |object| is a struct array, where each element is a separate object,%   with the fields: id, classification, position (x;y;z),%   velocity(vx;vy;vz), size(dx;dy;dz). Note: z=vy=vz=dx=dz=0%% The timeStamp for recorded vision and radar objects is a uint64 variable% holding microseconds since the Unix epoch. Timestamps are recorded about% 50 milliseconds apart. There is a complete synchronization between the% recordings of vision and radar detections, therefore the timestamps are% not used in further calculations. A = load(sensorRecordingFileName);visionObjects = A.vision;radarObjects = A.radar;laneReports = A.lane;inertialMeasurementUnit = A.inertialMeasurementUnit; timeStep = 0.05;                 % Data is provided every 50 millisecondsnumSteps = numel(visionObjects); % Number of recorded timestepsend

function [laneBoundaries, egoLane] = processLanes(laneReports, egoLane)% Lane boundaries are updated based on the laneReports from the recordings.% Since some laneReports contain invalid (isValid = false) reports or% impossible parameter values (-1e9), these lane reports are ignored and% the previous lane boundary is used.leftLane    = laneReports.left;rightLane   = laneReports.right; % Check the validity of the reported left lanecond = (leftLane.isValid && leftLane.confidence) && ...    ~(leftLane.headingAngle == -1e9 || leftLane.curvature == -1e9);if cond    egoLane.left = cast([leftLane.curvature, leftLane.headingAngle, leftLane.offset], 'double');end % Update the left lane boundary parameters or use the previous onesleftParams  = egoLane.left;leftBoundaries = parabolicLaneBoundary(leftParams);leftBoundaries.Strength = 1; % Check the validity of the reported right lanecond = (rightLane.isValid && rightLane.confidence) && ...    ~(rightLane.headingAngle == -1e9 || rightLane.curvature == -1e9);if cond    egoLane.right  = cast([rightLane.curvature, rightLane.headingAngle, rightLane.offset], 'double');end % Update the right lane boundary parameters or use the previous onesrightParams = egoLane.right;rightBoundaries = parabolicLaneBoundary(rightParams);rightBoundaries.Strength = 1; laneBoundaries = [leftBoundaries, rightBoundaries];end

function realRadarObjects = findNonClutterRadarObjects(radarObject, numRadarObjects, egoSpeed, laneBoundaries)% The radar objects include many objects that belong to the clutter.% Clutter is defined as a stationary object that is not in front of the% car. The following types of objects pass as nonclutter:%% # Any object in front of the car% # Any moving object in the area of interest around the car, including%   objects that move at a lateral speed around the car     % Allocate memory    normVs = zeros(numRadarObjects, 1);    inLane = zeros(numRadarObjects, 1);    inZone = zeros(numRadarObjects, 1);     % Parameters    LaneWidth = 3.6;            % What is considered in front of the car    ZoneWidth = 1.7*LaneWidth;  % A wider area of interest    minV = 1;                   % Any object that moves slower than minV is considered stationary    for j = 1:numRadarObjects        [vx, vy] = calculateGroundSpeed(radarObject(j).velocity(1),radarObject(j).velocity(2),egoSpeed);        normVs(j) = norm([vx,vy]);        laneBoundariesAtObject = computeBoundaryModel(laneBoundaries, radarObject(j).position(1));        laneCenter = mean(laneBoundariesAtObject);        inLane(j) = (abs(radarObject(j).position(2) - laneCenter) <= LaneWidth/2);        inZone(j) = (abs(radarObject(j).position(2) - laneCenter) <= max(abs(vy)*2, ZoneWidth));    end    realRadarObjectsIdx = union(...        intersect(find(normVs > minV), find(inZone == 1)), ...        find(inLane == 1));     realRadarObjects = radarObject(realRadarObjectsIdx);end

function [Vx,Vy] = calculateGroundSpeed(Vxi,Vyi,egoSpeed)% Inputs%   (Vxi,Vyi) : relative object speed%   egoSpeed  : ego vehicle speed% Outputs%   [Vx,Vy]   : ground object speed     Vx = Vxi + egoSpeed;    % Calculate longitudinal ground speed    theta = atan2(Vyi,Vxi); % Calculate heading angle    Vy = Vx * tan(theta);   % Calculate lateral ground speed end

function postProcessedDetections = processVideo(postProcessedDetections, visionFrame, t)% Process the video objects into objectDetection objectsnumRadarObjects = numel(postProcessedDetections);numVisionObjects = visionFrame.numObjects;if numVisionObjects    classToUse = class(visionFrame.object(1).position);    visionMeasCov = cast(diag([2,2,2,100]), classToUse);    % Process Vision Objects:    for i=1:numVisionObjects        object = visionFrame.object(i);        postProcessedDetections{numRadarObjects+i} = objectDetection(t,...            [object.position(1); object.velocity(1); object.position(2); 0], ...            'SensorIndex', 1, 'MeasurementNoise', visionMeasCov, ...            'MeasurementParameters', {1}, ...            'ObjectClassID', object.classification, ...            'ObjectAttributes', {object.id, object.size});    endendend

function postProcessedDetections = processRadar(postProcessedDetections, realRadarObjects, t)% Process the radar objects into objectDetection objectsnumRadarObjects = numel(realRadarObjects);if numRadarObjects    classToUse = class(realRadarObjects(1).position);    radarMeasCov = cast(diag([2,2,2,100]), classToUse);    % Process Radar Objects:    for i=1:numRadarObjects        object = realRadarObjects(i);        postProcessedDetections{i} = objectDetection(t, ...            [object.position(1); object.velocity(1); object.position(2); object.velocity(2)], ...            'SensorIndex', 2, 'MeasurementNoise', radarMeasCov, ...            'MeasurementParameters', {2}, ...            'ObjectAttributes', {object.id, object.status, object.amplitude, object.rangeMode});    endendend

function measurement = fcwmeas(state, sensorID)% The example measurements depend on the sensor type, which is reported by% the MeasurementParameters property of the objectDetection. The following% two sensorID values are used:%   sensorID=1: video objects, the measurement is [x;vx;y].%   sensorID=2: radar objects, the measurement is [x;vx;y;vy].% The state is:%   Constant velocity       state = [x;vx;y;vy]%   Constant turn           state = [x;vx;y;vy;omega]%   Constant acceleration   state = [x;vx;ax;y;vy;ay]     if numel(state) < 6 % Constant turn or constant velocity        switch sensorID            case 1 % video                measurement = [state(1:3); 0];            case 2 % radar                measurement = state(1:4);        end    else% Constant acceleration        switch sensorID            case 1 % video                measurement = [state(1:2); state(4); 0];            case 2 % radar                measurement = [state(1:2); state(4:5)];        end    endend

function jacobian = fcwmeasjac(state, sensorID)% The example measurements depend on the sensor type, which is reported by% the MeasurementParameters property of the objectDetection. We choose% sensorID=1 for video objects and sensorID=2 for radar objects.  The% following two sensorID values are used:%   sensorID=1: video objects, the measurement is [x;vx;y].%   sensorID=2: radar objects, the measurement is [x;vx;y;vy].% The state is:%   Constant velocity       state = [x;vx;y;vy]%   Constant turn           state = [x;vx;y;vy;omega]%   Constant acceleration   state = [x;vx;ax;y;vy;ay]     numStates = numel(state);    jacobian = zeros(4, numStates);     if numel(state) < 6 % Constant turn or constant velocity        switch sensorID            case 1 % video                jacobian(1,1) = 1;                jacobian(2,2) = 1;                jacobian(3,3) = 1;            case 2 % radar                jacobian(1,1) = 1;                jacobian(2,2) = 1;                jacobian(3,3) = 1;                jacobian(4,4) = 1;        end    else% Constant acceleration        switch sensorID            case 1 % video                jacobian(1,1) = 1;                jacobian(2,2) = 1;                jacobian(3,4) = 1;            case 2 % radar                jacobian(1,1) = 1;                jacobian(2,2) = 1;                jacobian(3,4) = 1;                jacobian(4,5) = 1;        end    endend