NewFitterGSL.h

/**************************************************************************
 * BASF2 (Belle Analysis Framework 2)                                     *
 * See https://github.com/tferber/OrcaKinfit, forked from                 *
 * https://github.com/iLCSoft/MarlinKinfit                                *
 *                                                                        *
 * Further information about the fit engine and the user interface        *
 * provided in MarlinKinfit can be found at                               *
 * https://www.desy.de/~blist/kinfit/doc/html/                            *
 * and in the LCNotes LC-TOOL-2009-001 and LC-TOOL-2009-004 available     *
 * from http://www-flc.desy.de/lcnotes/                                   *
 *                                                                        *
 * Adopted by: Torben Ferber (torben.ferber@desy.de) (TF)                 *
 *                                                                        *
 * This software is provided "as is" without any warranty.                *
 **************************************************************************/
 
#ifndef __NEWFITTERGSL_H
#define __NEWFITTERGSL_H
 
#include "analysis/OrcaKinFit/BaseFitter.h"
 
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_eigen.h>
 
namespace Belle2 {
  namespace OrcaKinFit {
 
// Class NewFitterGSL
 
    class NewFitterGSL : public BaseFitter {
    public:
      NewFitterGSL();
      virtual ~NewFitterGSL();
 
      virtual double fit() override;
 
      virtual int getError() const override;
 
      virtual double getProbability() const override;
      virtual double getChi2() const override;
      virtual int    getDoF() const override;
      virtual int  getIterations() const override;
 
      virtual int    getNcon() const;
 
      virtual int    getNsoft() const;
 
      virtual int    getNpar() const;
 
      virtual int    getNunm() const;
 
      virtual bool initialize() override;
 
      virtual void setDebug(int debuglevel);
 
      virtual int determineLambdas(gsl_vector* vecxnew,         
                                   const gsl_matrix* MatM,     
                                   const gsl_vector* vecx,     
                                   gsl_matrix* MatW,           
                                   gsl_vector* vecw,           
                                   double eps = 0              
                                  );
 
      virtual void calc2ndOrderCorr(gsl_vector* vecdxhat,        
                                    const gsl_vector* vecxnew,  
                                    const gsl_matrix* MatM,     
                                    gsl_matrix* MatW,     
                                    gsl_vector* vecw,     
                                    double eps = 0        
                                   );
 
      virtual gsl_matrix_view calcZ(int& rankA,               
                                    gsl_matrix* MatW1,       
                                    gsl_matrix* MatW2,       
                                    gsl_vector* vecw1,       
                                    gsl_vector* vecw2,       
                                    gsl_permutation* permW,  
                                    double eps = 0           
                                   );
 
      virtual gsl_matrix_view calcReducedHessian(int& rankH,                     
                                                 gsl_matrix* MatW1,       
                                                 const gsl_vector* vecx,        
                                                 gsl_matrix* MatW2,       
                                                 gsl_matrix* MatW3,       
                                                 gsl_vector* vecw1,       
                                                 gsl_vector* vecw2,       
                                                 gsl_permutation* permW,  
                                                 double eps = 0           
                                                );
 
      virtual gsl_vector_view calcReducedHessianEigenvalues(int& rankH,               
                                                            gsl_matrix* MatW1,       
                                                            const gsl_vector* vecx,        
                                                            gsl_matrix* MatW2,       
                                                            gsl_matrix* MatW3,       
                                                            gsl_vector* vecw1,       
                                                            gsl_vector* vecw2,       
                                                            gsl_permutation* permW,  
                                                            gsl_eigen_symm_workspace* eigenws,  
                                                            double eps = 0           
                                                           );
    public:
      virtual double calcChi2();
 
      void fillx(gsl_vector* vecx);
      void fillperr(gsl_vector* vece);
 
      // Fill matrix MatM, using lambdas from vecx
      void assembleM(gsl_matrix* MatM, const gsl_vector* vecx, bool errorpropagation = false);
 
      // Fill matrix MatM with 2nd derivative of Lagrangian, using lambdas from vecx
      void assembleG(gsl_matrix* MatM, const gsl_vector* vecx);
 
      // Scale matrix MatM to MatMscal using errors from vece
      void scaleM(gsl_matrix* MatMscal, const gsl_matrix* MatM, const gsl_vector* vece);
 
      // Fill vector y, using lambdas from vecx
      void assembley(gsl_vector* vecy, const gsl_vector* vecx);
 
      // Scale vector vecy to vecyscal using errors from vece
      void scaley(gsl_vector* vecyscal, const gsl_vector* vecy, const gsl_vector* vece);
 
      // Fill chi2 derivatives into vector y
      int assembleChi2Der(gsl_vector* vecy);
 
      // Fill vector y with values of constraints
      void addConstraints(gsl_vector* vecy);
 
      // Fill constraint derivatives into Matrix M
      void assembleConstDer(gsl_matrix* MatM);
 
      // Calculate Newton step update vector vecdx from current point vecx and errors vece
      int calcNewtonDx(gsl_vector* vecdx,              
                       gsl_vector* vecdxscal,   
                       gsl_vector* vecx,        
                       const gsl_vector* vece,        
                       gsl_matrix* MatM,        
                       gsl_matrix* MatMscal,    
                       gsl_vector* vecy,        
                       gsl_vector* vecyscal,    
                       gsl_matrix* MatW,        
                       gsl_matrix* MatW2,       
                       gsl_permutation* permW,  
                       gsl_vector* vecw         
                      );
 
