Пример #1
0
// 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];
  }
}
Пример #2
0
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]);
}
Пример #5
0
/**
 * 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;
}
Пример #6
0
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
		();
	}

}
Пример #7
0
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();

}
Пример #8
0
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);
        }
    }
}