void ParticleFilter::resetLocTo(float x, float y, float h,
LocNormalParams params)
{
// Reset the estimate
poseEstimate.set_x(x);
poseEstimate.set_y(y);
poseEstimate.set_h(h);
particles.clear();
float weight = 1.0f/parameters.numParticles;
for(int i = 0; i < parameters.numParticles; ++i)
{
// Get the new particles x,y, and h
float pX = sampleNormal(x, params.sigma_x);
float pY = sampleNormal(y, params.sigma_x);
float pH = sampleNormal(h, params.sigma_h);
Particle p(pX, pY, pH, weight);
particles.push_back(p);
}
}
double sampleNormal() {
double u = ((double) rand() / (RAND_MAX)) * 2 - 1;
double v = ((double) rand() / (RAND_MAX)) * 2 - 1;
double r = u * u + v * v;
if (r == 0 || r > 1) return sampleNormal();
double c = sqrt(-2 * log(r) / r);
return u * c;
}
void generate_tone (uint8_t *buffer, float amplitude, float freq, float phi, unsigned int seed) {
srand(seed);
for (size_t i=0; i<BUFFER_SIZE; i++) {
buffer[i]=(uint8_t)(128.0+amplitude*sin(phi+2*M_PI*freq*1e6*(i*DELTA_T))+0.5*sampleNormal());
}
}
void generate_white (uint8_t *buffer, float amplitude, unsigned int seed) {
srand(seed);
for (size_t i=0; i<BUFFER_SIZE; i++) {
buffer[i]=(uint8_t)(128.0+sampleNormal()*amplitude);
}
}
