LagrangeMultipliersFitter.cc

Go to the documentation of this file.

/* From SimpleFits Package
 * Designed an written by
 * author: Ian M. Nugent
 * Humboldt Foundations
 */
#include "FWCore/MessageLogger/interface/MessageLogger.h"
#include "RecoTauTag/ImpactParameter/interface/LagrangeMultipliersFitter.h"
#include "TDecompBK.h"
#include <iostream>

using namespace tauImpactParameter;

LagrangeMultipliersFitter::LagrangeMultipliersFitter()
    : isConfigured_(false),
      isFit_(false),
      epsilon_(0.00001),
      weight_(1.0),
      maxDelta_(0.1),
      nitermax_(100),
      chi2_(1e10),
      D_(1, 1),
      V_D_(1, 1) {}

bool LagrangeMultipliersFitter::fit() {
  if (cov_.GetNrows() != par_0_.GetNrows()) {
    // set cov to cov_0 until value is computed
    cov_.ResizeTo(par_0_.GetNrows(), par_0_.GetNrows());
    cov_ = cov_0_;
  }
  if (!isConfigured_)
    return false;
  if (isFit_)
    return isConverged();
  isFit_ = true;
  niter_ = 0;
  for (niter_ = 0; niter_ <= nitermax_; niter_++) {
    bool passed = applyLagrangianConstraints();
    if (!passed || (niter_ == nitermax_ && delta_ >= 4.0 * maxDelta_)) {
      edm::LogWarning("LagrangeMultipliersFitter::Fit")
          << "Reached Maximum number of iterations..." << niter_ << std::endl;
      return false;
    }
    if (isConverged())
      break;
  }
  computeVariance();
  return true;
}

bool LagrangeMultipliersFitter::applyLagrangianConstraints() {
  if (V_D_.GetNrows() != nConstraints())
    V_D_.ResizeTo(nConstraints(), nConstraints());
  if (D_.GetNrows() != nConstraints() || D_.GetNcols() != par_.GetNrows())
    D_.ResizeTo(nConstraints(), par_.GetNrows());

  // Setup intial values
  TVectorT<double> alpha_A = par_;
  TVectorT<double> alpha_0 = par_0_;
  TVectorT<double> delta_alpha_A = alpha_A - alpha_0;
  D_ = derivative();
  TVectorT<double> d = value(par_);
  TVectorT<double> C = D_ * delta_alpha_A - d;
  TMatrixTSym<double> V_alpha0 = cov_0_;
  TMatrixTSym<double> V_D_inv = V_alpha0;
  V_D_inv.Similarity(D_);
  double det = V_D_inv.Determinant();
  TDecompBK Inverter(V_D_inv);
  if (fabs(det) > 1e40) {
    edm::LogWarning("LagrangeMultipliersFitter::Fit")
        << "Fit failed: unable to invert SYM gain matrix LARGE Determinant" << det << " \n"
        << std::endl;
    return false;
  }
  if (!Inverter.Decompose()) {
    edm::LogWarning("LagrangeMultipliersFitter::Fit") << "Fit failed: unable to invert SYM gain matrix " << det << " \n"
                                                      << std::endl;
    return false;
  }
  V_D_ = Inverter.Invert();

  // solve equations
  TVectorT<double> lambda = -1.0 * V_D_ * C;
  TMatrixT<double> DT = D_;
  DT.T();
  TVectorT<double> alpha = alpha_0 - V_alpha0 * DT * lambda;

  // do while loop to see if the convergance criteria are satisfied
  double s(1), stepscale(0.01);
  chi2prev_ = chi2_;
  double currentchi2(chiSquareUsingInitalPoint(alpha_A, lambda)), currentdelta(constraintDelta(par_));
  TVectorT<double> alpha_s = alpha;
  // convergence in 2 step procedure to minimize chi2 within MaxDelta_ of the constriants
  // 1) Get within 5x MaxDelta_
  // 2) converge based on improving chi2 and constrianed delta
  unsigned int Proc = ConstraintMin;
  if (constraintDelta(par_) < 5 * maxDelta_)
    Proc = Chi2AndConstaintMin;
  int NIter = (int)(1.0 / stepscale);
  for (int iter = 0; iter < NIter; iter++) {
    // compute safty cutoff for numberical constraint
    double diff = 0;
    for (int l = 0; l < alpha_s.GetNrows(); l++) {
      if (diff < alpha_s(l) - alpha_A(l))
        diff = alpha_s(l) - alpha_A(l);
    }
    double delta_alpha_s = constraintDelta(alpha_s);
    if (Proc == ConstraintMin) {
      if (delta_alpha_s < currentdelta || iter == NIter || diff < 100 * epsilon_) {
        currentchi2 = chiSquareUsingInitalPoint(alpha_s, lambda);
        currentdelta = delta_alpha_s;
        ScaleFactor_ = s;
        break;
      }
    } else if (Proc == Chi2AndConstaintMin) {
      double chi2_s = chiSquareUsingInitalPoint(alpha_s, lambda);
      if ((delta_alpha_s < currentdelta /*+maxDelta_*/ && chi2_s < currentchi2) || iter == NIter ||
          diff < 100 * epsilon_) {
        currentchi2 = chi2_s;
        currentdelta = delta_alpha_s;
        ScaleFactor_ = s;
        break;
      }
    }
    s -= stepscale;
    alpha_s = alpha_A + s * (alpha - alpha_A);
  }
  // set chi2
  chi2_ = currentchi2;
  //set delta
  delta_ = currentdelta;
  par_ = alpha_s;
  return true;
}

