Fusion от дорожки к дорожке для автомобильных приложений безопасности

В этом примере показано, как плавить дорожки от двух транспортных средств для того, чтобы обеспечить более всестороннюю оценку среды, которая видна каждому транспортному средству. Пример демонстрирует использование термофиксатора уровня дорожки и объектного формата данных дорожки. В этом примере вы используете ведущий сценарий и модели от Automated Driving Toolbox™ и отслеживания и отслеживаете модели сплава от Sensor Fusion and Tracking Toolbox™.

Мотивация

Автомобильные приложения безопасности используют сплав данных из различных систем датчика, смонтированных на транспортном средстве. Отдельные транспортные средства плавят обнаружения датчика или при помощи централизованного средства отслеживания или путем проявления более децентрализованного подхода и плавления дорожек, произведенных отдельными датчиками. В дополнение к сплаву данных о внутритранспортном средстве сплав данных из нескольких транспортных средств предоставляет дополнительные преимущества, которые включают лучшее покрытие, ситуативную осведомленность и безопасность. [1] Этот подход cочетания датчиков межтранспортного средства использует в своих интересах множество датчиков и предоставляет лучше страховую защиту к каждому транспортному средству, потому что это использует данные, обновленные датчиками на других транспортных средствах в области. Правительства и производители транспортного средства долго распознавали потребность поделиться информацией между транспортными средствами для того, чтобы увеличить автомобильную безопасность. Например, Сервис Dedicated Short-Range Communications (DSRC) был установлен, чтобы обеспечить коммуникационное обслуживание для совместного пользования информацией межтранспортного средства. [2]

В то время как cочетание датчиков через несколько транспортных средств выгодно, большинство транспортных средств требуется, чтобы выполнять определенным требованиям техники безопасности, даже если только внутренние датчики доступны. Поэтому транспортное средство, вероятно, будет оборудовано средством отслеживания и/или термофиксатором дорожки, которые обеспечивают ситуативную осведомленность на одном уровне транспортного средства. В результате предположение, сделанное в этом примере, - то, что транспортные средства совместно используют ситуативную осведомленность путем широковещательной передачи дорожек и выполнения сплава от дорожки к дорожке.

Этот пример демонстрирует преимущество плавления дорожек от двух транспортных средств, чтобы улучшить ситуативную осведомленность и безопасность. Обратите внимание на то, что этот пример не симулирует системы связи. Вместо этого пример принимает, что система связи обеспечивает полосу пропускания, требуемую передать дорожки между этими двумя транспортными средствами.

Архитектура от дорожки к дорожке

Следующая блок-схема изображает основные функции в этих двух транспортных средствах.

Транспортное средство 1 имеет два датчика, каждое обеспечение обнаружения к локальному средству отслеживания. Средство отслеживания использует обнаружения от локальных датчиков до отслеживаемых объектов и выводит эти локальные дорожки к термофиксатору дорожки транспортного средства. Транспортное средство 2 имеет один датчик, который кормит обнаружениями локальное средство отслеживания на транспортном средстве 2. Локальные дорожки от транспортного средства 2 являются входом к локальному термофиксатору дорожки на транспортном средстве 2.

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

В этом примере вы используете trackerJPDA Объект (Sensor Fusion and Tracking Toolbox) задать каждое средство отслеживания транспортного средства.

% Create trackers for each vehicle
v1Tracker = trackerJPDA('TrackerIndex',1, 'DeletionThreshold', [4 4]); % Vehicle 1 tracker
v2Tracker = trackerJPDA('TrackerIndex',2, 'DeletionThreshold', [4 4]); % Vehicle 2 tracker
posSelector = [1 0 0 0 0 0; 0 0 1 0 0 0];

Обратите внимание на то, что в этой архитектуре, сплавленные дорожки от одного транспортного средства используются, чтобы обновить сплавленные дорожки на другом транспортном средстве. Эти сплавленные дорожки затем широковещательно передаются назад к первому транспортному средству. Избежать распространения слуха, быть осторожным, как дорожки от другого транспортного средства используются, чтобы обновить термофиксатор дорожки.

