Main Content

Esta página aún no se ha traducido para esta versión. Puede ver la versión más reciente de esta página en inglés.

stateEstimatorPF

Crear estimador de estado de filtro de partículas

Descripción

El objeto es un estimador de estado bayesiano recursivo que utiliza partículas discretas para aproximar la distribución posterior del estado estimado.stateEstimatorPF

El algoritmo de filtro de partículas calcula la estimación de estado de forma recursiva e implica dos pasos: predicción y corrección. El paso de predicción utiliza el estado anterior para predecir el estado actual en función de un modelo de sistema determinado. El paso de corrección utiliza la medición del sensor actual para corregir la estimación de estado. El algoritmo redistribuye periódicamente o remuestrea las partículas en el espacio de estado para que coincidan con la distribución posterior del estado estimado.

El estado estimado consta de variables de estado. Cada partícula representa una hipótesis de estado discreto de estas variables de estado. El conjunto de todas las partículas se utiliza para ayudar a determinar la estimación final del estado.

Puede aplicar el filtro de partículas a modelos de sistema no lineales arbitrarios. El ruido de proceso y medición puede seguir distribuciones arbitrarias no gaussianas.

Para obtener más información sobre el flujo de trabajo del filtro de partículas y la configuración de parámetros específicos, consulte:

Creación

Descripción

ejemplo

pf = stateEstimatorPF crea un objeto que permite la estimación de estado para un sistema simple con tres variables de estado. Utilice el initialize método para inicializar las partículas con una media conocida y covarianza o partículas distribuidas uniformemente dentro de los límites definidos. Para personalizar el sistema y los modelos de medición del filtro de partículas, modifique las propiedades y.StateTransitionFcnMeasurementLikelihoodFcn

Después de crear el objeto, utilice initialize para inicializar las propiedades y.NumStateVariablesNumParticles La función establece estas dos propiedades en función de las entradas.initialize

Propiedades

expandir todo

Esta propiedad es de solo lectura.

Número de variables de estado, especificadas como escalares. Esta propiedad se establece en función de las entradas a la initialize Método. El número de estados está implícito en función de las matrices especificadas para el estado inicial y la covarianza.

Esta propiedad es de solo lectura.

Número de partículas que se utilizan en el filtro, especificado como escalar. Puede especificar esta propiedad solo llamando al initialize Método.

Función de devolución de llamada para determinar la transición de estado entre los pasos del filtro de partículas, especificados como un identificador de función. La función de transición de estado evoluciona el estado del sistema para cada partícula. La firma de la función es:

function predictParticles = stateTransitionFcn(pf,prevParticles,varargin)

La función de devolución de llamada acepta al menos dos argumentos de entrada: el objeto, , y las partículas en el paso de tiempo anterior, .stateEstimatorPFpfprevParticles Estas partículas especificadas son la devolución de la llamada anterior del objeto. y tienen el mismo tamaño: -by- .predictParticlespredictParticlesprevParticlesNumParticlesNumStateVariables

También puede utilizar para pasar un número variable de argumentos de la función.vararginpredict Cuando llame:

predict(pf,arg1,arg2)

llamadas esencialmente como:MATLAB®stateTranstionFcn

stateTransitionFcn(pf,prevParticles,arg1,arg2)

Función de devolución de llamada que calcula la probabilidad de mediciones del sensor, especificada como un identificador de función. Una vez que hay una medición de sensor disponible, esta función de devolución de llamada calcula la probabilidad de que la medición sea coherente con la hipótesis de estado de cada partícula. Debe implementar esta función en función del modelo de medición. La firma de la función es:

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

La función de devolución de llamada acepta al menos tres argumentos de entrada:

  1. – El objeto asociadopfstateEstimatorPF

  2. – Las partículas que representan el estado del sistema previsto en el paso de tiempo actual como una matriz de tamaño -por-predictParticlesNumParticlesNumStateVariables

  3. – La medición del estado en el paso de tiempo actualmeasurement

También puede utilizar para pasar un número variable de argumentos.varargin La función pasa estos argumentos.correct Cuando llame:

correct(pf,measurement,arg1,arg2)

llamadas esencialmente como:MATLABmeasurementLikelihoodFcn

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

La devolución de llamada debe devolver exactamente una salida, , que es la probabilidad de lo dado para cada hipótesis de estado de partícula.likelihoodmeasurement

