Estoy trabajando en un experimento de filtro de partículas para la fusión multisensor y lo programé en MATLAB. Sin embargo, obtengo precisiones muy bajas para mis valores finales. Además, leí mucha literatura donde se habla de pdf de estado y observaciones, etc. pero mi conocimiento práctico es todavía muy inestable, ya que no he recibido capacitación formal en filtrado / estimaciones bayesianas, etc.
He ideado mi algoritmo de esta manera:
-
Inicializar partículas = Lo estoy haciendo como una distribución gaussiana: 10 partículas
-
Mueve las 10 partículas hacia adelante usando la ecuación de transición de estado: X_t + 1 = A * X_t + 0.1 * rand () (solo se inyecta ruido gaussiano hasta ahora)
-
Usando la observación, calcula los pesos para 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 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
-
Luego normalizo estos pesos para obtener norm_weight = weight / norm (peso) de modo que su suma sea igual a uno.
-
Luego sigo adelante para todas las observaciones. No he incluido el remuestreo todavía porque cuando ejecuto este experimento, no experimento ninguna degeneración, lo que también es muy desconcertante.
¿A dónde me voy mal? Me di cuenta de que no he 'computado' muchas ecuaciones bayesianas dadas en la literatura, es decir, p (x / z_t) = p (z_t / x) * p (x) / p (z) etc. y no lo hago ' No sé dónde encaja aquí tampoco. ¿Puede alguien ayudarme por favor?
Mi código de Matlab tiene este 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