Рассмотрите следующий пример распространения слуха: на некотором шаге обновления, транспортное средство 1 дорожка объект с помощью его внутренних датчиков. Транспортное средство 1 затем предохранители объектная дорожка и передачи это, чтобы транспортировать 2, который теперь плавит дорожку с ее собственными дорожками и узнает объект. До этой точки это - точно цель сплава от дорожки к дорожке: улучшать ситуативную осведомленность о транспортном средстве 2 с информацией от транспортного средства 1. Поскольку транспортное средство 2 теперь знает об объекте, оно начинает широковещательно передавать дорожку также, возможно, в пользу другого транспортного средства (не показанный в примере).

Однако транспортное средство 1 теперь получает информацию о дорожке от транспортного средства 2 об объекте что только транспортное средство 1 на самом деле дорожки. Так, термофиксатор дорожки на транспортном средстве 1 должен иметь в виду, что дорожки, которые это получает от транспортного средства 2 об этом объекте, на самом деле не содержат новой информации, обновленной независимым источником. Чтобы сделать различие между дорожками, которые содержат новую информацию и дорожки, которые только повторяют информацию, необходимо задать транспортное средство 2 как внешний источник к термофиксатору дорожки на транспортном средстве 1. Точно так же транспортное средство 1 должно быть задано как внешний источник к термофиксатору дорожки на транспортном средстве 2. Кроме того, необходимо задать только дорожки, которые обновляются термофиксатором дорожки на основе информации из внутреннего источника, как самосообщается. Путем выполнения так, термофиксатор дорожки в каждом транспортном средстве может проигнорировать обновления от дорожек, которые возвращаются назад и вперед между термофиксаторами дорожки без любой новой информации в них.

Локальное средство отслеживания каждого транспортного средства отслеживает объекты относительно системы координат транспортного средства, названной системой координат эго. Сплав от дорожки к дорожке сделан в системе координат сценария, которая является системой координат глобального уровня. Помощник egoToScenario функционируйте преобразовывает дорожки от системы координат эго до системы координат сценария. Точно так же функциональный scenarioToEgo преобразовывает дорожки от системы координат сценария до любой из систем координат эго. Оба преобразования используют StateParameters свойство objectTrack (Sensor Fusion and Tracking Toolbox) объекты. Обратите внимание на то, что, когда trackFuser объект вычисляет расстояние центральной дорожки (в системе координат сценария) к локальной дорожке (в любой системе координат), это использует StateParameters из локальной дорожки, чтобы выполнить координатное преобразование.

Достигнуть вышеупомянутого trackFuser определения, задайте следующие источники как fuserSourceConfiguration Объект (Sensor Fusion and Tracking Toolbox).

% Define sources for each vehicle
v1TrackerConfiguration = fuserSourceConfiguration('SourceIndex',1,'IsInternalSource',true, ...   % v1Tracker is internal to v1Fuser
    "CentralToLocalTransformFcn", @scenarioToEgo, 'LocalToCentralTransformFcn', @egoToScenario); % Coordinate transformation
v2FuserConfiguration = fuserSourceConfiguration('SourceIndex',4,'IsInternalSource',false);       % v2Fuser is external to v2Fuser
v1Sources = {v1TrackerConfiguration; v2FuserConfiguration};
v2TrackerConfiguration = fuserSourceConfiguration('SourceIndex',2,'IsInternalSource',true, ...   % v2Tracker is internal to v2Fuser
    "CentralToLocalTransformFcn", @scenarioToEgo, 'LocalToCentralTransformFcn', @egoToScenario); % Coordinate transformation
v1FuserConfiguration = fuserSourceConfiguration('SourceIndex',3,'IsInternalSource',false);       % v1Fuser is external to v2Fuser
v2Sources = {v2TrackerConfiguration; v1FuserConfiguration};

Можно теперь задать каждый термофиксатор дорожки транспортного средства как trackFuser Объект (Sensor Fusion and Tracking Toolbox).

stateParams = struct('Frame','Rectangular','Position',[0 0 0],'Velocity',[0 0 0]);
v1Fuser = trackFuser('FuserIndex',3,...
    'MaxNumSources',2,'SourceConfigurations',v1Sources,...
    'StateFusion','Intersection','DeletionThreshold',[3 3],...
    'StateParameters',stateParams);
v2Fuser = trackFuser('FuserIndex',4,...
    'MaxNumSources',2,'SourceConfigurations',v2Sources,'StateFusion',...
    'Intersection','DeletionThreshold',[3 3],...
    'StateParameters',stateParams);

% Initialize the following variables
fusedTracks1 = objectTrack.empty(0,1);
fusedTracks2 = objectTrack.empty(0,1);
wasFuser1Updated = false;
wasFuser2Updated = false;

