void rl_act_gauss::act_sample( void ) {
// printf( "gauss: %d samples\n", num_samples );
// in this case, the parameter is the mean of the gaussian.
// XXX we could also learn the variance, eh?
for ( int i=0; i<num_samples; i++ ) {
vals[i] = sample_gaussian( &rng_state ) + params(0);
grad( 0 ) += vals[i] - params(0);
}
// printf( "Gauss gradient: " ); grad.Print();
}
// x,y,theta for a specific particle. z is the sonar reading.
float calculate_likelihood(float locx, float locy, float theta, float z)
{
//sentinel minimum and wall values
//minimum = closest wall so far.
//
float By = 0, Bx = 0, Ax = 0, Ay = 0, m = 0, minimum = -1; //wall = -1;
float dx = 0, dy = 0, numerator = 0, denominator = 0, interX = 0, interY = 0;
for (int wallIterator = 0; wallIterator < NUMBER_OF_WALLS; ++wallIterator)
{
Ax = wallAxArray[wallIterator];
Ay = wallAyArray[wallIterator];
Bx = wallBxArray[wallIterator];
By = wallByArray[wallIterator];
dx = Bx - Ax;
dy = By - Ay;
//calculate m
numerator = dy*(Ax-locx) - dx*(Ay-locy);
float thetaformatt = theta;
float cosval = cos(theta);
float sinval = sin(theta);
float sinvaldx = dx*sin(theta);
float cosvaldy = dy*cos(theta);
denominator = dy*cos(theta) - dx*sin(theta);
m = numerator/denominator;
if ((m > 0) && (minimum > m || minimum == -1))
{
//check bound
//round or truncate?
interX = (float)(locx + m*cos(theta));
interY = (float)(locy + m*sin(theta));
if (between(interX, Ax, Bx) && between(interY, Ay, By))
{
minimum = m;
//wall = i;
}
}
}
return sample_gaussian(z, minimum, 0.01, 1);
}
void BoxesSystem::update_state_and_particles(const mat& x_t, const mat& P_t, const mat& u_t, mat& x_tp1, mat& P_tp1) {
int M = P_t.n_cols;
x_tp1 = this->dynfunc(x_t, u_t);
// receive noisy measurement
mat z_tp1 = this->obsfunc(x_tp1, this->box_centers, sample_gaussian(zeros<mat>(N*R_DIM,1), .01*this->R));
mat W(M, 1, fill::zeros);
mat r(N*R_DIM, 1, fill::zeros);
// for each particle, weight by gauss_likelihood of that measurement given particle/agent observation
for(int m=0; m < M; ++m) {
mat z_particle = this->obsfunc(x_tp1, P_t.col(m), r);
mat e = z_particle - z_tp1;
W(m) = this->gauss_likelihood(e, this->R);
}
W = W / accu(W);
double sampling_noise = uniform(0, 1/double(M));
P_tp1 = this->low_variance_sampler(P_t, W, sampling_noise);
}
