// Updates weights taking into account likelihood of each particle and given distance z measured by sonar. void updateWeightArray(float z) { float likelihood; for(int i=0; i<NUMBER_OF_PARTICLES; ++i) { likelihood = calculate_likelihood(xArray[i], yArray[i], thetaArray[i], z); weightArray[i] = likelihood*weightArray[i]; } }
void update_weight_array() { float sonar = SensorValue(sonar_sensor); for (int i = 0; i < NUMBER_OF_PARTICLES; ++i) { weightArray[i] *= calculate_likelihood(xArray[i], yArray[i], thetaArray[i], sonar); } }
void likelihood_f(double* integrals, double** results) { int i; es->background_integral = integrals[0]; for (i = 0; i < ap->number_streams; i++) es->stream_integrals[i] = integrals[i+1]; /******** * CALCULATE THE LIKELIHOOD ********/ int retval = calculate_likelihood(ap, es, sp); if (retval) { fprintf(stderr, "APP: error calculating likelihood: %d\n", retval); exit(retval); } (*results) = (double*)malloc(sizeof(double) * 2); (*results)[0] = es->prob_sum; (*results)[1] = es->bad_jacobians; // printf("calculated likelihood: %lf, bad_jacobs: %lf\n", (*results)[0], (*results)[1]); }
void likelihood_f(double* integrals, double* results, ASTRONOMY_PARAMETERS* ap, EVALUATION_STATE* es, STAR_POINTS* sp) { int i, retval; es->background_integral = integrals[0]; for (i = 0; i < ap->number_streams; i++) es->stream_integrals[i] = integrals[i+1]; retval = calculate_likelihood(ap, es, sp); if (retval) { fprintf(stderr, "APP: error calculating likelihood: %d\n", retval); exit(retval); } results[0] = es->prob_sum; results[1] = es->bad_jacobians; MW_DEBUG("calculated likelihood: %lf, bad_jacobs: %lf\n", results[0], results[1]); }
/** * For one network phi and one experiment extracted from the total data matrix X, * for one experiments states Gx use the viterbi algorithm * N: number of nodes * T: number of time points * R: number of replicates * X: data matrix (N x TxR) * GS: optim state matrix (N x TxR), initialised in R and given as argument * G: state matrix (N x sum_s(M_s)); for each stimulus s, there are M_s states * Glen: * TH: theta parameter matrix (N x 4) * tps: timepoint vector (T) * stimgrps: vector containing the stimulus indices * numexperimentsx: number of experiments (equals length of R and Ms) * hmmit: number of hmmiterations * Ms: number of system states for each experiment */ double hmmsearch(int *phi, const int N, const int *Tx, const int *Rx, const double *X, int *GS, int *G, int Glen, double *TH, const int *tps, const int *stimids, const int *stimgrps, const int numexperiments, const int hmmit, int *Ms) { // fixed for the moment, number of iterations in the em-algorithm int ncol_GS=0; int numstims, idstart=0, Gstart=0; for(int i=0; i!=numexperiments; ++i) { ncol_GS += Tx[i]*Rx[i]; } // printf("~~~~~ \n ncol_GS: %d\n",ncol_GS); // init A, TH and L // sort of a hack for getting the - infinity value double temp = 1.0; double infinity = -1 * (temp / (temp - 1.0)); int M = Glen/N; // total number of system states // allocate the transition probability matrix, for all experiments int A_sz = 0; for(int i=0; i!=numexperiments; ++i) { A_sz += pow(Ms[i],2); } // make a vector of doubles, holding the transition probabilities double *A = malloc(A_sz * sizeof(double)); init_A(A, Ms, numexperiments); // init transition probabilities (sparse MxM matrix) /* main loop: for each iteration in hmmiterations: * perform viterbi for each experiment separately * get the parameters TH and updates for A and GS for all experiments combined */ int Mexp, T, R; int startA=0, startX=0, startG=0; double *Aexp = NULL; double *Xexp = NULL; int *Gexp = NULL; int *GSexp = NULL; int allR, allT; double Lik = -1*infinity; double Likold = -1*infinity; int nLikEqual = 0; // keep track of the last 5 likelihood differences // if switching behaviour occurs, it can be seen here // take differences int K = 6; double diff, difftmp; //double *diffvec = calloc(K-1, sizeof(double)); double *diffvec = malloc(K * sizeof(double)); int *toswitch = calloc(N, sizeof(int)); int nsw=0, maxsw=10; // new state matrix object for(int it=0; it!=hmmit; ++it) { allR=0; allT=0; startA=0; startX=0; startG=0; // find switchable rows in TH nsw = find_switchable(TH, N, toswitch); if(nsw>0) { maxsw = min(nsw, maxsw); } /* here the experiment loop * extract each experiment from X according to R and stimgrps and run the hmm * indices of columns of X to be selected: * expind is equivalent to the experiment index * this defines the stimuli to take from stimgrps */ for(int expind=0; expind!=numexperiments; expind++) { //print_intmatrix(toswitch, 1, N); // if inconsistencies occur in the gamma matrix, // try switching the theta parameters upto // maxsw times to reduce the number of inconsitencies //maxsw = min(nsw, N); T = Tx[expind]; R = Rx[expind]; allR += R; allT += T; Mexp = Ms[expind]; for(int swind=0; swind!=maxsw; ++swind) { // extract the sub-transition matrix: Aexp = realloc(Aexp, Mexp*Mexp * sizeof(double)); extract_transitionmatrix(A, Aexp, Mexp, startA); // extract the sub-data matrix Xexp = realloc(Xexp, N*T*R*sizeof(double)); extract_datamatrix(X, Xexp, N, T, R, startX); // extract the sub-state matrix Gexp = realloc(Gexp, N*Mexp*sizeof(int)); extract_statematrix(G, Gexp, N, Mexp, startG); // extract the sub-optimstate matrix GSexp = realloc(GSexp, N*T*R*sizeof(int)); extract_statematrix(GS, GSexp, N, T*R, startX); // run the viterbi viterbi(T, Mexp, Xexp, Gexp, TH, N, R, Aexp, GSexp); // consitency check int inc = is_consistent(phi, GSexp, Gexp, N, T, R); if(inc>0 && nsw>0) { //printf("Inconsitensies in state series. Repeat HMM with modified thetaprime.\n"); // select a row to switch randomly int rnum = rand() % nsw; // a random number between 1 and nsw int hit = 0, ii=0; for(ii=0; ii!=N; ++ii) { if(toswitch[ii]!=0) { if(hit==rnum) { break; } hit++; } } switch_theta_row(TH, ii, N); // remove node as possible switch node toswitch[ii] = 0; nsw--; } else { // update the GSnew matrix update_statematrix(GS, GSexp, startX, N, T, R); // update the new transition prob matrix update_transitionmatrix(A, Aexp, Mexp, startA); break; } } // switch loop end // increment the experiment start indices startA += pow(Mexp, 2); startX += N*T*R; startG += N*Mexp; } // experiment loop end // number of replicates are the same in each experiment, since // pad-columns containing NAs were added allR = allR/numexperiments; // M-step // update the theta matrix estimate_theta(X, GS, TH, N, allT, allR); // calculate the new likelihood Lik = calculate_likelihood(X, GS, TH, N, allT, allR); //Liktmp$L diff = fabs((fabs(Lik) - fabs(Likold))); // count number of equal differences in the last 10 likelihoods // if all 5 differences are equal, then stop int stopit=0, count = 0; // if all diffs are equal, stopit remains 1 and the loop is aborted // check if diffvec is filled if(it>=K) { // check elements in diffvec for(int k=0; k!=(K-1); ++k) { if(k>0) { if(fabs(diffvec[k]-difftmp)<=0.001) { count++; //stopit = 0; } } // remember the original value at current position difftmp = diffvec[k]; // shift next value left one position if(k<K) { diffvec[k] = diffvec[k+1]; } else { // update the new difference at last position diffvec[k] = diff; } } if(count==(K-1)) { stopit = 1; // make sure that the higher likelihood is taken // if switching occurs if(Likold>Lik) { stopit = 0; } } } else { // if not filled then add elements diffvec[it] = diff; } // another termination criterion: count if liklihood terms do not change if(Lik==Likold) { nLikEqual++; } else { nLikEqual = 0; Likold = Lik; } // abort baum-welch, if Lik does not change anymore if(nLikEqual>=10 || stopit==1) { break; } } free(toswitch); free(diffvec); free(GSexp); free(Gexp); free(Xexp); free(Aexp); free(A); return Lik; }
void navigate_to_waypoint(float x, float y) { float med_x = 0, med_y = 0, med_theta = 0; float i, z; if (DEBUG) { writeDebugStream("Going towards point %f, %f\n",x,y); print_10_points(); print_10_cwa(); } float cosSum = 0, sinSum = 0; // estimate current posistion for (i=0; i < NUMBER_OF_PARTICLES; ++i) { med_x += xArray[i]*weightArray[i]; med_y += yArray[i]*weightArray[i]; //med_theta += thetaArray[i] * weightArray[i]; float theta = thetaArray[i]; cosSum += cosDegrees(theta) * weightArray[i]; sinSum += sinDegrees(theta) *weightArray[i]; } // Transform back from vector to angle if (cosSum > 0) med_theta = atan (sinSum / cosSum); else if (sinSum >=0 && cosSum < 0) med_theta = atan(sinSum / cosSum) + PI; else if (sinSum < 0 && cosSum < 0) med_theta = atan (sinSum / cosSum) - PI; else if (sinSum > 0 && cosSum == 0) med_theta = PI; else if (sinSum < 0 && cosSum == 0) med_theta = -PI; else med_theta = 0; if (DEBUG) writeDebugStream("Averages: x: %f y:%f theta:%f\n", med_x, med_y, med_theta); // calculate difference float dif_x = x - med_x; // dest - curr_pos float dif_y = y - med_y; // Setting the dif_? thresholds if ( abs(dif_x) < 0.01 ) dif_x = 0.0; if ( abs(dif_y) < 0.01 ) dif_y = 0.0; writeDebugStream("Dif_x: %f, dif_y: %f\n",dif_x, dif_y); float rotate_degs; // get the nr of degrees we want to turn. But in which direction ?! // Use of atan2 if ( dif_x > 0) rotate_degs = atan (dif_y / dif_x); else if (dif_y >=0 && dif_x < 0) rotate_degs = atan( dif_y / dif_x) + PI; else if (dif_y < 0 && dif_x < 0) rotate_degs = atan (dif_y / dif_x) - PI; else if (dif_y > 0 && dif_x == 0) rotate_degs = PI; else if (dif_y < 0 && dif_x == 0) rotate_degs = -PI; else rotate_degs = 0; rotate_degs = rotate_degs * 180.0 / PI; rotate_degs = normalize_angle_value(rotate_degs - med_theta); writeDebugStream("Rotate angle: %f\n",rotate_degs); // Print 10 points before rotation if (DEBUG) print_10_points(); // Rotate towards the correct position rotate(rotate_degs); points_update(rotate_degs, ROTATE_STATE); // Print 10 points after rotation , to make sure that we rotate correctly if (DEBUG) print_10_points(); // move forward float move_distance = sqrt(dif_x * dif_x + dif_y * dif_y); if (DEBUG) writeDebugStream("Moving distance: %f\n", move_distance); move_forward(MOVE_POWER, move_distance); // Update the position to the new points points_update(move_distance, MOVE_STATE); //wait1Msec(2000); PlaySound(soundBeepBeep); // Sonar measurement! z = SensorValue[sonar]; for ( i = 0; i < NUMBER_OF_PARTICLES; i++ ) { weightArray[i] = calculate_likelihood(xArray[i], yArray[i], thetaArray[i], z); } // Resampling after calculating likelihood resample(); if (DEBUG) { writeDebugStream("After Resampling\n"); print_10_cwa (); } }
void navigate_to_waypoint(float x, float y) { float med_x = 0, med_y = 0, med_theta = 0; float i, z; writeDebugStream("Going towards point %f, %f\n",x,y); if (DEBUG) { print_10_points(); print_10_cwa(); } // estimate current posistion for (i=0; i < NUMBER_OF_PARTICLES; ++i) { med_x += xArray[i]*weightArray[i]; med_y += yArray[i]*weightArray[i]; med_theta += thetaArray[i] * weightArray[i]; } med_theta = thetaArray[42]; // Game of life writeDebugStream("Averages: x: %f y:%f theta:%f\n", med_x, med_y, med_theta); // calculate difference float dif_x = x - med_x; // dest - curr_pos float dif_y = y - med_y; // Setting the dif_? thresholds if ( abs(dif_x) < 0.01 ) dif_x = 0.0; if ( abs(dif_y) < 0.01 ) dif_y = 0.0; writeDebugStream("Dif_x: %f, dif_y: %f\n",dif_x, dif_y); float rotate_degs;// = atan(dif_y / dif_x) * 180.0 / PI; // get the nr of degrees we want to turn. But in which direction ?! // If both are negative, then we need to add the - to the rotate_degrees if ( dif_x > 0) rotate_degs = atan (dif_y / dif_x); else if (dif_y >=0 && dif_x < 0) rotate_degs = atan( dif_y / dif_x) + PI; else if (dif_y < 0 && dif_x < 0) rotate_degs = atan (dif_y / dif_x) - PI; else if (dif_y > 0 && dif_x == 0) rotate_degs = PI; else if (dif_y < 0 && dif_x == 0) rotate_degs = -PI; else rotate_degs = 0; rotate_degs = rotate_degs * 180.0 / PI; rotate_degs = normalize_angle_value(rotate_degs - med_theta); writeDebugStream("Rotate angle: %f\n",rotate_degs); print_10_points(); // Rotate towards the correct position rotate(rotate_degs); points_update(rotate_degs, ROTATE_STATE); print_10_points(); // move forward float move_distance = sqrt(dif_x * dif_x + dif_y * dif_y); writeDebugStream("Moving distance: %f\n", move_distance); move_forward(MOVE_POWER, move_distance); // Update the position to the new points points_update(move_distance, MOVE_STATE); //wait1Msec(2000); PlaySound(soundBeepBeep); // Measure with sonar z = SensorValue[sonar]; writeDebugStream("[Move_to_waypoint] %f\n",z); writeDebugStream("Before Calculate likelihood\n"); print_10_cwa(); for ( i = 0; i < NUMBER_OF_PARTICLES; i++ ) { weightArray[i] = calculate_likelihood(xArray[i], yArray[i], thetaArray[i], z); } writeDebugStream("Before Resampling\n"); print_10_cwa(); // Resampling resample(); writeDebugStream("After Resampling\n"); print_10_cwa(); }
void match_prevalence(int treatmentNumber, int simNumber, double treatment, double startingBeta) { std::cout << "Treatment #" << treatmentNumber << " and simulation #" << simNumber << ":" << std::endl; std::array<double, INIT_NUM_STYPES> betas; betas.fill(startingBeta); std::array<double, INIT_NUM_STYPES> serotype_ranks; for(int i = 0; i < INIT_NUM_STYPES; i++) { serotype_ranks[i] = i + 1; } double best_likelihood = std::numeric_limits<double>::lowest(); std::array<double, INIT_NUM_STYPES> best_betas = betas; std::array<double, INIT_NUM_STYPES> best_serotype_ranks = serotype_ranks; double prevError = 10.0; double oldPrevError = 1.0; double weight = INIT_WEIGHT; std::array<int, INIT_NUM_STYPES + 1> observed_prevalence = {283, 237, 184, 117, 90, 85, 84, 70, 56, 54, 53, 51, 49, 49, 43, 38, 34, 34, 29, 25, 23, 21, 19, 18, 15, 0, 10079}; for(int attempt = 0; attempt < 10; attempt++) { SimPars thesePars(treatmentNumber, simNumber); thesePars.set_serotype_ranks(serotype_ranks); thesePars.set_betas(betas); Simulation thisSim(treatmentNumber, simNumber, &thesePars); thisSim.runDemSim(); auto serotype_counts = thisSim.runTestEpidSim(); std::array<double, INIT_NUM_STYPES + 1> expected_prevalence = {0}; std::array<double, INIT_NUM_STYPES> prevalence_error = {0}; double expected_population = std::accumulate(serotype_counts.begin(), serotype_counts.end(), 0.0); double infected_prevalence = 0; int observed_population = std::accumulate(observed_prevalence.begin(), observed_prevalence.begin() + INIT_NUM_STYPES + 1, 0); double target_prevalence = std::accumulate(observed_prevalence.begin(), observed_prevalence.begin() + INIT_NUM_STYPES, 0.0) / observed_population; for(int i = 0; i < INIT_NUM_STYPES; i++) { infected_prevalence += serotype_counts[i] / (double)expected_population; prevalence_error[i] = expected_prevalence[i] - observed_prevalence[i] / (double)observed_population; if(i == HFLU_INDEX) { prevalence_error[i] = 0; } expected_prevalence[i] = (serotype_counts[i] + 0.01) / (expected_population + INIT_NUM_STYPES * 0.01); } expected_prevalence.back() = 1 - infected_prevalence; double likelihood = calculate_likelihood(expected_prevalence, observed_prevalence); if(likelihood > best_likelihood) { best_likelihood = likelihood; best_betas = betas; best_serotype_ranks = serotype_ranks; } else { betas = best_betas; serotype_ranks = best_serotype_ranks; } std::cout << "Likelihood=" << likelihood << "(best=" << best_likelihood << ")" << std::endl; if(attempt % 2 == 0) { std::cout << "Fitting overall prevalence" << std::endl; prevError = infected_prevalence - target_prevalence; std::cout << "Observed prevalence=" << target_prevalence << "; expected prevalence=" << infected_prevalence << "; error=" << prevError << "; weight = " << weight << std::endl; if(abs(prevError) > PREV_ERROR_THOLD) { // if error changed signs and overshot, reduce weight if(prevError * oldPrevError < 0) { weight *= COOL_DOWN; } // if climbing too slowly, increase weight else if(abs(target_prevalence - prevError) / abs(target_prevalence - oldPrevError) > TEMP_THOLD) { weight *= WARM_UP; } adjustBeta(prevError, weight, betas); oldPrevError = prevError; } } else { std::cout << "Fitting per-serotype prevalence" << std::endl; adjust_ranks(prevalence_error, serotype_ranks); } } }