      // Calculate limited step after linesearch
      int calcLimitedDx(double& alpha,                  
                        double& mu,              
                        gsl_vector* vecxnew,     
                        int imode,               
                        gsl_vector* vecx,        
                        gsl_vector* vecdxhat,    
                        const gsl_vector* vecdx,       
                        const gsl_vector* vecdxscal,   
                        const gsl_vector* vece,        
                        const gsl_matrix* MatM,        
                        const gsl_matrix* MatMscal,    
                        gsl_matrix* MatW,        
                        gsl_vector* vecw         
                       );
 
      // Perform a line search
      int doLineSearch(double& alpha,                  
                       gsl_vector* vecxnew,     
                       int imode,               
                       double phi0,             
                       double dphi0,            
//                   double phiR,             ///< Merit function for given alpha
                       double eta,              
                       double zeta,             
                       double mu,               
                       const gsl_vector* vecx,        
                       const gsl_vector* vecdx,       
                       const gsl_vector* vece,        
                       gsl_vector* vecw         
                      );
 
      // Calculate mu for the merit function
      double calcMu(const gsl_vector* vecx,            
                    const gsl_vector* vece,           
                    const gsl_vector* vecdx,          
                    const gsl_vector* vecdxscal,      
                    const gsl_vector* xnew,           
                    const gsl_matrix* MatM,           
                    const gsl_matrix* MatMscal,       
                    gsl_vector* vecw            
                   );
 
      // Calculate the merit function at x
      double meritFunction(double mu,                  
                           const gsl_vector* vecx,    
                           const gsl_vector* vece     
                          );
      // Calculate the directional derivative of the merit function at x
      double meritFunctionDeriv(double mu,                  
                                const gsl_vector* vecx,    
                                const gsl_vector* vece,    
                                const gsl_vector* vecdx,   
                                gsl_vector* vecw     
                               );
 
      // Transfer values from vecx to FitObjects
      bool updateParams(gsl_vector* vecx);
 
 
      int invertM();
 
      int calcCovMatrix(gsl_matrix* MatW, gsl_permutation* permW, gsl_vector* vecx);
 
      enum {NPARMAX = 50, NCONMAX = 10, NUNMMAX = 10};
 
      int npar;      
      int ncon;      
      int nsoft;     
      int nunm;      
      int ierr;      
      int nit;       
 
      double fitprob;   
      double chi2;      
 
      static void ini_gsl_permutation(gsl_permutation*& p, unsigned int size);
      static void ini_gsl_vector(gsl_vector*& v, int unsigned size);
      static void ini_gsl_matrix(gsl_matrix*& m, int unsigned size1, unsigned int size2);
 
      static void debug_print(const gsl_matrix* m, const char* name);
      static void debug_print(const gsl_vector* v, const char* name);
 
      // z = x + a y
      static void add(gsl_vector* vecz, const gsl_vector* vecx, double a, const gsl_vector* vecy);
 
      // Check whether all elements are finite
      static bool isfinite(const gsl_vector* vec);
 
      // Check whether all elements are finite
      static bool isfinite(const gsl_matrix* mat);
 
      static void MoorePenroseInverse(gsl_matrix* Ainv,      
                                      gsl_matrix* A,        
                                      gsl_matrix* W,        
                                      gsl_vector* w,        
                                      double eps = 0        
                                     );
      double calcpTLp(const gsl_vector* vecdx,           
                      const gsl_matrix* MatM,           
                      gsl_vector* vecw            
                     );
 
      int solveSystem(gsl_vector* vecdxscal,
                      double& detW,
                      const gsl_vector* vecyscal,
                      const gsl_matrix* MatMscal,
                      gsl_matrix* MatW,
                      gsl_matrix* MatW2,
                      gsl_vector* vecw,
                      double epsLU,
                      double epsSV
                     );
 
      int solveSystemLU(gsl_vector* vecdxscal,
                        double&     detW,
                        const gsl_vector* vecyscal,
                        const gsl_matrix* MatMscal,
                        gsl_matrix* MatW,
                        gsl_vector* vecw,
                        double eps
                       );
 
      int solveSystemSVD(gsl_vector* vecdxscal,
//                     double& detW,
                         const gsl_vector* vecyscal,
                         const gsl_matrix* MatMscal,
                         gsl_matrix* MatW,
                         gsl_matrix* MatW2,
                         gsl_vector* vecw,
                         double eps
                        );
 
    public:
      unsigned int idim;
      gsl_vector* x;
      gsl_vector* xold;
      gsl_vector* xnew;
//     gsl_vector *xbest;
      gsl_vector* dx;
      gsl_vector* dxscal;
//     gsl_vector *grad;
      gsl_vector* y;
      gsl_vector* yscal;
      gsl_vector* perr;
      gsl_vector* v1;
      gsl_vector* v2;
//     gsl_vector *Meval;
 
      gsl_matrix* M;
      gsl_matrix* Mscal;
      gsl_matrix* W;
      gsl_matrix* W2;
      gsl_matrix* W3;
      // these are only used locally in calcCovMatrix
      gsl_matrix* M1;
      gsl_matrix* M2;
      gsl_matrix* M3;
      gsl_matrix* M4;
      gsl_matrix* M5;
//     gsl_matrix *Mevec;
      gsl_matrix* CC;
      gsl_matrix* CC1;
      gsl_matrix* CCinv;
 
      gsl_permutation* permW;
      gsl_eigen_symm_workspace* eigenws;
//    gsl_eigen_symmv_workspace *eigenwsv;
      unsigned int eigenwsdim;
 
      double chi2best;
      double chi2new;
      double chi2old;
      double fvalbest;
      double scale;
      double scalebest;
      double stepsize;
      double stepbest;
      enum {NITMAX = 100};
      double scalevals[NITMAX];
      double fvals[NITMAX];
 
      int imerit;
      bool try2ndOrderCorr;
 
      int debug;
    };
 
  }// end OrcaKinFit namespace
} // end Belle2 namespace
 
#endif // __NEWFITTERGSL_H