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

stateEstimatorPF

Crear estimador de estado de filtro de partículas

Descripción

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

El algoritmo del filtro de partículas calcula la estimación del 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 actual del sensor para corregir la estimación del estado. El algoritmo redistribuye periódicamente, o vuelve a muestrear, las partículas en el espacio de estados 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 discreta de estas variables de estado. El conjunto de todas las partículas se utiliza para ayudar a determinar la estimación del estado final.

Puede aplicar el filtro de partículas a modelos de sistemas no lineales arbitrarios. El ruido de proceso y de 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:

• Flujo de trabajo del filtro de partículas

• Parámetros del filtro de partículas

Creación

Sintaxis

pf = stateEstimatorPF

Descripción

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

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

Propiedades

NumStateVariables — Número de variables de estado

3 (predeterminado) | escalar

Esta propiedad o parámetro es de solo lectura.

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

NumParticles — Número de partículas utilizadas en el filtro.

1000 (predeterminado) | escalar

Esta propiedad o parámetro es de solo lectura.

Número de partículas utilizadas en el filtro, especificadas como escalar. Puede especificar esta propiedad solo llamando al método initialize .

StateTransitionFcn — Función de callback para determinar la transición de estado entre los pasos del filtro de partículas

identificador de función

Función de callback para determinar la transición de estado entre los pasos del filtro de partículas, especificada 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 callback acepta al menos dos argumentos de entrada: el objeto stateEstimatorPF , pf, y las partículas en el paso de tiempo anterior, prevParticles. Estas partículas especificadas son las predictParticles devueltas por la llamada anterior del objeto. predictParticles y prevParticles son del mismo tamaño: NumParticles-por- NumStateVariables.

También puede usar varargin para pasar un número variable de argumentos de la función predict . Cuando usted llama:

predict(pf,arg1,arg2)

MATLAB^® esencialmente llama a stateTranstionFcn como:

stateTransitionFcn(pf,prevParticles,arg1,arg2)

MeasurementLikelihoodFcn — Función de callback que calcula la probabilidad de mediciones del sensor.

identificador de función

Función de callback que calcula la probabilidad de mediciones del sensor, especificada como un identificador de función. Una vez que una medición del sensor está disponible, esta función de callback calcula la probabilidad de que la medición sea consistente con la hipótesis de estado de cada partícula. Debe implementar esta función según su modelo de medición. La firma de la función es:

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

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

1. pf – El objeto stateEstimatorPF asociado

2. predictParticles – Las partículas que representan el estado previsto del sistema en el paso de tiempo actual como un arreglo de tamaño NumParticles-por- NumStateVariables

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

También puede usar varargin para pasar un número variable de argumentos. Estos argumentos son pasados ​​por la función correct . Cuando usted llama:

correct(pf,measurement,arg1,arg2)

MATLAB esencialmente llama a measurementLikelihoodFcn como:

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

La callback debe devolver exactamente una salida, likelihood, que es la probabilidad de la measurement dada para cada hipótesis de estado de partícula.

IsStateVariableCircular — Indicador si las variables de estado tienen una distribución circular

[0 0 0] (predeterminado) | arreglo lógica

Indicador si las variables de estado tienen una distribución circular, especificada como un arreglo 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 IsStateVariableCircular es un vector fila. Cada elemento del vector indica si la variable de estado asociada es circular. Si el objeto tiene solo una variable de estado, entonces IsStateVariableCircular es un escalar.

ResamplingPolicy — Configuración de políticas que determinan cuándo activar el remuestreo

objeto

Configuración de política que determina cuándo activar el remuestreo, especificada como un objeto. Puede activar el remuestreo a intervalos fijos o puede activarlo dinámicamente, según la cantidad de partículas efectivas. Consulte resamplingPolicyPF para obtener más información.

ResamplingMethod — Método utilizado para el remuestreo de partículas.

'multinomial' (predeterminado) | 'residual' | 'stratified' | 'systematic'

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

StateEstimationMethod — Método utilizado para la estimación del estado.

'mean' (predeterminado) | 'maxweight'

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

Particles — Arreglo de valores de partículas

NumParticles-por- NumStateVariables matriz

Arreglo de valores de partículas, especificada como una matriz NumParticles-por- NumStateVariables . Cada fila corresponde a la hipótesis de estado de una sola partícula.

Weights — Pesos de partículas

NumParticles-por-1 vector

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

State — Mejor estimación estatal

vector

Esta propiedad o parámetro es de solo lectura.

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

State Covariance — Covarianza del sistema corregida

N-por- N matriz | []

Esta propiedad o parámetro es de solo lectura.

Varianza del sistema corregida, devuelta como una matriz N-por- N , donde N es igual a NumStateVariables propiedad. El estado corregido se calcula en base a la propiedad StateEstimationMethod y la MeasurementLikelihoodFcn. Si especifica un método de estimación de estado que no admite covarianza, la propiedad se establece en [].

Funciones del objeto

initialize	Inicializar el estado del filtro de partículas.
getStateEstimate	Extraiga la mejor estimación del estado y la covarianza de las partículas
predict	Predecir el estado del robot en el próximo paso
correct	Ajustar la estimación del estado según la medición del sensor

Ejemplos

Predicción y corrección del filtro de partículas

Cree un objeto stateEstimatorPF y ejecute un paso de predicción y corrección para la estimación del estado. El filtro de partículas proporciona una estimación del estado previsto en función del valor de retorno de StateTransitionFcn. Luego corrige el estado en función de una medida determinada y el valor de retorno 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 del estado medio y el método de remuestreo sistemático.

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

Inicialice el filtro de partículas en el 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 un paso de predicción y uno 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

Estimar la posición del robot en un bucle usando un filtro de partículas

Utilice el objeto stateEstimatorPF para rastrear un robot mientras se mueve en un espacio 2-D. A la posición medida se le agrega ruido aleatorio. Usando predict y correct, rastree el robot según la medición y un modelo de movimiento supuesto.

Inicialice el filtro de partículas y especifique la función de transición de estado predeterminada, la función de probabilidad de medición y la política de remuestreo.

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

Muestre 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 la siga el punto. Cree un arreglo para almacenar la posición prevista y estimada. Definir 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;

Inicie el ciclo 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 basándose en una función de onda sinusoidal con ruido aleatorio agregado 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 versus 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 estimado 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 el seguimiento a lo largo del tiempo.

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

Historial de versiones

Introducido en R2016a

R2019b: stateEstimatorPF fue renombrado

El objeto stateEstimatorPF pasó a llamarse robotics.ParticleFilter. Utilice stateEstimatorPF para toda la creación de objetos.

Consulte también

resamplingPolicyPF | initialize | getStateEstimate | predict | correct

Temas

• Track a Car-Like Robot Using Particle Filter (Robotics System Toolbox)
• Parámetros del filtro de partículas
• Flujo de trabajo del filtro de partículas