Предупреждение о прямом столкновении с помощью слияния датчика

Этот пример показывает, как выполнить предупреждение столкновения путем сплавления данных с датчиков зрения и радара для отслеживания объектов перед транспортным средством.

Обзор

Предупреждение столкновения (FCW) является важной функцией в помощи драйверу и автоматизированных системах вождения, где цель состоит в том, чтобы обеспечить правильное, своевременное и надежное предупреждение драйвера перед предстоящим столкновением с транспортным средством перед. Для достижения этой цели транспортные средства оснащаются обращенными вперед датчиками зрения и радарами. Слияние датчиков требуется, чтобы увеличить вероятность точных предупреждений и минимизировать вероятность ложных предупреждений.

Для целей этого примера тестовый автомобиль (автомобиль , оборудованный датчиком) оснащался различными датчиками и регистрировались их выходы. Для этого примера использовались следующие датчики:

  1. Датчик зрения, который предоставил списки наблюдаемых объектов с их классификацией и информацию о контурах маршрута. Списки объектов сообщались 10 раз в секунду. О контурах маршрута сообщалось 20 раз в секунду.

  2. Радарный датчик со средним и длинным режимами области значений, который предоставлял списки неклассифицированных наблюдаемых объектов. Списки объектов сообщались 20 раз в секунду.

  3. БИНС, который сообщил о скорости и скорости поворота автомобиля , оборудованного датчиком 20 раз в секунду.

  4. Видеокамера, которая записала видеоклип сцены перед машиной. Примечание: Это видео не используется трекером и служит только для отображения результатов отслеживания на видео для верификации.

Процесс предоставления предупреждения о прямом столкновении включает следующие шаги:

  1. Получите данные от датчиков.

  2. Предохраните данные датчика, чтобы получить список дорожек, т.е. предполагаемых положений и скоростей объектов перед автомобилем.

  3. Выдавать предупреждения на основе треков и критериев 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

Создайте Multi-Object Tracker

The multiObjectTracker отслеживает объекты вокруг автомобиля , оборудованного датчиком на основе списков объектов, сообщаемых датчиками зрения и радара. Путем сплавления информации с обоих датчиков вероятность ложного предупреждения столкновения уменьшается.

The setupTracker функция возвращает multiObjectTracker. При создании multiObjectTracker, примите во внимание следующее:

  1. FilterInitializationFcn: Вероятные модели движения и измерения. В этом случае ожидается, что объекты будут иметь постоянное ускорение движения. Хотя для этой модели можно сконфигурировать линейный фильтр Калмана, initConstantAccelerationFilter настраивает расширенный фильтр Калмана. Смотрите раздел 'Define a фильтр Калмана'.

  2. AssignmentThreshold: Как далеко могут падать обнаружения с трасс. Значение по умолчанию для этого параметра является 30. Если есть обнаружения, которые не назначены трекам, но должны быть, увеличьте это значение. Если есть обнаружения, которые назначаются трекам, которые находятся слишком далеко, уменьшите это значение. Этот пример использует 35.

  3. DeletionThreshold: Когда дорожка подтверждена, она не должна быть удалена при первом обновлении, чтобы никакое обнаружение не было назначено ей. Вместо этого он должен быть покрыт (предсказан), пока не станет ясно, что трек не получит никакой информации о датчике, чтобы обновить его. Логика состоит в том, что, если дорожка пропущена P из Q раз, она должна быть удалена. Значение по умолчанию для этого параметра является 5-out-of-5. При этом трекер вызывается 20 раз в секунду и датчиков два, поэтому модифицировать дефолт не приходится.

  4. 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

Обработка и форматирование обнаружений

Записанная информация должна быть обработана и отформатирована, прежде чем ее сможет использовать трекер. Это имеет следующие шаги:

  1. Фильтрация ненужных обнаружений загромождения радара. Радар сообщает о многих объектах, которые соответствуют фиксированным объектам, которые включают: охранные рельсы, медиану дороги, дорожные знаки и т.д. Если эти обнаружения используются в отслеживании, они создают ложные дорожки фиксированных объектов на ребрах дороги и поэтому должны быть удалены перед вызовом трекера. Радиолокационные объекты считаются несогласованными, если они либо неподвижны перед автомобилем, либо движутся в его окрестностях.

  2. Форматирование обнаружений как входных данных для трекера, т.е. массива 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 метод со следующими входами:

  1. tracker - The multiObjectTracker это было сконфигурировано ранее. Смотрите раздел 'Create the Multi-Object Tracker'.

  2. detections - Список objectDetection объекты, созданные processDetections

  3. time - текущее время сценария.

Выходы трекера: struct массив дорожек.

Найдите самый важный объект и выдайте предупреждение о прямом столкновении

Самый важный объект (MIO) определяется как трасса, которая находится в ego-полосе и находится ближе всего перед автомобилем, то есть с наименьшим положительным значением x. Чтобы снизить вероятность ложных предупреждений, рассматриваются только подтвержденные дорожки.

После того, как MIO найден, вычисляется относительная скорость между автомобилем и MIO. Относительное расстояние и относительная скорость определяют предупреждение столкновения. Существует 3 случая FCW:

  1. Сейф (зеленый): Нет автомобиля в эго-полосе (нет MIO), MIO удаляется от автомобиля, или расстояние до MIO остается постоянным.

  2. Внимание (желтый): MIO движется ближе к автомобилю, но все еще находится на расстоянии выше расстояния FCW. Расстояние FCW вычисляется с помощью протокола Euro NCAP AEB Test Protocol. Обратите внимание, что это расстояние изменяется с относительной скоростью между MIO и автомобилем и больше, когда скорость закрытия выше.

  3. Предупреждение (красный): MIO движется ближе к автомобилю, и его расстояние меньше, чем расстояние FCW,.$d_{FCW}$

Протокол Euro NCAP AEB Test Protocol определяет следующее вычисление расстояния:

$d_{FCW} = 1.2 * v_{rel} + \frac{v_{rel}^2}{2a_{max}}$

где:

$d_{FCW}$ - расстояние предупреждения столкновения.

$v_{rel}$ - относительная скорость между двумя транспортными средствами.

$a_{max}$ - максимальное замедление, определяемое как 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

См. также

Функции

Объекты

Похожие темы

Для просмотра документации необходимо авторизоваться на сайте