/** * Main function with all logic * deal with threads and other things */ static void process(const char * filename, unsigned long k, unsigned long t) { unsigned long n, i, start, end, rest, addition; Multiseq * multiseq; BestKVals * kval; MultiseqTreadSpace* threads; MultiseqTread* thread; assert_with_message(filename != NULL, "filename can not be NULL"); multiseq = muliseq_new(filename); kval = best_k_vals_new(k, compare, sizeof(MultiseqPaar)); threads = multiseq_threads(t); n = rest = muliseq_items(multiseq); thread = NULL; while ((thread = multiseq_thread_next(threads)) != NULL) { addition = calculate_addition(t, thread->id, rest); start = calculate_start(t, thread->id, rest, addition); end = rest; rest = calculate_rest(start); thread->i = start; thread->j = end; thread->multiseq = multiseq; thread->compare = eval_unit_edist; pthread_create_or_die(&thread->identifier, eval_seqrange_thread, thread); } while ((thread = multiseq_thread_next(threads)) != NULL) { pthread_join(thread->identifier, NULL); } n = muliseq_paars(multiseq); for (i = 0; i < n; i++) { const MultiseqPaar * element = muliseq_paar(multiseq, i); best_k_vals_insert(kval, element); } best_k_vals_reset_iter(kval); for (i = 0; i < k; i++) { const MultiseqPaar * element = (MultiseqPaar *) best_k_vals_next(kval); printf("%lu\t%lu\t%lu\n", element->id1, element->id2, element->distance); } best_k_vals_delete(kval); multiseq_threads_delete(threads); muliseq_delete(multiseq); }
/** * Handles the arithmetic operation signal. * * @param p0 the internal memory * @param p1 the knowledge memory * @param p2 the knowledge memory count * @param p3 the knowledge memory size * @param p4 the signal memory * @param p5 the signal memory count * @param p6 the signal memory size * @param p7 the shutdown flag * @param p8 the signal memory interrupt request flag * @param p9 the signal memory mutex * @param p10 the model / signal / operation * @param p11 the model / signal / operation count * @param p12 the details / parameters * @param p13 the details / parameters count * @param p14 the signal priority (Hand over as reference!) * @param p15 the signal identification (Hand over as reference!) * @param p16 the comparison result */ void handle_arithmetic_operation(void* p0, void* p1, void* p2, void* p3, void* p4, void* p5, void* p6, void* p7, void* p8, void* p9, void* p10, void* p11, void* p12, void* p13, void* p14, void* p15, void* p16) { log_terminated_message((void*) DEBUG_LEVEL_LOG_MODEL, (void*) L"Handle arithmetic operation."); // The comparison result. int* r = (int*) p16; if (*r == *NUMBER_0_INTEGER_MEMORY_MODEL) { compare_equal_arrays(p16, p10, p11, (void*) ADD_ARITHMETIC_OPERATION_CYBOL_MODEL, (void*) ADD_ARITHMETIC_OPERATION_CYBOL_MODEL_COUNT, (void*) WIDE_CHARACTER_PRIMITIVE_MEMORY_ABSTRACTION); if (*r != *NUMBER_0_INTEGER_MEMORY_MODEL) { calculate_addition(p12, p13, p1, p2, p3); } } if (*r == *NUMBER_0_INTEGER_MEMORY_MODEL) { compare_equal_arrays(p16, p10, p11, (void*) DIVIDE_ARITHMETIC_OPERATION_CYBOL_MODEL, (void*) DIVIDE_ARITHMETIC_OPERATION_CYBOL_MODEL_COUNT, (void*) WIDE_CHARACTER_PRIMITIVE_MEMORY_ABSTRACTION); if (*r != *NUMBER_0_INTEGER_MEMORY_MODEL) { //?? calculate_division(p12, p13, p1, p2, p3); } } if (*r == *NUMBER_0_INTEGER_MEMORY_MODEL) { compare_equal_arrays(p16, p10, p11, (void*) MULTIPLY_ARITHMETIC_OPERATION_CYBOL_MODEL, (void*) MULTIPLY_ARITHMETIC_OPERATION_CYBOL_MODEL_COUNT, (void*) WIDE_CHARACTER_PRIMITIVE_MEMORY_ABSTRACTION); if (*r != *NUMBER_0_INTEGER_MEMORY_MODEL) { //?? calculate_multiplication(p12, p13, p1, p2, p3); } } if (*r == *NUMBER_0_INTEGER_MEMORY_MODEL) { compare_equal_arrays(p16, p10, p11, (void*) SUBTRACT_ARITHMETIC_OPERATION_CYBOL_MODEL, (void*) SUBTRACT_ARITHMETIC_OPERATION_CYBOL_MODEL_COUNT, (void*) WIDE_CHARACTER_PRIMITIVE_MEMORY_ABSTRACTION); if (*r != *NUMBER_0_INTEGER_MEMORY_MODEL) { //?? calculate_subtraction(p12, p13, p1, p2, p3); } } }
int main(){ int i,j,n; printf("sin30=%f\n", sin(3.14/6)); /* double measurement[10] = {10, 19, 29, 38, 51, 59, 69, 80, 91, 101}; double measurement_variance = 1; double motion[10] = {10, 10, 10, 10, 10, 10, 10, 10, 10, 10}; double motion_variance = 1; double transpose[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}; calculate_transpose(*P_matrix, *transpose, 4, 4); printf("print the transpose: %f, %f, %f, %f\n", transpose[0][0], transpose[0][1], transpose[0][2], transpose[0][3]); double matrix_mul[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}; calculate_multiplication(*P_matrix, *transpose, 4, 4, 4, 4, *matrix_mul); double matrix_add[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}; calculate_addition(*P_matrix, *transpose, 4, 4, 4, 4, *matrix_add); printf(" matrix_add: %f, %f, %f, %f\n", matrix_add[0][0], matrix_add[0][1], matrix_add[0][2], matrix_add[0][3]); double matrix_tobeinversed[2][2] = {{1, 0}, {0, 1}}; double matrix_inverse[2][2] = {{0, 0}, {0, 0}}; calculate_square_inverse(*matrix_tobeinversed, *matrix_inverse); printf(" matrix_inverse: %f, %f, %f, %f\n", matrix_inverse[0][0], matrix_inverse[0][1], matrix_inverse[1][0], matrix_inverse[1][1]); for(i=0; i<sizeof(measurement)/sizeof(double); i++){ values = update(values[0], values[1], measurement[i], measurement_variance); values = predict(values[0], values[1], motion[i], motion_variance ); }*/ double F_transpose[4][4]; calculate_transpose(*F_matrix, *F_transpose, 4, 4); //print_matrix("F_transpose", *F_transpose, 4, 4); double H_transpose[4][2]; calculate_transpose(*H_matrix, *H_transpose, 2, 4); //print_matrix("H_transpose", *H_transpose, 4, 2); //double start_time = omp_get_wtime (); struct timeval tv; gettimeofday(&tv, 0); double start_mill = (tv.tv_sec) * 1000 + (tv.tv_usec) / 1000 ; //#pragma omp parallel { //#pragma omp for num_threads(1) for(n=0; n<NUM_MEASUREMENTS; n++){ //int tid = omp_get_thread_num(); //int num_threads = omp_get_num_threads (); //printf("tid=%d, num_threads=%d\n", tid, num_threads); //printf("this is the iteration number %d\n", n); // prediction //x = (F * x) + u double matrix_mul[4][1] = {{0}, {0}, {0}, {0}}; calculate_multiplication(*F_matrix, *x_matrix, 4, 4, 4, 1, *matrix_mul); //print_matrix("matrix_mul", *matrix_mul, 4, 1); copy_matrix(*matrix_mul, *x_matrix, 4, 1); ////print_matrix("x_matrix", *x_matrix, 4, 1); //P = F * P * F.transpose() double matrix_mult_PFt[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}; calculate_multiplication(*P_matrix, *F_transpose, 4, 4, 4, 4, *matrix_mult_PFt); //print_matrix("matrix_mult_PFt", *matrix_mult_PFt, 4, 4); calculate_multiplication(*F_matrix, *matrix_mult_PFt, 4, 4, 4, 4, *P_matrix); //print_matrix("P_matrix", *P_matrix, 4, 4); //# measurement update //Z = matrix([measurements[n]]) double Z_matrix[2][1]; /*for(i=0; i<2; i++){ Z_matrix[i][0] = measurements[i][n] ; }*/ if(measurements[1][n] >= 0 && measurements[1][n] < PI/2) { Z_matrix[0][0] = measurements[0][n]/sqrt((pow(tan(measurements[1][n]), 2) +1)); Z_matrix[1][0] = (measurements[0][n]*tan(measurements[1][n]))/sqrt((pow(tan(measurements[1][n]), 2) +1)); } else if(measurements[1][n] >= PI/2 && measurements[1][n] < PI) { Z_matrix[0][0] = -1*measurements[0][n]/sqrt((pow(tan(measurements[1][n]), 2) +1)); Z_matrix[1][0] = -1*(measurements[0][n]*tan(measurements[1][n]))/sqrt((pow(tan(measurements[1][n]), 2) +1)); } else if(measurements[1][n] >= PI && measurements[1][n] < 3*PI/2) { Z_matrix[0][0] = -1*measurements[0][n]/sqrt((pow(tan(measurements[1][n]), 2) +1)); Z_matrix[1][0] = -1*(measurements[0][n]*tan(measurements[1][n]))/sqrt((pow(tan(measurements[1][n]), 2) +1)); } else if(measurements[0][n] >= 3*PI/2 && measurements[0][n] < 2*PI) { Z_matrix[0][0] = measurements[0][n]/sqrt((pow(tan(measurements[1][n]), 2) +1)); Z_matrix[1][0] = (measurements[0][n]*tan(measurements[1][n]))/sqrt((pow(tan(measurements[1][n]), 2) +1)); } print_matrix("Z_matrix", *Z_matrix, 2, 1); //y = Z.transpose() - (H * x) /*H_matrix[0][0] = atan2(x_matrix[1][0], x_matrix[0][0])/x_matrix[0][0]; H_matrix[1][1] = sqrt(pow(x_matrix[0][0], 2) + pow(x_matrix[1][0], 2))/x_matrix[1][0]; printf("H00=%lf, H11=%lf\n",H_matrix[0][0], H_matrix[1][1] );*/ double matrix_mult_Hx[2][1]; calculate_multiplication(*H_matrix, *x_matrix, 2, 4, 4, 1, *matrix_mult_Hx); //print_matrix("matrix_mult_Hx", *matrix_mult_Hx, 2, 1); double y_matrix[2][1]; calculate_subtraction(*Z_matrix, *matrix_mult_Hx, 2, 1, 2, 1, *y_matrix); //print_matrix("y_matrix", *y_matrix, 2, 1); //S = H * P * H.transpose() + R double matrix_mult_PHt[4][2]; calculate_multiplication(*P_matrix, *H_transpose, 4, 4, 4, 2, *matrix_mult_PHt); //print_matrix("matrix_mult_PHt", *matrix_mult_PHt, 4, 2); double matrix_mult_HPHt[2][2]; calculate_multiplication(*H_matrix, *matrix_mult_PHt, 2, 4, 4, 2, *matrix_mult_HPHt); //print_matrix("matrix_mult_HPHt", *matrix_mult_HPHt, 2, 2); double S_matrix[2][2]; calculate_addition(*matrix_mult_HPHt, *R_matrix, 2, 2, 2, 2, *S_matrix); //print_matrix("S_matrix", *S_matrix, 2, 2); //K = P * H.transpose() * S.inverse() double S_inverse[2][2]; calculate_square_inverse(*S_matrix, *S_inverse); //print_matrix("S_inverse", *S_inverse, 2, 2); double matrix_mult_HtSi[4][2]; calculate_multiplication(*H_transpose, *S_inverse, 4, 2, 2, 2, *matrix_mult_HtSi); //print_matrix("matrix_mult_HtSi", *matrix_mult_HtSi, 4, 2); double K_matrix[4][2]; calculate_multiplication(*P_matrix, *matrix_mult_HtSi, 4, 4, 4, 2, *K_matrix); //print_matrix("K_matrix", *K_matrix, 4, 2); //x = x + (K * y) double matrix_mult_Ky[4][1]; calculate_multiplication(*K_matrix, *y_matrix, 4, 2, 2, 1, *matrix_mult_Ky); //print_matrix("matrix_mult_Ky", *matrix_mult_Ky, 4, 1); double matrix_add_x_Ky[4][1]; calculate_addition(*x_matrix, *matrix_mult_Ky, 4, 1, 4, 1, *matrix_add_x_Ky); //print_matrix("matrix_add_x_Ky", *matrix_add_x_Ky, 4, 1); copy_matrix(*matrix_add_x_Ky, *x_matrix, 4, 1); print_matrix("x_matrix", *x_matrix, 4, 1); //P = (I - (K * H)) * P double matrix_mult_KH[4][4]; calculate_multiplication(*K_matrix, *H_matrix, 4, 2, 2, 4, *matrix_mult_KH); //print_matrix("matrix_mult_KH", *matrix_mult_KH, 4, 4); double matrix_sub_I_KH[4][4]; calculate_subtraction(*I_matrix, *matrix_mult_KH, 4, 4, 4, 4, *matrix_sub_I_KH); //print_matrix("matrix_sub_I_KH", *matrix_sub_I_KH, 4, 4); double matrix_mult_IKHP[4][4]; calculate_multiplication(*matrix_sub_I_KH, *P_matrix, 4, 4, 4, 4, *matrix_mult_IKHP); //print_matrix("matrix_mult_IKHP", *matrix_mult_IKHP, 4, 4); copy_matrix(*matrix_mult_IKHP, *P_matrix, 4, 4); ////print_matrix("P_matrix", *P_matrix, 4, 2); } } gettimeofday(&tv, 0); double finish_mill = (tv.tv_sec) * 1000 + (tv.tv_usec) / 1000 ; printf("start_mill = %lf\n", start_mill); printf("finish_mill = %lf\n", finish_mill); printf("total time for the kalman loop execution = %lf\n", finish_mill-start_mill); /*double finish_time = omp_get_wtime(); double total_time = finish_time - start_time; printf("total time for the kalman loop execution = %lf\n", total_time);*/ }