Передайте столкновение, предупреждающее Используя Fusion датчика

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

Обзор

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

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

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

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

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

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

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

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

  2. Объедините данные о датчике, чтобы получить список дорожек, i.e., оцененные положения и скорости объектов перед автомобилем.

  3. Предупреждения о проблемах на основе дорожек и критериев FCW. Критерии FCW основаны на Евро процедура тестирования NCAP AEB и учитывают относительное расстояние и относительную скорость к объекту перед автомобилем.

Для получения дополнительной информации об отслеживании нескольких объектов, смотрите Несколько Объект, Отслеживающий (Computer Vision Toolbox).

Визуализация в этом примере сделана с помощью 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

Создайте мультиобъектное средство отслеживания

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

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

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

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

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

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

Процесс и формат обнаружения

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

  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 - multiObjectTracker это было сконфигурировано ранее. Смотрите раздел 'Create the Multi-Object Tracker'.

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

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

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

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

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

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

  1. Безопасный (зеленый): нет никакого автомобиля в маршруте эго (никакой MIO), MIO переезжает от автомобиля, или расстояние до MIO остается постоянным.

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

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

Евро Тестовый Протокол NCAP AEB задает следующий расчет расстояния:

$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

Сводные данные

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

Попытайтесь использовать различные параметры для средства отслеживания, чтобы видеть, как они влияют на качество отслеживания. Попытайтесь изменить фильтр отслеживания, чтобы использовать trackingKF или trackingUKF, или задавать различную модель движения, e.g., постоянная скорость или постоянный поворот. Наконец, можно попытаться задать собственную модель движения.

Вспомогательные Функции

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

Смотрите также

Функции

Объекты

Похожие темы

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