File indexing completed on 2024-12-22 04:18:16

0001 /***************************************************************************
0002  *                                                                         *
0003  *   copyright : (C) 2007 The University of Toronto                        *
0004  *                   netterfield@astro.utoronto.ca                         *
0005   *                                                                         *
0006  *   This program is free software; you can redistribute it and/or modify  *
0007  *   it under the terms of the GNU General Public License as published by  *
0008  *   the Free Software Foundation; either version 2 of the License, or     *
0009  *   (at your option) any later version.                                   *
0010  *                                                                         *
0011  ***************************************************************************/
0012 
0013 #ifndef NON_LINEAR_WEIGHTED_H
0014 #define NON_LINEAR_WEIGHTED_H
0015 
0016 #include <stdlib.h>
0017 #include <string.h>
0018 #include <math.h>
0019 #include <gsl/gsl_rng.h>
0020 #include <gsl/gsl_vector.h>
0021 #include <gsl/gsl_blas.h>
0022 #include <gsl/gsl_multifit_nlin.h>
0023 #include <gsl/gsl_statistics.h>
0024 #include <gsl/gsl_version.h>
0025 #include "common.h"
0026 
0027 struct data {
0028   size_t    n;
0029   const double* pdX;
0030   const double* pdY;
0031   const double* pdWeight;
0032 };
0033 
0034 int n_params = NUM_PARAMS;
0035 double offset_ = 0.0;
0036 
0037 void function_initial_estimate( const double* pdX, const double* pdY, int iLength, double* pdParameterEstimates );
0038 double function_calculate( double dX, double* pdParameters );
0039 void function_derivative( double dX, double* pdParameters, double* pdDerivatives );
0040 int function_f( const gsl_vector* pVectorX, void* pParams, gsl_vector* pVectorF );
0041 int function_df( const gsl_vector* pVectorX, void* pParams, gsl_matrix* pMatrixJ );
0042 int function_fdf( const gsl_vector* pVectorX, void* pParams, gsl_vector* pVectorF, gsl_matrix* pMatrixJ );
0043 bool kstfit_nonlinear_weighted(
0044   Kst::VectorPtr xVector, Kst::VectorPtr yVector, Kst::VectorPtr weightedVector,
0045   Kst::VectorPtr vectorOutYFitted, Kst::VectorPtr vectorOutYResiduals,
0046   Kst::VectorPtr vectorOutYParameters, Kst::VectorPtr vectorOutYCovariance,
0047   Kst::ScalarPtr scalarOutChi );
0048 
0049 
0050 int function_f( const gsl_vector* pVectorX, void* pParams, gsl_vector* pVectorF ) {
0051   double    dParameters[NUM_PARAMS];
0052   double    dY;
0053   data*     pData   = (data*)pParams;
0054   int   i;
0055 
0056   for( i=0; i<n_params; i++ ) {
0057     dParameters[i] = gsl_vector_get( pVectorX, i );
0058   }
0059 
0060   for( i=0; i<pData->n; i++ ) {
0061     dY  = function_calculate( pData->pdX[i], dParameters );
0062     gsl_vector_set( pVectorF, i, (dY - pData->pdY[i])*pData->pdWeight[i] );
0063   }
0064 
0065   return GSL_SUCCESS;
0066 }
0067 
0068 
0069 int function_df( const gsl_vector* pVectorX, void* pParams, gsl_matrix* pMatrixJ ) {
0070   double dParameters[NUM_PARAMS];
0071   double dDerivatives[NUM_PARAMS];
0072   data*  pData  = (data*)pParams;
0073   int i;
0074   int j;
0075 
0076   for( i=0; i<n_params; i++ ) {
0077     dParameters[i] = gsl_vector_get( pVectorX, i );
0078   }
0079 
0080   for( i=0; i<pData->n; i++ ) {
0081     function_derivative( pData->pdX[i], dParameters, dDerivatives );
0082 
0083     for( j=0; j<n_params; j++ ) {
0084       gsl_matrix_set( pMatrixJ, i, j, dDerivatives[j] * pData->pdWeight[i] );
0085     }
0086   }
0087 
0088   return GSL_SUCCESS;
0089 }
0090 
0091 
0092 int function_fdf( const gsl_vector* pVectorX, void* pParams, gsl_vector* pVectorF, gsl_matrix* pMatrixJ ) {  
0093   function_f( pVectorX, pParams, pVectorF );
0094   function_df( pVectorX, pParams, pMatrixJ );
0095 
0096   return GSL_SUCCESS;
0097 }
0098 
0099 
0100 bool kstfit_nonlinear_weighted(
0101   Kst::VectorPtr xVector, Kst::VectorPtr yVector, Kst::VectorPtr weightsVector,
0102   Kst::VectorPtr vectorOutYFitted, Kst::VectorPtr vectorOutYResiduals,
0103   Kst::VectorPtr vectorOutYParameters, Kst::VectorPtr vectorOutYCovariance,
0104   Kst::ScalarPtr scalarOutChi ) {
0105 
0106   const gsl_multifit_fdfsolver_type* pType;
0107   gsl_multifit_fdfsolver* pSolver;
0108   gsl_multifit_function_fdf function;
0109   gsl_vector_view vectorViewInitial;
0110   gsl_matrix* pMatrixCovariance;
0111   gsl_matrix *pMatrixJacobian;
0112   struct data d;  
0113   double dXInitial[NUM_PARAMS];
0114   double* pInputs[3];
0115   int iIterations = 0;
0116   int iLength;
0117   int bReturn = false;
0118   int iStatus;
0119   int i;
0120   int j;
0121 
0122   if (xVector->length() >= 2 &&
0123       yVector->length() >= 2 &&
0124       weightsVector->length() >= 2) {
0125     iLength = yVector->length();
0126     if( xVector->length() > iLength ) {
0127       iLength = xVector->length();
0128     }
0129 
0130     // do any necessary interpolation...
0131     pInputs[XVALUES] = (double*)malloc(iLength * sizeof( double ));
0132 
0133     double const *v_x = xVector->noNanValue();
0134     double const *v_y = yVector->noNanValue();
0135 
0136     if (xVector->length() == iLength) {
0137       for( i=0; i<iLength; i++) {
0138         pInputs[XVALUES][i] = v_x[i];
0139       }
0140     } else {
0141       for( i=0; i<iLength; i++) {
0142         pInputs[XVALUES][i] = interpolate( i, iLength, v_x, xVector->length() );
0143       }
0144     }
0145 
0146     pInputs[YVALUES] = (double*)malloc(iLength * sizeof( double ));
0147     if (yVector->length() == iLength) {
0148       for( i=0; i<iLength; i++) {
0149         pInputs[YVALUES][i] = v_y[i];
0150       }
0151     } else {
0152       for( i=0; i<iLength; i++) {
0153         pInputs[YVALUES][i] = interpolate( i, iLength, v_y, yVector->length() );
0154       }
0155     }
0156 
0157     pInputs[WEIGHTS] = (double*)malloc(iLength * sizeof( double ));
0158     if (weightsVector->length() == iLength) {
0159       for( i=0; i<iLength; i++) {
0160         pInputs[WEIGHTS][i] = weightsVector->value()[i];
0161       }
0162     } else {
0163       for( i=0; i<iLength; i++) {
0164         pInputs[WEIGHTS][i] = interpolate( i, iLength, weightsVector->value(), weightsVector->length() );
0165       }
0166     }
0167 
0168     if( iLength > NUM_PARAMS ) {
0169       vectorOutYFitted->resize(iLength);
0170       vectorOutYResiduals->resize(iLength);
0171       vectorOutYParameters->resize(NUM_PARAMS);
0172       vectorOutYCovariance->resize(NUM_PARAMS * NUM_PARAMS);
0173 
0174       pType   = gsl_multifit_fdfsolver_lmsder;
0175       pSolver = gsl_multifit_fdfsolver_alloc( pType, iLength, n_params );
0176       if( pSolver != NULL ) {
0177         d.n        = iLength;
0178         d.pdX        = pInputs[XVALUES];
0179         d.pdY      = pInputs[YVALUES];
0180         d.pdWeight = pInputs[WEIGHTS];
0181 
0182         function.f        = function_f;
0183         function.df       = function_df;
0184         function.fdf      = function_fdf;
0185         function.n        = iLength;
0186         function.p        = n_params;
0187         function.params = &d;
0188 
0189         pMatrixCovariance = gsl_matrix_alloc( n_params, n_params );
0190         if( pMatrixCovariance != NULL ) {
0191           function_initial_estimate( pInputs[XVALUES], pInputs[YVALUES], iLength, dXInitial );
0192           vectorViewInitial = gsl_vector_view_array( dXInitial, n_params );
0193 
0194           gsl_multifit_fdfsolver_set( pSolver, &function, &vectorViewInitial.vector );
0195 
0196           //
0197           // iterate to a solution...
0198           //
0199           do {
0200             iStatus = gsl_multifit_fdfsolver_iterate( pSolver );
0201             if( iStatus == GSL_SUCCESS ) {
0202               iStatus = gsl_multifit_test_delta( pSolver->dx, pSolver->x, 1.0e-4, 1.0e-4 );
0203             }
0204             iIterations++;
0205           }
0206           while( iStatus == GSL_CONTINUE && iIterations < MAX_NUM_ITERATIONS );
0207 
0208 #if GSL_MAJOR_VERSION >= 2
0209           pMatrixJacobian = gsl_matrix_alloc( iLength, n_params );
0210 #else
0211           pMatrixJacobian = pSolver->J;
0212 #endif
0213 
0214           if ( pMatrixJacobian != NULL) {
0215 #if GSL_MAJOR_VERSION >= 2
0216             gsl_multifit_fdfsolver_jac( pSolver, pMatrixJacobian );
0217 #endif
0218             gsl_multifit_covar( pMatrixJacobian, 0.0, pMatrixCovariance );
0219 
0220             //
0221             // determine the fitted values...
0222             //
0223             for( i=0; i<n_params; i++ ) {
0224               dXInitial[i] = gsl_vector_get( pSolver->x, i );
0225             }
0226 
0227             for( i=0; i<iLength; i++ ) {
0228               vectorOutYFitted->raw_V_ptr()[i] = function_calculate( pInputs[XVALUES][i], dXInitial );
0229               vectorOutYResiduals->raw_V_ptr()[i] = pInputs[YVALUES][i] - vectorOutYFitted->raw_V_ptr()[i];
0230             }
0231 
0232             //
0233             // fill in the parameter values and covariance matrix...
0234             //
0235             for( i=0; i<NUM_PARAMS; i++ ) {
0236               if (i<n_params) {
0237                 vectorOutYParameters->raw_V_ptr()[i] = gsl_vector_get( pSolver->x, i );
0238               } else {
0239                 vectorOutYParameters->raw_V_ptr()[i] = offset_;
0240               }
0241               for( j=0; j<NUM_PARAMS; j++ ) {
0242                 if ((i<n_params) && (j<n_params)) {
0243                   vectorOutYCovariance->raw_V_ptr()[(i*NUM_PARAMS)+j] = gsl_matrix_get( pMatrixCovariance, i, j );
0244                 } else {
0245                   vectorOutYCovariance->raw_V_ptr()[(i*NUM_PARAMS)+j] = 0.0;
0246                 }
0247               }
0248             }
0249 
0250             //
0251             // determine the value of chi^2/nu
0252             //
0253             scalarOutChi->setValue(gsl_blas_dnrm2( pSolver->f ));
0254 
0255             bReturn = true;
0256 
0257 #if GSL_MAJOR_VERSION >= 2
0258             gsl_matrix_free( pMatrixJacobian );
0259 #endif
0260           }
0261           gsl_matrix_free( pMatrixCovariance );
0262         }
0263         gsl_multifit_fdfsolver_free( pSolver );
0264       }
0265     }
0266     postcursor(true, pInputs);
0267   }
0268   return bReturn;
0269 }
0270 
0271 #endif