int init_subpopulation() {
int i,j;
int label_startindex = space;
double fixprob = 0.,frac_lastbin;
double subpop_crossover_step = dx;
double subpop_crossoverpoint = -space0*dx;
double subpop_last_fixationprob;
double subpop_current_fixationprob;
ww = (double*)calloc(space,sizeof(double));
vv = (double*)calloc(space,sizeof(double));
subpop_start_ww = (double*)malloc(space*sizeof(double));
subpop_start_vv = (double*)malloc(space*sizeof(double));
tmpw = (double*)malloc(space*sizeof(double));
tmpv = (double*)malloc(space*sizeof(double));
subpop_starting_mean_fitness = current_mean_fitness;
update_u(0);
initial_constraint();
switch(subpop_labeltype) {
case 0: label_startindex = space;
while (fixprob < subpop_expected_fixationprobability) {
label_startindex--;
fixprob += nn[label_startindex]*u[label_startindex];
}
if(label_startindex > space)print_error("subpop label threshold larger than simulationbox");
memcpy(&ww[label_startindex],&nn[label_startindex],(space-label_startindex)*sizeof(double));
memcpy(&vv[0],&nn[0],label_startindex*sizeof(double));
frac_lastbin = (fixprob - subpop_expected_fixationprobability)/(nn[label_startindex]*u[label_startindex]);
ww[label_startindex] = nn[label_startindex] * (1. - frac_lastbin);
vv[label_startindex] = nn[label_startindex] * frac_lastbin;
subpop_current_fixationprob = get_current_fixprob();
break;
case 1: for(i=0;i<space;i++) {
ww[i] = subpop_expected_fixationprobability * nn[i];
vv[i] = (1.-subpop_expected_fixationprobability) * nn[i];
}
subpop_current_fixationprob = get_current_fixprob();
break;
case 2: //fprintf(stderr,"# finding crossoverpoint\n");
j = 0;
if(subpop_labelparameter < 0)subpop_labelparameter = 10*dx; // default value
for(i=0;i<space;i++) {
ww[i] = cutoff_function((i-space0)*dx,subpop_crossoverpoint,subpop_labelparameter)*nn[i];
vv[i] = nn[i] - ww[i];
}
subpop_current_fixationprob = get_current_fixprob();
subpop_last_fixationprob = subpop_current_fixationprob;
while(fabs(subpop_current_fixationprob - subpop_expected_fixationprobability)>1e-10) {
subpop_crossoverpoint += subpop_crossover_step;
subpop_current_fixationprob = 0;
for(i=0;i<space;i++) {
ww[i] = cutoff_function((i-space0)*dx,subpop_crossoverpoint,subpop_labelparameter)*nn[i];
vv[i] = nn[i] - ww[i];
subpop_current_fixationprob += ww[i]*u[i];
}
//fprintf(stderr,"# %d %.10e %.10e %.10e\n",j++,subpop_current_fixationprob,subpop_crossover_step,subpop_crossoverpoint);
if( (subpop_current_fixationprob - subpop_expected_fixationprobability)*(subpop_last_fixationprob - subpop_expected_fixationprobability) < 0)subpop_crossover_step *= -.5;
subpop_last_fixationprob = subpop_current_fixationprob;
}
break;
default:print_error("label type not implemented");
break;
}
memcpy(subpop_start_ww,ww,space*sizeof(double));
memcpy(subpop_start_vv,vv,space*sizeof(double));
}
/*
This step feeds the latest sensor measurements into the problem and runs the
QP solver. This step uses takes much less time than the previous step, and so
should be executed at the last possible moment with the latest sensor data,
in order to make the control latency significantly less than the horizon step
length.
*/
void OptimalControlProblem::feedback_step(StateVector measurement) {
initial_constraint(measurement);
solve_qp();
}
