Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

correct

Covarianza de estado correcto y error de estado

Desde R2021b

Sintaxis

[associations,newLandmark] = correct(slamObj,measurement,measurementCovariance)

[associations,newLandmark] = correct(slamObj,measurement,measurementCovariance,varargin)

Descripción

[associations,newLandmark] = correct(slamObj,measurement,measurementCovariance) corrige el estado y su covarianza de estado asociada en función de measurement y measurementCovariance en el paso de tiempo actual. correct utiliza la función de asociación de datos especificada en la propiedad DataAssociationFcn del objeto ekfSLAM, slamObj, para asociar la medición a puntos de referencia y extraer nuevos puntos de referencia de la medición.

La función correct utiliza estas asociaciones para corregir el estado y la covarianza del estado asociado, y luego aumenta el estado con nuevos puntos de referencia.

[associations,newLandmark] = correct(slamObj,measurement,measurementCovariance,varargin) pasa todos los argumentos adicionales especificados en varargin a la propiedad subyacente DataAssociationFcn de slamObj.

Las primeras cuatro entradas a la propiedad DataAssociationFcn son la posición del punto de referencia, la covarianza de la posición del punto de referencia, la medición y la covarianza de la medición, seguidas de todos los argumentos en varargin.

Ejemplos

Realice SLAM de punto de referencia utilizando el filtro Kalman extendido

Cargue un conjunto de datos de pista de carreras que contenga el estado inicial del vehículo, la covarianza del estado inicial del vehículo, la covarianza del ruido del proceso, la entrada de control, el tamaño del paso de tiempo, la medición, la covarianza de la medición y los valores de la puerta de validación.

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

Cree un objeto ekfSLAM con estado inicial, covarianza de estado inicial y ruido de proceso.

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

Inicializa una variable para almacenar la pose.

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

Prediga el estado utilizando la entrada de control y el tamaño del paso de tiempo para la función de transición de estado. Luego, corrija el estado utilizando los datos de los puntos de referencia observados, la covarianza de medición y la puerta de validación para la función de asociación de datos.

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

Visualice el mapa creado.

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")

Argumentos de entrada

slamObj — Objeto EKF SLAM

objeto ekfSLAM

Objeto SLAM EKF, especificado como un objeto ekfSLAM.

measurement — Mediciones de punto de referencia en el medio ambiente.

matriz de N por K

Medidas de los puntos de referencia del entorno, especificadas como una matriz N por K. K es la dimensión de la medición. N es el número de mediciones.

Tipos de datos: single | double

measurementCovariance — Covarianza de medidas

Vector de K elementos | Matriz N*K-por-N*K

Covarianza de las mediciones, especificada como un vector de K elementos o una matriz de N*K por N*K. K es la dimensión de la medición. N es el número de mediciones. Cuando se especifica como vector, se utiliza el mismo valor de covarianza para todas las mediciones.

Tipos de datos: single | double

varargin — Lista de argumentos de entrada de longitud variable

lista separada por comas

Lista de argumentos de entrada de longitud variable, especificada como una lista separada por comas. Esta entrada se pasa directamente a la propiedad DataAssociationFcn de slamObj. Cuando usted llama:

correct(slamObj,measurement,measurementCovariance,arg1,arg2)

MATLAB^® básicamente llama a dataAssociationFcn como:

dataAssociationFcn(knownLandmarks,knownLandmarksCovariance, ... 
measurement,measurementCovariance,arg1,arg2)

Argumentos de salida

associations — Lista de asociaciones de puntos de referencia con mediciones.

matriz de P por 2

Lista de asociaciones de puntos de referencia con mediciones, devuelta como una matriz P por 2. P es el número de asociaciones. La primera columna de la matriz contiene los índices de los puntos de referencia asociados y la segunda columna contiene los índices de medición asociados.

newLandmark — Lista de índices de medidas que califican como nuevos puntos de referencia

Vector de Q elementos

Lista de índices de las mediciones que califican como nuevos puntos de referencia, devuelta como un vector de elementos Q. Q es el número de mediciones que califican como nuevos puntos de referencia.

Capacidades ampliadas

Generación de código C/C++
Genere código C y C++ mediante MATLAB® Coder™.

Historial de versiones

Introducido en R2021b

Consulte también

Objetos

• ekfSLAM

Funciones

• landmarkInfo | poseHistory | predict | removeLandmark | reset