I have implemented particle filter using MATLAB. The main issue in this code is with the last step Calculation of Weights. I have used the formula given in Page No 178, Equation 48. But when I print the values it exceeds and provide me with NaN. I have tried to reduce these values by using some multiplication terms, but of no use. Here's The code.
pf = robotics.ParticleFilter;
for i = 1:length(t)
[Pred(i,:),Cov] = predict(pf);
measurement(i,:) = dot(i,:) + 0.1*(rand([1 2])-0.1/2);
Another code I was using for this purpose is this, but still having the same issue. I know that I have to apply distribution function to get weights, and in this case I have applied somewhat like Gaussian Distribution, but same issue remain. Kindly guide me how can I correct this
for i = 1:length(t) weight(1,:)=[0.0001 0.0001]; [robotPred(i,:),robotCov] = predict(pf); measurement(i,:) = dot(i,:) + 0.1*(rand([1 2])-0.1/2); sigma_x=std(measurement); weight(i+1,:)= exp( (measurement(i,:) - robotPred(i,:)).^2/sigma_x.^2)*weight(i,:);
In the last step of weight calculation. Kindly guide me how do I calculate the weights correctly.