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