// from ROOT
#include "TMath.h"
#include "TString.h"
#include "TVector3.h"

// from hydra
#include "hades.h"
#include "hcategory.h"
#include "hcategorymanager.h"
#include "hmdccal2par.h"
#include "hmdccal2parsim.h"
#include "hmdctrackgdef.h"
#include "hmdctrackgfield.h"
#include "hphysicsconstants.h"
#include "hruntimedb.h"
#include "hmdcsizescells.h"

#include "hkalgeomtools.h"
#include "hkalmatrixtools.h"
#include "hkalplane.h"
#include "hkalsystem.h"

// C/C++ libraries
#include <iostream>
using namespace std;

//_HADES_CLASS_DESCRIPTION
///////////////////////////////////////////////////////////////////////////////
//
// This class is responsible for the Kalman filter.
//
// The Kalman filter is started via the 
// Bool_t fitTrack(const TObjArray &hits,  const TVectorD &iniStateVec, const TMatrixD &iniCovMat)
// function. It needs a series of measurement points (an array of class HKalMdcHit)
// and initial estimations for the track state and covariance matrix as input.
//
// It will then make an estimation for the initial track state based on the
// measurement hits and fill the covariance matrix with large values.
// The function will return a boolean to indicate whether problems (high chi^2 or
// unrealistic track parameters) were encountered during track fitting.
//
// Track propagation will be done with the Runge-Kutta method.
//
// After the Kalman filter is done, a measurement site may be eliminated
// from the calculation with the function
// excludeSite(Int_t iSite)
//
//
// Several options for the Kalman filter may be set:
// setNumIterations(Int_t kalRuns)
// Number of iterations. After each iteration the Kalman filter will use the
// result of the previous iteration as starting value.
// Default is 1.
//
// setDirection(Bool_t dir)
// Propagation direction (kIterForward, kIterBackward). Direction will switch
// after each iteration.
// Default is forward direction.
//
// setSmoothing(Bool_t smooth)
// Smooth or don't smooth track states after filter. Smoothing attempts to
// improve the filtered track states by using the results of all subsequent
// as well.
// Default is on.
//
// setRotation(Kalman::kalRotateOptions rotate)
// kNoRot: No coordinate transformation. Not recommended for high track angles (> 30°).
// kVarRot (default): Tilt the coordinate system along the initial track direction
// to avoid large values for the track state parameters tx or ty.
//
// setDoEnerLoss(Bool_t dedx)
// setDoMultScat(Bool_t ms)
// Include energy or multiple scattering in the Kalman filter.
// Default is on.
//
// setConstField(Bool_t constField)
// setFieldVector(const TVector3 &B)
// Tells the Kalman filter to use a constant magnetic field instead of the fiel map.
// Default is off.
//
// setFilterMethod(Kalman::filtMethod type)
// Changes which formulation of the Kalman equations to use. The formulations differ
// in speed and numerical stability. See the description of the functions named filter...
// for details.
//
// setDafPars(Double_t chi2cut, const Double_t *T, Int_t n)
// Change Parameters for the deterministic annealing filter.
// chi2cut: Cut-off parameter. Default: 9.
// T: Array with annealing factors. Default: 81, 21, 9, 4, 1.
// n: Number of annealing iterations. Default: 5.
//
//
// Notation:
// a: track state vector
// m: measurement vector
// r: residual of the measurement vector and the predicted measurement
// f(a): propagation function: Propagate track state a to the next
//       detector.
// h(a): projection function that calculates a measurement vector from
//       track state v
// F: Jacobian of f(a)
// H: Jacobian of h(a)
// C: covariance matrix of the track state
// Q: covariance matrix of the process noise
// V: covariance matrix of the measurement errors
//
//-----------------------------------------------------------------------
///////////////////////////////////////////////////////////////////////////////

ClassImp (HKalSystem)

//  -----------------------------------
//  Ctors and Dtor
//  -----------------------------------

HKalSystem::HKalSystem(Int_t n, Int_t measDim, Int_t stateDim,
                       HMdcTrackGField *fMap, Double_t fpol, Double_t chiMax)
    : TObject(), iniStateVec(stateDim) {
    // Constructor for a Kalman system.
    //
    // Input:
    // n:        the maximum number of measurement sites in a track.
    // measDim:  the dimension of a measurement vector (must be the same for all tracks)
    // stateDim: the dimension of the track state vector (must be the same for all tracks)
    // fMap:     pointer to field map.
    // fpol:     field scaling factor * polarity.
    // chiMax:   the maximum chi^2 where a track is still accepted.

    nMaxSites    = n;
    nHitsInTrack = n;
    nSites       = n;

    // DAF parameters.
    const Int_t nDafs    = 5;                        // Number of DAF iterations.
    Double_t    T[nDafs] = { 81., 9., 4., 1., 1. };  // Annealing factors (temperatures).
    dafT                 = TArrayD(nDafs, &T[0]);
    dafChi2Cut           = 9.;                       // Cut-off parameter.

    sites = new HKalTrackSite* [nSites];
    for(Int_t i = 0; i < nSites; i++) {
        sites[i] = new HKalTrackSite(measDim, stateDim);
        getSite(i)->setNdafs(nDafs);
    }

    setFieldMap(fMap, fpol);
    chi2High         = chiMax;
    trackNum         = 0;
    pid              = -1;

    bPrintWarn       = kTRUE;
    bFillPointArrays = kFALSE;

    setNumIterations(1);
    setDirection(kIterForward);
    setSmoothing(kTRUE);
    if(measDim == 1) {
        setHitType(Kalman::kWireHit);
    }
    if(measDim == 2) {
        setHitType(Kalman::kSegHit);
    }
    setFilterMethod(Kalman::kKalConv);
    setDoEnerLoss(kTRUE);
    setDoMultScat(kTRUE);

    setRotation(Kalman::kVarRot);
    pRotMat = new TRotation();
    trackPropagator.setRotationMatrix(pRotMat);


    //? remove ntuples
    hDafMonitor = new TNtupleD("daf", "", "temp:iter:site:nComp:chi2cut:trackNum:weight1:weight2:chi21:chi22");
    hKalMon = new TNtupleD("kalgain", "", "layer:trackNum:dxdx:dxdy:dydx:dydy:dtxdx:dtxdy:dtydx:dtydy:dqpdx:dqpdy");

    pointsTrack.SetOwner();
    fieldTrack.SetOwner();
}

HKalSystem::~HKalSystem() {
    //? daf test
    delete hDafMonitor;

    delete sites;
    delete pRotMat;
}

//  -----------------------------------
//  Implementation of protected methods
//  -----------------------------------

void HKalSystem::filterConventional(Int_t iSite) {
    // Filter a site using the conventional formulation of the Kalman filter equations.
    //
    // C_k = (I - K_k * H_k) * C^{k-1}_k
    // K_k = C^{k-1}_k * H^T_k * (V_k + H_k * C^{k-1}_k * H^T_k)^-1
    //
    // These are simpler equations compared to the Joseph or Swerling forumations.
    // Only one inversion of a matrix with the dimension of the measurement vector is neccessary.
    // However, the updated covariance matrix may not be positive definite due to numerical
    // computing problems.

    Kalman::coordSys sys = getFilterInCoordSys(); // sector or virtual layer coordinates
    HKalTrackSite *site = getSite(iSite);

    // Predicted state vector for this site.
    const TVectorD &predState = site->getStateVec(Kalman::kPredicted, sys);    // a^k-1_k
    TVectorD projMeasVec(getMeasDim());
    // Expected measurement vector.
    calcMeasVecFromState(projMeasVec, site, Kalman::kPredicted, sys);  // h_k(a^k-1_k)

    // Residual of measurement vector and expected measurement vector.
    TVectorD residual = site->getHitVec() - projMeasVec;

    const TMatrixD &predCov    = site->getStateCovMat(Kalman::kPredicted, sys); // C^{k-1}_k
    const TMatrixD &errMat     = site->getErrMat();                             // V_k
    const TMatrixD &fProj      = site->getStateProjMat(Kalman::kFiltered, sys); // H_k
    const TMatrixD  fProjTrans = TMatrixD(TMatrixD::kTransposed, fProj);

    // Component of the Kalman gain that needs to be inverted.
    TMatrixD R = errMat + fProj * predCov * fProjTrans;

#if kalDebug > 0
    if(!HKalMatrixTools::checkCond(R)) {
        if(bPrintWarn) {
            Warning("filterConventional()", "Matrix is ill-conditioned for inversion.");
        }
    }
#endif

    // Calc Kalman gain matrix.
    //       K_k        = C^{k-1}_k * H^T_k      * (V_k + H_k * C^{k-1}_k * H^T_k)^-1
    TMatrixD kalmanGain = predCov   * fProjTrans * R.Invert();

    //                 (I - K_k * H_k)
    TMatrixD covUpdate(TMatrixD(TMatrixD::kUnit,TMatrixD(getStateDim(sys),getStateDim(sys))) - kalmanGain * fProj);
    //       C_k     = (I - K_k * H_k) * C^{k-1}_k
    TMatrixD filtCov = covUpdate       * predCov;

    if(!filtCov.IsSymmetric()) {
        if(!HKalMatrixTools::makeSymmetric(filtCov)) {
            if(bPrintWarn) {
                Warning("filterConventional()", "Covariance matrix for filter step is not close to being symmetric.");
            }
        }
    }
    if(!HKalMatrixTools::checkIsPosDef(filtCov)) {
        if(bPrintWarn) {
            Warning("filterConventional()", "Covariance for the filtered state is not positive definite.");
        }
    }

    site->setStateCovMat(Kalman::kFiltered, filtCov, sys);

    // Calculate filtered state vector.
    //       a_k       = a^k-1_k   + K_k        * (m_k     - h_k(a^k-1_k) )
    TVectorD filtState = predState + kalmanGain * residual;
    site->setStateVec(Kalman::kFiltered, filtState, sys);

#if kalDebug > 2
    if(sys == Kalman::kSecCoord) {
        cout<<"Filtering in sector coordinates."<<endl;
    }
    if(sys == Kalman::kLayCoord) {
        cout<<"Filtering in virtual layer coordinates."<<endl;
    }
    cout<<"Filter with conventional Kalman equations."<<endl;
    cout<<"Expected state vector:"<<endl;
    predState.Print();
    cout<<"Expected measurement:"<<endl;
    projMeasVec.Print();
    cout<<"Measurement vector:"<<endl;
    site->getHitVec().Print();
    cout<<"Filtered state vector:"<<endl;
    filtState.Print();
    cout<<"Filtered covariance:"<<endl;
    filtCov.Print();
#endif
#if kalDebug > 3
    cout<<"Covariance from prediction:"<<endl;
    predCov.Print();
    cout<<"Measurement error matrix:"<<endl;
    errMat.Print();
    cout<<"Projector matrix:"<<endl;
    fProj.Print();
    cout<<"Kalman gain:"<<endl;
    kalmanGain.Print();
#endif
}

