@Test public void testResampling() { pf = new ParticleFilterTestable(); pf.setNumParticles(4); Particle tmp = new Particle(1.f, 2.f, 0.25f); pf.particleList.add(tmp); tmp = new Particle(3.f, 4.f, 0.01f); pf.particleList.add(tmp); tmp = new Particle(5.f, 6.f, 0.73f); pf.particleList.add(tmp); tmp = new Particle(7.f, 8.f, 0.01f); pf.particleList.add(tmp); pf.sigmaSensor = 1.f; pf.setScale(1.f); pf.resampling(); assertEquals(1.1f, pf.particleList.get(0).getX(), 0.01f); assertEquals(5.1f, pf.particleList.get(1).getX(), 0.01f); assertEquals(5.1f, pf.particleList.get(2).getX(), 0.01f); assertEquals(5.1f, pf.particleList.get(3).getX(), 0.01f); }
public void resampling() { final List<Particle> tmpParticleList = new ArrayList<Particle>(); final double[] cdf = new double[numParticles]; cdf[0] = particleList.get(0).getWeight(); for (int i = 1; i < numParticles; i++) { cdf[i] = cdf[i-1] + particleList.get(i).getWeight(); } final double[] u = new double[numParticles+1]; do { u[0] = nextRandom() * 1.f/numParticles; } while(!(u[0] > 0)); int k = 0; for (int j = 0; j < numParticles; j++) { while (u[j] > cdf[k]) { k++; } final double x = particleList.get(k).getX() + nextGaussian() * sigmaSensor * scale; final double y = particleList.get(k).getY() + nextGaussian() * sigmaSensor * scale; tmpParticleList.add(new Particle(x, y, 1.f/numParticles)); u[j+1] = u[j] + 1.f/numParticles; } particleList = tmpParticleList; showParticles(); }
ParticleFilter { public void resampling() { final List<Particle> tmpParticleList = new ArrayList<Particle>(); final double[] cdf = new double[numParticles]; cdf[0] = particleList.get(0).getWeight(); for (int i = 1; i < numParticles; i++) { cdf[i] = cdf[i-1] + particleList.get(i).getWeight(); } final double[] u = new double[numParticles+1]; do { u[0] = nextRandom() * 1.f/numParticles; } while(!(u[0] > 0)); int k = 0; for (int j = 0; j < numParticles; j++) { while (u[j] > cdf[k]) { k++; } final double x = particleList.get(k).getX() + nextGaussian() * sigmaSensor * scale; final double y = particleList.get(k).getY() + nextGaussian() * sigmaSensor * scale; tmpParticleList.add(new Particle(x, y, 1.f/numParticles)); u[j+1] = u[j] + 1.f/numParticles; } particleList = tmpParticleList; showParticles(); } }
ParticleFilter { public void resampling() { final List<Particle> tmpParticleList = new ArrayList<Particle>(); final double[] cdf = new double[numParticles]; cdf[0] = particleList.get(0).getWeight(); for (int i = 1; i < numParticles; i++) { cdf[i] = cdf[i-1] + particleList.get(i).getWeight(); } final double[] u = new double[numParticles+1]; do { u[0] = nextRandom() * 1.f/numParticles; } while(!(u[0] > 0)); int k = 0; for (int j = 0; j < numParticles; j++) { while (u[j] > cdf[k]) { k++; } final double x = particleList.get(k).getX() + nextGaussian() * sigmaSensor * scale; final double y = particleList.get(k).getY() + nextGaussian() * sigmaSensor * scale; tmpParticleList.add(new Particle(x, y, 1.f/numParticles)); u[j+1] = u[j] + 1.f/numParticles; } particleList = tmpParticleList; showParticles(); } ParticleFilter(final Context ctx, UserPositionManager userPositionManager); }
ParticleFilter { public void resampling() { final List<Particle> tmpParticleList = new ArrayList<Particle>(); final double[] cdf = new double[numParticles]; cdf[0] = particleList.get(0).getWeight(); for (int i = 1; i < numParticles; i++) { cdf[i] = cdf[i-1] + particleList.get(i).getWeight(); } final double[] u = new double[numParticles+1]; do { u[0] = nextRandom() * 1.f/numParticles; } while(!(u[0] > 0)); int k = 0; for (int j = 0; j < numParticles; j++) { while (u[j] > cdf[k]) { k++; } final double x = particleList.get(k).getX() + nextGaussian() * sigmaSensor * scale; final double y = particleList.get(k).getY() + nextGaussian() * sigmaSensor * scale; tmpParticleList.add(new Particle(x, y, 1.f/numParticles)); u[j+1] = u[j] + 1.f/numParticles; } particleList = tmpParticleList; showParticles(); } ParticleFilter(final Context ctx, UserPositionManager userPositionManager); void initialize(double curX, double curY); void updateAction(double x, double y); void updateSensor(double x, double y); void resampling(); void clearParticles(); void setShowParticles(boolean enabled); void setNumParticles(int particles); void setScale(float scale); NicGeoPoint calculatePosition(); }
ParticleFilter { public void resampling() { final List<Particle> tmpParticleList = new ArrayList<Particle>(); final double[] cdf = new double[numParticles]; cdf[0] = particleList.get(0).getWeight(); for (int i = 1; i < numParticles; i++) { cdf[i] = cdf[i-1] + particleList.get(i).getWeight(); } final double[] u = new double[numParticles+1]; do { u[0] = nextRandom() * 1.f/numParticles; } while(!(u[0] > 0)); int k = 0; for (int j = 0; j < numParticles; j++) { while (u[j] > cdf[k]) { k++; } final double x = particleList.get(k).getX() + nextGaussian() * sigmaSensor * scale; final double y = particleList.get(k).getY() + nextGaussian() * sigmaSensor * scale; tmpParticleList.add(new Particle(x, y, 1.f/numParticles)); u[j+1] = u[j] + 1.f/numParticles; } particleList = tmpParticleList; showParticles(); } ParticleFilter(final Context ctx, UserPositionManager userPositionManager); void initialize(double curX, double curY); void updateAction(double x, double y); void updateSensor(double x, double y); void resampling(); void clearParticles(); void setShowParticles(boolean enabled); void setNumParticles(int particles); void setScale(float scale); NicGeoPoint calculatePosition();  }