Help with particle filter for measurement denoising

1 view (last 30 days)
I'm attempting to implement a simple particle filter for measurement denoising and compare the result with least mean squares.
The particle filter is tracking, more or less, the measurement but the performance is not good enough. What am I doing wrong?
clear all;
close all;
clc;
% 1000 measurements of a variable, presenting REAL VALUE 7 and corrupted
% by gaussian noise with standard deviation 3
M = 1000;
stddev = 3;
Y = randn(M,1)*stddev + 7;
% LEAST MEAN SQUARES
fi = ones(M,1); % constant
teta=(fi'*fi)\fi'*Y % RESULT
% PARTICLE FILTER
N = 10; % number of particles
P = randn(N,1)*1 + 1; % initial particle set with variance and value
Yhist = []; % History of the average value of Y
Phist = []; % History of the average value of P
for i = 1:M-N
% Particle generation
Ph = randn(N,1)*stddev + P;
% Weight calculated based on the measured error between the particle
% set and the current mean value of the measurement
Pw = normpdf(mean(Y(1:i))-Ph, 0, stddev);
% Weight normalized to form a probability distribution (i.e. sum to 1).
Pw = Pw./sum(Pw);
% Multinomial resampling
idx = randsample(N,N,true,Pw);
P(idx) = Ph(idx);
% Outputs
Yhist = [Yhist; mean(Y(1:i))];
Phist = [Phist; mean(P)];
end
% RESULT
mean(P)
figure;
plot(1:size(Phist,1),Phist,'k');
hold all;
plot(1:size(Phist,1),Yhist,'r');
legend('Particle Filter', 'Measurement');
return

Answers (0)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!