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

stateEstimatorPF

Créer un estimateur d'état de filtre à particules

Description

L'objet stateEstimatorPF est un estimateur d'état bayésien récursif qui utilise des particules discrètes pour approximer la distribution a posteriori de l'état estimé.

L'algorithme du filtre à particules calcule l'estimation d'état de manière récursive et implique deux étapes : la prédiction et la correction. L'étape de prédiction utilise l'état précédent pour prédire l'état actuel sur la base d'un modèle de système donné. L'étape de correction utilise la mesure actuelle du capteur pour corriger l'estimation d'état. L'algorithme redistribue ou rééchantillonne périodiquement les particules dans l'espace d'état pour correspondre à la distribution postérieure de l'état estimé.

L'état estimé est constitué de variables d'état. Chaque particule représente une hypothèse d'état discrète de ces variables d'état. L’ensemble de toutes les particules est utilisé pour aider à déterminer l’estimation de l’état final.

Vous pouvez appliquer le filtre de particules à des modèles de systèmes non linéaires arbitraires. Le bruit de processus et de mesure peut suivre des distributions arbitraires non gaussiennes.

Pour plus d'informations sur le flux de travail du filtre à particules et la définition de paramètres spécifiques, voir :

• Particle Filter Workflow

• Particle Filter Parameters

Création

Syntaxe

pf = stateEstimatorPF

Description

pf = stateEstimatorPF crée un objet qui permet l'estimation de l'état d'un système simple avec trois variables d'état. Utilisez la méthode initialize pour initialiser les particules avec une moyenne et une covariance connues ou des particules uniformément distribuées dans des limites définies. Pour personnaliser le système et les modèles de mesure du filtre à particules, modifiez les propriétés StateTransitionFcn et MeasurementLikelihoodFcn .

Après avoir créé l'objet, utilisez initialize pour initialiser les propriétés NumStateVariables et NumParticles . La fonction initialize définit ces deux propriétés en fonction de vos entrées.

Propriétés

NumStateVariables — Nombre de variables d'état

3 (par défaut) | scalaire

Ce propriété est en lecture seule.

Nombre de variables d'état, spécifiées sous forme de scalaire. Cette propriété est définie en fonction des entrées de la méthode initialize . Le nombre d'états est implicite et basé sur les matrices spécifiées pour l'état initial et la covariance.

NumParticles — Nombre de particules utilisées dans le filtre

1000 (par défaut) | scalaire

Ce propriété est en lecture seule.

Nombre de particules utilisées dans le filtre, spécifié sous forme de scalaire. Vous pouvez spécifier cette propriété uniquement en appelant la méthode initialize .

StateTransitionFcn — Fonction de rappel pour déterminer la transition d'état entre les étapes du filtre à particules

poignée de fonction

Fonction de rappel pour déterminer la transition d'état entre les étapes du filtre à particules, spécifiée comme handle de fonction. La fonction de transition d'état fait évoluer l'état du système pour chaque particule. La signature de la fonction est :

function predictParticles = stateTransitionFcn(pf,prevParticles,varargin)

La fonction de rappel accepte au moins deux arguments d'entrée : l'objet stateEstimatorPF , pf, et les particules au pas de temps précédent, prevParticles. Ces particules spécifiées sont les predictParticles renvoyés par l'appel précédent de l'objet. predictParticles et prevParticles sont de la même taille : NumParticles-par- NumStateVariables.

Vous pouvez également utiliser varargin pour transmettre un nombre variable d'arguments à partir de la fonction predict . Quand vous appelez:

predict(pf,arg1,arg2)

MATLAB^® appelle essentiellement stateTranstionFcn comme :

stateTransitionFcn(pf,prevParticles,arg1,arg2)

MeasurementLikelihoodFcn — Fonction de rappel calculant la probabilité des mesures du capteur

poignée de fonction

Fonction de rappel calculant la probabilité des mesures du capteur, spécifiée comme handle de fonction. Une fois qu'une mesure de capteur est disponible, cette fonction de rappel calcule la probabilité que la mesure soit cohérente avec l'hypothèse d'état de chaque particule. Vous devez implémenter cette fonction en fonction de votre modèle de mesure. La signature de la fonction est :

function likelihood = measurementLikelihoodFcn(PF,predictParticles,measurement,varargin)

La fonction de rappel accepte au moins trois arguments d'entrée :

1. pf – L'objet stateEstimatorPF associé

2. predictParticles – Les particules qui représentent l'état prévu du système au pas de temps actuel sous la forme d'un tableau de tailles NumParticles-par- NumStateVariables

