00001 /*****************************************************************************
00002    Major portions of this software are copyrighted by the Medical College
00003    of Wisconsin, 1994-2000, and are released under the Gnu General Public
00004    License, Version 2.  See the file README.Copyright for details.
00005 ******************************************************************************/
00006 
00007 #include "mrilib.h"
00008 
00009 #undef  MAX_PARAMETERS
00010 #define MAX_PARAMETERS 10          /* maximum number of model parameters */
00011 
00012 typedef void void_func() ;
00013 
00014 typedef struct {
00015   int  numpar;                     /* number of parameters */
00016   void_func * func_func ;
00017   void_func * grad_func ;
00018   float pinit[MAX_PARAMETERS] ;
00019 } HUBER_interface ;
00020 
00021 /*--------------------------------------------------------------------*/
00022 
00023 static void sin_func( float * abc, int nt, float * t, float * val )
00024 {
00025    float a=abc[0], b=abc[1], c=abc[2], d=abc[3], e=abc[4] ;
00026    int ii ;
00027 
00028    for( ii=0 ; ii < nt ; ii++ ) val[ii] = a * sin(b*t[ii]+c) + d + e*t[ii] ;
00029 
00030    return ;
00031 }
00032 
00033 /*--------------------------------------------------------------------*/
00034 
00035 static void sin_grad( int k, float * abc, int nt, float * t, float * gval )
00036 {
00037    float a=abc[0], b=abc[1], c=abc[2], d=abc[3], e=abc[4] ;
00038    int ii ;
00039 
00040    switch( k ){
00041 
00042       case 0:
00043          for( ii=0 ; ii < nt ; ii++ ) gval[ii] = sin(b*t[ii]+c) ;
00044       return ;
00045 
00046       case 1:
00047          for( ii=0 ; ii < nt ; ii++ ) gval[ii] = a*t[ii]*cos(b*t[ii]+c) ;
00048       return ;
00049 
00050       case 2:
00051          for( ii=0 ; ii < nt ; ii++ ) gval[ii] = a*cos(b*t[ii]+c) ;
00052       return ;
00053 
00054       case 3:
00055          for( ii=0 ; ii < nt ; ii++ ) gval[ii] = 1 ;
00056       return ;
00057 
00058       case 4:
00059          for( ii=0 ; ii < nt ; ii++ ) gval[ii] = t[ii] ;
00060       return ;
00061    }
00062 }
00063 
00064 /*--------------------------------------------------------------------*/
00065 
00066 static float huber_aa( float c )
00067 {
00068    float a ;
00069    a =   0.5*erf(c/1.4142136)*(1.0-c*c)
00070        + 0.5*c*c - c*exp(-0.5*c*c)/sqrt(2.0*PI) ;
00071    return a ;
00072 }
00073 
00074 /*--------------------------------------------------------------------*/
00075 
00076 static HUBER_interface hhint = { 5, sin_func, sin_grad,
00077                                  {100.0,0.314,0.333,100.0,0.01} } ;
00078 
00079 static float range[5] = {100.0 , 0.310 , 1.555 , 300.0 , 0.02} ;
00080 
00081 #define XX(x)  (((x)<cc && (x)>-cc) ? 0.5*(xx)*(xx) : 0.5*(cc)*(cc))
00082 #define PSI(x) ( ((x)>=cc) ? cc : ((x)<=-cc) ? -cc : (x) )
00083 
00084 #define IGN 2
00085 
00086 void huber_func( int num, double to,double dt, float * vec, char ** str )
00087 {
00088    static float * t  = NULL ; static int ntold = 0 ;
00089    static float * v  = NULL ;
00090    static float * w  = NULL ;
00091    static float ** g = NULL ; static int ngold = 0 ;
00092    static float new_to,new_dt , old_to,old_dt ;
00093    static float cc = 1.0 , aa = -1.0 ;
00094    static char * strout = NULL ;
00095 
00096    float parm[MAX_PARAMETERS] , sig , xx , yy , *tau ;
00097    float pold[MAX_PARAMETERS] , rold ;
00098    int ii , nt , nparm , kk , nite ;
00099 
00100    nt    = num ;
00101    nparm = hhint.numpar ;
00102    if( nt <= 2*nparm ){ *str = NULL ; return ; }  /* do nothing */
00103 
00104    if( ntold < nt || ngold < nparm ){             /* reallocate memory */
00105       if( t != NULL ) free(t) ;
00106       if( v != NULL ) free(v) ;
00107       if( w != NULL ) free(w) ;
00108       t = (float *) malloc( sizeof(float)*nt ) ;
00109       v = (float *) malloc( sizeof(float)*nt ) ;
00110       w = (float *) malloc( sizeof(float)*nt ) ;
00111 
00112       if( g != NULL ){
00113          for( ii=0 ; ii < ngold ; ii++ ) free(g[ii]) ;
00114          free(g) ;
00115       }
00116       g = (float **) malloc( sizeof(float *)*nparm ) ;
00117       for( kk=0 ; kk < nparm ; kk++ )
00118          g[kk] = (float *) malloc( sizeof(float)*nt ) ;
00119       ngold = nparm ;
00120 
00121       ntold = nt ;
00122       old_to=-666.0 ; old_dt=-555.5 ;
00123 
00124       for( ii=0 ; ii < nt ; ii++ ) w[ii] = (ii<IGN) ? 0.0 : 1.0 ;
00125    }
00126 
00127    /* time points */
00128 
00129    new_to = to ; new_dt = dt ;
00130    if( new_to != old_to || new_dt != old_dt ){
00131       for( ii=0 ; ii < nt ; ii++ ) t[ii] = new_to + new_dt * ii ;
00132    }
00133 
00134    if( aa < 0.0 ) aa = huber_aa(cc) ;  /* Huber's 'a' constant */
00135 
00136    /* initialize parameters */
00137 
00138    for( kk=0 ; kk < nparm ; kk++ ) pold[kk] = hhint.pinit[kk] ;
00139    hhint.func_func( pold , nt , t , v ) ;
00140    xx = 0.0 ;
00141    for( ii=IGN ; ii < nt ; ii++ ) xx += fabs(vec[ii]-v[ii]) ;
00142    rold = xx ;
00143 
00144    for( nite=0 ; nite < 9999 ; nite++ ){
00145       for( kk=0 ; kk < nparm ; kk++ )
00146          parm[kk] = hhint.pinit[kk] + (drand48()-0.5)*range[kk] ;
00147       hhint.func_func( parm , nt , t , v ) ;
00148       xx = 0.0 ;
00149       for( ii=IGN ; ii < nt ; ii++ ) xx += fabs(vec[ii]-v[ii]) ;
00150       if( xx < rold ){
00151          for( kk=0 ; kk < nparm ; kk++ ) pold[kk] = parm[kk] ;
00152          rold = xx ;
00153       }
00154    }
00155    for( kk=0 ; kk < nparm ; kk++ ) parm[kk] = pold[kk] ;
00156    sig = rold / (nt-IGN) ;
00157 
00158    nite = 0 ;
00159    do{
00160       hhint.func_func( parm , nt , t , v ) ;         /* eval funcs */
00161 
00162       for( ii=0 ; ii < IGN ; ii++ ) v[ii] = 0.0 ;
00163       yy = 0 ;
00164       for( ii=IGN ; ii < nt ; ii++ ){
00165          v[ii] = vec[ii] - v[ii] ;                   /* residuals */
00166          xx = v[ii] / sig ; yy += XX(xx) ;
00167       }
00168       sig *= sqrt( yy / ((nt-hhint.numpar-IGN) * aa) ) ; /* Huber scale */
00169 
00170       for( ii=0 ; ii < nt ; ii++ ){                  /* Winsorize */
00171          xx = v[ii] / sig ; v[ii] = PSI(xx) * sig ;
00172       }
00173 
00174       for( kk=0 ; kk < nparm ; kk++ )                /* gradients */
00175          hhint.grad_func( kk , parm , nt , t , g[kk] ) ;
00176 
00177       tau = lsqfit( nt , v , w , nparm , g ) ;    /* fit residuals */
00178       if( tau == NULL ){
00179          fprintf(stderr,"*** lsqfit fails in huber!\n") ;
00180          *str = NULL ; return ;
00181       }
00182 
00183       for( kk=0 ; kk < nparm ; kk++ ) parm[kk] += tau[kk] ;  /* update */
00184 
00185       free(tau) ; nite++ ;
00186    } while(nite < 10) ;
00187 
00188    hhint.func_func( parm , nt , t , vec ) ;   /* eval funcs */
00189 
00190    if( strout != NULL ){ free(strout) ; strout = NULL ; }
00191 
00192    strout = THD_zzprintf( strout , "\n" ) ;
00193    for( kk=0 ; kk < nparm ; kk++ )
00194       strout = THD_zzprintf( strout , "param %d = %g\n" , kk,parm[kk] ) ;
00195 
00196    *str = strout ;
00197    return ;
00198 }