Example #1
0
void
test_f (const gsl_min_fminimizer_type * T, 
        const char * description, gsl_function *f,
        double lower_bound, double middle, double upper_bound, 
        double correct_minimum)
{
  int status;
  size_t iterations = 0;
  double m, a, b;
  double x_lower, x_upper;
  gsl_min_fminimizer * s;

  x_lower = lower_bound;
  x_upper = upper_bound;

  s = gsl_min_fminimizer_alloc (T) ;
  gsl_min_fminimizer_set (s, f, middle, x_lower, x_upper) ;
  
  do 
    {
      iterations++ ;

      status = gsl_min_fminimizer_iterate (s);

      m = gsl_min_fminimizer_x_minimum(s);
      a = gsl_min_fminimizer_x_lower(s);
      b = gsl_min_fminimizer_x_upper(s);

#ifdef DEBUG
      printf("%.12f %.18f %.12f %.18f %.12f %.18f status=%d\n", 
             a, GSL_FN_EVAL(f, a), m, GSL_FN_EVAL(f, m), b, GSL_FN_EVAL(f, b), status);
#endif

      if (a > b)
        gsl_test (GSL_FAILURE, "interval is invalid (%g,%g)", a, b);

      if (m < a || m > b)
        gsl_test (GSL_FAILURE, "m lies outside interval %g (%g,%g)", m, a, b);

      if (status) break ;

      status = gsl_min_test_interval (a, b, EPSABS, EPSREL);
    }
  while (status == GSL_CONTINUE && iterations < MAX_ITERATIONS);

  gsl_test (status, "%s, %s (%g obs vs %g expected) ", 
            gsl_min_fminimizer_name(s), description, 
            gsl_min_fminimizer_x_minimum(s), correct_minimum);

  /* check the validity of the returned result */

  if (!WITHIN_TOL (m, correct_minimum, EPSREL, EPSABS))
    {
      gsl_test (GSL_FAILURE, "incorrect precision (%g obs vs %g expected)", 
                m, correct_minimum);
    }

  gsl_min_fminimizer_free (s);

}
Example #2
0
  /**
   * The Brent minimization algorithm combines a parabolic interpolation with the golden section algorithm.
   * This produces a fast algorithm which is still robust. The outline of the algorithm can be summarized as
   * follows: on each iteration Brent's method approximates the function using an interpolating parabola
   * through three existing points. The minimum of the parabola is taken as a guess for the minimum.
   * If it lies within the bounds of the current interval then the interpolating point is accepted,
   * and used to generate a smaller interval. If the interpolating point is not accepted then the
   * algorithm falls back to an ordinary golden section step. The full details of Brent's method
   * include some additional checks to improve convergence.
   *
   * @author Sharmila Prasad (8/15/2011)
   *
   * @param x_lower - x_lower interval
   * @param x_upper - x_upper interval
   * @param Func - gsl_function, high-level driver for the algorithm
   *               Continuous function of one variable for the minimizers to operate on
   * @param x_minimum - x_minimum calculated parabola min value
   * @return double - status
   */
  int Photometry::brentminimizer(double x_lower, double x_upper, gsl_function *Func,
      double & x_minimum, double tolerance) {
    int status;
    int iter=0, max_iter=100;

    const gsl_min_fminimizer_type *T;
    gsl_min_fminimizer *s;
    //double m_expected = M_PI;

    T = gsl_min_fminimizer_brent;
    s = gsl_min_fminimizer_alloc(T);

    // This function sets, or resets, an existing minimizer s to use the function Func and
    // the initial search interval [x_lower, x_upper], with a guess for the location of
    // the minimum x_minimum. If the interval given does not contain a minimum, then
    // the function returns an error code of GSL_EINVAL.
    gsl_min_fminimizer_set(s, Func, x_minimum, x_lower, x_upper);

    do {
      iter++;
      status    = gsl_min_fminimizer_iterate(s);
      x_minimum = gsl_min_fminimizer_x_minimum(s);
      x_lower   = gsl_min_fminimizer_x_lower(s);
      x_upper   = gsl_min_fminimizer_x_upper(s);

      status = gsl_min_test_interval(x_lower, x_upper, tolerance, 0.0);
    } while(status == GSL_CONTINUE && iter < max_iter);

    // This function frees all the memory associated with the minimizer s.
    gsl_min_fminimizer_free(s);

    return status;
  }
Example #3
0
File: min.c Project: lemahdi/mglib
int
main (void)
{
  int status;
  int iter = 0, max_iter = 100;
  const gsl_min_fminimizer_type *T;
  gsl_min_fminimizer *s;
  double m = 2.0, m_expected = M_PI;
  double a = 0.0, b = 6.0;
  gsl_function F;

  F.function = &fn1;
  F.params = 0;

  T = gsl_min_fminimizer_brent;
  s = gsl_min_fminimizer_alloc (T);
  gsl_min_fminimizer_set (s, &F, m, a, b);

  printf ("using %s method\n",
          gsl_min_fminimizer_name (s));

  printf ("%5s [%9s, %9s] %9s %10s %9s\n",
          "iter", "lower", "upper", "min",
          "err", "err(est)");

  printf ("%5d [%.7f, %.7f] %.7f %+.7f %.7f\n",
          iter, a, b,
          m, m - m_expected, b - a);

  do
    {
      iter++;
      status = gsl_min_fminimizer_iterate (s);

      m = gsl_min_fminimizer_x_minimum (s);
      a = gsl_min_fminimizer_x_lower (s);
      b = gsl_min_fminimizer_x_upper (s);

      status 
        = gsl_min_test_interval (a, b, 0.001, 0.0);

      if (status == GSL_SUCCESS)
        printf ("Converged:\n");

      printf ("%5d [%.7f, %.7f] "
              "%.7f %+.7f %.7f\n",
              iter, a, b,
              m, m - m_expected, b - a);
    }
  while (status == GSL_CONTINUE && iter < max_iter);

  gsl_min_fminimizer_free (s);

  return status;
}
Example #4
0
File: fit.c Project: savila/HALOGEN
/*=============================================================================
 *                              MINIMIZATION
 *=============================================================================*/
