% 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; endendfunction [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); endfunction 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}); endendendfunction 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}); endendendfunction 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
联系客服
