void CGrid::computeLikelihood(const std::vector<PolarPose>& pose,
std::vector<float> &_true_likelihood,
std::vector<float> &_false_likelihood)
{
// float range_guassian_factor = cell_probability.human / normalDistribution(0.0, 0.0, stdev.range);
// float angle_guassian_factor = cell_probability.human / normalDistribution(0.0, 0.0, toRadian(stdev.angle));
for(size_t i = 0; i < grid_size; i++){
float range = map.cell.at(i).polar.range;
float angle = map.cell.at(i).polar.angle;
float detection_likelihood = cell_probability.unknown;
float miss_detection_likelihood = cell_probability.unknown;
float cell_prob = 0.0;
if(map.cell_inFOV.at(i)){
if(pose.empty()) miss_detection_likelihood = cell_probability.human; // ?????
else{
for(size_t p = 0; p < pose.size(); p++){
float Gr = (pose.at(p).range < 0.01) ? 1.0 : normalDistribution(range, pose.at(p).range, stdev.range);
float Ga = normalDistribution(angle, pose.at(p).angle, angles::from_degrees(stdev.angle));
cell_prob += Gr * Ga;
}
detection_likelihood = (((cell_prob)/(pose.size()) < cell_probability.free) ? cell_probability.free : (cell_prob)/(pose.size()));
}
}
_true_likelihood[i] = (detection_likelihood * TARGET_DETECTION_PROBABILITY_) + (miss_detection_likelihood * (1.0 - TARGET_DETECTION_PROBABILITY_));
_false_likelihood[i] = (detection_likelihood * FALSE_DETECTION_PROBABILITY_) + (miss_detection_likelihood * (1.0 - FALSE_DETECTION_PROBABILITY_));
}
}
double BlacksFormula(double f, double P, double L, double Rcap, double
vol, double tau, double dtau)
{
double d1 = (log(f) / Rcap) + ((vol*vol)/2)*tau/(vol*sqrt(tau));
double d2 = d1 - vol*sqrt(tau);
return P*dtau*L*(-f*normalDistribution(-d2) + Rcap*normalDistribution(-d1));
}
/*! @brief Generates a new set of parameters to be tested based on base_parameters and basedelta_parameters
*/
void EHCLSOptimiser::mutateParameters(std::vector<Parameter>& base_parameters, std::vector<float>& basedelta_parameters, std::vector<Parameter>& parameters)
{
// generate phi to mutate the BestParameters
float sigma = m_neta*exp(m_count_since_last_improvement/m_reset_limit - 1); // 0.04
std::vector<float> phi(base_parameters.size(), 0);
for (size_t i=0; i<base_parameters.size(); i++)
phi[i] = normalDistribution(0, sigma);
// mutate the BestParameters
std::vector<float> mutant;
mutant.reserve(base_parameters.size());
for (size_t i=0; i<base_parameters.size(); i++)
mutant.push_back(base_parameters[i] + phi[i]*(base_parameters[i].max() - base_parameters[i].min()));
// calculate the difference between the mutated state and the best one
std::vector<float> deltamutant = mutant - base_parameters;
// calculate the desired change in parameters
std::vector<float> deltaparameters = m_alpha*basedelta_parameters + (1-m_alpha)*deltamutant;
// now calculate the new parameters themselves
parameters.resize(base_parameters.size());
for (size_t i=0; i<base_parameters.size(); i++)
parameters[i].set(base_parameters[i] + deltaparameters[i]);
}
void PSOOptimiser::initSwarm()
{
// we want to start the swarm around initial_parameters
//debug << "Initial Swarm: " << endl;
for (int i=0; i<m_num_particles; i++)
{
m_swarm_best.push_back(m_initial_parameters);
m_swarm_best_fitness.push_back(0);
m_swarm_failures.push_back(0);
vector<Parameter> particle = m_initial_parameters;
vector<float> velocity = vector<float>(m_num_dimensions, 0);
for (int j=0; j<m_num_dimensions; j++)
{
float min = m_initial_parameters[j].min();
float max = m_initial_parameters[j].max();
particle[j].set(uniformDistribution(min, max));
velocity[j] = normalDistribution(0, (max-min)/8); // initial velocity between +/- range
}
//debug << i << ": " << Parameter::getAsVector(particle) << endl;
//debug << i << ": " << velocity << endl;
m_swarm_position.push_back(particle);
m_swarm_velocity.push_back(velocity);
}
m_best = m_initial_parameters;
m_best_fitness = 0;
}
ActuatorCircle::ActuatorCircle(
double lowerBound,
double upperBound,
size_t actuatorCount,
size_t startDelay,
size_t stopDelay
)
{
assert(lowerBound <= upperBound && "Upper bound is greater than lower bound");
this->setDelay(startDelay, stopDelay);
mersenneTwisterEngine.seed(std::random_device()());
double mean = lowerBound + (upperBound - lowerBound) / 2.f;
double deviation = std::fabs(upperBound - lowerBound) / 8.f;
std::normal_distribution<double> normalDistribution(mean, deviation);
for(size_t i = 0; i < actuatorCount; ++i) {
Actuator actuator(startDelay, stopDelay);
double randomPosition = normalDistribution(mersenneTwisterEngine);
actuators.push_back(std::make_pair(randomPosition, actuator));
}
}
void IgnitableComponent::HandleIgnite(gentity_t* fireStarter) {
if (!fireStarter) {
// TODO: Find out why this happens.
fireLogger.Notice("Received ignite message with no fire starter.");
}
if (level.time < immuneUntil) {
fireLogger.Debug("Not ignited: Immune against fire.");
return;
}
// Start burning on initial ignition.
if (!onFire) {
onFire = true;
this->fireStarter = fireStarter;
fireLogger.Notice("Ignited.");
} else {
if (alwaysOnFire && !this->fireStarter) {
// HACK: Igniting an alwaysOnFire entity will initialize the fire starter.
this->fireStarter = fireStarter;
fireLogger.Debug("Firestarter set.");
} else {
fireLogger.Debug("Re-ignited.");
}
}
// Refresh ignite time even if already burning.
igniteTime = level.time;
// The spread delay follows a normal distribution: More likely to spread early than late.
int spreadTarget = level.time + (int)std::abs(normalDistribution(randomGenerator));
// Allow re-ignition to update the spread delay to a lower value.
if (spreadTarget < spreadAt) {
fireLogger.DoNoticeCode([&]{
int newDelay = spreadTarget - level.time;
if (spreadAt == INT_MAX) {
fireLogger.Notice("Spread delay set to %.1fs.", newDelay * 0.001f);
} else {
int oldDelay = spreadAt - level.time;
fireLogger.Notice("Spread delay updated from %.1fs to %.1fs.",
oldDelay * 0.001f, newDelay * 0.001f);
}
});
spreadAt = spreadTarget;
}
}
void testSequence(){
int i;
long *fib;
unsigned char *primeNumbmer;
pdf *dist;
dist=normalDistribution(10.0, 5.0, 4, 0.1, 400);
printf("\nSequence\n");
printf("Normal distribution\n");
for (i=0;i<400;i++){
printf("%f\t%f\n",(dist+i)->x,(dist+i)->fx);
}
printf("gamma 1 =%f\n",tgamma(4));
printf("gamma 1 =%f\n",gammaFunction(4));
printf("p(a|b) =%f\n",bayes(0.8, 0.01, 0.096));
printf("factorial =%d\n",factorial(5));
printf("double factorial =%d\n",doubleFactorial(5));
fib=fibonacci(20);
primeNumbmer=prime(20);
printf("fibonacci\n");
for (i=0;i<20;i++){
printf("%d ",i);
}
printf("\n");
for (i=0;i<20;i++){
printf("%ld ",fib[i]);
}
printf("\n");
printf("prime\n");
for (i=0;i<20;i++){
if (primeNumbmer[i] ==1)
printf("%d ",i);
}
printf("\n");
}
void calculateTerrain(point3 a, point3 b, point3 c, point3 d)
{
int terrainDims = pow(2, terrainDepth) + 1;
terrain = new point3*[terrainDims];
for (int i = 0; i < terrainDims; i++){
terrain[i] = new point3[terrainDims];
}
//Set the terrains x and z values for each point
for (int i = 0; i < terrainDims; i++){
for (int j = 0; j < terrainDims; j++){
terrain[i][j][0] = a[0] + j * (b[0] - a[0]) / (terrainDims - 1);
terrain[i][j][2] = a[2] + i * (d[2] - a[2]) / (terrainDims - 1);
}
}
//Set the y value for the initial points
terrain[0][0][1] = a[1];
terrain[0][terrainDims - 1][1] = b[1];
terrain[terrainDims - 1][terrainDims - 1][1] = c[1];
terrain[terrainDims - 1][0][1] = d[1];
std::random_device rd;
std::mt19937 gen(rd());
for (int k = terrainDepth; k > 0; k--){
double H = 3 - D;
double squareSize = (b[0] - a[0]) / pow(2, terrainDepth - k);
if (squareSize < 0) squareSize = -squareSize;
float tmp = pow(squareSize, 2 * (3 - D));
//float tmp = (1 - pow(2, 2 * H - 2));
//float tmp = pow(pow(2, terrainDepth - k), 2 * H); //
tmp = R*sqrt(tmp);
//tmp = 0.2*squareSize;
std::normal_distribution<double> normalDistribution(0, tmp);
int squareSizeInGrid = pow(2, k);
for (int i = 0; i < terrainDims - 1; i += squareSizeInGrid){
for (int j = 0; j < terrainDims - 1; j += squareSizeInGrid){
double error;
//if first row, calculate top
if (i == 0){
error = normalDistribution(gen);//+(1.0 *rand() / RAND_MAX) * 2 * tmp - tmp;
terrain[0][j + squareSizeInGrid / 2][1] = (terrain[0][j][1] + terrain[0][j + squareSizeInGrid][1]) / 2 + error;
}
//if first column, calculate left
if (j == 0){
error = normalDistribution(gen);
terrain[i + squareSizeInGrid / 2][0][1] = (terrain[i][0][1] + terrain[i + squareSizeInGrid][0][1]) / 2 + error;
}
//calculate right
error = normalDistribution(gen);
terrain[i + squareSizeInGrid / 2][j + squareSizeInGrid][1] = (terrain[i][j + squareSizeInGrid][1] + terrain[i + squareSizeInGrid][j + squareSizeInGrid][1]) / 2 + error;
//calculate bottom
error = normalDistribution(gen);
terrain[i + squareSizeInGrid][j + squareSizeInGrid / 2][1] = (terrain[i + squareSizeInGrid][j][1] + terrain[i + squareSizeInGrid][j + squareSizeInGrid][1]) / 2 + error;
//calculate center
error = normalDistribution(gen);
terrain[i + squareSizeInGrid / 2][j + squareSizeInGrid / 2][1] = (terrain[i][j][1] + terrain[i][j + squareSizeInGrid][1] + terrain[i + squareSizeInGrid][j + squareSizeInGrid][1] + terrain[i + squareSizeInGrid][j][1]) / 4 + error;
}
}
}
//Smoothing Filter
float** smoothedHeight = new float*[terrainDims];
for (int i = 0; i < terrainDims; i++){
smoothedHeight[i] = new float[terrainDims];
}
float smoothness = 0.8;
for (int k = 0; k < smoothingPasses; k++){
for (int i = 0; i < terrainDims; i++){
for (int j = 0; j < terrainDims; j++){
float temp = 0;
int noNeighboors = 0;
//up
if (i > 0){
temp += terrain[i - 1][j][1];
noNeighboors++;
}
//up - left
if (i > 0 && j > 0){
temp += terrain[i - 1][j - 1][1];
noNeighboors++;
}
//up - left
if (i > 0 && j < terrainDims - 1){
temp += terrain[i - 1][j + 1][1];
noNeighboors++;
}
//down
if (i < terrainDims - 1){
temp += terrain[i + 1][j][1];
noNeighboors++;
}
//down - left
if (i < terrainDims - 1 && j > 0){
temp += terrain[i + 1][j - 1][1];
noNeighboors++;
}
//down - right
if (i < terrainDims - 1 && j < terrainDims - 1){
temp += terrain[i + 1][j + 1][1];
noNeighboors++;
}
//left
if (j > 0){
temp += terrain[i][j - 1][1];
noNeighboors++;
}
//right
if (j < terrainDims - 1){
temp += terrain[i][j + 1][1];
noNeighboors++;
}
temp /= noNeighboors;
smoothedHeight[i][j] = temp + (1 - smoothness) * (terrain[i][j][1] - temp);
//terrain[i][j][1] = temp + (1 - smoothness) * (terrain[i][j][1] - temp);
}
}
for (int i = 0; i < terrainDims; i++){
for (int j = 0; j < terrainDims; j++){
terrain[i][j][1] = smoothedHeight[i][j];
}
}
}
for (int i = 0; i < terrainDims; i++){
free(smoothedHeight[i]);
}
free(smoothedHeight);
}
Facility::Facility(int facil, std::string in, boost::variate_generator< boost::mt19937&, boost::random::triangle_distribution < > > * triangle, boost::variate_generator< boost::mt19937&, boost::random::uniform_real_distribution < > > * uniform, int incdist, double upinc, double lowinc, int infdist, double upinf, double lowinf)
{
facility = facil;
std::string input[11];
int i = 0;
std::stringstream ssin(in);
while (ssin.good() && i < 11) {
ssin >> input[i];
++i;
}
name = input[0];
subfacil = stoi(input[1]);
startS = stoi(input[2]);
startE = stoi(input[3]);
startI = stoi(input[4]);
bed_size = stoi(input[5]);
prev = stod(input[6]);
inc = stod(input[7]);
LOS_mean = stod(input[8]);
LOS_dev = stod(input[9]);
LOS_dist = stoi(input[10]);
RNGpoint = uniform;
triangleRNGpoint = triangle;
genpoint = new boost::mt19937 (time(0)); //create RNG
boost::random::lognormal_distribution< > lognormalDistribution(LOS_mean, LOS_dev);
boost::random::normal_distribution< > normalDistribution(LOS_mean, LOS_dev);
lognormRNGpoint = new boost::variate_generator< boost::mt19937&, boost::random::lognormal_distribution < > >(*genpoint, lognormalDistribution);
normalRNGpoint = new boost::variate_generator< boost::mt19937&, boost::random::normal_distribution < > >(*genpoint, normalDistribution);
inf_dist = infdist;
inc_dist = incdist;
lower_inf = lowinf;
upper_inf = upinf;
lower_inc = lowinc;
upper_inc = upinc;
//initialize agents
for (int i = 0; i < startS; i++) {
Agent * a = new Agent();
(*a).setState(0);
(*a).setEI(-1);
(*a).setIS(-1);
(*a).setDischarge(LOS());
(*a).setFacility(facility);
agents.push_back(a);
}
for (int i = 0; i < startE; i++) {
Agent * a = new Agent();
(*a).setState(1);
if (inc_dist == 1 || inc_dist == 0) { //uniform or single value
(*a).setEI(ceil(randUniform()*(upper_inc - lower_inc)) + lower_inc);
}
else if (inc_dist == 2) { //triangular
(*a).setEI(ceil(randTriangle()*(upper_inc - lower_inc)) + lower_inc);
}
(*a).setIS(-1);
(*a).setDischarge(LOS());
(*a).setFacility(facility);
agents.push_back(a);
}
for (int i = 0; i < startI; i++) {
Agent * a = new Agent();
(*a).setState(2);
(*a).setEI(-1);
if (inf_dist == 1 || inf_dist == 0) { //uniform or single value
(*a).setIS(ceil(randUniform()*(upper_inf - lower_inf)) + lower_inf);
}
else if (inf_dist == 2) { //triangular
(*a).setIS(ceil(randTriangle()*(upper_inf - lower_inf)) + lower_inf);
}
(*a).setDischarge(LOS());
(*a).setFacility(facility);
agents.push_back(a);
}
}
/* Generates a new set of parameters to be tested based on BestParameters and BestDeltaParameters
*/
void Optimiser::mutateBestParameters()
{
// generate phi to mutate the BestParameters
float sigma = 0.15*exp(CountSinceLastImprovement/ResetLimit - 1);
static float phi[SM_NUM_MODES][SH_NUM_JOINTS];
for (int i=0; i<SM_NUM_MODES; i++)
for (int j=0; j<SH_NUM_JOINTS; j++)
phi[i][j] = normalDistribution(1, sigma);
#if OPTIMISER_VERBOSITY > 4
thelog << "OPTIMISER: mutateBestParameters. Phi:" << endl;
for (int i=0; i<SM_NUM_MODES; i++)
for (int j=0; j<SH_NUM_JOINTS; j++)
thelog << phi[i][j] << ",";
thelog << endl;
#endif
// mutate the BestParameters
static float mutant[SM_NUM_MODES][SH_NUM_JOINTS];
for (int i=0; i<SM_NUM_MODES; i++)
for (int j=0; j<SH_NUM_JOINTS; j++)
mutant[i][j] = BestParameters[i][j] * phi[i][j]; // TODO: Add in C
#if OPTIMISER_VERBOSITY > 4
thelog << "OPTIMISER: mutateBestParameters. Mutant:" << endl;
for (int i=0; i<SM_NUM_MODES; i++)
for (int j=0; j<SH_NUM_JOINTS; j++)
thelog << mutant[i][j] << ",";
thelog << endl;
#endif
// calculate the difference between the mutated state and the best one
static float deltamutant[SM_NUM_MODES][SH_NUM_JOINTS];
for (int i=0; i<SM_NUM_MODES; i++)
for (int j=0; j<SH_NUM_JOINTS; j++)
deltamutant[i][j] = BestParameters[i][j] - mutant[i][j];
#if OPTIMISER_VERBOSITY > 4
thelog << "OPTIMISER: mutateBestParameters. Deltamutant:" << endl;
for (int i=0; i<SM_NUM_MODES; i++)
for (int j=0; j<SH_NUM_JOINTS; j++)
thelog << deltamutant[i][j] << ",";
thelog << endl;
#endif
// calculate the desired change in parameters
static float deltaparameters[SM_NUM_MODES][SH_NUM_JOINTS];
for (int i=0; i<SM_NUM_MODES; i++)
for (int j=0; j<SH_NUM_JOINTS; j++)
deltaparameters[i][j] = Alpha*BestDeltaParameters[i][j] + (1-Alpha)*deltamutant[i][j];
#if OPTIMISER_VERBOSITY > 4
thelog << "OPTIMISER: mutateBestParameters. Deltaparameters:" << endl;
for (int i=0; i<SM_NUM_MODES; i++)
for (int j=0; j<SH_NUM_JOINTS; j++)
thelog << deltaparameters[i][j] << ",";
thelog << endl;
#endif
// now calculate the new parameters themselves
static float newparameters[SM_NUM_MODES][SH_NUM_JOINTS];
for (int i=0; i<SM_NUM_MODES; i++)
for (int j=0; j<SH_NUM_JOINTS; j++)
newparameters[i][j] = BestParameters[i][j] + deltaparameters[i][j];
#if OPTIMISER_VERBOSITY > 2
thelog << "OPTIMISER: mutateBestParameters. New parameters:" << endl;
for (int i=0; i<SM_NUM_MODES; i++)
for (int j=0; j<SH_NUM_JOINTS; j++)
thelog << newparameters[i][j] << ",";
thelog << endl;
#endif
// now put the new parameters in the steps so that they will be used
for (int i=0; i<SM_NUM_MODES; i++)
{
for (int j=0; j<SH_NUM_JOINTS; j++)
{
LeftStep->StepSupportHardnesses[i][j] = newparameters[i][j];
RightStep->StepSupportHardnesses[i][j] = newparameters[i][j];
}
}
return;
}
void PSOOptimiser::updateSwarm()
{
debug << "Fitnesses: " << m_swarm_fitness << endl;
// update the personal and global bests
for (int i=0; i<m_num_particles; i++)
{
if (m_swarm_fitness[i] > m_swarm_best_fitness[i])
{
m_swarm_best_fitness[i] = m_swarm_fitness[i];
m_swarm_best[i] = m_swarm_position[i];
m_swarm_failures[i] = 0;
}
else
m_swarm_failures[i]++;;
if (m_swarm_fitness[i] > m_best_fitness)
{
m_best_fitness = m_swarm_fitness[i];
m_best = m_swarm_position[i];
}
}
debug << "Personal bests: " << m_swarm_best_fitness << endl;
debug << "Global best: " << m_best_fitness << " " << Parameter::getAsVector(m_best) << endl;
debug << "Current Swarm:" << endl;
// update the positions and velocities of the particles
for (int i=0; i<m_num_particles; i++)
{
if (m_swarm_failures[i] < m_reset_limit)
{
for (int j=0; j<m_num_dimensions; j++)
{
// Gaussian swarm
float cognitivefactor = fabs(normalDistribution(0,1))*(m_swarm_best[i][j] - m_swarm_position[i][j]);
float socialfactor = fabs(normalDistribution(0,1))*(m_best[j] - m_swarm_position[i][j]);
m_swarm_velocity[i][j] = cognitivefactor + socialfactor;
// PSO Swarm
/*float cognitivefactor = c1*uniformDistribution(0,1)*(m_swarm_best[i][j] - m_swarm_position[i][j]);
float socialfactor = c2*uniformDistribution(0,1)*(m_best[j] - m_swarm_position[i][j]);
m_swarm_velocity[i][j] = m_inertia*m_swarm_velocity[i][j] + cognitivefactor + socialfactor;
*/
// I need to clip each velocity
float max = (m_best[j].max() - m_best[j].min())/8;
if (m_swarm_velocity[i][j] < -max)
m_swarm_velocity[i][j] = -max;
else if (m_swarm_velocity[i][j] > max)
m_swarm_velocity[i][j] = max;
}
m_swarm_position[i] += m_swarm_velocity[i];
}
else
{
debug << "reset " << i << endl;
m_swarm_failures[i] = 0;
for (int j=0; j<m_num_dimensions; j++)
{
m_swarm_position[i][j] += normalDistribution(0, m_reset_fraction)*(m_swarm_position[i][j].max() - m_swarm_position[i][j].min());
m_swarm_velocity[i][j] = normalDistribution(0, (m_swarm_position[i][j].max() - m_swarm_position[i][j].min())/8);
}
}
debug << "pos " << i << ": " << Parameter::getAsVector(m_swarm_position[i]) << endl;
debug << "vel" << i << ": " << m_swarm_velocity[i] << endl;
}
// clear the fitnesses
m_swarm_fitness.clear();
}
// Normal Probability Distribution Function
double n(const double x)
{
boost::math::normal_distribution<double> normalDistribution(0, 1);
return boost::math::pdf(normalDistribution, x);
}
