Правильная ошибочная ковариация состояния и состояния
[ корректирует состояние и его ковариацию ассоциированной страны на основе 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.
Загрузите набор данных гоночной трассы, который содержит начальное состояние транспортного средства, начальную ковариацию состояния транспортного средства, ковариацию шума процесса, вход управления, размер временного шага, измерение, ковариацию измерения и значения логического элемента валидации.
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")
