//matrix_sub static void matrix_sub_TwoMatrixesWithElementsSixTenTwoElevenAndOneTwoThreeFour_MatrixWithElementsFiveEightMinusOneSeven(void ** state) { matrix_t * mat1 = matrix_test(6,10,2,11); matrix_t * mat2 = matrix_test(1,2,3,4); assert_int_equal(matrix_getElementOfMatrix(matrix_sub(mat1,mat2),0,0),5); assert_int_equal(matrix_getElementOfMatrix(matrix_sub(mat1,mat2),0,1),8); assert_int_equal(matrix_getElementOfMatrix(matrix_sub(mat1,mat2),1,0),-1); assert_int_equal(matrix_getElementOfMatrix(matrix_sub(mat1,mat2),1,1),7); matrix_free(mat1); matrix_free(mat2); }
/* calculate percent error between A and B in terms of Frobenius norm: 100*norm(A - B)/norm(A) */ double get_percent_error_between_two_mats(mat *A, mat *B){ int m,n; double normA, normB, normA_minus_B; mat *A_minus_B; m = A->nrows; n = A->ncols; A_minus_B = matrix_new(m,n); matrix_copy(A_minus_B, A); matrix_sub(A_minus_B, B); normA = get_matrix_frobenius_norm(A); normB = get_matrix_frobenius_norm(B); normA_minus_B = get_matrix_frobenius_norm(A_minus_B); matrix_delete(A_minus_B); return 100.0*normA_minus_B/normA; }
strategy_profile_t *new_point(strategy_profile_t *x, strategy_profile_t *xp, double eps) { //printf("=======\n"); strategy_profile_t *np = malloc(sizeof(strategy_profile_t)); np->players = xp->players; np->strategies = malloc(sizeof(matrix_t *) * np->players); int i; for (i = 0; i < x->players; ++i) { matrix_t *tmp = matrix_sub(xp->strategies[i], x->strategies[i]); matrix_mul_const_in(tmp, eps); np->strategies[i] = matrix_add(x->strategies[i], tmp); matrix_free(tmp); //matrix_print(matrix_trans(xp->strategies[i])); //matrix_print(matrix_trans(x->strategies[i])); //matrix_print(matrix_trans(np->strategies[i])); } return np; }
DECLARE_TEST(matrix, ops) { matrix_t mat; VECTOR_ALIGN float32_t aligned[] = { 1, -2, 3, -4, -5, 6, -7, 8, 9, 10, 11, 12, -13, -14, -15, -16 }; float32_t unaligned[] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 }; mat = matrix_transpose(matrix_aligned(aligned)); EXPECT_VECTOREQ(mat.row[0], vector(1, -5, 9, -13)); EXPECT_VECTOREQ(mat.row[1], vector(-2, 6, 10, -14)); EXPECT_VECTOREQ(mat.row[2], vector(3, -7, 11, -15)); EXPECT_VECTOREQ(mat.row[3], vector(-4, 8, 12, -16)); mat = matrix_add(matrix_zero(), matrix_zero()); EXPECT_VECTOREQ(mat.row[0], vector_zero()); EXPECT_VECTOREQ(mat.row[1], vector_zero()); EXPECT_VECTOREQ(mat.row[2], vector_zero()); EXPECT_VECTOREQ(mat.row[3], vector_zero()); mat = matrix_add(matrix_zero(), matrix_identity()); EXPECT_VECTOREQ(mat.row[0], vector(1, 0, 0, 0)); EXPECT_VECTOREQ(mat.row[1], vector(0, 1, 0, 0)); EXPECT_VECTOREQ(mat.row[2], vector(0, 0, 1, 0)); EXPECT_VECTOREQ(mat.row[3], vector(0, 0, 0, 1)); mat = matrix_add(matrix_identity(), matrix_zero()); EXPECT_VECTOREQ(mat.row[0], vector(1, 0, 0, 0)); EXPECT_VECTOREQ(mat.row[1], vector(0, 1, 0, 0)); EXPECT_VECTOREQ(mat.row[2], vector(0, 0, 1, 0)); EXPECT_VECTOREQ(mat.row[3], vector(0, 0, 0, 1)); mat = matrix_add(matrix_aligned(aligned), matrix_unaligned(unaligned)); EXPECT_VECTOREQ(mat.row[0], vector(1, -1, 5, -1)); EXPECT_VECTOREQ(mat.row[1], vector(-1, 11, -1, 15)); EXPECT_VECTOREQ(mat.row[2], vector(17, 19, 21, 23)); EXPECT_VECTOREQ(mat.row[3], vector(-1, -1, -1, -1)); mat = matrix_add(matrix_aligned(aligned), matrix_transpose(matrix_aligned(aligned))); EXPECT_VECTOREQ(mat.row[0], vector(2, -7, 12, -17)); EXPECT_VECTOREQ(mat.row[1], vector(-7, 12, 3, -6)); EXPECT_VECTOREQ(mat.row[2], vector(12, 3, 22, -3)); EXPECT_VECTOREQ(mat.row[3], vector(-17, -6, -3, -32)); mat = matrix_sub(matrix_zero(), matrix_zero()); EXPECT_VECTOREQ(mat.row[0], vector_zero()); EXPECT_VECTOREQ(mat.row[1], vector_zero()); EXPECT_VECTOREQ(mat.row[2], vector_zero()); EXPECT_VECTOREQ(mat.row[3], vector_zero()); mat = matrix_sub(matrix_zero(), matrix_identity()); EXPECT_VECTOREQ(mat.row[0], vector(-1, 0, 0, 0)); EXPECT_VECTOREQ(mat.row[1], vector(0, -1, 0, 0)); EXPECT_VECTOREQ(mat.row[2], vector(0, 0, -1, 0)); EXPECT_VECTOREQ(mat.row[3], vector(0, 0, 0, -1)); mat = matrix_add(matrix_identity(), matrix_zero()); EXPECT_VECTOREQ(mat.row[0], vector(1, 0, 0, 0)); EXPECT_VECTOREQ(mat.row[1], vector(0, 1, 0, 0)); EXPECT_VECTOREQ(mat.row[2], vector(0, 0, 1, 0)); EXPECT_VECTOREQ(mat.row[3], vector(0, 0, 0, 1)); mat = matrix_sub(matrix_aligned(aligned), matrix_unaligned(unaligned)); EXPECT_VECTOREQ(mat.row[0], vector(1, -3, 1, -7)); EXPECT_VECTOREQ(mat.row[1], vector(-9, 1, -13, 1)); EXPECT_VECTOREQ(mat.row[2], vector(1, 1, 1, 1)); EXPECT_VECTOREQ(mat.row[3], vector(-25, -27, -29, -31)); mat = matrix_sub(matrix_aligned(aligned), matrix_transpose(matrix_aligned(aligned))); EXPECT_VECTOREQ(mat.row[0], vector(0, 3, -6, 9)); EXPECT_VECTOREQ(mat.row[1], vector(-3, 0, -17, 22)); EXPECT_VECTOREQ(mat.row[2], vector(6, 17, 0, 27)); EXPECT_VECTOREQ(mat.row[3], vector(-9, -22, -27, 0)); mat = matrix_mul(matrix_zero(), matrix_zero()); EXPECT_VECTOREQ(mat.row[0], vector_zero()); EXPECT_VECTOREQ(mat.row[1], vector_zero()); EXPECT_VECTOREQ(mat.row[2], vector_zero()); EXPECT_VECTOREQ(mat.row[3], vector_zero()); mat = matrix_mul(matrix_zero(), matrix_identity()); EXPECT_VECTOREQ(mat.row[0], vector_zero()); EXPECT_VECTOREQ(mat.row[1], vector_zero()); EXPECT_VECTOREQ(mat.row[2], vector_zero()); EXPECT_VECTOREQ(mat.row[3], vector_zero()); mat = matrix_mul(matrix_identity(), matrix_zero()); EXPECT_VECTOREQ(mat.row[0], vector_zero()); EXPECT_VECTOREQ(mat.row[1], vector_zero()); EXPECT_VECTOREQ(mat.row[2], vector_zero()); EXPECT_VECTOREQ(mat.row[3], vector_zero()); mat = matrix_mul(matrix_identity(), matrix_identity()); EXPECT_VECTOREQ(mat.row[0], vector(1, 0, 0, 0)); EXPECT_VECTOREQ(mat.row[1], vector(0, 1, 0, 0)); EXPECT_VECTOREQ(mat.row[2], vector(0, 0, 1, 0)); EXPECT_VECTOREQ(mat.row[3], vector(0, 0, 0, 1)); mat = matrix_mul(matrix_aligned(aligned), matrix_unaligned(unaligned)); EXPECT_VECTOREQ(mat.row[0], vector(-8 + 24 - 48, 1 - 10 + 27 - 4 * 13, 2 - 12 + 30 - 4 * 14, 3 - 14 + 33 - 4 * 15)); EXPECT_VECTOREQ(mat.row[1], vector(6 * 4 - 7 * 8 + 8 * 12, -5 * 1 + 6 * 5 - 7 * 9 + 8 * 13, -5 * 2 + 6 * 6 - 7 * 10 + 8 * 14, -5 * 3 + 6 * 7 - 7 * 11 + 8 * 15)); EXPECT_VECTOREQ(mat.row[2], vector(10 * 4 + 11 * 8 + 12 * 12, 9 * 1 + 10 * 5 + 11 * 9 + 12 * 13, 9 * 2 + 10 * 6 + 11 * 10 + 12 * 14, 9 * 3 + 10 * 7 + 11 * 11 + 12 * 15)); EXPECT_VECTOREQ(mat.row[3], vector(-14 * 4 - 15 * 8 - 16 * 12, -13 * 1 - 14 * 5 - 15 * 9 - 16 * 13, -13 * 2 - 14 * 6 - 15 * 10 - 16 * 14, -13 * 3 - 14 * 7 - 15 * 11 - 16 * 15)); return 0; }
void lars_estimate(lars_type * lars , int max_vars , double max_beta , bool verbose) { int nvars = matrix_get_columns( lars->X ); int nsample = matrix_get_rows( lars->X ); matrix_type * X = matrix_alloc( nsample, nvars ); // Allocate local X and Y variables matrix_type * Y = matrix_alloc( nsample, 1 ); // which will hold the normalized data lars_estimate_init( lars , X , Y); // during the estimation process. { matrix_type * G = matrix_alloc_gram( X , true ); matrix_type * mu = matrix_alloc( nsample , 1 ); matrix_type * C = matrix_alloc( nvars , 1 ); matrix_type * Y_mu = matrix_alloc_copy( Y ); int_vector_type * active_set = int_vector_alloc(0,0); int_vector_type * inactive_set = int_vector_alloc(0,0); int active_size; if ((max_vars <= 0) || (max_vars > nvars)) max_vars = nvars; { int i; for (i=0; i < nvars; i++) int_vector_iset( inactive_set , i , i ); } matrix_set( mu , 0 ); while (true) { double maxC = 0; /* The first step is to calculate the correlations between the covariates, and the current residual. All the currently inactive covariates are searched; the covariate with the greatest correlation with (Y - mu) is selected and added to the active set. */ matrix_sub( Y_mu , Y , mu ); // Y_mu = Y - mu matrix_dgemm( C , X , Y_mu , true , false , 1.0 , 0); // C = X' * Y_mu { int i; int max_set_index = 0; for (i=0; i < int_vector_size( inactive_set ); i++) { int set_index = i; int var_index = int_vector_iget( inactive_set , set_index ); double value = fabs( matrix_iget(C , var_index , 0) ); if (value > maxC) { maxC = value; max_set_index = set_index; } } /* Remove element corresponding to max_set_index from the inactive set and add it to the active set: */ int_vector_append( active_set , int_vector_idel( inactive_set , max_set_index )); } active_size = int_vector_size( active_set ); /* Now we have calculated the correlations between all the covariates and the current residual @Y_mu. The correlations are stored in the matrix @C. The value of the maximum correlation is stored in @maxC. Based on the value of @maxC we have added one new covariate to the model, technically by moving the index from @inactive_set to @active_set. */ /*****************************************************************/ { matrix_type * weights = matrix_alloc( active_size , 1); double scale; /*****************************************************************/ /* This scope should compute and initialize the variables @weights and @scale. */ { matrix_type * subG = matrix_alloc( active_size , active_size ); matrix_type * STS = matrix_alloc( active_size , active_size ); matrix_type * sign_vector = matrix_alloc( active_size , 1); int i , j; /* STS = S' o S where 'o' is the Schur product and S is given by: [ s1 s2 s3 s4 ] S = [ s1 s2 s3 s4 ] [ s1 s2 s3 s4 ] [ s1 s2 s3 s4 ] Where si is the sign of the correlation between (active) variable 'i' and Y_mu. */ for (i=0; i < active_size ; i++) { int vari = int_vector_iget( active_set , i ); double signi = sgn( matrix_iget( C , vari , 0)); matrix_iset( sign_vector , i , 0 , signi ); for (j=0; j < active_size; j++) { int varj = int_vector_iget( active_set , j ); double signj = sgn( matrix_iget( C , varj , 0)); matrix_iset( STS , i , j , signi * signj ); } } // Extract the elements from G corresponding to active indices and // copy to the matrix subG: for (i=0; i < active_size ; i++) { int ii = int_vector_iget( active_set , i ); for (j=0; j < active_size; j++) { int jj = int_vector_iget( active_set , j ); matrix_iset( subG , i , j , matrix_iget(G , ii , jj)); } } // Weights matrix_inplace_mul( subG , STS ); matrix_inv( subG ); { matrix_type * ones = matrix_alloc( active_size , 1 ); matrix_type * GA1 = matrix_alloc( active_size , 1 ); matrix_set( ones , 1.0 ); matrix_matmul( GA1 , subG , ones ); scale = 1.0 / sqrt( matrix_get_column_sum( GA1 , 0 )); matrix_mul( weights , GA1 , sign_vector ); matrix_scale( weights , scale ); matrix_free( GA1 ); matrix_free( ones ); } matrix_free( sign_vector ); matrix_free( subG ); matrix_free( STS ); } /******************************************************************/ /* The variables weight and scale have been calculated, proceed to calculate the step length @gamma. */ { int i; double gamma; { matrix_type * u = matrix_alloc( nsample , 1 ); int j; for (i=0; i < nsample; i++) { double row_sum = 0; for (j =0; j < active_size; j++) row_sum += matrix_iget( X , i , int_vector_iget( active_set , j)) * matrix_iget(weights , j , 0 ); matrix_iset( u , i , 0 , row_sum ); } gamma = maxC / scale; if (active_size < matrix_get_columns( X )) { matrix_type * equi_corr = matrix_alloc( nvars , 1 ); matrix_dgemm( equi_corr , X , u , true , false , 1.0 , 0); // equi_corr = X'·u for (i=0; i < (nvars - active_size); i++) { int var_index = int_vector_iget( inactive_set , i ); double gamma1 = (maxC - matrix_iget(C , var_index , 0 )) / ( scale - matrix_iget( equi_corr , var_index , 0)); double gamma2 = (maxC + matrix_iget(C , var_index , 0 )) / ( scale + matrix_iget( equi_corr , var_index , 0)); if ((gamma1 > 0) && (gamma1 < gamma)) gamma = gamma1; if ((gamma2 > 0) && (gamma2 < gamma)) gamma = gamma2; } matrix_free( equi_corr ); } /* Update the current estimated 'location' mu. */ matrix_scale( u , gamma ); matrix_inplace_add( mu , u ); matrix_free( u ); } /* We have calculated the step length @gamma, and the @weights. Update the @beta matrix. */ for (i=0; i < active_size; i++) matrix_iset( lars->beta , int_vector_iget( active_set , i ) , active_size - 1 , gamma * matrix_iget( weights , i , 0)); if (active_size > 1) for (i=0; i < nvars; i++) matrix_iadd( lars->beta , i , active_size - 1 , matrix_iget( lars->beta , i , active_size - 2)); matrix_free( weights ); } } if (active_size == max_vars) break; if (max_beta > 0) { double beta_norm2 = matrix_get_column_abssum( lars->beta , active_size - 1 ); if (beta_norm2 > max_beta) { // We stop - we will use an interpolation between this beta estimate and // the previous, to ensure that the |beta| = max_beta criteria is satisfied. if (active_size >= 2) { double beta_norm1 = matrix_get_column_abssum( lars->beta , active_size - 2 ); double s = (max_beta - beta_norm1)/(beta_norm2 - beta_norm1); { int j; for (j=0; j < nvars; j++) { double beta1 = matrix_iget( lars->beta , j , active_size - 2 ); double beta2 = matrix_iget( lars->beta , j , active_size - 1 ); matrix_iset( lars->beta , j , active_size - 1 , beta1 + s*(beta2 - beta1)); } } } break; } } } matrix_free( G ); matrix_free( mu ); matrix_free( C ); matrix_free( Y_mu ); int_vector_free( active_set ); int_vector_free( inactive_set ); matrix_resize( lars->beta , nvars , active_size , true ); if (verbose) matrix_pretty_fprint( lars->beta , "beta" , "%12.5f" , stdout ); lars_select_beta( lars , active_size - 1); } matrix_free( X ); matrix_free( Y ); }
int altitude_estimator::update(float accelz, float baro, float dt) { if (dt > 0.2f || dt < 0) return -1; bool invalid_baro_data = isnan(baro); float dtsq = dt*dt; float dtsq2 = dtsq/2; float F[16] = { 1, dt, dtsq2, dtsq2, 0, 1, dt, dt, 0, 0, 1, 0, 0, 0, 0, 1, }; float FT[16] = { 1, 0, 0, 0, dt, 1, 0, 0, dtsq2, dt, 1, 0, dtsq2, dt, 0, 1, }; static float Hab[2*4] = { 1, 0, 0, 0, 0, 0, 1, 0, }; static float HabT[4*2] = { 1, 0, 0, 0, 0, 1, 0, 0, }; // accelerometer only static float Ha[2*4] = { 0, 0, 1, 0, }; static float HaT[4*2] = { 0, 0, 1, 0, }; float zk_ab[2] = {baro, accelz}; float zk_a[2] = {accelz}; float *H = invalid_baro_data ? Ha : Hab; float *HT = invalid_baro_data ? HaT : HabT; float *zk = invalid_baro_data ? zk_a : zk_ab; int observation_count = invalid_baro_data ? 1 : 2; float state1[4]; float P1[16]; float tmp[16]; float tmp2[16]; float tmp3[16]; float kg[8]; // predict matrix_mul(state1, F, 4, 4, state, 4, 1); matrix_mul(tmp, P, 4, 4, FT, 4, 4); matrix_mul(P1, F, 4, 4, tmp, 4, 4); // covariance float Q2[16]; for(int i=0; i<16; i++) Q2[i] = Q[i] * dt / 0.003f; // normalize Q to dt = 0.003s matrix_add(P1, Q2, 4, 4); // controll vector //state1[2] = state1[2] * 0.8f * 0.2f * (target_accel); // if (invalid_baro_data) // state1[1] *= 0.995f; // update // kg matrix_mul(tmp, P1, 4, 4, HT, 4, observation_count); matrix_mul(tmp2, H, observation_count, 4, P1, 4, 4); matrix_mul(tmp3, tmp2, observation_count, 4, HT, 4, observation_count); if (observation_count == 2) { matrix_add(tmp3, compensate_land_effect ? R2 : R, observation_count, observation_count); inverse_matrix2x2(tmp3); } else { tmp3[0] += R[3]; tmp3[0] = 1.0f / tmp3[0]; } matrix_mul(kg, tmp, 4, observation_count, tmp3, observation_count, observation_count); // update state // residual matrix_mul(tmp, H, observation_count, 4, state1, 4, 1); matrix_sub(zk, tmp, observation_count, 1); matrix_mul(tmp, kg, 4, observation_count, zk, observation_count, 1); matrix_mov(state, state1, 4, 1); matrix_add(state, tmp, 4, 1); // update P float I[16] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, }; matrix_mul(tmp, kg, 4, observation_count, H, observation_count, 4); matrix_sub(I, tmp, 4, 4); matrix_mul(P, I, 4, 4, P1, 4, 4); TRACE("time=%.3f,state:%.2f,%.2f,%.2f,%.2f, raw:%.3f, accelz:%.3f \n", systimer->gettime()/1000000.0f, state[0], state[1], state[2], state[3], baro, accelz); return 0; }
void cox_test(double** covariates, size_t num_features_in_covariate, size_t num_samples, double* time, double* censor, double* coefficients, double** variance) { // declare variables, init values and allocate memory // gsl matrices matrix_t* coefficients_matrix_p = NULL; matrix_t* information_matrix_p = NULL; matrix_t* information_matrix_inverse_p = NULL; matrix_t* score_matrix_p = NULL; matrix_t* error_matrix_p = NULL; matrix_t* variance_matrix_p = NULL; // other variables double denominator = 0, numerator = 0; double error1 = 1, error2 = 1; double* risk_factor = (double*) calloc(num_samples, sizeof(double)); double* score = (double*) calloc(num_features_in_covariate, sizeof(double)); double** expected_covariate = (double**) calloc(num_features_in_covariate, sizeof(double*)); double** information = (double**) calloc(num_features_in_covariate, sizeof(double*)); for (size_t i = 0; i < num_features_in_covariate; i++) { coefficients[i] = 0.1; expected_covariate[i] = (double*) calloc(num_samples, sizeof(double)); information[i] = (double*) calloc(num_features_in_covariate, sizeof(double)); } // create gsl matrices coefficients_matrix_p = matrix_new(num_features_in_covariate, 1); matrix_init(0.1, coefficients_matrix_p); information_matrix_p = matrix_new(num_features_in_covariate, num_features_in_covariate); score_matrix_p = matrix_new(num_features_in_covariate, 1); error_matrix_p = matrix_new(num_features_in_covariate, 1); information_matrix_inverse_p = matrix_new(num_features_in_covariate, num_features_in_covariate); while((error1 > COX_ERROR_LIMIT) || (error2 > COX_ERROR_LIMIT)) { for (size_t i = 0; i < num_samples; ++i) { risk_factor[i] = 1.0; for (size_t s = 0; s < num_features_in_covariate; ++s) { risk_factor[i] *= exp(coefficients[s] * covariates[s][i]); } } for (size_t j = 0; j < num_features_in_covariate; j++) { score[j] = 0.0; for (size_t i = 0; i < num_samples; i++) { for (size_t k = 0; k < num_samples; k++) { if (time[k] >= time[i]) { denominator += risk_factor[k]; numerator += covariates[j][k] * risk_factor[k]; } } expected_covariate[j][i] = numerator / denominator; score[j] += censor[i] * (covariates[j][i] - expected_covariate[j][i]); numerator = 0.0; denominator = 0.0; } } for (size_t r = 0; r < num_features_in_covariate; r++) { for (size_t s = 0; s < num_features_in_covariate; s++) { information[r][s] = 0.0; for (size_t i = 0; i < num_samples; i++) { for (size_t k = 0; k < num_samples; k++) { if (time[k] >= time[i]) { denominator += risk_factor[k]; numerator += (covariates[r][k] * covariates[s][k] * risk_factor[k]); } } information[r][s] += censor[i] * (expected_covariate[r][i] * expected_covariate[s][i] - (numerator / denominator)); numerator = 0.0; denominator = 0.0; } } } // fill information_matrix matrix_fill(information, num_features_in_covariate, num_features_in_covariate, information_matrix_p); // fill the matrix with data // fill score_matrix from score array for (size_t i = 0; i < num_features_in_covariate; i++) { matrix_set(i, 0, score[i], score_matrix_p); } // calculate error matrix: inv(information_matrix) * score_matrix matrix_inv(information_matrix_p, information_matrix_inverse_p); matrix_mul(information_matrix_inverse_p, score_matrix_p, error_matrix_p); // calculate coefficients matrix coefficients_matrix_p = matrix_sub(coefficients_matrix_p, error_matrix_p); // fill coefficientes for (size_t i = 0; i < num_features_in_covariate; i++) { coefficients[i] = matrix_get(i, 0, coefficients_matrix_p); } error1 = sqrt(matrix_Fnorm(error_matrix_p)); error2 = sqrt(matrix_Fnorm(score_matrix_p)); } // end of while // calculate variance: (-1 * inv(information_matrix)) variance_matrix_p = matrix_scale(information_matrix_inverse_p, -1.0); for (size_t i = 0; i < num_features_in_covariate; i++) { for (size_t j = 0; j < num_features_in_covariate; j++) { variance[i][j] = matrix_get(i, j, variance_matrix_p); } } // free gsl matrices matrix_free(coefficients_matrix_p); matrix_free(information_matrix_p); matrix_free(information_matrix_inverse_p); matrix_free(score_matrix_p); matrix_free(error_matrix_p); variance_matrix_p = NULL; // points to information_matrix_inverse_p previously freed matrix_free(variance_matrix_p); // free other resources free(risk_factor); free(score); for (size_t i = 0; i < num_features_in_covariate; i++) { free(expected_covariate[i]); free(information[i]); } free(expected_covariate); free(information); return; }
/* Function: strassen_multiply -------------------------- Internal function. Matrix multiplication through Strenssen algorithm. Calculates C = AB. Dimensions of A and B must be power of 2. Parameters: a - matrix A rA_s - row start index of A rA_e - row end index of A cA_s - column start index of A cA_e - column end index of A b - matrix B rB_s - column start index 0f B rB_e - column end index of B cB_s - column start index of B cB_e - column end index of B c - result matrix */ void strassen_multiply( double **a, int rA_s, int rA_e, int cA_s, int cA_e, double **b, int rB_s, int rB_e, int cB_s, int cB_e, double **c) { if (((cA_e - cA_s) < 1) || ((rA_e - rA_s) < 1) || ((cB_e - cB_s) < 1) || ((cA_e - cA_s + 1 < SMALL_DIM) && (rA_e - rA_s + 1 < SMALL_DIM) && (cB_e - cB_s + 1 < SMALL_DIM))) { for (int i = 0; i <= (rA_e - rA_s); ++i) { for (int j = 0; j <= (cB_e - cB_s); ++j) { c[i][j] = 0; for (int k = 0; k <= (cA_e - cA_s); ++k) { c[i][j] += a[i + rA_s][k + cA_s] * b[k + rB_s][j + cB_s]; } } } } else { // Intermediate matrix initialization double ***m = (double***)malloc(7 * sizeof(double**)); double ***c_sub = (double***)malloc(4 * sizeof(double**)); int mR = rA_e - rA_s + 1; int mC = cB_e - cB_s + 1; for (int i = 0; i < 7; ++i) { m[i] = (double**)malloc((mR / 2) * sizeof(double*)); for (int j = 0; j < mR / 2; ++j) { m[i][j] = (double*)malloc((mC / 2) * sizeof(double)); } } // Gets results of 7 intermediate matrices int rA_m = (rA_s + rA_e) / 2; int cA_m = (cA_s + cA_e) / 2; int rB_m = (rB_s + rB_e) / 2; int cB_m = (cB_s + cB_e) / 2; // Temporary pointers double **temp1; double **temp2; // Matrix m1 temp1 = matrix_sum(a, rA_s, rA_m, cA_s, cA_m, rA_m + 1, rA_e, cA_m + 1, cA_e); temp2 = matrix_sum(b, rB_s, rB_m, cB_s, cB_m, rB_m + 1, rB_e, cB_m + 1, cB_e); strassen_multiply(temp1, 0, rA_m - rA_s, 0, cA_m - cA_s, temp2, 0, rB_m - rB_s, 0, cB_m - cB_s, m[0]); clear2D(&temp1, rA_m - rA_s + 1); clear2D(&temp2, rB_m - rB_s + 1); // Matrix m2 temp1 = matrix_sum(a, rA_m + 1, rA_e, cA_s, cA_m, rA_m + 1, rA_e, cA_m + 1, cA_e); strassen_multiply(temp1, 0, rA_m - rA_s, 0, cA_m - cA_s, b, rB_s, rB_m, cB_s, cB_m, m[1]); clear2D(&temp1, rA_e - rA_m); // Matrix m3 temp1 = matrix_sub(b, rB_s, rB_m, cB_m + 1, cB_e, rB_m + 1, rB_e, cB_m + 1, cB_e); strassen_multiply(a, rA_s, rA_m, cA_s, cA_m, temp1, 0, rB_m - rB_s, 0, cB_m - cB_s, m[2]); clear2D(&temp1, rB_m - rB_s + 1); // Matrix m4 temp1 = matrix_sub(b, rB_m + 1, rB_e, cB_s, cB_m, rB_s, rB_m, cB_s, cB_m); strassen_multiply(a, rA_m + 1, rA_e, cA_m + 1, cA_e, temp1, 0, rB_m - rB_s, 0, cB_m - cB_s, m[3]); clear2D(&temp1, rB_e - rB_m); // Matrix m5 temp1 = matrix_sum(a, rA_s, rA_m, cA_s, cA_m, rA_s, rA_m, cA_m + 1, cA_e); strassen_multiply(temp1, 0, rA_m - rA_s, 0, cA_m - cA_s, b, rB_m + 1, rB_e, cB_m + 1, cB_e, m[4]); clear2D(&temp1, rA_m - rA_s + 1); // Matrix m6 temp1 = matrix_sub(a, rA_m + 1, rA_e, cA_s, cA_m, rA_s, rA_m, cA_s, cA_m); temp2 = matrix_sum(b, rB_s, rB_m, cB_s, cB_m, rB_s, rB_m, cB_m + 1, cB_e); strassen_multiply(temp1, 0, rA_m - rA_s, 0, cA_m - cA_s, temp2, 0, rB_m - rB_s, 0, cB_m - cB_s, m[5]); clear2D(&temp1, rA_e - rA_m); clear2D(&temp2, rB_m - rB_s + 1); // Matrix m7 temp1 = matrix_sub(a, rA_s, rA_m, cA_m + 1, cA_e, rA_m + 1, rA_e, cA_m + 1, cA_e); temp2 = matrix_sum(b, rB_m + 1, rB_e, cB_s, cB_m, rB_m + 1, rB_e, cB_m + 1, cB_e); strassen_multiply(temp1, 0, rA_m - rA_s, 0, cA_m - cA_s, temp2, 0, rB_m - rB_s, 0, cB_m - cB_s, m[6]); clear2D(&temp1, rA_m - rA_s + 1); clear2D(&temp2, rB_e - rB_m); // Calculates all result sub-matrices temp1 = sum(m[0], m[3], mR / 2, mC / 2); temp2 = sub(temp1, m[4], mR / 2, mC / 2); c_sub[0] = sum(temp2, m[6], mR / 2, mC / 2); clear2D(&temp1, mR / 2); clear2D(&temp2, mR / 2); c_sub[1] = sum(m[2], m[4], mR / 2, mC / 2); c_sub[2] = sum(m[1], m[3], mR / 2, mC / 2); temp1 = sum(m[0], m[2], mR / 2, mC / 2); temp2 = sum(temp1, m[5], mR / 2, mC / 2); c_sub[3] = sub(temp2, m[1], mR / 2, mC / 2); clear2D(&temp1, mR / 2); clear2D(&temp2, mR / 2); // Free intermediate matrices for (int i = 0; i < 7; ++i) { for (int j = 0; j < mR / 2; ++j) { free(m[i][j]); } free(m[i]); } free(m); // Combine sub-matrices for (int i = 0; i < 4; ++i) { for (int j = 0; j < mR / 2; ++j) { for (int k = 0; k < mC / 2; ++k) { c[(i / 2) * mR / 2 + j][(i % 2) * mC / 2 + k] = c_sub[i][j][k]; } } } // Free sub mmatrices for (int i = 0; i < 4; ++i) { for (int j = 0; j < mR / 2; ++j) { free(c_sub[i][j]); } free(c_sub[i]); } free(c_sub); } }