@Test
public void testCalculatePosition() {

    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);
    NicGeoPoint pos = pf.calculatePosition();
    assertEquals(4.f, pos.getX(), 0.2f);
    assertEquals(5.f, pos.getY(), 0.2f);
}