void HKalSystem::filterJoseph(Int_t iSite) {
    // This filter method uses the Joseph stabilized update of the covariance matrix.
    //
    // K_k = C^k-1_k * H^T * (V_k + H_k * C^k-1_k * H_k^T)^-1
    // C^k = (I - K_k * H) * C^k-1_k * (I - K_k * H)^T + K_k * V_k * K_k^T
    //
    // This method is numerically more stable than the conventional or Swerling
    // formulations of the Kalman equations.
    // It guarantees that the updated covariance matrix is positive definite if
    // the covariance from the prediction step and the measurement noise covariance
    // are also positive definite.

    Kalman::coordSys sys = getFilterInCoordSys();
    HKalTrackSite *site  = getSite(iSite);

    // Predicted state vector for this site.
    const TVectorD &predState  = site->getStateVec(kPredicted, sys); // a^k-1_k
    TVectorD projMeasVec(getMeasDim());
    // Expected measurement vector.
    calcMeasVecFromState(projMeasVec, site, Kalman::kPredicted, sys);  // h_k(a^k-1_k)

    // Residual of measurement vector and expected measurement vector.
    TVectorD residual = site->getHitVec() - projMeasVec;

    const TMatrixD &errMat     = site->getErrMat();                             // V_k
    const TMatrixD &predCov    = site->getStateCovMat(Kalman::kPredicted, sys); // C^k-1_k
    const TMatrixD &fProj      = site->getStateProjMat(Kalman::kFiltered, sys); // H_k
    const TMatrixD  fProjTrans = TMatrixD(TMatrixD::kTransposed, fProj);

    TMatrixD R(errMat + fProj * predCov * fProjTrans);

#if kalDebug > 0
    if(!HKalMatrixTools::checkCond(R)) {
        if(bPrintWarn) {
            Warning("filterJospeh()", "Matrix is ill-conditioned for inversion.");
        }
    }
#endif

    //       K_k        = C^k-1_k * H^T        * (V_k + H_k * C^k-1_k * H_k^T)^-1
    TMatrixD kalmanGain = predCov * fProjTrans * R.Invert();

    //                (I - K_k * H_k)
    TMatrixD covUpdate(TMatrixD(TMatrixD::kUnit,TMatrixD(getStateDim(sys),getStateDim(sys))) - kalmanGain * fProj);
    TMatrixD covUpdateT(TMatrixD::kTransposed, covUpdate);
    //       C^k     = (I - K_k * H) * C^k-1_k * (I - K_k * H)^T + K_k        * V_k    * K_k^T
    TMatrixD filtCov = covUpdate     * predCov * covUpdateT      + kalmanGain * errMat * TMatrixD(TMatrixD::kTransposed, kalmanGain);
    if(!filtCov.IsSymmetric()) {
        if(!HKalMatrixTools::makeSymmetric(filtCov)) {
            if(bPrintWarn) {
                Warning("filterJoseph()", "Covariance matrix for filter step is not close to being symmetric.");
            }
        }
    }
    site->setStateCovMat(kFiltered, filtCov, sys);

    // Calculate filtered state vector.
    //       a_k       = a^k-1_k   + K_k        * (m_k     - h_k(a^k-1_k) )
    TVectorD filtState = predState + kalmanGain * residual;
    site->setStateVec(kFiltered, filtState, sys);

#if kalDebug > 2
    if(sys == Kalman::kSecCoord) {
        cout<<"Filtering in sector coordinates."<<endl;
    }
    if(sys == Kalman::kLayCoord) {
        cout<<"Filtering in virtual layer coordinates."<<endl;
    }
    cout<<"Filter with Joseph-stabilized Kalman equations."<<endl;
    cout<<"Expected state vector:"<<endl;
    predState.Print();
    cout<<"Expected measurement:"<<endl;
    projMeasVec.Print();
    cout<<"Measurement vector:"<<endl;
    site->getHitVec().Print();
    cout<<"Filtered state vector:"<<endl;
    filtState.Print();
    cout<<"Filtered covariance:"<<endl;
    filtCov.Print();
#endif
#if kalDebug > 3
    cout<<"Covariance from prediction:"<<endl;
    predCov.Print();
    cout<<"Measurement error matrix:"<<endl;
    errMat.Print();
    cout<<"Projector matrix:"<<endl;
    fProj.Print();
    cout<<"Kalman gain:"<<endl;
    kalmanGain.Print();
#endif
}

void HKalSystem::filterSequential(Int_t iSite) {
    // Implements the sequential Kalman filter..
    // May only be used if:
    // the measurement noise matrix is diagonal or it is constant
    // and the projection function is h(a) = H * a.
    //
    // The advantage of this method is that is avoids matrix inversions.

    Kalman::coordSys sys = getFilterInCoordSys();
    HKalTrackSite *site = getSite(iSite);

    Int_t sdim = getStateDim();
    Int_t mdim = getMeasDim();

    const TMatrixD &errMat    = site->getErrMat();
    const TVectorD &measVec   = site->getHitVec();

    TMatrixD filtCov   = site->getStateCovMat(Kalman::kPredicted, sys);
    TVectorD filtState = site->getStateVec(Kalman::kPredicted, sys);

    const TMatrixD &fProj   = site->getStateProjMat(Kalman::kFiltered, sys);

#if kalDebug > 2
    if(sys == Kalman::kSecCoord) {
        cout<<"Filtering in sector coordinates."<<endl;
    }
    if(sys == Kalman::kLayCoord) {
        cout<<"Filtering in virtual layer coordinates."<<endl;
    }
    cout<<"Filter with sequential Kalman filter."<<endl;
    cout<<"Expected state vector:"<<endl;
    filtState.Print();
    cout<<"Expected measurement:"<<endl;
    (fProj * filtState).Print();
    cout<<"Measurement vector:"<<endl;
    site->getHitVec().Print();
#endif
#if kalDebug > 3
    cout<<"Measurement error matrix:"<<endl;
    errMat.Print();
    cout<<"Projector matrix:"<<endl;
    fProj.Print();
#endif

    TMatrixD kalmanGain(sdim, 1);
    TMatrixD I(sdim, sdim);
    I.UnitMatrix();

    Double_t hrow[sdim];
    TMatrixD H(1, sdim);
    TMatrixD Ht(sdim, 1);

    // The main idea of the sequential filter is to process the components of
    // the measurement vector one at a time.
    //
    // Literature:
    // Optimal State Estimation
    // by Dan Simon
    // Chapter 6.1.: Sequential Kalman filtering

    for(Int_t iMeas = 0; iMeas < mdim; iMeas++) {
        fProj.ExtractRow(iMeas, 0, hrow);
        H .Use(0, 0, 0, H.GetNcols()-1, hrow);
        Ht.Use(0, Ht.GetNrows()-1, 0, 0, hrow);

        Double_t alpha = (H * filtCov * Ht)(0,0) + errMat(iMeas, iMeas);
        kalmanGain = (1./alpha) * filtCov * Ht;
        Double_t residual = (measVec(iMeas) - (H * filtState)(0));
        for(Int_t i = 0; i < filtState.GetNrows(); i++) {
            filtState(i) += kalmanGain(i, 0) * residual;
        }
        filtCov = (I - kalmanGain * H) * filtCov;
    }

    if(!filtCov.IsSymmetric()) {
        if(!HKalMatrixTools::makeSymmetric(filtCov)) {
            if(bPrintWarn) {
                Warning("filterJoseph()", "Covariance matrix for filter step is not close to being symmetric.");
            }
        }
    }

    // Store results.
    site->setStateCovMat(Kalman::kFiltered, filtCov, sys);
    site->setStateVec(Kalman::kFiltered, filtState, sys);

#if kalDebug > 2
    cout<<"Filtered state vector:"<<endl;
    filtState.Print();
    cout<<"Filtered covariance:"<<endl;
    filtCov.Print();
#endif
}

void HKalSystem::filterSwerling(Int_t iSite) {
    // Swerling calculation of covariance matrix for filtered state and the Kalman gain matrix.
    //
    // C_k = [ (C^k-1_k)^-1 + H^T_k * (V_k)^-1 * H_k ]^-1
    // K_k = C_k * H^T_k * (V_k)^-1
    //
    // This form is computationally more complex than the conventional or Joseph forms of the Kalman filter.
    // It requires two inversions of matrices with the dimension of the state vector and one inversion
    // of a matrix with the dimension of the measurement vector.

    Kalman::coordSys sys = getFilterInCoordSys();
    HKalTrackSite *site  = getSite(iSite);

    // Predicted state vector for this site.
    const TVectorD &predState  = site->getStateVec(kPredicted, sys);         // a^k-1_k

    TVectorD projMeasVec(getMeasDim());
    // Expected measurement vector.
    calcMeasVecFromState(projMeasVec, site, Kalman::kPredicted, sys);        // h_k(a^k-1_k)

    // Residual of measurement vector and expected measurement vector.
    TVectorD residual = site->getHitVec() - projMeasVec;

    const TMatrixD &predCov = site->getStateCovMat(Kalman::kPredicted, sys); // C^k-1_k
    const TMatrixD  predCovInv(TMatrixD::kInverted, predCov);

    const TMatrixD errInv(TMatrixD::kInverted, site->getErrMat());           // (V_k)^-1

    const TMatrixD &fProj   = site->getStateProjMat(Kalman::kFiltered, sys); // H_k
    const TMatrixD fProjTrans = TMatrixD(TMatrixD::kTransposed, fProj);

    //       C_k        = [                           (C^k-1_k)^-1 + H^T_k      * (V_k)^-1 * H_k  ]^-1
    TMatrixD filtCov    = TMatrixD(TMatrixD::kInverted, predCovInv + fProjTrans * errInv   * fProj);

    if(!filtCov.IsSymmetric()) {
        if(!HKalMatrixTools::makeSymmetric(filtCov)) {
            if(bPrintWarn) {
                Warning("filterSwerling()", "Covariance matrix for filter step is not close to being symmetric.");
            }
        }
    }

    site->setStateCovMat(kFiltered, filtCov, sys);

    // Kalman gain matrix.
    //       K_k        = C_k     * H^T_k      * G_k
    TMatrixD kalmanGain = filtCov * fProjTrans * errInv;

    // Calculate filtered state vector.
    //       a_k       = a^k-1_k   + K_k        * (m_k - h_k(a^k-1_k) )
    TVectorD filtState = predState + kalmanGain * residual;
    site->setStateVec(kFiltered, filtState, sys);

#if kalDebug > 2
    if(sys == Kalman::kSecCoord) {
        cout<<"Filtering in sector coordinates."<<endl;
    }
    if(sys == Kalman::kLayCoord) {
        cout<<"Filtering in virtual layer coordinates."<<endl;
    }
    cout<<"Filter with Swerling Kalman equations."<<endl;
    cout<<"Expected state vector:"<<endl;
    predState.Print();
    cout<<"Expected measurement:"<<endl;
    projMeasVec.Print();
    cout<<"Measurement vector:"<<endl;
    site->getHitVec().Print();
    cout<<"Filtered state vector:"<<endl;
    filtState.Print();
    cout<<"Filtered covariance:"<<endl;
    filtCov.Print();
#endif
#if kalDebug > 3
    cout<<"Covariance from prediction:"<<endl;
    predCov.Print();
    cout<<"Measurement error matrix:"<<endl;
    site->getErrMat().Print();
    cout<<"Projector matrix:"<<endl;
    fProj.Print();
    cout<<"Kalman gain:"<<endl;
    kalmanGain.Print();
#endif

}

void HKalSystem::covUpdateUD(Int_t iFromSite, Int_t iToSite) {
    // Update of the covariance matrix' U and D factors after the prediction step.
    //
    // Only the matrix upper triangular U with the elements of D on its diagonal
    // are stored in the site.
    //
    // Literature:
    // Kalman Filtering: Theory and Practice using MATLAB, 3rd edition
    // Chapter 6.5: Square Root and UD Filters
    // Mohinder S. Grewal, Angus P. Andrews
    // and
    // Catherine L. Thornton, Triangular covariance factorizations for Kalman filtering,
    // Ph.D. thesis, University of California at Los Angeles, 1976

    HKalTrackSite *fromSite = getSite(iFromSite);
    HKalTrackSite *toSite   = getSite(iToSite);

    Int_t sdim = getStateDim();

    TMatrixD Uin(sdim, sdim);
    TMatrixD Din(sdim, sdim);
    const TMatrixD &covPrev = fromSite->getStateCovMat(Kalman::kFiltered);
    HKalMatrixTools::resolveUD(Uin, Din, covPrev);
    TMatrixD predCov(sdim, sdim);

    const TMatrixD &fProp = fromSite->getStatePropMat(Kalman::kFiltered);
    TMatrixD propU = fProp * Uin;

    TMatrixD  fProc       = fromSite->getStateProcMat(Kalman::kFiltered);

    TMatrixD UQ(fProc.GetNrows(), fProc.GetNcols());
    TMatrixD DQ(fProc.GetNrows(), fProc.GetNcols());
    HKalMatrixTools::resolveUD(UQ, DQ, fProc);

#if kalDebug > 0
    if(fProp.GetNrows() != covPrev.GetNrows() || fProp.GetNcols() != covPrev.GetNcols()) {
        ::Error("covUpdateUD()",
                Form("Dimensions of covariance and propagator matrix do not match. Covariance is a %ix%i matrix while propagator is %ix%i.",
                    covPrev.GetNrows(), covPrev.GetNcols(), fProp.GetNrows(), fProp.GetNcols()));
    }
    if(fProc.GetNrows() != covPrev.GetNrows() || fProc.GetNcols() != covPrev.GetNcols()) {
        ::Error("covUpdateUD()",
                Form("Dimensions of covariance and process noise matrix do not match. Covariance is a %ix%i matrix while process noise is %ix%i.",
                    covPrev.GetNrows(), covPrev.GetNcols(), fProc.GetNrows(), fProc.GetNcols()));
    }
#endif

    for(Int_t i = sdim - 1; i >= 0; i--) {
        Double_t sigma = 0.;
        for(Int_t j = 0; j < sdim; j++) {
            sigma += propU(i,j) * Din(j,j) * propU(i,j);
            sigma += UQ(i,j) * DQ(j,j) * UQ(i,j);
        }
        predCov(i,i) = sigma;
        for(Int_t j = 0; j < i; j++) {
            sigma = 0.;
            for(Int_t k = 0; k < sdim; k++) {
                sigma += propU(i,k) * Din(k,k) * propU(j,k);
                sigma += UQ(i,k) * fProc(k,k) * UQ(j,k);
            }
            predCov(j,i) = sigma / predCov(i,i);
            for(Int_t k = 0; k < sdim; k++) {
                propU(j,k) -= predCov(j,i) * propU(i,k);
            }
            for(Int_t k = 0; k < sdim; k++) {
                UQ(j,k) -= predCov(j,i) * UQ(i,k);
            }
        }
    }

    toSite->setStateCovMat(Kalman::kPredicted, predCov);
}

