#include #include #include #include #define True 1 #define False 0 #define DEBUG 1 #define MAIN 1 static float a_data[] = { -2.9730, -1.3520, 0, 0, 0, 0, 0, 0, 0.8100, -3.2920, 0, 0, 0, 0, 0, 0, 5.6860, 5.4770, -27.9460, -4.8140, 0, 0, 0, 0, 10.0840, 17.2900, -69.7320, -20.3540, 0, 0, 0, 0, -7.4680, -5.2230, 31.4590, 3.2510, -16.3460, 0, 0, 0, -7.1240, -6.9220, 35.1730, 6.1250, -12.0520, -4.1940, 0, 0, 27.7530, 25.5390, -133.2210, -21.6390, 49.5530, 12.7810, -13.7670, 0, 12.3930, 13.0540, -63.8820, -12.2340, 19.1180, 9.8250, -34.8400, 11.2870, 12.5990, 14.2840, -67.6410, -14.0190, 17.5820, 12.5210, -53.0700, 19.1930 }; static float b_data[] = { 0.6100, 0.5900, -0.5300, -0.6300, 0.5900, 0.5300, 0.3900, 0.5800, 0.6300 }; static float w_data[] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; /* expected eigenvalues 208.4470 0 0 0 0 0 0 0 0 52.9557 0 0 0 0 0 0 0 0 27.1750 0 0 0 0 0 0 0 0 4.2216 0 0 0 0 0 0 0 0 3.6019 0 0 0 0 0 0 0 0 3.4245 0 0 0 0 0 0 0 0 2.1391 0 0 0 0 0 0 0 0 1.6692 expected response with rank 8 -0.1112 -0.2066 -0.0238 -0.1181 0.0117 0.0002 -0.1765 -0.4110 expected response with rank 7 -0.112 -0.206 -0.024 -0.116 0.023 0.119 -0.011 -0.033 */ int my_multifit_wlinear_svd (const gsl_matrix * X, const gsl_vector * w, const gsl_vector * y, double tol, size_t * rank, gsl_vector * c, gsl_matrix * cov, double *chisq, gsl_multifit_linear_workspace * work); int main (int argc, char **argv) { int i, j, n; int first; int indx = 0; int nrows=9; int ncols = 8; float chi = 0; size_t rank; gsl_matrix *a, *cov; gsl_vector *b, *w, *x; int res; double chisq; /* allocate for arrays, vectors */ float *matrix_a = (float *) malloc(nrows*ncols*sizeof(float)); float *vector_b = (float *) malloc(nrows*sizeof(float)); float *vector_x = (float *) malloc(ncols*sizeof(float)); float *vector_w = (float *) malloc(nrows*sizeof(float)); /* copy data */ memcpy(matrix_a, a_data, ncols * nrows * sizeof(float)); for (i = 0; i < nrows; i++) { vector_b[i] = b_data[i]; } for (i = 0; i < nrows; i++) { vector_w[i] = w_data[i]; } printf("\nmatrix_a has %6d rows %6d cols \n", nrows, ncols); for (i = 0; i < nrows; i++) { for (j = 0; j < ncols; j++) { printf(" %8.2f ", matrix_a[i*ncols + j]); } printf(" \n"); } printf("vector_b\n"); for (i = 0; i < nrows; i++) { printf(" %8.4f with weight %4.1f\n", vector_b[i], vector_w[i]); } printf(" \n"); a = gsl_matrix_calloc(nrows, ncols); b = gsl_vector_alloc (nrows); w = gsl_vector_alloc (nrows); x = gsl_vector_alloc (ncols); cov = gsl_matrix_alloc (ncols, ncols); /* make a copy of matrix_a for SVD routine */ printf("ncols=%d nrows=%d\n", ncols, nrows); for (i = 0; i < nrows; i++) { for (j = 0; j < ncols; j++) { printf("%8.1f ", matrix_a[i*ncols + j]); gsl_matrix_set(a, i, j, matrix_a[i*ncols + j]); } printf("\n"); } /* and make a copy of the input vector b, in GSL format */ for (i = 0; i < nrows; i++) { gsl_vector_set(b, i, vector_b[i]); } /* and make a copy of the input vector w, in GSL format */ for (i = 0; i < nrows; i++) { if(vector_w[i] <= 0.001) { gsl_vector_set(w, i, 0.0); } else { gsl_vector_set(w, i, vector_w[i]); } } for (i = 0; i < nrows; i++) { printf("%2d bpm diff=%8.2f weight=%4.3f\n", i, vector_b[i], vector_w[i]); } // make a fit using the svd method with limiting the rank (do not use very small eigenvalues) gsl_multifit_linear_workspace * work = gsl_multifit_linear_alloc (nrows, ncols); res = gsl_multifit_wlinear_svd (a, w, b, (double) 0.02, &rank, x, cov, &chisq, work); gsl_multifit_linear_free (work); printf("chisq=%f rank=%d\n", (float) chisq, rank); for (i = 0; i < ncols; i++) { printf("2 - %2d correction vector %8.3f\n", i, gsl_vector_get(x, i)); vector_x[i] = gsl_vector_get(x, i); } } int my_multifit_wlinear_svd (const gsl_matrix * X, const gsl_vector * w, const gsl_vector * y, double tol, size_t * rank, gsl_vector * c, gsl_matrix * cov, double *chisq, gsl_multifit_linear_workspace * work) { if (X->size1 != y->size) { GSL_ERROR ("number of observations in y does not match rows of matrix X", GSL_EBADLEN); } else if (X->size2 != c->size) { GSL_ERROR ("number of parameters c does not match columns of matrix X", GSL_EBADLEN); } else if (w->size != y->size) { GSL_ERROR ("number of weights does not match number of observations", GSL_EBADLEN); } else if (cov->size1 != cov->size2) { GSL_ERROR ("covariance matrix is not square", GSL_ENOTSQR); } else if (c->size != cov->size1) { GSL_ERROR ("number of parameters does not match size of covariance matrix", GSL_EBADLEN); } else if (X->size1 != work->n || X->size2 != work->p) { GSL_ERROR ("size of workspace does not match size of observation matrix", GSL_EBADLEN); } else { const size_t n = X->size1; const size_t p = X->size2; printf("n=%d p=%d\n", n, p); size_t i, j, p_eff; gsl_matrix *A = work->A; gsl_matrix *Q = work->Q; gsl_matrix *QSI = work->QSI; gsl_vector *S = work->S; gsl_vector *t = work->t; gsl_vector *xt = work->xt; gsl_vector *D = work->D; /* Scale X, A = sqrt(w) X */ gsl_matrix_memcpy (A, X); for (i = 0; i < n; i++) { double wi = gsl_vector_get (w, i); if (wi < 0) wi = 0; { gsl_vector_view row = gsl_matrix_row (A, i); gsl_vector_scale (&row.vector, sqrt (wi)); } } /* Balance the columns of the matrix A */ //gsl_linalg_balance_columns (A, D); /* Decompose A into U S Q^T */ gsl_linalg_SV_decomp_mod (A, QSI, Q, S, xt); /* Solve sqrt(w) y = A c for c, by first computing t = sqrt(w) y */ for (i = 0; i < n; i++) { double wi = gsl_vector_get (w, i); double yi = gsl_vector_get (y, i); if (wi < 0) wi = 0; gsl_vector_set (t, i, sqrt (wi) * yi); } gsl_blas_dgemv (CblasTrans, 1.0, A, t, 0.0, xt); /* Scale the matrix Q, Q' = Q S^-1 */ gsl_matrix_memcpy (QSI, Q); { double alpha0 = gsl_vector_get (S, 0); p_eff = 0; for (j = 0; j < p; j++) { gsl_vector_view column = gsl_matrix_column (QSI, j); double alpha = gsl_vector_get (S, j); if (alpha <= tol * alpha0) { alpha = 0.0; } else { alpha = 1.0 / alpha; p_eff++; } gsl_vector_scale (&column.vector, alpha); } *rank = p_eff; } gsl_vector_set_zero (c); /* Solution */ gsl_blas_dgemv (CblasNoTrans, 1.0, QSI, xt, 0.0, c); /* Unscale the balancing factors */ //gsl_vector_div (c, D); /* Form covariance matrix cov = (Q S^-1) (Q S^-1)^T */ for (i = 0; i < p; i++) { gsl_vector_view row_i = gsl_matrix_row (QSI, i); double d_i = gsl_vector_get (D, i); for (j = i; j < p; j++) { gsl_vector_view row_j = gsl_matrix_row (QSI, j); double d_j = gsl_vector_get (D, j); double s; gsl_blas_ddot (&row_i.vector, &row_j.vector, &s); gsl_matrix_set (cov, i, j, s / (d_i * d_j)); gsl_matrix_set (cov, j, i, s / (d_i * d_j)); } } /* Compute chisq, from residual r = y - X c */ { double r2 = 0; for (i = 0; i < n; i++) { double yi = gsl_vector_get (y, i); double wi = gsl_vector_get (w, i); gsl_vector_const_view row = gsl_matrix_const_row (X, i); double y_est, ri; gsl_blas_ddot (&row.vector, c, &y_est); ri = yi - y_est; r2 += wi * ri * ri; } *chisq = r2; } return GSL_SUCCESS; } }