TMatrixD LagrangeMultipliersFitter::derivative() {  // alway evaluated at current par
  TMatrixD Derivatives(nConstraints(), par_.GetNrows());
  TVectorD par_plus(par_.GetNrows());
  TVectorD value_par(nConstraints());
  TVectorD value_par_plus(nConstraints());
  for (int j = 0; j < par_.GetNrows(); j++) {
    for (int i = 0; i < par_.GetNrows(); i++) {
      par_plus(i) = par_(i);
      if (i == j)
        par_plus(i) = par_(i) + epsilon_;
    }
    value_par = value(par_);
    value_par_plus = value(par_plus);
    for (int i = 0; i < nConstraints(); i++) {
      Derivatives(i, j) = (value_par_plus(i) - value_par(i)) / epsilon_;
    }
  }
  return Derivatives;
}

bool LagrangeMultipliersFitter::isConverged() {
  if (delta_ < maxDelta_) {
    return true;
  }
  return false;
}

double LagrangeMultipliersFitter::chiSquare(const TVectorT<double>& delta_alpha,
                                            const TVectorT<double>& lambda,
                                            const TMatrixT<double>& D,
                                            const TVectorT<double>& d) {
  double c2 = lambda * (D * delta_alpha + d);
  return c2;
}

double LagrangeMultipliersFitter::chiSquareUsingInitalPoint(const TVectorT<double>& alpha,
                                                            const TVectorT<double>& lambda) {
  if (cov_0_.GetNrows() != V_alpha0_inv_.GetNrows()) {
    TMatrixTSym<double> V_alpha0 = cov_0_;
    V_alpha0_inv_.ResizeTo(cov_0_.GetNrows(), cov_0_.GetNrows());
    TDecompBK Inverter(V_alpha0);
    if (!Inverter.Decompose()) {  // handle rare case where inversion is not possible (ie assume diagonal)
      edm::LogWarning("LagrangeMultipliersFitter::chiSquareUsingInitalPoint")
          << "Error non-invertable Matrix... Calculating under assumption that correlations can be neglected!!!"
          << std::endl;
      for (int j = 0; j < par_.GetNrows(); j++) {
        for (int i = 0; i < par_.GetNrows(); i++) {
          if (i == j)
            V_alpha0_inv_(i, j) = 1.0 / V_alpha0(i, j);
          else
            V_alpha0_inv_(i, j) = 0.0;
        }
      }
    } else {
      V_alpha0_inv_ = Inverter.Invert();
    }
  }

  TVectorT<double> alpha_0 = par_0_;
  TVectorT<double> dalpha = alpha - alpha_0;
  double c2_var = dalpha * (V_alpha0_inv_ * dalpha);
  const TVectorT<double>& alpha_v = alpha;
  double c2_constraints = lambda * value(alpha_v);
  double c2 = c2_var + c2_constraints;
  return c2;
}

double LagrangeMultipliersFitter::constraintDelta(const TVectorT<double>& par) {
  TVectorD d_par = value(par);
  double delta_d(0);
  for (int i = 0; i < d_par.GetNrows(); i++) {
    delta_d += fabs(d_par(i));
  }
  return delta_d;
}

TMatrixT<double> LagrangeMultipliersFitter::computeVariance() {
  TMatrixTSym<double> V_alpha0 = cov_0_;
  TMatrixTSym<double> DTV_DD = V_D_.SimilarityT(D_);
  TMatrixT<double> DTV_DDV = DTV_DD * V_alpha0;
  TMatrixT<double> VDTV_DDV = V_alpha0 * DTV_DDV;
  TMatrixT<double> CovCor = VDTV_DDV;
  //CovCor*=ScaleFactor_;
  if (V_corr_prev_.GetNrows() != V_alpha0.GetNrows()) {
    V_corr_prev_.ResizeTo(V_alpha0.GetNrows(), V_alpha0.GetNrows());
    V_corr_prev_ = CovCor;
  } else {
    V_corr_prev_ *= (1 - ScaleFactor_);
    CovCor += V_corr_prev_;
    V_corr_prev_ = CovCor;
  }

  TMatrixT<double> V_alpha = V_alpha0 - CovCor;
  for (int i = 0; i < cov_.GetNrows(); i++) {
    for (int j = 0; j <= i; j++) {
      cov_(i, j) = V_alpha(i, j);
    }
  }
  return cov_;
}