void HKalSystem::filterUD(Int_t iSite) {
    // Update of the covariance matrix and state vector for the filter step
    // using the U-D filter. A UD decomposition of the covariance matrix
    // is used and only the U and D matrices are propagated.
    //
    // This method increases the numerical precision of the Kalman filter.
    // It cannot be used together with the annealing filter.
    //
    // Literature:
    // Optimal State Estimation: Kalman, H infinity and Nonlinear Approaches
    // Chapter 6.4: U-D filtering
    // Dan Simon
    //
    // Kalman Filtering: Theory and Practice using MATLAB, 3rd edition
    // Chapter 6.5: Square Root and UD Filters
    // Mohinder S. Grewal, Angus P. Andrews
    //
    // G. J. Bierman, Factorization Methods for Discrete Sequential Estimation,
    // Academic Press, New York 1977

    Kalman::coordSys sys = getFilterInCoordSys();
    HKalTrackSite *site = getSite(iSite);
    Int_t sdim = getStateDim(sys);
    Int_t mdim = getMeasDim();

    const TVectorD &measVec   = site->getHitVec(); // m_k
    const TMatrixD &errMat    = site->getErrMat(); // V_k

    TVectorD projMeasVec(getMeasDim());
    const TVectorD &predState = site->getStateVec(Kalman::kPredicted, sys);
    // Expected measurement vector.
    calcMeasVecFromState(projMeasVec, site, Kalman::kPredicted, getFilterInCoordSys());  // h_k(a^k-1_k)

    // Residual.
    TVectorD resVec = measVec - projMeasVec;

    // Extract the U and D matrices.
    const TMatrixD &predCov   = site->getStateCovMat(Kalman::kPredicted, sys);
    TMatrixD U(sdim, sdim);
    TMatrixD D(sdim, sdim);
    HKalMatrixTools::resolveUD(U, D, predCov);

    const TMatrixD &fProj   = site->getStateProjMat(Kalman::kFiltered, sys); // H_k

    TVectorD filtState = predState;

    // Algorithm from the CD supplied with Grewal's and Andrews' book
    // Kalman Filtering: Theory and Practice Using Matlab.
    // Chapter 6, File: bierman.m

    // Process one component of the measurement vector at at time.
    for(Int_t iMeas = 0; iMeas < mdim; iMeas++) {
        // Get row number iMeas from the projector matrix.
        Double_t hrow[sdim];
        fProj.ExtractRow(iMeas, 0, hrow);
        TMatrixD H(1, sdim, hrow);

        Double_t residual = measVec(iMeas) - (H * filtState)(0);

        // U^T * H^T
        TMatrixD UtHt(TMatrixD(TMatrixD::kTransposed, U) * H.Transpose(H));

        TVectorD kalmanGain(sdim, (D * UtHt).GetMatrixArray());

        Double_t alpha = errMat(iMeas, iMeas);
        Double_t gamma = 1. / alpha;

        // Calculate U,D factors of covariance matrix and the Kalman gain.
        for(Int_t j = 0; j < sdim; j++) {
            Double_t beta = alpha;
            alpha += UtHt(j,0) * kalmanGain(j);
            Double_t lambda = - UtHt(j,0) * gamma;
            gamma = 1. / alpha;
            D(j,j) = beta * gamma * D(j,j);

            for(Int_t i = 0; i < j; i++) {
                beta = U(i,j);
                U(i,j) = beta + kalmanGain(i) * lambda;
                kalmanGain(i) += kalmanGain(j) * beta;
            }
        }
        kalmanGain *= gamma * residual;
        filtState = filtState + kalmanGain;
    }

    // Store the U and D factors of the covariance matrix in a single matrix.
    // The diagonal elements of U are always 1. Replace them with the elements
    // of D.
    for(Int_t i = 0; i < D.GetNrows(); i++) {
        U(i,i) = D(i,i);
    }

    site->setStateCovMat(Kalman::kFiltered, U, sys);
    site->setStateVec(Kalman::kFiltered, filtState, sys);

#if kalDebug > 2
    if(sys == Kalman::kSecCoord) {
        cout<<"Filtering in sector coordinates."<<endl;
    }
    if(sys == Kalman::kLayCoord) {
        cout<<"Filtering in virtual layer coordinates."<<endl;
    }
    cout<<"Filter with UD Kalman equations."<<endl;
    cout<<"Expected state vector:"<<endl;
    predState.Print();
    cout<<"Expected measurement:"<<endl;
    projMeasVec.Print();
    cout<<"Measurement vector:"<<endl;
    site->getHitVec().Print();
    cout<<"Filtered state vector:"<<endl;
    filtState.Print();
    cout<<"U and D factors of filtered covariance:"<<endl;
    U.Print();
#endif
#if kalDebug > 3
    cout<<"U and D factors of covariance from prediction:"<<endl;
    predCov.Print();
    cout<<"Measurement error matrix:"<<endl;
    errMat.Print();
    cout<<"Projector matrix:"<<endl;
    fProj.Print();
#endif

}

void HKalSystem::updateChi2Filter(Int_t iSite) {
    // Update chi^2 after having filtered site number iSite.

    Kalman::coordSys sys = getFilterInCoordSys();
    HKalTrackSite *site = getSite(iSite);

    Int_t mdim = getMeasDim();

    // Residual between predicted and filtered state vectors.
    const TVectorD &predState = site->getStateVec(Kalman::kPredicted, sys);
    const TVectorD &filtState = site->getStateVec(Kalman::kFiltered, sys);
    TVectorD diffPredFilt = filtState - predState;
    // Need a TMatrixD object later.
    TMatrixD diffPredFiltTrans(1, getStateDim(sys), diffPredFilt.GetMatrixArray()); // (a_k - a^k-1_k )^T

    TMatrixD predCov   = site->getStateCovMat(Kalman::kPredicted, sys); // C^k-1_k
    if(filtType == Kalman::kKalUD) {
        Int_t sdim = getStateDim(sys);
        TMatrixD U(sdim, sdim);
        TMatrixD D(sdim, sdim);
        HKalMatrixTools::resolveUD(U, D, predCov);
        predCov = U * D * TMatrixD(TMatrixD::kTransposed, U);
    }
    TMatrixD predCovInv(TMatrixD::kInverted, predCov);

    const TMatrixD &errMat    = site->getErrMat(); // V_k
    const TMatrixD  errInv(TMatrixD::kInverted, errMat);

    TVectorD projFiltMeasVec(mdim);
    // Expected measurement from filter step.
    calcMeasVecFromState(projFiltMeasVec, site, Kalman::kFiltered, sys);

    TVectorD diffMeasFilt = site->getHitVec() - projFiltMeasVec;

    // Transform TVectorD objects into matrices for later matrix multiplications.
    TMatrixD diffMeasFiltTrans(1, mdim , diffMeasFilt.GetMatrixArray()); // (m_k - a_k )^T.

    // Update chi2.
    //                   (a_k - a^k-1_k  )^T * (C^k-1_k)^-1 * (a_k - a^k-1_k)
    Double_t chi2Inc =   (diffPredFiltTrans  * predCovInv   * diffPredFilt  )(0)
                   //    (m_k - h_k(a_k))^T  * (V_k)^-1     * (m_k - h_k(a_k)   )
                       + (diffMeasFiltTrans  * errInv       * diffMeasFilt      )(0);

    chi2 += chi2Inc;
#if kalDebug > 2
    cout<<"Site "<<iSite<<" added "<<chi2Inc<<" to chi2."<<endl;
    cout<<"Residual predicted - filtered state vectors: "<<endl;
    diffPredFilt.Print();
    cout<<"Residual measurement - measurement from filtered state: "<<endl;
    diffMeasFilt.Print();
#endif
}

Kalman::coordSys HKalSystem::getFilterInCoordSys() const {
    if(hitType == Kalman::kWireHit) {
        return Kalman::kLayCoord;
    }
    return Kalman::kSecCoord;
}

//  -----------------------------------
//  Implementation of public methods
//  -----------------------------------

void HKalSystem::calcMeasVecFromState(TVectorD &projMeasVec, const HKalTrackSite *site,
                                      Kalman::kalFilterTypes stateType, Kalman::coordSys sys, Int_t iHit) const {
    // Implements the projection function h(a) that calculates a measurement vector
    // from a track state.
    //
    // Output:
    // projMeasVec: The calculated measurement vector.
    // Input:
    // site:      Pointer to current site of the track.
    // stateType: Which state (predicted, filtered, smoothed) to project on a measurement vector.
    // sys:       Coordinate system to use (sector or virtual layer coordinates)
    // iHit:      Index of the competitor in the measurement hit.

    Int_t mdim = getMeasDim();
#if kalDebug > 0
    if(projMeasVec.GetNrows() != mdim) {
        Warning("calcMeasVec", Form("Needed to resize TVectorD. Dimension of measurement vector (%i) does not match that of function parameter (%i).", mdim, projMeasVec.GetNrows()));
        projMeasVec.ResizeTo(mdim);
    }
#endif

    //Int_t sys = getFilterInCoordSys();

    const TVectorD &sv = site->getStateVec(stateType, sys);

    // Measurement vector is x and y coordinates of the track state for segment hits.
    if(getHitType() == Kalman::kSegHit) {
        if(mdim == 2) {
            projMeasVec(0) = sv(kIdxX0);
            projMeasVec(1) = sv(kIdxY0);
        } else {
            projMeasVec = site->getStateProjMat() * sv;
        }
    }

    if(getHitType() == Kalman::kWireHit) {
        if(mdim == 1) {
            if(sys == Kalman::kSecCoord) {
                // State vector is in sector coordinates.
                // The projection of the track state onto a measurement vector is
                // the insection of a straight line defined by the track position
                // and direction with the drift chamber cell.

                TVector3 posAt;
                HKalTrackState::calcPosAtPlane(posAt, site->getHitVirtPlane(), sv);
                TVector3 dir;
                HKalTrackState::calcDir(dir, sv);

                Double_t mindist, alpha;
                Int_t s = site->getSector();
                Int_t m = site->getModule();
                Int_t l = site->getLayer();
                Int_t c = site->getCell(iHit);
                getImpact(alpha, mindist, posAt, dir, s, m, l, c);

                projMeasVec(0) = mindist;
            } else {
                // State vector is in virtual layer coordinates.
                // The second component corresponds to a drift radius.
                projMeasVec(0) = sv(kIdxY0);
            }
        } else {
            Error("calcMeasVecFromState()", Form("Wire hit has dimension %i", mdim));
        }
    }
}

