В этом примере показано, как выполнить прямое предупреждение столкновения путем объединения данных от видения и радарных датчиков к отслеживаемым объектам перед транспортным средством.
Прямое предупреждение столкновения (FCW) является важной функцией в помощи драйвера и автоматизированных системах вождения, где цель состоит в том, чтобы обеспечить правильные, своевременные, и надежные предупреждения драйверу перед предстоящим столкновением с транспортным средством впереди. Чтобы достигнуть цели, транспортные средства оборудованы видением по ходу движения и радарными датчиками. Сплав датчика требуется, чтобы увеличивать вероятность точных предупреждений и минимизировать вероятность ложных предупреждений.
В целях этого примера тестовый автомобиль (автомобиль, оборудованный датчиком) был оборудован различными датчиками, и их выходные параметры были зарегистрированы. Датчики, используемые в этом примере, были:
Датчик видения, который предоставил спискам наблюдаемых объектов с их классификацией и информацией о контурах маршрута. О списках объектов сообщили 10 раз в секунду. О контурах маршрута сообщили 20 раз в секунду.
Радарный датчик со средним диапазоном и режимами дальнего действия, которые предоставили списки несекретных наблюдаемых объектов. О списках объектов сообщили 20 раз в секунду.
IMU, который сообщил о скорости и угловой скорости вращения автомобиля, оборудованного датчиком 20 раз в секунду.
Видеокамера, которая записала видеоклип сцены перед автомобилем. Примечание: Это видео не используется средством отслеживания и только служит, чтобы отобразить результаты отслеживания на видео для верификации.
Процесс обеспечения прямого столкновения, предупреждающего, включает следующие шаги:
Получите данные из датчиков.
Объедините данные о датчике, чтобы получить список дорожек, т.е. оцененные положения и скорости объектов перед автомобилем.
Предупреждения о проблемах на основе дорожек и критериев FCW. Критерии FCW основаны на Евро процедура тестирования NCAP AEB и учитывают относительное расстояние и относительную скорость к объекту перед автомобилем.
Для получения дополнительной информации об отслеживании нескольких объектов, смотрите Несколько Объект, Отслеживающий (Computer Vision Toolbox).
Визуализация в этом примере сделана с помощью monoCamerabirdsEyePlot
Этим примером является скрипт с основной частью, показанной здесь и стандартные программы помощника в форме локальных функций в разделах, которые следуют. Для получения дополнительной информации о локальных функциях, смотрите, Добавляют Функции к Скриптам (MATLAB).
% 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
multiObjectTracker
setupTracker функция возвращает multiObjectTracker. При создании multiObjectTracker, рассмотрите следующее:
FilterInitializationFcn: Вероятное движение и модели измерения. В этом случае объекты, как ожидают, будут иметь постоянное ускоряющее движение. Несмотря на то, что можно сконфигурировать линейный Фильтр Калмана для этой модели, initConstantAccelerationFilter конфигурирует расширенный Фильтр Калмана. Смотрите раздел 'Define a Kalman filter'.
AssignmentThreshold: Как далеко обнаружения могут упасть от дорожек. Значение по умолчанию для этого параметра равняется 30. Если существуют обнаружения, которые не присвоены дорожкам, но должны быть, увеличить это значение. Если существуют обнаружения, которые присвоены дорожкам, которые слишком далеки, уменьшают это значение. Этот пример использует 35.
DeletionThreshold: Когда дорожка подтверждена, она не должна быть удалена на первом обновлении, что никакое обнаружение не присвоено ей. Вместо этого это должно курсироваться (предсказанное), пока не ясно, что дорожка не получает информации датчика, чтобы обновить его. Логика - это, если дорожка пропущена P-out-of-_ времена Q_ она shold быть удаленной. Значение по умолчанию для этого параметра равняется 5 5. В этом случае средство отслеживания называется 20 раз в секунду и существует два датчика, таким образом, нет никакой потребности изменить значение по умолчанию.
ConfirmationThreshold: Параметры для подтверждения дорожки. Новый трек инициализируется каждым неприсвоенным обнаружением. Некоторые из этих обнаружений могут быть ложными, таким образом, все дорожки инициализируются как 'Tentative'. Чтобы подтвердить дорожку, это должно быть обнаружено, по крайней мере, M времена в обновлениях средства отслеживания N. Выбор M и N зависит от видимости объектов. Этот пример использует значение по умолчанию 2 обнаружений из 3 обновлений.
Выходные параметры setupTracker :
tracker - multiObjectTracker это сконфигурировано для этого случая.
positionSelector - Матрица A, которая задает, какими элементами Вектора состояния является положение: position = positionSelector * State
velocitySelector - Матрица A, которая задает, какими элементами Вектора состояния является скорость: 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
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
Записанная информация должна быть обработана и отформатирована, прежде чем она сможет использоваться средством отслеживания. Это имеет следующие шаги:
Отфильтровывание ненужного радара создает помехи обнаружениям. Показания радара много объектов, которые соответствуют фиксированным объектам, которые включают: поручни, дорожная медиана, дорожные знаки, и т.д. Если эти обнаружения используются в отслеживании, они создают ложные дорожки фиксированных объектов в ребрах дороги и поэтому должны быть удалены прежде, чем вызвать средство отслеживания. Радарные объекты рассматриваются непомехой, если они являются или стационарными перед автомобилем или перемещающийся в его близость.
Форматирование обнаружений, как введено к средству отслеживания, т.е. массиву objectDetectionprocessVideo и 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 - multiObjectTracker это было сконфигурировано ранее. Смотрите раздел 'Create the Multi-Object Tracker'.
detections - Список objectDetection объекты, который был создан processDetections
time - Текущее время сценария.
Выходом от средства отслеживания является struct массив дорожек.
Самый важный объект (MIO) задан как дорожка, которая находится в маршруте эго и является самой близкой перед автомобилем, т.е. с наименьшим положительным x значением. Чтобы понизить вероятность ложных предупреждений, только подтвержденные дорожки рассматриваются.
Если MIO найден, относительная скорость между автомобилем и MIO вычисляется. Относительное расстояние и относительная скорость определяют прямое предупреждение столкновения. Существует 3 случая FCW:
Безопасный (зеленый): нет никакого автомобиля в маршруте эго (никакой MIO), MIO переезжает от автомобиля, или расстояние до MIO остается постоянным.
(Желтая) осторожность: MIO придвигается поближе к автомобилю, но все еще на расстоянии выше расстояния FCW. Расстояние FCW вычисляется с помощью Евро Тестовый Протокол NCAP AEB. Обратите внимание на то, что это расстояние меняется в зависимости от относительной скорости между MIO и автомобилем, и больше, когда скорость сближения выше.
Предупредите (красный): MIO придвигается поближе к автомобилю, и его расстояние меньше расстояния FCW
.
Евро Тестовый Протокол NCAP AEB задает следующий расчет расстояния:
где:
прямое расстояние предупреждения столкновения.
относительная скорость между этими двумя транспортными средствами.
максимальное замедление, заданное, чтобы быть 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
Этот пример показал, как создать прямую систему оповещения столкновения для транспортного средства, оборудованного видением, радаром и датчиками IMU. Это использовало 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 маршруты и обеспечивают персистентную оценку маршрута эго
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 Преобразует объекты видения, о которых сообщают, в список 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