Programación en MATLAB del filtro de partículas: ¿qué está fallando?

Estoy trabajando en un experimento de filtro de partículas para la fusión de sensores múltiples y lo acabo de programar en MATLAB. Sin embargo, obtengo precisiones muy bajas para mis valores finales. Además, leo mucha literatura en la que se habla de pdf de estado y observaciones, etc., pero mi conocimiento práctico aún es extremadamente inestable, ya que no he tenido capacitación formal en filtrado/estimaciones bayesianas, etc.

He ideado mi algoritmo así:

1. Inicializar partículas = lo estoy haciendo como una distribución gaussiana - 10 partículas

2. Mueva las 10 partículas hacia adelante usando la ecuación de transición de estado: X_t+1 = A*X_t + 0.1*rand() (hasta ahora solo inyecta ruido gaussiano)

3. Usando la observación, calcule los pesos de las partículas. Hago esto como una raíz cuadrada media de la diferencia entre el estado predicho y la observación. Por ejemplo, si mi azimut(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.

4. Luego normalizo estos pesos para obtener norm_weight = peso/norma(peso) para que su suma sea igual a uno.

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

¿Dónde me estoy equivocando? Me di cuenta de que no he "computado" muchas de las ecuaciones bayesianas que se dan en la literatura, es decir, p(x/z_t) = p(z_t/x)*p(x)/p(z), etc. Tampoco sé dónde encaja aquí. ¿Puede alguien por favor ayudarme?

Mi código Matlab se ve así:

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