ekfSLAM

Выполните одновременную локализацию и картографию с помощью расширенного Фильтра Калмана

расширьте все на странице

    Описание

    ekfSLAM объект выполняет одновременную локализацию и картографию (SLAM) с помощью расширенного фильтра Калмана (EKF). Это берет в наблюдаемых ориентирах средой и сравнивает их с известными ориентирами, чтобы найти ассоциации и новые ориентиры. Используйте ассоциации, чтобы откорректировать ковариацию состояния и состояния. Новые ориентиры увеличиваются в векторе состояния.

    Создание

    Синтаксис

    slamObj = ekfSLAM
    slamObj = ekfSLAM(Name,Value)
    slamObj = ekfSLAM('MaxNumLandmark',N,Name,Value)
    slamObj = ekfSLAM('MaxNumLandmark',N,‘MaxNumPoseStored’,M,Name,Value)

    Описание

    slamObj = ekfSLAM создает объект EKF SLAM со свойствами по умолчанию.

    пример

    slamObj = ekfSLAM(Name,Value) свойства наборов с помощью одного или нескольких аргументов пары "имя-значение" в дополнение к любой комбинации входных параметров от предыдущих синтаксисов. Любые незаданные свойства имеют значения по умолчанию.

    slamObj = ekfSLAM('MaxNumLandmark',N,Name,Value) задает верхнюю границу на количестве ориентиров N позволенный в векторе состояния при генерации кода. Этот предел на количестве ориентиров применяется только при генерации кода.

    slamObj = ekfSLAM('MaxNumLandmark',N,‘MaxNumPoseStored’,M,Name,Value) задает максимальный размер истории положения M наряду с максимальным количеством ориентиров N в векторе состояния при генерации кода. Эти пределы применяются только при генерации кода.

    Свойства

    развернуть все

    Вы не можете изменить значение свойств State, StateCovariance, StateTransitionFcn и MaxNumLandmark после того, как объект будет создан. Установите значение этих свойств как значение по умолчанию или при создании объекта.

    Вектор состояния в виде M - вектор-столбец элемента.

    Типы данных: single | double

    Ошибочная ковариация оценки состояния в виде M-by-M матрица. M является количеством состояний в векторе состояния.

    Типы данных: single | double

    Функция изменения состояния в виде указателя на функцию. Эта функция вычисляет вектор состояния на временном шаге k от вектора состояния на временном шаге k-1. Функция может взять дополнительные входные параметры, такие как входные параметры управления или размер временного шага.

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

    Функция рассматривает неаддитивный шум процесса и должна иметь эту подпись:

    [pose(k),jacPose,jacControl] = StateTransitionFcn(pose(k-1),controlInput,parameters)

    • pose(k) предполагаемое положение во время k.

    • jacPose якобиан StateTransitionFcn относительно pose(k-1).

    • jacControl якобиан StateTransitionFcn относительно controlInput.

    • controlInput вход для распространения состояния.

    • parameters любые дополнительные аргументы, требуемые функцией изменения состояния.

    Типы данных: function_handle

    Функция измерения в виде указателя на функцию. Эта функция вычисляет N - вектор измерения элемента для M - вектор состояния элемента.

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

    Функция рассматривает аддитивный шум измерения и должна иметь эту подпись:

    [measurements(k),jacPose,jacLandmarks] = MeasurementFcn(pose(k),landmarks)

    • pose(k) предполагаемое положение во время k.

    • measurements(k) предполагаемое измерение во время k.

    • landmarks положения ориентиров.

    • jacPose якобиан MeasurementFcn относительно pose(k).

    • jacLandmarks якобиан MeasurementFcn относительно landmarks.

    Типы данных: function_handle

    Обратная функция измерения в виде указателя на функцию. Эта функция вычисляет знаменательную позицию M - вектора состояния элемента для N - вектор измерения элемента.

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

    Функция должна иметь эту подпись:

    [landmarks(k),jacPose,jacMeasurements] = InverseMeasurementFcn(pose(k),measurements)

    • pose(k) предполагаемое положение во время k.

    • landmarks(k) знаменательное положение во время k.

    • measurements наблюдаемые ориентиры во время k.

    • jacPose якобиан InverseMeasurementFcn относительно pose(k).

    • jacMeasurements якобиан InverseMeasurementFcn относительно measurements.

    Типы данных: function_handle

    Ассоциация данных функционирует в виде указателя на функцию. Эта функция сопоставляет измерения с ориентирами, уже доступными в векторе состояния. Функция может взять дополнительные входные параметры.

    Функция должна иметь эту подпись:

    [associations,newLandmarks] = DataAssociationFcn(knownLandmarks,knownLandmarksCovariance,observedLandmarks,observedLandmarksCovariance,parameters)

    • knownLandmarks известные ориентиры в карте.

    • knownLandmarksCovariance ковариация knownLandmarks.

    • observedLandmarks наблюдаемые ориентиры в среде.

    • observedLandmarksCovariance ковариация observedLandmarks.

    • parameters любые дополнительные требуемые аргументы.

    • associations список ассоциаций от knownLandmarks к observedLandmarks.

    • newLandmarks индексы observedLandmarks это квалифицирует как новые ориентиры.

    Типы данных: function_handle

    Ковариация шума процесса в виде W-by-W матрица. W является количеством шумовых частей процесса.

    Типы данных: single | double

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

    Типы данных: single | double

    Максимальное количество ориентиров в векторе состояния в виде положительного целого числа.

    Типы данных: single | double

    Максимальный размер истории положения в виде положительного целого числа.

    Типы данных: single | double

    Функции объекта

    copyСоздайте глубокую копию объекта EKF SLAM
    correctПравильная ошибочная ковариация состояния и состояния
    landmarkInfoПолучите знаменательную информацию
    poseHistoryПолучите откорректированную и предсказанную историю положения
    predictПредскажите ошибочную ковариацию состояния и состояния
    removeLandmarkУдалите ориентир из вектора состояния
    resetСбросьте ошибочную ковариацию оценки состояния и оценки состояния

    Примеры

    свернуть все

    Скрипт Open Live Script

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

    load("racetrackDataset.mat","initialState","initialStateCovariance", ...
     "processNoise","controllerInputs","timeStep", ...
     "measurements","measCovar","validationGate");

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

    ekfSlamObj = ekfSLAM("State",initialState, ...
                     "StateCovariance",initialStateCovariance, ...
                     "ProcessNoise",processNoise);

    Инициализируйте переменную, чтобы сохранить положение.

    storedPose = nan(size(controllerInputs,1)+1,3);
