Contenu principal

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

correct

Corriger la covariance de l'état et de l'erreur d'état

Depuis R2021b

    Description

    [associations,newLandmark] = correct(slamObj,measurement,measurementCovariance) corrige l'état et sa covariance d'état associée en fonction du measurement et du measurementCovariance au pas de temps actuel. correct utilise la fonction d'association de données spécifiée dans la propriété DataAssociationFcn de l'objet ekfSLAM , slamObj, pour associer la mesure à des repères et extraire de nouveaux repères de la mesure.

    La fonction correct utilise ces associations pour corriger l'état et la covariance d'état associée, puis augmente l'état avec de nouveaux repères.

    [associations,newLandmark] = correct(slamObj,measurement,measurementCovariance,varargin) transmet tous les arguments supplémentaires spécifiés dans varargin à la propriété DataAssociationFcn sous-jacente de slamObj.

    Les quatre premières entrées de la propriété DataAssociationFcn sont la position du point de repère, la covariance de la position du point de repère, la mesure et la covariance de la mesure, suivies de tous les arguments dans varargin.

    exemple

    Exemples

    réduire tout

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

    Figure contains an axes object. The axes object contains 4 objects of type line. One or more of the lines displays its values using only markers These objects represent Robot trajectory, Landmarks, Start, End.

    Arguments d'entrée

    réduire tout

    Objet EKF SLAM, spécifié comme objet ekfSLAM .

    Mesures des points de repère dans l'environnement, spécifiés sous la forme d'une matrice N-par- K . K est la dimension de la mesure. N est le nombre de mesures.

    Types de données : single | double

    Covariance des mesures, spécifiée comme un vecteur d'éléments K ou N* K-by- N* K matrice. K est la dimension de la mesure. N est le nombre de mesures. Lorsqu'elle est spécifiée sous forme de vecteur, la même valeur de covariance est utilisée pour toutes les mesures.

    Types de données : single | double

    Liste d’arguments d’entrée de longueur variable, spécifiée sous forme de liste séparée par des virgules. Cette entrée est transmise directement dans la propriété DataAssociationFcn de slamObj. Quand vous appelez:

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

    MATLAB® appelle essentiellement le dataAssociationFcn comme :

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

    Arguments de sortie

    réduire tout

    Liste des associations de points de repère aux mesures, renvoyées sous forme de matrice P-par-2. P est le nombre d’associations. La première colonne de la matrice contient les indices des amers associés, et la deuxième colonne contient les indices de mesure associés.

    Liste des indices des mesures considérées comme de nouveaux points de repère, renvoyés sous forme de vecteur d'éléments Q. Q est le nombre de mesures considérées comme de nouveaux points de repère.

    Capacités étendues

    développer tout

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

    Historique des versions

    Introduit dans R2021b