WWFitterFast.C

Go to the documentation of this file.

// Class WWFitterFast
//
// Author: Jenny Boehme, Benno List
// Last update: $Date: 2005/01/12 10:11:46 $
//          by: $Author: blist $
// 
// Description: kinematic fit a la WWFGO
// DISCLAIMER: the only object oriented stuff in here is the
//             interface to fit objects (jets, neutrinos,....)
//             and constraints (px, py, M_W, ....)
//             which replaces the calls to WWKCNS.
//             The WWFitterFast::fit() method is almost an
//             'F to C' translation of WWFGO. It is NOT
//             considered to be good C++, but was done
//             on purpose as a first implementation.
//             An OO version might follow later!     
//  
//
// This is a faster version of WWFitter,
// which replaces (in  the iteration loop) all calculations
// of inverse matrices with the solution of corresponding systems
// of equations
//
// IMPORTANT: This Fitter is yet untested for the case of 
// unmeasured quantities (i.e., neutrinos!)!!!
//           

#include<iostream>
#include<cmath>

#include "jbltools/kinfit/WWFitterFast.h" 

#include "jbltools/kinfit/BaseFitObject.h"
#include "jbltools/kinfit/BaseConstraint.h"
#include "jbltools/kinfit/ftypes.h"
#include "jbltools/kinfit/cernlib.h"

using std::cout;
using std::endl;

// constructor
WWFitterFast::WWFitterFast() : npar(0), nmea(0), nunm(0), ncon(0), ierr (0)  {};

// destructor
WWFitterFast::~WWFitterFast() {};