void HKalSystem::calcProjector(HKalTrackSite *site) {
    // The projection function h(a) calculates a measurement vector from a track state a.
    // The projector matrix is the Jacobian of this function, i.e. @h(a)/@a.
    // The projector matrix is stored in the site's filtered track state.

    Kalman::coordSys sys = getFilterInCoordSys(); // sector or virtual layer coordinates

    TMatrixD fProj(getMeasDim(), getStateDim(sys));

    fProj.Zero();

    // For segment hits, x0 and y0 are coordinates of the measurement
    // in the sector system.
    if(getHitType() == Kalman::kSegHit) {
        fProj(0, kIdxX0) = 1.;
        fProj(1, kIdxY0) = 1.;
    }
    if(getHitType() == Kalman::kWireHit) {
        if(sys == Kalman::kSecCoord) {

            // Matrix that transforms from track coordinate system (sector coordinates
            // rotated in initial track direction) to sector coordinate system.
            TRotation transSec = pRotMat->Inverse();


            HMdcSizesCells *fSizesCells = HMdcSizesCells::getExObject();  // check if is already there
            if(!fSizesCells) {
                fSizesCells = HMdcSizesCells::getObject();
                fSizesCells->initContainer();
            }

            Int_t s = site->getSector();
            Int_t m = site->getModule();
            Int_t l = site->getLayer();
            Int_t c = site->getCell();
            HMdcSizesCellsLayer  &fSizesCellsLayer  = (*fSizesCells)[s][m][l];

            // Rotation matrix from sector coordinate system to  rotated layer system: array indices 0-8.
            // Translation vector from layer coordinate system to rotated layer system: array indices 9-11.
            const Double_t *transRotLayArr = fSizesCellsLayer.getRLSysRSec(c);

            // Track state has to be transformed from track to sector and then from sector to
            // rotated layer coordinates.
            // Calculate the composition R of both rotations.
            // For the projector matrix, only the matrix elements for the calculation
            // of the v component matter.
            Double_t R1 = transRotLayArr[1] * transSec(0,0) + transRotLayArr[4] * transSec(1,0) + transRotLayArr[7] * transSec(2,0);
            Double_t R4 = transRotLayArr[1] * transSec(0,1) + transRotLayArr[4] * transSec(1,1) + transRotLayArr[7] * transSec(2,1);
            Double_t R7 = transRotLayArr[1] * transSec(0,2) + transRotLayArr[4] * transSec(1,2) + transRotLayArr[7] * transSec(2,2);

            // Wire coordinate system:
            // Axis U points along wire.
            // Axis V is on layer and perpendicular to wire.
            // Axis W is perpendicular to U and V.

            //     W
            //
            //     ^
            //     |  |------------*----|
            //     |  | cell        *  -|---ThetaV
            //     |  |            / *  |
            //     |  |           /90 * |
            //     |  | driftDist/     *|
            //     |  |         /       *
            //   --|--|--------*^-|-----|*---------> V
            //     |  |      Wire |     | *
            //     |  |           |     |  *
            //        |         ThetaV  |   *
            //        |                 |    *
            //        |-----------------|     * track

            // Projection function:
            // h(a) = driftDist = v * cos(ThetaV) = v / sqrt(1 + tan(ThetaV)^2)

            // In order to calculate the drift radius from a track state with position vector (x,y,z)
            // on the measurement layer and track direction tangents tx and ty, transform the
            // position and direction vectors to the wire coordinate system.

            const TVectorD &sv = site->getStateVec(Kalman::kPredicted);

            // Transform track position to wire system. v is component on the layer that is
            // perpendicular to the wire.
            TVector3 posAt;
            HKalTrackState::calcPosAtPlane(posAt, site->getHitMeasLayer(), sv);

            const TVector3 &n = site->getHitMeasLayer().getNormal();

            // Rotate from track to sector coordinates.
            if(rotCoords != Kalman::kNoRot) {
                posAt.Transform(transSec);
            }
            // Calculate position in wire coordinates.
            Double_t v, w;
            //v = R1 * posAt.X() + R4 * posAt.Y() + R7 * posAt.Z() - (fSizesCellsLayer[c]).getWirePos()
            //    - (transRotLayArr[1]*transRotLayArr[9] + transRotLayArr[4]*transRotLayArr[10] + transRotLayArr[7]*transRotLayArr[11]);
            fSizesCellsLayer.getYZinWireSys(posAt.X(), posAt.Y(), posAt.Z(), c, v, w);
            Double_t signV = (v < 0.) ? -1. : 1.;
            // No distinction between hits left and right of the wire.
            v = TMath::Abs(v);

            // Transform track direction vector (tx,ty,1).
            // tv = Tan(ThetaV) = Tan(pv / pw).
            Double_t tv = R1 * sv(kIdxTanPhi) + R4 * sv(kIdxTanTheta) + R7;

            // cosine of angle ThetaV.
            // cos(alpha)  = 1. / sqrt(1 + tan(alpha)^2)
            Double_t cv  = 1. / TMath::Sqrt (1. + TMath::Power(tv, 2.));
            Double_t cv3 = 1. / TMath::Power(1. + TMath::Power(tv, 2.), 1.5);

            // Projector matrix @h(a)/@a
            fProj(0, kIdxX0)       = signV * (R1 - R7 * n.X()/n.z()) * cv;
            fProj(0, kIdxY0)       = signV * (R4 - R7 * n.Y()/n.Z()) * cv;
            fProj(0, kIdxTanPhi)   = - v * R1 * tv * cv3;
            fProj(0, kIdxTanTheta) = - v * R4 * tv * cv3;
            fProj(0, kIdxQP)       = 0.;

        } else {
            // Using virtual layer coordinate system for the filter step.
            // In this system axis U points along the wire and axis V from the wire to
            // the point of closest approach of the track to the wire.
            // The track state's component along the V axis corresponds to a drift radius.
            fProj(0, kIdxY0) = 1.;
        }
    }

    // Store result.
    site->setStateProjMat(Kalman::kFiltered, fProj, sys);
}

Bool_t HKalSystem::checkCovMat(const TMatrixD &fCov) const {
    // Covariance matrices must be positive definite, i.e. symmetric and
    // all its eigenvalues are real and positive. Returns true if this
    // is the case.

    Bool_t   bCovOk  = kTRUE;

    // Diagonal elements should not be zero.
    for(Int_t i = 0; i < fCov.GetNrows(); i++) {
        if(fCov(i, i) == 0.) {
            if(bPrintWarn) {
                Warning("checkCovMat()", "A diagonal elment of the covariance matrix is 0.");
            }
            bCovOk = kFALSE;
        }
    }

    // Covariance matrix should be symmetric.
    if(filtType != Kalman::kKalUD) {
        if(!fCov.IsSymmetric()) {
            if(bPrintWarn) {
                Warning("checkCovMat()", "Covariance matrix is not symmetric.");
            }
            bCovOk = kFALSE;
        }
    }

    if(filtType != Kalman::kKalUD) {
        if(!HKalMatrixTools::checkIsPosDef(fCov)) {
            if(bPrintWarn) {
                Warning("checkCovMat()", "Covariance matrix is not positive definite.");
            }
            bCovOk = kFALSE;
        }
    } else {
        TMatrixD U(fCov.GetNrows(), fCov.GetNcols());
        TMatrixD D(fCov.GetNrows(), fCov.GetNcols());
        HKalMatrixTools::resolveUD(U, D, fCov);
        TMatrixD UD = U * D * TMatrixD(TMatrixD::kTransposed, U);
        if(!HKalMatrixTools::checkIsPosDef(UD)) {
            if(bPrintWarn) {
                Warning("checkCovMat()", "Covariance matrix is not positive definite.");
            }
            bCovOk = kFALSE;
        }
    }

    return bCovOk;
}

Double_t HKalSystem::getNdf() const {
    // Returns number degrees of freedom.
    //
    // ndf = sum(p_i,j * mdim) - sdim.
    // i:     Index of measurement layer.
    // j:     Index of competitor on layer i.
    // p_i,j: Assignment probability of competitor j on layer i.
    // mdim:  Dimension of measurement vector.
    // sdim:  Dimension of track state vector.

    Double_t ndf = 0.;
    Int_t mdim   = getMeasDim();
    Int_t sdim   = getStateDim();

    if(bDoDaf) {
        for(Int_t i = 0; i < getNsites(); i++) {
            HKalTrackSite *site = getSite(i);
            ndf += site->getHitsTotalWeight();
        }
        ndf *= mdim;
        ndf -= sdim;
    } else {
        // Didn't use the annealing filter. Each measurement layer only has one hit.
        ndf = getNsites() * mdim - sdim;
    }
    return ndf;
}

Bool_t HKalSystem::getImpact(Double_t& alpha, Double_t& mindist, const TVector3 &pos, const TVector3 dir,
                             Int_t sec, Int_t mod, Int_t lay, Int_t cell) const {
    // Calculates impact angle and drift distance of a straight line defined
    // by position and direction with an MDC cell. Returns true if the line
    // intersects the drift cell, otherwise false.
    //
    // Output:
    // alpha:   Impact angle, 0 <= alpha <= 90°.
    // mindist: Drift distance.
    //
    // Input:
    // pos: track position.
    // dir: track direction.
    // sec/mod/lay/cell: Sector, module, layer, drift cell number.

    TVector3 pos1 = pos;
    TVector3 pos2 = pos + 0.5 * dir;

    // getImpact() from fSizesCellsLayer expects sector coordinates.
    if(rotCoords != Kalman::kNoRot) {
        TRotation rotInv = pRotMat->Inverse();
        pos1.Transform(rotInv);
        pos2.Transform(rotInv);
    }

    HMdcSizesCells *fSizesCells = HMdcSizesCells::getExObject();  // check if is already there
    if(!fSizesCells) {
        fSizesCells = HMdcSizesCells::getObject();
        fSizesCells->initContainer();
    }

    HMdcSizesCellsLayer  &fSizesCellsLayer  = (*fSizesCells)[sec][mod][lay];

    // Calculate the impact angle alpha of the track with the drift cell.
    return (fSizesCellsLayer.getImpact(pos1.x(), pos1.y(), pos1.z(), pos2.x(), pos2.y(), pos2.z(),
                                       cell, alpha, mindist));
}

Bool_t HKalSystem::fitTrack(const TObjArray &hits, const TVectorD &iniSv, const TMatrixD &iniCovMat,
                            Int_t pId) {
    // Runs the Kalman filter over a set of measurement hits.
    // Returns false if the Kalman filter encountered problems during
    // track fitting or the chi^2 is too high.
    //
    // Input:
    // hits:        the array with the measurement hits. Elements must be of class
    //              HKalMdcHit.
    // iniStateVec: initial track state that the filter will use.
    // iniCovMat:   initial covariance matrix that the filter will use.
    // pId:         Geant particle id.
    // momGeant:    Geant momentum that will be written in the output tree. Ignore if this is not simulation data.

#if kalDebug > 1
    cout<<"******************************"<<endl;
    cout<<"**** Fit track number "<<trackNum<<" ****"<<endl;
    cout<<"******************************"<<endl;
#endif

    // Reset variables.
    bTrackAccepted = kTRUE;
    chi2 = 0.;
    fieldTrack.Clear();
    pointsTrack.Clear();
    trackLength = 0.;

    if(iniStateVec.GetNrows() != iniSv.GetNrows()) {
        iniStateVec.ResizeTo(iniSv.GetNrows());
        if(bPrintWarn) {
            Warning("fitTrack()", Form("Passed initial state vector has dimension %i. Class member of HKalSystem has dimension %i.",
                                       iniSv.GetNrows(), iniStateVec.GetNrows()));
        }
    }
    iniStateVec = iniSv;

    // Add new hits to measurement sites.
    updateSites(hits);
    pid = pId;

    // Depending on the direction for propagation, set the index of the first site (istart)
    // and last site (igoal).
    // idir: increment (forward propagation) or decrement (backward propagation) sites indices.
    Int_t fromSite, toSite;
    if(getDirection() == kIterForward) {
        fromSite = 0;
        toSite   = getNsites() - 1;
    } else {
        fromSite = getNsites() - 1;
        toSite   = 0;
    }

    // Store the state vector and covariance matrix for the first site.
    HKalTrackSite *first = getSite(fromSite);

    first->setStateVec   (kPredicted, iniStateVec);
    first->setStateVec   (kFiltered,  iniStateVec);
    if(filtType == Kalman::kKalUD) {
        TMatrixD iniCovUD = iniCovMat;
        if(!HKalMatrixTools::decomposeUD(iniCovUD)) {
            Warning("fitTrack()","Could not decompose initial covariance matrix into UD factors.");
        }
        first->setStateCovMat(kPredicted, iniCovUD);
        first->setStateCovMat(kFiltered , iniCovUD);
    } else {
        first->setStateCovMat(kPredicted, iniCovMat);
        first->setStateCovMat(kFiltered , iniCovMat);
    }

    first->transSecToVirtLay(Kalman::kPredicted, (filtType == kKalUD));
    first->transSecToVirtLay(Kalman::kFiltered, (filtType == kKalUD));

    // Tilt coordinate system.
    // There are three options:
    // 1. kNoRot:
    //    Don't tilt the coordinate system.
    // 2. kVarRot:
    //    Tilt the coordinate system so that the z-axis will always
    //    point in the initial track direction. Assuming track curvature
    //    is not large this ensures that the track state parameters
    //    tx = tan(p_x / p_z) and ty = tan(p_y / p_z) remain small.
    //    Angles close to 90° would result in large values for tx and ty
    //    due to the tangens and poor results for the Kalman filter.
    if(rotCoords == Kalman::kVarRot) {
        // Calculate rotation matrix depending on track direction.
        TVector3 dir;
        HKalTrackState::calcDir(dir, iniStateVec);
        Double_t rotXangle = dir.Theta();
        Double_t rotYangle = - TMath::Pi()/2 + dir.Phi();
        setRotationAngles(rotXangle, rotYangle);
    }
    if(rotCoords == Kalman::kVarRot) {
        // Rotate coordinates of all sites.
        transformFromSectorToTrack();
    }

    // Propagation dir may change when doing several Kalman Filter iterations.
    Bool_t orgDirection = getDirection();

    // At least one site has competing hits. Run the Deterministic Annealing Filter.
    if(bDoDaf) {
        bTrackAccepted &= fitWithDaf();

        // Fit with standard Kalman Filter.
    } else {
        // Run the prediction and filter steps.
        for(Int_t i = 0; i < nKalRuns; i++) {
            chi2 = 0.;

            bTrackAccepted &= predictAndFilter(fromSite, toSite);

            if(i < nKalRuns-1) { // At least one more iteration to do.
                // Set the filtered state of the last site from the previous iteration as the
                // prediction for the next iteration.
                getSite(toSite)->setStateVec(kPredicted, getSite(toSite)->getStateVec(kFiltered));
                //getSite(toSite)->setStateCovMat(kPredicted , getSite(toSite)->getStateCovMat(kFiltered));

                // Change propagation direction in each iteration.
                setDirection(!direction);
                Int_t oldTo = toSite;
                toSite      = fromSite;
                fromSite    = oldTo;
            }
        }
        // Do smoothing.
        if(bDoSmooth) {
            // Transform covariances from UD format.
            if(filtType == Kalman::kKalUD) {
                for(Int_t iSite = 0; iSite < getNsites(); iSite++) {
                    HKalTrackSite *site = getSite(iSite);
                    TMatrixD cov = site->getStateCovMat(Kalman::kPredicted);
                    TMatrixD U(cov.GetNrows(), cov.GetNcols());
                    TMatrixD D(cov.GetNrows(), cov.GetNcols());
                    HKalMatrixTools::resolveUD(U, D, cov);
                    cov = U * D * TMatrixD(TMatrixD::kTransposed, U);
                    site->setStateCovMat(Kalman::kPredicted, cov);

                    cov = site->getStateCovMat(kFiltered);
                    HKalMatrixTools::resolveUD(U, D, cov);
                    cov = U * D * TMatrixD(TMatrixD::kTransposed, U);
                    site->setStateCovMat(Kalman::kFiltered, cov);
                }
            }
            smoothAll();
        }
    }

    // Direction changes when running Kalman Filter with several iterations.
    // Restore original direction passed by user.
    setDirection(orgDirection);

    // Transform hit and state vectors from track back to sector coordinates.
    if(rotCoords == Kalman::kVarRot) {
        transformFromTrackToSector();
    }

    // Check for large chi^2 values.
    if(getChi2() > chi2High) {
        bTrackAccepted = kFALSE;
        if(bPrintWarn) {
            Error("fitTrack()", Form("High chi^2: %f.", getChi2()));
        }
    }
    trackNum++;

#if kalDebug > 1
    if(bTrackAccepted) {
        cout<<"**** Fitted track ****"<<endl;
    } else {
        cout<<"**** Errors during track fitting ****"<<endl;
    }
#endif

    return bTrackAccepted;
}

