# MATLAB programming of Particle Filter - what is going wrong?

I'm working on a particle filter experiment for multi-sensor fusion and I just programmed it in MATLAB. However, I get very low accuracies for my final values. Plus, I read a lot of literature where they talk about pdf of state and observations etc. but my practical knowledge is still extremely shaky, since I've had no formal training in filtering/Bayesian estimates etc.

I have devised my algorithm like this:

1. Initialize particles = I'm doing it as a Gaussian distribution - 10 particles

2. Move the 10 particles forward using the state transition equation: X_t+1 = A*X_t + 0.1*rand() (only injecting Gaussian noise so far)

3. Using the observation, calculate the weights for the particles. I do this like a root mean square of the difference between predicted state and observation. For example, if my azimuth(a) = 40, pitch(p) = 3, roll(r)=4 in my state and in my observation it is a = 39, p = 3, r = 3, then I do rms = sqrt((40-39)^2 + (3-3)^2 + (4-3)^2). Then my weight is assigned as 1/rms in order for it to be inversely proportional to the 'distance' between the prediction and observation

4. Then I normalize these weights to get norm_weight = weight/norm(weight) so that their sum is equal to one.

5. Then I continue forward for all the observations. I have not included resampling yet because when I run this experiment, I do not experience any degeneracy, which is also very puzzling.

Where am I going wrong? I realized that I haven't 'computed' a lot of the Bayesian equations given in the literature i.e. p(x/z_t) = p(z_t/x)*p(x)/p(z) etc. and I don't know where it fits in here either. Can somebody please help me?

My Matlab code looks like this:

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
• I would try on math or stats. Perhaps even quant, as I understand econometricians use this technique a lot. Commented Feb 24, 2011 at 8:09
• give it time, someone will come through and be helpful, I would like to run your code, but I am absent matlab on my current computer. Commented Feb 24, 2011 at 13:07