3. measurement – La mesure de l’état au pas de temps actuel

Vous pouvez également utiliser varargin pour transmettre un nombre variable d'arguments. Ces arguments sont passés par la fonction correct . Quand vous appelez:

correct(pf,measurement,arg1,arg2)

MATLAB appelle essentiellement measurementLikelihoodFcn comme :

measurementLikelihoodFcn(pf,predictParticles,measurement,arg1,arg2)

Le rappel doit renvoyer exactement une sortie, likelihood, qui est la probabilité du measurement donné pour chaque hypothèse d'état de particule.

IsStateVariableCircular — Indicateur si les variables d'état ont une distribution circulaire

[0 0 0] (par défaut) | tableau logique

Indicateur si les variables d'état ont une distribution circulaire, spécifiée sous forme de tableau logique. Les distributions circulaires (ou angulaires) utilisent une fonction de densité de probabilité avec une plage de [-pi,pi]. Si l'objet a plusieurs variables d'état, alors IsStateVariableCircular est un vecteur ligne. Chaque élément vectoriel indique si la variable d'état associée est circulaire. Si l'objet n'a qu'une seule variable d'état, alors IsStateVariableCircular est un scalaire.

ResamplingPolicy — Paramètres de stratégie qui déterminent quand déclencher le rééchantillonnage

objet

Paramètres de stratégie qui déterminent quand déclencher le rééchantillonnage, spécifiés en tant qu'objet. Vous pouvez déclencher le rééchantillonnage soit à intervalles fixes, soit de manière dynamique, en fonction du nombre de particules effectives. Voir resamplingPolicyPF pour plus d'informations.

ResamplingMethod — Méthode utilisée pour le rééchantillonnage des particules

'multinomial' (par défaut) | 'residual' | 'stratified' | 'systematic'

Méthode utilisée pour le rééchantillonnage des particules, spécifiée comme 'multinomial', 'residual', 'stratified' et 'systematic'.

StateEstimationMethod — Méthode utilisée pour l'estimation de l'état

'mean' (par défaut) | 'maxweight'

Méthode utilisée pour l'estimation de l'état, spécifiée comme 'mean' et 'maxweight'.

Particles — Tableau de valeurs de particules

NumParticles-par- NumStateVariables matrice

Tableau de valeurs de particules, spécifié sous la forme d'une matrice NumParticles-par- NumStateVariables . Chaque ligne correspond à l'hypothèse d'état d'une seule particule.

Weights — Poids des particules

NumParticles-par-1 vecteur

Poids des particules, spécifiés sous la forme d'un vecteur NumParticles-by-1. Chaque poids est associé à la particule de la même ligne dans la propriété Particles .

State — Meilleure estimation de l'état

vecteur

Ce propriété est en lecture seule.

Meilleure estimation d'état, renvoyée sous forme de vecteur de longueur NumStateVariables. L'estimation est extraite sur la base de la propriété StateEstimationMethod .

State Covariance — Covariance du système corrigée

N-par- N matrice | []

Ce propriété est en lecture seule.

Variation du système corrigée, renvoyée sous la forme d'une matrice N-par- N , où N est égal à NumStateVariables propriété. L'état corrigé est calculé en fonction de la propriété StateEstimationMethod et de la MeasurementLikelihoodFcn. Si vous spécifiez une méthode d'estimation d'état qui ne prend pas en charge la covariance, la propriété est définie sur [].

Fonctions d'objet

initialize	Initialiser l'état du filtre à particules
getStateEstimate	Extraire la meilleure estimation d'état et la covariance des particules
predict	Predict state of robot in next time step
correct	Adjust state estimate based on sensor measurement

Exemples

Prédiction et correction des filtres à particules

Créez un objet stateEstimatorPF et exécutez une étape de prédiction et de correction pour l'estimation de l'état. Le filtre à particules donne une estimation de l'état prédit basée sur la valeur de retour de StateTransitionFcn. Il corrige ensuite l'état en fonction d'une mesure donnée et de la valeur de retour de MeasurementLikelihoodFcn.

Créez un filtre à particules avec les trois états par défaut.

pf = stateEstimatorPF

pf = 
  stateEstimatorPF with properties:

           NumStateVariables: 3
                NumParticles: 1000
          StateTransitionFcn: @nav.algs.gaussianMotion
    MeasurementLikelihoodFcn: @nav.algs.fullStateMeasurement
     IsStateVariableCircular: [0 0 0]
            ResamplingPolicy: [1x1 resamplingPolicyPF]
            ResamplingMethod: 'multinomial'
       StateEstimationMethod: 'mean'
            StateOrientation: 'row'
                   Particles: [1000x3 double]
                     Weights: [1000x1 double]
                       State: 'Use the getStateEstimate function to see the value.'
             StateCovariance: 'Use the getStateEstimate function to see the value.'

