void iter_ext_kalman_step( m_elem *z_in ) { int iteration = 1; m_elem est_change; m_elem *prev_state; m_elem *new_state; m_elem *temp; generate_system_transfer( state_pre, sys_transfer ); estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre ); apply_system( state_post, state_pre ); /* Now iterate, updating the probability and the system model until no change is noticed between iteration steps */ prev_state = iter_state0; new_state = iter_state1; generate_measurement_transfer( state_pre, mea_transfer ); update_prob( cov_pre, mea_noise_cov, mea_transfer, cov_post, kalman_gain ); update_system( z_in, state_pre, kalman_gain, prev_state ); est_change = calc_state_change( state_pre, prev_state ); while( (est_change < ITERATION_THRESHOLD) && (iteration++ < ITERATION_DIVERGENCE) ) { #ifdef DEBUG_ITER print_vector( "\titer state", prev_state, 10 ); #endif /* Update the estimate */ generate_measurement_transfer( prev_state, mea_transfer ); update_prob( cov_pre, mea_noise_cov, mea_transfer, cov_post, kalman_gain ); update_system( z_in, prev_state, kalman_gain, new_state ); est_change = calc_state_change( prev_state, new_state ); temp = prev_state; prev_state = new_state; new_state = temp; } vec_copy( prev_state, state_post, state_size ); #ifdef PRINT_DEBUG printf( "iekf: step %3d, %2d iters\n", global_step, iteration ); #endif global_step++; }
BroadEstepper1::BroadEstepper1(CanaryOptions& opts, valarray<double> vals_in, CanaryPrior & prior_in, vector<int> & clusters_in) : _opts(opts) { N = vals_in.size(); G = clusters_in.size(); _vals.resize(vals_in.size()); _vals = vals_in; _prior = prior_in; _cvec.resize(G); _cvec = clusters_in; _prop.resize(G,1.0/G); _mean.resize(G); _var.resize(G); for (unsigned int k=0; k<G; k++) { int cpos = _cvec[k]; // The algorithm converges to a cycle dependent on initial values. If it // converged to a point - as it should - it would make sense to uncomment. // _prop[k] = _prior.prop()[cpos]; _mean[k] = _prior.mean()[cpos]; _var[k] = _prior.var()[cpos]; } _prob.ReSize(N,G); _prob = 0.0; update_prob(); }
void extended_kalman_step( uFloat *z_in ) { #ifdef PRINT_DEBUG printf( "ekf: step %d\n", global_step ); #endif #ifdef PRINT_DEBUG printVector( "measurements ", z_in, MEAS_SIZE ); #endif /***************** Gain Loop ***************** First, linearize locally, then do normal gain loop */ generate_system_transfer( state_pre, sys_transfer ); generate_measurement_transfer( state_pre, mea_transfer ); estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre ); update_prob( cov_pre, mea_noise_cov, mea_transfer, cov_post, kalman_gain ); /************** Estimation Loop ***************/ rungeKutta( state_post, state_pre ); update_system( z_in, state_pre, kalman_gain, state_post ); global_step++; }
void BroadEstepper1::update_broad() { update_prop_broad(); update_mean_broad(); update_var_broad(); update_prob(); }
void empty_board(board_t *b, index_t size) { b->size = size; b->len = (size + 1) * (size + 2) + 1; b->hash = 0; b->ko_pos = -1; b->last_move = -1; for (index_t i = 0; i < max_len; i++) { b->stones[i] = STONE_BORDER; } for (index_t i = 0; i < size; i++) for (index_t j = 0; j < size; j++) { index_t pos = POS(b, i, j); index_t k = i * size + j; b->stones[pos] = STONE_EMPTY; b->list_pos[pos] = k; b->list[k] = pos; } for (index_t i = 0; i < size; i++) for (index_t j = 0; j < size; j++) { index_t pos = POS(b, i, j); b->nbr3x3[pos] = recalc_nbr3x3(b, pos); } b->empty_ptr = b->size * b->size; b->group_ptr = b->size * b->size; b->vis_cnt = 0; memset(b->vis, 0, sizeof(index_t) * b->len); memset(b->prob_sum0, 0, sizeof(b->prob_sum0)); memset(b->prob_sum1, 0, sizeof(b->prob_sum1)); memset(b->prob_sum2, 0, sizeof(b->prob_sum2)); for (index_t i = 0; i < b->len; i++) { update_prob(b, i, STONE_BLACK); update_prob(b, i, STONE_WHITE); } }
void kalman_init( m_elem **GQGt, m_elem **Phi, m_elem **H, m_elem **R, m_elem **P, m_elem *x, int num_state, int num_measurement ) { alloc_globals( num_state, num_measurement ); /* Init the global variables using the arguments. */ vec_copy( x, state_post, state_size ); mat_copy( P, cov_post, state_size, state_size ); sys_noise_cov = GQGt; mea_noise_cov = R; sys_transfer = Phi; mea_transfer = H; /***************** Gain Loop *****************/ estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre ); update_prob( cov_pre, mea_noise_cov, mea_transfer, cov_post, kalman_gain ); }
int main(int argc, char **argv) { //and N the size of the problem int N = GRAPH_SIZE*GRAPH_SIZE; //We define the graph using a matrix of size N //This matrix contain the distance between each node //if two nodes are not connected then the value is 0 int * G = malloc(sizeof(int)*N); //We define the level of pheromone for each edge in a matrix float * T = malloc(sizeof(float)*N); //We define the matrice of the probabilities float * P = malloc(sizeof(float)*N); //Initialize the graph and the level of pheromone init_graph(G,GRAPH_SIZE); init_pheromone(T,GRAPH_SIZE); //Initialize the probabilities float * sum = sum_prob(G,T,GRAPH_SIZE); update_prob(G,T,P,GRAPH_SIZE,sum); printf("graph: \n"); print_int(G,GRAPH_SIZE); printf("pheromone: \n"); print_float(T,GRAPH_SIZE); printf("probabilities: \n"); print_float(P,GRAPH_SIZE); //Array that contain the length of the path generated by each ant int * L = malloc(sizeof(int)*NB_ANT); //number of iteration int iter = 0; //Minimum length int Lmin = INT_MAX; //Shortest path int * best_path = malloc(sizeof(int)*GRAPH_SIZE); //Initialize srand to get different random number srand(time(NULL)); while(iter<ITER_MAX) { //for each ant construct a path from the starting point to the final point int k; for(k=0 ; k<NB_ANT ; k++) { printf("\nThe %dth ant : \n",k+1); int i; //initialize the array that contain the solution int * kth_solution = malloc(sizeof(int)*GRAPH_SIZE); for(i=0; i<GRAPH_SIZE ; i++) { kth_solution[i]=0; } //Generate the solution i=1; float rdm; int j; while(kth_solution[i-1] != GRAPH_SIZE-1) { printf("kth_sol[i-1]: %d \n",kth_solution[i-1]); //select the next node based on the probability //generate a random number between 0 and 1 with 0 excluded rdm = (rand()/(float)RAND_MAX); printf("random number : %f \n",rdm); //Probability to select the next node float Pnext = 0; for(j=0; j<GRAPH_SIZE; j++) { Pnext += P[GRAPH_SIZE*kth_solution[i-1] + j]; //if the random number is less or equal to //the probability to select the next node we select it printf("probability to move from %d to %d: %f \n",kth_solution[i-1],j,Pnext); if( rdm <= Pnext ) { kth_solution[i]=j; break; } } printf("kth_sol[i]: %d \n",kth_solution[i]); i++; } //printf("test \n"); //Calculate the length of the path L[k]=0; i=0; while(kth_solution[i] != GRAPH_SIZE-1) { L[k] += G[kth_solution[i]*GRAPH_SIZE + kth_solution[i+1]]; i++; } //find the shortest length and path if(L[k]<=Lmin) { Lmin = L[k]; memcpy(best_path, kth_solution, sizeof(int)*GRAPH_SIZE ); } //update pheromone values based on the solution update_pheromone1(T,GRAPH_SIZE,kth_solution,L[k]); free(kth_solution); } //evaporation of the pheromone update_pheromone2(T,GRAPH_SIZE); //update probabilities sum = sum_prob(G,T,GRAPH_SIZE); update_prob(G,T,P,GRAPH_SIZE,sum); //increment iter iter++; printf("pheromone: \n"); print_float(T,GRAPH_SIZE); printf("probabilities: \n"); print_float(P,GRAPH_SIZE); } //Best path printf("the best path: \n"); int i=1; printf("%d ",best_path[0]); while(best_path[i-1] != GRAPH_SIZE-1) { printf("%d ",best_path[i]); i++; } printf("\n"); printf("length of the best path: %d \n",Lmin); //Free the memory free(G); free(T); free(P); free(L); free(best_path); return 0; }
void put_stone(board_t *b, index_t pos, stone_t color) { // basic checks if (pos < 0 || pos >= b->len || b->stones[pos] != STONE_EMPTY) return; stone_t oppocolor = OTHER_C(color); // records some variable b->vis_cnt ++; b->nbr3x3_cnt = 0; if (b->ko_pos >= 0) { touch_nbr3x3(b, b->ko_pos); } b->ko_pos = detect_ko(b, pos); if (b->ko_pos >= 0 && b->stones[b->ko_pos] == color) b->ko_pos = -1; if (b->last_move >= 0) { touch_nbr3x3(b, b->last_move); } // put a stone b->stones[pos] = color; b->next_in_group[pos] = pos; b->base_of_group[pos] = pos; b->group_size[pos] = 1; b->pseudo_liberties[pos] = 0; b->group_liberties_sum[pos] = 0; b->group_liberties_sum_squared[pos] = 0; b->atari_of_group[pos] = -1; index_swap(b, b->list_pos[pos], --b->empty_ptr); index_swap(b, b->list_pos[pos], --b->group_ptr); b->hash += p4423[pos] * (hash_t)color; update_empty_neighbour(b, pos); // update neighbour group's pseudo_liberties add_stone_update_liberties(b, pos); add_stone_update_3x3(b, pos); clear_atari_bits_3x3(b->nbr3x3[pos]); // try to capture others if (b->stones[N(b, pos)] == oppocolor) { try_delete_group(b, b->base_of_group[N(b, pos)]); } if (b->stones[S(b, pos)] == oppocolor) { try_delete_group(b, b->base_of_group[S(b, pos)]); } if (b->stones[W(b, pos)] == oppocolor) { try_delete_group(b, b->base_of_group[W(b, pos)]); } if (b->stones[E(b, pos)] == oppocolor) { try_delete_group(b, b->base_of_group[E(b, pos)]); } // merge with friendly neighbours' group if (b->stones[N(b, pos)] == color) { try_merge_group(b, b->base_of_group[N(b, pos)], b->base_of_group[pos]); } if (b->stones[S(b, pos)] == color) { try_merge_group(b, b->base_of_group[S(b, pos)], b->base_of_group[pos]); } if (b->stones[W(b, pos)] == color) { try_merge_group(b, b->base_of_group[W(b, pos)], b->base_of_group[pos]); } if (b->stones[E(b, pos)] == color) { try_merge_group(b, b->base_of_group[E(b, pos)], b->base_of_group[pos]); } // check suicide if (!try_delete_group(b, b->base_of_group[pos])) { if (b->ko_pos >= 0 && ( b->next_in_group[pos] != pos || find_atari(b, pos) < 0 || b->atari_of_group[pos] != b->ko_pos)) { b->ko_pos = -1; } else { b->ko_color = oppocolor; } } else b -> ko_pos = -1; if (b->ko_pos >= 0) { touch_nbr3x3(b, b->ko_pos); } b->last_move = pos; for (index_t i = 0; i < b->nbr3x3_cnt; i++) { index_t p = b->nbr3x3_changed[i]; update_prob(b, p, STONE_BLACK); update_prob(b, p, STONE_WHITE); } }
int main(int argc, char *argv[]) { char *configfile = NULL; FILE *fin, *bin; char *linebuf = NULL; size_t buflen = 0; int iterations = 3; int mode = 3; int c; float d; float *loglik; float p; int i, j, k; opterr = 0; while ((c = getopt(argc, argv, "c:n:hp:")) != -1) { switch (c) { case 'c': configfile = optarg; break; case 'h': usage(); exit(EXIT_SUCCESS); case 'n': iterations = atoi(optarg); break; case 'p': mode = atoi(optarg); if (mode != 1 && mode != 2 && mode != 3) { fprintf(stderr, "illegal mode: %d\n", mode); exit(EXIT_FAILURE); } break; case '?': fprintf(stderr, "illegal options\n"); exit(EXIT_FAILURE); default: abort(); } } if (configfile == NULL) { fin = stdin; } else { fin = fopen(configfile, "r"); if (fin == NULL) { handle_error("fopen"); } } i = 0; while ((c = getline(&linebuf, &buflen, fin)) != -1) { if (c <= 1 || linebuf[0] == '#') continue; if (i == 0) { if (sscanf(linebuf, "%d", &nstates) != 1) { fprintf(stderr, "config file format error: %d\n", i); freeall(); exit(EXIT_FAILURE); } prior = (float *) malloc(sizeof(float) * nstates); if (prior == NULL) handle_error("malloc"); trans = (float *) malloc(sizeof(float) * nstates * nstates); if (trans == NULL) handle_error("malloc"); xi = (float *) malloc(sizeof(float) * nstates * nstates); if (xi == NULL) handle_error("malloc"); pi = (float *) malloc(sizeof(float) * nstates); if (pi == NULL) handle_error("malloc"); } else if (i == 1) { if (sscanf(linebuf, "%d", &nobvs) != 1) { fprintf(stderr, "config file format error: %d\n", i); freeall(); exit(EXIT_FAILURE); } obvs = (float *) malloc(sizeof(float) * nstates * nobvs); if (obvs == NULL) handle_error("malloc"); gmm = (float *) malloc(sizeof(float) * nstates * nobvs); if (gmm == NULL) handle_error("malloc"); } else if (i == 2) { /* read initial state probabilities */ bin = fmemopen(linebuf, buflen, "r"); if (bin == NULL) handle_error("fmemopen"); for (j = 0; j < nstates; j++) { if (fscanf(bin, "%f", &d) != 1) { fprintf(stderr, "config file format error: %d\n", i); freeall(); exit(EXIT_FAILURE); } prior[j] = logf(d); } fclose(bin); } else if (i <= 2 + nstates) { /* read state transition probabilities */ bin = fmemopen(linebuf, buflen, "r"); if (bin == NULL) handle_error("fmemopen"); for (j = 0; j < nstates; j++) { if (fscanf(bin, "%f", &d) != 1) { fprintf(stderr, "config file format error: %d\n", i); freeall(); exit(EXIT_FAILURE); } trans[IDX((i - 3),j,nstates)] = logf(d); } fclose(bin); } else if (i <= 2 + nstates * 2) { /* read output probabilities */ bin = fmemopen(linebuf, buflen, "r"); if (bin == NULL) handle_error("fmemopen"); for (j = 0; j < nobvs; j++) { if (fscanf(bin, "%f", &d) != 1) { fprintf(stderr, "config file format error: %d\n", i); freeall(); exit(EXIT_FAILURE); } obvs[IDX((i - 3 - nstates),j,nobvs)] = logf(d); } fclose(bin); } else if (i == 3 + nstates * 2) { if (sscanf(linebuf, "%d %d", &nseq, &length) != 2) { fprintf(stderr, "config file format error: %d\n", i); freeall(); exit(EXIT_FAILURE); } data = (int *) malloc (sizeof(int) * nseq * length); if (data == NULL) handle_error("malloc"); } else if (i <= 3 + nstates * 2 + nseq) { /* read data */ bin = fmemopen(linebuf, buflen, "r"); if (bin == NULL) handle_error("fmemopen"); for (j = 0; j < length; j++) { if (fscanf(bin, "%d", &k) != 1 || k < 0 || k >= nobvs) { fprintf(stderr, "config file format error: %d\n", i); freeall(); exit(EXIT_FAILURE); } data[(i - 4 - nstates * 2) * length + j] = k; } fclose(bin); } i++; } fclose(fin); if (linebuf) free(linebuf); if (i < 4 + nstates * 2 + nseq) { fprintf(stderr, "configuration incomplete.\n"); freeall(); exit(EXIT_FAILURE); } if (mode == 3) { loglik = (float *) malloc(sizeof(float) * nseq); if (loglik == NULL) handle_error("malloc"); for (i = 0; i < iterations; i++) { init_count(); for (j = 0; j < nseq; j++) { loglik[j] = forward_backward(data + length * j, length, 1); } p = sumf(loglik, nseq); update_prob(); printf("iteration %d log-likelihood: %.4f\n", i + 1, p); printf("updated parameters:\n"); printf("# initial state probability\n"); for (j = 0; j < nstates; j++) { printf(" %.4f", exp(prior[j])); } printf("\n"); printf("# state transition probability\n"); for (j = 0; j < nstates; j++) { for (k = 0; k < nstates; k++) { printf(" %.4f", exp(trans[IDX(j,k,nstates)])); } printf("\n"); } printf("# state output probility\n"); for (j = 0; j < nstates; j++) { for (k = 0; k < nobvs; k++) { printf(" %.4f", exp(obvs[IDX(j,k,nobvs)])); } printf("\n"); } printf("\n"); } free(loglik); } else if (mode == 2) { for (i = 0; i < nseq; i++) { viterbi(data + length * i, length); } } else if (mode == 1) { loglik = (float *) malloc(sizeof(float) * nseq); if (loglik == NULL) handle_error("malloc"); for (i = 0; i < nseq; i++) { loglik[i] = forward_backward(data + length * i, length, 0); } p = sumf(loglik, nseq); for (i = 0; i < nseq; i++) printf("%.4f\n", loglik[i]); printf("total: %.4f\n", p); free(loglik); } freeall(); return 0; }
MaxResults adjust_max_map(Model B, PoSition **Pos, double *adj_map) /*=======================================================================*/ /* FUNCTION NAME : adjust_max_map */ /* */ /* DESCRIPTION : Improves the maximal alignment by including motif */ /* motif elements whose inclusion (whether it is forward */ /* or reverse complement) improves upon the maximum */ /* calculated map value */ /*=======================================================================*/ { double map_fwd, map_rev, map_same; int n, t, newpos; MaxResults M; /** Have to set these -- will then be returned **/ int last_inc; double dMax, dLocMax; double **dProbArray; int maxnum; ProbStruct P; /* BT 2/21/97 */ int start, end; int nSeq; int typ; int p; int len; init_maxdata(&M); map_fwd = map_rev = map_same = (*adj_map); for( nSeq = 0; nSeq < B->IP->nNumSequences; nSeq += SeqInc( B, nSeq ) ) { len = SequenceLength( B, nSeq ); start = SequenceStartPos( B, nSeq ); end = SequenceEndPos( B, nSeq ); for(n = start; n <= end; n++) { for(t = 0; t < B->IP->nNumMotifTypes; t++) { if(((B->IP->is_defined[cl_E] && B->RP->nSites[nSeq] < B->RP->nMaxBlocks) || (! B->IP->is_defined[cl_E])) && PossibleStartPos( Pos, n, t, len, nSeq, B) && !OverlapsPrevMotif(n,B->IP->nNumMotifTypes,Pos) && !Pos[t][n].nMotifStartPos) { newpos = -1; /* BT 3/12/97 */ if( AnyOverlap(B, n, t, Pos, &newpos, &typ) ) { for( p = 0; p < SeqInc( B, nSeq ); p++ ) { adjust_counts(B,DELETE,newpos + p * len, typ, Pos[typ][newpos + p * len].RevComp); B->IP->nNumMotifs[typ][Pos[typ][newpos + p * len].RevComp]--; B->RP->nSites[nSeq + p]--; not_in_motif(Pos, newpos + p * len,B,typ); } } /* Calculate the map values for the current position */ /* being excluded (map_same), included as a forward */ /* motif (map_fwd), or being included as a reverse */ /* complement motif (map_rev). */ map_same = CalcMapProb(B, B->IP->is_defined[cl_R]); if( PossibleStartPos( Pos, n, t, len, nSeq, B) ) { for( p = 0; p < SeqInc( B, nSeq ); p++ ) { adjust_counts(B, ADD, n + p * len,t,FALSE); B->RP->nSites[nSeq+p]++; B->IP->nNumMotifs[t][FORWARD]++; /* BT 3/12/97 */ } map_fwd = CalcMapProb(B, B->IP->is_defined[cl_R]); for( p = 0; p < SeqInc( B, nSeq ); p++ ) { adjust_counts(B, DELETE, n + p * len,t,FALSE); B->RP->nSites[nSeq+p]--; B->IP->nNumMotifs[t][FORWARD]--; /* BT 3/12/97 */ } if(B->IP->RevComplement) { for( p = 0; p < SeqInc( B, nSeq ); p++ ) { adjust_counts(B, ADD, n + p * len, t,TRUE); B->RP->nSites[nSeq+p]++; B->IP->nNumMotifs[t][REVERSE]++; /* BT 3/12/97 */ } map_rev = CalcMapProb(B, B->IP->is_defined[cl_R]); for( p = 0; p < SeqInc( B, nSeq ); p++ ) { adjust_counts(B, DELETE, n + p * len, t, TRUE); B->RP->nSites[nSeq+p]--; B->IP->nNumMotifs[t][REVERSE]--; /* BT 3/12/97 */ } } /* See if the forward map improves the maximal */ /* map calculated so far */ if((map_fwd >= map_same) && (map_fwd >= map_rev) && (map_fwd >= (*adj_map))) { for( p = 0; p < SeqInc( B, nSeq ); p++ ) { adjust_counts(B, ADD, n + p * len,t, FALSE); B->RP->nSites[nSeq+p]++; B->IP->nNumMotifs[t][FORWARD]++; set_in_motif(Pos,n + p * len,B,t,FALSE); } (*adj_map) = map_fwd; /* as this segment in alignment, we need to check */ /* segments do not overlap with this segment */ n += (MotifWidth( B, t ) - 1); } /* See if the reverse complement map improves */ /* the maximal map calculated so far */ else if(B->IP->RevComplement && (map_rev >= map_same) && (map_rev >= map_fwd) && (map_rev >= (*adj_map))) { for( p = 0; p < SeqInc( B, nSeq ); p++ ) { adjust_counts(B, ADD, n + p * len,t, TRUE); B->RP->nSites[nSeq+p]++; B->IP->nNumMotifs[t][REVERSE]++; set_in_motif(Pos,n + p * len,B,t,TRUE); } (*adj_map) = map_rev; /* as this segment in alignment, we need to check */ /* segments do not overlap with this segment */ n += (MotifWidth( B, t ) - 1); } /* if we removed a motif and things did not improve when we */ /* added n, put the old motif back */ /* BT 3/12/97 */ else if( newpos >= 0 ) { for( p = 0; p < SeqInc( B, nSeq ); p++ ) { adjust_counts(B, ADD,newpos + p * len,typ,Pos[typ][newpos + p * len].RevComp); B->IP->nNumMotifs[typ][Pos[typ][newpos+p*len].RevComp]++; set_in_motif(Pos,newpos + p*len,B,typ, Pos[typ][newpos+p*len].RevComp); B->RP->nSites[nSeq+p]++; } } else if(Pos[t][n].nMotifStartPos) /* as this segment in alignment, we need to check */ /* segments do not overlap with this segment */ n += (MotifWidth( B, t ) - 1); } } } } } maxnum = findMaxNumMotif(B->IP); init_prob( &P, B->IP ); /* BT 2/21/97 */ update_prob( &P, B, TRUE ); update_posterior_prob(B); /* BT 3/17/97 */ dProbArray = setElementProb( B, Pos, P ); M = setMaxData(B->F, B->IP, (*adj_map), 1, Pos, &last_inc, &dMax, &dLocMax, M, dProbArray, B); FREEP(dProbArray, maxnum); M.frequency = NULL; free_prob( &P, B->IP ); return M; }