Bool_t HKalSystem::fitWithDaf() {
    // Fit track using the deterministic annealing filter.
    //
    // For details on the annealing filter see:
    //
    // R. Fruehwirth, A. Strandlie, Comput. Phys. Commun. 120 (1999) 197.
    // Track fitting with ambiguities and noise:
    // a study of elastic tracking and nonlinear filters
    //
    // Sebastian Fleischmann, Dimploma Thesis, University of Wuppertal
    // Track Reconstruction in the ATLAS Experiment - the Deterministic Annealing Filter
    // http://www.atlas.uni-wuppertal.de/diplom/WUD06-09_sebastian_fleischmann.pdf

    Bool_t bTrackOk   = kTRUE;
    const Int_t nDafs = dafT.GetSize(); // Number of DAF iterations.

    // Array that stores weights (assignment probabilities) of competing hits.
    Float_t weights[getNsites()][2][nDafs];

    Int_t fromSite, toSite;
    if(getDirection() == kIterForward) {
        fromSite = 0;
        toSite   = getNsites() - 1;
    } else {
        fromSite = getNsites() - 1;
        toSite   = 0;
    }

    for(Int_t iDaf = 0; iDaf < nDafs; iDaf++) {

#if kalDebug > 1
        cout<<"********************************************"<<endl;
        cout<<"**** DAF iteration "<< iDaf<<" with temperature "<<dafT[iDaf]<<" ****"<<endl;
        cout<<"********************************************"<<endl;
#endif

        // In each iteration do Kalman prediction/filter steps and then smooth back.
        chi2 = 0.;
        trackLength = 0.;
        bTrackOk &= predictAndFilter(fromSite, toSite);
        smoothAll();

        for(Int_t iSite = 0; iSite < getNsites(); iSite++) {
            HKalTrackSite *site = getSite(iSite);
            Double_t weightNorm = 0.;

#if kalDebug > 1
            cout<<"Running DAF for site "<<iSite<<endl;
#endif

            for(Int_t iHit = 0; iHit < site->getNcompetitors(); iHit++) {
                const TVectorD &measVec = site->getHitVec(iHit);
                TVectorD smooMeasVec(getMeasDim());
                calcMeasVecFromState(smooMeasVec, site, Kalman::kSmoothed, Kalman::kSecCoord, iHit);

#if kalDebug > 2
                cout<<"Updating measurement "<<iHit<<" of "<<site->getNcompetitors()<<endl;
                cout<<"Measurement vector: "<<endl;
                measVec.Print();
                cout<<"Expected measurement from smoothing: "<<endl;
                smooMeasVec.Print();
#endif

                TVectorD  residual     = measVec - smooMeasVec;                      // Residual r
                TMatrixD  residualTrans(1, getMeasDim(), residual.GetMatrixArray()); // r^T
                const TMatrixD &errMat = site->getErrMat(iHit);                      // Measurement error covariance V
#if kalDebug > 0
                if(errMat.NonZeros() == 0) {
                    Error("fitWithDaf()", Form("Measurement error matrix of hit %i is not set.", iHit));
                }
#endif
                TMatrixD  invErrMat    = TMatrixD(TMatrixD::kInverted, errMat);      // G = V^-1

                // Normalization of hit's chi2 distribution: 1 / ((2*pi)^n/2 * sqrt(T * det(V)))
                Double_t  normChi2     = 1. / (TMath::Power(2.*TMath::Pi(), (Double_t)getMeasDim()/2.) * TMath::Sqrt(dafT[iDaf] * errMat.Determinant()));

                // Standardized distance between smoothed track state and measurement vector:
                //        chi2         =  r^T           * (V_k)^-1  * r
                Double_t  chi2         = (residualTrans * invErrMat * residual)(0);

#if kalDebug > 2
                cout<<"Hit's chi2: "<<chi2<<endl;
#endif

                site->setHitChi2(chi2, iHit);

                // Normalization for hit's weight recalculation:
                // sum( normchi2 * exp(-chi2/(2T)) + normchi2 * exp(-chi2cut/(2T))
                weightNorm            += normChi2 * TMath::Exp(- chi2       / (2. * dafT[iDaf]));
                weightNorm            += normChi2 * TMath::Exp(- dafChi2Cut / (2. * dafT[iDaf]));
            }

            // Recalculate weight of each hit.
            for(Int_t iHit = 0; iHit < site->getNcompetitors(); iHit++) {
                const TMatrixD &errMat = site->getErrMat(iHit);
                // New weight of hit.
                // 1 / ((2*pi)^n/2 * sqrt(T * det(V))) * exp(-chi2/(2T)) / weightNorm
                Double_t normChi2      = 1. / (TMath::Power(2.*TMath::Pi(), (Double_t)getMeasDim()/2.) * TMath::Sqrt(dafT[iDaf] * errMat.Determinant()));
                Double_t hitWeight     = normChi2 * TMath::Exp(-site->getHitChi2(iHit) / (2. * dafT[iDaf])) / weightNorm;
                weights[iSite][iHit][iDaf] = hitWeight;

#if kalDebug > 2
                cout<<"Updating weight of hit "<<iHit<<" out of "<<site->getNcompetitors()<<": old weight "<<site->getHitWeight(iHit)<<", new weight "<<hitWeight<<endl;
#endif

                site->setHitWeight(hitWeight, iHit);
                site->setHitWeightHist(hitWeight, iDaf, iHit);
            }
        }

        //? print daf results
        for(Int_t iSite = 0; iSite < getNsites(); iSite++) {
            HKalTrackSite *site = getSite(iSite);
            Int_t nComp = site->getNcompetitors();
            Double_t w0 = site->getHitWeight(0);
            Double_t w1 = -1.;
            if(nComp > 1) {
                w1 = site->getHitWeight(1);
            }
            Double_t chi0 = site->getHitChi2(0);
            Double_t chi1 = -1.;
            if(nComp > 1) {
                chi1 = site->getHitChi2(1);
            }
            Double_t arr[] = {
                dafT[iDaf], iDaf, iSite, nComp, dafChi2Cut, getTrackNum(), w0, w1, chi0, chi1
            };
            hDafMonitor->Fill(arr);
        }
    }

#if kalDebug > 1
        cout<<"**** Finished with Deterministic Annealing Filter ****"<<endl;
#endif

    return bTrackOk;
}

Bool_t HKalSystem::predictAndFilter(Int_t fromSite, Int_t toSite) {
    // Do the prediction and filter steps of the Kalman filter by propagating
    // the track between the measurement sites fromSite and toSite.
    //
    // fromSite: index of track site to start propagation from
    // toSite:   index of track site to propagate to.

    Bool_t bTrackOk = kTRUE;

    HKalTrackSite *site = getSite(fromSite);
    site->calcEffMeasVec();
    calcProjector(site);

    if(hitType == Kalman::kWireHit) {
        bTrackOk = predictAndFilterWireHits(fromSite, toSite);
    } else {
        bTrackOk = predictAndFilter2Dhits  (fromSite, toSite);
    }
    return bTrackOk;
}

