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

ekfSLAM

Realice localización y mapeo simultáneos utilizando el filtro Kalman extendido

Desde R2021b

Descripción

El objeto ekfSLAM realiza localización y mapeo simultáneos (SLAM) utilizando un filtro de Kalman extendido (EKF). Toma puntos de referencia observados en el entorno y los compara con puntos de referencia conocidos para encontrar asociaciones y nuevos puntos de referencia. Utilice las asociaciones para corregir el estado y la covarianza del estado. Los nuevos puntos de referencia se aumentan en el vector estatal.

Creación

Sintaxis

slamObj = ekfSLAM

slamObj = ekfSLAM(Name,Value)

slamObj = ekfSLAM('MaxNumLandmark',N,Name,Value)

slamObj = ekfSLAM('MaxNumLandmark',N,‘MaxNumPoseStored’,M,Name,Value)

Descripción

slamObj = ekfSLAM crea un objeto EKF SLAM con propiedades predeterminadas.

slamObj = ekfSLAM(Name,Value) establece propiedades utilizando uno o más argumentos de par nombre-valor además de cualquier combinación de argumentos de entrada de sintaxis anteriores. Cualquier propiedad no especificada tendrá los valores predeterminados.

slamObj = ekfSLAM('MaxNumLandmark',N,Name,Value) especifica un límite superior en el número de puntos de referencia N permitidos en el vector de estado al generar código. Este límite en la cantidad de puntos de referencia se aplica solo al generar código.

slamObj = ekfSLAM('MaxNumLandmark',N,‘MaxNumPoseStored’,M,Name,Value) especifica el tamaño máximo del historial de pose M junto con el número máximo de puntos de referencia N en el vector de estado mientras se genera el código. Estos límites se aplican solo al generar código.

Propiedades

No se puede cambiar el valor de las propiedades State, StateCovariance, StateTransitionFcn y MaxNumLandmark después de se crea el objeto. Establezca el valor de estas propiedades como predeterminado o mientras crea el objeto.

State — vector de estado

[0; 0; 0] (predeterminado) | M-elemento vector columna

Vector de estado, especificado como un vector columna de elemento M .

Tipos de datos: single | double

StateCovariance — Covarianza del error de estimación del estado

eye(3) (predeterminado) | M-por- M matriz

Covarianza del error de estimación estatal, especificada como una matriz M-por- M . M es el número de estados en el vector de estados.

Tipos de datos: single | double

StateTransitionFcn — Función de transición de estado

nav.algs.velocityMotionModel (predeterminado) | identificador de función

Función de transición de estado, especificada como identificador de función. Esta función calcula el vector de estado en el paso de tiempo k a partir del vector de estado en el paso de tiempo k-1. La función puede tomar parámetros de entrada adicionales, como entradas de control o tamaño del paso de tiempo.

La función también calcula los jacobianos con respecto a la pose actual y la entrada del controlador. Si no se especifica, los jacobianos se calculan utilizando diferenciación numérica en cada llamada a la función predict . Este cálculo puede aumentar el tiempo de procesamiento y la inexactitud numérica.

La función considera el ruido del proceso no aditivo y debe tener esta firma:

[pose(k),jacPose,jacControl] = StateTransitionFcn(pose(k-1),controlInput,parameters)

• pose(k) es la pose estimada en el momento k.

• jacPose es el jacobiano de StateTransitionFcn con respecto a pose(k-1).

• jacControl es el jacobiano de StateTransitionFcn con respecto a controlInput.

• controlInput es la entrada para propagar el estado.

• parameters son los argumentos adicionales requeridos por la función de transición de estado.

Tipos de datos: function_handle

MeasurementFcn — Función de medición

nav.algs.rangeBearingMeasurement (predeterminado) | identificador de función

Función de medición, especificada como identificador de función. Esta función calcula un vector de medición de elemento N para un vector de estado de elemento M.

La función también calcula los jacobianos con respecto a la pose actual y la posición del punto de referencia. Si no se especifica, los jacobianos se calculan utilizando diferenciación numérica en cada llamada a la función correct . Este cálculo puede aumentar el tiempo de procesamiento y la inexactitud numérica.

La función considera el ruido de medición aditivo y debe tener esta firma:

