MATLAB programming of Particle Filter - what is going wrong?

Question

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

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.