Bool_t HKalSystem::predictAndFilter2Dhits(Int_t fromSite, Int_t toSite) {
    // Do the prediction and filter steps of the Kalman filter by propagating
    // the track between sites for two-dimensional measurement vectors.
    //
    // fromSite: index of track site to start propagation from
    // toSite:   index of track site to propagate to.

    Bool_t propagationOk = kTRUE;

    Int_t iDir;
    if(fromSite < toSite) { // propagate in forward direction
        iDir = +1;
    } else {                // propagate in backward direction
        iDir = -1;
    }


    Double_t energyLoss = 0.;

    // Search for first active measurement site.
    Int_t iCurSite  = fromSite;        // Index of current site k-1.
    while(!getSite(iCurSite)->getIsActive()) {
        iCurSite += iDir;
        if((iDir < 0 && iCurSite < toSite) || (iDir > 0 && iCurSite > toSite)) {
            Error("predictAndFilterWireHits()", Form("No two valid sites between %i and %i.", fromSite, toSite));
            return kFALSE;
        }
    }

    Int_t iNextSite = iCurSite + iDir; // Index of next site k.

    while((iDir > 0 && iNextSite <= toSite) || (iDir < 0 && iNextSite >= toSite)) {

#if kalDebug > 1
        cout<<"##### Prediction from site "<<iCurSite<<" to site "<<iNextSite<<" #####"<<endl;
#endif

        HKalTrackSite    *curSite              = getSite(iCurSite);
        HKalTrackSite    *nextSite             = getSite(iNextSite);

        // Skip inactive sites.
        if(!nextSite->getIsActive()) {
            iNextSite += iDir;
            continue;
        }

        const TVectorD   &filtStateCurSite     = curSite ->getStateVec(kFiltered); // a_(k-1): filtered state vector of current site
        const HKalMdcMeasLayer &planeCurSite   = curSite ->getHitMeasLayer();      // Plane to propagate from.
        const HKalMdcMeasLayer &planeNextSite  = nextSite->getHitMeasLayer();      // Plane to propagate to.

        Int_t sdim = getStateDim();

#if kalDebug > 0
        Int_t nRows, nCols;
        nRows = filtStateCurSite.GetNrows();
        if(nRows != sdim) {
            Error("predictAndFilter()", Form("Filtered state vector of site %i must have dimension %i, but has dimension %i", iCurSite, nRows, sdim));
            exit(1);
        }
#endif

        TVectorD predStateNextSite(sdim);    // a^(k-1)_k: predicted state for next site, to be calculated
        TMatrixD propMatCurSite(sdim, sdim); // F_k-1: propagation matrix, to be calculated
        TMatrixD procMatCurSite(sdim, sdim); // Q_k-1: process noise matrix, to be calculated

        // Propagate track to next measurement site. Calculate the expected state vector
        // a^(k-1)_k = f(a_k-1), the Jacobian F_k-1 and process noise covariance Q_k-1.
        propagationOk &= trackPropagator.propagateToPlane(propMatCurSite, procMatCurSite,
                                                          predStateNextSite, filtStateCurSite,
                                                          planeCurSite, planeNextSite,
                                                          planeCurSite, planeNextSite,
                                                          pid, getDirection());

        // Fill arrays with RK stepping points.
        if(bFillPointArrays) {
            const TObjArray &pointsTrk = trackPropagator.getPointsTrack();
            for(Int_t i = 0; i < pointsTrk.GetEntries(); i++) {
                pointsTrack.Add(pointsTrk.At(i));
            }
            const TObjArray &fieldTrk = trackPropagator.getFieldTrack();
            for(Int_t i = 0; i < fieldTrk.GetEntries(); i++) {
                fieldTrack.Add(fieldTrk.At(i));
            }
        }

        trackLength += trackPropagator.getTrackLength();

        // Store propagation results.
        // -----------
        nextSite->setStateVec(kPredicted, predStateNextSite);  // a^(k-1)_k = f(a_k-1), store prediction for next site
        curSite ->setStatePropMat(kFiltered, propMatCurSite);  // F_k-1, store propagator matrix

        if(filtType == Kalman::kKalUD) {
            if(!HKalMatrixTools::decomposeUD(procMatCurSite)) {
                Warning("predictAndFilter2DHits()","Could not decompose process noise into UD factors.");
            }
        }
        curSite ->setStateProcMat(kFiltered, procMatCurSite);  // Q_k-1, store process noise covariance

        energyLoss += trackPropagator.getEnergyLoss();
        nextSite->setEnergyLoss(energyLoss);
        // -----------

        // Calculate covariance matrix for prediction of next site.
        // -----------

        if(filtType == Kalman::kKalUD) {
            covUpdateUD(iCurSite, iNextSite);
        } else {
            const TMatrixD &covMatCurSite = curSite->getStateCovMat(kFiltered);              // C_k-1
#if kalDebug > 0
            nRows = covMatCurSite.GetNrows();
            nCols = covMatCurSite.GetNcols();
            if(nRows != getStateDim() || nCols != getStateDim()) {
                Error(Form("Prediction to site %i.", iNextSite), Form("Covariance matrix is %ix%i, but must be %ix%i matrix.", nRows, nCols, getStateDim(), getStateDim() ));
                exit(1);
            }
#endif
            TMatrixD propMatCurSiteTrans = TMatrixD(TMatrixD::kTransposed, propMatCurSite); // F_k-1

            //       C^k-1_k        = F_k-1          * C_k-1         * F^T_k-1             + Q_k-1
            TMatrixD covMatNextSite = propMatCurSite * covMatCurSite * propMatCurSiteTrans + procMatCurSite;
            if(!covMatNextSite.IsSymmetric()) {
                if(!HKalMatrixTools::makeSymmetric(covMatNextSite)) {
                    if(bPrintWarn) {
                        Warning("predictAndFilter2DHits()", "Covariance matrix for prediction is not close to being symmetric.");
                    }
                }
            }
            nextSite->setStateCovMat(Kalman::kPredicted, covMatNextSite);
        }
        // -----------

        // Filter next site and calculate new chi2.
        if(!filter(iNextSite)) {
            Error("predictAndFilter()", Form("Failed to filter site %i. Skipping this site.", iNextSite));
            propagationOk = kFALSE;
            nextSite->setActive(kFALSE);
        } else {
            updateChi2Filter(iNextSite);
            iCurSite = iNextSite;
        }
        iNextSite += iDir;

    } // end loop through sites
    return propagationOk;
}

Bool_t HKalSystem::filter(Int_t iSite) {
    // Do the filter step in Kalman filter for a site. Can only be called after the
    // prediction step.
    // iSite: index of site to filter

    HKalTrackSite *site = getSite(iSite);

    site->calcEffMeasVec();
    calcProjector(site);

#if kalDebug > 1
    cout<<"#### Filtering site "<<iSite<<" ####"<<endl;
#endif

    const TMatrixD &predCov = site->getStateCovMat(Kalman::kPredicted);

#if kalDebug > 0
    Int_t nRows, nCols;

    Int_t mdim = getMeasDim();
    Int_t sdim = getStateDim();

    // Check dimensions.
    const TVectorD &predState = site->getStateVec(Kalman::kPredicted);
    nRows = predState.GetNrows();
    if(nRows != sdim) {
        Error(Form("Filter site %i.", iSite), Form("State vector from prediction must have dimension %i, but has dimension %i", nRows, sdim));
        exit(1);
    }

    const TVectorD &measVec = site->getHitVec();
    nRows = measVec.GetNrows();
    if(nRows != mdim) {
        Error(Form("Filter site %i.", iSite), Form("Measurement vector must have dimension %i, but has dimension %i", nRows, mdim));
        exit(1);
    }

    const TMatrixD &errMat = site->getErrMat();
    nRows = errMat.GetNrows();
    nCols = errMat.GetNcols();
    if(nRows != mdim || nCols != mdim) {
        Error(Form("Filter site %i.", iSite), Form("Measurement error matrix is %ix%i, but must be %ix%i matrix.", nRows, nCols, mdim, mdim ));
        exit(1);
    }

    // Check weight.
    if(!bDoDaf && site->getNcompetitors() == 1 && !TMath::AreEqualAbs(site->getHitWeight(0), 1., 1.e-5)) {
        Warning("filter()", Form("Site %i has no competitors, but weight of %f instead of 1.", iSite, site->getHitWeight(0)));
    }

    // Check covariance matrix.
    if(bPrintWarn && !checkCovMat(predCov)) {
        Warning("filter()", Form("Errors found in predicted covariance matrix for site %i.", iSite));
    }

#endif

    if(!HKalMatrixTools::checkValidElems(predCov)) {
        Error("filter()", Form("Predicted covariance matrix for site %i contains INFs or NaNs.", iSite));
        return kFALSE;
    }

    if(!site->getIsActive()) {
        Error("filter()", Form("Site number %i is inactive.", iSite));
        return kFALSE;
    }

    switch(filtType) {
    case Kalman::kKalConv:
        filterConventional(iSite);
        break;
    case Kalman::kKalJoseph:
        filterJoseph(iSite);
        break;
    case Kalman::kKalUD:
        filterUD(iSite);
        break;
    case Kalman::kKalSeq:
        filterSequential(iSite);
        break;
    case Kalman::kKalSwer:
        filterSwerling(iSite);
        break;
    default:
        filterConventional(iSite);
        break;
    }

    return kTRUE;
}

Bool_t HKalSystem::predictAndFilterWireHits(Int_t fromSite, Int_t toSite) {
    // Do the prediction and filter steps of the Kalman filter by propagating
    // the track between measurement sites for drift chamber hits.
    //
    // fromSite: index of track site to start propagation from
    // toSite:   index of track site to propagate to.

    Int_t sdim = getStateDim(Kalman::kSecCoord);

    Bool_t propagationOk = kTRUE;

    Int_t iDir;
    if(fromSite < toSite) { // propagate in forward direction
        iDir = +1;
    } else {                // propagate in backward direction
        iDir = -1;
    }

    Double_t energyLoss = 0.;

    // Find first active site.
    Int_t iCurSite  = fromSite;        // Index of current site k-1.
    while(!getSite(iCurSite)->getIsActive()) {
        iCurSite += iDir;
        if((iDir < 0 && iCurSite < toSite) || (iDir > 0 && iCurSite > toSite)) {
            Error("predictAndFilterWireHits()", Form("No two valid sites between %i and %i.", fromSite, toSite));
            return kFALSE;
        }
    }

    Int_t iNextSite = iCurSite + iDir; // Index of next site k to propagate to.

    while((iDir > 0 && iNextSite <= toSite) || (iDir < 0 && iNextSite >= toSite)) {

#if kalDebug > 1
        cout<<"##### Prediction from site "<<iCurSite<<" to site "<<iNextSite<<" ####"<<endl;
#endif

        HKalTrackSite    *curSite              = getSite(iCurSite);
        HKalTrackSite    *nextSite             = getSite(iNextSite);

        // Skip inactive sites.
        if(!nextSite->getIsActive()) {
            iNextSite += iDir;
            continue;
        }

        const TVectorD   &filtStateCurSite     = curSite ->getStateVec(kFiltered); // a_(k-1): filtered state vector of current site

#if kalDebug > 0
        Int_t svdim = filtStateCurSite.GetNrows();
        if(sdim != svdim) {
            Error(Form("Prediction to site %i.", iNextSite), Form("State vector must have dimension %i, but has dimension %i", sdim, svdim));
            exit(1);
        }
#endif

        // Propagate track to next site. Calculate f(a_k-1) and F_k-1.
        // -----------

        const HKalPlane &virtPlaneCurSite     = curSite ->getHitVirtPlane(); // Plane to propagate from.
        const HKalMdcMeasLayer &planeCurSite  = curSite ->getHitMeasLayer(); // Needed by Runge-Kutta for material budget.
        const HKalMdcMeasLayer &planeNextSite = nextSite->getHitMeasLayer(); // Plane to propagate to.

        TVectorD predStateNextSite(sdim);    // a^(k-1)_k: predicted state for next site.
        TMatrixD propMatCurSite(sdim, sdim); // F_k-1: propagation matrix.
        TMatrixD procMatCurSite(sdim, sdim); // Q_k-1: process noise matrix.

        propagationOk &= trackPropagator.propagateToPlane(propMatCurSite, procMatCurSite,
                                                          predStateNextSite, filtStateCurSite,
                                                          virtPlaneCurSite, planeNextSite, // Planes to propagate from/to.
                                                          planeCurSite, planeNextSite,     // Measurement layers for process noise.
                                                          pid, getDirection());

        trackLength += trackPropagator.getTrackLength();
        // -----------

        // Calculate virtual plane.
        // The virtual will be aligned as follows:
        // One of the plane's axis points along the wire, the other one
        // from the point of closest approach (PCA) on the wire to the PCA
        // on the track.
        // The current track position will be the centre of the plane.
        // Using such a virtual plane will make the projection function and
        // projector matrix easy to calculate.

        // Assume the track is a straigh line in the vicinity of the current
        // track position.
        TVector3 posAt;
        HKalTrackState::calcPosAtPlane(posAt, planeNextSite, predStateNextSite);
        TVector3 dir;
        HKalTrackState::calcDir(dir, predStateNextSite);

        TVector3 pos2 = posAt + .5 * dir;

        // Find the PCA to a segment defined by the wire endpoints.
        TVector3 wirePoint1, wirePoint2;
        nextSite->getHitWirePts(wirePoint1, wirePoint2, 0);

        Int_t iflag;
        Double_t length, mindist;
        TVector3 pcaTrack, pcaWire;

        // Find the PCA's on the wire and the track.
        HKalGeomTools::Track2ToLine(pcaTrack, pcaWire, iflag, mindist, length, posAt, pos2, wirePoint1, wirePoint2);

        // Make virtual plane.
        // origin: current track position
        // u axis: along wire
        // v axis: from origin to PCA on track
        TVector3 u = (wirePoint2 - wirePoint1).Unit();
        TVector3 v = (pcaTrack - pcaWire).Unit();

        // Repeating the above for all competitors of a site would result in
        // different virtual layers. Since the wires are parallel and the track
        // is approximately a straight line these layers would be parallel, i.e.
        // its axis u and v would be (anit-)parallel.
        // One common virtual layer is used for all of the measurement site's
        // competing hits. The centre point of this virtual plane will be the
        // predicted track position. The predicted position will usually be
        // located between the competing wires.
        //
        // When calculating the residual in the filter step the drift radii of
        // both wires will be projected onto this virtual layer.
        // Placing the virtual plane between the wires keeps the projection error
        // small, allows that a common, simple projection function and projector
        // matrix can be used for both competitors and avoids a bias towards one
        // of the two competing wires.
        //
        //
        //                              Axis v of common virtual plane is parallel to v1 and v2
        //                                   /
        //         |------------*----|------/----------|
        //      PCA for hit 1 -> *   |     /    cell 2 |
        //         |            / *  |    /            |
        //         |           /90 * |   /             |
        //Axis v for hit 1 -> /     *|  /              |
        //         |         /       * /    Wire 2     |
        //    -----|--------X--------|*-------X--------------> Measurement layer
        //         |      Wire 1     / *     /         |
        //         |                /|  *90 / <- Axis v for hit 2
        //         |               / |   * /           |
        //         |cell 1        /  |    * <- PCA for hit 2
        //         |-------------/---|-----*-----------|
        //                      /           * track

        for(Int_t i = 0; i < nextSite->getNcompetitors(); i++) {
            nextSite->setHitVirtPlane(posAt, u, v, i);
        }
        // -----------

        if(bFillPointArrays) {
            const TObjArray &pointsRKTrk = trackPropagator.getPointsTrack();
            for(Int_t i = 0; i < pointsRKTrk.GetEntries(); i++) {
                pointsTrack.Add(pointsRKTrk.At(i));
            }

            const TObjArray &fieldRKTrk = trackPropagator.getFieldTrack();
            for(Int_t i = 0; i < fieldRKTrk.GetEntries(); i++) {
                fieldTrack.Add(fieldRKTrk.At(i));
            }
        }

        // Update covariance matrix.
        // -----------
        nextSite->setStateVec(kPredicted, predStateNextSite);  // a^(k-1)_k = f(a_k-1), store prediction for next site
        curSite ->setStatePropMat(kFiltered, propMatCurSite);  // F_k-1, store propagator matrix
        if(filtType == Kalman::kKalUD) {
            if(!HKalMatrixTools::decomposeUD(procMatCurSite)) {
                Warning("predictAndFilterWireHits()","Could not decompose process noise into UD factors. The matrix is not symmetric.");
            }
        }
        curSite ->setStateProcMat(kFiltered, procMatCurSite);  // Q_k-1, store process noise covariance

        energyLoss += trackPropagator.getEnergyLoss();
        nextSite->setEnergyLoss(energyLoss);

        TMatrixD propMatCurSiteTrans = TMatrixD(TMatrixD::kTransposed, propMatCurSite); // F_k-1

        if(filtType == Kalman::kKalUD) {
            covUpdateUD(iCurSite, iNextSite);
        } else {
            const TMatrixD &covMatCurSite = curSite->getStateCovMat(kFiltered, Kalman::kSecCoord);
#if kalDebug > 0
            Int_t nRows = covMatCurSite.GetNrows();
            Int_t nCols = covMatCurSite.GetNcols();
            if(nRows != sdim || nCols != sdim) {
                Error("predictAndFilterWireHit()", Form("Covariance matrix of site %i is %ix%i, but must be %ix%i matrix.", iNextSite, nRows, nCols, sdim, sdim ));
                exit(1);
            }
#endif
            TMatrixD covMatNextSite = propMatCurSite * covMatCurSite * propMatCurSiteTrans + procMatCurSite;
            if(!covMatNextSite.IsSymmetric()) {
                if(!HKalMatrixTools::makeSymmetric(covMatNextSite)) {
                    if(bPrintWarn) {
                        Warning("predictAndFilterWireHits()", "Covariance matrix for prediction is not close to being symmetric.");
                    }
                }
            }
            nextSite->setStateCovMat(kPredicted, covMatNextSite, Kalman::kSecCoord);
        }
        // -----------

        // Transform state vector to virtual plane coordinates.
        if(getFilterInCoordSys() == Kalman::kLayCoord) {
            nextSite->transSecToVirtLay(Kalman::kPredicted, (filtType == Kalman::kKalUD));
        }

        // Filter next site and calculate new chi2.
        if(!filterWireHit(iNextSite)) {
            Error("predictAndFilterWireHits()", Form("Failed to filter site %i. Skipping this site.", iNextSite));
            propagationOk = kFALSE; //?
            nextSite->setActive(kFALSE);
        } else {
            updateChi2Filter(iNextSite);
            iCurSite = iNextSite;
        }
        iNextSite += iDir;

    } // End loop through all sites.
    return propagationOk;
}