[measurements(k),jacPose,jacLandmarks] = MeasurementFcn(pose(k),landmarks)

• pose(k) es la pose estimada en el momento k.

• measurements(k) es la medida estimada en el momento k.

• landmarks son las posiciones de los puntos de referencia.

• jacPose es el jacobiano de MeasurementFcn con respecto a pose(k).

• jacLandmarks es el jacobiano de MeasurementFcn con respecto a landmarks.

Tipos de datos: function_handle

InverseMeasurementFcn — Función de medición inversa

nav.algs.rangeBearingInverseMeasurement (predeterminado) | identificador de función

Función de medición inversa, especificada como identificador de función. Esta función calcula la posición del punto de referencia como un vector de estado de elemento M para un vector de medición de elemento N.

La función también calcula los jacobianos con respecto a la pose y medida actuales. Si no se especifica, los jacobianos se calculan utilizando diferenciación numérica en cada llamada a la función correct . Este cálculo puede aumentar el tiempo de procesamiento y la inexactitud numérica.

La función debe tener esta firma:

[landmarks(k),jacPose,jacMeasurements] = InverseMeasurementFcn(pose(k),measurements)

• pose(k) es la pose estimada en el momento k.

• landmarks(k) es la posición de punto de referencia en el momento k.

• measurements son los puntos de referencia observados en el momento k.

• jacPose es el jacobiano de InverseMeasurementFcn con respecto a pose(k).

• jacMeasurements es el jacobiano de InverseMeasurementFcn con respecto a measurements.

Tipos de datos: function_handle

DataAssociationFcn — Función de asociación de datos

nav.algs.associateMaxLikelihood (predeterminado) | identificador de función

Función de asociación de datos, especificada como identificador de función. Esta función asocia las medidas con los puntos de referencia ya disponibles en el vector de estado. La función puede tomar parámetros de entrada adicionales.

La función debe tener esta firma:

[associations,newLandmarks] = DataAssociationFcn(knownLandmarks,knownLandmarksCovariance,observedLandmarks,observedLandmarksCovariance,parameters)

• knownLandmarks son puntos de referencia conocidos en el mapa.

• knownLandmarksCovariance es la covarianza de knownLandmarks.

• observedLandmarks son los puntos de referencia observados en el entorno.

• observedLandmarksCovariance es la covarianza de observedLandmarks.

• parameters son argumentos adicionales necesarios.

• associations es una lista de asociaciones desde knownLandmarks hasta observedLandmarks.

• newLandmarks son los índices de observedLandmarks que califican como nuevos puntos de referencia.

Tipos de datos: function_handle

ProcessNoise — Covarianza del ruido del proceso

eye(2) (predeterminado) | W-por- W matriz

Covarianza del ruido del proceso, especificada como una matriz W-por- W . W es el número de términos de ruido del proceso.

Tipos de datos: single | double

MaxAssociationRange — Alcance máximo de puntos de referencia que se comprobarán para su asociación

inf (predeterminado) | número entero positivo

Rango máximo para comprobar la asociación de los puntos de referencia, especificado como un entero positivo.

Tipos de datos: single | double

MaxNumLandmark — Número máximo de puntos de referencia en el vector de estado

inf (predeterminado) | número entero positivo

Número máximo de puntos de referencia en el vector de estado, especificado como un entero positivo.

Tipos de datos: single | double

MaxNumPoseStored — Tamaño máximo del historial de poses.

inf (predeterminado) | número entero positivo

Tamaño máximo del historial de pose, especificado como un número entero positivo.

Tipos de datos: single | double

Funciones del objeto

copy	Crear una copia profunda del objeto EKF SLAM
correct	Covarianza de estado correcto y error de estado
landmarkInfo	Recuperar información de puntos de referencia
poseHistory	Recuperar el historial de poses corregido y previsto
predict	Predecir la covarianza de estado y error de estado
removeLandmark	Eliminar punto de referencia del vector de estado
reset	Restablecer la covarianza del error de estimación del estado y del estado

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

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

• lidarSLAM

Funciones

• copy | correct | landmarkInfo | poseHistory | predict | removeLandmark | reset

Temas

• SLAM basado en EKF con puntos de referencia