Этот пример показывает, как выполнить предупреждение столкновения путем сплавления данных с датчиков зрения и радара для отслеживания объектов перед транспортным средством.
Предупреждение столкновения (FCW) является важной функцией в помощи драйверу и автоматизированных системах вождения, где цель состоит в том, чтобы обеспечить правильное, своевременное и надежное предупреждение драйвера перед предстоящим столкновением с транспортным средством перед. Для достижения этой цели транспортные средства оснащаются обращенными вперед датчиками зрения и радарами. Слияние датчиков требуется, чтобы увеличить вероятность точных предупреждений и минимизировать вероятность ложных предупреждений.
Для целей этого примера тестовый автомобиль (автомобиль , оборудованный датчиком) оснащался различными датчиками и регистрировались их выходы. Для этого примера использовались следующие датчики:
Датчик зрения, который предоставил списки наблюдаемых объектов с их классификацией и информацию о контурах маршрута. Списки объектов сообщались 10 раз в секунду. О контурах маршрута сообщалось 20 раз в секунду.
Радарный датчик со средним и длинным режимами области значений, который предоставлял списки неклассифицированных наблюдаемых объектов. Списки объектов сообщались 20 раз в секунду.
БИНС, который сообщил о скорости и скорости поворота автомобиля , оборудованного датчиком 20 раз в секунду.
Видеокамера, которая записала видеоклип сцены перед машиной. Примечание: Это видео не используется трекером и служит только для отображения результатов отслеживания на видео для верификации.
Процесс предоставления предупреждения о прямом столкновении включает следующие шаги:
Получите данные от датчиков.
Предохраните данные датчика, чтобы получить список дорожек, т.е. предполагаемых положений и скоростей объектов перед автомобилем.
Выдавать предупреждения на основе треков и критериев FCW. Критерии FCW основаны на процедуре тестирования Euro NCAP AEB и учитывают относительное расстояние и относительную скорость по отношению к объекту перед автомобилем.
Для получения дополнительной информации об отслеживании нескольких объектов см. Сопровождение нескольких объектов.
Визуализация в этом примере выполняется с помощью
и monoCamera
. Для краткости функции, которые создают и обновляют отображение, были перемещены в вспомогательные функции за пределами этого примера. Для получения дополнительной информации о том, как использовать эти отображения, смотрите Аннотации видео с использованием обнаружений в координатах транспортного средства и Визуализация покрытия датчика, обнаружений и дорожек.birdsEyePlot
Этот пример является скриптом, с основным телом, показанным здесь, и стандартными программами в виде локальных функций в следующих разделах. Для получения дополнительной информации о локальных функциях смотрите Добавление функций к скриптам.
% Set up the display [videoReader, videoDisplayHandle, bepPlotters, sensor] = helperCreateFCWDemoDisplay('01_city_c2s_fcw_10s.mp4', 'SensorConfigurationData.mat'); % Read the recorded detections file [visionObjects, radarObjects, inertialMeasurementUnit, laneReports, ... timeStep, numSteps] = readSensorRecordingsFile('01_city_c2s_fcw_10s_sensor.mat'); % An initial 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 car laneWidth = 3.6; % meters egoLane = struct('left', [0 0 laneWidth/2], 'right', [0 0 -laneWidth/2]); % Prepare some time variables time = 0; % Time since the beginning of the recording currentStep = 0; % Current timestep snapTime = 9.3; % The time to capture a snapshot of the display % Initialize the tracker [tracker, positionSelector, velocitySelector] = setupTracker(); while currentStep < numSteps && ishghandle(videoDisplayHandle) % Update scenario counters currentStep = currentStep + 1; time = time + timeStep; % Process the sensor detections as objectDetection inputs to the tracker [detections, laneBoundaries, egoLane] = processDetections(... visionObjects(currentStep), radarObjects(currentStep), ... inertialMeasurementUnit(currentStep), laneReports(currentStep), ... egoLane, time); % Using the list of objectDetections, return the tracks, updated to time confirmedTracks = updateTracks(tracker, detections, time); % Find the most important object and calculate the forward collision % warning mostImportantObject = findMostImportantObject(confirmedTracks, egoLane, positionSelector, velocitySelector); % Update video and birds-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; end end
The
отслеживает объекты вокруг автомобиля , оборудованного датчиком на основе списков объектов, сообщаемых датчиками зрения и радара. Путем сплавления информации с обоих датчиков вероятность ложного предупреждения столкновения уменьшается.multiObjectTracker
The setupTracker
функция возвращает multiObjectTracker
. При создании multiObjectTracker
, примите во внимание следующее:
FilterInitializationFcn
: Вероятные модели движения и измерения. В этом случае ожидается, что объекты будут иметь постоянное ускорение движения. Хотя для этой модели можно сконфигурировать линейный фильтр Калмана, initConstantAccelerationFilter
настраивает расширенный фильтр Калмана. Смотрите раздел 'Define a фильтр Калмана'.
AssignmentThreshold
: Как далеко могут падать обнаружения с трасс. Значение по умолчанию для этого параметра является 30. Если есть обнаружения, которые не назначены трекам, но должны быть, увеличьте это значение. Если есть обнаружения, которые назначаются трекам, которые находятся слишком далеко, уменьшите это значение. Этот пример использует 35.
DeletionThreshold
: Когда дорожка подтверждена, она не должна быть удалена при первом обновлении, чтобы никакое обнаружение не было назначено ей. Вместо этого он должен быть покрыт (предсказан), пока не станет ясно, что трек не получит никакой информации о датчике, чтобы обновить его. Логика состоит в том, что, если дорожка пропущена P из Q раз, она должна быть удалена. Значение по умолчанию для этого параметра является 5-out-of-5. При этом трекер вызывается 20 раз в секунду и датчиков два, поэтому модифицировать дефолт не приходится.
ConfirmationThreshold
: Параметры для подтверждения дорожки. Новая дорожка инициализируется с каждым неназначенным обнаружением. Некоторые из этих обнаружений могут быть ложными, поэтому все дорожки инициализируются следующим 'Tentative'
. Чтобы подтвердить дорожку, она должна быть обнаружена как минимум M раз в N обновлениях трекера. Выбор M и N зависит от видимости объектов. Этот пример использует по умолчанию 2 обнаружения из 3 обновлений.
Выходные выходы setupTracker
являются:
tracker
- The multiObjectTracker
настроенный для этого случая.
positionSelector
- Матрица, которая задает, какие элементы вектора State являются позицией: position = positionSelector * State
velocitySelector
- Матрица, которая задает, какие элементы вектора State являются скоростью: velocity = velocitySelector * State
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
The multiObjectTracker
заданный в предыдущем разделе, использует функцию инициализации фильтра, заданную в этом разделе, для создания фильтра Калмана (линейного, расширенного или неароматизированного). Этот фильтр затем используется для отслеживания каждого объекта вокруг автомобиля , оборудованного датчиком.
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); end end
Записанная информация должна быть обработана и отформатирована, прежде чем ее сможет использовать трекер. Это имеет следующие шаги:
Фильтрация ненужных обнаружений загромождения радара. Радар сообщает о многих объектах, которые соответствуют фиксированным объектам, которые включают: охранные рельсы, медиану дороги, дорожные знаки и т.д. Если эти обнаружения используются в отслеживании, они создают ложные дорожки фиксированных объектов на ребрах дороги и поэтому должны быть удалены перед вызовом трекера. Радиолокационные объекты считаются несогласованными, если они либо неподвижны перед автомобилем, либо движутся в его окрестностях.
Форматирование обнаружений как входных данных для трекера, т.е. массива
элементы. Смотрите objectDetection
processVideo
и processRadar
вспомогательные функции в конце этого примера.
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
Чтобы обновить трекер, вызовите updateTracks
метод со следующими входами:
tracker
- The multiObjectTracker
это было сконфигурировано ранее. Смотрите раздел 'Create the Multi-Object Tracker'.
detections
- Список objectDetection
объекты, созданные processDetections
time
- текущее время сценария.
Выходы трекера: struct
массив дорожек.
Самый важный объект (MIO) определяется как трасса, которая находится в ego-полосе и находится ближе всего перед автомобилем, то есть с наименьшим положительным значением x. Чтобы снизить вероятность ложных предупреждений, рассматриваются только подтвержденные дорожки.
После того, как MIO найден, вычисляется относительная скорость между автомобилем и MIO. Относительное расстояние и относительная скорость определяют предупреждение столкновения. Существует 3 случая FCW:
Сейф (зеленый): Нет автомобиля в эго-полосе (нет MIO), MIO удаляется от автомобиля, или расстояние до MIO остается постоянным.
Внимание (желтый): MIO движется ближе к автомобилю, но все еще находится на расстоянии выше расстояния FCW. Расстояние FCW вычисляется с помощью протокола Euro NCAP AEB Test Protocol. Обратите внимание, что это расстояние изменяется с относительной скоростью между MIO и автомобилем и больше, когда скорость закрытия выше.
Предупреждение (красный): MIO движется ближе к автомобилю, и его расстояние меньше, чем расстояние FCW,.
Протокол Euro NCAP AEB Test Protocol определяет следующее вычисление расстояния:
где:
- расстояние предупреждения столкновения.
- относительная скорость между двумя транспортными средствами.
- максимальное замедление, определяемое как 40% ускорения свободного падения.
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); end
Этот пример показал, как создать систему предупреждения столкновения для транспортного средства, оснащенного датчиками зрения, радара и БИНС. Он использовал objectDetection
объекты для передачи отчетов датчика в multiObjectTracker
объект, который сплавлял их и отслеживал объекты перед автомобилем , оборудованным датчиком.
Попробуйте использовать различные параметры для трекера, чтобы увидеть, как они влияют на качество отслеживания. Попробуйте изменить фильтр отслеживания, чтобы использовать trackingKF
или trackingUKF
или для определения другой модели движения, например, постоянной скорости или постоянного поворота. Наконец, можно попытаться задать свою собственную модель движения.
readSensorRecordingsFile Считывает записанные данные о датчике из файла
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 milliseconds numSteps = numel(visionObjects); % Number of recorded timesteps end
processLanes Преобразование сообщений о датчиках в parabolicLaneBoundary
переулки и поддерживает постоянную оценку ego lane
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 lane cond = (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 ones leftParams = egoLane.left; leftBoundaries = parabolicLaneBoundary(leftParams); leftBoundaries.Strength = 1; % Check the validity of the reported right lane cond = (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 ones rightParams = egoLane.right; rightBoundaries = parabolicLaneBoundary(rightParams); rightBoundaries.Strength = 1; laneBoundaries = [leftBoundaries, rightBoundaries]; end
findNonClutterRadarObjects Удаляет радиолокационные объекты, которые считаются частью загромождения
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
calculateGroundSpeed Вычисляет истинную скорость земли объекта, сообщаемого радаром, от относительной скорости и автомобиля , оборудованного датчиком скорости
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
processVideo Converts сообщил объекты зрения в список objectDetection
объекты
function postProcessedDetections = processVideo(postProcessedDetections, visionFrame, t) % Process the video objects into objectDetection objects numRadarObjects = 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}); end end end
processRadar Преобразует зарегистрированные радиолокационные объекты в список objectDetection
объекты
function postProcessedDetections = processRadar(postProcessedDetections, realRadarObjects, t) % Process the radar objects into objectDetection objects numRadarObjects = 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}); end end end
fcwmeas Функция измерения, используемая в этом примере предупреждения прямого столкновения
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 end end
fcwmeasjac Якобиан функции измерения, используемой в этом примере предупреждения столкновения
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 end end
birdsEyePlot
| monoCamera
| multiObjectTracker
| objectDetection
| trackingEKF
| trackingKF
| trackingUKF