LM_fit.h

Go to the documentation of this file.

/***************************************************************************
                          LM_fit.h  -  Levenbaerg-Marquardt non-linear data fit
                             -------------------
    begin                : Wed Dec 15 1999
    copyright            : (C) 1999 by AMB
    email                : bilgic@hft.e-technik.uni-dortmund.de
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 ***************************************************************************/


#ifndef TBCI_LM_FIT_H
#define TBCI_LM_FIT_H

#include "tbci/basics.h"
#include "tbci/vector.h"
#include "tbci/matrix.h"
#include "tbci/constants.h"
#include "tbci/solver/gauss_jordan.h"
//#include "list.h"

#ifdef HAVE_SIGNALS
# include <signal.h>
#endif
/*
#include <cstdlib>
#include <cmath>
#include <iostream>
#include <iomanip>
*/


NAMESPACE_TBCI

#ifdef PRAGMA_I
# pragma interface "LM_fit.h"
#endif

#define DELTAX 1e-30


#define DELTA(i,j) ((i==j)?1:0)
        
/*
inline int delta (int i, int j)
{
   return ((i==j) ? 1 : 0);
}
 */
        
INST(template <typename T> class Vector friend \
     int Basis_Trafo (long int, long int, Vector<T>&);)

template <typename T>
int Basis_Trafo (long int value, long int basis, Vector<T>& index)
/* transforms the integer 'value' into the basis 'basis';
   index contain the resulting digits with lowest significant first */
{
   int n = index.size();
   int i = 0;

   index = (T)0;
   if (value == 0) return 1; //special zero treatment
   if (n < (int)CSTD__ ceil (MATH__ sqrt(2*MATH__ log((double)value) / MATH__ log((double)basis))) || basis <= 1) return 0;
   while (value > 0)
   {
      index(i) = value%basis;
      value /= basis;
      i++;
   }
   return 1;
}


#define CON2 2.0
#define CON SQRT2
#define BIG 1.0e30
#define NTAB 10
#define SAFE 2.0

INST(template <typename T> class Vector friend \
     T Partial_Del (T (*func)(const Vector<T>&, const Vector<T>&), \
                    const Vector<T>&, const Vector<T>&, \
                    int, T, T&);)
template <typename T>
T Partial_Del (T (*func)(const Vector<T>&, const Vector<T>&),
               const Vector<T>& x, const Vector<T>& p,
               int mu, T h, T& err)
  // calculate partial derivative of func with respect to mu
  // Acc. to Num. recipes, ch. 5.7
{
  //static T maxarg1 ALIGN(MIN_ALIGN), maxarg2 ALIGN(MIN_ALIGN);
  T errt ALIGN(MIN_ALIGN), fac, hh, ans = (T)0;
  int xlen = x.size();
  Matrix<T> a(NTAB,NTAB);
  Vector<T> buf_plus(xlen);
  Vector<T> buf_minus(xlen);
#ifdef FOURTHORDER
  Vector<T> buf_plus2(xlen);
  Vector<T> buf_minus2(xlen);
#endif
  if (mu >= xlen)
        STD__ cerr << "\n error: partial_del: derivate variable index out of range \n";
  hh = h ;
  buf_plus = x; buf_minus = x; buf_plus(mu) += hh; buf_minus(mu) -= hh;
#ifdef FOURTHORDER
  buf_plus2 = x; buf_minus2 = x; buf_plus2(mu) += hh*2; buf_minus2 -= hh*2;
  a(0,0) = (-(*func)(buf_plus2, p) + 8.0*(*func)(buf_plus, p)
                - 8.0*(*func)(buf_minus, p) + (*func)(buf_minus2, p)) / (hh*12.0);
#else
  a(0,0) = ((*func)(buf_plus, p) - (*func)(buf_minus, p)) / (2.0*hh);
#endif
  err = BIG;
  for (int i = 1; i < NTAB; i++) {
        hh /= CON;
        buf_plus = x; buf_minus = x; buf_plus(mu) += hh; buf_minus(mu) -= hh;
#ifdef FOURTHORDER
        buf_plus2 = x; buf_minus2 = x; buf_plus2(mu) += hh*2; buf_minus2 -= hh*2;
        a(0,i) = (-(*func)(buf_plus2, p) + 8.0*(*func)(buf_plus, p)
                  - 8.0*(*func)(buf_minus, p) + (*func)(buf_minus2, p)) / (hh*12.0);
#else
        a(0,i) = ((*func)(buf_plus, p) - (*func)(buf_minus, p)) / (2.0*hh);
#endif
        fac = CON2;
        for (REGISTER int j = 1; j <= i; j++) {
                a(j,i) = (a(j-1,i)*fac - a(j-1,i-1))/(fac-1.0);
                fac *= CON2;
                errt = MAX (MATH__ fabs(a(j,i)-a(j-1,i)), MATH__ fabs(a(j,i)-a(j-1,i-1)));
                if (errt <= err) {
                        err = errt;
                        ans = a(j,i);
                }
        }
        if (MATH__ fabs (a(i,i)-a(i-1,i-1)) >= MATH__ fabs (SAFE*err)) break;
  }
  return ans;
}