Bool_t HKalSystem::filterWireHit(Int_t iSite) {
    // Do the filter step in Kalman filter for a site. The prediction must have been done
    // for the site before calling this.
    // iSite: index of site to filter

    HKalTrackSite *site  = getSite(iSite);

#if kalDebug > 1
    cout<<"#### Filter site "<<iSite<<" ####"<<endl;
#endif

    // Convert the measured drift time to a drift radius than will
    // be used to calculate the residual in the filter step.
    for(Int_t i = 0; i < site->getNcompetitors(); i++) {
        Double_t driftTime = site->getHitDriftTime(i);

        // Calculate the intersection of the track with the drift cell.
        // The track is approximated as a straight line definfed by the
        // predicted track position and direction.

        TVector3 posAt, dir;
        site->getPosAndDirFromState(posAt, dir, Kalman::kPredicted);

        Int_t s = site->getSector();
        Int_t m = site->getModule();
        Int_t l = site->getLayer();
        Int_t c = site->getCell(i);
        Double_t mindist, alpha;
        getImpact(alpha, mindist, posAt, dir, s, m, l, c);

        if(!TMath::Finite(alpha)) {
            Error("filter()", Form("Calculated non-finite value for impact angle %f.", alpha));
            return kFALSE;
        }
        if(!TMath::Finite(mindist)) {
            Error("filter()", Form("Calculated non-finite value for drift radius %f.", mindist));
            return kFALSE;
        }

        // The HMdcCal2Par container can convert the measured drift time into a
        // drift radius together with the impact angle.
        HMdcCal2Par *cal2   = (HMdcCal2Par   *)gHades->getRuntimeDb()->getContainer("MdcCal2Par");
        Double_t dist   [1] = { cal2->calcDistance   (s, m, alpha, driftTime) };

        Double_t sigTof[4] = { 1.32, 1.29, 2.2, 2.2 }; //? ns
        Double_t vd[4] = { 1./24., 1./24., 1./25., 1./25.5 }; //? mm/ns
        Double_t errTof = vd[m] * sigTof[m];

        Double_t distErr[1] = { cal2->calcDistanceErr(s, m, alpha, driftTime)  + errTof };

#if kalDebug > 2
        cout<<"Converted drift time "<<driftTime<<" to radius "<<dist[0]<<" with error: "<<distErr[0]<<endl;
#endif

        if(dist[0] < 0.) {
            if(dist[0] > -0.5) {
                dist[0] = 0.;
            } else {
                Error("filter()", Form("Drift time %f with impact angle %f yielded negative distance %f to sense wire.",
                                       driftTime, alpha, dist[0]));
                return kFALSE;
            }
        }

        // Store drift radius as the "measurement" vector that is used for filtering.
        site->setHitAndErr(TVectorD(1, &dist[0]), TVectorD(1, &distErr[0]), i);
        site->setHitImpactAngle(alpha, i);
    }

    // Filter this site.
    if(!filter(iSite)) {
        return kFALSE;
    }

    // Filtering wire hits is done in the coordinate system of the virtual layer.
    // Convert the results to sector system.
    if(getFilterInCoordSys() == Kalman::kLayCoord) {
        site->transVirtLayToSec(Kalman::kFiltered, (filtType == Kalman::kKalUD));
    }

    return kTRUE;
}

Bool_t HKalSystem::smoothAll() {
    // Do smoothing for all measurement sites. Requires that the prediction and
    // filter steps have been done before.

    Int_t iStartSite, iGoalSite, iDir;
    // For smoothing, iterate through the sites in opposite direction as in the
    // prediction/filter steps.
    Bool_t dir = getDirection();
    if(dir == kIterForward) {
        iStartSite = getNsites() - 1;
        iGoalSite  = 0;
        iDir       = -1;
    } else {
        iStartSite = 0;
        iGoalSite  = getNsites() - 1;
        iDir       = +1;
    }

    // The result of filtering for the last site coincides with that of smoothing.
    HKalTrackSite *cur = getSite(iStartSite);
    cur->setStateVec   (kSmoothed,    cur->getStateVec(kFiltered));
    cur->setStateCovMat(kSmoothed,    cur->getStateCovMat(kFiltered));
    cur->setStateCovMat(kInvFiltered, cur->getStateCovMat(kFiltered));
    iStartSite += iDir;

    Int_t iSite = iStartSite;
    while((dir == kIterBackward && iSite <= iGoalSite) || (dir == kIterForward && iSite >= iGoalSite)) {
        HKalTrackSite* cur     = getSite(iSite); // Site k to be smoothed in this step.
        if(!cur->getIsActive()) { // Skip inactive sites.
            iSite += iDir;
            continue;
        }

#if kalDebug > 1
        cout<<"**** Smoothing site "<<iSite<<" **** "<<endl;
#endif

        Int_t iPreSite = iSite - iDir; // Site k+1 that has been smoothed in the previous step.
        HKalTrackSite* pre = getSite(iPreSite);
        while(!pre->getIsActive()) { // Skip inactive sites.
            iPreSite -= iDir;
            if(iPreSite < 0 || iPreSite >= getNsites()) {
                Error("smoothAll()", Form("No active site found to smooth site number %i.", iSite));
                return kFALSE;
            }
            pre = getSite(iPreSite);
        }

        // Predicted state vector of site k+1.
        const TVectorD &prePredState  = pre->getStateVec    (kPredicted);          // a^k_k+1

        // Smoothed state vector of site k+1.
        const TVectorD &preSmooState  = pre->getStateVec    (kSmoothed);           // a^n_k+1

        // Filtered state vector of site k.
        const TVectorD &curFiltState  = cur->getStateVec    (kFiltered);           // a_k

        // Covariance from filter step for site k.
        const TMatrixD &curFiltCov    = cur->getStateCovMat (kFiltered);           // C_k

        // Covariance and inverted covariance from prediction for site k+1.
        const TMatrixD &prePredCov    = pre->getStateCovMat (kPredicted);          // C^k_k+1
        const TMatrixD  prePredCovInv = TMatrixD(TMatrixD::kInverted, prePredCov); // (C^k_k-1)^-1

        // Propagator and transposed propagator from predicion for site k.
        const TMatrixD &curProp       = cur->getStatePropMat(kFiltered);           // F_k
        const TMatrixD  curPropTrans  = TMatrixD(TMatrixD::kTransposed, curProp);  // F^T_k

        // Covariance from smoothing step for site k+1.
        const TMatrixD &preSmooCov    = pre->getStateCovMat (kSmoothed);           // C^n_k+1

        // Gain matrix for smoothing.
        //       A_k          = C_k        * F^T_k        * (C^k_k+1)^-1
        TMatrixD smootherGain = curFiltCov * curPropTrans * prePredCovInv;

        // Calculate residual of smoothed and prediction.
        TVectorD residual = preSmooState - prePredState;

#if kalDebug > 2
        cout<<"Residual"<<endl;
        residual.Print();
        cout<<"Smoother gain matrix "<<endl;
        smootherGain.Print();
#endif

        if(residual.GetNrows() > 5) { // z-coordinate is no degree of freedom since track must always be on a layer.
            residual(kIdxZ0) = 0.;
        }

        //       a^n_k        = a_k          + A_k          * (a^n_k+1 - a^k_k+1)
        TVectorD curSmooState = curFiltState + smootherGain * residual;

        cur->setStateVec(kSmoothed,    curSmooState);
        cur->setStateVec(kInvFiltered, curSmooState); // Inverse filtering works with smoothed states.

        // Calculate covariance for smoothed state. Needed for possible inverse filtering.
        //       C^n_k      = C_k        + A_k          * (C^n_k-1    - C^k_k+1   ) * A^T_k
        TMatrixD curSmooCov = curFiltCov + smootherGain * (preSmooCov - prePredCov) * TMatrixD(TMatrixD::kTransposed, smootherGain);
        cur->setStateCovMat(kSmoothed,    curSmooCov);
        cur->setStateCovMat(kInvFiltered, curSmooCov);

        // Need the smoothing results in virt. layer coordinates for inverse filtering.
        if(getFilterInCoordSys() == Kalman::kLayCoord) {
            cur->transSecToVirtLay(kSmoothed);
        }

        iSite += iDir;
    }

    return kTRUE;
}

