Cette page a été traduite par traduction automatique. Cliquez ici pour voir la dernière version en anglais.

ekfSLAM

Effectuez une localisation et un mappage simultanés à l'aide du filtre de Kalman étendu

Depuis R2021b

Description

L'objet ekfSLAM effectue une localisation et un mappage simultanés (SLAM) à l'aide d'un filtre de Kalman étendu (EKF). Il prend en compte les points de repère observés dans l'environnement et les compare avec des points de repère connus pour trouver des associations et de nouveaux points de repère. Utilisez les associations pour corriger l'état et la covariance de l'état. Les nouveaux points de repère sont augmentés dans le vecteur d'état.

Création

Syntaxe

slamObj = ekfSLAM

slamObj = ekfSLAM(Name,Value)

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

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

Description

slamObj = ekfSLAM crée un objet EKF SLAM avec des propriétés par défaut.

slamObj = ekfSLAM(Name,Value) définit les propriétés en utilisant un ou plusieurs arguments de paire nom-valeur en plus de toute combinaison d'arguments d'entrée des syntaxes précédentes. Toutes les propriétés non spécifiées ont des valeurs par défaut.

slamObj = ekfSLAM('MaxNumLandmark',N,Name,Value) spécifie une limite supérieure sur le nombre de points de repère N autorisés dans le vecteur d'état lors de la génération du code. Cette limite sur le nombre de points de repère s'applique uniquement lors de la génération de code.

slamObj = ekfSLAM('MaxNumLandmark',N,‘MaxNumPoseStored’,M,Name,Value) spécifie la taille maximale de l'historique de pose M ainsi que le nombre maximum de points de repère N dans le vecteur d'état lors de la génération du code. Ces limites s'appliquent uniquement lors de la génération de code.

Propriétés

Vous ne pouvez pas modifier la valeur des propriétés State, StateCovariance, StateTransitionFcn et MaxNumLandmark après la l'objet est créé. Définissez la valeur de ces propriétés par défaut ou lors de la création de l'objet.

State — Vecteur d'état

[0; 0; 0] (par défaut) | M-élément colonne vecteur

Vecteur d'état, spécifié comme vecteur de colonne d'élément M.

Types de données : single | double

StateCovariance — Covariance de l'erreur d'estimation de l'état

eye(3) (par défaut) | M-par- M matrice

Covariance de l'erreur d'estimation d'état, spécifiée sous la forme d'une matrice M-par- M . M est le nombre d’états dans le vecteur d’état.

Types de données : single | double

StateTransitionFcn — Fonction de transition d'état

nav.algs.velocityMotionModel (par défaut) | poignée de fonction

Fonction de transition d’état, spécifiée comme handle de fonction. Cette fonction calcule le vecteur d'état au pas de temps k à partir du vecteur d'état au pas de temps k-1. La fonction peut prendre des paramètres d'entrée supplémentaires, tels que les entrées de contrôle ou la taille du pas de temps.

La fonction calcule également les Jacobiens par rapport à la pose actuelle et à l'entrée du contrôleur. S'ils ne sont pas spécifiés, les Jacobiens sont calculés en utilisant la différenciation numérique à chaque appel à la fonction predict . Ce calcul peut augmenter le temps de traitement et l'imprécision numérique.

La fonction prend en compte le bruit de processus non additif et doit avoir cette signature :

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

• pose(k) est la pose estimée à l'instant k.

• jacPose est le jacobien de StateTransitionFcn par rapport à pose(k-1).

• jacControl est le jacobien de StateTransitionFcn par rapport à controlInput.

• controlInput est l’entrée pour propager l’état.

• parameters sont tous les arguments supplémentaires requis par la fonction de transition d'état.

Types de données : function_handle

MeasurementFcn — Fonction de mesure

nav.algs.rangeBearingMeasurement (par défaut) | poignée de fonction

Fonction de mesure, spécifiée comme poignée de fonction. Cette fonction calcule un vecteur de mesure d'élément N pour un vecteur d'état d'élément M.

La fonction calcule également les Jacobiens par rapport à la pose actuelle et à la position du point de repère. S'ils ne sont pas spécifiés, les Jacobiens sont calculés en utilisant la différenciation numérique à chaque appel à la fonction correct . Ce calcul peut augmenter le temps de traitement et l'imprécision numérique.

La fonction prend en compte le bruit de mesure additif et doit avoir cette signature :

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

• pose(k) est la pose estimée à l'instant k.

• measurements(k) est la mesure estimée à l'instant k.

• landmarks sont les positions des points de repère.

• jacPose est le jacobien de MeasurementFcn par rapport à pose(k).