Spécifiez la méthode d'estimation de l'état moyen et la méthode de rééchantillonnage systématique.

pf.StateEstimationMethod = 'mean';
pf.ResamplingMethod = 'systematic';

Initialisez le filtre à particules à l'état [4 1 9] avec une covariance unitaire (eye(3)). Utilisez 5 000 particules.

initialize(pf,5000,[4 1 9],eye(3));

En supposant une mesure [4.2 0.9 9], exécutez une étape prédite et une étape correcte.

[statePredicted,stateCov] = predict(pf);
[stateCorrected,stateCov] = correct(pf,[4.2 0.9 9]);

Obtenez la meilleure estimation d'état basée sur l'algorithme StateEstimationMethod .

stateEst = getStateEstimate(pf)

stateEst = 1×3

    4.1562    0.9185    9.0202

Estimer la position du robot dans une boucle à l'aide du filtre à particules

Utilisez l'objet stateEstimatorPF pour suivre un robot lorsqu'il se déplace dans un espace 2D. La position mesurée comporte un bruit aléatoire ajouté. À l'aide de predict et correct, suivez le robot en fonction de la mesure et d'un modèle de mouvement supposé.

Initialisez le filtre à particules et spécifiez la fonction de transition d'état par défaut, la fonction de vraisemblance de mesure et la politique de rééchantillonnage.

pf = stateEstimatorPF;
pf.StateEstimationMethod = 'mean';
pf.ResamplingMethod = 'systematic';

Échantillonnez 1 000 particules avec une position initiale de [0 0] et une covariance unitaire.

initialize(pf,1000,[0 0],eye(2));

Avant l’estimation, définissez un chemin d’onde sinusoïdale que le point doit suivre. Créez un tableau pour stocker la position prévue et estimée. Définir l'amplitude du bruit.

t = 0:0.1:4*pi;
dot = [t; sin(t)]';
robotPred = zeros(length(t),2);
robotCorrected = zeros(length(t),2);
noise = 0.1;

Commencez la boucle de prédiction et de correction de la position estimée en fonction des mesures. Le rééchantillonnage des particules se produit sur la base de la propriété ResamplingPolicy. Le robot se déplace sur la base d'une fonction d'onde sinusoïdale avec un bruit aléatoire ajouté à la mesure.

for i = 1:length(t)
    % Predict next position. Resample particles if necessary.
    [robotPred(i,:),robotCov] = predict(pf);
    % Generate dot measurement with random noise. This is
    % equivalent to the observation step.
    measurement(i,:) = dot(i,:) + noise*(rand([1 2])-noise/2);
    % Correct position based on the given measurement to get best estimation.
    % Actual dot position is not used. Store corrected position in data array.
    [robotCorrected(i,:),robotCov] = correct(pf,measurement(i,:));
end

Tracez le chemin réel par rapport à la position estimée. Les résultats réels peuvent varier en raison du caractère aléatoire de la distribution des particules.

plot(dot(:,1),dot(:,2),robotCorrected(:,1),robotCorrected(:,2),'or')
xlim([0 t(end)])
ylim([-1 1])
legend('Actual position','Estimated position')
grid on

La figure montre à quel point l'état estimé correspond à la position réelle du robot. Essayez de régler le nombre de particules ou de spécifier une position initiale et une covariance différentes pour voir comment cela affecte le suivi au fil du temps.

Références

[1] Arulampalam, M.S., S. Maskell, N. Gordon, and T. Clapp. "A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking." IEEE Transactions on Signal Processing. Vol. 50, No. 2, Feb 2002, pp. 174-188.

[2] Chen, Z. "Bayesian Filtering: From Kalman Filters to Particle Filters, and Beyond." Statistics. Vol. 182, No. 1, 2003, pp. 1-69.

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 R2016a

R2019b: stateEstimatorPF a été renommé

L'objet stateEstimatorPF a été renommé robotics.ParticleFilter. Utilisez stateEstimatorPF pour toute création d'objet.

Voir aussi

resamplingPolicyPF | initialize | getStateEstimate | predict | correct

Rubriques

• Track a Car-Like Robot Using Particle Filter (Robotics System Toolbox)
• Particle Filter Parameters
• Particle Filter Workflow