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);

    norm_weight = weight/norm(weight);

    for i = 1:length(apriori_state)


  • 5
    \$\begingroup\$ I would try on math or stats. Perhaps even quant, as I understand econometricians use this technique a lot. \$\endgroup\$
    – drxzcl
    Commented Feb 24, 2011 at 8:09
  • 1
    \$\begingroup\$ 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. \$\endgroup\$
    – Kortuk
    Commented Feb 24, 2011 at 13:07

1 Answer 1


I have a few comments about things to fix.

1st your for j = 1:length(observationx) doesn't seem to have an end. My guess is you should have this just before the line norm_weight = weight/norm(weight);.

2nd your apriori_state, rms, and weight change size every iteration. This can cause matlab to run very slow. In order to improve this you need to preallocate the variable. This can be done like so: apriori_state = zeros(size(resultx_1));

As far as actual functionality I don't see anything that pops at as being wrong. The math and stats site linked to in a comment might be able to find functionality errors faster then I can.

  • \$\begingroup\$ Hi,thanks for the reply. I guess I missed out the extra end when I copied it from MATLAB. Do you know if there is a separate approach to deal with time-correlated measurement/process noise? I haven't taken that into account and I wonder if it is the problem. Even in the case when the state is very close to the observation, I don't get satisfactory results. \$\endgroup\$
    – Imelza
    Commented Mar 17, 2011 at 4:56

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.