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