correct
Правильная ошибочная ковариация состояния и состояния
Синтаксис
Описание
[ корректирует состояние и его ковариацию ассоциированной страны на основе associations,newLandmark] = correct(slamObj,measurement,measurementCovariance)measurement и measurementCovariance на шаге текущего времени. correct использует функцию ассоциации данных, заданную в свойстве DataAssociationFcn ekfSLAM объект, slamObj, сопоставлять измерение к ориентирам и извлекать новые ориентиры из измерения.
correct функционируйте использует эти ассоциации, чтобы откорректировать ковариацию и ассоциированной страны состояния, затем увеличивает состояние с новыми ориентирами.
[ передачи все дополнительные аргументы заданы в associations,newLandmark] = correct(slamObj,measurement,measurementCovariance,varargin)varargin к базовому свойству DataAssociationFcn slamObj.
Первые четыре входных параметров к DataAssociationFcn свойство является знаменательным положением, знаменательной ковариацией положения, измерением и ковариацией измерения, сопровождаемой всеми аргументами в 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")