Bool_t HKalSystem::excludeSite(Int_t iSite) {
    // Exclude a site from the measurement. Requires that prediction, filtering and smoothing
    // has been done before. If smoothing has been disabled it will be done in this function.
    // iSite: index in sites array of the site to exclude

    Kalman::coordSys sys = getFilterInCoordSys();

    if(iSite < 0 || iSite > getNsites() - 1) {
        Warning("excludeSite()", "Invalid site index.");
        return kFALSE;
    }

    // Transform state vector from sector to track coordinates.
    if(rotCoords == Kalman::kVarRot) {
        transformFromSectorToTrack();
    }

    // Do smoothing including site k if it hasn't been done before.
    if(!bDoSmooth) {
        if(!smoothAll()) {
            Error("excludeSite()", Form("Cannot exclude site %i. Smoothing of track failed.", iSite));
            return kFALSE;
        }
    }

    HKalTrackSite* exSite = getSite(iSite);                           // Site k to exclude.

    if(!exSite->getIsActive()) {
        Error("excludeSite()", Form("Attempted to exclude inactive site number %i.", iSite));
        return kFALSE;
    }

    // Smoothed state vector of site to exclude.
    TVectorD exSmooState  = exSite->getStateVec(kSmoothed, sys);       // a^n_k

    // Covariance and inverse covariance from smoothing step.
    TMatrixD exSmooCov    = exSite->getStateCovMat(kSmoothed, sys);    // C^n_k
    TMatrixD exSmooCovInv = TMatrixD(TMatrixD::kInverted, exSmooCov);  // (C^n_k)^-1

    // Measurement and inverse measurement matrices.
    TMatrixD exErr        = exSite->getErrMat();                       // V_k
    TMatrixD exErrInv     = TMatrixD(TMatrixD::kInverted, exErr);      // G_k

    // Measurement vector.
    TVectorD exMeasVec    = exSite->getHitVec();                       // m_k

    const TMatrixD &fProj = exSite->getStateProjMat(Kalman::kFiltered, sys); // H_k
    const TMatrixD fProjTrans = TMatrixD(TMatrixD::kTransposed, fProj);

    // Calculate a measurement vector from smoothed state vector.
    TVectorD exSmooProjMeasVec(getMeasDim());
    calcMeasVecFromState(exSmooProjMeasVec, exSite, Kalman::kSmoothed, Kalman::kSecCoord, 0); //? daf?

    //                             - V_k   + H_k   * C^n_k     * H_k
    TMatrixD kalmanGainHelper = -1.* exErr + fProj * exSmooCov * fProj;
    //       K^n*_k     = C^n_k     * H^T_k      * (- V_k   + H_k   * C^n_k     * H_k  )^-1
    TMatrixD kalmanGain = exSmooCov * fProjTrans * TMatrixD(TMatrixD::kInverted, kalmanGainHelper);

    // Inverse filter: calculate new state vector at site k excluding measurement at this site.
    //       a^n*_k         = a^n_k       + K^n*_k     * (m_k       - h_k(a^n_k)   )
    TVectorD exInvFiltState = exSmooState + kalmanGain * (exMeasVec - exSmooProjMeasVec);
    exSite->setStateVec(kInvFiltered, exInvFiltState, sys);

    // Calculate new covariance at site k.
    //       C^n*_k       =         (                     (C^n_k)^-1   - H^T_k      * G_k      * H_k  )^-1
    TMatrixD exInvFiltCov = TMatrixD(TMatrixD::kInverted, exSmooCovInv - fProjTrans * exErrInv * fProj);
    exSite->setStateCovMat(kInvFiltered, exInvFiltCov, sys);

    // Update chi^2.
    TVectorD projMeasVecInvFilt(getMeasDim());
    calcMeasVecFromState(projMeasVecInvFilt, exSite, Kalman::kInvFiltered, Kalman::kSecCoord, 0); //? daf?

    // Transform TVectorD objects into column vectors for matrix multiplication.
    TMatrixD diffStateTrans(1, getStateDim(), (exInvFiltState - exSmooState).GetMatrixArray()); // (a^n*_k - a^n_k)^T
    TMatrixD resTrans(1, getMeasDim(), (exMeasVec - projMeasVecInvFilt).GetMatrixArray());      // (m_k - h_k(a^n*_k). Transposed residual.

    //        (a^n*_k - a^n_k)^T    * (C^n_k)^-1 * (a^n*_k         - a^n_k      )
    chi2 -=   (diffStateTrans       * exSmooCov  * (exInvFiltState - exSmooState)  )(0)
    //        (m_k - h_k(a^n*_k))^T * G          * (m_k       - h_k(a^n*_k)       )
            - (resTrans             * exErrInv   * (exMeasVec - projMeasVecInvFilt))(0);

    // Transform state vectors back from track to sector coordinates.
    if(rotCoords == Kalman::kVarRot) {
        transformFromTrackToSector();
    }

    exSite->transVirtLayToSec(Kalman::kInvFiltered);

    return kTRUE;
}

void HKalSystem::sortHits() {
    // Sort the competitors in each site by weight.

    for(Int_t iSite = 0; iSite < getNsites(); iSite++) {
        getSite(iSite)->sortHits();
    }
}
/*
void HKalSystem::traceToMETA(const TVector3& metaHit, const TVector3& metaNormVec,
                             const TVector3* pathCorrPos, const TVector3* pathCorrNorm) {
    // Propagates the track from the last fitted MDC position till track intersection
    // with META plane (is defined by META-hit and normal vector to rod's plane.
    // Should only be called after having filtered all sites.
    //
    // Input:
    // metaHit:
    // metaNormVec:
    // pathCorrPos:
    // pathCorrNorm:

    trackLength=trackLengthAtLastMDC;

    Double_t  pointOnMETA[3], normVectorOfMETA[3];
    pointOnMETA[0] = metaHit.getX();
    pointOnMETA[1] = metaHit.getY();
    pointOnMETA[2] = metaHit.getZ();

    normVectorOfMETA[0] = metaNormVec.getX();
    normVectorOfMETA[1] = metaNormVec.getY();
    normVectorOfMETA[2] = metaNormVec.getZ();

    Int_t sdim = getStateDim();
    TMatrixD F(sdim, sdim);
    TMatrixD Q(sdim, sdim);
    TVectorD svMeta(sdim);
    const HKalPlane &mdcPlaneLast = ;
    const HKalPlane &metaPlane;
    const HKalMdcMeasLayer &mdcLayerLast = ;
    const HKalMdcMeasLayer &metaLayer = ;
    trackPropagator.propagateToPlane(F, Q, svMeta,
   
    Float_t step = stepSizeAtLastMDC;
    Float_t stepFac=1.;
    jstep = 0;
    Double_t upos[3], udir[3];
    for(Int_t i=0;i<3;i++) {
        upos[i]=posNearLastMDC[i];   // tracking position near last MDC
        udir[i]=dirAtLastMDC[i];     // and direction
    }
    Double_t dist=0.;
    Bool_t rc=kTRUE;
    do {
        rc=findIntersectionPoint(upos,udir,pointOnMETA,normVectorOfMETA,&trackPosOnMETA[0]);
        if (!rc) break;
        dist=distance(upos,trackPosOnMETA);
        if ((polbending>=0&&upos[2]>trackPosOnMETA[2])||(polbending<0&&upos[1]>trackPosOnMETA[1])) {
            trackLength-=dist;
            break;
    } else if (dist<maxDist) {
      trackLength+=dist;
      break;
    } else if (upos[1]>=2475.||upos[1]<=0.) {  
      trackLength=0.;
      break;
    }
    if (dist<step) { step=dist; }
    stepFac=rkgtrk(step,pfit,upos,udir,1); // we use pfit as value for momentum to propagate
    step*=stepFac;
  } while (jstep<maxNumSteps);
  if (!rc&&jstep>=maxNumSteps) {
    trackLength=0.;
  }
  HMdcSizesCells::calcMdcSeg(upos[0],upos[1],upos[2],
        upos[0]+udir[0],upos[1]+udir[1],upos[2]+udir[2],
        zSeg2,rSeg2,thetaSeg2,phiSeg2);
  if (pathCorrPos!=0&&pathCorrNorm!=0&&trackLength>0.) {
    Double_t p1[3], pn[3], p2[3];
    p1[0]=pathCorrPos->getX();
    p1[1]=pathCorrPos->getY();
    p1[2]=pathCorrPos->getZ();
    pn[0]=pathCorrNorm->getX();
    pn[1]=pathCorrNorm->getY();
    pn[2]=pathCorrNorm->getZ();
    findIntersectionPoint(trackPosOnMETA,udir,p1,pn,&p2[0]);
    trackLength-=distance(trackPosOnMETA,p2);
  }
}
*/

void HKalSystem::transformFromSectorToTrack() {
    // Transform all sites using the Kalman system's rotation matrix.

#if kalDebug > 0
    if(pRotMat->IsIdentity()) {
        Warning("transformFromSectorToTrack()", "Transformation matrix has not been set.");
    }
#endif
    for(Int_t i = 0; i < getNsites(); i++) {
        getSite(i)->transform(*pRotMat);
    }
}

void HKalSystem::transformFromTrackToSector() {
    // Transform all sites using the inverse rotation matrix stored in the Kalman system.
    // This will transform the sites back to sector coordinates after a call of
    // transformFromSectorToTrack() and if the rotation matrix has not been modified
    // in the meantime.

    TRotation rotInv = pRotMat->Inverse();
    // Rotate back sites.
    for(Int_t i = 0; i < getNsites(); i++) {
        getSite(i)->transform(rotInv);
    }

    // Rotate back points from propagation.

    for(Int_t i = 0; i < pointsTrack.GetEntries(); i++) {

#if kalDebug > 0
        if(!pointsTrack.At(i)->InheritsFrom("TVector3")) {
            Error("transformFromTrackToSector()", Form("Object at index %i in trackPoints array is of class %s. Expected class is TVector3.", i, pointsTrack.At(i)->ClassName()));
            return;
        }
#endif

        ((TVector3*)pointsTrack.At(i))->Transform(rotInv);
    }
}

void HKalSystem::updateSites(const TObjArray &hits) {
    // Replace sites of the Kalman system with new hits.
    // hits: array with new measurement hits. Must be of class HKalMdcHit.

    for(Int_t i = 0; i < getNsites(); i++) {
        getSite(i)->clearHits();
        getSite(i)->setActive(kTRUE);
    }

    bDoDaf = kFALSE;
    Int_t iHit  = 0;
    Int_t iSite = 0;

    while(iHit < hits.GetEntries() - 1) {
#if kalDebug > 0
        if(!hits.At(iHit)->InheritsFrom("HKalMdcHit")) {
            Error("updateSites()", Form("Object at index %i in hits array is of class %s. Expected class is HKalMdcHit.", iHit, hits.At(iHit)->ClassName()));
            exit(1);
        }
#endif

        HKalMdcHit *hit     = (HKalMdcHit*)hits.At(iHit);
        HKalMdcHit *nexthit = (HKalMdcHit*)hits.At(iHit + 1);

        getSite(iSite)->addHit(hit);

        // nexthit is in the same measurement site as the previous hit.
        if(hit->areCompetitors(*hit, *nexthit)) {
            bDoDaf = kTRUE;
        // nexthit is not a competitor. Store in the next site.
        } else {
            iSite++;
        }
        iHit++;
    }

    // Add last hit in array to a site.
    HKalMdcHit *lastHit = (HKalMdcHit*)hits.At(hits.GetEntries()-1);
    getSite(iSite)->addHit(lastHit);

    nSites = iSite + 1;
    nHitsInTrack = hits.GetEntries();

    Int_t newHitType = lastHit->getHitType();

    if(newHitType != getHitType()) {
        setHitType((Kalman::kalHitTypes)newHitType);
    }

    for(Int_t i = 0; i < getNsites(); i++) {
        getSite(i)->setNdafs(getNdafs()); // Needs to be redone because the sites' hits were deleted.
    }
}

void HKalSystem::setDafPars(Double_t chi2cut, const Double_t *T, Int_t n) {
    // Set parameters for the deterministic annealing filter.
    //
    // input:
    // chi2cut: Cut-off parameter.
    // T:       Array with annealing factors (temperatures).
    // n:       Number of annealing factors.

    dafChi2Cut = chi2cut;
    dafT.Set(n, &T[0]);

    for(Int_t i = 0; i < getNsites(); i++) {
        getSite(i)->setNdafs(n);
    }
}

void HKalSystem::setPrintWarnings(Bool_t print) {
    // Print or suppress warning messages.
    bPrintWarn = print;
    trackPropagator.setPrintWarnings(print);
}

void HKalSystem::setRotation(Kalman::kalRotateOptions rotate) {
    // Set options for the Kalman filter.
    // kalRotateOptions: Tilt coordinate system.
    // There are three options:
    // 1. kNoRot:
    //    Don't tilt the coordinate system.
    // 2. kVarRot:
    //    Tilt the coordinate system so that the z-axis will always
    //    point in the initial track direction. Assuming track curvature
    //    is not large this ensures that the track state parameters
    //    tx = tan(p_x / p_z) and ty = tan(p_y / p_z) remain small.
    //    Angles close to 90° would result in large values for tx and ty
    //    due to the tangens and poor results for the Kalman filter.

    rotCoords = rotate;
    if( rotCoords == kVarRot) {
        trackPropagator.setRotateBfieldVecs(kTRUE);
    } else {
        trackPropagator.setRotateBfieldVecs(kFALSE);
    }
}

void HKalSystem::setRotationAngles(Double_t rotXangle, Double_t rotYangle) {
    // Set the rotation matrix.
    // rotXangle: rotation angle around x axis in radians.
    // rotYangle: rotation angle around y axis in radians.
    pRotMat->SetToIdentity();
    pRotMat->RotateX(rotXangle);
    pRotMat->RotateY(rotYangle);
}