Indicador si las variables de estado tienen una distribución circular, especificada como una matriz lógica. Las distribuciones circulares (o angulares) utilizan una función de densidad de probabilidad con un rango de .[-pi,pi] Si el objeto tiene varias variables de estado, entonces es un vector de fila.IsStateVariableCircular Cada elemento vectorial indica si la variable de estado asociada es circular. Si el objeto solo tiene una variable de estado, es un escalar.IsStateVariableCircular

Configuración de directiva que determina cuándo desencadenar el remuestreo, especificada como un objeto. Puede desencadenar el remuestreo a intervalos fijos o puede activarlo dinámicamente, en función del número de partículas efectivas. Consulte para obtener más información.resamplingPolicyPF

Método utilizado para el remuestreo de partículas, especificado como , , , y .'multinomial''residual''stratified''systematic'

Método utilizado para la estimación de estado, especificado como y .'mean''maxweight'

Matriz de valores de partículas, especificada como matriz -by-.NumParticlesNumStateVariables Cada fila corresponde a la hipótesis de estado de una sola partícula.

Pesos de partículas, especificados como un vector -by-1.NumParticles Cada peso está asociado a la partícula en la misma fila de la propiedad.Particles

Esta propiedad es de solo lectura.

Mejor estimación de estado, devuelto como vector con longitud .NumStateVariables La estimación se extrae en función de la propiedad.StateEstimationMethod

Esta propiedad es de solo lectura.

Se ha corregido la varianza del sistema, devuelta como una matriz -by-, donde es igual a la propiedad.NNNNumStateVariables El estado corregido se calcula en función de la propiedad y el archivo .StateEstimationMethodMeasurementLikelihoodFcn Si especifica un método de estimación de estado que no admite la covarianza, la propiedad se establece en .[]

Funciones del objeto

initializeInitialize the state of the particle filter
getStateEstimateExtract best state estimate and covariance from particles
predictPredict state of robot in next time step
correctAdjust state estimate based on sensor measurement

Ejemplos

contraer todo

Cree un objeto y ejecute un paso de predicción y corrección para la estimación de estado.stateEstimatorPF El filtro de partículas proporciona una estimación de estado predicho basada en el valor devuelto de .StateTransitionFcn A continuación, corrige el estado en función de una medida determinada y el valor devuelto de .MeasurementLikelihoodFcn

Cree un filtro de partículas con los tres estados predeterminados.

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.'  

Especifique el método de estimación de estado medio y el método de remuestreo sistemático.

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

Inicializar el filtro de partículas en estado [4 1 9] con covarianza unitaria ( ).eye(3) Utilice 5000 partículas.

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

Suponiendo una medición [4.2 0.9 9], ejecute una predicción y un paso correcto.

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

Obtenga la mejor estimación de estado basada en el algoritmo.StateEstimationMethod

stateEst = getStateEstimate(pf)
stateEst = 1×3

    4.1562    0.9185    9.0202

Utilice el objeto para rastrear un robot mientras se mueve en un espacio 2D.stateEstimatorPF La posición medida tiene ruido aleatorio añadido. Usando y , rastrear el robot basado en la medición y en un modelo de movimiento asumido.predictcorrect

Inicializar el filtro de partículas y especificar la función de transición de estado predeterminada, la función de probabilidad de medición y la directiva de remuestreo.

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

Muestra 1000 partículas con una posición inicial de [0 0] y covarianza unitaria.

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

Antes de la estimación, defina una trayectoria de onda sinusoidal para que el punto lo siga. Cree una matriz para almacenar la posición pronosticada y estimada. Defina la amplitud del ruido.

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

Comience el bucle para predecir y corregir la posición estimada en función de las mediciones. El remuestreo de partículas se produce en función de la propiedad.ResamplingPolicy El robot se mueve en función de una función de onda sinusoidal con ruido aleatorio añadido a la medición.

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

Trazar la ruta real frente a la posición estimada. Los resultados reales pueden variar debido a la aleatoriedad de las distribuciones de partículas.

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 figura muestra qué tan cerca coincide el estado de estimación con la posición real del robot. Intente ajustar el número de partículas o especificar una posición inicial y una covarianza diferentes para ver cómo afecta al seguimiento a lo largo del tiempo.

Consideraciones de compatibilidad

expandir todo

Cambio de comportamiento en versión futura

Referencias

[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.

Capacidades ampliadas

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

Introducido en R2016a