• jacLandmarks est le jacobien de MeasurementFcn par rapport à landmarks.

Types de données : function_handle

InverseMeasurementFcn — Fonction de mesure inverse

nav.algs.rangeBearingInverseMeasurement (par défaut) | poignée de fonction

Fonction de mesure inverse, spécifiée comme poignée de fonction. Cette fonction calcule la position du point de repère en tant que vecteur d'état d'élément M pour un vecteur de mesure d'élément N.

La fonction calcule également les Jacobiens par rapport à la pose et à la mesure actuelles. S'ils ne sont pas spécifiés, les Jacobiens sont calculés en utilisant la différenciation numérique à chaque appel à la fonction correct . Ce calcul peut augmenter le temps de traitement et l'imprécision numérique.

La fonction doit avoir cette signature :

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

• pose(k) est la pose estimée à l'instant k.

• landmarks(k) est la position de repère à l'instant k.

• measurements sont les repères observés à l'instant k.

• jacPose est le jacobien de InverseMeasurementFcn par rapport à pose(k).

• jacMeasurements est le jacobien de InverseMeasurementFcn par rapport à measurements.

Types de données : function_handle

DataAssociationFcn — Fonction d'association de données

nav.algs.associateMaxLikelihood (par défaut) | poignée de fonction

Fonction d'association de données, spécifiée comme descripteur de fonction. Cette fonction associe les mesures aux amers déjà disponibles dans le vecteur d'état. La fonction peut prendre des paramètres d'entrée supplémentaires.

La fonction doit avoir cette signature :

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

• knownLandmarks sont des points de repère connus sur la carte.

• knownLandmarksCovariance est la covariance de knownLandmarks.

• observedLandmarks sont les repères observés dans l’environnement.

• observedLandmarksCovariance est la covariance de observedLandmarks.

• parameters sont les arguments supplémentaires requis.

• associations est une liste d'associations de knownLandmarks à observedLandmarks.

• newLandmarks sont les indices de observedLandmarks qui sont considérés comme de nouveaux repères.

Types de données : function_handle

ProcessNoise — Covariance du bruit de processus

eye(2) (par défaut) | W-par- W matrice

Covariance du bruit de processus, spécifiée sous la forme d'une matrice W-par- W . W est le nombre de termes de bruit de processus.

Types de données : single | double

MaxAssociationRange — Portée maximale des points de repère dont l'association doit être vérifiée

inf (par défaut) | entier positif

Plage maximale pour les points de repère dont l'association doit être vérifiée, spécifiée sous la forme d'un entier positif.

Types de données : single | double

MaxNumLandmark — Nombre maximum de points de repère dans le vecteur d'état

inf (par défaut) | entier positif

Nombre maximum de points de repère dans le vecteur d'état, spécifié sous la forme d'un entier positif.

Types de données : single | double

MaxNumPoseStored — Taille maximale de l'historique des poses

inf (par défaut) | entier positif

Taille maximale de l'historique des poses, spécifiée sous la forme d'un entier positif.

Types de données : single | double

Fonctions d'objet

copy	Créer une copie complète de l'objet EKF SLAM
landmarkInfo	Retrieve landmark information
landmarkInfo	Retrieve landmark information
poseHistory	Récupérer l'historique des poses corrigées et prédites
predict	Prédire l'état et la covariance des erreurs d'état
removeLandmark	Supprimer le point de repère du vecteur d'état
reset	Réinitialiser l'état et la covariance de l'erreur d'estimation de l'état

Exemples

Effectuer un SLAM historique à l'aide du filtre de Kalman étendu

Chargez un ensemble de données de piste de course contenant l'état initial du véhicule, la covariance de l'état initial du véhicule, la covariance du bruit de processus, l'entrée de commande, la taille du pas de temps, la mesure, la covariance de la mesure et les valeurs de la porte de validation.

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

Créez un objet ekfSLAM avec l'état initial, la covariance de l'état initial et le bruit de processus.

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

Initialisez une variable pour stocker la pose.

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

Prédisez l'état à l'aide de l'entrée de contrôle et de la taille du pas de temps pour la fonction de transition d'état. Ensuite, corrigez l’état à l’aide des données des points de repère observés, de la covariance des mesures et de la porte de validation pour la fonction d’association de données.

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

Visualisez la carte créée.

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

Capacités étendues

Génération de code C/C++
Générez du code C et C++ avec MATLAB® Coder™.

Historique des versions

Introduit dans R2021b

Voir aussi

Objets

• lidarSLAM

Fonctions

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

Rubriques

• EKF-Based Landmark SLAM