double minimize(double a,double b, double m){
	  int status;
	  int iter = 0, max_iter = 100;
	  const gsl_min_fminimizer_type *T;
	  gsl_min_fminimizer *s;
	  double epsabs= 0.005; //eps of solution
	  double epsrel= 0.0; //eps of solution

	  gsl_function F;

	  F.function = &min_func;
	  F.params = 0;

	  T = gsl_min_fminimizer_brent;
	  s = gsl_min_fminimizer_alloc (T);
	  gsl_min_fminimizer_set (s, &F, m, a, b);

	  printf ("using %s method\n",
	          gsl_min_fminimizer_name (s));

	  printf ("%5s [%9s, %9s] %9s %9s\n",
	          "iter", "lower", "upper", "min",
	          "err(est)");

	  printf ("%5d [%.7f, %.7f] %.7f %.7f\n",
	          iter, a, b,
	          m, b - a);

	  do
	    {
	      iter++;
	      status = gsl_min_fminimizer_iterate (s);

	      m = gsl_min_fminimizer_x_minimum (s);
	      a = gsl_min_fminimizer_x_lower (s);
	      b = gsl_min_fminimizer_x_upper (s);

	      status
	        = gsl_min_test_interval (a, b, epsabs, epsrel);

	      if (status == GSL_SUCCESS)
	        printf ("Converged:\n");

	      printf ("%5d [%.7f, %.7f] "
	              "%.7f %.7f\n",
	              iter, a, b,
	              m, b - a);
	    }
	  while (status == GSL_CONTINUE && iter < max_iter);

	  gsl_min_fminimizer_free (s);

	  return m;
	}
Example #5
0
double iteractive_max(Params *params,double a, double b) {
  const gsl_min_fminimizer_type *T;
  gsl_min_fminimizer *s;
  gsl_function F;
  int status;
  int iter = 0, max_iter = 100;
  double m=(a+b)/2.0;

  printf("a=%lg b=%lg\n",a,b);

  printf("f(a)=%lg f(b)=%lg f((a+b)/2)=%lg\n",max_fun(a,params),max_fun(b,params),max_fun((a+b)/2.0,params));

  F.function = &max_fun;
  F.params = (void*)params;

  T = gsl_min_fminimizer_brent;
  s = gsl_min_fminimizer_alloc (T);
  gsl_min_fminimizer_set (s, &F, m, a, b);

  printf ("using %s method\n",gsl_min_fminimizer_name (s));

  fflush(stdout);

  printf("Start Interactions\n");

  fflush(stdout);

  do
    {
      iter++;
      status = gsl_min_fminimizer_iterate (s);

      m = gsl_min_fminimizer_x_minimum (s);
      a = gsl_min_fminimizer_x_lower (s);
      b = gsl_min_fminimizer_x_upper (s);

      status = gsl_min_test_interval (a, b, 0.001, 0.0);

      if (status == GSL_SUCCESS)
        printf ("Converged:\n");

      printf ("%5d [%.7f, %.7f] "
              "%.7f %.7f\n",
              iter, a, b,
              m,  b - a);
    }
  while (status == GSL_CONTINUE && iter < max_iter);

  gsl_min_fminimizer_free (s);

  return m;
}
Example #6
0
Minimiser::Solution Minimiser::solution() {
    Solution sol;
    sol.x.lower = gsl_min_fminimizer_x_lower(s);
    sol.x.upper = gsl_min_fminimizer_x_upper(s);
    sol.x.minimum = gsl_min_fminimizer_x_minimum(s);
    sol.f.lower = gsl_min_fminimizer_f_lower(s);
    sol.f.upper = gsl_min_fminimizer_f_upper(s);
    sol.f.minimum = gsl_min_fminimizer_f_minimum(s);
    sol.iterations = iterations;
    sol.converged = (status == GSL_SUCCESS);
    sol.statusCode = status;
    return sol;
}
Example #7
0
int main ()
{
  int status;
  int iter = 0, max_iter = 100; /*Max. number of iterations*/
  const gsl_min_fminimizer_type *T;
  gsl_min_fminimizer *s;
  double m = 0.7; /* Starting point for the search*/
  double a = -4.0, b = 1.0; /* The interval in which the minimum lies*/
  gsl_function F;
     
  F.function = &fn_1; /* Function to Minimize*/
  F.params = 0;
     
  T = gsl_min_fminimizer_goldensection; /*Set the minimization algorithm - Uses Golden Section*/
  s = gsl_min_fminimizer_alloc (T); /* Initialize the minimizer*/
  gsl_min_fminimizer_set (s, &F, m, a, b); /*Set up the minimizer*/
     
  printf ("Using %s method\n", gsl_min_fminimizer_name (s));
  printf ("%5s [%9s, %9s] %9s \n","iter", "lower", "upper", "min", "err", "err(est)");
  printf ("%5d [%.7f, %.7f] %.7f \n",  iter, a, b, m);

  /* Set up the iterative minimization procedure*/
     
  do
    {
      iter++;
      status = gsl_min_fminimizer_iterate(s);
     
      m = gsl_min_fminimizer_x_minimum (s);
      a = gsl_min_fminimizer_x_lower (s);
      b = gsl_min_fminimizer_x_upper (s);
     
      status = gsl_min_test_interval (a, b, 0.001, 0.0);
     
      if (status == GSL_SUCCESS)
	printf ("Converged:\n");
     
      printf ("%5d [%.7f, %.7f] %.7f\n",iter, a, b, m);
    } while (status == GSL_CONTINUE && iter < max_iter);
     
  gsl_min_fminimizer_free (s);
     
  return status;
}
Example #8
0
void
test_f_e (const gsl_min_fminimizer_type * T, 
          const char * description, gsl_function *f,
          double lower_bound, double middle, double upper_bound, 
          double correct_minimum)
{
  int status;
  size_t iterations = 0;
  double x_lower, x_upper;
  double a, b;
  gsl_min_fminimizer * s;

  x_lower = lower_bound;
  x_upper = upper_bound;

  s = gsl_min_fminimizer_alloc (T) ;
  status = gsl_min_fminimizer_set (s, f, middle, x_lower, x_upper) ; 

  if (status != GSL_SUCCESS) 
    {
      gsl_min_fminimizer_free (s) ;
      gsl_test (status == GSL_SUCCESS, "%s, %s", T->name, description);
      return ;
    }

  do 
    {
      iterations++ ;
      gsl_min_fminimizer_iterate (s);
      a = gsl_min_fminimizer_x_lower(s);
      b = gsl_min_fminimizer_x_upper(s);

      status = gsl_min_test_interval (a, b, EPSABS, EPSREL);
    }
  while (status == GSL_CONTINUE && iterations < MAX_ITERATIONS);

  gsl_test (!status, "%s, %s", gsl_min_fminimizer_name(s), description, 
            gsl_min_fminimizer_x_minimum(s) - correct_minimum);

  gsl_min_fminimizer_free (s);
}
Example #9
0
/** ***********************************************************************************************/
double get_best_stepsize(double delta,double lower,double upper,int maxiters_hessian, struct fnparams *gparams,
			 double (* compute_mlik_nm_brent) (double finitestepsize, void *params), gsl_min_fminimizer *s1, double *finitestepsize,double *saverror)
{
  gsl_function F1; 
  /*const gsl_min_fminimizer_type *T1;
  gsl_min_fminimizer *s1;*/ 
  int status=GSL_SUCCESS;
  int iter;double myerror=0.0;
  *finitestepsize=delta;/** --copy-- current delta to finitestepsize as finitstepsize will be changed in here **/
  /** could probably do the memory withouth alloc free in here but outside of function to avoid extra mallocs */
   F1.function = compute_mlik_brent;
   F1.params = gparams;
   gsl_min_fminimizer_set (s1, &F1, *finitestepsize,lower,upper);
   iter=0;
        do
         {
           iter++; 
	 /* Rprintf("iter=%d\n",iter);*/
	   status = gsl_min_fminimizer_iterate (s1);
           *finitestepsize = gsl_min_fminimizer_x_minimum (s1);
           lower=gsl_min_fminimizer_x_lower (s1);
	   upper=gsl_min_fminimizer_x_upper (s1);
          status = gsl_min_test_interval (lower,upper, 0.00001, 0.0);
	  /*Rprintf("[%e,%e] min=%e\n",lower,upper,finitestepsize);*/
           /*if (status==GSL_SUCCESS)*/ 
             /*break;*/
         }
        while (status == GSL_CONTINUE && iter < maxiters_hessian); 
     
     /** now get the error in hessian **/
     
     /*compute_mlik_nm_brent(finitestepsize, &gparams);*/
    
     /*gsl_min_fminimizer_free (s1);*/
 
  myerror=compute_mlik_brent(*finitestepsize, gparams);
  *saverror=myerror;/** this is a pointer to the dag->HessianError[nodeid] */
  Rprintf("Brent minimiser: error in mlik=%e in [%e,%e] with best h=%e\n",myerror,lower,upper,*finitestepsize);

return(myerror);  
}
Example #10
0
void interpolate_minimum(interp_info *interp,
                         double       x_lo_in,
                         double       x_guess_in,
                         double       x_hi_in,
                         double       threshold,
                         double *     x_minima,
                         double *     y_minima) {
    double                         x_lo;
    double                         x_hi;
    double                         x_guess;
    double                         r_val;
    const gsl_min_fminimizer_type *T;
    gsl_min_fminimizer *           s;
    gsl_function                   F;
    int                            status;
    int                            iter = 0;
    int                            max_iter;

    x_lo       = x_lo_in;
    x_guess    = x_guess_in;
    x_hi       = x_hi_in;
    max_iter   = GBP_MAX(100, interp->n);
    F.function = interpolate_minimum_function;
    F.params   = (void *)interp;
    T          = gsl_min_fminimizer_brent;
    s          = gsl_min_fminimizer_alloc(T);
    gsl_min_fminimizer_set(s, &F, x_guess, x_lo, x_hi);

    do {
        iter++;
        status  = gsl_min_fminimizer_iterate(s);
        x_lo    = gsl_min_fminimizer_x_lower(s);
        x_guess = gsl_min_fminimizer_x_minimum(s);
        x_hi    = gsl_min_fminimizer_x_upper(s);
        status  = gsl_min_test_interval(x_lo, x_hi, 0., threshold);
    } while(status == GSL_CONTINUE && iter < max_iter);
    (*x_minima) = x_guess;
    (*y_minima) = interpolate(interp, x_guess);
}
Example #11
0
void FC_FUNC_(oct_1dminimize, OCT_1DMINIMIZE)(double *a, double *b, double *m, func1 f, int *status)
{
  int iter = 0;
  int max_iter = 100;
  const gsl_min_fminimizer_type *T;
  gsl_min_fminimizer *s;
  gsl_function F;
  param_f1_t p;

  p.func = f;

  F.function = &fn1;
  F.params = (void *) &p;

  T = gsl_min_fminimizer_brent;
  s = gsl_min_fminimizer_alloc (T);

  *status = gsl_min_fminimizer_set (s, &F, *m, *a, *b);

  gsl_set_error_handler_off();

  do
    {
      iter++;
      *status = gsl_min_fminimizer_iterate (s);

      *m = gsl_min_fminimizer_x_minimum (s);
      *a = gsl_min_fminimizer_x_lower (s);
      *b = gsl_min_fminimizer_x_upper (s);

      *status = gsl_min_test_interval (*a, *b, 0.00001, 0.0);

      /*if (*status == GSL_SUCCESS) printf ("Converged:\n");*/
      /*printf ("%5d [%.7f, %.7f] %.7f \n", iter, *a, *b,*m);*/
    }
  while (*status == GSL_CONTINUE && iter < max_iter);
  gsl_min_fminimizer_free(s);

}
Example #12
0
/* this function minimizes the one-dimensional function, starting at point
   x0, in the interval [x_min, x_max]. */