Определение сценария

Следующий сценарий показывает два транспортных средства, едущие по улице. Транспортное средство 1 лидирует, оборудованное двумя перспективными датчиками: ближний радар и датчик видения. Транспортное средство 2, ведущие 10 метров позади транспортного средства 1, оборудовано радаром дальним. Правая сторона улицы содержит припаркованные транспортные средства. Пешеход стоит между транспортными средствами. Этот пешеход показывается точкой приблизительно в X = 60 метров.

Из-за короткого расстояния между транспортным средством 2 и транспортным средством 1, большая часть транспортного средства 2 радарных покрытия датчика закрываются транспортным средством 1. В результате большинство дорожек, которые обеспечивает термофиксатор дорожки на транспортном средстве 2, сначала инициализируется широковещательной передачей дорожек от транспортного средства 1.

% Create the drivingScenario object and the two vehicles
[scenario, vehicle1, vehicle2] = createDrivingScenario;

% Create all the sensors
[sensors, numSensors, attachedVehicle] = createSensors(scenario);

% Create display
[f,plotters] = createT2TDisplay(scenario, sensors, attachedVehicle);

Следующий график преследования замечен с точки зрения второго транспортного средства. Стрелка указывает на положение пешехода, который почти полностью закрывается припаркованными транспортными средствами и первым транспортным средством.

% Define each vehicle as a vehicle, sensors, a tracker, and plotters
v1 = struct('Actor', {vehicle1}, 'Sensors', {sensors(attachedVehicle==1)}, 'Tracker', {v1Tracker}, 'DetPlotter', {plotters.veh1DetPlotter}, 'TrkPlotter', {plotters.veh1TrkPlotter});
v2 = struct('Actor', {vehicle2}, 'Sensors', {sensors(attachedVehicle==2)}, 'Tracker', {v2Tracker}, 'DetPlotter', {plotters.veh2DetPlotter}, 'TrkPlotter', {plotters.veh2TrkPlotter});

Симуляция и результаты

Следующий код запускает симуляцию.

running = true;

