// 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);
}
}
}
