Загрузите набор данных

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

load("victoriaParkDataset.mat","controllerInput", ...
     "measurements","gpsLatLong","deadReckoning");

Настройте параметры

Задайте начальное состояние транспортного средства и ковариацию состояния.

initialState = [gpsLatLong(1,2) gpsLatLong(1,1) deg2rad(37)]';
initialCovar = eps*eye(3);

Задайте ковариацию шума процесса в скорости и держащихся командах.

sigmaVelocity = 2;      % [m/s]
sigmaSteer= deg2rad(6); % [rad]
processNoise = [sigmaVelocity^2 0; 0 sigmaSteer^2];

Задайте ковариацию измерения в области значений и подшипнике.

sigmaRange = 1;            % [m]
sigmaBearing = deg2rad(3); % [rad]
measCovar = [sigmaRange^2 sigmaBearing^2];

Укажите максимальный диапазон, в котором можно проверять ориентиры на ассоциацию.

maxSensorRange = 30; % [m]

Задайте размер временного шага, в который перемещается транспортное средство.

timeStep = 0.025; % [sec]

Задайте пороги для функции ассоциации данных, nav.algs.associateMaxLikelihood. Знаменательный порог отклонения является хи-квадратом ($${\chi }^2$$) табличное значение распределения для 95% корректирует ассоциацию.

landmarkRejectionThres = 5.991;  % maximum distance for association
landmarkAugmentationThres = 200; % minimum distance for creation of new landmark
validationGate = [landmarkRejectionThres landmarkAugmentationThres];

Установите флаг, который определяет, построить ли карту во время запуска.

plotOnTheRun = false;

Получите количество отсчетов от набора данных.

numSamples = size(controllerInput,1);

Инициализируйте фигуру или индикатор выполнения

Если живая опция графика включена, создайте новую фигуру и настройте указатели для графического вывода различных компонентов. В противном случае создайте панель ожидания, которая обновляется с пропорцией набора данных, который был выполнен. Установите панель ожидания так, чтобы нажатие на Cancel остановило происходящий расчет. Закрытие панели ожидания останавливает выполнение.

if plotOnTheRun
    [robotHandle,covarianceHandle,sensorHandle, ...
     observationHandle,landmarkHandle, ...
     deadRecHandle,estTrajHandle] = ...
        exampleHelperInitializeVisualizationEKFSLAM(initialState,gpsLatLong);
else
    waitBarHandle = waitbar(0,'Example initializing...', ...
        'Name','ekfSLAM example', ...
        'CreateCancelBtn','setappdata(gcbf,''canceling'',1)');
    setappdata(waitBarHandle,'canceling',0);
end

Настройте ekfSLAM

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

ekfSlamObj = ekfSLAM('State',initialState, ...
    'StateCovariance',initialCovar, ...
    'StateTransitionFcn',@exampleHelperVictoriaParkStateTransition);
ekfSlamObj.ProcessNoise = processNoise;
ekfSlamObj.MaxAssociationRange = maxSensorRange;

Основной цикл

Основной цикл состоит из этих первичных операций:

Предсказание

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

predict вызовы метода функция заданы в StateTransitionFcn свойство ekfSlamObj предсказать состояние транспортного средства. predict метод передает вход управления, другие необходимые входные параметры и текущее положение транспортного средства к StateTransitionFcn.

for count = 1:numSamples
    predict(ekfSlamObj,controllerInput(count,:),timeStep);

Знаменательная экстракция

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

    observedLandmarks = measurements{count};

Коррекция

ekfSLAM объект корректирует состояние на основе данного измерения, возвращая список совпадающих ориентиров и список новых ориентиров. correct метод использует функцию, заданную в DataAssociationFcn свойство ekfSlamObj сопоставлять наблюдаемые ориентиры или измерения к известным ориентирам. Функция ассоциации данных возвращает список ассоциаций и список новых ориентиров. correct метод использует ассоциации, чтобы обновить веру транспортного средства и карты с помощью корреляции между наблюдаемыми ориентирами и известными ориентирами. Кроме того, correct метод увеличивает положение и ковариацию любого нового ориентира в State и StateCovariance векторы, соответственно.

Если живая опция графика включена, обновите фигуру со сканами в текущее время.

  if ~isempty(observedLandmarks)
        correct(ekfSlamObj,observedLandmarks,measCovar,validationGate);
        % Update the corrected position of vehicle in the figure
        if plotOnTheRun
            exampleHelperUpdateScans(ekfSlamObj.State, ...
                              ekfSlamObj.MaxAssociationRange, ...
                              observedLandmarks, ...
                              sensorHandle,observationHandle);
        end
  end

Используйте drawnow обновить фигуру с текущим положением транспортного средства и всех известных ориентиров и их ковариации.

Если живой график не включен, и панель ожидания активна, то обновите часть выполняемого набора данных.

    if plotOnTheRun
        addpoints(deadRecHandle,deadReckoning(count,1),deadReckoning(count,2));
        addpoints(estTrajHandle,ekfSlamObj.State(1),ekfSlamObj.State(2));
        exampleHelperUpdateRobotAndLandmarks(ekfSlamObj.State,ekfSlamObj.StateCovariance, ...
            robotHandle,covarianceHandle,landmarkHandle);
        drawnow limitrate
    else
        % update the wait bar
        waitbar(count/numSamples,waitBarHandle, ...
            sprintf("%2.2f%% executed",count/numSamples*100));
        % Check for clicked Cancel button
        if getappdata(waitBarHandle,"canceling")
            break
        end
    end

end

Используйте delete функционируйте, чтобы закрыть панель ожидания, если все расчеты завершены.

if ~plotOnTheRun
    delete(waitBarHandle);
end

Визуализируйте карту

Создайте географический график с основной истиной из данных о GPS. Постройте траекторию транспортного средства, наряду со всеми обнаруженными ориентирами и их связанной ковариацией, на том же географическом графике.

% get the corrected and predicted poses
[corrPoses,predPoses] = poseHistory(ekfSlamObj);

% show the map
exampleHelperShowMap(ekfSlamObj.State,ekfSlamObj.StateCovariance, ...
                     gpsLatLong,corrPoses,predPoses);