int f_min (double x_min, double x0, double x_max, double *minimum, struct f_min_params *par) {
    size_t iter;
    int status;
    double x_lo, x_hi;
    gsl_function func;

    /* allocates the memory for the minimizer */
    gsl_min_fminimizer *s = gsl_min_fminimizer_alloc (par->type);

    /* defines the function to minimize and to be handed to the minimizer */
    func.function = par->func;
    func.params = par->func_p;

    /* initializes the minimizer */
    gsl_min_fminimizer_set (s, &func, x0, x_min, x_max);

    /* iterate */
    iter = 0;
    do {
        iter++;
        status = gsl_min_fminimizer_iterate (s);

        /* check interval for convergence */
        x_lo = gsl_min_fminimizer_x_lower (s);
        x_hi = gsl_min_fminimizer_x_upper (s);
        status = gsl_min_test_interval (x_lo, x_hi, par->eps_abs, par->eps_rel);

        if (par->verbose)
            printf ("iter %lu: x_lo = %f x_hi = %f\n", iter, x_lo, x_hi);

    } while (status==GSL_CONTINUE && iter<par->max_iter);

    /* get the value of the minimum and free the memory */
    *minimum = gsl_min_fminimizer_x_minimum (s);
    gsl_min_fminimizer_free (s);

    return status;
}
Example #13
0
int Minimizer::minimize(gsl_function F, double startValue, double lowerBound,
                        double upperBound) {
  gsl_min_fminimizer_set(s, &F, startValue, lowerBound, upperBound);
  int iter = 0;
  int status;
  double a, b;
  do {
    iter++;
    status = gsl_min_fminimizer_iterate(s);
    if (status == GSL_EBADFUNC || status == GSL_FAILURE) {
      return -1;
    }

    finalX = gsl_min_fminimizer_x_minimum(s);
    a = gsl_min_fminimizer_x_lower(s);
    b = gsl_min_fminimizer_x_upper(s);

    // stopping rule
    // see
    // http://www.gnu.org/software/gsl/manual/html_node/Minimization-Stopping-Parameters.html
    status = gsl_min_test_interval(a, b, epsabs, epsrel);

    if (status == GSL_SUCCESS) {
      // printf ("Converged:\n");
      finalY = gsl_min_fminimizer_f_minimum(s);
      return 0;
    }
    // printf("%5d\ta=%g\tb=%g\tfinalX=%g\n", iter, a, b, finalX);
    // printf ("%5d .7f, [.7f] "
    //         "%.7f %+.7f %.7f\n",
    //         iter, a, b,
    //         m, m - m_expected, b - a);
  } while (status == GSL_CONTINUE && iter < this->maxIter);

  return -1;
}
Example #14
0
static VALUE rb_gsl_min_fminimizer_x_minimum(VALUE obj)
{
  gsl_min_fminimizer *gmf = NULL;
  Data_Get_Struct(obj, gsl_min_fminimizer, gmf);
  return rb_float_new(gsl_min_fminimizer_x_minimum(gmf));
}
Example #15
0
bool PairMatchingCut::Get_CDA_FromCalculation( EventClass &E, int t, int a, double &z, double &cda, int &iterations)
{
	//=====================================================================//

	double z1,z2;
	//------------------------------------------------------------------//
	// Find the range where to look for the CDA                         //
	//------------------------------------------------------------------//
	if( E.dcmin[t] < E.dcmin[a])
	{
          z1 = E.geo->dc(std::min(E.dcmax[t], E.dcmin[a])).center().z();
          z2 = E.geo->dc(std::max(E.dcmax[t], E.dcmin[a])).center().z();
	}
	else
	{
          z1 = E.geo->dc(std::min(E.dcmax[a], E.dcmin[t])).center().z();
          z2 = E.geo->dc(std::max(E.dcmax[a], E.dcmin[t])).center().z();
	}
	z1 = z1 -0.2;
	z2 = z2 +0.2;

	// L corresponding to the maximum frequency:
	// L1 --> omega1, L2 --> omega2, omega_max = w1 + w2 --> Lmin

	double lmin = fabs(E.wavelen[t])*fabs(E.wavelen[a])/(fabs(E.wavelen[t])+fabs(E.wavelen[a]));
	// cout<<" In Get_CDA_FromCalculation 2   lmin ="<<lmin<<endl;
	// cout<<" In Get_CDA_FromCalculation 2   wavelen ="<<E.wavelen[t]<<"   "<<E.wavelen[a]<<endl;
	// cout<<" In Get_CDA_FromCalculation 2   ptot ="<<E.ptot[t]<<"   "<<E.costh[a]<<endl;
	//------------------------------------------------------------------//
	// Sample the distance to isolate local minima                      //
	// First decide about the number of points in the sample            //
	//------------------------------------------------------------------//
	int nsample = int(12*(1.+fabs(z1-z2)/lmin));

	TrackDistanceUV distance(E, t, a, z1, z2);
	vector<double> dd(nsample);
	for(int i = 0; i<nsample; i++) {
		double z = z1 + (z2-z1)*i/double(nsample-1);
		dd[i] = distance(z);
	}

	double A,M,B;
	vector<MinBracket> brackets;
	for(int i=1; i<nsample-1; i++) {
		if( (dd[i] < dd[i-1]) && (dd[i] <= dd[i+1]) ) {
			A = z1 + (z2-z1)*(i-1)/double(nsample-1);
			M = z1 + (z2-z1)*i/double(nsample-1);
			B = z1 + (z2-z1)*(i+1)/double(nsample-1);
			brackets.push_back(MinBracket(A,M,B));
		}
	}

	//------------------------------------------------------------------//
	// Check the ends of the search interval                            //
	//------------------------------------------------------------------//

	// cout<<" In Get_CDA_FromCalculation 3"<<endl;
	// The distance of approach
	vector<double> Alldist;
	// Position of minimal distance of approach
	vector<double> Allz;
	vector<int> Alliter;

	if(dd[0] <= dd[1]) {
		double ztest = z1 + std::min(z2-z1, tolerance_abs);
		double ftest = distance(ztest);
		if( (ftest <= dd[0]) && (ftest <= dd[1])) {
			//std::cerr<<"                z1 bracket"<<std::endl;
			brackets.push_back(MinBracket(z1,ztest, z1 + (z2-z1)/double(nsample-1)));
		}
		else {
			//std::cerr<<"                z1 min"<<std::endl;
			// Extract the distances of approach here
			Alldist.push_back(dd[0]);
			// Position of minimal distance of approach
			Allz.push_back(z1);
			Alliter.push_back(0);
		}
	}

	if(dd[nsample-1] <= dd[nsample-2]) {
		double ztest = z2 - std::min(z2-z1, tolerance_abs);
		double ftest = distance(ztest);
		if( (ftest <= dd[nsample-1]) && (ftest <= dd[nsample-2])) {
			//std::cerr<<"                z2 bracket"<<std::endl;
			brackets.push_back(MinBracket(z1 + (z2-z1)*(nsample-2)/double(nsample-1), ztest, z2));
		}
		else {
			//std::cerr<<"                z2 min"<<std::endl;
			// Extract the distances of approach here
			Alldist.push_back(dd[nsample-1]);
			// Position of minimal distance of approach
			Allz.push_back(z2);
			Alliter.push_back(0);
		}
	}
	// cout<<" In Get_CDA_FromCalculation 4   brackets size ="<<brackets.size()<<endl;

	//------------------------------------------------------------------//
	// Loop over the brackets and for each one of them, find a minimum. //
	//------------------------------------------------------------------//

	for(unsigned i=0; i<brackets.size(); i++) {
		//std::cerr<<"Bracket{"<<brackets[i].a<<", "<<brackets[i].m<<", "<<brackets[i].b<<std::endl;
		const gsl_min_fminimizer_type *T = gsl_min_fminimizer_brent;
		gsl_min_fminimizer *s = gsl_min_fminimizer_alloc (T);

		gsl_function F;

		F.function = &TrackDistanceUV::fwrapper;
		F.params = &distance;

		gsl_min_fminimizer_set(s, &F, brackets[i].m, brackets[i].a, brackets[i].b);

		int iter=0;
		while(1)
		{
			iter++;
			int status;
			if(GSL_SUCCESS != (status = gsl_min_fminimizer_iterate (s)) )
			{
				gsl_min_fminimizer_free(s);
				Log->warn("PairMatchingCut: Get_CDA_FromCalculation(): failure from gsl_min_fminimizer_iterate(): status==%i", status);
				return false;
			}

			M = gsl_min_fminimizer_x_minimum (s);
			A = gsl_min_fminimizer_x_lower (s);
			B = gsl_min_fminimizer_x_upper (s);

			status = gsl_min_test_interval (A, B, tolerance_abs, tolerance_rel);

			if (status == GSL_SUCCESS) { 
				//printf ("Converged:\n");
				//printf ("%5d [%.7f, %.7f] %.7f %.7f\n", iter, a, b, m, b - a);

				// Extract the distances of approach here
				Alldist.push_back(gsl_min_fminimizer_f_minimum(s));
				// Position of minimal distance of approach
				Allz.push_back(M);
				Alliter.push_back(iter);
				break;
			}
			else if(status == GSL_CONTINUE) {
				if(iter>max_iter) {
					gsl_min_fminimizer_free(s);
					Log->warn("PairMatchingCut: Get_CDA_FromCalculation(): exceeded max_iter=");
					return false;
				}
			}
			else {
				gsl_min_fminimizer_free(s);
				Log->warn("PairMatchingCut: Get_CDA_FromCalculation(); failure from gsl_min_test_interval(): status=");
				return false;
			}	

		} // while(1) minimize

		gsl_min_fminimizer_free(s);

	} // for(brackets)

	// cout<<" In Get_CDA_FromCalculation 5    Alldist size ="<<Alldist.size()<<endl;
	//------------------------------------------------------------------//
	// The CDA is the minimum of all the distances of approach          //
	//------------------------------------------------------------------//
	int MinIndex = -1;
	cda = 500.;
	z	= 1000.;
	iterations = 0;
	// cout<<" In Get_CDA_FromCalculation 6   cda = "<<cda<<endl;
	for (int i = 0; i < Alldist.size(); i++)
	{
		// cout<<" In Get_CDA_FromCalculation 6.1   AllDist[0] = "<<Alldist[0]<<endl;
		if ( Alldist[i] < cda )
		{
			MinIndex = i;
			cda	= Alldist[i];
			// cout<<" In Get_CDA_FromCalculation 6.2   cda = "<<cda<<endl;
		}
	}
	if (MinIndex == -1)
	{
		Log->warn("PairMatchingCut: Get_CDA_FromCalculation(); The cda could not be found.");
		cda	= 500;	// Safer with a large number since we cut on the small cda.
		return false;
	}
	else
	{
		z = Allz[MinIndex];
		iterations = Alliter[MinIndex];
	}
	// cout<<" In Get_CDA_FromCalculation 7   cda = "<<cda<<endl;
	
	return true;
}
void shearlayerkcrit_driver(char *input_file_name)
{
    PARAMS_STRUCT *params;
    GRID_STRUCT *grid;
    ROTATION_STRUCT *rotation;
    COMPRESSED_MATRIX *matrix;
    ARPACK_CONTROL *arpack_params;
    RESULTS_STRUCT *results;
    OUTPUT_CONTROL *output_control;

    double shear_width, shear_radius, E;

    //Parameters needed for the root-finding routine
    int status;
    int iter=0, max_iter=50;
    const gsl_root_fsolver_type *Troot;
    const gsl_min_fminimizer_type *Tmin;
    gsl_root_fsolver *sroot;
    gsl_min_fminimizer *smin;
    double k_low, k_high, k_guess;
    double k_min = NAN;
    double k_max = NAN;
    double k_peak = NAN;
    double gr_peak;
    double errabs, errrel;
    double width_prefactor;
    gsl_function F;
    struct FUNCTION_PARAMS function_params;

    //Get the physical parameters for the computation
    params = malloc(sizeof(PARAMS_STRUCT));
    probgen(input_file_name, params);

    //Set up the grid, based on the physical parameters
    grid = gridgen(params);

    //Set up the rotation profile of a shear layer. Derive the width
    //from the Ekman number, E=\nu/\Omega r^2, width = rE^(1/4)
    //Use r = (r2-r1) and Omega = (Omega1-Omega2)/2.

    shear_radius = get_dparam("shear_radius", input_file_name);
    /*
    width_prefactor = get_dparam("width_prefactor", input_file_name);
    E = params->nu/(0.5*fabs(params->omega1 - params->omega2) *
    	  pow((params->r2-params->r1),2));
    shear_width = width_prefactor*(params->r2-params->r1)*pow(E, 0.25);
    printf("Using shear layer width %g cm\n", shear_width);
    */
    shear_width = get_dparam("shear_width", input_file_name);
    rotation = shearlayer(params, grid, shear_width, shear_radius);

    //Set up the matrix structure for the computations.
    matrix = create_matrix(5*grid->numcells);

    //Setup the ARPACK parameters
    arpack_params = setup_arpack(input_file_name);

    //Setup the output control structure
    output_control = malloc(sizeof(OUTPUT_CONTROL));

    //Pull the error params from the input file to decide when
    //we have converged
    errabs = get_dparam("errabs", input_file_name);
    errrel = get_dparam("errrel", input_file_name);

    //Put pointers to all of our control structures in function_params
    function_params.params = params;
    function_params.grid = grid;
    function_params.rotation = rotation;
    function_params.matrix = matrix;
    function_params.arpack_params = arpack_params;

    //Assign the evaluation function and params structure to
    //the gsl_function
    F.function = &mindampingrate;
    F.params = &function_params;

    gsl_set_error_handler(&err_handler);

    /* Now we find the peak of the growth rate, by minimizing the
       damping rate. We set what we hope are reasonable numbers
       for the bounds and initial guess.
    */

    k_low = 0.01;
    k_high = 1000;
    k_guess = params->k;
    Tmin = gsl_min_fminimizer_brent;
    smin = gsl_min_fminimizer_alloc(Tmin);
    status = gsl_min_fminimizer_set(smin, &F, k_guess, k_low, k_high);
    //Make sure that we didn't thrown an error on initialization
    if (status == GSL_SUCCESS) {
        //Now iterate!
        iter = 0;
        do
        {
            iter++;
            status = gsl_min_fminimizer_iterate(smin);
            //Make sure that we didn't thrown an error in the iteration routine
            if (status != GSL_SUCCESS) {
                fprintf(stderr, "Aborted attempt to find k_peak.\n");
                break;
            }

            params->k = gsl_min_fminimizer_x_minimum(smin);
            k_low = gsl_min_fminimizer_x_lower(smin);
            k_high = gsl_min_fminimizer_x_upper(smin);
            status = gsl_min_test_interval(k_low, k_high, errabs, errrel);

            if(status == GSL_SUCCESS && params->VERBOSE) {
                printf("Converged with k_peak=%g\n", params->k);
            }
        }
        while (status == GSL_CONTINUE && iter < max_iter);
        //Save the peak growth rate for printing later, then free the solver
        gr_peak = -gsl_min_fminimizer_f_minimum(smin);
    } else {
        fprintf(stderr, "Aborted attempt to find k_peak.\n");
    }
    gsl_min_fminimizer_free(smin);

    //Check to make sure we converged. If not, don't save the results.
    if (status == GSL_SUCCESS) {
        k_peak = params->k;

        //Make sure everything is set up correctly for normal run
        params->kva = params->k*params->va;
        free(grid->r);
        free(grid->x);
        free(grid->r2inv);
        free(grid);
        grid = gridgen(params);

        //Now do a normal run with the chosen k
        arpack_params->sigma = find_sigma(matrix, params, grid, rotation,
                                          arpack_params);
        results = eigensolve(matrix, params, grid, rotation, arpack_params);

        //Setup the structures needed to output the data files, and write them.
        get_sparam("basefilename", input_file_name, output_control->basefilename);
        strcat(output_control->basefilename, "_kpeak");
        wnetcdf(params, grid, rotation, output_control, arpack_params, results);

        free(results->lambda);
        free(results->z);
        free(results->residual);
        free(results);
    }


    /* Now do a root finding search for k_min. */

    /*
    //Set up the root solver.
    Troot = gsl_root_fsolver_brent;
    sroot = gsl_root_fsolver_alloc(Troot);

    //Set the initial bounds for the search. We're searching for k_min,
    //so search from 0 up to k_peak.
    k_low = 0;
    k_high = k_peak;
    status = gsl_root_fsolver_set(sroot, &F, k_low, k_high);
    //Make sure that we didn't thrown an error on initialization
    if (status == GSL_SUCCESS) {
      //Now iterate!
      iter = 0;
      do
        {
    iter++;
    status = gsl_root_fsolver_iterate(sroot);
    //Make sure that we didn't thrown an error in the iteration routine
    if (status != GSL_SUCCESS) {
      fprintf(stderr, "Aborted attempt to find k_min.\n");
      break;
    }

    params->k = gsl_root_fsolver_root(sroot);
    k_low = gsl_root_fsolver_x_lower(sroot);
    k_high = gsl_root_fsolver_x_upper(sroot);
    status = gsl_root_test_interval(k_low, k_high, errabs, errrel);

    if(status == GSL_SUCCESS && params->VERBOSE) {
      printf("Converged with k_min=%g\n", params->k);
    }
        }
      while (status == GSL_CONTINUE && iter < max_iter);
    } else {
      fprintf(stderr, "Aborted attempt to find k_min.\n");
    }
    gsl_root_fsolver_free (sroot);

    //Check to make sure we converged. If not, don't save the results.
    if (status == GSL_SUCCESS) {
      k_min = params->k;

      //Make sure everything is set up correctly for the normal run
      params->kva = params->k*params->va;
      free(grid->r);
      free(grid->x);
      free(grid->r2inv);
      free(grid);
      grid = gridgen(params);

      //Now do a normal run with the chosen k
      arpack_params->sigma = find_sigma(matrix, params, grid, rotation,
    			      arpack_params);
      results = eigensolve(matrix, params, grid, rotation, arpack_params);

      //Set the new file name, and write the output
      get_sparam("basefilename", input_file_name, output_control->basefilename);
      strcat(output_control->basefilename, "_kmin");
      wnetcdf(params, grid, rotation, output_control, arpack_params, results);

      free(results->lambda);
      free(results->z);
      free(results->residual);
      free(results);
    }
    */

    /* Now move on to solving for k_max. */
    Troot = gsl_root_fsolver_brent;
    sroot = gsl_root_fsolver_alloc(Troot);

    //Set the initial bounds for the search. We're searching for k_max,
    //so search from k_peak to a large number
    k_low = k_peak;
    k_high = 10000;
    status = gsl_root_fsolver_set(sroot, &F, k_low, k_high);
    //Make sure that we didn't thrown an error on initialization
    if (status == GSL_SUCCESS) {
        //Now iterate!
        iter = 0;
        do
        {
            iter++;
            status = gsl_root_fsolver_iterate(sroot);
            //Make sure that we didn't thrown an error in the iteration routine
            if (status != GSL_SUCCESS) {
                fprintf(stderr, "Aborted attempt to find k_max.\n");
                break;
            }

            params->k = gsl_root_fsolver_root(sroot);
            k_low = gsl_root_fsolver_x_lower(sroot);
            k_high = gsl_root_fsolver_x_upper(sroot);
            status = gsl_root_test_interval(k_low, k_high, errabs, errrel);

            if(status == GSL_SUCCESS && params->VERBOSE) {
                printf("Converged with k_max=%g\n", params->k);
            }
        }
        while (status == GSL_CONTINUE && iter < max_iter);
    } else {
        fprintf(stderr, "Aborted attempt to find k_max.\n");
    }
    gsl_root_fsolver_free (sroot);

    //Check to make sure we converged. If not, don't save the results.
    if (status == GSL_SUCCESS) {
        k_max = params->k;

        //Make sure everything is set up correctly for the normal run
        params->kva = params->k*params->va;
        free(grid->r);
        free(grid->x);
        free(grid->r2inv);
        free(grid);
        grid = gridgen(params);

        //Now do a normal run with the chosen k
        arpack_params->sigma = find_sigma(matrix, params, grid, rotation,
                                          arpack_params);
        results = eigensolve(matrix, params, grid, rotation, arpack_params);

        //Set the new file name, and write the output
        get_sparam("basefilename", input_file_name, output_control->basefilename);
        strcat(output_control->basefilename, "_kmax");
        wnetcdf(params, grid, rotation, output_control, arpack_params, results);

        free(results->lambda);
        free(results->z);
        free(results->residual);
        free(results);
    }

    printf("Found k_min = %g, k_peak = %g, k_max = %g\n", k_min, k_peak, k_max);
    printf("Peak growth rate: %g\n", gr_peak);

    free(matrix->A);
    free(matrix->B);
    free(matrix->Bb);
    free(matrix);
    free(params);
    free(grid->r);
    free(grid->x);
    free(grid->r2inv);
    free(grid);
    free(rotation->omega);
    free(rotation);
    free(output_control);

    return;
}
Example #17
0
void find_periodic_solution_one_dim(double vz_init, double th_init, struct_all_ode *sao) {
    struct_state_ode sx;
    int nb_jump, ij, iq;
    double t_max = 2;
    double vth_init, vth_min, vth_max, vth_tolabs, deltaf;

    int status;
    int iter = 0, max_iter = 100;
    const gsl_min_fminimizer_type *T;
    gsl_min_fminimizer *s;
    gsl_function F;
    // init the function to minimize
    vth_init = 0 * M_PI / 180;
    vth_min = -200 * M_PI / 180;
    vth_max = 200 * M_PI / 180;
    vth_tolabs = 0.001 * M_PI / 180;
    // find vth init such as to obtain a periodic solution
    if (sao == NULL) {
        sao = new_all_ode(sao, NULL);
    }
    sao->eps_rel = 1e-6;
    sao->eps_abs = 1e-6;
    sao->h = 1e-6;
    sao->hmax = 1;
    sao->mode = MODE_FLY;
    sao->x[INDX_PH] = 0 * M_PI / 180;
    sao->x[INDX_TH] = th_init;
    sao->x[INDX_X] = 0;
    sao->x[INDX_Z] = 0;
    sao->x[INDX_R] = sao->r0;
    sao->x[INDX_VPH] = 0 * M_PI / 180;
    sao->x[INDX_VTH] = 0;
    sao->x[INDX_VR] = 0; // vr for flying model
    sao->x[INDX_ROOT] = 0;
    // trajectoire periodique => pas de perte d'energie a l'aterrisage,
    // vx =-vz.tan(theta) est imposee par vz et theta
    // le seul parametre permettant de fixer la periodicite est vtheta
    sao->x[INDX_TH] = th_init;
    sao->x[INDX_VX] = -vz_init * sin(th_init) / cos(th_init);
    sao->x[INDX_VZ] = vz_init;
    sx.NX = sao->NX;
    memcpy(sx.x, sao->x, sx.NX * sizeof (double));
    sx.mode = sao->mode;
    sx.time_s = 0;
    sx.NX = sao->NX;
    sao->initial_state = sx;
    F.function = &get_delta_x_on_period_one_dim;
    F.params = (void *) sao;

    T = gsl_min_fminimizer_brent;
    s = gsl_min_fminimizer_alloc(T);
    gsl_min_fminimizer_set(s, &F, vth_init, vth_min, vth_max);

    do {
        iter++;
        status = gsl_min_fminimizer_iterate(s);

        vth_init = gsl_min_fminimizer_x_minimum(s);
        vth_min = gsl_min_fminimizer_x_lower(s);
        vth_max = gsl_min_fminimizer_x_upper(s);

        status
                = gsl_min_test_interval(vth_min, vth_max, vth_tolabs, 0.0);

        if (status == GSL_SUCCESS) {
            deltaf = get_delta_x_on_period_one_dim(vth_init, sao);

            printf("Converged, delta f= %.5e:\n", deltaf);
            printf("%5d [%.7f, %.7f] "
                    "%.7f  delta=%.7f\n",
                    iter, vth_min, vth_max,
                    vth_init, vth_max - vth_min);
        }
    } while (status == GSL_CONTINUE && iter < max_iter);
    gsl_min_fminimizer_free(s);
    memcpy(sao->x, sao->initial_state.x, sao->NX * sizeof (double));
    sao->time_second = 0;
    sao->mode = sx.mode;
    /*
    sao->print_values_fly_to_sol = 1;
    printf("--- initial state at time t= %e ----------\n", sao->final_state.time_s);
    print_state(sao->initial_state.x, NULL);
    print_energies(sao, sao->initial_state.x);

    printf("--- final state at time t= %e ----------\n", sao->final_state.time_s);
    print_state(sao->final_state.x, NULL);
    print_energies(sao, sao->final_state.x);*/

}
REAL8 XLALMinimizeEThincaParameterOverTravelTime( REAL8 travelTime,
                                                  EThincaMinimizer *minimizer,
                                                  INT4   exttrig
                                                )
{
  REAL8 ethinca;


  /* If colocated detectors or known sky position, just return the e-thinca parameter */
  if (travelTime == 0.0 || exttrig )
  {
    ethinca = minimizeEThincaParameterOverTimeDiff( travelTime, minimizer );
    if ( XLAL_IS_REAL8_FAIL_NAN(ethinca) )
    {
      XLAL_ERROR_REAL8( XLAL_EFUNC );
    }
    return ethinca;
  }
  else
  {
    gsl_function        F;
    INT4                min_status;
    INT4                iter = 0;
    const INT4          max_iter = 100;
    REAL8               epsilon = 1.0 / 16384.0;
    REAL8               m = 0.0;
    REAL8               a = - travelTime, b = travelTime; /* Upper and lower bounds */
    REAL8               minEThinca, maxEThinca;
    REAL8               midEThinca;
    gsl_min_fminimizer  *s = gsl_min_fminimizer_alloc( minimizer->workSpace->T );

    if ( !s )
    {
      XLAL_ERROR_REAL8( XLAL_ENOMEM );
    }


    F.function = &minimizeEThincaParameterOverTimeDiff;
    F.params   = minimizer;

    /* Calculate e-thinca parameter at start, end and mid points */
    minEThinca = minimizeEThincaParameterOverTimeDiff( a, minimizer );
    maxEThinca = minimizeEThincaParameterOverTimeDiff( b, minimizer );
    midEThinca = minimizeEThincaParameterOverTimeDiff( m, minimizer );
    if ( XLAL_IS_REAL8_FAIL_NAN(minEThinca) || XLAL_IS_REAL8_FAIL_NAN(maxEThinca)
         || XLAL_IS_REAL8_FAIL_NAN(midEThinca) )
    {
      gsl_min_fminimizer_free( s );
      XLAL_ERROR_REAL8( XLAL_EFUNC );
    }

    /* Check we have contained a minimum. Otherwise take appropriate action */
    if ( midEThinca >= minEThinca || midEThinca >= maxEThinca )
    {
      REAL8 testEThinca; /* To contain the lowest end-point */
      if ( minEThinca < maxEThinca )
      {
        testEThinca = minEThinca;
        m           = a + 2.0 * epsilon;
      }
      else
      {
        testEThinca = maxEThinca;
        m = b - 2.0 * epsilon;
      }
      midEThinca = minimizeEThincaParameterOverTimeDiff( m, minimizer );
      if ( XLAL_IS_REAL8_FAIL_NAN(midEThinca) )
      {
        gsl_min_fminimizer_free( s );
        XLAL_ERROR_REAL8( XLAL_EFUNC );
      }

      /* If we still don't have the minimum return the lowest end-point */
      if ( midEThinca >= testEThinca )
      {
        gsl_min_fminimizer_free( s );
        return testEThinca;
      }
    }

    /* Set up the GSL minimizer */
    XLAL_CALLGSL( min_status = gsl_min_fminimizer_set_with_values(s, &F,
                       m, midEThinca, a, minEThinca, b, maxEThinca) );
    if ( min_status != GSL_SUCCESS )
    {
      gsl_min_fminimizer_free( s );
      XLAL_ERROR_REAL8( XLAL_EFUNC );
    }

    /* Loop to perform the minimization */
    do
    {
        iter++;
        XLAL_CALLGSL( min_status = gsl_min_fminimizer_iterate (s) );
        if (min_status != GSL_SUCCESS )
        {
            gsl_min_fminimizer_free( s );
            XLAL_ERROR_REAL8( XLAL_EFUNC );
        }

        m = gsl_min_fminimizer_x_minimum (s);
        a = gsl_min_fminimizer_x_lower (s);
        b = gsl_min_fminimizer_x_upper (s);

        XLAL_CALLGSL( min_status = gsl_min_test_interval (a, b, epsilon, 0.0) );
        if (min_status != GSL_CONTINUE && min_status != GSL_SUCCESS )
        {
          gsl_min_fminimizer_free( s );
          XLAL_ERROR_REAL8( XLAL_EFUNC );
        }
    }
    while ( min_status == GSL_CONTINUE && iter < max_iter );
    /* End of minimization routine */

    /* Throw an error if max iterations would have been exceeded */
    if ( iter == max_iter && min_status == GSL_CONTINUE )
    {
      gsl_min_fminimizer_free( s );
      XLAL_ERROR_REAL8( XLAL_EMAXITER );
    }

    /* Get the minimum e-thinca param, and free memory for minimizer */
    ethinca = gsl_min_fminimizer_f_minimum( s );
    gsl_min_fminimizer_free( s );
    XLALPrintInfo( "%s: Number of iterations = %d\n", __func__, iter);
  }

  /* Return the required e-thinca value */
  return ethinca;
}
Example #19
0
// Find correction
double lodo_correct(lodo_t *self)
{
  int i, ni, erc;
  vector2_t q;
  double r;
  double interval;
  double offset, err;
  double best_offset, best_err, best_outliers;
  gsl_function func;

  // Pre-compute some values used in the test function
  for (i = 0; i < self->num_ranges; i++)
  {
    // Get range relative to laser
    r = self->scan.ranges[i];
    if (r > self->range_max)
    {
      self->scan_points[i].ni = -1;
      continue;
    }

    // Compute cartesian points relative to robot
    q.x = r * cos(i * self->range_step + self->range_start);
    q.y = r * sin(i * self->range_step + self->range_start);    //printf("ni=%d\n",ni);

    q = pose2_add_pos(q, self->laser_pose);

    // Compute range relative to robot
    r = vector2_mag(q);
    ni = LODO_LTOR(r);
	//printf ("ni = %d\tself->pef_num_ranges = %d\n", ni, self->pef_num_ranges);
    if (ni < 0 || ni > self->pef_num_ranges)
      ni = -1;
    self->scan_points[i].ni = ni;
  }

  best_offset = 0.0;
  best_err = DBL_MAX;

  // Initialize the minimizer
  func.function = (double (*) (double, void*)) lodo_correct_func;
  func.params = self;
  erc = gsl_min_fminimizer_set(self->mini, &func,
                               0.0, -self->fit_interval, +self->fit_interval);

  // If the minimizer failes, revert to exhaustive search
  if (erc != GSL_SUCCESS)
  {
    //printf("brute force\n\n");
    best_err = DBL_MAX;
    for (i = -100; i <= 100; i++)
    {
      offset = i * self->fit_interval / 100.0;
      err = lodo_test_offset(self, offset, NULL);
      //printf("%d %f %f\n", i, offset, err);
      if (err < best_err)
      {
        best_err = err;
        best_offset = offset;
      }
    }
    //printf("\n\n");
  }
  else
  {
    for (i = 0; i < 10; i++) // HACK
    {
      erc = gsl_min_fminimizer_iterate(self->mini);
      if (erc == GSL_EBADFUNC)
        assert(0);
      if (erc == GSL_FAILURE)
        break;

      // Test for convergence
      interval = gsl_min_fminimizer_x_upper(self->mini);
      interval -= gsl_min_fminimizer_x_lower(self->mini);
      if (interval < 0.01 * M_PI / 180) // HACK
        break;
    }

    best_offset = gsl_min_fminimizer_x_minimum(self->mini);
    best_err = gsl_min_fminimizer_f_minimum(self->mini);
  }

  // Re-do the test to get the outlier count
  lodo_test_offset(self, best_offset, &best_outliers);

  // Check it his is a good fit.
  if (best_err > self->fit_err_thresh)
  {
    self->fit_valid = 0;
    self->fit_add = 0;
    self->fit_correct = 0;
    return 0;
  }

  self->fit_valid = 1;

  // See if should add this scan to the map.  The logic is this: if we
  // have a good fit, but there are lots of points that are "outliers"
  // (i.e., large-ish error value), then these points should be added
  // to the map.  Works for both turning in place and "bursting"
  // through doorways.
  if (fabs(best_offset) < 5 * M_PI / 180) // HACK
  {
    if (best_outliers > self->fit_outlier_frac)
      self->fit_add = 1;
    else
      self->fit_add = 0;
  }

  // Correct the scan pose
  self->fit_correct = best_offset;
  self->scan.cpose.rot = pose2_add_rot(self->fit_correct, self->scan.cpose);

  return 0;
}
/*
-------------------------------------------------------------------------
 * This function minimises fContact defined above using the
 * Brent method. It returns the minima with a negative sign (which then
 * becomes the maxima of the actual contact function. This can be compared
 * to 1 to check if two ellipsoids indeed overlap.
 * ------------------------------------------------------------------------*/