% For repeatable results, set the random number seed
s = rng;
rng(2019)
snaptimes = [0.5, 2.6, 4.4, 6.3, inf];
snaps = cell(numel(snaptimes,1));
i = 1;
f.Visible = 'on';
while running && ishghandle(f)
    time  = scenario.SimulationTime;
    
    % Detect and track at the vehicle level
    [tracks1,wasTracker1Updated] = detectAndTrack(v1,time,posSelector);
    [tracks2,wasTracker2Updated] = detectAndTrack(v2,time,posSelector);
    
    % Keep the tracks from the previous fuser update
    oldFusedTracks1 = fusedTracks1;
    oldFusedTracks2 = fusedTracks2;
    
    % Update the fusers
    if wasTracker1Updated || wasFuser2Updated
        tracksToFuse1 = [tracks1;oldFusedTracks2];
        if isLocked(v1Fuser) || ~isempty(tracksToFuse1)
            [fusedTracks1,~,~,info1] = v1Fuser(tracksToFuse1,time);
            wasFuser1Updated = true;
            pos = getTrackPositions(fusedTracks1,posSelector);
            plotTrack(plotters.veh1FusePlotter,pos);
        else
            wasFuser1Updated = false;
            fusedTracks1 = objectTrack.empty(0,1);
        end
    else
        wasFuser1Updated = false;
        fusedTracks1 = objectTrack.empty(0,1);
    end
    if wasTracker2Updated || wasFuser1Updated
        tracksToFuse2 = [tracks2;oldFusedTracks1];
        if isLocked(v2Fuser) || ~isempty(tracksToFuse2)
            [fusedTracks2,~,~,info2] = v2Fuser(tracksToFuse2,time);
            wasFuser2Updated = true;
            pos = getTrackPositions(fusedTracks2,posSelector);
            ids = string([fusedTracks2.TrackID]');
            plotTrack(plotters.veh2FusePlotter,pos,ids);
        else
            wasFuser2Updated = false;
            fusedTracks2 = objectTrack.empty(0,1);
        end
    else
        wasFuser2Updated = false;
        fusedTracks2 = objectTrack.empty(0,1);
    end
    
    % Update the display
    updateT2TDisplay(plotters, scenario, sensors, attachedVehicle)
    
    % Advance the scenario one time step and exit the loop if the scenario is complete
    running = advance(scenario);
    
    % Snap a shot at required times
    if time >= snaptimes(i)
        snaps{i} = getframe(f);
        i = i + 1;
    end
end

Figure contains 2 axes objects and other objects of type uipanel. Axes object 1 contains 5 objects of type patch, line. These objects represent radar, vision, Detections, Local Tracks, Fuser Tracks. Axes object 2 contains 9 objects of type patch, line, text. These objects represent radar, Detections, Local Tracks, Fuser Tracks.

Рисунок показывает сцену и отслеживающие результаты в конце сценария.

Отслеживание в начале Симуляции

Когда симуляция начинается, транспортное средство 1 обнаруживает транспортные средства, припаркованные на правой стороне улицы, затем дорожки, сопоставленные с припаркованными транспортными средствами, подтверждены. В это время единственный объект обнаружил и прослеженный транспортным средством, 2 средства отслеживания являются транспортным средством 1 сразу перед ним. Однажды транспортное средство 1 термофиксатор дорожки подтверждает дорожки, это широковещательно передает их и транспортное средство, 2 термофиксатора дорожки плавят их. В результате транспортное средство 2 узнает припаркованные транспортные средства, прежде чем оно сможет обнаружить их самостоятельно.

showsnap(snaps, 1)

Figure Snap #1 contains an axes object. The axes object contains an object of type image.

Отслеживание Пешехода со стороны улицы

В то время как симуляция продолжается, транспортное средство 2 может обнаружить и отследить транспортные средства, припаркованные в стороне также, и плавит их с дорожками, прибывающими из транспортного средства 1. Транспортное средство 2 может обнаружить и отследить пешехода приблизительно 4 секунды в симуляцию и транспортное средство 2 предохранителя дорожка, сопоставленная с пешеходом приблизительно 4,4 секунды в симуляцию (см. снимок состояния 2). Однако это садится в транспортное средство 2 приблизительно за две секунды до того, как это сможет обнаружить и отследить пешехода своими собственными датчиками (см. снимок состояния 3). Эти две секунды могли оказать огромное влияние на безопасность пешехода, если бы тот пешеход начал пересекать улицу.

showsnap(snaps, 2)

Figure Snap #2 contains an axes object. The axes object contains an object of type image.

showsnap(snaps, 3)

Figure Snap #3 contains an axes object. The axes object contains an object of type image.

Предотвращение распространения слуха

Наконец, отметьте, как, когда транспортные средства передают объекты, и эти объекты выходят из своего поля зрения, сплавленные дорожки, сопоставленные с этими объектами, пропущены обоими средствами отслеживания (см. снимок состояния 4). Отбрасывание дорожек демонстрирует, что сплавленная широковещательная передача дорожек назад и вперед между этими двумя транспортными средствами не используется, чтобы распространить слухи.

showsnap(snaps, 4)

Figure Snap #4 contains an axes object. The axes object contains an object of type image.

% Restart the driving scenario to return the actors to their initial positions.
restart(scenario);

% Release all the sensor objects so they can be used again.
for sensorIndex = 1:numSensors
    release(sensors{sensorIndex});
end

% Return the random seed to its previous value
rng(s)

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

В этом примере вы видели, как сплав от дорожки к дорожке может улучшить ситуативную осведомленность и увеличить безопасность в автомобильных приложениях. Вы видели, как настроить trackFuser выполнять сплав от дорожки к дорожке и как задать источники или как внутренние или как внешние при помощи fuserSourceConfiguration объект. Путем выполнения так, вы избегаете распространения слуха и отслеживаете только сплавленные дорожки, которые, как действительно наблюдает каждое транспортное средство, обеспечены.

Ссылки

[1] Bharanidhar Duraisamy, Тило Шварц и Кристиан Уохлер, "Алгоритмы сплава уровня дорожки для автомобильных приложений безопасности", 2 013 Международных конференций по вопросам Signal Processing, Image Processing & Pattern Recognition, IEEE, 2013.

[2] Федеральная комиссия по связи, "Специализированное Коммуникационное обслуживание Малой дальности", https://www.fcc.gov/wireless/bureau-divisions/mobility-division/dedicated-short-range-communications-dsrc-service.

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

createDrivingScenario

function [scenario, egoVehicle, secondVehicle] = createDrivingScenario
% createDrivingScenario Returns the drivingScenario defined in the Designer

% Construct a drivingScenario object.
scenario = drivingScenario('SampleTime', 0.05);

% Add all road segments
roadCenters = [50.8 0.5 0; 253.4 1.5 0];
roadWidth = 12;
road(scenario, roadCenters, roadWidth);

roadCenters = [100.7 -100.6 0; 100.7 103.7 0];
road(scenario, roadCenters);

roadCenters = [201.1 -99.2 0; 199.7 99.5 0];
road(scenario, roadCenters);

% Add the ego vehicle
egoVehicle = vehicle(scenario, 'ClassID', 1, 'Position', [65.1 -0.9 0]);
waypoints = [71 -0.5 0; 148.7 -0.5 0];
speed = 12;
trajectory(egoVehicle, waypoints, speed);

% Add the second vehicle
secondVehicle = vehicle(scenario, 'ClassID', 1, 'Position', [55.1 -0.9 0]);
waypoints = [61 -0.5 0; 138.7 -0.5 0];
speed = 12;
trajectory(secondVehicle, waypoints, speed);

% Add the parked cars
vehicle(scenario, 'ClassID', 1, 'Position', [111.0 -3.6 0]);
vehicle(scenario, 'ClassID', 1, 'Position', [140.6 -3.6 0]);
vehicle(scenario, 'ClassID', 1, 'Position', [182.6 -3.6 0]);
vehicle(scenario, 'ClassID', 1, 'Position', [211.3 -4.1 0]);

% Add pedestrian
actor(scenario, 'ClassID', 4, 'Length', 0.5, 'Width', 0.5, ...
    'Height', 1.7, 'Position', [130.3 -2.7 0], 'RCSPattern', [-8 -8;-8 -8]);

% Add parked truck
vehicle(scenario, 'ClassID', 2, 'Length', 8.2, 'Width', 2.5, ...
    'Height', 3.5, 'Position', [117.5 -3.5 0]);
end

createSensors

function [sensors, numSensors, attachedVehicle] = createSensors(scenario)
% createSensors Returns all sensor objects to generate detections
% Units used in createSensors and createDrivingScenario
% Distance/Position - meters
% Speed             - meters/second
% Angles            - degrees
% RCS Pattern       - dBsm

% Assign into each sensor the physical and radar profiles for all actors
profiles = actorProfiles(scenario);
sensors{1} = radarDetectionGenerator('SensorIndex', 1, ...
    'SensorLocation', [3.7 0], 'MaxRange', 50, 'FieldOfView', [60 5], ...
    'ActorProfiles', profiles, 'HasOcclusion', true, 'HasFalseAlarms', false);
sensors{2} = visionDetectionGenerator('SensorIndex', 2, ...
    'MaxRange', 100, 'SensorLocation', [1.9 0], 'DetectorOutput', 'Objects only', ...
    'ActorProfiles', profiles);
sensors{3} = radarDetectionGenerator('SensorIndex', 3, ...
    'SensorLocation', [3.7 0], 'MaxRange', 120, 'FieldOfView', [30 5], ...
    'ActorProfiles', profiles, 'HasOcclusion', true, 'HasFalseAlarms', false);
attachedVehicle = [1;1;2];
numSensors = numel(sensors);
end

scenarioToEgo

function trackInEgo = scenarioToEgo(trackInScenario)
% Performs coordinate transformation from scenario to ego coordinates
% trackInScenario has StateParameters defined to transform it from scenario
% coordinates to ego coordinates
% We assume a constant velocity model with state [x;vx;y;vy;z;vz]
egoPosInScenario = trackInScenario.StateParameters.Position;
egoVelInScenario = trackInScenario.StateParameters.Velocity;
stateInScenario = trackInScenario.State;
stateShift = [egoPosInScenario(1);egoVelInScenario(1);egoPosInScenario(2);egoVelInScenario(2);egoPosInScenario(3);egoVelInScenario(3)];
stateInEgo = stateInScenario - stateShift;
trackInEgo = objectTrack('UpdateTime',trackInScenario.UpdateTime,'State',stateInEgo,'StateCovariance',trackInScenario.StateCovariance,'StateParameters',trackInScenario.StateParameters);
end

egoToScenario

function trackInScenario = egoToScenario(trackInEgo)
% Performs coordinate transformation from ego to scenario coordinates
% trackInEgo has StateParameters defined to transform it from ego
% coordinates to scenario coordinates
% We assume a constant velocity model with state [x;vx;y;vy;z;vz]
egoPosInScenario = trackInEgo.StateParameters.Position;
egoVelInScenario = trackInEgo.StateParameters.Velocity;
stateInScenario = trackInEgo.State;
stateShift = [egoPosInScenario(1);egoVelInScenario(1);egoPosInScenario(2);egoVelInScenario(2);egoPosInScenario(3);egoVelInScenario(3)];
stateInEgo = stateInScenario + stateShift;
trackInScenario = objectTrack('UpdateTime',trackInEgo.UpdateTime,'State',stateInEgo,'StateCovariance',trackInEgo.StateCovariance,'StateParameters',trackInEgo.StateParameters);
end

detectAndTrack

function [tracks,wasTrackerUpdated] = detectAndTrack(agent,time,posSelector)
% Create detections from the vehicle
poses = targetPoses(agent.Actor);
[detections,isValid] = vehicleDetections(agent.Actor.Position,agent.Sensors,poses,time,agent.DetPlotter);

% Update tracks for the vehicle
if isValid
    agent.Tracker.StateParameters = struct(...
        'Frame','Rectangular', ...
        'Position', agent.Actor.Position, ...
        'Velocity', agent.Actor.Velocity);
    tracks = agent.Tracker(detections,time);
    tracksInScenario = tracks;
    for i = 1:numel(tracks)
        tracksInScenario(i) = egoToScenario(tracks(i));
    end
    pos = getTrackPositions(tracksInScenario,posSelector);
    plotTrack(agent.TrkPlotter,pos)
    wasTrackerUpdated = true;
else
    tracks = objectTrack.empty(0,1);
    wasTrackerUpdated = false;
end
end

function [objectDetections,isValid] = vehicleDetections(position, sensors, poses, time, plotter)
% Provides the detections for each vehicle. 

numSensors = numel(sensors);
objectDetections = {};
isValidTime      = false(1, numSensors);

% Generate detections for each sensor
for sensorIndex = 1:numSensors
    sensor = sensors{sensorIndex};
    [objectDets, ~, isValidTime(sensorIndex)] = sensor(poses, time);
    objectDets = cellfun(@(d) setAtt(d), objectDets, 'UniformOutput', false);
    
    if isa(sensors{sensorIndex},'radarDetectionGenerator')
        objectDets = helperClusterDetections(objectDets, 5);
    end
    numObjects = numel(objectDets);
    objectDetections = [objectDetections; objectDets(1:numObjects)]; %#ok<AGROW>
end
isValid = any(isValidTime);

% Plot detections
if numel(objectDetections)>0
    detPos = cellfun(@(d)d.Measurement(1:2), objectDetections, 'UniformOutput', false);
    detPos = cell2mat(detPos')' + position(1:2);
    plotDetection(plotter, detPos);
end
end

function d = setAtt(d)
% Set the attributes to be struct
d.ObjectAttributes = struct;
end

function detectionClusters = helperClusterDetections(detections, vehicleSize)
% helperClusterDetections  Helper to cluster detections in the example
N = numel(detections);
distances = zeros(N);
for i = 1:N
    for j = i+1:N
        if detections{i}.SensorIndex == detections{j}.SensorIndex
            distances(i,j) = norm(detections{i}.Measurement(1:2) - detections{j}.Measurement(1:2));
        else
            distances(i,j) = inf;
        end
    end
end
leftToCheck = 1:N;
i = 0;
detectionClusters = cell(N,1);
while ~isempty(leftToCheck)    
    % Remove the detections that are in the same cluster as the one under
    % consideration
    underConsideration = leftToCheck(1);
    clusterInds = (distances(underConsideration, leftToCheck) < vehicleSize);
    detInds = leftToCheck(clusterInds);
    clusterDets = [detections{detInds}];
    clusterMeas = [clusterDets.Measurement];
    meas = mean(clusterMeas, 2);
    i = i + 1;
    detectionClusters{i} = detections{detInds(1)};
    detectionClusters{i}.Measurement = meas;
    leftToCheck(clusterInds) = [];    
end
detectionClusters(i+1:end) = [];

% Since the detections are now for clusters, modify the noise to represent
% that they are of the whole car
for i = 1:numel(detectionClusters)
    measNoise = eye(6);
    measNoise(1:2,1:2) = vehicleSize^2 * eye(2);
    measNoise(4:5,4:5) = eye(2) * vehicleSize^2;
    detectionClusters{i}.MeasurementNoise = measNoise;
end
end

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

Приложения

Объекты

Похожие темы

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