vei_t SOSDM::store_SO(BinVector& v1, BinVector& b1)
{
vei_t c=0;
vei_t max;
max=max_invdist(v1);
oas_t valu;
total_sig=0.0;
for (vei_t j=0;j<par.size;j++)
{
valu = signal_thres(v1, av[j], max);
if (valu>0)
{
for (vei_t i=0;i<par.dsize;i++)
{
if (b1.get_comp(i)==1)
dvd[j].set_comp((dvd[j].get_comp(i)+valu),i);
else
dvd[j].set_comp((dvd[j].get_comp(i)-valu),i);
}
total_sig += valu;
c++;
error_calc(v1, av[j], valu, j);
}
}
return c;
}
int fuzzy::fuzzy_controller(int input, int s_point)
{
set_point = s_point;
crisp_ip = input;
error_p = error_calc(crisp_ip, set_point);
ch_error_p = ch_error_calc(error, &error_1);
err_set = membership_determiner(_n_membr_sets, error_p);
ch_err_set = membership_determiner(_n_membr_sets, ch_error_p);
ch_op_set = ch_op_determiner(_n_membr_sets, err_set, ch_err_set);
ch_op_p = defuzzifier(_n_membr_sets, ch_op_set);
ch_op = depercentizer(ch_op_p, ch_op_max, ch_op_min);
return (ch_op);
}
/* fit potential energy parameters via simulated annealing */
int surface_fit(system_t *system) {
//override default values if specified in input file
double temperature = ((system->fit_start_temp ) ? system->fit_start_temp : TEMPERATURE );
double max_energy = ((system->fit_max_energy ) ? system->fit_max_energy : MAX_ENERGY );
double schedule = ((system->fit_schedule ) ? system->fit_schedule : SCHEDULE );
//used only if qshift is on
double quadrupole=0;
int i, j, // generic counters
nCurves, // number of curves to fit against
nPoints, // number of points in each curve
nSteps;
double *r_input=0; // Master list of r-values
curveData_t *curve=0; // Array to hold curve point data
param_t *param_ptr;
param_g *params;
double r_min, r_max, r_inc;
double current_error, last_error, global_minimum;
qshiftData_t * qshiftData = NULL; //used only with qshift
// ee_local variables
int a = 0;
int globalFound = 0;
double oldGlobal = 0;
double initEps[2];
double initSig[2];
double globalEps[2];
double globalSig[2];
// Record number of curves for convenient reference
nCurves = system->fit_input_list.data.count;
// Read the curve data from the input files into a local array
curve = readFitInputFiles( system, nCurves );
if(!curve) {
error( "SURFACE: An error occurred while reading the surface-fit input files.\n" );
return (-1);
}
// Record number of points in each curve, for convenient reference
nPoints = curve[0].nPoints;
// Make a master list of R-values
r_input = curve[0].r;
curve[0].r = 0;
// Free the memory used by all the remaining lists (i.e. all lists
// other than the one whose address was just transferred).
for( i=1; i<nCurves; i++ )
free(curve[i].r);
// Allocate memory for the output and global arrays
if ( -1 == alloc_curves ( nCurves, nPoints, curve ) ) {
free(r_input);
return -1;
}
// Determine Independent Domain
//////////////////////////////////
r_min = r_input[0];
r_max = r_input[nPoints-1];
r_inc = r_input[1] - r_input[0];
// Output the geometry for visual verification
output_pqrs ( system, nCurves, curve );
// Get the Initial Error
//////////////////////////////////
//obtain curves
get_curves ( system, nCurves, curve, r_min, r_max, r_inc );
//calc current error
current_error = error_calc ( system, nCurves, nPoints, curve, max_energy);
//record parameters. we will determine which ones need to be adjusted later
params = record_params ( system );
// print some header info
for(param_ptr = params->type_params; param_ptr; param_ptr = param_ptr->next) {
printf( "SURFACE: Atom type: %d @ %s\n", param_ptr->ntypes, param_ptr->atomtype );
if(param_ptr->epsilon != 0) {
initEps[a] = param_ptr->epsilon;
initSig[a] = param_ptr->sigma;
a++;
}
}
if ( ! system->fit_boltzmann_weight )
printf("*** any input energy values greater than %f K will not contribute to the fit ***\n", max_energy);
//write params and current_error to stdout
printf("*** Initial Fit: \n");
output_params ( temperature, current_error, params );
printf("*****************\n");
//set the minimum error we've found
global_minimum = current_error;
last_error = current_error;
for( j=0; j<nCurves; j++ )
for(i = 0; i < nPoints; i++)
curve[j].global[i] = curve[j].output[i];
//initialize stuff for qshift
if ( system->surf_qshift_on ) {
qshiftData = malloc(sizeof(qshiftData_t));
quadrupole = calcquadrupole(system);
}
// ANNEALING
// Loop over temperature. When the temperature reaches MIN_TEMPERATURE
// we quit. Assuming we find an error smaller than the initial error
// we'll spit out a fit_geometry.pqr file, and you'll want to re-anneal.
for(nSteps = 0; temperature > MIN_TEMPERATURE; ++nSteps) {
// randomly perturb the parameters
surf_perturb ( system, quadrupole, qshiftData, params );
// apply the trial parameters
surface_dimer_parameters ( system, params);
get_curves ( system, nCurves, curve, r_min, r_max, r_inc );
// calc error
current_error = error_calc ( system, nCurves, nPoints, curve, max_energy);
int condition = 0;
if (system->surf_descent) condition = current_error < last_error;
else condition = get_rand() < exp(-(current_error - last_error)/temperature);
// DO MC at this 'temperature'
if(condition) {
//ACCEPT MOVE /////
apply_new_parameters ( params );
last_error = current_error;
if(nSteps >= EQUIL) {
nSteps = 0;
//write params and current_error to stdout
output_params ( temperature, current_error, params );
// output the new global minimum
if(current_error < global_minimum) {
system->fit_best_square_error = global_minimum = current_error;
new_global_min ( system, nCurves, nPoints, curve );
// Store the LJ parameters corresponding to global min
a = 0;
globalFound = 1;
oldGlobal = current_error; // keep track for ee_local
for(param_ptr = params->type_params; param_ptr; param_ptr = param_ptr->next) {
if(param_ptr->epsilon != 0) {
globalEps[a] = param_ptr->epsilon;
globalSig[a] = param_ptr->sigma;
a++;
}
}
}
}
}
else {
// MOVE REJECTED ////
revert_parameters ( system, params );
} //END DO MONTE CARLO
// decrement the temperature
temperature = temperature*schedule; // schedule
} //END ANNEALING
// Output the Fit Curves
output_fit ( nCurves, nPoints, curve, max_energy, r_input );
// Exhaustive Enumeration -- Chris Cioce
if (system->ee_local) {
int b, c, d, nE1, nE2, nS1, nS2;
double currEps[2];
double currSig[2];
double newGlobal=0;
double E1start, E1stop, E1incr, E2start, E2stop, E2incr, E1c, E2c; // E1, E2 (unused)
double S1start, S1stop, S1incr, S2start, S2stop, S2incr, S1c, S2c; // S1, S2 (unused)
if(globalFound == 1) {
printf("\nEE_LOCAL ~> SA found an improved surface. Using current global minimum parameters for EE.\n");
currEps[0] = globalEps[0];
currEps[1] = globalEps[1];
currSig[0] = globalSig[0];
currSig[1] = globalSig[1];
} else {
printf("\nEE_LOCAL ~> SA did not find an improved surface. Using initial parameters for EE.\n");
currEps[0] = initEps[0];
currEps[1] = initEps[1];
currSig[0] = initSig[0];
currSig[1] = initSig[1];
}
for(a=0; a<2; a++) {
printf("EE_LOCAL ~> InitEps[%d]: %f\tInitSig[%d]: %f\tGlobalEps[%d]: %f\tGlobalSig[%d]: %f\n",a,initEps[a],a,initSig[a],a,globalEps[a],a,globalSig[a]);
}
E1start = currEps[0] - (currEps[0]*system->range_eps);
E1stop = currEps[0] + (currEps[0]*system->range_eps);
E1incr = currEps[0] * system->step_eps;
E2start = currEps[1] - (currEps[1]*system->range_eps);
E2stop = currEps[1] + (currEps[1]*system->range_eps);
E2incr = currEps[1] * system->step_eps;
S1start = currSig[0] - (currSig[0]*system->range_sig);
S1stop = currSig[0] + (currSig[0]*system->range_sig);
S1incr = currSig[0] * system->step_sig;
S2start = currSig[1] - (currSig[1]*system->range_sig);
S2stop = currSig[1] + (currSig[1]*system->range_sig);
S2incr = currSig[1] * system->step_sig;
nE1 = ((E1stop - E1start) / E1incr) + 1;
nE2 = ((E2stop - E2start) / E2incr) + 1;
nS1 = ((S1stop - S1start) / S1incr) + 1;
nS2 = ((S2stop - S2start) / S2incr) + 1;
printf("EE_LOCAL ~> E1start = %f\tE1stop = %f\tE1incr = %f\tnE1 = %d\n",E1start,E1stop,E1incr,nE1);
printf("EE_LOCAL ~> E2start = %f\tE2stop = %f\tE2incr = %f\tnE2 = %d\n",E2start,E2stop,E2incr,nE2);
printf("EE_LOCAL ~> S1start = %f\tS1stop = %f\tS1incr = %f\tnS1 = %d\n",S1start,S1stop,S1incr,nS1);
printf("EE_LOCAL ~> S2start = %f\tS2stop = %f\tS2incr = %f\tnS2 = %d\n",S2start,S2stop,S2incr,nS2);
printf("EE_LOCAL ~> Will now perform %d total iterations in parameter subspace...\n",(nE1*nE2*nS1*nS2));
E1c = E1start;
E2c = E2start;
S1c = S1start;
S2c = S2start;
// Set initial parameters
for(param_ptr = params->type_params; param_ptr; param_ptr = param_ptr->next) {
if(param_ptr->epsilon == currEps[0]) {
param_ptr->epsilon = E1c;
}
if(param_ptr->epsilon == currEps[1]) {
param_ptr->epsilon = E2c;
}
if(param_ptr->sigma == currSig[0]) {
param_ptr->sigma = S1c;
}
if(param_ptr->sigma == currSig[1]) {
param_ptr->sigma = S2c;
}
}
// apply initial parameters
surface_dimer_parameters ( system, params);
get_curves ( system, nCurves, curve, r_min, r_max, r_inc );
//calc current error
current_error = error_calc ( system, nCurves, nPoints, curve, max_energy);
//record parameters. we will determine which ones need to be adjusted later
//params = record_params ( system );
//write params and current_error to stdout <-- just for visual verification
printf("*** Initial Fit: \n");
output_params ( 0.0, current_error, params );
printf("*****************\n");
// Main loops
globalFound = 0;
int master = 1;
for(a=0; a<=nE1; a++) {
for(b=0; b<=nE2; b++) {
for(c=0; c<=nS1; c++) {
for(d=0; d<=nS2; d++) {
//printf("MASTER: %d\ta: %d\t(%f)\tb: %d\t(%f)\tc: %d\t(%f)\td: %d\t(%f)\n", master, a, E1c, b, E2c, c, S1c, d, S2c); // Debug output
// apply parameters
surface_dimer_parameters ( system, params);
get_curves ( system, nCurves, curve, r_min, r_max, r_inc );
// calc current error
current_error = error_calc ( system, nCurves, nPoints, curve, max_energy);
//output_params ( 0.0, current_error, params ); // Debug output
// check for new global minimum
if(current_error < global_minimum) {
system->fit_best_square_error = global_minimum = current_error;
new_global_min ( system, nCurves, nPoints, curve );
// Store the LJ parameters corresponding to global min
a = 0;
globalFound = 1;
newGlobal = current_error;
for(param_ptr = params->type_params; param_ptr; param_ptr = param_ptr->next) {
if(param_ptr->epsilon != 0) {
globalEps[a] = param_ptr->epsilon;
globalSig[a] = param_ptr->sigma;
a++;
}
}
}
// Set updated S2 parameter
for(param_ptr = params->type_params; param_ptr; param_ptr = param_ptr->next) {
if(param_ptr->sigma == S2c) {
param_ptr->sigma = (S2c+S2incr);
}
}
S2c += S2incr;
master++;
} // END S2 loop
// Set updated S1 & S2 parameters
for(param_ptr = params->type_params; param_ptr; param_ptr = param_ptr->next) {
if(param_ptr->sigma == S1c) {
param_ptr->sigma = (S1c+S1incr);
}
if(param_ptr->sigma == S2c) {
param_ptr->sigma = S2start;
}
}
S1c += S1incr;
S2c = S2start;
} // END S1 loop
// Set updated E2 & S1 parameters
for(param_ptr = params->type_params; param_ptr; param_ptr = param_ptr->next) {
if(param_ptr->epsilon == E2c) {
param_ptr->epsilon = (E2c+E2incr);
}
if(param_ptr->sigma == S1c) {
param_ptr->sigma = S1start;
}
}
E2c += E2incr;
S1c = S1start;
} // END E2 loop
// Set updated E1 & E2 parameters
for(param_ptr = params->type_params; param_ptr; param_ptr = param_ptr->next) {
if(param_ptr->epsilon == E1c) {
param_ptr->epsilon = (E1c+E1incr);
}
if(param_ptr->epsilon == E2c) {
param_ptr->epsilon = E2start;
}
}
E1c += E1incr;
E2c = E2start;
} // END E1 loop
// If a new global min is found, write to stdout
// TODO: Adjust printf statements to prettify final output :D
if(globalFound == 1) {
printf("\nEE_LOCAL ~> EE found an improved surface. Parameters written to fit_geometry.pqr\n");
//printf("EE_LOCAL ~> E1: %f\tS1: %f\tE2: %f\tS2: %f\n", globalEps[0], globalSig[0], globalEps[1], globalSig[1]);
printf("EE_LOCAL ~> SA Global Min: %f\n", oldGlobal);
printf("EE_LOCAL ~> EE Global Min: %f\n", newGlobal);
printf(" ----------------------------\n");
printf(" ----------------------------\n");
printf(" ====> %.2f %% improvement\n\n", ((oldGlobal-newGlobal)/oldGlobal)*100);
} else {
printf("EE_LOCAL ~> EE was unable to find an improved surface versus SA. Oh well, at least you tried...and for that, we thank you!\n\n");
}
} // END ee_local
// Return all memory back to the system
free_all_mem (nCurves, curve, params, qshiftData, r_input);
return(0);
}
/* Functions */
void Robot_drive_task(void)
{
Robot_PWM_init(); // Initialize motors
int32_t pid_coeff[3]; // PID Coefficients
int8_t e[3];
int8_t e2[3];
int32_t u;
int32_t u2;
pid_terms_calc(KP, KI, KD, F_SAMP, pid_coeff);
uint8_t blk_cnt[4]; // Consecutive BLK read counters
uint8_t num_iterations = 0;
fw_motors(5, 60); //Test :)
while(TRUE)
{
// Pend on semaphore - Light Sensor values updated
Semaphore_pend(Sema_lightsense, BIOS_WAIT_FOREVER);
switch(drivestate)
{
case DRIVESTATE_IDLE:
// Do nothing :)
break;
case DRIVESTATE_FWD:
linedetection(lightsnsr_val, blk_cnt, 0, 1, &num_iterations);
linecheck(&u, &u2, &num_iterations, num_moves);
error_calc(lightsnsr_val[0], e);
error_calc(lightsnsr_val[1], e2);
u = findu(e, u, pid_coeff);
u2 = findu(e2, u2, pid_coeff);
fwd_pid(u, u2, dutycycle);
break;
case DRIVESTATE_REV:
linedetection(lightsnsr_val, blk_cnt, 1, 0, &num_iterations);
linecheck(&u, &u2, &num_iterations, num_moves);
error_calc(lightsnsr_val[0], e);
error_calc(lightsnsr_val[1], e2);
u = findu(e, u, pid_coeff);
u2 = findu(e2, u2, pid_coeff);
rev_pid(u, u2, dutycycle);
break;
case DRIVESTATE_STRAFELEFT:
linedetection(lightsnsr_val, blk_cnt, 2, 3, &num_iterations);
linecheck(&u, &u2, &num_iterations, num_moves);
error_calc(lightsnsr_val[0], e);
error_calc(lightsnsr_val[1], e2);
u = findu(e, u, pid_coeff);
u2 = findu(e2, u2, pid_coeff);
tl_pid(u, u2, dutycycle);
break;
case DRIVESTATE_STRAFERIGHT:
linedetection(lightsnsr_val, blk_cnt, 3, 2, &num_iterations);
linecheck(&u, &u2, &num_iterations, num_moves);
error_calc(lightsnsr_val[0], e);
error_calc(lightsnsr_val[1], e2);
u = findu(e, u, pid_coeff);
u2 = findu(e2, u2, pid_coeff);
tl_pid(u, u2, dutycycle);
break;
case DRIVESTATE_TURNCW:
intersectiondetect(lightsnsr_val, blk_cnt, &num_iterations);
linecheck(&u, &u2, &num_iterations, num_moves);
break;
case DRIVESTATE_TURNCCW:
intersectiondetect(lightsnsr_val, blk_cnt, &num_iterations);
linecheck(&u, &u2, &num_iterations, num_moves);
break;
}
}
}
int main(int argc, char* argv[]){
//float beta;
//int row,col;
string file_name = FILE_NAME;
HostMatrix<float> X;
HostMatrix<float> X_test;
HostMatrix<float> Y;
HostMatrix<float> Y_test;
HostMatrix<float> Input;
HostMatrix<float> Target;
std::map<string,int> Classes;
std::map<int,string> ClassesLookup;
readFile(file_name,Input,Target,Classes,ClassesLookup);
int kfold = 1;
int correct_instances = 0;
int incorrect_instances = 0;
int total_instances = 0;
int **confusionMatrix;
confusionMatrix = (int**) malloc(sizeof(int*)*Classes.size());
for(int i = 0; i < (int)Classes.size(); i++){
confusionMatrix[i] = (int*) malloc(sizeof(int)*Classes.size());
memset(confusionMatrix[i],0,sizeof(int)*Classes.size());
}
float Pet_mean = 0;
float Ped_mean = 0;
unsigned int seed = (unsigned)time(0);
/***************RUN INFORMATION*************/
writeHeader(Input,Classes.size(),seed);
/*******************************************/
if(!InitCUDA()) {
return 1;
}
culaStatus status;
status = culaInitialize();
std::cout << "Starting " << std::endl;
float center_time = 0;
float width_time = 0;
float weight_time = 0;
float scaling_time = 0;
unsigned int time_total = 0;
unsigned int testing_time = 0;
unsigned int training_time = 0;
clock_t initialTimeTotal = clock();
do{
X = crossvalidationTrain(Input,KFOLDS,kfold);
X_test = crossvalidationTest(Input,KFOLDS,kfold);
Y = crossvalidationTrain(Target,KFOLDS,kfold);
Y_test = crossvalidationTest(Target,KFOLDS,kfold);
HostMatrix<float> Weights;
HostMatrix<float> Centers;
/*Train Network*/
clock_t initialTime = clock();
RadialBasisFunction RBF(NETWORK_SIZE,RNEIGHBOURS,SCALING_FACTOR,Classes.size());
RBF.SetSeed(seed);
RBF.Train(X,Y);
training_time = (clock() - initialTime);
center_time += RBF.times[0];
width_time += RBF.times[1];
weight_time += RBF.times[2];
scaling_time += RBF.times[3];
/*Test Network*/
initialTime = clock();
std::cout << "Testing" << std::endl;
HostMatrix<float> out_test;
out_test = RBF.Test(X_test);
for(int i = 0; i< X_test.Rows();i++){
float max = 0;
float out_class = 0;
for(int j = 0; j < (int) Classes.size(); j++){
if(out_test(i,j) > max){
out_class = (float)j;
max = out_test(i,j);
}
}
out_test(i,0) = out_class+1;
}
for (int i = 0; i < out_test.Rows(); i++)
{
out_test(i,0) = (float)round(out_test(i,0));
if(out_test(i,0) <= 0) out_test(i,0) = 1;
if(out_test(i,0) > Classes.size()) out_test(i,0) = (float)Classes.size();
std::cout << Y_test(i,0) << " " << out_test(i,0) << std::endl;
}
correct_instances += out_test.Rows() - error_calc(Y_test,out_test);
incorrect_instances += error_calc(Y_test,out_test);
total_instances += out_test.Rows();
/*Add values to Confusion Matrix*/
for(int i = 0; i < Y_test.Rows(); i++){
confusionMatrix[((int)Y_test(i,0))-1][((int)out_test(i,0))-1] = confusionMatrix[((int)Y_test(i,0))-1][((int)out_test(i,0))-1] + 1;
}
testing_time = (clock() - initialTime);
/*Increment fold number, for use in crossvalidation*/
kfold++;
}while(kfold <= KFOLDS);
time_total = (clock() - initialTimeTotal);
/*****************MEASURES****************/
measures(correct_instances,total_instances,incorrect_instances,confusionMatrix,Classes,ClassesLookup);
writeFooter(center_time,width_time,weight_time,scaling_time,training_time,testing_time,time_total);
culaShutdown();
cudaThreadExit();
return 0;
}