#include "TMath.h"
#include "TString.h"
#include "TVector3.h"
#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 "hkalifilt.h"
#include <iostream>
using namespace std;
ClassImp (HKalIFilt)
HKalIFilt::HKalIFilt(Int_t n, Int_t measDim, Int_t stateDim,
HMdcTrackGField *fMap, Double_t fpol)
: TObject(), iniStateVec(stateDim), svVertex(stateDim), svMeta(stateDim) {
betaInput = -1.;
nMaxSites = n;
nHitsInTrack = n;
nSites = n;
sites = new HKalTrackSite* [nSites];
for(Int_t i = 0; i < nSites; i++) {
sites[i] = new HKalTrackSite(measDim, stateDim);
}
setFieldMap(fMap, fpol);
trackNum = 0;
pid = -1;
setPrintWarnings(kFALSE);
setPrintErrors(kFALSE);
bFillPointArrays = kFALSE;
nCondErrs = 0;
nCovSymErrs = 0;
nCovPosDefErrs = 0;
setNumIterations(1);
setDirection(kIterForward);
setSmoothing(kTRUE);
setFilterMethod(Kalman::kKalConv);
setDoEnerLoss(kTRUE);
setDoMultScat(kTRUE);
setRotation(Kalman::kVarRot);
pRotMat = new TRotation();
trackPropagator.setRotationMatrix(pRotMat);
pointsTrack.SetOwner();
fieldTrack.SetOwner();
}
HKalIFilt::~HKalIFilt() {
#if kalDebug > 0
cout<<"Covariance symmetry errors: "<<nCovSymErrs
<<", pos. def. errors: "<<nCovPosDefErrs
<<", covariance ill-conditioned: "<<nCondErrs<<endl;
#endif
delete sites;
delete pRotMat;
}
Bool_t HKalIFilt::checkSitePreFilter(Int_t iSite) const {
HKalTrackSite *site = getSite(iSite);
#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();
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 = getHitVec(site);
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 = getHitErrMat(site);
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);
}
if(!checkCovMat(predCov)) {
Warning("filter()",
Form("Errors found in predicted covariance matrix for site %i.",
iSite));
}
#endif
if(!HKalMatrixTools::checkValidElems(predCov)) {
if(bPrintErr) {
Error("filter()",
Form("Predicted covariance matrix for site %i contains INFs or NaNs.",
iSite));
}
return kFALSE;
}
if(!site->getIsActive()) {
if(bPrintErr) {
Error("filter()", Form("Site number %i is inactive.", iSite));
}
return kFALSE;
}
return kTRUE;
}
void HKalIFilt::filterConventional(Int_t iSite) {
Kalman::coordSys sys = getFilterInCoordSys();
HKalTrackSite *site = getSite(iSite);
const TVectorD &predState = site->getStateVec(Kalman::kPredicted, sys);
TVectorD projMeasVec(getMeasDim());
calcMeasVecFromState(projMeasVec, site, Kalman::kPredicted, sys);
TVectorD residual = getHitVec(site) - projMeasVec;
const TMatrixD &predCov = site->getStateCovMat(Kalman::kPredicted, sys);
const TMatrixD &errMat = getHitErrMat(site);
const TMatrixD &fProj = site->getStateProjMat(Kalman::kFiltered, sys);
const TMatrixD fProjTrans = TMatrixD(TMatrixD::kTransposed, fProj);
TMatrixD R = errMat + fProj * predCov * fProjTrans;
#if kalDebug > 0
if(!HKalMatrixTools::checkCond(R)) {
nCondErrs++;
if(bPrintWarn) {
Warning("filterConventional()",
"Matrix is ill-conditioned for inversion.");
}
}
#endif
TMatrixD kalmanGain = predCov * fProjTrans * R.Invert();
TMatrixD covUpdate(TMatrixD(TMatrixD::kUnit,
TMatrixD(getStateDim(sys),getStateDim(sys))) -
kalmanGain * fProj);
TMatrixD filtCov = covUpdate * predCov;
#if kalDebug > 0
Int_t asymCells = HKalMatrixTools::checkSymmetry(filtCov, 1.e-9);
if(asymCells > 0) {
nCovSymErrs += asymCells;
if(bPrintWarn) {
Warning("filterConventional()",
"Covariance matrix for filter step is not symmetric.");
}
}
if(!HKalMatrixTools::isPosDef(filtCov)) {
nCovPosDefErrs++;
if(bPrintWarn) {
Warning("filterConventional()",
"Covariance for the filtered state is not positive definite.");
}
}
#endif
if(!filtCov.IsSymmetric()) {
if(!HKalMatrixTools::makeSymmetric(filtCov)) {
if(bPrintWarn) {
Warning("filterConventional()",
"Covariance matrix for filter step is not close to being symmetric.");
}
}
}
site->setStateCovMat(Kalman::kFiltered, filtCov, sys);
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;
getHitVec(site).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 HKalIFilt::filterJoseph(Int_t iSite) {
Kalman::coordSys sys = getFilterInCoordSys();
HKalTrackSite *site = getSite(iSite);
const TVectorD &predState = site->getStateVec(kPredicted, sys);
TVectorD projMeasVec(getMeasDim());
calcMeasVecFromState(projMeasVec, site, Kalman::kPredicted, sys);
TVectorD residual = getHitVec(site) - projMeasVec;
const TMatrixD &errMat = getHitErrMat(site);
const TMatrixD &predCov = site->getStateCovMat(Kalman::kPredicted, sys);
const TMatrixD &fProj = site->getStateProjMat(Kalman::kFiltered, sys);
const TMatrixD fProjTrans = TMatrixD(TMatrixD::kTransposed, fProj);
TMatrixD R(errMat + fProj * predCov * fProjTrans);
#if kalDebug > 0
if(!HKalMatrixTools::checkCond(R)) {
nCondErrs++;
if(bPrintWarn) {
Warning("filterJospeh()",
"Matrix is ill-conditioned for inversion.");
}
}
#endif
TMatrixD kalmanGain = predCov * fProjTrans * R.Invert();
TMatrixD covUpdate(TMatrixD(TMatrixD::kUnit,
TMatrixD(getStateDim(sys),getStateDim(sys))) -
kalmanGain * fProj);
TMatrixD covUpdateT(TMatrixD::kTransposed, covUpdate);
TMatrixD filtCov = covUpdate * predCov * covUpdateT +
kalmanGain * errMat * TMatrixD(TMatrixD::kTransposed, kalmanGain);
#if kalDebug > 0
Int_t asymCells = HKalMatrixTools::checkSymmetry(filtCov, 1.e-9);
if(asymCells > 0) {
nCovSymErrs += asymCells;
if(bPrintWarn) {
Warning("filterJoseph()",
"Covariance matrix for filter step is not symmetric.");
}
}
if(!HKalMatrixTools::isPosDef(filtCov)) {
nCovPosDefErrs++;
if(bPrintWarn) {
Warning("filterJoseph()",
"Covariance for the filtered state is not positive definite.");
}
}
#endif
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);
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;
getHitVec(site).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 HKalIFilt::filterSequential(Int_t iSite) {
Kalman::coordSys sys = getFilterInCoordSys();
HKalTrackSite *site = getSite(iSite);
Int_t sdim = getStateDim();
Int_t mdim = getMeasDim();
const TMatrixD &errMat = getHitErrMat(site);
const TVectorD &measVec = getHitVec(site);
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;
getHitVec(site).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);
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.");
}
}
}
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 HKalIFilt::filterSwerling(Int_t iSite) {
Kalman::coordSys sys = getFilterInCoordSys();
HKalTrackSite *site = getSite(iSite);
const TVectorD &predState = site->getStateVec(kPredicted, sys);
TVectorD projMeasVec(getMeasDim());
calcMeasVecFromState(projMeasVec, site, Kalman::kPredicted, sys);
TVectorD residual = getHitVec(site) - projMeasVec;
const TMatrixD &predCov = site->getStateCovMat(Kalman::kPredicted, sys);
#if kalDebug > 0
if(!HKalMatrixTools::checkCond(predCov)) {
nCondErrs++;
if(bPrintWarn) {
Warning("filterSwerling()",
"Matrix is ill-conditioned for inversion.");
}
}
#endif
const TMatrixD predCovInv(TMatrixD::kInverted, predCov);
const TMatrixD errInv(TMatrixD::kInverted, getHitErrMat(site));
const TMatrixD &fProj = site->getStateProjMat(Kalman::kFiltered, sys);
const TMatrixD fProjTrans = TMatrixD(TMatrixD::kTransposed, fProj);
TMatrixD filtCov = TMatrixD(TMatrixD::kInverted,
predCovInv + fProjTrans * errInv * fProj);
#if kalDebug > 0
Int_t asymCells = HKalMatrixTools::checkSymmetry(filtCov, 1.e-9);
if(asymCells > 0) {
nCovSymErrs += asymCells;
if(bPrintWarn) {
Warning("filterSwerling()",
"Covariance matrix for filter step is not symmetric.");
}
}
if(!HKalMatrixTools::isPosDef(filtCov)) {
nCovPosDefErrs++;
if(bPrintWarn) {
Warning("filterSwerling()",
"Covariance for the filtered state is not positive definite.");
}
}
#endif
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);
TMatrixD kalmanGain = filtCov * fProjTrans * errInv;
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;
getHitVec(site).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;
getHitErrMat(site).Print();
cout<<"Projector matrix:"<<endl;
fProj.Print();
cout<<"Kalman gain:"<<endl;
kalmanGain.Print();
#endif
}
void HKalIFilt::filterUD(Int_t iSite) {
Kalman::coordSys sys = getFilterInCoordSys();
HKalTrackSite *site = getSite(iSite);
Int_t sdim = getStateDim(sys);
Int_t mdim = getMeasDim();
const TVectorD &measVec = getHitVec(site);
const TMatrixD &errMat = getHitErrMat(site);
TVectorD projMeasVec(getMeasDim());
const TVectorD &predState = site->getStateVec(Kalman::kPredicted, sys);
calcMeasVecFromState(projMeasVec, site, Kalman::kPredicted, sys);
TVectorD resVec = measVec - projMeasVec;
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);
TVectorD filtState = predState;
for(Int_t iMeas = 0; iMeas < mdim; iMeas++) {
Double_t hrow[sdim];
fProj.ExtractRow(iMeas, 0, hrow);
TMatrixD H(1, sdim, hrow);
Double_t residual = measVec(iMeas) - (H * filtState)(0);
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;
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;
}
#if kalDebug > 0
if(!HKalMatrixTools::isPosDef(D)) {
nCovPosDefErrs++;
if(bPrintWarn) {
Warning("filterUD()",
"D-matrix for the filtered state is not positive definite.");
}
}
#endif
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;
getHitVec(site).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 HKalIFilt::propagateCovConv(Int_t iFromSite, Int_t iToSite) {
HKalTrackSite *fromSite = getSite(iFromSite);
HKalTrackSite *toSite = getSite(iToSite);
const TMatrixD &covMatFromSite = fromSite->getStateCovMat(kFiltered);
#if kalDebug > 0
Int_t nRows = covMatFromSite.GetNrows();
Int_t nCols = covMatFromSite.GetNcols();
if(nRows != getStateDim() || nCols != getStateDim()) {
Error(Form("Prediction to site %i.", iToSite),
Form("Covariance matrix is %ix%i, but must be %ix%i matrix.",
nRows, nCols, getStateDim(), getStateDim()));
exit(1);
}
#endif
const TMatrixD &propMatFromSite = fromSite->getStatePropMat(Kalman::kFiltered);
const TMatrixD &procMatFromSite = fromSite->getStateProcMat(Kalman::kFiltered);
TMatrixD propMatFromSiteTrans = TMatrixD(TMatrixD::kTransposed, propMatFromSite);
TMatrixD covMatToSite = propMatFromSite * covMatFromSite *
propMatFromSiteTrans + procMatFromSite;
if(!covMatToSite.IsSymmetric()) {
if(!HKalMatrixTools::makeSymmetric(covMatToSite)) {
if(bPrintWarn) {
Warning("predictAndFilter2DHits()",
"Covariance matrix for prediction is not close to being symmetric.");
}
}
}
toSite->setStateCovMat(Kalman::kPredicted, covMatToSite);
}
void HKalIFilt::propagateCovUD(Int_t iFromSite, Int_t iToSite) {
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);
}
Bool_t HKalIFilt::propagateTrack(Int_t iFromSite, Int_t iToSite) {
HKalTrackSite *fromSite = getSite(iFromSite);
HKalTrackSite *toSite = getSite(iToSite);
Bool_t propagationOk = kTRUE;
const TVectorD &filtStateFromSite = fromSite->getStateVec(kFiltered);
const HKalMdcMeasLayer &planeFromSite = fromSite->getHitMeasLayer();
const HKalMdcMeasLayer &planeToSite = toSite ->getHitMeasLayer();
Int_t sdim = getStateDim();
#if kalDebug > 0
Int_t nRows;
nRows = filtStateFromSite.GetNrows();
if(nRows != sdim) {
if(bPrintErr) {
Error("propagateTrack()",
Form("Filtered state vector of site %i must have dimension %i, but has dimension %i",
iFromSite, nRows, sdim));
}
return kFALSE;
}
#endif
TVectorD predStateToSite(sdim);
TMatrixD propMatFromSite(sdim, sdim);
TMatrixD procMatFromSite(sdim, sdim);
if(!trackPropagator.propagateToPlane(propMatFromSite, procMatFromSite,
predStateToSite, filtStateFromSite,
planeFromSite, planeToSite,
planeFromSite, planeToSite,
pid, getDirection())) {
if(bPrintErr) {
Error("propagateTrack()",
Form("Propagation from measurement site %i in sec %i/mod %i/lay %i to site %i in sec %i/mod %i/lay %i failed. Skipping this site.",
iFromSite, planeFromSite.getSector(),
planeFromSite.getModule(), planeFromSite.getLayer(),
iToSite, planeToSite.getSector(),
planeToSite.getModule(), planeToSite.getLayer()));
}
toSite->setActive(kFALSE);
return kFALSE;
}
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();
toSite ->setStateVec(kPredicted, predStateToSite);
fromSite->setStatePropMat(kFiltered, propMatFromSite);
if(filtType == Kalman::kKalUD) {
if(!HKalMatrixTools::decomposeUD(procMatFromSite)) {
Warning("predictAndFilter2DHits()",
"Could not decompose process noise into UD factors.");
}
}
fromSite->setStateProcMat(kFiltered, procMatFromSite);
toSite->setEnergyLoss(fromSite->getEnergyLoss() +
trackPropagator.getEnergyLoss());
return propagationOk;
}
void HKalIFilt::updateChi2Filter(Int_t iSite) {
Kalman::coordSys sys = getFilterInCoordSys();
HKalTrackSite *site = getSite(iSite);
Int_t mdim = getMeasDim();
const TVectorD &predState = site->getStateVec(Kalman::kPredicted, sys);
const TVectorD &filtState = site->getStateVec(Kalman::kFiltered, sys);
TVectorD diffPredFilt = filtState - predState;
TMatrixD diffPredFiltTrans(1, getStateDim(sys),
diffPredFilt.GetMatrixArray());
TMatrixD predCov = site->getStateCovMat(Kalman::kPredicted, sys);
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 = getHitErrMat(site);
const TMatrixD errInv(TMatrixD::kInverted, errMat);
TVectorD projFiltMeasVec(mdim);
calcMeasVecFromState(projFiltMeasVec, site, Kalman::kFiltered, sys);
TVectorD diffMeasFilt = getHitVec(site) - projFiltMeasVec;
TMatrixD diffMeasFiltTrans(1, mdim , diffMeasFilt.GetMatrixArray());
Double_t chi2Inc = (diffPredFiltTrans * predCovInv * diffPredFilt )(0)+
(diffMeasFiltTrans * errInv * diffMeasFilt )(0);
site->setChi2(chi2Inc);
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
}
void HKalIFilt::newTrack(const TObjArray &hits, const TVectorD &iniSv,
const TMatrixD &iniCovMat, Int_t pId) {
bTrackAccepted = kTRUE;
chi2 = 0.;
fieldTrack.Clear();
pointsTrack.Clear();
trackLength = 0.;
trackLengthAtLastMdc = 0.;
trackPropagator.Clear();
if(iniStateVec.GetNrows() != iniSv.GetNrows()) {
iniStateVec.ResizeTo(iniSv.GetNrows());
if(bPrintWarn) {
Warning("fitTrack()", Form("Passed initial state vector has dimension %i. Class member of HKalIFilt has dimension %i.",
iniSv.GetNrows(), iniStateVec.GetNrows()));
}
}
iniStateVec = iniSv;
updateSites(hits);
trackNum++;
pid = pId;
Int_t fromSite;
if(getDirection() == kIterForward) {
fromSite = 0;
} else {
fromSite = getNsites() - 1;
}
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));
if(rotCoords == Kalman::kVarRot) {
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) {
transformFromSectorToTrack();
}
}
Bool_t HKalIFilt::calcMeasVecFromState(TVectorD &projMeasVec,
HKalTrackSite const * const site,
Kalman::kalFilterTypes stateType,
Kalman::coordSys sys) const {
#if kalDebug > 0
Int_t mdim = getMeasDim();
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
const TVectorD &sv = site->getStateVec(stateType, sys);
projMeasVec = site->getStateProjMat() * sv;
return kTRUE;
}
Bool_t HKalIFilt::checkCovMat(const TMatrixD &fCov) const {
Bool_t bCovOk = kTRUE;
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;
}
}
if(filtType != Kalman::kKalUD) {
if(!fCov.IsSymmetric()) {
if(bPrintWarn) {
Warning("checkCovMat()", "Covariance matrix is not symmetric.");
}
bCovOk = kFALSE;
}
}
if(filtType != Kalman::kKalUD) {
if(!HKalMatrixTools::isPosDef(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::isPosDef(UD)) {
if(bPrintWarn) {
Warning("checkCovMat()",
"Covariance matrix is not positive definite.");
}
bCovOk = kFALSE;
}
}
return bCovOk;
}
Bool_t HKalIFilt::excludeSite(Int_t iSite) {
Kalman::coordSys sys = getFilterInCoordSys();
if(iSite < 0 || iSite > getNsites() - 1) {
Warning("excludeSite()", "Invalid site index.");
return kFALSE;
}
if(rotCoords == Kalman::kVarRot) {
transformFromSectorToTrack();
}
if(!bDoSmooth) {
if(!smooth()) {
if(bPrintErr) {
Error("excludeSite()",
Form("Cannot exclude site %i. Smoothing of track failed.", iSite));
}
return kFALSE;
}
}
HKalTrackSite* exSite = getSite(iSite);
if(!exSite->getIsActive()) {
if(bPrintErr) {
Error("excludeSite()",
Form("Attempted to exclude inactive site number %i.", iSite));
}
return kFALSE;
}
if(getFilterInCoordSys() == Kalman::kLayCoord &&
exSite->getNcompetitors() > 2) {
Error("excludeSite()",
"This function does not work with competing wire measurements.");
return kFALSE;
}
TVectorD exSmooState = exSite->getStateVec(kSmoothed, sys);
TMatrixD exSmooCov = exSite->getStateCovMat(kSmoothed, sys);
TMatrixD exSmooCovInv = TMatrixD(TMatrixD::kInverted, exSmooCov);
TMatrixD exErr = getHitErrMat(exSite);
TMatrixD exErrInv = TMatrixD(TMatrixD::kInverted, exErr);
TVectorD exMeasVec = getHitVec(exSite);
const TMatrixD &fProj = exSite->getStateProjMat(Kalman::kFiltered, sys);
const TMatrixD fProjTrans = TMatrixD(TMatrixD::kTransposed, fProj);
TVectorD exSmooProjMeasVec(getMeasDim());
calcMeasVecFromState(exSmooProjMeasVec, exSite, Kalman::kSmoothed,
Kalman::kSecCoord);
TMatrixD kalmanGainHelper = -1.* exErr + fProj * exSmooCov * fProj;
TMatrixD kalmanGain = exSmooCov * fProjTrans *
TMatrixD(TMatrixD::kInverted, kalmanGainHelper);
TVectorD exInvFiltState = exSmooState + kalmanGain * (exMeasVec - exSmooProjMeasVec);
exSite->setStateVec(kInvFiltered, exInvFiltState, sys);
TMatrixD exInvFiltCov = TMatrixD(TMatrixD::kInverted,
exSmooCovInv - fProjTrans * exErrInv * fProj);
exSite->setStateCovMat(kInvFiltered, exInvFiltCov, sys);
TVectorD projMeasVecInvFilt(getMeasDim());
calcMeasVecFromState(projMeasVecInvFilt, exSite,
Kalman::kInvFiltered, Kalman::kSecCoord);
TMatrixD diffStateTrans(1, getStateDim(),
(exInvFiltState - exSmooState).GetMatrixArray());
TMatrixD resTrans(1, getMeasDim(),
(exMeasVec - projMeasVecInvFilt).GetMatrixArray());
chi2 -= (diffStateTrans * exSmooCov * (exInvFiltState - exSmooState) )(0)
- (resTrans * exErrInv * (exMeasVec - projMeasVecInvFilt))(0);
if(rotCoords == Kalman::kVarRot) {
transformFromTrackToSector();
}
exSite->transVirtLayToSec(Kalman::kInvFiltered, 0);
return kTRUE;
}
Bool_t HKalIFilt::filter(Int_t iSite) {
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 HKalIFilt::fitTrack(const TObjArray &hits, const TVectorD &iniSv,
const TMatrixD &iniCovMat, Int_t pId) {
#if kalDebug > 1
cout<<"******************************"<<endl;
cout<<"**** Fit track number "<<trackNum<<" ****"<<endl;
cout<<"******************************"<<endl;
#endif
#if kalDebug > 2
cout<<"Initial state vector:"<<endl;
iniSv.Print();
cout<<"Initial covariance:"<<endl;
iniCovMat.Print();
cout<<"Initial particle hypothesis: "<<pId<<endl;
#endif
newTrack(hits, iniSv, iniCovMat, pId);
if(getNsites() == 0) {
if(bPrintWarn) {
Warning("fitTrack()", "Track contained no measurements.");
}
return kFALSE;
}
Bool_t orgDirection = getDirection();
Int_t fromSite, toSite;
if(getDirection() == kIterForward) {
fromSite = 0;
toSite = getNsites() - 1;
} else {
fromSite = getNsites() - 1;
toSite = 0;
}
for(Int_t i = 0; i < nKalRuns; i++) {
chi2 = 0.;
bTrackAccepted &= runFilter(fromSite, toSite);
if(i < nKalRuns-1) {
getSite(toSite)->setStateVec(kPredicted, getSite(toSite)->getStateVec(kFiltered));
setDirection(!direction);
Int_t oldTo = toSite;
toSite = fromSite;
fromSite = oldTo;
}
}
if(bDoSmooth) {
if(!smooth()) {
if(getPrintErr()) {
Error("fitTrack()", "Smoothing track failed.");
}
bTrackAccepted = kFALSE;
}
}
setDirection(orgDirection);
if(rotCoords == Kalman::kVarRot) {
transformFromTrackToSector();
}
setTrackLengthAtLastMdc(trackLength);
#if kalDebug > 1
if(bTrackAccepted) {
cout<<"**** Fitted track ****"<<endl;
} else {
cout<<"**** Errors during track fitting ****"<<endl;
}
#endif
if(!bTrackAccepted) chi2 = -1.F;
return bTrackAccepted;
}
Bool_t HKalIFilt::propagate(Int_t iFromSite, Int_t iToSite) {
if(!propagateTrack(iFromSite, iToSite)) {
return kFALSE;
}
if(filtType == Kalman::kKalUD) {
propagateCovUD(iFromSite, iToSite);
} else {
propagateCovConv(iFromSite, iToSite);
}
return kTRUE;
}
Bool_t HKalIFilt::propagateToPlane(TVectorD& svTo, const TVectorD &svFrom,
const HKalMdcMeasLayer &measLayFrom,
const HKalMdcMeasLayer &measLayTo,
Int_t pid, Bool_t dir) {
Int_t sdim = svTo.GetNrows();
TMatrixD F(sdim, sdim);
TMatrixD Q(sdim, sdim);
return trackPropagator.propagateToPlane(F, Q,
svTo, svFrom,
measLayFrom, measLayTo,
measLayFrom, measLayTo,
pid, dir);
}
Bool_t HKalIFilt::runFilter(Int_t iFromSite, Int_t toSite) {
HKalTrackSite *fromSite = getSite(iFromSite);
if(!fromSite) {
return kFALSE;
}
if(!calcProjector(iFromSite)) {
if(getPrintErr()) {
Error("runFilter()",
Form("Bad projector matrix for site %i.", iFromSite));
}
return kFALSE;
}
Bool_t trackOk = kTRUE;
Int_t iDir;
if(iFromSite < toSite) {
iDir = +1;
} else {
iDir = -1;
}
Int_t iCurSite = iFromSite;
while(!getSite(iCurSite)->getIsActive()) {
iCurSite += iDir;
if((iDir < 0 && iCurSite < toSite) || (iDir > 0 && iCurSite > toSite)) {
if(bPrintErr) {
Error("predictAndFilter2DHits()",
Form("No two valid sites between %i and %i.", iFromSite, toSite));
}
return kFALSE;
}
}
Int_t iNextSite = iCurSite + iDir;
while((iDir > 0 && iNextSite <= toSite) ||
(iDir < 0 && iNextSite >= toSite)) {
HKalTrackSite *nextSite = getSite(iNextSite);
#if kalDebug > 1
HKalTrackSite *curSite = getSite(iCurSite);
cout<<"##### Prediction from site "<<iCurSite<<" at "
<<curSite->getSector()<<" / "<<curSite->getModule()<<" / "
<<curSite->getLayer()
<<" to site "<<iNextSite<<" at "
<<nextSite->getSector()<<" / "<<nextSite->getModule()<<" / "
<<nextSite->getLayer()
<<" #####"<<endl;
#endif
if(!nextSite->getIsActive()) {
iNextSite += iDir;
continue;
}
if(!propagate(iCurSite, iNextSite)) {
iNextSite += iDir;
continue;
}
if(!calcProjector(iNextSite)) {
if(getPrintErr()) {
Error("runFilter()",
Form("Errors calculating projector matrix for site %i.",
iNextSite));
}
nextSite->setActive(kFALSE);
return kFALSE;
}
if(!checkSitePreFilter(iNextSite)) {
if(getPrintErr()) {
Error("runFilter()",
Form("Unable to filter site %i.", iNextSite));
}
nextSite->setActive(kFALSE);
return kFALSE;
}
if(!filter(iNextSite)) {
if(bPrintErr) {
Error("runFilter()",
Form("Failed to filter site %i. Skipping this site.", iNextSite));
}
trackOk = kFALSE;
nextSite->setActive(kFALSE);
} else {
updateChi2Filter(iNextSite);
iCurSite = iNextSite;
}
iNextSite += iDir;
}
return trackOk;
}
Bool_t HKalIFilt::smooth() {
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);
}
}
Int_t iStartSite, iGoalSite, iDir;
Bool_t dir = getDirection();
if(dir == kIterForward) {
iStartSite = getNsites() - 1;
iGoalSite = 0;
iDir = -1;
} else {
iStartSite = 0;
iGoalSite = getNsites() - 1;
iDir = +1;
}
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);
if(!cur->getIsActive()) {
iSite += iDir;
continue;
}
#if kalDebug > 1
cout<<"**** Smoothing site "<<iSite<<" **** "<<endl;
#endif
Int_t iPreSite = iSite - iDir;
HKalTrackSite* pre = getSite(iPreSite);
while(!pre->getIsActive()) {
iPreSite -= iDir;
if(iPreSite < 0 || iPreSite >= getNsites()) {
if(bPrintWarn) {
Warning("smoothAll()", Form("No active site found to smooth site number %i.", iSite));
}
return kFALSE;
}
pre = getSite(iPreSite);
}
const TVectorD &prePredState = pre->getStateVec (kPredicted);
const TVectorD &preSmooState = pre->getStateVec (kSmoothed);
const TVectorD &curFiltState = cur->getStateVec (kFiltered);
const TMatrixD &curFiltCov = cur->getStateCovMat (kFiltered);
const TMatrixD &prePredCov = pre->getStateCovMat (kPredicted);
const TMatrixD prePredCovInv = TMatrixD(TMatrixD::kInverted, prePredCov);
const TMatrixD &curProp = cur->getStatePropMat(kFiltered);
const TMatrixD curPropTrans = TMatrixD(TMatrixD::kTransposed, curProp);
const TMatrixD &preSmooCov = pre->getStateCovMat (kSmoothed);
TMatrixD smootherGain = curFiltCov * curPropTrans * prePredCovInv;
TVectorD residual = preSmooState - prePredState;
if(residual.GetNrows() > 5) {
residual(kIdxZ0) = 0.;
}
TVectorD curSmooState = curFiltState + smootherGain * residual;
#if kalDebug > 2
cout<<"Residual"<<endl;
residual.Print();
cout<<"Smoother gain matrix "<<endl;
smootherGain.Print();
cout<<"Filtered State:"<<endl;
curFiltState.Print();
cout<<"Smoothed State:"<<endl;
curSmooState.Print();
#endif
cur->setStateVec(kSmoothed, curSmooState);
cur->setStateVec(kInvFiltered, curSmooState);
TMatrixD curSmooCov = curFiltCov +
smootherGain * (preSmooCov - prePredCov) *
TMatrixD(TMatrixD::kTransposed, smootherGain);
cur->setStateCovMat(kSmoothed, curSmooCov);
cur->setStateCovMat(kInvFiltered, curSmooCov);
if(getFilterInCoordSys() == Kalman::kLayCoord) {
cur->transSecToVirtLay(kSmoothed, 0);
}
iSite += iDir;
}
return kTRUE;
}
void HKalIFilt::sortHits() {
for(Int_t iSite = 0; iSite < getNsites(); iSite++) {
getSite(iSite)->sortHits();
}
}
Bool_t HKalIFilt::traceToVertex() {
HKalTrackSite *fromSite = getSite(0);
Int_t sec = fromSite->getSector();
Int_t mod = fromSite->getModule();
Double_t xVertex = 0.;
Double_t yVertex = 0.;
Double_t zVertex = -500.;
if(mod == 1) zVertex = -650;
if(mod > 2) {
if(getPrintErr()) {
Error("traceToVertex", "No inner segment.");
}
return kFALSE;
}
HGeomVector vertex(xVertex, yVertex, zVertex);
HMdcSizesCells *fSizesCells = HMdcSizesCells::getExObject();
if(!fSizesCells) {
fSizesCells = HMdcSizesCells::getObject();
fSizesCells->initContainer();
}
HMdcSizesCellsMod &fSizesCellsMod = (*fSizesCells)[sec][mod];
vertex = fSizesCellsMod.getSecTrans()->transFrom(vertex);
TVector3 pointForVertexRec(vertex.getX(), vertex.getY(), vertex.getZ());
const TVectorD &svFrom = fromSite->getStateVec(Kalman::kSmoothed);
const HKalMdcMeasLayer &planeFrom = fromSite->getHitMeasLayer();
HKalMdcMeasLayer planeVertex(pointForVertexRec, planeFrom.getNormal(),
planeFrom.getMaterialPtr(kFALSE),
planeFrom.getMaterialPtr(kFALSE),
-1, sec, 0, 1, 0.);
Int_t sdim = getStateDim();
#if metaDebug > 0
Int_t nRows = svFrom.GetNrows();
if(nRows != sdim) {
if(bPrintErr) {
Error("traceToVertex()",
Form("Smoothed state vector of site %i must have dimension %i, but has dimension %i", 0, nRows, sdim));
}
return kFALSE;
}
if(!bDoSmooth) {
if(getPrintErr()) {
Error("traceToVertex()",
"Cannot trace to vertex without doing smoothing.");
}
return kFALSE;
}
#endif
TMatrixD F(sdim, sdim);
TMatrixD Q(sdim, sdim);
if(!trackPropagator.propagateToPlane(F, Q,
svVertex, svFrom,
planeFrom, planeVertex,
planeFrom, planeVertex,
pid, kIterBackward)) {
if(getPrintErr()) {
Error("traceToVertex()",
"Failed propagation of smoothed track to vertex.");
}
return kFALSE;
}
TVector3 posVertex;
HKalTrackState::calcPosAtPlane(posVertex, planeVertex, svVertex);
HMdcSizesCellsSec& fSCSec = (*fSizesCells)[sec];
const HGeomVector& target = fSCSec.getTargetMiddlePoint();
Double_t dist = TMath::Sqrt(TMath::Power(target.getX() - posVertex.X(), 2.) +
TMath::Power(target.getY() - posVertex.Y(), 2.) +
TMath::Power(target.getZ() - posVertex.Z(), 2.));
#if metaDebug > 2
cout<<"Tracklength from target to first MDC: "
<<trackPropagator.getTrackLength()<<", dist correction :"<<dist<<endl;
#endif
#if metaDebug > 3
cout<<"Position at target: "<<endl;
posVertex.Print();
#endif
trackLength += dist + trackPropagator.getTrackLength();
setTrackLengthAtLastMdc(trackLength);
return kTRUE;
}
Bool_t HKalIFilt::traceToMeta(const TVector3& metaHit, const TVector3& metaNormVec) {
HKalTrackSite *fromSite = getSite(getNsites() - 1);
trackLength = trackLengthAtLastMdc;
Int_t sdim = getStateDim();
TMatrixD F(sdim, sdim);
TMatrixD Q(sdim, sdim);
const TVectorD &svFrom = fromSite->getStateVec(Kalman::kSmoothed);
const HKalMdcMeasLayer &planeFrom = fromSite->getHitMeasLayer();
HKalMdcMeasLayer planeMeta(metaHit, metaNormVec,
planeFrom.getMaterialPtr(kFALSE),
planeFrom.getMaterialPtr(kFALSE),
fromSite->getModule(), fromSite->getSector(),
0, 1, 0);
if(!trackPropagator.propagateToPlane(F, Q,
svMeta, svFrom,
planeFrom, planeMeta,
planeFrom, planeMeta,
pid, kIterForward)) {
if(getPrintErr()) {
Error("traceToMeta()",
"Failed propagation of smoothed track to Meta.");
}
return kFALSE;
}
HKalTrackState::calcPosAtPlane(posMeta, planeMeta, svMeta);
trackLength += trackPropagator.getTrackLength();
#if metaDebug > 1
cout<<"Kalman filter: trace to Meta hit ("<<metaHit.X()<<"/"<<metaHit.Y()
<<"/"<<metaHit.Z()<<")"
<<" with normal vector ("<<metaNormVec.X()<<"/"<<metaNormVec.Y()
<<"/"<<metaNormVec.Z()<<")."<<endl;
#endif
#if metaDebug > 2
cout<<"Distance last MDC to Meta: "<<trackPropagator.getTrackLength()<<endl;
#endif
#if metaDebug > 3
cout<<"State at Meta: "<<endl;
svMeta.Print();
cout<<"Reconstructed pos at Meta: ("<<posMeta.X()<<"/"<<posMeta.Y()
<<"/"<<posMeta.Z()<<")"<<endl;
#endif
return kTRUE;
}
void HKalIFilt::transformFromSectorToTrack() {
#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 HKalIFilt::transformFromTrackToSector() {
TRotation rotInv = pRotMat->Inverse();
for(Int_t i = 0; i < getNsites(); i++) {
getSite(i)->transform(rotInv);
}
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 HKalIFilt::updateSites(const TObjArray &hits) {
for(Int_t i = 0; i < getNsites(); i++) {
getSite(i)->clearHits();
getSite(i)->setActive(kTRUE);
}
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);
if(!hit->areCompetitors(*hit, *nexthit)) {
getSite(iSite)->addHit(hit);
iSite++;
}
iHit++;
}
HKalMdcHit *lastHit = (HKalMdcHit*)hits.At(hits.GetEntries()-1);
if(hits.GetEntries() > 1) {
HKalMdcHit *secondLastHit = (HKalMdcHit*)hits.At(hits.GetEntries()-2);
if(!lastHit->areCompetitors(*lastHit, *secondLastHit)) {
getSite(iSite)->addHit(lastHit);
iSite++;
}
} else {
getSite(iSite)->addHit(lastHit);
iSite++;
}
nSites = iSite;
nHitsInTrack = hits.GetEntries();
#if kalDebug > 1
cout<<"New track has "<<nSites<<" measurement sites."<<endl;
#endif
}
TMatrixD const& HKalIFilt::getHitErrMat(HKalTrackSite* const site) const {
return site->getErrMat(0);
}
TVectorD const& HKalIFilt::getHitVec(HKalTrackSite* const site) const {
return site->getHitVec(0);
}
void HKalIFilt::getMetaPos(Double_t &x, Double_t &y, Double_t &z) const {
x = posMeta.X();
y = posMeta.Y();
z = posMeta.Z();
}
void HKalIFilt::getVertexPos(Double_t &x, Double_t &y, Double_t &z) const {
x = posVertex.X();
y = posVertex.Y();
z = posVertex.Z();
}
Double_t HKalIFilt::getNdf() const {
return getNsites() * getMeasDim() - getStateDim();
}
void HKalIFilt::setPrintErrors(Bool_t print) {
bPrintErr = print;
trackPropagator.setPrintErrors(print);
}
void HKalIFilt::setPrintWarnings(Bool_t print) {
bPrintWarn = print;
trackPropagator.setPrintWarnings(print);
}
void HKalIFilt::setRotation(Kalman::kalRotateOptions rotate) {
rotCoords = rotate;
if( rotCoords == kVarRot) {
trackPropagator.setRotateBfieldVecs(kTRUE);
} else {
trackPropagator.setRotateBfieldVecs(kFALSE);
}
}
void HKalIFilt::setRotationAngles(Double_t rotXangle, Double_t rotYangle) {
pRotMat->SetToIdentity();
pRotMat->RotateX(rotXangle);
pRotMat->RotateY(rotYangle);
}