// do it (~ transcription of WWFGO as of ww113)
double WWFitterFast::fit() {

  // cout statements
  bool debug = false;

  // order parameters etc
  initialize();
  
  // initialize eta, etasv, y 
  FDouble eta[NPARMAX];
  FDouble etasv[NPARMAX];
  FDouble y[NPARMAX];           
  
  for (unsigned int i = 0; i < fitobjects.size(); ++i) {
    for (int ilocal = 0; ilocal < fitobjects[i]->getNPar(); ++ilocal) {
      int iglobal = fitobjects[i]->getGlobalParNum (ilocal);
      if (iglobal >= 0) y[iglobal] = eta[iglobal] = fitobjects[i]->getParam(ilocal);
      //if (debug) cout << "eta[" << iglobal << "] = " << eta[iglobal] 
      //                << " for jet " << i << " and ilocal = " << ilocal << endl;
    }
  }

  // initialize dfeta ( = d F / d eta)
  FDouble dfeta[NCONMAX][NPARMAX];   //  npar x nconstraint
  for (int k=0; k < ncon; k++) {
    for (int j=0; j < npar; j++) dfeta[k][j]=0;
    constraints[k]->getDerivatives(NPARMAX, dfeta[k]);
    //if (debug) for (int j=0; j < npar; j++) 
      //cout << "dfeta[" << k << "][" << j << 
      //                     "] = " << dfeta[k][j] << endl;
  }
  

  // R & S & W1
  FDouble R[NCONMAX];                      // list of constraints
  FDouble S[NCONMAX][NCONMAX];             // nconstraint  x  nconstraint
  FDouble W1[NUNMMAX][NUNMMAX];            // nunmeasured  x  nunmeasured
  FDouble V[NPARMAX][NPARMAX];             // error matrix of parameters
  FDouble dxi[NUNMMAX];                    // shift of unmeasured parameters
  FDouble alam[NCONMAX];                   // Lagrange multipliers

  // chi2's, step size, # iterations
  double chinew=0, chit=0, chik=0;
  double alph = 1.;
  nit = 0;
  // convergence criteria (as in WWINIT)
  // int nitmax = 20;
  int nitmax = 200;
  double chik0 = 100.;
  double chit0 = 100.;
  double dchikc = 1.0E-3;
  double dchitc = 1.0E-4;
  double dchikt = 1.0E-2;
  double dchik  = 1.05;
  double chimxw = 1000.;
  double almin = 0.05;
 

  // repeat with or with out smaller steps size 
  bool repeat = true;
  bool scut = false;
  bool calcerr = true;
 
  // start of iterations
  while (repeat) {
    bool updatesuccess = true;
  
//*-- If necessary, retry smaller step, same direction
    if (scut) {
      for (int j=0; j < npar; j++) eta[j] = etasv[j];
      updatesuccess = updateFitObjects (eta);
      if (!updatesuccess) {
        std::cerr << "WWFitterFast::fit: old parameters are garbage!" << std::endl;
        return -1;
      }
      for (int k = 0; k < ncon; ++k)  {
        for (int j=0; j < npar; j++) dfeta[k][j]=0;
        constraints[k]->getDerivatives(NPARMAX, dfeta[k]);
      }
    } 
    else {
      for (int j=0; j < npar; j++) etasv[j] = eta[j];
      chik0 = chik;
      chit0 = chit;
    }
    
    // Get covariance matrix
    for (int i = 0; i < npar; ++i) 
      for (int j = 0; j < npar; ++j) V[i][j] = 0;
    for (unsigned int ifitobj = 0; ifitobj<fitobjects.size(); ++ifitobj) {
      fitobjects[ifitobj]->addToGlobCov (&V[0][0], NPARMAX);
    }  
    
    
// *-- Evaluate r and S.
    for (int k = 0; k < ncon; ++k) R[k] = constraints[k]->getValue();
    for (int j = 0; j < nmea; ++j) {
      double dy = y[j]-eta[j];
      for (int k = 0; k < ncon; ++k) {
        R[k] += dfeta[k][j]*dy;
      }
    }
    
    for (int l = 0; l < ncon; ++l) {
      for (int k = 0; k < ncon; ++k) S[k][l] = 0;
    }
    for (int i = 0; i < nmea; ++i) {
      for (int j = 0; j < nmea; ++j) {
        double v=V[i][j];
        for (int k = 0; k < ncon; ++k){
          double dfetav = dfeta[k][i]*v;
          for (int l = 0; l < ncon; ++l) {
            S[k][l] += dfetav * dfeta[l][j]; 
          }
        }
      }
    }
    

// *-- Calculate new unmeasured quantities:
// Calculate W1_ij = dfeta_k,nmea+i * Sinv_kl * dfeta_l,nmea+j
// This is the same as W1_ij = dfeta_k,nmea+i * xhelp_kj,
// where x is the solution of S_ik * xhelp_kj = dfeta_i,nmea+j
  
    if (nunm > 0) {
      FDouble xhelp[NCONMAX][NPARMAX]; 
      for (int k=0; k < ncon; k++) {
        for (int j=0; j < npar; j++) xhelp[k][j] = dfeta[k][nmea+j];
      }
      dseqn (ncon, &S[0][0], NCONMAX, nunm, &xhelp[0][0]);
      
      for (int i = 0; i < nunm; ++i) {
        for (int j = 0; j < nunm; ++j) W1[i][j] = 0;
      }  
      for (int k = 0; k < ncon; ++k) {
       for (int j = 0; j < nunm; ++j) {
         for (int i = 0; i < nunm; ++i) {
           W1[i][j] += dfeta[k][nmea+i] * xhelp[j][k];
         }
       }
      }
      

      
      // calculate shift of unmeasured parameters

      // we need W^-1 just to calculate dxi = -W^-1 dfeta Sinv R,
      // so instead of inverting W, just solve the system
      // W*dxi = -dfeta Sinv R = work
      
      // Additionally, instead of evaluating Sr = Sinv * R, we solve
      // S * Sr = R
      
      std::cerr << "WWFitterFast::fit(): Untested!!!" << endl;
      
      for (int i = 0; i < nunm; ++i) dxi[i] = 0;
      
      FDouble Sr[NCONMAX];
      
      for (int l = 0; l < ncon; ++l) Sr[l] = R[l];
      dsfeqn (ncon, &S[0][0], NCONMAX, 1, Sr);
      
      for (int k = 0; k < ncon; ++k) {
        for (int i = 0; i < nunm; ++i) {
          dxi[i] -= dfeta[k][nmea+i]* Sr[k];
        }
      }
      
      dseqn (nunm, &W1[0][0], NUNMMAX, ierr, 1, dxi);
    }
    
    
// *-- And now update unmeasured parameter.
    for (int i = 0; i < nunm; ++i) {
      eta[nmea+i] += dxi[i];
    }
    
// *-- Calculate new Lagrange multipliers.
    // Again, instead of multiplying alam = Sinv * (R + dfeta*dx), 
    // we solve S*alam = R + dfeta*dx
    for (int k = 0; k < ncon; ++k) {
      alam[k] = R[k];
    }
    for (int j = 0; j < nunm; ++j) {
      double d = dxi[j];
      int nmeaj = nmea+j;
      for (int k = 0; k < ncon; ++k) {
        alam[k] += dfeta[k][nmeaj] * d;
      }
    }
    if (nunm > 0) {
      dsfeqn (ncon, &S[0][0], NCONMAX, 1, alam);
    }
    else {
      dseqn (ncon, &S[0][0], NCONMAX, 1, alam);
    }
// *-- Calculate new fitted parameters.
    for (int i = 0; i < nmea; ++i) eta[i] = y[i];
    for (int k = 0; k < ncon; ++k) {
      double a = alam[k];
      for (int j = 0; j < nmea; ++j) {
        double dfetaa =  dfeta[k][j] * a;
        for (int i = 0; i < nmea; ++i) {
          eta[i] -= V[i][j] * dfetaa;
        }
      }
    }
    
// *-- Calculate constraints and their derivatives.
    // since the constraints ask the fitobjects for their parameters, 
    // we need to update the fitobjects first!
    // COULD BE DONE: update also ERRORS! (now only in the very end!)
    updatesuccess = updateFitObjects (eta);
         
    if (debug) {
      cout << "After adjustment of all parameters:\n";
      for (int k = 0; k < ncon; ++k) {
        cout << "Value of constraint " << k << " = " << constraints[k]->getValue()
             << endl;
      }
    }
    for (int k=0; k < ncon; k++) {
      for (int j=0; j < npar; j++) dfeta[k][j]=0;
      constraints[k]->getDerivatives(NPARMAX, dfeta[k]);
    }

// *-- Calculate new chisq.

    // Instead of evaluating Vinvdeta = Vinv * deta,
    // solve V * Vinvdeta = deta:
    
    FDouble deta[NPARMAX];
    FDouble Vinvdeta[NPARMAX];
    
    chit = 0;
    for (int i = 0; i < nmea; ++i) {
      Vinvdeta[i] = deta[i] = (y[i]-eta[i]);
    } 
    
    dseqn (nmea, &V[0][0], NPARMAX, 1, Vinvdeta);
     
    chit = 0;
    for (int i = 0; i < nmea; ++i) {
      chit  += deta[i] * Vinvdeta[i];
    }
    chik = 0;
    for (int k = 0; k < ncon; ++k) chik += std::abs(2*alam[k]*constraints[k]->getValue());
    
    chinew = chit + chik;
    
//*-- Calculate change in chisq, and check constraints are satisfied.
    nit++;
    
    bool sconv = (std::abs(chik-chik0) < dchikc) 
              && (std::abs(chit-chit0) < dchitc*chit) 
              && (chik < dchikt*chit);
    // Second convergence criterium:
    // If all constraints are fulfilled to better than 1E-8,
    // and all parameters have changed by less than 1E-8,
    // assume convergence
    // This criterium assumes that all constraints and all parameters
    // are "of order 1", i.e. their natural values are around 1 to 100,
    // as for GeV or radians
    double eps = 1E-8;
    bool sconv2 = true;
    for (int k = 0; sconv2 && (k < ncon); ++k) 
      sconv2 &= (std::abs(constraints[k]->getValue()) < eps);
    if (sconv2 && debug) 
      cout << "All constraints fulfilled to better than " << eps << endl;
       
    for (int j = 0; sconv2 && (j < npar); ++j) 
      sconv2 &= (std::abs(eta[j] - etasv[j]) < eps);
    if (sconv2 && debug) 
      cout << "All parameters stable to better than " << eps << endl;
    sconv |= sconv2;
             
    bool sbad  = (chik > dchik*chik0) 
              && (chik > dchikt*chit)
              && (chik > chik0 + 1.E-10);
              
    if (nit > nitmax) {
// *-- Out of iterations
      repeat = false;
      ierr = 1;
    }  
    else if (sconv && updatesuccess) {
// *-- Converged
      repeat = false;
      ierr = 0;
    }  
    else if ( nit > 2 && chinew > chimxw  && updatesuccess) {
// *-- Chi2  crazy ?
      repeat = false;
      calcerr = false;
      ierr = 2;
    }  
    else if (sbad && nit > 1  || !updatesuccess) {
// *-- ChiK increased, try smaller step
      if ( alph == almin ) {
        repeat = false;
        calcerr = false;
        ierr = 3;
      }  
      else {
        alph  =  std::max (almin, 0.5 * alph);
        scut  =  true;
        repeat = true;
        ierr = 4;
      }
    }    
    else {
// *-- Keep going..
      alph  =  std::min (alph+0.1, 1. );
      repeat = true;
      ierr = 5;
    }
    
    if (debug) cout << "======== NIT = " << nit << ",  CHI2 = " << chinew 
                                     << ",  ierr = " << ierr << endl;
                                     
    if (debug) 
      for (unsigned int i = 0; i < fitobjects.size(); ++i) 
        cout << "fitobject " << i << ": " << *fitobjects[i] << endl;                                 

  }   // end of while (repeat)
  
// *-- End of iterations - calculate errors.
  FDouble VETA[NPARMAX][NPARMAX];
  for (int i = 0; i < npar; ++i) 
    for (int j = 0; j < npar; ++j) VETA[i][j] = 0;
    
  FDouble Sinv[NCONMAX][NCONMAX];             // nconstraint  x  nconstraint
 
  if (calcerr) {
  
// *-- Evaluate S and invert.
    for (int k = 0; k < ncon; ++k) {
      for (int l = 0; l < ncon; ++l) {
        S[k][l] = 0;
      }
    }
    for (int i = 0; i < npar; ++i) {
      for (int j = 0; j < npar; ++j) {
        double v = V[i][j];
        for (int k = 0; k < ncon; ++k) {
          double dfetav = dfeta[k][i] * v;
          for (int l = 0; l < ncon; ++l) {
            S[k][l] += dfetav * dfeta[l][j]; 
          }
        }
      }
    }
    for (int k = 0; k < ncon; ++k) {
      for (int l = 0; l < ncon; ++l) {
        Sinv[k][l] = S[k][l];
      }
    }
   
// *-- Invert S, testing for singularity first.
// skip singularity test, since it only fixes some machine dependency
// of DSINV/RSINV (-> cernlib)
    dsinv(ncon, &Sinv[0][0], NCONMAX, ierr);

// *-- Calculate G.  
//  (same as W1, but for measured parameters) 
    FDouble G[NPARMAX][NPARMAX];
    for (int i = 0; i < nmea; ++i) {
      for (int j = 0; j < nmea; ++j) G[i][j] = 0;
    }
    for (int k = 0; k < ncon; ++k) {
      for (int l = 0; l < ncon; ++l) {
        double s = Sinv[k][l];
        for (int i = 0; i < nmea; ++i) {
          double dfetas = s*dfeta[k][i];
          for (int j = 0; j < nmea; ++j) {
            G[i][j] += dfetas * dfeta[l][j];
          }
        }
      }
    }
    
    if (nunm > 0) {

// *-- Calculate H.
      FDouble H[NPARMAX][NUNMMAX];
      for (int i = 0; i < nmea; ++i) {
        for (int j = 0; j < nunm; ++j) {
          H[i][j] = 0;
          for (int k = 0; k < ncon; ++k) {
            for (int l = 0; l < ncon; ++l) {
              H[i][j] += dfeta[k][i] * Sinv[k][l] * dfeta[l][nmea+j];
            }
          }
        }
      }
      
// *-- Calculate U**-1 and invert.
//   (same as W1)
      FDouble U[NUNMMAX][NUNMMAX];
      for (int i = 0; i < nunm; ++i) {
        for (int j = 0; j < nunm; ++j) {
          U[i][j] = 0;
          for (int k = 0; k < ncon; ++k) {
            for (int l = 0; l < ncon; ++l) {
              U[i][j] += dfeta[k][nmea+i] * Sinv[k][l] * dfeta[l][nmea+j];
            }
          }
        }
      }
      dsinv(nunm, &U[0][0], NUNMMAX, ierr);
     
// *-- U is now error matrix of unmeasured parameters
      for (int i = 0; i < nunm; ++i) {
        for (int j = 0; j < nunm; ++j) {
          VETA[nmea+i][nmea+j] = U[i][j];
        }
      }

// *-- Covariance matrix between measured and unmeasured parameters.
      for (int i = 0; i < nmea; ++i) {
        for (int j = 0; j < nunm; ++j) {
          VETA[i][nmea+j] = 0.;
          for (int ii = 0; ii < nmea; ++ii) {
            for (int jj = 0; jj < nunm; ++jj) {
              VETA[i][nmea+j] -= V[i][ii] * H[ii][jj] * U[jj][j];
            }
          }
        }
      }
      
// *-- Fill in symmetric part:
      for (int i = 0; i < nmea; ++i) {
        for (int j = 0; j < nunm; ++j) {
          VETA[nmea+j][i] = VETA[i][nmea+j];
        }
      }
      
// *-- Calculate G-HUH.
      for (int i = 0; i < nmea; ++i) {
        for (int j = 0; j < nmea; ++j) {
          for (int ii = 0; ii < nunm; ++ii) {
            for (int jj = 0; jj < nunm; ++jj) {
              G[i][j] -= H[i][ii] * U[ii][jj] * H[j][jj];
            }
          }
        }
      }
      
    }  // endif nunm > 0

// *-- Calculate I-GV.
    FDouble GV[NPARMAX][NPARMAX];
    for (int i = 0; i < nmea; ++i) {
      for (int j = 0; j < nmea; ++j) {
        if (i == j) GV[i][j] = 1.;
        else GV[i][j] = 0.;
        for (int ii = 0; ii < nmea; ++ii) {
          GV[i][j] -= G[i][ii] * V[ii][j];
        }
      }
    }

// *-- And finally error matrix on fitted parameters.
    for (int i = 0; i < nmea; ++i) {
      for (int j = 0; j < nmea; ++j) {
        VETA[i][j] = 0.;
        for (int ii = 0; ii < nmea; ++ii) {
          VETA[i][j] += V[i][ii] * GV[ii][j];
        }
      }
    }

    // update errors in fitobjects
    // (consider only diagonal elements of VETA for the moment...)
    for (unsigned int ifitobj = 0; ifitobj < fitobjects.size(); ++ifitobj) {
      for (int ilocal = 0; ilocal < fitobjects[ifitobj]->getNPar(); ++ilocal) {
        int iglobal = fitobjects[ifitobj]->getGlobalParNum (ilocal); 
        if (iglobal >= 0) fitobjects[ifitobj]->setError(ilocal, 
                             std::sqrt(std::fabs(VETA[iglobal][iglobal]))); 
      }
    }
    
  } // endif calcerr == true

// *-- Turn chisq into probability.
  FReal chi = FReal(chinew);
  fitprob = (ncon-nunm > 0) ? prob(chi,ncon-nunm) : 0.5;
  chi2 = chinew;

  return fitprob;
    
};

