TrackFindingTMTT/src/KFbase.cc

0001 ///=== This is the base class for the Kalman Combinatorial Filter track fit algorithm.
0002
0003 ///=== Written by: S. Summers, K. Uchida, M. Pesaresi, I.Tomalin
0004
0005 #include "L1Trigger/TrackFindingTMTT/interface/KFbase.h"
0006 #include "L1Trigger/TrackFindingTMTT/interface/Utility.h"
0007 #include "L1Trigger/TrackFindingTMTT/interface/TP.h"
0008 #include "L1Trigger/TrackFindingTMTT/interface/KalmanState.h"
0009 #include "L1Trigger/TrackFindingTMTT/interface/StubKiller.h"
0010 #include "L1Trigger/TrackFindingTMTT/interface/PrintL1trk.h"
0011 #include "FWCore/ServiceRegistry/interface/Service.h"
0012 #include "CommonTools/UtilAlgos/interface/TFileService.h"
0013 #include "DataFormats/Math/interface/deltaPhi.h"
0014 #include "TMatrixD.h"
0015
0016 #include <algorithm>
0017 #include <functional>
0018 #include <fstream>
0019 #include <iomanip>
0020 #include <atomic>
0021 #include <sstream>
0022
0023 using namespace std;
0024
0025 namespace tmtt {
0026
0027   /* Initialize cfg parameters */
0028
0029   KFbase::KFbase(const Settings *settings, const uint nHelixPar, const string &fitterName, const uint nMeas)
0030       : TrackFitGeneric(settings, fitterName) {
0031     nHelixPar_ = nHelixPar;
0032     nMeas_ = nMeas;
0033     numEtaRegions_ = settings->numEtaRegions();
0034   }
0035
0036   /* Do track fit */
0037
0038   L1fittedTrack KFbase::fit(const L1track3D &l1track3D) {
0039     iPhiSec_ = l1track3D.iPhiSec();
0040     iEtaReg_ = l1track3D.iEtaReg();
0041     resetStates();
0042     numUpdateCalls_ = 0;
0043
0044     vector<Stub *> stubs = l1track3D.stubs();
0045
0046     auto orderByLayer = [](const Stub *a, const Stub *b) { return bool(a->layerId() < b->layerId()); };
0047     sort(stubs.begin(), stubs.end(), orderByLayer);  // Makes debug printout pretty.
0048
0049     //TP
0050     const TP *tpa(nullptr);
0051     if (l1track3D.matchedTP()) {
0052       tpa = l1track3D.matchedTP();
0053     }
0054     tpa_ = tpa;
0055
0056     //track information dump
0057     if (settings_->kalmanDebugLevel() >= 1) {
0058       PrintL1trk() << "===============================================================================";
0059       std::stringstream text;
0060       text << std::fixed << std::setprecision(4);
0061       text << "Input track cand: [phiSec,etaReg]=[" << l1track3D.iPhiSec() << "," << l1track3D.iEtaReg() << "]";
0062       text << " HT(m,c)=(" << l1track3D.cellLocationHT().first << "," << l1track3D.cellLocationHT().second
0063            << ") q/pt=" << l1track3D.qOverPt() << " tanL=" << l1track3D.tanLambda() << " z0=" << l1track3D.z0()
0064            << " phi0=" << l1track3D.phi0() << " nStubs=" << l1track3D.numStubs() << " d0=" << l1track3D.d0();
0065       PrintL1trk() << text.str();
0066       if (not settings_->hybrid())
0067         printTP(tpa);
0068       if (settings_->kalmanDebugLevel() >= 2) {
0069         printStubLayers(stubs, l1track3D.iEtaReg());
0070         printStubs(stubs);
0071       }
0072     }
0073
0074     //Kalman Filter
0075     const KalmanState *cand = doKF(l1track3D, stubs, tpa);
0076
0077     //return L1fittedTrk for the selected state (if KF produced one it was happy with).
0078     if (cand != nullptr) {
0079       // Get track helix params.
0080       TVectorD trackPars = trackParams(cand);
0081       double d0 = (nHelixPar_ == 5) ? trackPars[D0] : 0.;
0082
0083       L1fittedTrack fitTrk(settings_,
0084                            &l1track3D,
0085                            cand->stubs(),
0086                            cand->hitPattern(),
0087                            trackPars[QOVERPT],
0088                            d0,
0089                            trackPars[PHI0],
0090                            trackPars[Z0],
0091                            trackPars[T],
0092                            cand->chi2rphi(),
0093                            cand->chi2rz(),
0094                            nHelixPar_);
0095
0096       // Store supplementary info, specific to KF fitter.
0097       fitTrk.setInfoKF(cand->nSkippedLayers(), numUpdateCalls_);
0098
0099       // If doing 5 parameter fit, optionally also calculate helix params & chi2 with beam-spot constraint applied,
0100       // and store inside L1fittedTrack object.
0101       if (settings_->kalmanAddBeamConstr()) {
0102         if (nHelixPar_ == 5) {
0103           double chi2rphi_bcon = 0.;
0104           TVectorD trackPars_bcon = trackParams_BeamConstr(cand, chi2rphi_bcon);
0105
0106           // Check scaled chi2 cut
0107           vector<double> kfLayerVsChiSqCut = settings_->kfLayerVsChiSq5();
0108           double chi2scaled = chi2rphi_bcon / settings_->kalmanChi2RphiScale() + fitTrk.chi2rz();
0109           bool accepted = true;
0110           if (chi2scaled > kfLayerVsChiSqCut[cand->nStubLayers()])
0111             accepted = false;
0112
0113           fitTrk.setBeamConstr(trackPars_bcon[QOVERPT], trackPars_bcon[PHI0], chi2rphi_bcon, accepted);
0114         }
0115       }
0116
0117       // Fitted track params must lie in same sector as HT originally found track in.
0118       if (!settings_->hybrid()) {  // consistentSector() function not yet working for Hybrid.
0119
0120         // Bodge to take into account digitisation in sector consistency check.
0121         if (settings_->enableDigitize())
0122           fitTrk.digitizeTrack("KF4ParamsComb");
0123
0124         if (!fitTrk.consistentSector()) {
0125           if (settings_->kalmanDebugLevel() >= 1)
0126             PrintL1trk() << "Track rejected by sector consistency test";
0127           L1fittedTrack rejectedTrk;
0128           return rejectedTrk;
0129         }
0130       }
0131
0132       return fitTrk;
0133
0134     } else {  // Track rejected by fitter
0135
0136       if (settings_->kalmanDebugLevel() >= 1) {
0137         bool goodTrack = (tpa && tpa->useForAlgEff());  // Matches truth particle.
0138         if (goodTrack) {
0139           int tpin = tpa->index();
0140           PrintL1trk() << "TRACK LOST: eta=" << l1track3D.iEtaReg() << " pt=" << l1track3D.pt() << " tp=" << tpin;
0141
0142           for (auto stub : stubs) {
0143             int kalmanLay =
0144                 this->kalmanLayer(l1track3D.iEtaReg(), stub->layerIdReduced(), stub->barrel(), stub->r(), stub->z());
0145             std::stringstream text;
0146             text << std::fixed << std::setprecision(4);
0147             text << "    Stub: lay_red=" << stub->layerIdReduced() << " KFlay=" << kalmanLay << " r=" << stub->r()
0148                  << " z=" << stub->z() << "   assoc TPs =";
0149             for (const TP *tp_i : stub->assocTPs())
0150               text << " " << tp_i->index();
0151             PrintL1trk() << text.str();
0152             if (stub->assocTPs().empty())
0153               PrintL1trk() << " none";
0154           }
0155           PrintL1trk() << "=====================";
0156         }
0157       }
0158
0159       //dump on the missed TP for efficiency calculation.
0160       if (settings_->kalmanDebugLevel() >= 3) {
0161         if (tpa && tpa->useForAlgEff()) {
0162           PrintL1trk() << "TP for eff. missed addr. index : " << tpa << " " << tpa->index();
0163           printStubs(stubs);
0164         }
0165       }
0166
0167       L1fittedTrack rejectedTrk;
0168       return rejectedTrk;
0169     }
0170   }
0171
0172   /* Do track fit (internal function) */
0173
0174   const KalmanState *KFbase::doKF(const L1track3D &l1track3D, const vector<Stub *> &stubs, const TP *tpa) {
0175     const KalmanState *finished_state = nullptr;
0176
0177     map<unsigned int, const KalmanState *, std::greater<unsigned int>>
0178         best_state_by_nstubs;  // Best state (if any) for each viable no. of stubs on track value.
0179
0180     // seed helix params & their covariance.
0181     TVectorD x0 = seedX(l1track3D);
0182     TMatrixD pxx0 = seedC(l1track3D);
0183     TMatrixD K(nHelixPar_, 2);
0184     TMatrixD dcov(2, 2);
0185
0186     const KalmanState *state0 = mkState(l1track3D, 0, -1, nullptr, x0, pxx0, K, dcov, nullptr, 0, 0);
0187
0188     // internal containers - i.e. the state FIFO. Contains estimate of helix params in last/next layer, with multiple entries if there were multiple stubs, yielding multiple states.
0189     vector<const KalmanState *> new_states;
0190     vector<const KalmanState *> prev_states;
0191     prev_states.push_back(state0);
0192
0193     // Get dead layers, if any.
0194     bool remove2PSCut = settings_->kalmanRemove2PScut();
0195     set<unsigned> kfDeadLayers = kalmanDeadLayers(remove2PSCut);
0196
0197     // arrange stubs into Kalman layers according to eta region
0198     int etaReg = l1track3D.iEtaReg();
0199     map<int, vector<Stub *>> layerStubs;
0200
0201     for (auto stub : stubs) {
0202       // Get Kalman encoded layer ID for this stub.
0203       int kalmanLay = this->kalmanLayer(etaReg, stub->layerIdReduced(), stub->barrel(), stub->r(), stub->z());
0204
0205       if (kalmanLay != invalidKFlayer_) {
0206         if (layerStubs[kalmanLay].size() < settings_->kalmanMaxStubsPerLayer()) {
0207           layerStubs[kalmanLay].push_back(stub);
0208         } else {
0209           // If too many stubs, FW keeps the last stub.
0210           layerStubs[kalmanLay].back() = stub;
0211         }
0212       }
0213     }
0214
0215     // iterate using state->nextLayer() to determine next Kalman layer(s) to add stubs from
0216     constexpr unsigned int nTypicalLayers = 6;  // Number of tracker layers a typical track can pass through.
0217     // If user asked to add up to 7 layers to track, increase number of iterations by 1.
0218     const unsigned int maxIterations = std::max(nTypicalLayers, settings_->kalmanMaxNumStubs());
0219     for (unsigned iteration = 0; iteration < maxIterations; iteration++) {
0220       bool easy = (l1track3D.numStubs() < settings_->kalmanMaxStubsEasy());
0221       unsigned int kalmanMaxSkipLayers =
0222           easy ? settings_->kalmanMaxSkipLayersEasy() : settings_->kalmanMaxSkipLayersHard();
0223
0224       // update each state from previous iteration (or seed) using stubs in next Kalman layer
0225       vector<const KalmanState *>::const_iterator i_state = prev_states.begin();
0226       for (; i_state != prev_states.end(); i_state++) {
0227         const KalmanState *the_state = *i_state;
0228
0229         unsigned int layer = the_state->nextLayer();  // Get KF layer where stubs to be searched for next
0230         unsigned nSkipped = the_state->nSkippedLayers();
0231
0232         // If this layer is known to be dead, skip to the next layer (layer+1)
0233         // The next_states_skipped will then look at layer+2
0234         // However, if there are stubs in this layer, then don't skip (e.g. our phi/eta boundaries might not line up exactly with a dead region)
0235         // Continue to skip until you reach a functioning layer (or a layer with stubs)
0236         unsigned nSkippedDeadLayers = 0;
0237         unsigned nSkippedAmbiguousLayers = 0;
0238         while (kfDeadLayers.find(layer) != kfDeadLayers.end() && layerStubs[layer].empty()) {
0239           layer += 1;
0240           ++nSkippedDeadLayers;
0241         }
0242         while (this->kalmanAmbiguousLayer(etaReg, layer) && layerStubs[layer].empty()) {
0243           layer += 1;
0244           ++nSkippedAmbiguousLayers;
0245         }
0246
0247         // containers for updated state+stub combinations
0248         vector<const KalmanState *> next_states;
0249         vector<const KalmanState *> next_states_skipped;
0250
0251         // find stubs for this layer
0252         // (If layer > 6, this will return empty vector, so safe).
0253         vector<Stub *> thislay_stubs = layerStubs[layer];
0254
0255         // find stubs for next layer if we skip a layer, except when we are on the penultimate layer,
0256         // or we have exceeded the max skipped layers
0257         vector<Stub *> nextlay_stubs;
0258
0259         // If the next layer (layer+1) is a dead layer, then proceed to the layer after next (layer+2), if possible
0260         // Also note if we need to increase "skipped" by one more for these states
0261         unsigned nSkippedDeadLayers_nextStubs = 0;
0262         unsigned nSkippedAmbiguousLayers_nextStubs = 0;
0263         if (nSkipped < kalmanMaxSkipLayers) {
0264           if (kfDeadLayers.find(layer + 1) != kfDeadLayers.end() && layerStubs[layer + 1].empty()) {
0265             nextlay_stubs = layerStubs[layer + 2];
0266             nSkippedDeadLayers_nextStubs++;
0267           } else if (this->kalmanAmbiguousLayer(etaReg, layer + 1) && layerStubs[layer + 1].empty()) {
0268             nextlay_stubs = layerStubs[layer + 2];
0269             nSkippedAmbiguousLayers_nextStubs++;
0270           } else {
0271             nextlay_stubs = layerStubs[layer + 1];
0272           }
0273         }
0274
0275         // If track was not rejected by isGoodState() is previous iteration, failure here usually means the tracker ran out of layers to explore.
0276         // (Due to "kalmanLay" not having unique ID for each layer within a given eta sector).
0277         if (settings_->kalmanDebugLevel() >= 2 && best_state_by_nstubs.empty() && thislay_stubs.empty() &&
0278             nextlay_stubs.empty())
0279           PrintL1trk() << "State is lost by start of iteration " << iteration
0280                        << " : #thislay_stubs=" << thislay_stubs.size() << " #nextlay_stubs=" << nextlay_stubs.size()
0281                        << " layer=" << layer << " eta=" << l1track3D.iEtaReg();
0282
0283         // If we skipped over a dead layer, only increment "nSkipped" after the stubs in next+1 layer have been obtained
0284         nSkipped += nSkippedDeadLayers;
0285         nSkipped += nSkippedAmbiguousLayers;
0286
0287         // check to guarantee no fewer than 2PS hits per state at iteration 1
0288         // (iteration 0 will always include a PS hit, but iteration 1 could use 2S hits
0289         // unless we include this)
0290         if (iteration == 1 && !remove2PSCut) {
0291           vector<Stub *> temp_thislaystubs;
0292           vector<Stub *> temp_nextlaystubs;
0293           for (auto stub : thislay_stubs) {
0294             if (stub->psModule())
0295               temp_thislaystubs.push_back(stub);
0296           }
0297           for (auto stub : nextlay_stubs) {
0298             if (stub->psModule())
0299               temp_nextlaystubs.push_back(stub);
0300           }
0301           thislay_stubs = temp_thislaystubs;
0302           nextlay_stubs = temp_nextlaystubs;
0303         }
0304
0305         // loop over each stub in this layer and check for compatibility with this state
0306         for (unsigned i = 0; i < thislay_stubs.size(); i++) {
0307           Stub *stub = thislay_stubs[i];
0308
0309           // Update helix params by adding this stub.
0310           const KalmanState *new_state = kalmanUpdate(nSkipped, layer, stub, the_state, tpa);
0311
0312           // Cut on track chi2, pt etc.
0313           if (isGoodState(*new_state))
0314             next_states.push_back(new_state);
0315         }
0316
0317         // loop over each stub in next layer if we skip, and check for compatibility with this state
0318         for (unsigned i = 0; i < nextlay_stubs.size(); i++) {
0319           Stub *stub = nextlay_stubs[i];
0320
0321           const KalmanState *new_state =
0322               kalmanUpdate(nSkipped + 1 + nSkippedDeadLayers_nextStubs + nSkippedAmbiguousLayers_nextStubs,
0323                            layer + 1 + nSkippedDeadLayers_nextStubs + nSkippedAmbiguousLayers_nextStubs,
0324                            stub,
0325                            the_state,
0326                            tpa);
0327
0328           if (isGoodState(*new_state))
0329             next_states_skipped.push_back(new_state);
0330         }
0331
0332         // post Kalman filter local sorting per state
0333         auto orderByChi2 = [](const KalmanState *a, const KalmanState *b) {
0334           return bool(a->chi2scaled() < b->chi2scaled());
0335         };
0336         sort(next_states.begin(), next_states.end(), orderByChi2);
0337         sort(next_states_skipped.begin(), next_states_skipped.end(), orderByChi2);
0338
0339         new_states.insert(new_states.end(), next_states.begin(), next_states.end());
0340         new_states.insert(new_states.end(), next_states_skipped.begin(), next_states_skipped.end());
0341       }  //end of state loop
0342
0343       // copy new_states into prev_states for next iteration or end if we are on
0344       // last iteration by clearing all states and making final state selection
0345
0346       auto orderByMinSkipChi2 = [](const KalmanState *a, const KalmanState *b) {
0347         return bool((a->chi2scaled()) * (a->nSkippedLayers() + 1) < (b->chi2scaled()) * (b->nSkippedLayers() + 1));
0348       };
0349       sort(new_states.begin(), new_states.end(), orderByMinSkipChi2);  // Sort by chi2*(skippedLayers+1)
0350
0351       unsigned int nStubs = iteration + 1;
0352       // Success. We have at least one state that passes all cuts. Save best state found with this number of stubs.
0353       if (nStubs >= settings_->kalmanMinNumStubs() && not new_states.empty())
0354         best_state_by_nstubs[nStubs] = new_states[0];
0355
0356       if (nStubs == settings_->kalmanMaxNumStubs()) {
0357         // We're done.
0358         prev_states.clear();
0359         new_states.clear();
0360         break;
0361       } else {
0362         // Continue iterating.
0363         prev_states = new_states;
0364         new_states.clear();
0365       }
0366     }
0367
0368     if (not best_state_by_nstubs.empty()) {
0369       // Select state with largest number of stubs.
0370       finished_state = best_state_by_nstubs.begin()->second;  // First element has largest number of stubs.
0371       if (settings_->kalmanDebugLevel() >= 1) {
0372         std::stringstream text;
0373         text << std::fixed << std::setprecision(4);
0374         text << "Track found! final state selection: nLay=" << finished_state->nStubLayers()
0375              << " hitPattern=" << std::hex << finished_state->hitPattern() << std::dec
0376              << " phiSec=" << l1track3D.iPhiSec() << " etaReg=" << l1track3D.iEtaReg() << " HT(m,c)=("
0377              << l1track3D.cellLocationHT().first << "," << l1track3D.cellLocationHT().second << ")";
0378         TVectorD y = trackParams(finished_state);
0379         text << " q/pt=" << y[QOVERPT] << " tanL=" << y[T] << " z0=" << y[Z0] << " phi0=" << y[PHI0];
0380         if (nHelixPar_ == 5)
0381           text << " d0=" << y[D0];
0382         text << " chosen from states:";
0383         for (const auto &p : best_state_by_nstubs)
0384           text << " " << p.second->chi2() << "/" << p.second->nStubLayers();
0385         PrintL1trk() << text.str();
0386       }
0387     } else {
0388       if (settings_->kalmanDebugLevel() >= 1) {
0389         PrintL1trk() << "Track lost";
0390       }
0391     }
0392
0393     return finished_state;
0394   }
0395
0396   /*--- Update a helix state by adding a stub. */
0397
0398   const KalmanState *KFbase::kalmanUpdate(
0399       unsigned nSkipped, unsigned int layer, Stub *stub, const KalmanState *state, const TP *tpa) {
0400     if (settings_->kalmanDebugLevel() >= 4) {
0401       PrintL1trk() << "---------------";
0402       PrintL1trk() << "kalmanUpdate";
0403       PrintL1trk() << "---------------";
0404       printStub(stub);
0405     }
0406
0407     numUpdateCalls_++;  // For monitoring, count calls to updator per track.
0408
0409     // Helix params & their covariance.
0410     TVectorD vecX = state->vectorX();
0411     TMatrixD matC = state->matrixC();
0412     if (state->barrel() && !stub->barrel()) {
0413       if (settings_->kalmanDebugLevel() >= 4) {
0414         PrintL1trk() << "STATE BARREL TO ENDCAP BEFORE ";
0415         PrintL1trk() << "state : " << vecX[0] << " " << vecX[1] << " " << vecX[2] << " " << vecX[3];
0416         PrintL1trk() << "cov(x): ";
0417         matC.Print();
0418       }
0419       if (settings_->kalmanDebugLevel() >= 4) {
0420         PrintL1trk() << "STATE BARREL TO ENDCAP AFTER ";
0421         PrintL1trk() << "state : " << vecX[0] << " " << vecX[1] << " " << vecX[2] << " " << vecX[3];
0422         PrintL1trk() << "cov(x): ";
0423         matC.Print();
0424       }
0425     }
0426     // Matrix to propagate helix reference point from one layer to next.
0427     TMatrixD matF = matrixF(stub, state);
0428     TMatrixD matFtrans(TMatrixD::kTransposed, matF);
0429     if (settings_->kalmanDebugLevel() >= 4) {
0430       PrintL1trk() << "matF";
0431       matF.Print();
0432     }
0433
0434     // Multiply matrices to get helix params relative to reference point at next layer.
0435     TVectorD vecXref = matF * vecX;
0436     if (settings_->kalmanDebugLevel() >= 4) {
0437       PrintL1trk() << "vecFref = [";
0438       for (unsigned i = 0; i < nHelixPar_; i++)
0439         PrintL1trk() << vecXref[i] << ", ";
0440       PrintL1trk() << "]";
0441     }
0442
0443     // Get stub residuals.
0444     TVectorD delta = residual(stub, vecXref, state->candidate().qOverPt());
0445     if (settings_->kalmanDebugLevel() >= 4) {
0446       PrintL1trk() << "delta = " << delta[0] << ", " << delta[1];
0447     }
0448
0449     // Derivative of predicted (phi,z) intercept with layer w.r.t. helix params.
0450     TMatrixD matH = matrixH(stub);
0451     if (settings_->kalmanDebugLevel() >= 4) {
0452       PrintL1trk() << "matH";
0453       matH.Print();
0454     }
0455
0456     if (settings_->kalmanDebugLevel() >= 4) {
0457       PrintL1trk() << "previous state covariance";
0458       matC.Print();
0459     }
0460     // Get scattering contribution to helix parameter covariance (currently zero).
0461     TMatrixD matScat(nHelixPar_, nHelixPar_);
0462
0463     // Get covariance on helix parameters at new reference point including scattering..
0464     TMatrixD matCref = matF * matC * matFtrans + matScat;
0465     if (settings_->kalmanDebugLevel() >= 4) {
0466       PrintL1trk() << "matCref";
0467       matCref.Print();
0468     }
0469     // Get hit position covariance matrix.
0470     TMatrixD matV = matrixV(stub, state);
0471     if (settings_->kalmanDebugLevel() >= 4) {
0472       PrintL1trk() << "matV";
0473       matV.Print();
0474     }
0475
0476     TMatrixD matRinv = matrixRinv(matH, matCref, matV);
0477     if (settings_->kalmanDebugLevel() >= 4) {
0478       PrintL1trk() << "matRinv";
0479       matRinv.Print();
0480     }
0481
0482     // Calculate Kalman Gain matrix.
0483     TMatrixD matK = getKalmanGainMatrix(matH, matCref, matRinv);
0484     if (settings_->kalmanDebugLevel() >= 4) {
0485       PrintL1trk() << "matK";
0486       matK.Print();
0487     }
0488
0489     // Update helix state & its covariance matrix with new stub.
0490     TVectorD new_vecX(nHelixPar_);
0491     TMatrixD new_matC(nHelixPar_, nHelixPar_);
0492     adjustState(matK, matCref, vecXref, matH, delta, new_vecX, new_matC);
0493
0494     // Update track fit chi2 with new stub.
0495     double new_chi2rphi = 0, new_chi2rz = 0;
0496     this->adjustChi2(state, matRinv, delta, new_chi2rphi, new_chi2rz);
0497
0498     if (settings_->kalmanDebugLevel() >= 4) {
0499       if (nHelixPar_ == 4)
0500         PrintL1trk() << "adjusted x = " << new_vecX[0] << ", " << new_vecX[1] << ", " << new_vecX[2] << ", "
0501                      << new_vecX[3];
0502       else if (nHelixPar_ == 5)
0503         PrintL1trk() << "adjusted x = " << new_vecX[0] << ", " << new_vecX[1] << ", " << new_vecX[2] << ", "
0504                      << new_vecX[3] << ", " << new_vecX[4];
0505       PrintL1trk() << "adjusted C ";
0506       new_matC.Print();
0507       PrintL1trk() << "adjust chi2rphi=" << new_chi2rphi << " chi2rz=" << new_chi2rz;
0508     }
0509
0510     const KalmanState *new_state = mkState(
0511         state->candidate(), nSkipped, layer, state, new_vecX, new_matC, matK, matV, stub, new_chi2rphi, new_chi2rz);
0512
0513     return new_state;
0514   }
0515
0516   /* Create a KalmanState, containing a helix state & next stub it is to be updated with. */
0517
0518   const KalmanState *KFbase::mkState(const L1track3D &candidate,
0519                                      unsigned nSkipped,
0520                                      unsigned layer,
0521                                      const KalmanState *last_state,
0522                                      const TVectorD &vecX,
0523                                      const TMatrixD &matC,
0524                                      const TMatrixD &matK,
0525                                      const TMatrixD &matV,
0526                                      Stub *stub,
0527                                      double chi2rphi,
0528                                      double chi2rz) {
0529     auto new_state = std::make_unique<const KalmanState>(
0530         settings_, candidate, nSkipped, layer, last_state, vecX, matC, matK, matV, stub, chi2rphi, chi2rz);
0531
0532     const KalmanState *p_new_state = new_state.get();
0533     listAllStates_.push_back(std::move(new_state));  // Vector keeps ownership of all states.
0534     return p_new_state;
0535   }
0536
0537   /* Product of H*C*H(transpose) (where C = helix covariance matrix) */
0538
0539   TMatrixD KFbase::matrixHCHt(const TMatrixD &matH, const TMatrixD &matC) const {
0540     TMatrixD matHtrans(TMatrixD::kTransposed, matH);
0541     return matH * matC * matHtrans;
0542   }
0543
0544   /* Get inverted Kalman R matrix: inverse(V + HCHt) */
0545
0546   TMatrixD KFbase::matrixRinv(const TMatrixD &matH, const TMatrixD &matCref, const TMatrixD &matV) const {
0547     TMatrixD matHCHt = matrixHCHt(matH, matCref);
0548     TMatrixD matR = matV + matHCHt;
0549     TMatrixD matRinv(2, 2);
0550     if (matR.Determinant() > 0) {
0551       matRinv = TMatrixD(TMatrixD::kInverted, matR);
0552     } else {
0553       // Protection against rare maths instability.
0554       const TMatrixD unitMatrix(TMatrixD::kUnit, TMatrixD(2, 2));
0555       const double big = 9.9e9;
0556       matRinv = big * unitMatrix;
0557     }
0558     if (settings_->kalmanDebugLevel() >= 4) {
0559       PrintL1trk() << "matHCHt";
0560       matHCHt.Print();
0561       PrintL1trk() << "matR";
0562       matR.Print();
0563     }
0564     return matRinv;
0565   }
0566
0567   /* Determine Kalman gain matrix K */
0568
0569   TMatrixD KFbase::getKalmanGainMatrix(const TMatrixD &matH, const TMatrixD &matCref, const TMatrixD &matRinv) const {
0570     TMatrixD matHtrans(TMatrixD::kTransposed, matH);
0571     TMatrixD matCrefht = matCref * matHtrans;
0572     TMatrixD matK = matCrefht * matRinv;
0573     return matK;
0574   }
0575
0576   /* Calculate stub residual w.r.t. helix */
0577
0578   TVectorD KFbase::residual(const Stub *stub, const TVectorD &vecX, double candQoverPt) const {
0579     TVectorD vd = vectorM(stub);  // Get (phi relative to sector, z) of hit.
0580     TMatrixD h = matrixH(stub);
0581     TVectorD hx = h * vecX;  // Get intercept of helix with layer (linear approx).
0582     TVectorD delta = vd - hx;
0583
0584     // Calculate higher order corrections to residuals.
0585     // TO DO: Check if these could be determined using Tracklet/HT input track helix params,
0586     //        so only need applying at input to KF, instead of very iteration?
0587
0588     if (not settings_->kalmanHOfw()) {
0589       TVectorD correction(2);
0590
0591       float inv2R = (settings_->invPtToInvR()) * 0.5 * candQoverPt;
0592       float tanL = vecX[T];
0593       float z0 = vecX[Z0];
0594
0595       float deltaS = 0.;
0596       if (settings_->kalmanHOhelixExp()) {
0597         // Higher order correction correction to circle expansion for improved accuracy at low Pt.
0598         double corr = stub->r() * inv2R;
0599
0600         // N.B. In endcap 2S, this correction to correction[0] is exactly cancelled by the deltaS-dependent correction to it below.
0601         correction[0] += (1. / 6.) * pow(corr, 3);
0602
0603         deltaS = (1. / 6.) * (stub->r()) * pow(corr, 2);
0604         correction[1] -= deltaS * tanL;
0605
0606         if (nHelixPar_ == 5) {
0607           float d0 = vecX[D0];
0608           correction[0] += (1. / 6.) * pow(d0 / stub->r(), 3);  // Division by r hard in FPGA?
0609         }
0610       }
0611
0612       if ((not stub->barrel()) && not(stub->psModule())) {
0613         // These corrections rely on inside --> outside tracking, so r-z track params in 2S modules known.
0614         float rShift = (stub->z() - z0) / tanL - stub->r();
0615
0616         if (settings_->kalmanHOhelixExp())
0617           rShift -= deltaS;
0618
0619         if (settings_->kalmanHOprojZcorr() == 1) {
0620           // Add correlation term related to conversion of stub residuals from (r,phi) to (z,phi).
0621           correction[0] += inv2R * rShift;
0622         }
0623
0624         if (settings_->kalmanHOalpha() == 1) {
0625           // Add alpha correction for non-radial 2S endcap strips..
0626           correction[0] += stub->alpha() * rShift;
0627         }
0628       }
0629
0630       // Apply correction to residuals.
0631       delta += correction;
0632     }
0633
0634     delta[0] = reco::deltaPhi(delta[0], 0.);
0635
0636     return delta;
0637   }
0638
0639   /* Update helix state & its covariance matrix with new stub */
0640
0641   void KFbase::adjustState(const TMatrixD &matK,
0642                            const TMatrixD &matCref,
0643                            const TVectorD &vecXref,
0644                            const TMatrixD &matH,
0645                            const TVectorD &delta,
0646                            TVectorD &new_vecX,
0647                            TMatrixD &new_matC) const {
0648     new_vecX = vecXref + matK * delta;
0649     const TMatrixD unitMatrix(TMatrixD::kUnit, TMatrixD(nHelixPar_, nHelixPar_));
0650     TMatrixD tmp = unitMatrix - matK * matH;
0651     new_matC = tmp * matCref;
0652   }
0653
0654   /* Update track fit chi2 with new stub */
0655
0656   void KFbase::adjustChi2(const KalmanState *state,
0657                           const TMatrixD &matRinv,
0658                           const TVectorD &delta,
0659                           double &chi2rphi,
0660                           double &chi2rz) const {
0661     // Change in chi2 (with r-phi/r-z correlation term included in r-phi component)
0662     double delChi2rphi = delta[PHI] * delta[PHI] * matRinv[PHI][PHI] + 2 * delta[PHI] * delta[Z] * matRinv[PHI][Z];
0663     double delChi2rz = delta[Z] * delta[Z] * matRinv[Z][Z];
0664
0665     if (settings_->kalmanDebugLevel() >= 4) {
0666       PrintL1trk() << "delta(chi2rphi)=" << delChi2rphi << " delta(chi2rz)= " << delChi2rz;
0667     }
0668     chi2rphi = state->chi2rphi() + delChi2rphi;
0669     chi2rz = state->chi2rz() + delChi2rz;
0670     return;
0671   }
0672
0673   /* Reset internal data ready for next track. */
0674
0675   void KFbase::resetStates() { listAllStates_.clear(); }
0676
0677   /* Get Kalman layer mapping (i.e. layer order in which stubs should be processed) */
0678
0679   unsigned int KFbase::kalmanLayer(
0680       unsigned int iEtaReg, unsigned int layerIDreduced, bool barrel, float r, float z) const {
0681     if (nEta_ != numEtaRegions_)
0682       throw cms::Exception("LogicError")
0683           << "ERROR KFbase::getKalmanLayer hardwired value of nEta_ differs from NumEtaRegions cfg param";
0684
0685     unsigned int kfEtaReg;  // KF VHDL eta sector def: small in barrel & large in endcap.
0686     if (iEtaReg < numEtaRegions_ / 2) {
0687       kfEtaReg = numEtaRegions_ / 2 - 1 - iEtaReg;
0688     } else {
0689       kfEtaReg = iEtaReg - numEtaRegions_ / 2;
0690     }
0691
0692     unsigned int kalmanLay =
0693         barrel ? layerMap_[kfEtaReg][layerIDreduced].first : layerMap_[kfEtaReg][layerIDreduced].second;
0694
0695     // Switch back to the layermap that is consistent with current FW when "maybe layer" is not used
0696     if (not settings_->kfUseMaybeLayers()) {
0697       switch (kfEtaReg) {
0698         case 6:  //case 6: B1 B2+D1 D2 D3 D4 D5
0699           if (layerIDreduced > 2) {
0700             kalmanLay--;
0701           }
0702           break;
0703         default:
0704           break;
0705       }
0706     }
0707
0708     /*
0709   // Fix cases where a barrel layer only partially crosses the eta sector.
0710   // (Logically should work, but actually reduces efficiency -- INVESTIGATE).
0711
0712   const float barrelHalfLength = 120.;
0713   const float barrel4Radius = 68.8;
0714   const float barrel5Radius = 86.1;
0715
0716   if ( not barrel) {
0717     switch ( kfEtaReg ) {
0718     case 4:
0719       if (layerIDreduced==3) {  // D1
0720         float disk1_rCut = barrel5Radius*(std::abs(z)/barrelHalfLength);
0721         if (r > disk1_rCut) kalmanLay++;
0722       }
0723       break;
0724     case 5:
0725       if (layerIDreduced==3) { // D1
0726         float disk1_rCut = barrel4Radius*(std::abs(z)/barrelHalfLength);
0727         if (r > disk1_rCut) kalmanLay++;
0728       }
0729       if (layerIDreduced==4) { // D2
0730         float disk2_rCut = barrel4Radius*(std::abs(z)/barrelHalfLength);
0731         if (r > disk2_rCut) kalmanLay++;
0732       }
0733       break;
0734     default:
0735       break;
0736     }
0737   }
0738   */
0739
0740     return kalmanLay;
0741   }
0742
0743   /*=== Check if particles in given eta sector are uncertain to go through the given KF layer. */
0744   /*=== (If so, count layer for numbers of hit layers, but not for number of skipped layers). */
0745
0746   bool KFbase::kalmanAmbiguousLayer(unsigned int iEtaReg, unsigned int kfLayer) {
0747     // Only helps in extreme forward sector, and there not significantly.
0748     // UNDERSTAND IF CAN BE USED ELSEWHERE.
0749
0750     constexpr bool ambiguityMap[nEta_ / 2][nKFlayer_] = {
0751         {false, false, false, false, false, false, false},
0752         {false, false, false, false, false, false, false},
0753         {false, false, false, false, false, false, false},
0754         {false, false, false, false, false, false, false},
0755         {false, false, false, false, false, false, false},
0756         {false, false, true, false, false, false, false},
0757         {true, true, false, false, false, false, false},
0758         {true, false, false, false, false, false, false},
0759     };
0760
0761     unsigned int kfEtaReg;  // KF VHDL eta sector def: small in barrel & large in endcap.
0762     if (iEtaReg < numEtaRegions_ / 2) {
0763       kfEtaReg = numEtaRegions_ / 2 - 1 - iEtaReg;
0764     } else {
0765       kfEtaReg = iEtaReg - numEtaRegions_ / 2;
0766     }
0767
0768     bool ambiguous = false;
0769     if (settings_->kfUseMaybeLayers() && kfLayer < nKFlayer_)
0770       ambiguous = ambiguityMap[kfEtaReg][kfLayer];
0771
0772     return ambiguous;
0773   }
0774
0775   /* Adjust KF algorithm to allow for any dead tracker layers */
0776
0777   set<unsigned> KFbase::kalmanDeadLayers(bool &remove2PSCut) const {
0778     // Kill scenarios described StubKiller.cc
0779
0780     // By which Stress Test scenario (if any) are dead modules being emulated?
0781     const StubKiller::KillOptions killScenario = static_cast<StubKiller::KillOptions>(settings_->killScenario());
0782     // Should TMTT tracking be modified to reduce efficiency loss due to dead modules?
0783     const bool killRecover = settings_->killRecover();
0784
0785     set<pair<unsigned, bool>> deadGPlayers;  // GP layer ID & boolean indicating if in barrel.
0786
0787     // Range of sectors chosen to cover dead regions from StubKiller.
0788     if (killRecover) {
0789       if (killScenario == StubKiller::KillOptions::layer5) {  // barrel layer 5
0790         if (iEtaReg_ >= 3 && iEtaReg_ <= 7 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0791           deadGPlayers.insert(pair<unsigned, bool>(4, true));
0792         }
0793       } else if (killScenario == StubKiller::KillOptions::layer1) {  // barrel layer 1
0794         if (iEtaReg_ <= 7 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0795           deadGPlayers.insert(pair<unsigned, bool>(1, true));
0796         }
0797         remove2PSCut = true;
0798       } else if (killScenario == StubKiller::KillOptions::layer1layer2) {  // barrel layers 1 & 2
0799         if (iEtaReg_ <= 7 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0800           deadGPlayers.insert(pair<unsigned, bool>(1, true));
0801         }
0802         if (iEtaReg_ >= 1 && iEtaReg_ <= 7 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0803           deadGPlayers.insert(pair<unsigned, bool>(2, true));
0804         }
0805         remove2PSCut = true;
0806       } else if (killScenario == StubKiller::KillOptions::layer1disk1) {  // barrel layer 1 & disk 1
0807         if (iEtaReg_ <= 7 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0808           deadGPlayers.insert(pair<unsigned, bool>(1, true));
0809         }
0810         if (iEtaReg_ <= 3 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0811           deadGPlayers.insert(pair<unsigned, bool>(3, false));
0812         }
0813         remove2PSCut = true;
0814       }
0815     }
0816
0817     set<unsigned> kfDeadLayers;
0818     for (const auto &p : deadGPlayers) {
0819       unsigned int layer = p.first;
0820       bool barrel = p.second;
0821       float r = 0.;  // This fails for r-dependent parts of kalmanLayer(). FIX
0822       float z = 999.;
0823       unsigned int kalmanLay = this->kalmanLayer(iEtaReg_, layer, barrel, r, z);
0824       kfDeadLayers.insert(kalmanLay);
0825     }
0826
0827     return kfDeadLayers;
0828   }
0829
0830   //=== Function to calculate approximation for tilted barrel modules (aka B) copied from Stub class.
0831
0832   float KFbase::approxB(float z, float r) const {
0833     return settings_->bApprox_gradient() * std::abs(z) / r + settings_->bApprox_intercept();
0834   }
0835
0836   /* Print truth particle */
0837
0838   void KFbase::printTP(const TP *tp) const {
0839     TVectorD tpParams(5);
0840     bool useForAlgEff(false);
0841     if (tp) {
0842       useForAlgEff = tp->useForAlgEff();
0843       tpParams[QOVERPT] = tp->qOverPt();
0844       tpParams[PHI0] = tp->phi0();
0845       tpParams[Z0] = tp->z0();
0846       tpParams[T] = tp->tanLambda();
0847       tpParams[D0] = tp->d0();
0848     }
0849     std::stringstream text;
0850     text << std::fixed << std::setprecision(4);
0851     if (tp) {
0852       text << "  TP index = " << tp->index() << " useForAlgEff = " << useForAlgEff << " ";
0853       const string helixNames[5] = {"qOverPt", "phi0", "z0", "tanL", "d0"};
0854       for (int i = 0; i < tpParams.GetNrows(); i++) {
0855         text << helixNames[i] << ":" << tpParams[i] << ", ";
0856       }
0857       text << "  inv2R = " << tp->qOverPt() * settings_->invPtToInvR() * 0.5;
0858     } else {
0859       text << "  Fake";
0860     }
0861     PrintL1trk() << text.str();
0862   }
0863
0864   /* Print tracker layers with stubs */
0865
0866   void KFbase::printStubLayers(const vector<Stub *> &stubs, unsigned int iEtaReg) const {
0867     std::stringstream text;
0868     text << std::fixed << std::setprecision(4);
0869     if (stubs.empty())
0870       text << "stub layers = []\n";
0871     else {
0872       text << "stub layers = [ ";
0873       for (unsigned i = 0; i < stubs.size(); i++) {
0874         text << stubs[i]->layerId();
0875         if (i != stubs.size() - 1)
0876           text << ", ";
0877       }
0878       text << " ]   ";
0879       text << "KF stub layers = [ ";
0880       for (unsigned j = 0; j < stubs.size(); j++) {
0881         unsigned int kalmanLay =
0882             this->kalmanLayer(iEtaReg, stubs[j]->layerIdReduced(), stubs[j]->barrel(), stubs[j]->r(), stubs[j]->z());
0883         text << kalmanLay;
0884         if (j != stubs.size() - 1)
0885           text << ", ";
0886       }
0887       text << " ]\n";
0888     }
0889     PrintL1trk() << text.str();
0890   }
0891
0892   /* Print a stub */
0893
0894   void KFbase::printStub(const Stub *stub) const {
0895     std::stringstream text;
0896     text << std::fixed << std::setprecision(4);
0897     text << "stub ";
0898     text << "index=" << stub->index() << " ";
0899     text << "layerId=" << stub->layerId() << " ";
0900     text << "r=" << stub->r() << " ";
0901     text << "phi=" << stub->phi() << " ";
0902     text << "z=" << stub->z() << " ";
0903     text << "sigmaX=" << stub->sigmaPerp() << " ";
0904     text << "sigmaZ=" << stub->sigmaPar() << " ";
0905     text << "TPids=";
0906     std::set<const TP *> tps = stub->assocTPs();
0907     for (auto tp : tps)
0908       text << tp->index() << ",";
0909     PrintL1trk() << text.str();
0910   }
0911
0912   /* Print all stubs */
0913
0914   void KFbase::printStubs(const vector<Stub *> &stubs) const {
0915     for (auto &stub : stubs) {
0916       printStub(stub);
0917     }
0918   }
0919
0920 }  // namespace tmtt