i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

updated pf_filter.m

Browse Source
This commit is contained in:
matlabbe
2015-06-16 08:31:38 -04:00
parent 53f7719655
commit bc18d4bf7d
1 changed files with 13 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
Matlab/ParticleFilter/pf_filter.m
View File

@@ -1,17 +1,24 @@
function filtered = pf_filter(x, nParticles, noise, lambda) function filtered = pf_filter(x, nParticles, noise, lambda)
particles = zeros(nParticles,1) ; particles = ones(nParticles,1)*x(1) ;
weights = zeros(nParticles,1); weights = ones(nParticles,1);
filtered=zeros(1,length(x)); filtered=zeros(1,length(x));
for i = 1:length(x); for i = 1:length(x);
for j = 1:nParticles for j = 1:nParticles
rn = sqrt(-2.0*log(rand))*cos(2*pi*rand); % randn c++ rn = sqrt(-2.0*log(rand))*cos(2*pi*rand); % randn c++
particles(j) = particles(j) + noise*rn ; noisyP = particles(j) + noise*rn ;
dist = abs(particles(j) - x(i)); dist = abs(noisyP - x(i));
weights(j) = exp(-lambda*dist); tmp = exp(-lambda*dist);
if isfinite(tmp) && tmp > 0
particles(j) = noisyP;
weights(j) = tmp;
end
end end
weights = weights ./(sum(weights(:))); if sum(weights(:)) > 0
weights = weights ./sum(weights(:));
end
filtered(i) = weights'*particles; filtered(i) = weights'*particles;
particles = pf_resample(particles, weights); particles = pf_resample(particles, weights);
end end