void TransformationModelBSpline::computeFit_()
{
// construct the fit matrix:
gsl_matrix * fit_matrix = gsl_matrix_alloc(size_, ncoeffs_);
bsplines_ = gsl_vector_alloc(ncoeffs_);
for (size_t i = 0; i < size_; ++i)
{
double xi = gsl_vector_get(x_, i);
gsl_bspline_eval(xi, bsplines_, workspace_);
for (size_t j = 0; j < ncoeffs_; ++j)
{
double bspline = gsl_vector_get(bsplines_, j);
gsl_matrix_set(fit_matrix, i, j, bspline);
}
}
// do the fit:
gsl_multifit_linear_workspace * multifit = gsl_multifit_linear_alloc(
size_, ncoeffs_);
coeffs_ = gsl_vector_alloc(ncoeffs_);
cov_ = gsl_matrix_alloc(ncoeffs_, ncoeffs_);
double chisq;
gsl_multifit_wlinear(fit_matrix, w_, y_, coeffs_, cov_, &chisq, multifit);
// clean-up:
gsl_matrix_free(fit_matrix);
gsl_multifit_linear_free(multifit);
// for linear extrapolation (natural spline):
computeLinear_(xmin_, slope_min_, offset_min_, sd_err_left_);
computeLinear_(xmax_, slope_max_, offset_max_, sd_err_right_);
}
void quadratic_fit(void) {
int i, n;
double xi, yi, ei, chisq;
gsl_matrix *X, *cov;
gsl_vector *y, *w, *c;
n = 7;
double x_vec[] = { 0, 100, 200, 300, 400, 500, 600 };
double y_vec[] = { 0, 12.5, 50.0, 112.5, 200.0, 312.5, 450.0 };
double e_vec[] = { 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1 };
X = gsl_matrix_alloc(n, 3);
y = gsl_vector_alloc(n);
w = gsl_vector_alloc(n);
c = gsl_vector_alloc(3);
cov = gsl_matrix_alloc(3, 3);
for (i = 0; i < n; i++) {
xi = x_vec[i];
yi = y_vec[i];
ei = e_vec[i];
printf("%g %g +/- %g\n", xi, yi, ei);
gsl_matrix_set(X, i, 0, 1.0);
gsl_matrix_set(X, i, 1, xi);
gsl_matrix_set(X, i, 2, xi * xi);
gsl_vector_set(y, i, yi);
gsl_vector_set(w, i, 1.0 / (ei * ei));
}
{
gsl_multifit_linear_workspace * work = gsl_multifit_linear_alloc(n, 3);
gsl_multifit_wlinear(X, w, y, c, cov, &chisq, work);
gsl_multifit_linear_free(work);
}
#define C(i) (gsl_vector_get(c,(i)))
#define COV(i,j) (gsl_matrix_get(cov,(i),(j)))
{
printf("# best fit: Y = %g + %g X + %g X^2\n", C(0), C(1), C(2));
printf("# covariance matrix:\n");
printf("[ %+.5e, %+.5e, %+.5e \n", COV(0,0), COV(0,1), COV(0,2));
printf(" %+.5e, %+.5e, %+.5e \n", COV(1,0), COV(1,1), COV(1,2));
printf(" %+.5e, %+.5e, %+.5e ]\n", COV(2,0), COV(2,1), COV(2,2));
printf("# chisq = %g\n", chisq);
}
gsl_matrix_free(X);
gsl_vector_free(y);
gsl_vector_free(w);
gsl_vector_free(c);
gsl_matrix_free(cov);
}
int
fit(double image1[], double image2[], double error[], float factors[],
int n)
{
/*
if (gsl_fit_wlinear
(image1, 1, error, 1, image2, 1, n, &beta, &alpha, &cov00, &cov01,
&cov11, &chisq)) {
fprintf(stderr, "Linear Fitting Failed!\n");
return (1);
}
factors[0] = alpha;
factors[1] = beta;
*/
/*SECOND SET OF VARIABLES!!!*/
int i;
double chisq;
gsl_matrix *X, *cov;
gsl_vector *y, *w, *c;
X = gsl_matrix_alloc(n, 2);
y = gsl_vector_alloc(n);
w = gsl_vector_alloc(n);
c = gsl_vector_alloc(2);
cov = gsl_matrix_alloc(2, 2);
for (i = 0; i < n; i++) {
gsl_matrix_set(X, i, 0, 1.0);
gsl_matrix_set(X, i, 1, image1[i]);
gsl_vector_set(y, i, image2[i]);
gsl_vector_set(w, i, error[i]);
}
gsl_multifit_linear_workspace *work = gsl_multifit_linear_alloc(n, 2);
gsl_multifit_wlinear(X, w, y, c, cov, &chisq, work);
gsl_multifit_linear_free(work);
#define C(i) (gsl_vector_get(c,(i)))
// printf("# best fit: Y = %g + %g X\n", C(0), C(1));
gsl_matrix_free(X);
gsl_vector_free(y);
gsl_vector_free(w);
gsl_vector_free(c);
gsl_matrix_free(cov);
factors[0] = C(1);
factors[1] = C(0);
return 0;
}
/* solve system with lambda = 0 and test against OLS solution */
static void
test_reg1(const gsl_matrix * X, const gsl_vector * y,
const gsl_vector * wts, const double tol,
gsl_multifit_linear_workspace * w, const char * desc)
{
const size_t n = X->size1;
const size_t p = X->size2;
double rnorm, snorm, chisq;
gsl_vector *c0 = gsl_vector_alloc(p);
gsl_vector *c1 = gsl_vector_alloc(p);
gsl_matrix *cov = gsl_matrix_alloc(p, p);
size_t j;
if (wts)
{
gsl_matrix *Xs = gsl_matrix_alloc(n, p);
gsl_vector *ys = gsl_vector_alloc(n);
gsl_multifit_wlinear(X, wts, y, c0, cov, &chisq, w);
gsl_multifit_linear_wstdform1(NULL, X, wts, y, Xs, ys, w);
gsl_multifit_linear_svd(Xs, w);
gsl_multifit_linear_solve(0.0, Xs, ys, c1, &rnorm, &snorm, w);
gsl_matrix_free(Xs);
gsl_vector_free(ys);
}
else
{
gsl_multifit_linear(X, y, c0, cov, &chisq, w);
gsl_multifit_linear_svd(X, w);
gsl_multifit_linear_solve(0.0, X, y, c1, &rnorm, &snorm, w);
}
gsl_test_rel(rnorm*rnorm, chisq, tol,
"test_reg1: %s, lambda = 0, n=%zu p=%zu chisq", desc, n, p);
/* test c0 = c1 */
for (j = 0; j < p; ++j)
{
double c0j = gsl_vector_get(c0, j);
double c1j = gsl_vector_get(c1, j);
gsl_test_rel(c1j, c0j, tol, "test_reg1: %s, lambda = 0, n=%zu p=%zu c0/c1",
desc, n, p);
}
gsl_vector_free(c0);
gsl_vector_free(c1);
gsl_matrix_free(cov);
}
void PolynomialFit::fit()
{
if (d_init_err)
return;
if (d_p > d_n){
QMessageBox::critical((ApplicationWindow *)parent(), tr("QtiPlot - Fit Error"),
tr("You need at least %1 data points for this fit operation. Operation aborted!").arg(d_p));
return;
}
gsl_matrix *X = gsl_matrix_alloc (d_n, d_p);
for (int i = 0; i <d_n; i++){
for (int j= 0; j < d_p; j++)
gsl_matrix_set (X, i, j, pow(d_x[i],j));
}
gsl_vector_view y = gsl_vector_view_array (d_y, d_n);
gsl_vector_view w = gsl_vector_view_array (d_w, d_n);
gsl_multifit_linear_workspace * work = gsl_multifit_linear_alloc (d_n, d_p);
if (d_weighting == NoWeighting)
gsl_multifit_linear (X, &y.vector, d_param_init, covar, &chi_2, work);
else
gsl_multifit_wlinear (X, &w.vector, &y.vector, d_param_init, covar, &chi_2, work);
for (int i = 0; i < d_p; i++)
d_results[i] = gsl_vector_get(d_param_init, i);
gsl_multifit_linear_free (work);
gsl_matrix_free (X);
generateFitCurve();
if (show_legend)
showLegend();
ApplicationWindow *app = (ApplicationWindow *)parent();
if (app->writeFitResultsToLog)
app->updateLog(logFitInfo(0, 0));
}
CAMLprim value ml_gsl_multifit_linear(value wo, value x, value y,
value c, value cov, value ws)
{
double chisq;
_DECLARE_MATRIX2(x,cov);
_DECLARE_VECTOR2(y,c);
_CONVERT_MATRIX2(x,cov);
_CONVERT_VECTOR2(y,c);
if(wo == Val_none)
gsl_multifit_linear(&m_x, &v_y, &v_c, &m_cov,
&chisq, MultifitWS_val(ws));
else {
value w=Field(wo, 0);
_DECLARE_VECTOR(w);
_CONVERT_VECTOR(w);
gsl_multifit_wlinear(&m_x, &v_w, &v_y, &v_c, &m_cov,
&chisq, MultifitWS_val(ws));
}
return copy_double(chisq);
}
void gslpolyfit(double *x,double *y0,int n,int d,double *c0)
{
#ifdef HAVE_GSL_LIB
int i;
double chisq;
gsl_matrix *X,*cov;
gsl_vector *y,*w,*c;
gsl_multifit_linear_workspace *work;
X=gsl_matrix_alloc(n,d+1);
y=gsl_vector_alloc(n);
w=gsl_vector_alloc(n);
c=gsl_vector_alloc(d+1);
cov=gsl_matrix_alloc(d+1,d+1);
for (i=0;i<n;i++)
{
int j;
double xi;
for (j=0,xi=1.;j<=d;j++,xi*=x[i])
gsl_matrix_set(X,i,j,xi);
gsl_vector_set(y,i,y0[i]);
gsl_vector_set(w,i,1.0);
}
work = gsl_multifit_linear_alloc(n,d+1);
gsl_multifit_wlinear(X,w,y,c,cov,&chisq,work);
gsl_multifit_linear_free(work);
for (i=0;i<=d;i++)
c0[i]=gsl_vector_get(c,i);
gsl_matrix_free(cov);
gsl_vector_free(c);
gsl_vector_free(w);
gsl_vector_free(y);
gsl_matrix_free(X);
#else
printf("\n\n\a*** Support for Gnu Scientic Library required but not available! ***\n\n"
"*** Contact author. ***\n\n");
exit(10);
#endif
}
double *LinearModelFitTwoIndependentWeighted(int n, double *y, double *x1, double *x2, double *weights)
{
#define NPARAMS 2
// Allocate
gsl_matrix *matrixX = gsl_matrix_alloc(n, NPARAMS);
gsl_vector *vectorW = gsl_vector_alloc(n);
gsl_vector *vectorY = gsl_vector_alloc(n);
gsl_vector *vectorC = gsl_vector_alloc(1 + NPARAMS);
// Copy data
int i;
for (i = 0; i < n; i++)
{
// NPARAMS
gsl_matrix_set(matrixX, i, 0, x1[i]);
gsl_matrix_set(matrixX, i, 1, x2[i]);
gsl_vector_set(vectorW, i, weights[i]);
gsl_vector_set(vectorY, i, y[i]);
}
// Compute
gsl_multifit_linear_workspace *work = gsl_multifit_linear_alloc(n, NPARAMS);
int ret = gsl_multifit_wlinear(matrixX, vectorW, vectorY, vectorC, /*gsl_matrix * cov*/ NULL, /*double * chisq*/ NULL, work);
gsl_multifit_linear_free(work);
static double coef[NPARAMS + 1];
// NPARAMS+1
coef[0] = gsl_vector_get(vectorC, 0);
coef[1] = gsl_vector_get(vectorC, 1);
coef[2] = gsl_vector_get(vectorC, 2);
// Free
gsl_matrix_free(matrixX);
gsl_vector_free(vectorW);
gsl_vector_free(vectorY);
gsl_vector_free(vectorC);
return coef;
}
// Linear 2D fit
void lfit2d(float *x,float *y,float *z,int n,float *a)
{
int i,j,m;
double chisq;
gsl_matrix *X,*cov;
gsl_vector *yy,*w,*c;
X=gsl_matrix_alloc(n,3);
yy=gsl_vector_alloc(n);
w=gsl_vector_alloc(n);
c=gsl_vector_alloc(3);
cov=gsl_matrix_alloc(3,3);
// Fill matrices
for(i=0;i<n;i++) {
gsl_matrix_set(X,i,0,1.0);
gsl_matrix_set(X,i,1,x[i]);
gsl_matrix_set(X,i,2,y[i]);
gsl_vector_set(yy,i,z[i]);
gsl_vector_set(w,i,1.0);
}
// Do fit
gsl_multifit_linear_workspace *work=gsl_multifit_linear_alloc(n,3);
gsl_multifit_wlinear(X,w,yy,c,cov,&chisq,work);
gsl_multifit_linear_free(work);
// Save parameters
for (i=0;i<3;i++)
a[i]=gsl_vector_get(c,(i));
gsl_matrix_free(X);
gsl_vector_free(yy);
gsl_vector_free(w);
gsl_vector_free(c);
gsl_matrix_free(cov);
return;
}
void linfit_order(int order, int n, float *x, float *y, float *w, float **params) {
int i,j;
double chisq;
gsl_matrix *X, *cov;
gsl_vector *Y, *W, *c;
gsl_multifit_linear_workspace *work;
X = gsl_matrix_alloc(n, order);
Y = gsl_vector_alloc(n);
W = gsl_vector_alloc(n);
c = gsl_vector_alloc(order);
cov = gsl_matrix_alloc(order, order);
for (i=0; i<n; i++) {
for (j=0; j<order; j++) {
gsl_matrix_set(X, i, j, pow((double)x[i], (double)j));
}
gsl_vector_set(Y, i, (double)y[i]);
gsl_vector_set(W, i, (double)w[i]);
}
work = gsl_multifit_linear_alloc(n, order);
gsl_multifit_wlinear(X, W, Y, c, cov, &chisq, work);
*params = malloc(order * sizeof(float));
for (j=0; j<order; j++) {
(*params)[j] = (float)(gsl_vector_get(c, j));
}
gsl_matrix_free(X);
gsl_vector_free(Y);
gsl_vector_free(W);
gsl_vector_free(c);
gsl_matrix_free(cov);
}
void
test_longley ()
{
gsl_multifit_linear_workspace * work =
gsl_multifit_linear_alloc (longley_n, longley_p);
gsl_multifit_robust_workspace * work_rob =
gsl_multifit_robust_alloc (gsl_multifit_robust_ols, longley_n, longley_p);
gsl_matrix_view X = gsl_matrix_view_array (longley_x, longley_n, longley_p);
gsl_vector_view y = gsl_vector_view_array (longley_y, longley_n);
gsl_vector * c = gsl_vector_alloc (longley_p);
gsl_vector * r = gsl_vector_alloc (longley_n);
gsl_matrix * cov = gsl_matrix_alloc (longley_p, longley_p);
double chisq, chisq_res;
double expected_c[7] = { -3482258.63459582,
15.0618722713733,
-0.358191792925910E-01,
-2.02022980381683,
-1.03322686717359,
-0.511041056535807E-01,
1829.15146461355 };
double expected_sd[7] = { 890420.383607373,
84.9149257747669,
0.334910077722432E-01,
0.488399681651699,
0.214274163161675,
0.226073200069370,
455.478499142212 } ;
double expected_chisq = 836424.055505915;
gsl_vector_view diag = gsl_matrix_diagonal (cov);
gsl_vector_view exp_c = gsl_vector_view_array(expected_c, longley_p);
gsl_vector_view exp_sd = gsl_vector_view_array(expected_sd, longley_p);
/* test unweighted least squares */
gsl_multifit_linear (&X.matrix, &y.vector, c, cov, &chisq, work);
gsl_multifit_linear_residuals(&X.matrix, &y.vector, c, r);
gsl_blas_ddot(r, r, &chisq_res);
test_longley_results("longley gsl_multifit_linear",
c, &exp_c.vector,
&diag.vector, &exp_sd.vector,
chisq, chisq_res, expected_chisq);
/* test robust least squares */
gsl_multifit_robust (&X.matrix, &y.vector, c, cov, work_rob);
test_longley_results("longley gsl_multifit_robust",
c, &exp_c.vector,
&diag.vector, &exp_sd.vector,
1.0, 1.0, 1.0);
/* test weighted least squares */
{
size_t i, j;
gsl_vector * w = gsl_vector_alloc (longley_n);
double expected_cov[7][7] = { { 8531122.56783558,
-166.727799925578, 0.261873708176346, 3.91188317230983,
1.1285582054705, -0.889550869422687, -4362.58709870581},
{-166.727799925578, 0.0775861253030891, -1.98725210399982e-05,
-0.000247667096727256, -6.82911920718824e-05, 0.000136160797527761,
0.0775255245956248},
{0.261873708176346, -1.98725210399982e-05, 1.20690316701888e-08,
1.66429546772984e-07, 3.61843600487847e-08, -6.78805814483582e-08,
-0.00013158719037715},
{3.91188317230983, -0.000247667096727256, 1.66429546772984e-07,
2.56665052544717e-06, 6.96541409215597e-07, -9.00858307771567e-07,
-0.00197260370663974},
{1.1285582054705, -6.82911920718824e-05, 3.61843600487847e-08,
6.96541409215597e-07, 4.94032602583969e-07, -9.8469143760973e-08,
-0.000576921112208274},
{-0.889550869422687, 0.000136160797527761, -6.78805814483582e-08,
-9.00858307771567e-07, -9.8469143760973e-08, 5.49938542664952e-07,
0.000430074434198215},
{-4362.58709870581, 0.0775255245956248, -0.00013158719037715,
-0.00197260370663974, -0.000576921112208274, 0.000430074434198215,
2.23229587481535 }} ;
gsl_vector_set_all (w, 1.0);
gsl_multifit_wlinear (&X.matrix, w, &y.vector, c, cov, &chisq, work);
gsl_multifit_linear_residuals(&X.matrix, &y.vector, c, r);
gsl_blas_ddot(r, r, &chisq_res);
test_longley_results("longley gsl_multifit_wlinear",
c, &exp_c.vector,
NULL, NULL,
chisq, chisq_res, expected_chisq);
for (i = 0; i < longley_p; i++)
{
for (j = 0; j < longley_p; j++)
{
gsl_test_rel (gsl_matrix_get(cov,i,j), expected_cov[i][j], 1e-7,
"longley gsl_multifit_wlinear cov(%d,%d)", i, j) ;
}
}
gsl_vector_free(w);
}
gsl_vector_free(c);
gsl_vector_free(r);
gsl_matrix_free(cov);
gsl_multifit_linear_free (work);
gsl_multifit_robust_free (work_rob);
} /* test_longley() */
int
main (int argc, char **argv)
{
int i, n;
double xi, yi, ei, chisq;
gsl_matrix *X, *cov;
gsl_vector *y, *w, *c;
if (argc != 2)
{
fprintf (stderr,"usage: fit n < data\n");
exit (-1);
}
n = atoi (argv[1]);
X = gsl_matrix_alloc (n, 3);
y = gsl_vector_alloc (n);
w = gsl_vector_alloc (n);
c = gsl_vector_alloc (3);
cov = gsl_matrix_alloc (3, 3);
for (i = 0; i < n; i++)
{
int count = fscanf (stdin, "%lg %lg %lg",
&xi, &yi, &ei);
if (count != 3)
{
fprintf (stderr, "error reading file\n");
exit (-1);
}
printf ("%g %g +/- %g\n", xi, yi, ei);
gsl_matrix_set (X, i, 0, 1.0);
gsl_matrix_set (X, i, 1, xi);
gsl_matrix_set (X, i, 2, xi*xi);
gsl_vector_set (y, i, yi);
gsl_vector_set (w, i, 1.0/(ei*ei));
}
{
gsl_multifit_linear_workspace * work
= gsl_multifit_linear_alloc (n, 3);
gsl_multifit_wlinear (X, w, y, c, cov,
&chisq, work);
gsl_multifit_linear_free (work);
}
#define C(i) (gsl_vector_get(c,(i)))
#define COV(i,j) (gsl_matrix_get(cov,(i),(j)))
{
printf ("# best fit: Y = %g + %g X + %g X^2\n",
C(0), C(1), C(2));
printf ("# covariance matrix:\n");
printf ("[ %+.5e, %+.5e, %+.5e \n",
COV(0,0), COV(0,1), COV(0,2));
printf (" %+.5e, %+.5e, %+.5e \n",
COV(1,0), COV(1,1), COV(1,2));
printf (" %+.5e, %+.5e, %+.5e ]\n",
COV(2,0), COV(2,1), COV(2,2));
printf ("# chisq = %g\n", chisq);
}
gsl_matrix_free (X);
gsl_vector_free (y);
gsl_vector_free (w);
gsl_vector_free (c);
gsl_matrix_free (cov);
return 0;
}
/*=============================================================================
* FITTING CODE
*=============================================================================*/
int find_best_alpha(){
//double alpha_2pcf[num_alpha][nr], trials_2pcf[num_alpha][ntrials][total_nr], alpha_err[num_alpha][nr];
double **alpha_2pcf, ***trials_2pcf, **alpha_err;
double low, high, da;
long i,nend,k,j,ir,jk;
double chi2;
double yi,yerr,xi,minerr;
int nbreak, iii,ind,min_ind,max_ind;
long thisseed[Nalpha*num_alpha*ntrials];
#ifdef ALLOUT
char Output2PCF[LINELENGTH];
char ThisFile[15];
#endif
ncoeffs = num_alpha-1;
nbreak =ncoeffs-2;
// Allocate alphavec, which is our result
//alphavec = (double *) calloc(Nalpha,sizeof(double));
//betavec = (double *) calloc(Nalpha,sizeof(double));
alpha_2pcf = (double **) calloc(num_alpha,sizeof(double *));
alpha_err = (double **) calloc(num_alpha,sizeof(double *));
trials_2pcf = (double ***) calloc(num_alpha,sizeof(double **));
for (i=0;i<num_alpha;i++){
alpha_2pcf[i] = (double *) calloc(nr,sizeof(double));
alpha_err[i] = (double *) calloc(nr,sizeof(double));
trials_2pcf[i] = (double **) calloc(ntrials,sizeof(double*));
for (j=0;j<ntrials;j++)
trials_2pcf[i][j] = (double *) calloc(total_nr,sizeof(double));
}
// allocate a cubic bspline workspace (k = 4)
bw = gsl_bspline_alloc(4, nbreak);
B = gsl_vector_alloc(ncoeffs);
alpha_grid = gsl_vector_alloc(num_alpha);
chi2_alpha = gsl_vector_alloc(num_alpha);
X = gsl_matrix_alloc(num_alpha,ncoeffs);
c = gsl_vector_alloc(ncoeffs);
weights = gsl_vector_alloc(num_alpha);
cov = gsl_matrix_alloc(ncoeffs, ncoeffs);
mw = gsl_multifit_linear_alloc(num_alpha, ncoeffs);
// Loop through each mass bin
nend = 0;
iii = 0;
seed = time(NULL);
for (i=0;i<Nalpha;i++){
for(j=0;j<num_alpha;j++){
for(k=0;k<ntrials;k++){
thisseed[iii] = seed + iii;
iii++;
}
}
}
fprintf(stderr,"STARTING LOOPS...\n");
iii = 0;
for (i=0;i<Nalpha;i++){
// Get where to place halos to
nend += ncuts[i];
// Calculate a more optimum alpha_grid for this bin
if (i < 2)
low = alpha_ratio_1*best_alpha;
else{
low = alpha_ratio*best_alpha;
}
high = 1.05*best_alpha;
da = (high-low)/(num_alpha-1);
fprintf(stderr,"STARTING THREADS\n");
#ifndef NO_PAR_FIT
#pragma omp parallel for num_threads(nthreads) private(jk,j,k) \
shared(num_alpha,ntrials,i,alpha_grid,low,da,stderr,Nalpha,alphavec,iii,mcuts,ListOfParticles,\
NPartPerCell,x,y,z,Lbox,Npart,Nlin,HaloMass,nend,mpart,recalc_frac,betavec,thisseed,trials_2pcf,rho,maxr,\
total_nr,Nhalos) default(none)
#endif
for(jk=0;jk<num_alpha*ntrials;jk++){
fprintf(stderr,"Thread %d/%d\n",omp_get_thread_num(),omp_get_num_threads());
float *hx,*hy,*hz,*hR;
double *thisalphavec;
double *ercorr;
unsigned long long *DD;
hx = (float *) calloc(Nhalos,sizeof(float));
hy = (float *) calloc(Nhalos,sizeof(float));
hz = (float *) calloc(Nhalos,sizeof(float));
hR = (float *) calloc(Nhalos,sizeof(float));
thisalphavec = (double *) calloc(Nalpha,sizeof(double));
DD = (unsigned long long *) calloc(total_nr,sizeof(unsigned long long));
ercorr = (double *) calloc(total_nr,sizeof(double));
k=jk%ntrials;
j=jk/ntrials;
fprintf(stderr,"MOVED MEMORY\n");
fprintf(stderr,"GOT k,j: %ld, %ld\n",k,j);
fprintf(stderr,"sizeof M : %f MB\n",(Nlin*Nlin*Nlin*sizeof(double)/1024./1024.));
//define the alpha grid for this mass bin
gsl_vector_set(alpha_grid,j,low+j*da);
// create a local alphavec, to which we add the current gridded alpha
int itemp;
for (itemp=0;itemp<Nalpha;itemp++)
thisalphavec[itemp] = alphavec[itemp];
//fprintf(stderr,"mem copied\n");
thisalphavec[i] = gsl_vector_get(alpha_grid,j);
// Place the halos
#ifdef NO_PAR_FIT
place_halos(nend,HaloMass, Nlin, Npart, x, y, z, NULL,NULL,NULL,Lbox,
rho,thisseed[jk+i*num_alpha*ntrials],
mpart,nthreads,thisalphavec, betavec, mcuts, Nalpha, recalc_frac,hx, hy, hz,
NULL,NULL,NULL,hR,ListOfParticles,NPartPerCell);
#else
place_halos(nend,HaloMass, Nlin, Npart, x, y, z, NULL,NULL,NULL,Lbox,
rho,thisseed[jk+i*num_alpha*ntrials],
mpart,1,thisalphavec, betavec, mcuts, Nalpha, recalc_frac,hx, hy, hz,
NULL,NULL,NULL,hR,ListOfParticles,NPartPerCell);
#endif
fprintf(stderr,"correlating...\n");
//Get the 2PCF
correlate(nend, Lbox,hx,hy,hz,trials_2pcf[j][k], ercorr, DD,total_nr,maxr,1);
fprintf(stderr,"...correlated\n");
free(hx);
free(hy);
free(hz);
free(hR);
free(thisalphavec);
free(ercorr);
free(DD);
}
for (jk=0;jk<num_alpha*ntrials;jk++){
k=jk%ntrials;
j=jk/ntrials;
fprintf(stderr,"RAWCORR %ld, %ld: %e\n",j,k,trials_2pcf[j][k][0]);
}
// #pragma omp parallel for private(ir,j,chi2,k) shared(num_alpha,stderr,i,nr,alpha_2pcf) default(none)
fprintf(stderr,"GOT CORRELATIONS , GETTING CHI2...\n");
for(j=0;j<num_alpha;j++){
//Get mean and stdev of trials_2pcf
chi2 = 0.0;
for (ir=0;ir<nr;ir++){
alpha_2pcf[j][ir] = 0.0;
for(k=0;k<ntrials;k++){
alpha_2pcf[j][ir] += trials_2pcf[j][k][ir+total_nr-nr-2];
}
alpha_2pcf[j][ir] = alpha_2pcf[j][ir]/ntrials;
alpha_err[j][ir] = 0.0;
for(k=0;k<ntrials;k++){
alpha_err[j][ir] += pow((trials_2pcf[j][k][ir+total_nr-nr-2]-alpha_2pcf[j][ir]),2);
}
alpha_err[j][ir] = pow((alpha_err[j][ir]/(ntrials-1)),0.5);
// Now get chi^2 values
#ifdef REL_CHI2
chi2 += pow(((alpha_2pcf[j][ir]-nbody_2pcf[i][ir+total_nr-nr-2])/nbody_2pcf[i][ir+total_nr-nr-2]),2);
#else
chi2 += pow(((alpha_2pcf[j][ir]-nbody_2pcf[i][ir+total_nr-nr-2])/alpha_err[j][ir]),2);
#endif
fprintf(stderr,"%ld, %ld, %e, %e, %e, %e\n",j,ir,alpha_2pcf[j][ir],nbody_2pcf[i][ir+total_nr-nr-2],alpha_err[j][ir],chi2);
}
gsl_vector_set(chi2_alpha,j,chi2/nr);
gsl_vector_set(weights,j,nr/chi2);
}
// #endif //NO_PARALLEL_FIT
//*/
#ifdef ALLOUT
//OUTPUT SOME THINGS FOR CHECKING
sprintf(ThisFile,"/raw.2pcf.%ld",i);
strcpy(Output2PCF,OutputDir);
strcat(Output2PCF,ThisFile);
FILE* output_2pcf;
output_2pcf = fopen(Output2PCF,"w");
//header
fprintf(output_2pcf,"# r, ");
for (k=0;k<num_alpha;k++){
fprintf(output_2pcf,"%e\t",gsl_vector_get(alpha_grid,k));
}
fprintf(output_2pcf,"\n");
//table
for (ir=0;ir<nr;ir++){
fprintf(output_2pcf,"%e\t",(ir+total_nr-nr+0.5)*maxr/total_nr);
for(k=0;k<num_alpha-1;k++){
fprintf(output_2pcf,"%e\t",alpha_2pcf[k][ir]);
}
fprintf(output_2pcf,"%e\n",alpha_2pcf[num_alpha-1][ir]);
}
fclose(output_2pcf);
sprintf(ThisFile,"/raw.err.%ld",i);
strcpy(Output2PCF,OutputDir);
strcat(Output2PCF,ThisFile);
FILE* output_err;
output_err = fopen(Output2PCF,"w");
//header
fprintf(output_err,"# r, ");
for (k=0;k<num_alpha;k++){
fprintf(output_err,"%e\t",gsl_vector_get(alpha_grid,k));
}
fprintf(output_err,"\n");
//table
for (ir=0;ir<nr;ir++){
fprintf(output_err,"%e\t",(ir+total_nr-nr+0.5)*maxr/total_nr);
for(k=0;k<num_alpha-1;k++){
fprintf(output_err,"%e\t",alpha_err[k][ir]);
}
fprintf(output_err,"%e\n",alpha_err[num_alpha-1][ir]);
}
fclose(output_err);
sprintf(ThisFile,"/chi2.%ld",i);
strcpy(Output2PCF,OutputDir);
strcat(Output2PCF,ThisFile);
FILE* chifile;
chifile = fopen(Output2PCF,"w");
fprintf(chifile,"# alpha, chi2, weight\n");
for (k=0;k<num_alpha;k++){
fprintf(chifile,"%e\t%e\t%e\n",gsl_vector_get(alpha_grid,k),gsl_vector_get(chi2_alpha,k),
gsl_vector_get(weights,k));
}
fclose(chifile);
#endif
// Check if final value or initial value is the smallest
minerr = gsl_vector_get(chi2_alpha,0);
ind = 0;
for(k=1;k<num_alpha;k++){
if(minerr>gsl_vector_get(chi2_alpha,k)){
minerr = gsl_vector_get(chi2_alpha,k);
ind = k;
}
}
if(ind==0){
fprintf(stderr,"ERROR: alpha_grid doesn't extend low enough, set alpha_ratio lower");
}
if(ind==num_alpha-1){
fprintf(stderr,"ERROR: alpha_grid doesn't extend high enough, set best_alpha higher");
}
if (ind>=2){
min_ind = ind-2;
}else{
min_ind = 0;
}
if (ind<=num_alpha-3){
max_ind = ind+2;
}else{
max_ind = num_alpha-1;
}
/* use uniform breakpoints on interval */
gsl_bspline_knots_uniform(gsl_vector_get(alpha_grid,0), gsl_vector_get(alpha_grid,num_alpha-1), bw);
/* construct the fit matrix X */
for (k = 0; k < num_alpha; ++k){
double xi = gsl_vector_get(alpha_grid, k);
/* compute B_j(xi) for all j */
gsl_bspline_eval(xi, B, bw);
/* fill in row i of X */
for (j = 0; j < ncoeffs; ++j){
double Bj = gsl_vector_get(B, j);
gsl_matrix_set(X, k, j, Bj);
}
}
fprintf(stderr,"Got to the fit part\n");
/* do the fit */
gsl_multifit_wlinear(X, weights, chi2_alpha, c, cov, &chisq, mw);
// PRINT OUT EVERYTHING WE HAVE SO FAR
fprintf(stderr,"alpha\tchi2_alpha\t\n");
for (k=0;k<num_alpha;k++){
fprintf(stderr,"%e\t%e\t\n",gsl_vector_get(alpha_grid,k),
gsl_vector_get(chi2_alpha,k));
}
#ifdef VERB
dof = num_alpha - ncoeffs;
tss = gsl_stats_wtss(weights->data, 1, chi2_alpha->data, 1, chi2_alpha->size);
Rsq = 1.0 - chisq / tss;
fprintf(stderr, "chisq/dof = %e, Rsq = %f\n",chisq / dof, Rsq);
#endif
for (xi=gsl_vector_get(alpha_grid,0);xi<gsl_vector_get(alpha_grid,num_alpha-1);xi+=0.01){
gsl_bspline_eval(xi, B, bw);
gsl_multifit_linear_est(B, c, cov, &yi, &yerr);
fprintf(stderr,"%e\t%e\t%e\n",xi,yi,gsl_vector_get(B,0));
}
//DO THE MINIMIZATION
alphavec[i] = minimize(gsl_vector_get(alpha_grid,min_ind), gsl_vector_get(alpha_grid,max_ind),
gsl_vector_get(alpha_grid,ind));
best_alpha = alphavec[i];
fprintf(stderr,"\n Best alpha: %f\n\n",best_alpha);
}
return 0;
}
void
test_pontius ()
{
size_t i, j;
{
gsl_multifit_linear_workspace * work =
gsl_multifit_linear_alloc (pontius_n, pontius_p);
gsl_matrix * X = gsl_matrix_alloc (pontius_n, pontius_p);
gsl_vector_view y = gsl_vector_view_array (pontius_y, pontius_n);
gsl_vector * c = gsl_vector_alloc (pontius_p);
gsl_vector * r = gsl_vector_alloc (pontius_n);
gsl_matrix * cov = gsl_matrix_alloc (pontius_p, pontius_p);
gsl_vector_view diag;
double chisq;
double expected_c[3] = { 0.673565789473684E-03,
0.732059160401003E-06,
-0.316081871345029E-14};
double expected_sd[3] = { 0.107938612033077E-03,
0.157817399981659E-09,
0.486652849992036E-16 };
double expected_chisq = 0.155761768796992E-05;
for (i = 0 ; i < pontius_n; i++)
{
for (j = 0; j < pontius_p; j++)
{
gsl_matrix_set(X, i, j, pow(pontius_x[i], j));
}
}
gsl_multifit_linear (X, &y.vector, c, cov, &chisq, work);
gsl_test_rel (gsl_vector_get(c,0), expected_c[0], 1e-10, "pontius gsl_fit_multilinear c0") ;
gsl_test_rel (gsl_vector_get(c,1), expected_c[1], 1e-10, "pontius gsl_fit_multilinear c1") ;
gsl_test_rel (gsl_vector_get(c,2), expected_c[2], 1e-10, "pontius gsl_fit_multilinear c2") ;
diag = gsl_matrix_diagonal (cov);
gsl_test_rel (gsl_vector_get(&diag.vector,0), pow(expected_sd[0],2.0), 1e-10, "pontius gsl_fit_multilinear cov00") ;
gsl_test_rel (gsl_vector_get(&diag.vector,1), pow(expected_sd[1],2.0), 1e-10, "pontius gsl_fit_multilinear cov11") ;
gsl_test_rel (gsl_vector_get(&diag.vector,2), pow(expected_sd[2],2.0), 1e-10, "pontius gsl_fit_multilinear cov22") ;
gsl_test_rel (chisq, expected_chisq, 1e-10, "pontius gsl_fit_multilinear chisq") ;
gsl_multifit_linear_residuals(X, &y.vector, c, r);
gsl_blas_ddot(r, r, &chisq);
gsl_test_rel (chisq, expected_chisq, 1e-10, "pontius gsl_fit_multilinear residuals") ;
gsl_vector_free(c);
gsl_vector_free(r);
gsl_matrix_free(cov);
gsl_matrix_free(X);
gsl_multifit_linear_free (work);
}
{
gsl_multifit_linear_workspace * work =
gsl_multifit_linear_alloc (pontius_n, pontius_p);
gsl_matrix * X = gsl_matrix_alloc (pontius_n, pontius_p);
gsl_vector_view y = gsl_vector_view_array (pontius_y, pontius_n);
gsl_vector * w = gsl_vector_alloc (pontius_n);
gsl_vector * c = gsl_vector_alloc (pontius_p);
gsl_vector * r = gsl_vector_alloc (pontius_n);
gsl_matrix * cov = gsl_matrix_alloc (pontius_p, pontius_p);
double chisq;
double expected_c[3] = { 0.673565789473684E-03,
0.732059160401003E-06,
-0.316081871345029E-14};
double expected_chisq = 0.155761768796992E-05;
double expected_cov[3][3] ={
{2.76754385964916e-01 , -3.59649122807024e-07, 9.74658869395731e-14},
{-3.59649122807024e-07, 5.91630591630603e-13, -1.77210703526497e-19},
{9.74658869395731e-14, -1.77210703526497e-19, 5.62573661988878e-26} };
for (i = 0 ; i < pontius_n; i++)
{
for (j = 0; j < pontius_p; j++)
{
gsl_matrix_set(X, i, j, pow(pontius_x[i], j));
}
}
gsl_vector_set_all (w, 1.0);
gsl_multifit_wlinear (X, w, &y.vector, c, cov, &chisq, work);
gsl_test_rel (gsl_vector_get(c,0), expected_c[0], 1e-10, "pontius gsl_fit_multilinear c0") ;
gsl_test_rel (gsl_vector_get(c,1), expected_c[1], 1e-10, "pontius gsl_fit_multilinear c1") ;
gsl_test_rel (gsl_vector_get(c,2), expected_c[2], 1e-10, "pontius gsl_fit_multilinear c2") ;
for (i = 0; i < pontius_p; i++)
{
for (j = 0; j < pontius_p; j++)
{
gsl_test_rel (gsl_matrix_get(cov,i,j), expected_cov[i][j], 1e-10,
"pontius gsl_fit_wmultilinear cov(%d,%d)", i, j) ;
}
}
gsl_test_rel (chisq, expected_chisq, 1e-10, "pontius gsl_fit_wmultilinear chisq") ;
gsl_multifit_linear_residuals(X, &y.vector, c, r);
gsl_blas_ddot(r, r, &chisq);
gsl_test_rel (chisq, expected_chisq, 1e-10, "pontius gsl_fit_wmultilinear residuals") ;
gsl_vector_free(w);
gsl_vector_free(c);
gsl_vector_free(r);
gsl_matrix_free(cov);
gsl_matrix_free(X);
gsl_multifit_linear_free (work);
}
}
int
gsl_multifit_robust(const gsl_matrix * X,
const gsl_vector * y,
gsl_vector * c,
gsl_matrix * cov,
gsl_multifit_robust_workspace *w)
{
/* check matrix and vector sizes */
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 (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 != w->n || X->size2 != w->p)
{
GSL_ERROR
("size of workspace does not match size of observation matrix",
GSL_EBADLEN);
}
else
{
int s;
double chisq;
const double tol = GSL_SQRT_DBL_EPSILON;
int converged = 0;
size_t numit = 0;
const size_t n = y->size;
double sigy = gsl_stats_sd(y->data, y->stride, n);
double sig_lower;
size_t i;
/*
* if the initial fit is very good, then finding outliers by comparing
* them to the residual standard deviation is difficult. Therefore we
* set a lower bound on the standard deviation estimate that is a small
* fraction of the standard deviation of the data values
*/
sig_lower = 1.0e-6 * sigy;
if (sig_lower == 0.0)
sig_lower = 1.0;
/* compute initial estimates using ordinary least squares */
s = gsl_multifit_linear(X, y, c, cov, &chisq, w->multifit_p);
if (s)
return s;
/* save Q S^{-1} of original matrix */
gsl_matrix_memcpy(w->QSI, w->multifit_p->QSI);
gsl_vector_memcpy(w->D, w->multifit_p->D);
/* compute statistical leverage of each data point */
s = gsl_linalg_SV_leverage(w->multifit_p->A, w->resfac);
if (s)
return s;
/* correct residuals with factor 1 / sqrt(1 - h) */
for (i = 0; i < n; ++i)
{
double h = gsl_vector_get(w->resfac, i);
if (h > 0.9999)
h = 0.9999;
gsl_vector_set(w->resfac, i, 1.0 / sqrt(1.0 - h));
}
/* compute residuals from OLS fit r = y - X c */
s = gsl_multifit_linear_residuals(X, y, c, w->r);
if (s)
return s;
/* compute estimate of sigma from ordinary least squares */
w->stats.sigma_ols = gsl_blas_dnrm2(w->r) / sqrt((double) w->stats.dof);
while (!converged && ++numit <= w->maxiter)
{
double sig;
/* adjust residuals by statistical leverage (see DuMouchel and O'Brien) */
s = gsl_vector_mul(w->r, w->resfac);
if (s)
return s;
/* compute estimate of standard deviation using MAD */
sig = robust_madsigma(w->r, w);
/* scale residuals by standard deviation and tuning parameter */
gsl_vector_scale(w->r, 1.0 / (GSL_MAX(sig, sig_lower) * w->tune));
/* compute weights using these residuals */
s = w->type->wfun(w->r, w->weights);
if (s)
return s;
gsl_vector_memcpy(w->c_prev, c);
/* solve weighted least squares with new weights */
s = gsl_multifit_wlinear(X, w->weights, y, c, cov, &chisq, w->multifit_p);
if (s)
return s;
/* compute new residuals r = y - X c */
s = gsl_multifit_linear_residuals(X, y, c, w->r);
if (s)
return s;
converged = robust_test_convergence(w->c_prev, c, tol);
}
/* compute final MAD sigma */
w->stats.sigma_mad = robust_madsigma(w->r, w);
/* compute robust estimate of sigma */
w->stats.sigma_rob = robust_robsigma(w->r, w->stats.sigma_mad, w->tune, w);
/* compute final estimate of sigma */
w->stats.sigma = robust_sigma(w->stats.sigma_ols, w->stats.sigma_rob, w);
/* store number of iterations */
w->stats.numit = numit;
{
double dof = (double) w->stats.dof;
double rnorm = w->stats.sigma * sqrt(dof); /* see DuMouchel, sec 4.2 */
double ss_err = rnorm * rnorm;
double ss_tot = gsl_stats_tss(y->data, y->stride, n);
/* compute R^2 */
w->stats.Rsq = 1.0 - ss_err / ss_tot;
/* compute adjusted R^2 */
w->stats.adj_Rsq = 1.0 - (1.0 - w->stats.Rsq) * (n - 1.0) / dof;
/* compute rmse */
w->stats.rmse = sqrt(ss_err / dof);
/* store SSE */
w->stats.sse = ss_err;
}
/* calculate covariance matrix = sigma^2 (X^T X)^{-1} */
s = robust_covariance(w->stats.sigma, cov, w);
if (s)
return s;
/* raise an error if not converged */
if (numit > w->maxiter)
{
GSL_ERROR("maximum iterations exceeded", GSL_EMAXITER);
}
return s;
}
} /* gsl_multifit_robust() */
static void update_doppler(int in_np, int out_np, float *sr2gr, meta_parameters *meta)
{
// Update the doppler
// In going from slant to ground, and potentially changing the width,
// we will likely change the shape of the doppler polynomial. So, solve
// for the coefficients of the new polynomial. Ignore the constant term
// v1 = d1*x1 + d2*x1*x1 <-- d1,d2 doppler coefs
// v2 = d1*x2 + d2*x2*x2 <-- x1,x2 are image center and right
// New poly:
// v1 = a*y1 + b*y1*y1 <-- a,b new doppler coefs
// v2 = a*y2 + b*y2*y2 <-- y1,y2 are image center and right in new geom
// Solving for b:
// b = (v1*y2 - v2*y1)/(y2*y1^2 - y1*y2^2)
// Then a:
// a = (v1 - b*y1*y1)/y1
double is2 = in_np-1; // Input sample 2 ( x2 in the above )
double os1 = out_np/2; // Output sample 1 ( y1 in the above )
double is1 = sr2gr[(int)os1]; // Input sample 1 ( x1 in the above )
double os2 = out_np-1; // Output sample 2 ( y2 in the above )
double d1 = meta->sar->range_doppler_coefficients[1];
double d2 = meta->sar->range_doppler_coefficients[2];
double v1 = d1*is1 + d2*is1*is1;
double v2 = d1*is2 + d2*is2*is2;
double b = (v1*os2 - v2*os1)/(os1*os1*os2 - os2*os2*os1);
double a = (v1 - b*os1*os1)/os1;
// Debug code: print out the doppler across the image
// a and b will be the starting points for a least squares fit
const int N=1000;
double chisq, xi[N], yi[N];
int ii;
for (ii=0; ii<N; ++ii) {
xi[ii] = (double)ii/(double)N * (double)(out_np-1);
// the sr2gr array maps a ground range index to a slant range index
double s = sr2gr[(int)xi[ii]];
yi[ii] = d1*s + d2*s*s;
}
gsl_matrix *X, *cov;
gsl_vector *y, *w, *c;
X = gsl_matrix_alloc(N,3);
y = gsl_vector_alloc(N);
w = gsl_vector_alloc(N);
c = gsl_vector_alloc(3);
cov = gsl_matrix_alloc(3, 3);
for (ii=0; ii<N; ++ii) {
gsl_matrix_set(X, ii, 0, 1.0);
gsl_matrix_set(X, ii, 1, xi[ii]);
gsl_matrix_set(X, ii, 2, xi[ii]*xi[ii]);
gsl_vector_set(y, ii, yi[ii]);
gsl_vector_set(w, ii, 1.0);
}
gsl_multifit_linear_workspace *work = gsl_multifit_linear_alloc(N, 3);
gsl_multifit_wlinear(X, w, y, c, cov, &chisq, work);
gsl_multifit_linear_free(work);
double c0 = gsl_vector_get(c, 0);
double c1 = gsl_vector_get(c, 1);
double c2 = gsl_vector_get(c, 2);
gsl_matrix_free(X);
gsl_vector_free(y);
gsl_vector_free(w);
gsl_vector_free(c);
gsl_matrix_free(cov);
// now the x and y vectors are the desired doppler polynomial
double ee1=0, ee2=0;
for (ii=0; ii<out_np; ii+=100) {
// ii: index in ground range, s: index in slant range
double s = sr2gr[ii];
// dop1: doppler in slant, dop2: doppler in ground (should agree)
double dop1 = d1*s + d2*s*s;
double dop2 = a*ii + b*ii*ii;
double dop3 = c0 + c1*ii + c2*ii*ii;
double e1 = fabs(dop1-dop2);
double e2 = fabs(dop1-dop3);
ee1 += e1;
ee2 += e2;
if (ii % 1000 == 0)
printf("%5d -> %8.3f %8.3f %8.3f %5d -> %5d %7.4f %7.4f\n",
ii, dop1, dop2, dop3, (int)s, ii, e1, e2);
}
printf("Original: %14.9f %14.9f %14.9f\n", 0., d1, d2);
printf("First : %14.9f %14.9f %14.9f\n", 0., a, b);
printf("Second : %14.9f %14.9f %14.9f\n\n", c0, c1, c2);
printf("Overall errors: %8.4f %8.4f\n", ee1, ee2);
meta->sar->range_doppler_coefficients[0] += c0;
meta->sar->range_doppler_coefficients[1] = c1;
meta->sar->range_doppler_coefficients[2] = c2;
}
void LowessSmoothing::smoothData(const DoubleVector & input_x, const DoubleVector & input_y, DoubleVector & smoothed_output)
{
if (input_x.size() != input_y.size())
{
throw Exception::InvalidValue(__FILE__, __LINE__, __PRETTY_FUNCTION__, "Sizes of x and y values not equal! Aborting... ", String(input_x.size()));
}
// unable to smooth over 2 or less data points (we need at least 3)
if (input_x.size() <= 2)
{
smoothed_output = input_y;
return;
}
Size input_size = input_y.size();
// alpha_ = 1 / ( input_size / window_size_ );
// const Size q = floor( input_size * alpha );
const Size q = (window_size_ < input_size) ? window_size_ : input_size - 1;
DoubleVector distances(input_size, 0.0);
DoubleVector sortedDistances(input_size, 0.0);
// DoubleVector smooth_yvals_Vec(input_size);
gsl_matrix * X = gsl_matrix_alloc(input_size, 3);
gsl_matrix * cov = gsl_matrix_alloc(3, 3);
gsl_vector * weights = gsl_vector_alloc(input_size);
gsl_vector * c = gsl_vector_alloc(3);
gsl_vector * x = gsl_vector_alloc(3);
gsl_vector * yvals_ = gsl_vector_alloc(input_size);
DoubleReal y, yErr, chisq;
// Setup the model matrix X for a quadratic fit.
// yvals_ = new double[input_size];
// Size idx = 0;
for (Size p_idx = 0; p_idx < input_y.size(); ++p_idx)
{
DoubleReal rt = input_x[p_idx];
gsl_matrix_set(X, p_idx, 0, 1.0);
gsl_matrix_set(X, p_idx, 1, rt);
gsl_matrix_set(X, p_idx, 2, rt * rt);
gsl_vector_set(yvals_, p_idx, input_y[p_idx]);
// ++idx;
}
//for(DoubleVector::const_iterator outer_peak_it = input_y.begin(); outer_peak_it != input_y.end(); ++outer_peak_it )
for (Size outer_idx = 0; outer_idx < input_y.size(); ++outer_idx)
{
// Compute distances.
// Size inner_idx = 0;
for (Size inner_idx = 0; inner_idx < input_y.size(); ++inner_idx)
{
distances[inner_idx] = std::fabs(input_x[outer_idx] - input_x[inner_idx]);
sortedDistances[inner_idx] = distances[inner_idx];
// ++inner_idx;
}
// Sort distances in order from smallest to largest.
// std::sort(sortedDistances, sortedDistances + input_size);
std::sort(sortedDistances.begin(), sortedDistances.end());
// Compute weights.
for (Size inner_idx = 0; inner_idx < input_size; ++inner_idx)
{
gsl_vector_set(weights, inner_idx, tricube_(distances[inner_idx], sortedDistances[q]));
}
gsl_multifit_linear_workspace * work = gsl_multifit_linear_alloc(input_size, 3);
gsl_multifit_wlinear(X, weights, yvals_, c, cov, &chisq, work);
gsl_multifit_linear_free(work);
DoubleReal rt = input_x[outer_idx];
gsl_vector_set(x, 0, 1.0);
gsl_vector_set(x, 1, rt);
gsl_vector_set(x, 2, rt * rt);
gsl_multifit_linear_est(x, c, cov, &y, &yErr);
smoothed_output.push_back(y);
}
gsl_matrix_free(X);
gsl_vector_free(weights);
gsl_vector_free(c);
gsl_matrix_free(cov);
return;
}
//spline locations held fixed in Mpc^-1; CMB basically fixed P(k) in these units
void dopksmoothbspline_(double *kvals, double *lnpklinear, double *lnpksmooth, int *npts) {
double kmaxsuppress = 0.01*0.7;
size_t n, ncoeffs, nbreak;
gsl_bspline_workspace *bw;
gsl_vector *B;
gsl_vector *c, *w, *x, *y;
gsl_matrix *X, *cov;
gsl_multifit_linear_workspace *mw;
double deltak,lastk;
int i,j,countkeep;
nbreak = 9;
gsl_vector *mybreaks = gsl_vector_alloc(nbreak);
gsl_vector_set(mybreaks,0,(0.001*0.7));
gsl_vector_set(mybreaks,1,(0.025*0.7));
gsl_vector_set(mybreaks,2,(0.075*0.7));
gsl_vector_set(mybreaks,3,(0.125*0.7));
gsl_vector_set(mybreaks,4,(0.175*0.7));
gsl_vector_set(mybreaks,5,(0.225*0.7));
gsl_vector_set(mybreaks,6,(0.275*0.7));
gsl_vector_set(mybreaks,7,(0.325*0.7));
gsl_vector_set(mybreaks,8,(0.375*0.7));
countkeep = 0;
for(i=0;i<(*npts);i++) {
if((kvals[i]) >= gsl_vector_get(mybreaks,0) && (kvals[i]) <= gsl_vector_get(mybreaks,nbreak-1)) {
countkeep += 1;
}
}
n = countkeep;
ncoeffs = nbreak + 2;
/* allocate a cubic bspline workspace (k = 4) */
bw = gsl_bspline_alloc(4, nbreak);
B = gsl_vector_alloc(ncoeffs);
x = gsl_vector_alloc(n);
y = gsl_vector_alloc(n);
X = gsl_matrix_alloc(n, ncoeffs);
c = gsl_vector_alloc(ncoeffs);
w = gsl_vector_alloc(n);
cov = gsl_matrix_alloc(ncoeffs, ncoeffs);
mw = gsl_multifit_linear_alloc(n, ncoeffs);
i=0;
for(j=0;j<(*npts);j++) {
if((kvals[j]) >= gsl_vector_get(mybreaks,0) && (kvals[j]) <= gsl_vector_get(mybreaks,nbreak-1)) {
gsl_vector_set(x,i,(kvals[j]));
gsl_vector_set(y,i,exp(lnpklinear[j])*pow(kvals[j],1.5));
if(j>0) {
deltak = kvals[j] - kvals[j-1];
}
else {
deltak = kvals[0];
if(kvals[1] - kvals[0] < deltak) {
deltak = kvals[1]-kvals[0];
}
}
gsl_vector_set(w,i,deltak);
i+=1;
}
}
gsl_bspline_knots(mybreaks,bw);
for(i=0;i<n;i++) {
double xi = gsl_vector_get(x,i);
gsl_bspline_eval(xi,B,bw);
for(j=0;j<ncoeffs;j++) {
double Bj = gsl_vector_get(B,j);
gsl_matrix_set(X,i,j,Bj);
}
}
//do fit
double yi,yierr,chisq;
gsl_multifit_wlinear(X,w,y,c,cov,&chisq,mw);
i = 0;
for(j=0;j<(*npts);j++) {
if((kvals[j]) >= gsl_vector_get(mybreaks,0) && (kvals[j]) <= gsl_vector_get(mybreaks,nbreak-1)) {
gsl_bspline_eval(gsl_vector_get(x,i),B,bw);
gsl_multifit_linear_est(B,c,cov,&yi,&yierr);
lnpksmooth[j] = log(yi*pow(kvals[j],-1.5));
i += 1;
}
else {
lnpksmooth[j] = lnpklinear[j];
}
//spline is wacky at small k -- suppress difference at k < 0.01
if(kvals[j] < kmaxsuppress) {
lnpksmooth[j] = lnpklinear[j];
}
}
assert(i==n);
gsl_bspline_free(bw);
gsl_vector_free(B);
gsl_vector_free(x);
gsl_vector_free(y);
gsl_vector_free(mybreaks);
gsl_matrix_free(X);
gsl_vector_free(c);
gsl_vector_free(w);
gsl_matrix_free(cov);
gsl_multifit_linear_free(mw);
}
int
main (void)
{
const size_t n = N;
const size_t ncoeffs = NCOEFFS;
const size_t nbreak = NBREAK;
size_t i, j;
gsl_bspline_workspace *bw;
gsl_vector *B;
double dy;
gsl_rng *r;
gsl_vector *c, *w;
gsl_vector *x, *y;
gsl_matrix *X, *cov;
gsl_multifit_linear_workspace *mw;
double chisq;
double Rsq;
double dof;
gsl_rng_env_setup();
r = gsl_rng_alloc(gsl_rng_default);
/* allocate a cubic bspline workspace (k = 4) */
bw = gsl_bspline_alloc(4, nbreak);
B = gsl_vector_alloc(ncoeffs);
x = gsl_vector_alloc(n);
y = gsl_vector_alloc(n);
X = gsl_matrix_alloc(n, ncoeffs);
c = gsl_vector_alloc(ncoeffs);
w = gsl_vector_alloc(n);
cov = gsl_matrix_alloc(ncoeffs, ncoeffs);
mw = gsl_multifit_linear_alloc(n, ncoeffs);
printf("#m=0,S=0\n");
/* this is the data to be fitted */
for (i = 0; i < n; ++i)
{
double sigma;
double xi = (15.0 / (N - 1)) * i;
double yi = cos(xi) * exp(-0.1 * xi);
sigma = 0.1 * yi;
dy = gsl_ran_gaussian(r, sigma);
yi += dy;
gsl_vector_set(x, i, xi);
gsl_vector_set(y, i, yi);
gsl_vector_set(w, i, 1.0 / (sigma * sigma));
printf("%f %f\n", xi, yi);
}
/* use uniform breakpoints on [0, 15] */
gsl_bspline_knots_uniform(0.0, 15.0, bw);
/* construct the fit matrix X */
for (i = 0; i < n; ++i)
{
double xi = gsl_vector_get(x, i);
/* compute B_j(xi) for all j */
gsl_bspline_eval(xi, B, bw);
/* fill in row i of X */
for (j = 0; j < ncoeffs; ++j)
{
double Bj = gsl_vector_get(B, j);
gsl_matrix_set(X, i, j, Bj);
}
}
/* do the fit */
gsl_multifit_wlinear(X, w, y, c, cov, &chisq, mw);
dof = n - ncoeffs;
Rsq = 1.0 - chisq / gsl_stats_wtss(w->data, 1, y->data, 1, y->size);
fprintf(stderr, "chisq/dof = %e, Rsq = %f\n", chisq / dof, Rsq);
/* output the smoothed curve */
{
double xi, yi, yerr;
printf("#m=1,S=0\n");
for (xi = 0.0; xi < 15.0; xi += 0.1)
{
gsl_bspline_eval(xi, B, bw);
gsl_multifit_linear_est(B, c, cov, &yi, &yerr);
printf("%f %f\n", xi, yi);
}
}
gsl_rng_free(r);
gsl_bspline_free(bw);
gsl_vector_free(B);
gsl_vector_free(x);
gsl_vector_free(y);
gsl_matrix_free(X);
gsl_vector_free(c);
gsl_vector_free(w);
gsl_matrix_free(cov);
gsl_multifit_linear_free(mw);
return 0;
} /* main() */
void
Delta_vv_t::hybridFit(const std::vector<double> & subData,
const std::vector<double> & subSensor)
{
Logger * log = Logger::get_instance();
log->debug(boost::format("Entering function %1%")
% __PRETTY_FUNCTION__);
log->increase_indent();
// These constants are used to flag "bad" data
const double stdDevData = computeVariance(subData);
const double meanSubData = computeMean(subData);
const double stdDevSensor = computeVariance(subSensor);
const double meanSubSensor = computeMean(subSensor);
log->debug(boost::format("stdDevData = %1%, meanSubData = %2%, "
"stdDevSensor = %3%, meanSubSensor = %4%")
% stdDevData
% meanSubData
% stdDevSensor
% meanSubSensor);
// Mask missing pid
log->debug("Going to mask those pIDs for which gains are too «strange»");
log->increase_indent();
std::vector<double> locData;
std::vector<double> locGains;
std::vector<double> locDipole;
std::vector<double> locSensor;
for (unsigned int idx=0; idx<pid.size(); ++idx) {
if ((gain[idx]==0)||(std::isinf(gain[idx]))
||(std::isnan(subSensor[idx]))||(subSensor[idx]==0)
||(subSensor[idx]>(meanSubSensor+5*stdDevSensor))||(subSensor[idx]<(meanSubSensor-5*stdDevSensor))
||(std::isnan(subData[idx]))||(subData[idx]==0)
||(subData[idx]>(meanSubData+5*stdDevData))||(subData[idx]<(meanSubData-5*stdDevData)))
{
log->debug(boost::format("Skipping pid %1%")
% pid[idx]);
continue;
}
locData.push_back(subData[idx]);
locGains.push_back(gain[idx]);
locDipole.push_back(dipole[idx]);
locSensor.push_back(subSensor[idx]);
}
log->decrease_indent();
log->debug(boost::format("Done, %1% elements kept for the fit")
% locData.size());
double meanSensor = computeMean(locSensor);
// Multi-parameters fitting
// Prepare data
gsl_matrix *data, *cov;
gsl_vector *loadVolt, *weights, *coeff;
data = gsl_matrix_alloc (locData.size(), 2);
loadVolt = gsl_vector_alloc (locData.size());
weights = gsl_vector_alloc (locData.size());
coeff = gsl_vector_alloc (2);
cov = gsl_matrix_alloc (2, 2);
for(size_t idx = 0; idx < locData.size(); idx++) {
gsl_matrix_set (data, idx, 0, 1.0);
gsl_matrix_set (data, idx, 1, locSensor[idx] - meanSensor);
gsl_vector_set (loadVolt, idx, locData[idx] * locGains[idx]);
gsl_vector_set (weights, idx, std::fabs(locDipole[idx]));
}
// Fit
double chisq;
gsl_multifit_linear_workspace * work =
gsl_multifit_linear_alloc (locData.size(), 2);
gsl_multifit_wlinear (data, weights, loadVolt, coeff, cov,
&chisq, work);
gsl_multifit_linear_free (work);
log->debug(boost::format("Interpolation coefficients: c_0 = %1%, "
"c_1 = %2%, \u03c7\u00b2 = %3%")
% gsl_vector_get(coeff, 0)
% gsl_vector_get(coeff, 1)
% chisq);
// Save results
std::vector<double> returnGains;
std::vector<int> returnPids;
for (unsigned int idx=0; idx<pid.size(); ++idx) {
if ((gain[idx]==0)||(std::isinf(gain[idx]))
||(std::isnan(subSensor[idx]))||(subSensor[idx]==0)
||(subSensor[idx]>(meanSubSensor+5*stdDevSensor))||(subSensor[idx]<(meanSubSensor-5*stdDevSensor))
||(std::isnan(subData[idx]))||(subData[idx]==0)
||(subData[idx]>(meanSubData+5*stdDevData))||(subData[idx]<(meanSubData-5*stdDevData)))
continue;
returnGains.push_back((gsl_vector_get(coeff,0)+gsl_vector_get(coeff,1)*(subSensor[idx]-meanSensor))/subData[idx]);
returnPids.push_back(pid[idx]);
}
// Memory free
gsl_matrix_free (data);
gsl_vector_free (loadVolt);
gsl_vector_free (weights);
gsl_vector_free (coeff);
gsl_matrix_free (cov);
gain.swap(returnGains);
pid.swap(returnPids);
log->decrease_indent();
log->debug(boost::format("Quitting function %1%")
% __PRETTY_FUNCTION__);
}
void
test_longley ()
{
size_t i, j;
{
gsl_multifit_linear_workspace * work =
gsl_multifit_linear_alloc (longley_n, longley_p);
gsl_matrix_view X = gsl_matrix_view_array (longley_x, longley_n, longley_p);
gsl_vector_view y = gsl_vector_view_array (longley_y, longley_n);
gsl_vector * c = gsl_vector_alloc (longley_p);
gsl_matrix * cov = gsl_matrix_alloc (longley_p, longley_p);
gsl_vector_view diag;
double chisq;
double expected_c[7] = { -3482258.63459582,
15.0618722713733,
-0.358191792925910E-01,
-2.02022980381683,
-1.03322686717359,
-0.511041056535807E-01,
1829.15146461355 };
double expected_sd[7] = { 890420.383607373,
84.9149257747669,
0.334910077722432E-01,
0.488399681651699,
0.214274163161675,
0.226073200069370,
455.478499142212 } ;
double expected_chisq = 836424.055505915;
gsl_multifit_linear (&X.matrix, &y.vector, c, cov, &chisq, work);
gsl_test_rel (gsl_vector_get(c,0), expected_c[0], 1e-10, "longley gsl_fit_multilinear c0") ;
gsl_test_rel (gsl_vector_get(c,1), expected_c[1], 1e-10, "longley gsl_fit_multilinear c1") ;
gsl_test_rel (gsl_vector_get(c,2), expected_c[2], 1e-10, "longley gsl_fit_multilinear c2") ;
gsl_test_rel (gsl_vector_get(c,3), expected_c[3], 1e-10, "longley gsl_fit_multilinear c3") ;
gsl_test_rel (gsl_vector_get(c,4), expected_c[4], 1e-10, "longley gsl_fit_multilinear c4") ;
gsl_test_rel (gsl_vector_get(c,5), expected_c[5], 1e-10, "longley gsl_fit_multilinear c5") ;
gsl_test_rel (gsl_vector_get(c,6), expected_c[6], 1e-10, "longley gsl_fit_multilinear c6") ;
diag = gsl_matrix_diagonal (cov);
gsl_test_rel (gsl_vector_get(&diag.vector,0), pow(expected_sd[0],2.0), 1e-10, "longley gsl_fit_multilinear cov00") ;
gsl_test_rel (gsl_vector_get(&diag.vector,1), pow(expected_sd[1],2.0), 1e-10, "longley gsl_fit_multilinear cov11") ;
gsl_test_rel (gsl_vector_get(&diag.vector,2), pow(expected_sd[2],2.0), 1e-10, "longley gsl_fit_multilinear cov22") ;
gsl_test_rel (gsl_vector_get(&diag.vector,3), pow(expected_sd[3],2.0), 1e-10, "longley gsl_fit_multilinear cov33") ;
gsl_test_rel (gsl_vector_get(&diag.vector,4), pow(expected_sd[4],2.0), 1e-10, "longley gsl_fit_multilinear cov44") ;
gsl_test_rel (gsl_vector_get(&diag.vector,5), pow(expected_sd[5],2.0), 1e-10, "longley gsl_fit_multilinear cov55") ;
gsl_test_rel (gsl_vector_get(&diag.vector,6), pow(expected_sd[6],2.0), 1e-10, "longley gsl_fit_multilinear cov66") ;
gsl_test_rel (chisq, expected_chisq, 1e-10, "longley gsl_fit_multilinear chisq") ;
gsl_vector_free(c);
gsl_matrix_free(cov);
gsl_multifit_linear_free (work);
}
{
gsl_multifit_linear_workspace * work =
gsl_multifit_linear_alloc (longley_n, longley_p);
gsl_matrix_view X = gsl_matrix_view_array (longley_x, longley_n, longley_p);
gsl_vector_view y = gsl_vector_view_array (longley_y, longley_n);
gsl_vector * w = gsl_vector_alloc (longley_n);
gsl_vector * c = gsl_vector_alloc (longley_p);
gsl_matrix * cov = gsl_matrix_alloc (longley_p, longley_p);
double chisq;
double expected_c[7] = { -3482258.63459582,
15.0618722713733,
-0.358191792925910E-01,
-2.02022980381683,
-1.03322686717359,
-0.511041056535807E-01,
1829.15146461355 };
double expected_cov[7][7] = { { 8531122.56783558,
-166.727799925578, 0.261873708176346, 3.91188317230983,
1.1285582054705, -0.889550869422687, -4362.58709870581},
{-166.727799925578, 0.0775861253030891, -1.98725210399982e-05,
-0.000247667096727256, -6.82911920718824e-05, 0.000136160797527761,
0.0775255245956248},
{0.261873708176346, -1.98725210399982e-05, 1.20690316701888e-08,
1.66429546772984e-07, 3.61843600487847e-08, -6.78805814483582e-08,
-0.00013158719037715},
{3.91188317230983, -0.000247667096727256, 1.66429546772984e-07,
2.56665052544717e-06, 6.96541409215597e-07, -9.00858307771567e-07,
-0.00197260370663974},
{1.1285582054705, -6.82911920718824e-05, 3.61843600487847e-08,
6.96541409215597e-07, 4.94032602583969e-07, -9.8469143760973e-08,
-0.000576921112208274},
{-0.889550869422687, 0.000136160797527761, -6.78805814483582e-08,
-9.00858307771567e-07, -9.8469143760973e-08, 5.49938542664952e-07,
0.000430074434198215},
{-4362.58709870581, 0.0775255245956248, -0.00013158719037715,
-0.00197260370663974, -0.000576921112208274, 0.000430074434198215,
2.23229587481535 }} ;
double expected_chisq = 836424.055505915;
gsl_vector_set_all (w, 1.0);
gsl_multifit_wlinear (&X.matrix, w, &y.vector, c, cov, &chisq, work);
gsl_test_rel (gsl_vector_get(c,0), expected_c[0], 1e-10, "longley gsl_fit_wmultilinear c0") ;
gsl_test_rel (gsl_vector_get(c,1), expected_c[1], 1e-10, "longley gsl_fit_wmultilinear c1") ;
gsl_test_rel (gsl_vector_get(c,2), expected_c[2], 1e-10, "longley gsl_fit_wmultilinear c2") ;
gsl_test_rel (gsl_vector_get(c,3), expected_c[3], 1e-10, "longley gsl_fit_wmultilinear c3") ;
gsl_test_rel (gsl_vector_get(c,4), expected_c[4], 1e-10, "longley gsl_fit_wmultilinear c4") ;
gsl_test_rel (gsl_vector_get(c,5), expected_c[5], 1e-10, "longley gsl_fit_wmultilinear c5") ;
gsl_test_rel (gsl_vector_get(c,6), expected_c[6], 1e-10, "longley gsl_fit_wmultilinear c6") ;
for (i = 0; i < longley_p; i++)
{
for (j = 0; j < longley_p; j++)
{
gsl_test_rel (gsl_matrix_get(cov,i,j), expected_cov[i][j], 1e-7,
"longley gsl_fit_wmultilinear cov(%d,%d)", i, j) ;
}
}
gsl_test_rel (chisq, expected_chisq, 1e-10, "longley gsl_fit_wmultilinear chisq") ;
gsl_vector_free(w);
gsl_vector_free(c);
gsl_matrix_free(cov);
gsl_multifit_linear_free (work);
}
}
/** Executes the algorithm
*
*/
void SplineBackground::exec()
{
API::MatrixWorkspace_sptr inWS = getProperty("InputWorkspace");
int spec = getProperty("WorkspaceIndex");
if (spec > static_cast<int>(inWS->getNumberHistograms()))
throw std::out_of_range("WorkspaceIndex is out of range.");
const MantidVec& X = inWS->readX(spec);
const MantidVec& Y = inWS->readY(spec);
const MantidVec& E = inWS->readE(spec);
const bool isHistogram = inWS->isHistogramData();
const int ncoeffs = getProperty("NCoeff");
const int k = 4; // order of the spline + 1 (cubic)
const int nbreak = ncoeffs - (k - 2);
if (nbreak <= 0)
throw std::out_of_range("Too low NCoeff");
gsl_bspline_workspace *bw;
gsl_vector *B;
gsl_vector *c, *w, *x, *y;
gsl_matrix *Z, *cov;
gsl_multifit_linear_workspace *mw;
double chisq;
int n = static_cast<int>(Y.size());
bool isMasked = inWS->hasMaskedBins(spec);
std::vector<int> masked(Y.size());
if (isMasked)
{
for(API::MatrixWorkspace::MaskList::const_iterator it=inWS->maskedBins(spec).begin();it!=inWS->maskedBins(spec).end();++it)
masked[it->first] = 1;
n -= static_cast<int>(inWS->maskedBins(spec).size());
}
if (n < ncoeffs)
{
g_log.error("Too many basis functions (NCoeff)");
throw std::out_of_range("Too many basis functions (NCoeff)");
}
/* allocate a cubic bspline workspace (k = 4) */
bw = gsl_bspline_alloc(k, nbreak);
B = gsl_vector_alloc(ncoeffs);
x = gsl_vector_alloc(n);
y = gsl_vector_alloc(n);
Z = gsl_matrix_alloc(n, ncoeffs);
c = gsl_vector_alloc(ncoeffs);
w = gsl_vector_alloc(n);
cov = gsl_matrix_alloc(ncoeffs, ncoeffs);
mw = gsl_multifit_linear_alloc(n, ncoeffs);
/* this is the data to be fitted */
int j = 0;
for (MantidVec::size_type i = 0; i < Y.size(); ++i)
{
if (isMasked && masked[i]) continue;
gsl_vector_set(x, j, (isHistogram ? (0.5*(X[i]+X[i+1])) : X[i])); // Middle of the bins, if a histogram
gsl_vector_set(y, j, Y[i]);
gsl_vector_set(w, j, E[i]>0.?1./(E[i]*E[i]):0.);
++j;
}
if (n != j)
{
gsl_bspline_free(bw);
gsl_vector_free(B);
gsl_vector_free(x);
gsl_vector_free(y);
gsl_matrix_free(Z);
gsl_vector_free(c);
gsl_vector_free(w);
gsl_matrix_free(cov);
gsl_multifit_linear_free(mw);
throw std::runtime_error("Assertion failed: n != j");
}
double xStart = X.front();
double xEnd = X.back();
/* use uniform breakpoints */
gsl_bspline_knots_uniform(xStart, xEnd, bw);
/* construct the fit matrix X */
for (int i = 0; i < n; ++i)
{
double xi=gsl_vector_get(x, i);
/* compute B_j(xi) for all j */
gsl_bspline_eval(xi, B, bw);
/* fill in row i of X */
for (j = 0; j < ncoeffs; ++j)
{
double Bj = gsl_vector_get(B, j);
gsl_matrix_set(Z, i, j, Bj);
}
}
/* do the fit */
gsl_multifit_wlinear(Z, w, y, c, cov, &chisq, mw);
/* output the smoothed curve */
API::MatrixWorkspace_sptr outWS = WorkspaceFactory::Instance().create(inWS,1,X.size(),Y.size());
{
outWS->getAxis(1)->setValue(0, inWS->getAxis(1)->spectraNo(spec));
double xi, yi, yerr;
for (MantidVec::size_type i=0;i<Y.size();i++)
{
xi = X[i];
gsl_bspline_eval(xi, B, bw);
gsl_multifit_linear_est(B, c, cov, &yi, &yerr);
outWS->dataY(0)[i] = yi;
outWS->dataE(0)[i] = yerr;
}
outWS->dataX(0) = X;
}
gsl_bspline_free(bw);
gsl_vector_free(B);
gsl_vector_free(x);
gsl_vector_free(y);
gsl_matrix_free(Z);
gsl_vector_free(c);
gsl_vector_free(w);
gsl_matrix_free(cov);
gsl_multifit_linear_free(mw);
setProperty("OutputWorkspace",outWS);
}
// [[Rcpp::export]]
Rcpp::List fitData(Rcpp::DataFrame ds) {
const size_t ncoeffs = NCOEFFS;
const size_t nbreak = NBREAK;
const size_t n = N;
size_t i, j;
Rcpp::DataFrame D(ds); // construct the data.frame object
RcppGSL::vector<double> y = D["y"]; // access columns by name,
RcppGSL::vector<double> x = D["x"]; // assigning to GSL vectors
RcppGSL::vector<double> w = D["w"];
gsl_bspline_workspace *bw;
gsl_vector *B;
gsl_vector *c;
gsl_matrix *X, *cov;
gsl_multifit_linear_workspace *mw;
double chisq, Rsq, dof, tss;
bw = gsl_bspline_alloc(4, nbreak); // allocate a cubic bspline workspace (k = 4)
B = gsl_vector_alloc(ncoeffs);
X = gsl_matrix_alloc(n, ncoeffs);
c = gsl_vector_alloc(ncoeffs);
cov = gsl_matrix_alloc(ncoeffs, ncoeffs);
mw = gsl_multifit_linear_alloc(n, ncoeffs);
gsl_bspline_knots_uniform(0.0, 15.0, bw); // use uniform breakpoints on [0, 15]
for (i = 0; i < n; ++i) { // construct the fit matrix X
double xi = gsl_vector_get(x, i);
gsl_bspline_eval(xi, B, bw); // compute B_j(xi) for all j
for (j = 0; j < ncoeffs; ++j) { // fill in row i of X
double Bj = gsl_vector_get(B, j);
gsl_matrix_set(X, i, j, Bj);
}
}
gsl_multifit_wlinear(X, w, y, c, cov, &chisq, mw); // do the fit
dof = n - ncoeffs;
tss = gsl_stats_wtss(w->data, 1, y->data, 1, y->size);
Rsq = 1.0 - chisq / tss;
Rcpp::NumericVector FX(151), FY(151); // output the smoothed curve
double xi, yi, yerr;
for (xi = 0.0, i=0; xi < 15.0; xi += 0.1, i++) {
gsl_bspline_eval(xi, B, bw);
gsl_multifit_linear_est(B, c, cov, &yi, &yerr);
FX[i] = xi;
FY[i] = yi;
}
Rcpp::List res =
Rcpp::List::create(Rcpp::Named("X")=FX,
Rcpp::Named("Y")=FY,
Rcpp::Named("chisqdof")=Rcpp::wrap(chisq/dof),
Rcpp::Named("rsq")=Rcpp::wrap(Rsq));
gsl_bspline_free(bw);
gsl_vector_free(B);
gsl_matrix_free(X);
gsl_vector_free(c);
gsl_matrix_free(cov);
gsl_multifit_linear_free(mw);
y.free();
x.free();
w.free();
return(res);
}
bool Reference::recalculateStationHeadingCurvature()
{
if (this->numOfPoints <= 1) { return false; }
// Recalculate s
for(int i = 0; i < this->numOfPoints ; i++) {
if(i == 0) { this->s[0] = 0; }
else {
double ds = hypot(this->x[i] - this->x[i-1], this->y[i] - this->y[i-1]);
this->s[i] = this->s[i-1] + ds;
}
}
// Recalculate theta, k and dk
if(this->numOfPoints < NUM_FIT_FRONT + NUM_FIT_BACK + 1) {
int i = 0;
for( i = 0; i < this->numOfPoints; i++) {
if(i == this->numOfPoints - 1) {
this->theta[i] = this->theta[i-1];
this->k[i] = 0;
break;
}
this->theta[i] = atan2(this->y[i+1] - this->y[i], this->x[i+1] - this->x[i]);
this->k[i] = 0;
}
}
else {
for(int i = 0; i < this->numOfPoints; i++) {
int id0 = (i-NUM_FIT_FRONT >= 0) ? (i-NUM_FIT_FRONT):(0);
int id1 = (i+NUM_FIT_BACK < this->numOfPoints) ? (i+NUM_FIT_BACK):(this->numOfPoints-1);
double si, xi, yi, /*ei,*/ chisq;
gsl_matrix *S, *cov;
gsl_vector *x, *y, *w, *cx, *cy;
int n = id1-id0+1;
S = gsl_matrix_alloc (n, 3);
x = gsl_vector_alloc (n);
y = gsl_vector_alloc (n);
w = gsl_vector_alloc (n);
cx = gsl_vector_alloc (3);
cy = gsl_vector_alloc (3);
cov = gsl_matrix_alloc (3, 3);
for (int j = 0; j < n; j++) {
si = this->s[id0+j];
xi = this->x[id0+j];
yi = this->y[id0+j];
gsl_matrix_set (S, j, 0, 1.0);
gsl_matrix_set (S, j, 1, si);
gsl_matrix_set (S, j, 2, si*si);
gsl_vector_set (x, j, xi);
gsl_vector_set (y, j, yi);
gsl_vector_set (w, j, 1.0);
}
gsl_multifit_linear_workspace * work = gsl_multifit_linear_alloc (n, 3);
gsl_multifit_wlinear (S, w, x, cx, cov, &chisq, work);
gsl_multifit_linear_free (work);
work = gsl_multifit_linear_alloc (n, 3);
gsl_multifit_wlinear (S, w, y, cy, cov, &chisq, work);
gsl_multifit_linear_free (work);
#define Cx(i) (gsl_vector_get(cx,(i)))
#define Cy(i) (gsl_vector_get(cy,(i)))
double s_ = this->s[i];
// double x_ = Cx(2) * s_ * s_ + Cx(1) * s_ + Cx(0);
double xd_ = 2 * Cx(2) * s_ + Cx(1);
double xdd_ = 2 * Cx(2);
// double y_ = Cy(2) * s_ * s_ + Cy(1) * s_ + Cy(0);
double yd_ = 2 * Cy(2) * s_ + Cy(1);
double ydd_ = 2 * Cy(2);
this->theta[i] = atan2(yd_, xd_);
this->k[i] = (xd_ * ydd_ - yd_ * xdd_)
/ ( sqrt( (xd_*xd_ + yd_*yd_)*(xd_*xd_ + yd_*yd_)*(xd_*xd_ + yd_*yd_) ) );
gsl_matrix_free(S);
gsl_vector_free (x);
gsl_vector_free (y);
gsl_vector_free (w);
gsl_vector_free (cx);
gsl_vector_free (cy);
gsl_matrix_free (cov);
}
}
return true;
}
void bSplineGSLOriginalDemo ()
{
const size_t n = N;
const size_t ncoeffs = NCOEFFS;
const size_t nbreak = NBREAK;
size_t i, j;
gsl_bspline_workspace *bw;
gsl_vector *B;
double dy;
gsl_rng *r;
gsl_vector *c, *w;
gsl_vector *x, *y;
gsl_matrix *X, *cov;
gsl_multifit_linear_workspace *mw;
double chisq, Rsq, dof, tss;
vector<double> xControl, yControl, xFit, yFit;
gsl_rng_env_setup();
r = gsl_rng_alloc(gsl_rng_default);
/* allocate a cubic bspline workspace (k = 4) */
bw = gsl_bspline_alloc(4, nbreak);
B = gsl_vector_alloc(ncoeffs);
x = gsl_vector_alloc(n);
y = gsl_vector_alloc(n);
X = gsl_matrix_alloc(n, ncoeffs);
c = gsl_vector_alloc(ncoeffs);
w = gsl_vector_alloc(n);
cov = gsl_matrix_alloc(ncoeffs, ncoeffs);
mw = gsl_multifit_linear_alloc(n, ncoeffs);
printf("#m=0,S=0\n");
/* this is the data to be fitted */
for (i = 0; i < n; ++i)
{
double sigma;
double xi = (15.0 / (N - 1)) * i;
double yi = cos(xi) * exp(-0.1 * xi);
sigma = 0.1 * yi;
dy = gsl_ran_gaussian(r, sigma);
yi += dy;
gsl_vector_set(x, i, xi);
xControl.push_back(xi);
gsl_vector_set(y, i, yi);
yControl.push_back(yi);
gsl_vector_set(w, i, 1.0 / (sigma * sigma));
printf("%f %f\n", xi, yi);
}
/* use uniform breakpoints on [0, 15] */
gsl_bspline_knots_uniform(0.0, 15.0, bw);
/* construct the fit matrix X */
for (i = 0; i < n; ++i)
{
double xi = gsl_vector_get(x, i);
/* compute B_j(xi) for all j */
gsl_bspline_eval(xi, B, bw);
/* fill in row i of X */
for (j = 0; j < ncoeffs; ++j)
{
double Bj = gsl_vector_get(B, j);
gsl_matrix_set(X, i, j, Bj);
}
}
/* do the fit */
gsl_multifit_wlinear(X, w, y, c, cov, &chisq, mw);
dof = n - ncoeffs;
tss = gsl_stats_wtss(w->data, 1, y->data, 1, y->size);
Rsq = 1.0 - chisq / tss;
fprintf(stderr, "chisq/dof = %e, Rsq = %f\n",
chisq / dof, Rsq);
/* output the smoothed curve */
{
double xi, yi, yerr;
printf("#m=1,S=0\n");
for (xi = 0.0; xi < 15.0; xi += 0.1)
{
gsl_bspline_eval(xi, B, bw);
gsl_multifit_linear_est(B, c, cov, &yi, &yerr);
xFit.push_back(xi);
yFit.push_back(yi);
printf("%f %f\n", xi, yi);
}
}
gsl_rng_free(r);
gsl_bspline_free(bw);
gsl_vector_free(B);
gsl_vector_free(x);
gsl_vector_free(y);
gsl_matrix_free(X);
gsl_vector_free(c);
gsl_vector_free(w);
gsl_matrix_free(cov);
gsl_multifit_linear_free(mw);
TGraph *gr = LoadGraphFromVectors(xControl, yControl);
TGraph *grFit = LoadGraphFromVectors(xFit, yFit);
gr->SetMarkerColor(kRed);
TCanvas *c1 = new TCanvas("c1", "Graph", 200, 10, 700, 500);
gr->Draw("apz");
grFit->Draw("SAME");
c1->Update();
}
QMap<double, double> SGMResolutionOptimization::curve(QList<QVariant> stateParameters, AMRegionsList* contextParameters){
double _maxenergy = maximumEnergy(contextParameters);
double _minenergy = minimumEnergy(contextParameters);
double _slit = stateParameters.at(0).toDouble();
double _y1, _y2, _y3, _x1, _x2, _x3;
double _maxRes = 0;
SGMBeamlineInfo::sgmGrating _grating = (SGMBeamlineInfo::sgmGrating)stateParameters.at(1).toInt();
SGMBeamlineInfo::sgmHarmonic _harmonic = (SGMBeamlineInfo::sgmHarmonic)stateParameters.at(2).toInt();
if(!SGMBeamlineInfo::sgmInfo()->energyRangeValidForSettings(_grating, _harmonic, _minenergy, _maxenergy))
{
}
else if((_harmonic == 3) && (_grating == 2)){
_y1 = (0.95)*(0.5229*log(_slit)+1.4221);
_y2 = (0.95)*(0.4391*log(_slit)+1.2617);
_y3 = (0.95)*(0.421*log(_slit)+0.9037);
_x1 = 2000;
_x2 = 1200;
_x3 = 800;
_maxRes = 12500;
}
else{
if( (_grating == 0) && SGMBeamlineInfo::sgmInfo()->energyRangeValidForSettings(_grating, _harmonic, _minenergy, _maxenergy) ){
_y1 = 0.4568*log(_slit)+1.0199;
_y2 = 0.4739*log(_slit)+0.605;
_y3 = 0.4257*log(_slit)+0.4438;
_x1 = 600;
_x2 = 400;
_x3 = 250;
_maxRes = 17500;
}
else if( (_grating == 1) && SGMBeamlineInfo::sgmInfo()->energyRangeValidForSettings(_grating, _harmonic, _minenergy, _maxenergy) ){
_y1 = 0.425*log(_slit)+1.4936;
_y2 = 0.4484*log(_slit)+1.0287;
_y3 = 0.4029*log(_slit)+0.7914;
_x1 = 1200;
_x2 = 800;
_x3 = 500;
_maxRes = 15000;
}
else if( (_grating == 2) && SGMBeamlineInfo::sgmInfo()->energyRangeValidForSettings(_grating, _harmonic, _minenergy, _maxenergy) ){
_y1 = 0.5229*log(_slit)+1.4221;
_y2 = 0.4391*log(_slit)+1.2617;
_y3 = 0.421*log(_slit)+0.9037;
_x1 = 2000;
_x2 = 1200;
_x3 = 800;
_maxRes = 13750;
}
}
int i, n;
double xi, yi, ei, chisq;
gsl_matrix *X, *cov;
gsl_vector *y, *w, *c;
n = 3;
X = gsl_matrix_alloc (n, 3);
y = gsl_vector_alloc (n);
w = gsl_vector_alloc (n);
c = gsl_vector_alloc (3);
cov = gsl_matrix_alloc (3, 3);
double ix[3];
double iy[3];
double ie[3];
ix[0] = _x1;
ix[1] = _x2;
ix[2] = _x3;
iy[0] = _y1;
iy[1] = _y2;
iy[2] = _y3;
ie[0] = 0.1*iy[0];
ie[1] = 0.1*iy[1];
ie[2] = 0.1*iy[2];
for (i = 0; i < n; i++)
{
xi = ix[i];
yi = iy[i];
ei = ie[i];
gsl_matrix_set (X, i, 0, 1.0);
gsl_matrix_set (X, i, 1, xi);
gsl_matrix_set (X, i, 2, xi*xi);
gsl_vector_set (y, i, yi);
gsl_vector_set (w, i, 1.0/(ei*ei));
}
gsl_multifit_linear_workspace * work
= gsl_multifit_linear_alloc (n, 3);
gsl_multifit_wlinear (X, w, y, c, cov,
&chisq, work);
gsl_multifit_linear_free (work);
#define C(i) (gsl_vector_get(c,(i)))
#define COV(i,j) (gsl_matrix_get(cov,(i),(j)))
QMap<double, double> rCurve;
double tmpStart, tmpEnd, tmpDelta;//, tmpVal;
for( int x = 0; x < contextParameters->count(); x++){
tmpStart = contextParameters->start(x);
tmpDelta = contextParameters->delta(x);
tmpEnd = contextParameters->end(x);
if( tmpStart >= 250 && tmpStart <= 2000 && tmpEnd >= 250 && tmpEnd <= 2000 ){
for( double y = tmpStart; ((tmpDelta > 0) ? (y <= tmpEnd) : (y >= tmpEnd)); y += tmpDelta ){
//rCurve[y] = !SGMBeamline::sgm()->energyValidForSettings(_grating, _harmonic, y) ? 0.0 :y/(pow(10, C(2)*y*y + C(1)*y + C(0))*1e-3);
if(!SGMBeamlineInfo::sgmInfo()->energyValidForSettings(_grating, _harmonic, y))
rCurve[y] = 0.0;
else{
rCurve[y] = y/(pow(10, C(2)*y*y + C(1)*y + C(0))*1e-3);
}
}
}
else
rCurve[250] = 0;
}
gsl_matrix_free (X);
gsl_vector_free (y);
gsl_vector_free (w);
gsl_vector_free (c);
gsl_matrix_free (cov);
return rCurve;
}
void
test_filip ()
{
size_t i, j;
{
gsl_multifit_linear_workspace * work =
gsl_multifit_linear_alloc (filip_n, filip_p);
gsl_matrix * X = gsl_matrix_alloc (filip_n, filip_p);
gsl_vector_view y = gsl_vector_view_array (filip_y, filip_n);
gsl_vector * c = gsl_vector_alloc (filip_p);
gsl_matrix * cov = gsl_matrix_alloc (filip_p, filip_p);
gsl_vector_view diag;
double chisq;
double expected_c[11] = { -1467.48961422980,
-2772.17959193342,
-2316.37108160893,
-1127.97394098372,
-354.478233703349,
-75.1242017393757,
-10.8753180355343,
-1.06221498588947,
-0.670191154593408E-01,
-0.246781078275479E-02,
-0.402962525080404E-04 };
double expected_sd[11] = { 298.084530995537,
559.779865474950,
466.477572127796,
227.204274477751,
71.6478660875927,
15.2897178747400,
2.23691159816033,
0.221624321934227,
0.142363763154724E-01,
0.535617408889821E-03,
0.896632837373868E-05 };
double expected_chisq = 0.795851382172941E-03;
for (i = 0 ; i < filip_n; i++)
{
for (j = 0; j < filip_p; j++)
{
gsl_matrix_set(X, i, j, pow(filip_x[i], j));
}
}
gsl_multifit_linear (X, &y.vector, c, cov, &chisq, work);
gsl_test_rel (gsl_vector_get(c,0), expected_c[0], 1e-7, "filip gsl_fit_multilinear c0") ;
gsl_test_rel (gsl_vector_get(c,1), expected_c[1], 1e-7, "filip gsl_fit_multilinear c1") ;
gsl_test_rel (gsl_vector_get(c,2), expected_c[2], 1e-7, "filip gsl_fit_multilinear c2") ;
gsl_test_rel (gsl_vector_get(c,3), expected_c[3], 1e-7, "filip gsl_fit_multilinear c3") ;
gsl_test_rel (gsl_vector_get(c,4), expected_c[4], 1e-7, "filip gsl_fit_multilinear c4") ;
gsl_test_rel (gsl_vector_get(c,5), expected_c[5], 1e-7, "filip gsl_fit_multilinear c5") ;
gsl_test_rel (gsl_vector_get(c,6), expected_c[6], 1e-7, "filip gsl_fit_multilinear c6") ;
gsl_test_rel (gsl_vector_get(c,7), expected_c[7], 1e-7, "filip gsl_fit_multilinear c7") ;
gsl_test_rel (gsl_vector_get(c,8), expected_c[8], 1e-7, "filip gsl_fit_multilinear c8") ;
gsl_test_rel (gsl_vector_get(c,9), expected_c[9], 1e-7, "filip gsl_fit_multilinear c9") ;
gsl_test_rel (gsl_vector_get(c,10), expected_c[10], 1e-7, "filip gsl_fit_multilinear c10") ;
diag = gsl_matrix_diagonal (cov);
gsl_test_rel (gsl_vector_get(&diag.vector,0), pow(expected_sd[0],2.0), 1e-6, "filip gsl_fit_multilinear cov00") ;
gsl_test_rel (gsl_vector_get(&diag.vector,1), pow(expected_sd[1],2.0), 1e-6, "filip gsl_fit_multilinear cov11") ;
gsl_test_rel (gsl_vector_get(&diag.vector,2), pow(expected_sd[2],2.0), 1e-6, "filip gsl_fit_multilinear cov22") ;
gsl_test_rel (gsl_vector_get(&diag.vector,3), pow(expected_sd[3],2.0), 1e-6, "filip gsl_fit_multilinear cov33") ;
gsl_test_rel (gsl_vector_get(&diag.vector,4), pow(expected_sd[4],2.0), 1e-6, "filip gsl_fit_multilinear cov44") ;
gsl_test_rel (gsl_vector_get(&diag.vector,5), pow(expected_sd[5],2.0), 1e-6, "filip gsl_fit_multilinear cov55") ;
gsl_test_rel (gsl_vector_get(&diag.vector,6), pow(expected_sd[6],2.0), 1e-6, "filip gsl_fit_multilinear cov66") ;
gsl_test_rel (gsl_vector_get(&diag.vector,7), pow(expected_sd[7],2.0), 1e-6, "filip gsl_fit_multilinear cov77") ;
gsl_test_rel (gsl_vector_get(&diag.vector,8), pow(expected_sd[8],2.0), 1e-6, "filip gsl_fit_multilinear cov88") ;
gsl_test_rel (gsl_vector_get(&diag.vector,9), pow(expected_sd[9],2.0), 1e-6, "filip gsl_fit_multilinear cov99") ;
gsl_test_rel (gsl_vector_get(&diag.vector,10), pow(expected_sd[10],2.0), 1e-6, "filip gsl_fit_multilinear cov1010") ;
gsl_test_rel (chisq, expected_chisq, 1e-7, "filip gsl_fit_multilinear chisq") ;
gsl_vector_free(c);
gsl_matrix_free(cov);
gsl_matrix_free(X);
gsl_multifit_linear_free (work);
}
{
gsl_multifit_linear_workspace * work =
gsl_multifit_linear_alloc (filip_n, filip_p);
gsl_matrix * X = gsl_matrix_alloc (filip_n, filip_p);
gsl_vector_view y = gsl_vector_view_array (filip_y, filip_n);
gsl_vector * w = gsl_vector_alloc (filip_n);
gsl_vector * c = gsl_vector_alloc (filip_p);
gsl_matrix * cov = gsl_matrix_alloc (filip_p, filip_p);
double chisq;
double expected_c[11] = { -1467.48961422980,
-2772.17959193342,
-2316.37108160893,
-1127.97394098372,
-354.478233703349,
-75.1242017393757,
-10.8753180355343,
-1.06221498588947,
-0.670191154593408E-01,
-0.246781078275479E-02,
-0.402962525080404E-04 };
/* computed using GNU Calc */
double expected_cov[11][11] ={ { 7.9269341767252183262588583867942e9, 1.4880416622254098343441063389706e10, 1.2385811858111487905481427591107e10, 6.0210784406215266653697715794241e9, 1.8936652526181982747116667336389e9, 4.0274900618493109653998118587093e8, 5.8685468011819735806180092394606e7, 5.7873451475721689084330083708901e6, 3.6982719848703747920663262917032e5, 1.3834818802741350637527054170891e4, 2.301758578713219280719633494302e2 },
{ 1.4880416622254098334697515488559e10, 2.7955091668548290835529555438088e10, 2.3286604504243362691678565997033e10, 1.132895006796272983689297219686e10, 3.5657281653312473123348357644683e9, 7.5893300392314445528176646366087e8, 1.1066654886143524811964131660002e8, 1.0921285448484575110763947787775e7, 6.9838139975394769253353547606971e5, 2.6143091775349597218939272614126e4, 4.3523386330348588614289505633539e2 },
{ 1.2385811858111487890788272968677e10, 2.3286604504243362677757802422747e10, 1.9412787917766676553608636489674e10, 9.4516246492862131849077729250098e9, 2.9771226694709917550143152097252e9, 6.3413035086730038062129508949859e8, 9.2536164488309401636559552742339e7, 9.1386304643423333815338760248027e6, 5.8479478338916429826337004060941e5, 2.1905933113294737443808429764554e4, 3.6493161325305557266196635180155e2 },
{ 6.0210784406215266545770691532365e9, 1.1328950067962729823273441573365e10, 9.4516246492862131792040001429636e9, 4.6053152992000107509329772255094e9, 1.4517147860312147098138030287038e9, 3.0944988323328589376402579060072e8, 4.5190223822292688669369522708712e7, 4.4660958693678497534529855690752e6, 2.8599340736122198213681258676423e5, 1.0720394998549386596165641244705e4, 1.7870937745661967319298031044424e2 },
{ 1.8936652526181982701620450132636e9, 3.5657281653312473058825073094524e9, 2.9771226694709917514149924058297e9, 1.451714786031214708936087401632e9, 4.5796563896564815123074920050827e8, 9.7693972414561515534525103622773e7, 1.427717861635658545863942948444e7, 1.4120161287735817621354292900338e6, 9.0484361228623960006818614875557e4, 3.394106783764852373199087455398e3, 5.6617406468519495376287407526295e1 },
{ 4.0274900618493109532650887473599e8, 7.589330039231444534478894935778e8, 6.3413035086730037947153564986653e8, 3.09449883233285893390542947998e8, 9.7693972414561515475770399055121e7, 2.0855726248311948992114244257719e7, 3.0501263034740400533872858749566e6, 3.0187475839310308153394428784224e5, 1.9358204633534233524477930175632e4, 7.2662989867560017077361942813911e2, 1.2129002231061036467607394277965e1 },
{ 5.868546801181973559370854830868e7, 1.1066654886143524778548044386795e8, 9.2536164488309401413296494869777e7, 4.5190223822292688587853853162072e7, 1.4277178616356585441556046753562e7, 3.050126303474040051574715592746e6, 4.4639982579046340884744460329946e5, 4.4212093985989836047285007760238e4, 2.8371395028774486687625333589972e3, 1.0656694507620102300567296504381e2, 1.7799982046359973175080475654123e0 },
{ 5.7873451475721688839974153925406e6, 1.0921285448484575071271480643397e7, 9.1386304643423333540728480344578e6, 4.4660958693678497427674903565664e6, 1.4120161287735817596182229182587e6, 3.0187475839310308117812257613082e5, 4.4212093985989836021482392757677e4, 4.3818874017028389517560906916315e3, 2.813828775753142855163154605027e2, 1.0576188138416671883232607188969e1, 1.7676976288918295012452853715408e-1 },
{ 3.6982719848703747742568351456818e5, 6.9838139975394768959780068745979e5, 5.8479478338916429616547638954781e5, 2.8599340736122198128717796825489e5, 9.0484361228623959793493985226792e4, 1.9358204633534233490579641064343e4, 2.8371395028774486654873647731797e3, 2.8138287757531428535592907878017e2, 1.8081118503579798222896804627964e1, 6.8005074291434681866415478598732e-1, 1.1373581557749643543869665860719e-2 },
{ 1.3834818802741350562839757244708e4, 2.614309177534959709397445440919e4, 2.1905933113294737352721470167247e4, 1.0720394998549386558251721913182e4, 3.3941067837648523632905604575131e3, 7.2662989867560016909534954790835e2, 1.0656694507620102282337905013451e2, 1.0576188138416671871337685672492e1, 6.8005074291434681828743281967838e-1, 2.5593857187900736057022477529078e-2, 4.2831487599116264442963102045936e-4 },
{ 2.3017585787132192669801658674163e2, 4.3523386330348588381716460685124e2, 3.6493161325305557094116270974735e2, 1.7870937745661967246233792737255e2, 5.6617406468519495180024059284629e1, 1.2129002231061036433003571679329e1, 1.7799982046359973135014027410646e0, 1.7676976288918294983059118597214e-1, 1.137358155774964353146460100337e-2, 4.283148759911626442000316269063e-4, 7.172253875245080423800933453952e-6 } };
double expected_chisq = 0.795851382172941E-03;
for (i = 0 ; i < filip_n; i++)
{
for (j = 0; j < filip_p; j++)
{
gsl_matrix_set(X, i, j, pow(filip_x[i], j));
}
}
gsl_vector_set_all (w, 1.0);
gsl_multifit_wlinear (X, w, &y.vector, c, cov, &chisq, work);
gsl_test_rel (gsl_vector_get(c,0), expected_c[0], 1e-7, "filip gsl_fit_multilinear c0") ;
gsl_test_rel (gsl_vector_get(c,1), expected_c[1], 1e-7, "filip gsl_fit_multilinear c1") ;
gsl_test_rel (gsl_vector_get(c,2), expected_c[2], 1e-7, "filip gsl_fit_multilinear c2") ;
gsl_test_rel (gsl_vector_get(c,3), expected_c[3], 1e-7, "filip gsl_fit_multilinear c3") ;
gsl_test_rel (gsl_vector_get(c,4), expected_c[4], 1e-7, "filip gsl_fit_multilinear c4") ;
gsl_test_rel (gsl_vector_get(c,5), expected_c[5], 1e-7, "filip gsl_fit_multilinear c5") ;
gsl_test_rel (gsl_vector_get(c,6), expected_c[6], 1e-7, "filip gsl_fit_multilinear c6") ;
gsl_test_rel (gsl_vector_get(c,7), expected_c[7], 1e-7, "filip gsl_fit_multilinear c7") ;
gsl_test_rel (gsl_vector_get(c,8), expected_c[8], 1e-7, "filip gsl_fit_multilinear c8") ;
gsl_test_rel (gsl_vector_get(c,9), expected_c[9], 1e-7, "filip gsl_fit_multilinear c9") ;
gsl_test_rel (gsl_vector_get(c,10), expected_c[10], 1e-7, "filip gsl_fit_multilinear c10") ;
for (i = 0; i < filip_p; i++)
{
for (j = 0; j < filip_p; j++)
{
gsl_test_rel (gsl_matrix_get(cov,i,j), expected_cov[i][j], 1e-6,
"filip gsl_fit_wmultilinear cov(%d,%d)", i, j) ;
}
}
gsl_test_rel (chisq, expected_chisq, 1e-7, "filip gsl_fit_multilinear chisq") ;
gsl_vector_free(w);
gsl_vector_free(c);
gsl_matrix_free(cov);
gsl_matrix_free(X);
gsl_multifit_linear_free (work);
}
}
void
test_pontius ()
{
size_t i, j;
gsl_multifit_linear_workspace * work =
gsl_multifit_linear_alloc (pontius_n, pontius_p);
gsl_multifit_robust_workspace * work_rob =
gsl_multifit_robust_alloc (gsl_multifit_robust_ols, pontius_n, pontius_p);
gsl_matrix * X = gsl_matrix_alloc (pontius_n, pontius_p);
gsl_vector_view y = gsl_vector_view_array (pontius_y, pontius_n);
gsl_vector * c = gsl_vector_alloc (pontius_p);
gsl_vector * r = gsl_vector_alloc (pontius_n);
gsl_matrix * cov = gsl_matrix_alloc (pontius_p, pontius_p);
double chisq, chisq_res;
double expected_c[3] = { 0.673565789473684E-03,
0.732059160401003E-06,
-0.316081871345029E-14};
double expected_sd[3] = { 0.107938612033077E-03,
0.157817399981659E-09,
0.486652849992036E-16 };
double expected_chisq = 0.155761768796992E-05;
gsl_vector_view diag = gsl_matrix_diagonal (cov);
gsl_vector_view exp_c = gsl_vector_view_array(expected_c, pontius_p);
gsl_vector_view exp_sd = gsl_vector_view_array(expected_sd, pontius_p);
for (i = 0 ; i < pontius_n; i++)
{
for (j = 0; j < pontius_p; j++)
{
gsl_matrix_set(X, i, j, pow(pontius_x[i], j));
}
}
/* test unweighted least squares */
gsl_multifit_linear (X, &y.vector, c, cov, &chisq, work);
gsl_multifit_linear_residuals(X, &y.vector, c, r);
gsl_blas_ddot(r, r, &chisq_res);
test_pontius_results("pontius gsl_multifit_linear",
c, &exp_c.vector,
&diag.vector, &exp_sd.vector,
chisq, chisq_res, expected_chisq);
/* test robust least squares */
gsl_multifit_robust (X, &y.vector, c, cov, work_rob);
test_pontius_results("pontius gsl_multifit_robust",
c, &exp_c.vector,
&diag.vector, &exp_sd.vector,
1.0, 1.0, 1.0);
/* test weighted least squares */
{
gsl_vector * w = gsl_vector_alloc (pontius_n);
double expected_cov[3][3] ={
{2.76754385964916e-01 , -3.59649122807024e-07, 9.74658869395731e-14},
{-3.59649122807024e-07, 5.91630591630603e-13, -1.77210703526497e-19},
{9.74658869395731e-14, -1.77210703526497e-19, 5.62573661988878e-26} };
gsl_vector_set_all (w, 1.0);
gsl_multifit_wlinear (X, w, &y.vector, c, cov, &chisq, work);
gsl_multifit_linear_residuals(X, &y.vector, c, r);
gsl_blas_ddot(r, r, &chisq_res);
test_pontius_results("pontius gsl_multifit_wlinear",
c, &exp_c.vector,
NULL, NULL,
chisq, chisq_res, expected_chisq);
for (i = 0; i < pontius_p; i++)
{
for (j = 0; j < pontius_p; j++)
{
gsl_test_rel (gsl_matrix_get(cov,i,j), expected_cov[i][j], 1e-10,
"pontius gsl_multifit_wlinear cov(%d,%d)", i, j) ;
}
}
gsl_vector_free(w);
}
gsl_vector_free(c);
gsl_vector_free(r);
gsl_matrix_free(cov);
gsl_matrix_free(X);
gsl_multifit_linear_free (work);
gsl_multifit_robust_free (work_rob);
}
static void update_doppler(int in_np, int out_np, float *gr2sr, meta_parameters *meta)
{
double d1 = meta->sar->range_doppler_coefficients[1];
double d2 = meta->sar->range_doppler_coefficients[2];
// least squares fit
const int N=1000;
double chisq, xi[N], yi[N];
int ii;
for (ii=0; ii<N; ++ii) {
xi[ii] = (double)ii/(double)N * (double)(out_np-1);
// the gr2sr array maps a slant range index to a ground range index
double g = gr2sr[(int)xi[ii]];
yi[ii] = d1*g + d2*g*g;
}
gsl_matrix *X, *cov;
gsl_vector *y, *w, *c;
X = gsl_matrix_alloc(N,3);
y = gsl_vector_alloc(N);
w = gsl_vector_alloc(N);
c = gsl_vector_alloc(3);
cov = gsl_matrix_alloc(3, 3);
for (ii=0; ii<N; ++ii) {
gsl_matrix_set(X, ii, 0, 1.0);
gsl_matrix_set(X, ii, 1, xi[ii]);
gsl_matrix_set(X, ii, 2, xi[ii]*xi[ii]);
gsl_vector_set(y, ii, yi[ii]);
gsl_vector_set(w, ii, 1.0);
}
gsl_multifit_linear_workspace *work = gsl_multifit_linear_alloc(N, 3);
gsl_multifit_wlinear(X, w, y, c, cov, &chisq, work);
gsl_multifit_linear_free(work);
double c0 = gsl_vector_get(c, 0);
double c1 = gsl_vector_get(c, 1);
double c2 = gsl_vector_get(c, 2);
gsl_matrix_free(X);
gsl_vector_free(y);
gsl_vector_free(w);
gsl_vector_free(c);
gsl_matrix_free(cov);
// now the x and y vectors are the desired doppler polynomial
double ee2=0;
for (ii=0; ii<out_np; ii+=100) {
// ii: index in slant range, g: index in ground range
double g = gr2sr[ii];
// dop1: doppler in slant, dop2: doppler in ground (should agree)
double dop1 = d1*g + d2*g*g;
double dop3 = c0 + c1*ii + c2*ii*ii;
double e2 = fabs(dop1-dop3);
ee2 += e2;
if (ii % 1000 == 0)
printf("%5d -> %8.3f %8.3f %5d -> %5d %7.4f\n",
ii, dop1, dop3, (int)g, ii, e2);
}
printf("Original: %14.9f %14.9f %14.9f\n", 0., d1, d2);
printf("Modified: %14.9f %14.9f %14.9f\n\n", c0, c1, c2);
printf("Overall errors: %8.4f\n", ee2);
meta->sar->range_doppler_coefficients[0] += c0;
meta->sar->range_doppler_coefficients[1] = c1;
meta->sar->range_doppler_coefficients[2] = c2;
}