storedPose(1,:) = ekfSlamObj.State(1:3);

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

    for count = 1:size(controllerInputs,1)
    % Predict the state
    predict(ekfSlamObj,controllerInputs(count,:),timeStep);
 
    % Get the landmarks in the environment
    observedLandmarks = measurements{count};
 
    % Correct the state
    if ~isempty(observedLandmarks)
        correct(ekfSlamObj,observedLandmarks, ...
                measCovar,validationGate);
    end
  
    % Log the estimated pose
    storedPose(count+1,:) = ekfSlamObj.State(1:3);
end

    Визуализируйте созданную карту.

    fig = figure;
figAx = axes(fig);
axis equal
grid minor
hold on
plot(figAx,storedPose(:,1),storedPose(:,2),"g.-")
landmarks = reshape(ekfSlamObj.State(4:end),2,[])';
plot(figAx,landmarks(:,1),landmarks(:,2),"m+")
plot(figAx,storedPose(1,1),storedPose(1,2),"k*")
plot(figAx,storedPose(end,1),storedPose(end,2),"rd")
legend("Robot trajectory","Landmarks","Start","End")

    Figure contains an axes object. The axes object contains 4 objects of type line. These objects represent Robot trajectory, Landmarks, Start, End.

    Расширенные возможности

    Генерация кода C/C++
    Генерация кода C и C++ с помощью MATLAB® Coder™.

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

    Объекты

    Функции

    Темы

    Введенный в R2021b

    Документация Navigation Toolbox

    Поддержка

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