bool WWFitterFast::initialize() {
  // tell fitobjects the global ordering of their parameters:
  int iglobal = 0;
  // measured parameters first!
  for (unsigned int ifitobj = 0; ifitobj < fitobjects.size(); ++ifitobj) {
    for (int ilocal = 0; ilocal < fitobjects[ifitobj]->getNPar(); ++ilocal) {
      if (fitobjects[ifitobj]->isParamMeasured(ilocal) &&
          !fitobjects[ifitobj]->isParamFixed(ilocal)) {
        fitobjects[ifitobj]->setGlobalParNum (ilocal, iglobal);
        ++iglobal;
      }
    }
  }
  nmea = iglobal;
  // now  unmeasured parameters!
  for (unsigned int ifitobj = 0; ifitobj < fitobjects.size(); ++ifitobj) {
    for (int ilocal = 0; ilocal < fitobjects[ifitobj]->getNPar(); ++ilocal) {
      if (!fitobjects[ifitobj]->isParamMeasured(ilocal) &&
          !fitobjects[ifitobj]->isParamFixed(ilocal)) {
        fitobjects[ifitobj]->setGlobalParNum (ilocal, iglobal);
        ++iglobal;
      }
    }
  }
  npar = iglobal;
  nunm = npar - nmea;
  
  // set number of constraints
  ncon = constraints.size();
  
  return true;

};
  
bool WWFitterFast::updateFitObjects (double eta[]) {
  bool debug = false;
  bool result = true;
  for (unsigned int ifitobj = 0; ifitobj < fitobjects.size(); ++ifitobj) {
    for (int ilocal = 0; ilocal < fitobjects[ifitobj]->getNPar(); ++ilocal) {
      int iglobal = fitobjects[ifitobj]->getGlobalParNum (ilocal); 
      if (!fitobjects[ifitobj]->isParamFixed (ilocal) && iglobal >= 0) {
        if (debug) cout << "Parameter " << iglobal 
                        << " (" << fitobjects[ifitobj]->getName()
                        << ": " << fitobjects[ifitobj]->getParamName(ilocal)
                        << ") set to " << eta[iglobal];
        result &= fitobjects[ifitobj]->setParam(ilocal, eta[iglobal]); 
        eta[iglobal] = fitobjects[ifitobj]->getParam(ilocal);
        if (debug) cout << " => " << eta[iglobal] << endl;
      }
    }
  }
  return result;
};

int WWFitterFast::getError() const {return ierr;}
double WWFitterFast::getProbability() const {return fitprob;}
double WWFitterFast::getChi2() const {return chi2;}
int WWFitterFast::getIterations() const {return nit;}

---