INST(template <typename T> class Vector friend \
     T Chisq (Vector<T>&, Vector<T>&, Vector<T>&);)

template <typename T>
T Chisq (Vector<T>& data, Vector<T>& func, Vector<T>& sig)
   // calculates the chi-squared sum

{
   T chi ALIGN(MIN_ALIGN) = 0;
   int len=data.size();

   for(REGISTER int i=0;i<len;i++)  chi+=sqr((data(i)-func(i))/sig(i));
   return(chi);
}


INST(template <typename T> class Vector friend \
     T Chisquare_2D (T (*func)(const Vector<T>&, const Vector<T>&), \
                     Vector<T>&, Vector<T>&, Vector<T>&, Vector<T>&, Vector<T>&);)

template <typename T>
T Chisquare_2D (T (*func)(const Vector<T>&, const Vector<T>&),
            Vector<T>& param, Vector<T>& x, Vector<T>& y, Vector<T>& z, Vector<T>& sigma)
{
  Vector<T> arg(2);

  T chi ALIGN(MIN_ALIGN) = 0, wfvaldiff;
  int anzpkt = x.size();

  for (REGISTER int i = 0; i < anzpkt; i++) {
        arg(0) = x(i); arg(1) = y(i);
        wfvaldiff = (z(i) - (*func)(param, arg)) / sigma(i);
        chi += sqr (wfvaldiff);
  }
  return chi;
}


INST(template <typename T> class Vector friend \
     TVector<T> Grid_Min_2D(T (*func)(const Vector<T>&, const Vector<T>&), \
                            Vector<T>&, Vector<T>&, Vector<T>&, \
                            Vector<T>&, Vector<T>&, \
                            Vector<T>&, Vector<T>&, \
                            long int, int, char);)

template <typename T>
TVector<T> Grid_Min_2D(T (*func)(const Vector<T>&, const Vector<T>&),
                    Vector<T>& x, Vector<T>& y, Vector<T>& z,
                    Vector<T>& sig, Vector<T>& pmin,
                    Vector<T>& pmax, Vector<T>& lambda,
                    long int res, int max_it, char v)
{

  int plen=pmin.size();
  int xlen=x.size();
  long int len=0;
  Vector<T> p(pmin);
  TVector<T> min_pos(pmin);
  Vector<T> factor(plen);
  Vector<T> arg(2);
  Vector<T> fval(x.size());
  Vector<T> index(plen);
  Vector<T> b(0, plen);
  Vector<T> a(0, plen);
  T min ALIGN(MIN_ALIGN) = 0, chi=0 /*, err=0.0, fac=1e-8*/;

  len = (long int) MATH__ pow ((double)res, plen);
  for (int k=0;k<xlen;k++) {  arg(0)=x(k); arg(1)=y(k); fval(k)=(*func)(p, arg);  }
  min=Chisq(z, fval, sig);
  STD__ cout << "\n parameter space discretisation with " << len << " points in progress ... \n ";
  for (REGISTER int j = 0; j < plen; j++)
          factor(j) = (pmax(j) - pmin(j)) / ((T)res - (T)1);
  for (long int i=0; i<len; i++) {
          // linearize recursive for structure by basis trafo
          if ( !Basis_Trafo(i, res, index) )
                  STD__ cerr << "\n error: basis trafo failed\n";
          for (REGISTER int j=0; j<plen; j++)
                  p(j) = factor(j) * index(j) + pmin(j); //p-space discr.
#if 0
          //do max_it steepest gradient steps
          for (int l=0; l<max_it; l++) {
                  // setup Vector
                  err=0.0;
                  for (int k=0; k<plen; k++) {
                          for (int j=0; j<xlen; j++) {
                                  arg(0)=x(j); arg(1)=y(j);
                                  b(k)+=(z(j)-(*func)(p, arg))/(sig(j)*sig(j))*
                                          Partial_Del(func, p, arg, k, p(k)*fac, err);
                          }
                  }
                  // setup diagonal Matrix for steepest gradient const
                  err=0.0;
                  for (int k=0; k<plen; k++) {
                    for (REGISTER int j=0; j<xlen; j++) {
                            arg(0)=x(j); arg(1)=y(j);
                            a(k)+=sqr(Partial_Del(func, p, arg, k, p(k)*fac, err))/
                                    (sig(j)*sig(j));
                    }
                  }
                  for (int k=0; k<plen; k++)
                          p(k)+=b(k)*lambda(k)/(a(k)+DELTAX);
          }
#endif          
          // calc func values with new parameter
          for (int l=0; l<xlen; l++) {  arg(0)=x(l); arg(1)=y(l); fval(l)=(*func)(p, arg); }
          // calc chi2 sqared
          chi=Chisq(z, fval, sig);
          if ( chi<min ) {  min=chi; min_pos=p;  } //choose new min
          if ( i%1000==0 ) STD__ cerr << i << "\t\t\t\r";
#ifdef SIGHANDLE
          if (sigint) { sigint--; return 0; }
#endif
  }
  STD__ cout << " \n done! \n\n" << STD__ endl;
  return min_pos;
}


