Filtro de partículas en Matlab: ¿qué está fallando?

Question

He publicado este pregunta en Electronics.Stackexchange y alguien me dijo que sería mejor publicarla aquí.

Es una implementación del Filtro de Partículas usando MATLAB pero los resultados nunca siguen las observaciones. He cambiado la función de ponderación para ser gaussiano, pero todavía no sirve.

NOTA: No he tenido en cuenta que el ruido del proceso/medición está relacionado con el tiempo. ¿Cambiará esto significativamente la precisión de mi programa? Si es así, ¿cómo puedo corregirlo?

MI ANTIGUA PREGUNTA

Estoy trabajando en un experimento de filtro de partículas para la fusión de multisensores y acabo de programarlo en MATLAB. Sin embargo, obtengo precisiones muy bajas para mis valores finales. Además, he leído mucha literatura donde se habla de pdf de estado y observaciones, etc., pero mi conocimiento práctico es todavía extremadamente inestable, ya que he tenido ninguna formación formal en el filtrado / estimaciones bayesianas, etc.

He ideado mi algoritmo así:

1. Inicializar las partículas = Lo hago como una distribución gaussiana - 10 partículas

2. Mueve las 10 partículas hacia adelante utilizando la ecuación de transición de estado:

\begin{equation} X_{t+1} = A \times X_t + 0.1 \times rand() \end{equation} De momento sólo estoy inyectando ruido gaussiano.

1. A partir de la observación, calcula los pesos de las partículas. Hago esto como una raíz cuadrada de la diferencia entre el estado predicho y la observación. Por ejemplo, si mi acimut(a) = 40, pitch(p) = 3, roll(r)=4 en mi estado y en mi observación es a = 39, p = 3, r = 3, entonces hago rms = sqrt((40-39)^2 + (3-3)^2 + (4-3)^2). Entonces mi peso se asigna como 1/rms para que sea inversamente proporcional a la "distancia" entre la predicción y la observación

2. A continuación, normalizo estos pesos para obtener norm_weight = weight/norm(weight) para que su suma sea igual a uno.

3. Luego sigo adelante para todas las observaciones. Todavía no he incluido el remuestreo porque cuando ejecuto este experimento, no experimento ninguna degeneración, lo que también es muy desconcertante.

¿En qué me estoy equivocando? Me he dado cuenta de que no he "calculado" muchas de las ecuaciones bayesianas dadas en la literatura, es decir

\begin{equation} p(x|z_t) = p(z_t|x)\times p(x)/p(z) \end{equation} y tampoco sé dónde encaja aquí. ¿Puede alguien ayudarme, por favor?

Mi código de Matlab tiene el siguiente aspecto:

function resultx = particlefilter(resultx_1, observationx, A, noiseP)
  for j = 1:length(observationx)
    for i = 1:length(resultx_1)
      apriori_state{i} = A*resultx_1{i} + noiseP;
      rms(i) = sqrt((observationx{j}(1) - apriori_state{i}(1))^2 +
                     (observationx{j}(2) - apriori_state{i}(2))^2);
      weight(i) = 1/rms(i);
    end;
    norm_weight = weight/norm(weight);
    for i = 1:length(apriori_state)
      plot(apriori_state{i});
    end
    disp(rms);
    disp(norm_weight);
  end

Answer 1

El problema puede provenir de la relación de aptitud que utiliza en su código. La inversa de las distancias puede dar diferencias muy grandes en las probabilidades de remuestreo de la nueva generación de las muestras. Por ejemplo, dejemos que x representan a la rms valores en su diseño y que sean

x = [0.1 1 1 3 10 50]

Cuando se usan los inversos, eso lleva a

y = (1./x) ./ norm(1./x)
y = 
    0.9896    0.0990    0.0990    0.0330    0.0099    0.0020

Como ves, la primera muestra domina la siguiente generación. Puede converger a propuestas erróneas muy fácilmente. Como alternativa, puede utilizar una función de tipo sigmoide. Por ejemplo,

z = exp(-x) ./ norm(exp(-x))
z = 
    0.8659    0.3521    0.3521    0.0476    0.0000    0.0000

Ahora bien, las partículas de la próxima generación probablemente incluyan algunas muestras similares a las de la segunda y la tercera. Así, el esquema de generación será más robusto a las propuestas erróneas.

Editar : Además, después de dividir por las normas, la suma del resultado no es 1. Se puede utilizar sum en su lugar.

y = (1./x) ./ sum(1./x)
y = 
    0.8030    0.0803    0.0803    0.0268    0.0080    0.0016

z = exp(-x) ./ sum(exp(-x))
z =
    0.5353    0.2176    0.2176    0.0295    0.0000    0.0000