REAL8 XLALCheckOverlapOfEllipsoids (
        const gsl_vector   *ra,
        const gsl_vector   *rb,
        fContactWorkSpace  *workSpace )
{
    gsl_function        F;
    INT4                min_status;
    INT4                iter = 0, max_iter = 100;
    REAL8               m = 0.6180339887;
    REAL8               a = 0.0L, b = 1.0L;
    gsl_min_fminimizer  *s = workSpace->s;

    /* Sanity check on input arguments */
    if ( !ra || !rb || !workSpace )
      XLAL_ERROR_REAL8( XLAL_EFAULT );

    if ( ra->size != rb->size || ra->size != workSpace->n )
      XLAL_ERROR_REAL8( XLAL_EBADLEN);


    /* Set r_AB to be rb - ra */
    XLAL_CALLGSL( gsl_vector_memcpy( workSpace->r_AB, rb) );
    XLAL_CALLGSL( gsl_vector_sub (workSpace->r_AB, ra) );

    if ( gsl_vector_isnull( workSpace->r_AB ))
    {
      XLALPrintWarning("Position vectors ra and rb are identical.\n");
      return 0;
    }

    F.function = &fContact;
    F.params   = workSpace;

    XLAL_CALLGSL( min_status = gsl_min_fminimizer_set (s, &F, m, a, b) );
    if ( min_status != GSL_SUCCESS )
      XLAL_ERROR_REAL8( XLAL_EFUNC );

    do
    {
        iter++;
        XLAL_CALLGSL( min_status = gsl_min_fminimizer_iterate (s) );
        if (min_status != GSL_SUCCESS )
        {
          if (min_status == GSL_EBADFUNC)
            XLAL_ERROR_REAL8( XLAL_EFUNC | XLAL_EFPINVAL );
          else if (min_status == GSL_EZERODIV)
            XLAL_ERROR_REAL8( XLAL_EFUNC | XLAL_EFPDIV0 );
          else
            XLAL_ERROR_REAL8( XLAL_EFUNC );
        }

        m = gsl_min_fminimizer_x_minimum (s);
        a = gsl_min_fminimizer_x_lower (s);
        b = gsl_min_fminimizer_x_upper (s);

        XLAL_CALLGSL( min_status = gsl_min_test_interval (a, b, workSpace->convParam, 0.0) );
        if (min_status != GSL_CONTINUE && min_status != GSL_SUCCESS )
          XLAL_ERROR_REAL8( XLAL_EFUNC );
    }
    while (min_status == GSL_CONTINUE && iter < max_iter );
    /* End of minimization routine */

    /* Throw an error if max iterations would have been exceeded */
    if ( iter == max_iter && min_status == GSL_CONTINUE )
    {
      XLAL_ERROR_REAL8( XLAL_EMAXITER );
    }

    return ( -(s->f_minimum) );
}