INST(template <typename T> class Vector friend \
     T LM_fit_2D(Vector<T>&, Vector<T>&, Vector<T>&, Vector<T>&, Vector<T>&, \
                 Vector<T>&, Vector<T>&,T (*func)(const Vector<T>&, const Vector<T>&), \
                 Vector<T>, Vector<T>&, T, int, char);)

template <typename T>
T LM_fit_2D(Vector<T>& x, Vector<T>& y, Vector<T>& z, Vector<T>& sig, Vector<T>& param,
          Vector<T>& plow, Vector<T>& phigh,
          T (*func)(const Vector<T>&, const Vector<T>&),
          Vector<T> lambda, Vector<T>& lscale,
          T tol, int iter, char ver)
{
        T chi ALIGN(MIN_ALIGN), chi_new, chi_min, chi_dif, err=0.0, fac=1.0e-8;
        int len = x.size();
        int plen = param.size();
        Vector<T> param2(plen); // Vector<T> for new parameter set
        Vector<T> param_best(plen); // Vector<T> for best parameter set
        Vector<T> fval(len); // Vector for fit-func values
        Vector<T> arg(2); // Vector for arguments of fit-func (2dim)

                // comp chi-squared
        chi = Chisquare_2D<T> (func, param, x, y, z, sig);
        STD__ cout << " \n chi-squared with start parameters is: " << chi;
        chi_min = chi;
                // ini matrices
        Matrix<T> a(0.0, plen,plen);
        Matrix<T> b(0.0, plen,1);
        STD__ cout << "\n initializing matrices and starting loop... \n";
        int j=0; // iteration counter
        param_best=param;
        do {
                int k;
                // setup Matrix
                for (k=0; k<plen; k++)
                        for (int l=0; l<plen; l++) {
                        for (int i=0; i<len; i++) {
                                arg(0) = x(i); arg(1)=y(i);
                        a(k,l) += Partial_Del(func, param, arg, k, param(k)*fac, err)*
                                        Partial_Del(func, param, arg, l, param(l)*fac, err) /(sig(i)*sig(i));
                        }
                        //STD__ cerr << "\nDEBUG matrix:\n " << a << STD__ flush;
                        //STD__ cerr << "\nDEBUG param:\n " << param << STD__ flush;
                        //STD__ cerr << "\nDEBUG arg:\n " << arg << STD__ flush;
                        //STD__ cerr << "\nDEBUG delk:\n " << Partial_Del(func, param, arg, k, param(k)*fac, err) << STD__ flush;
                        //STD__ cerr << "\nDEBUG dell:\n " << Partial_Del(func, param, arg, l, param(l)*fac, err) << STD__ flush;
                          a(k,l) *= ((T)1+lambda(k)*(T)DELTA(k,l));
                        }
                        // setup Vector<T>
                for (k=0;k<plen;k++) {
                        for (int i=0;i<len;i++) {
                                arg(0)=x(i); arg(1)=y(i);
                                b(k,0) += (z(i)-(*func)(param, arg))/(sig(i)*sig(i))*
                        Partial_Del(func, param, arg, k, param(k)*fac, err);
                        }
        }
                // solve equ a.x=b
                char exceed = gaussj (a, b);
                // calc new parameter
        for (REGISTER int i=0;i<plen;i++) param2(i)=param(i)+b(i,0);
                // check parameter range
        exceed = 0;
        for (REGISTER int f=0; f<plen; f++) {
                        if ((param2(f) > phigh(f)) && (phigh(f)!=plow(f)) ) exceed = 1;
                        if ((param2(f) < plow(f))  && (phigh(f)!=plow(f)) ) exceed = 1;
        }
           // calc chi2 sqared
          chi_new = Chisquare_2D <T> (func, param2, x, y, z, sig);
          chi_dif = MATH__ fabs (chi_new - chi);
          if ((chi_new >= chi) || (exceed == 1)) lambda *= lscale(0);
                //increase lambda cause downhill is exhausted or limits are exceeded
        else {
        // decrease lambda and update trial solution
        lambda*=(T)1/lscale(1);
        param=param2;
                // update best est. param if new min is reached
        if (chi_new<chi_min) {
                        chi_min=chi_new;
                    param_best=param;
                  }
        }
        chi=chi_new; // update chi
        if (ver=='y' || ver=='Y')
                        STD__ cerr << "\n iteration " << j << "   " << param <<
                                "   " << param2 << "   " << chi_new;
#ifdef SIGHANDLE
        if (sigint) { sigint--; param = param_best; return 1e38; }
#endif
        j++;
        if (j%10 == 0)  STD__ cerr << ".";
                // end loop if tolerance is reached or number of iterations exceeded
        } while( (chi_dif >= tol) && (j < iter) );
        param = param_best; // return best parameter set found
        STD__ cout << "\n loop finished after " << j << " iterations with last delta chi: "
        << chi_dif << "\n and chi-squared: " << chi_new << STD__ endl;
        return (chi_new);
}


NAMESPACE_END

#endif /* TBCI_LM_FIT_H */