predict
Предскажите ошибочную ковариацию состояния и состояния
Описание
predict( предсказывает ошибочную ковариацию состояния и состояния. slamObj,controlInput)predict использует свойство StateTransitionFcn ekfSLAM объект, slamObj, и вход controlInput контроллера предсказать состояние.
predict( передачи все дополнительные аргументы заданы в slamObj,controlInput,varargin)varargin к базовому свойству StateTransitionFcn slamObj.
Первый вход к StateTransitionFcn положение от предыдущего временного шага, сопровождаемого всеми пользовательскими аргументами в varargin.
Примеры
Выполните ориентир SLAM Используя расширенный фильтр Калмана
Загрузите набор данных гоночной трассы, который содержит начальное состояние транспортного средства, начальную ковариацию состояния транспортного средства, ковариацию шума процесса, вход управления, размер временного шага, измерение, ковариацию измерения и значения логического элемента валидации.
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")
