Back to home page

Project CMSSW displayed by LXR

 
 

    


File indexing completed on 2022-05-07 00:41:30

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       constexpr unsigned int invalidKFlayer = 7;
0206       if (kalmanLay != invalidKFlayer) {
0207         if (layerStubs[kalmanLay].size() < settings_->kalmanMaxStubsPerLayer()) {
0208           layerStubs[kalmanLay].push_back(stub);
0209         } else {
0210           // If too many stubs, FW keeps the last stub.
0211           layerStubs[kalmanLay].back() = stub;
0212         }
0213       }
0214     }
0215 
0216     // iterate using state->nextLayer() to determine next Kalman layer(s) to add stubs from
0217     constexpr unsigned int nTypicalLayers = 6;  // Number of tracker layers a typical track can pass through.
0218     // If user asked to add up to 7 layers to track, increase number of iterations by 1.
0219     const unsigned int maxIterations = std::max(nTypicalLayers, settings_->kalmanMaxNumStubs());
0220     for (unsigned iteration = 0; iteration < maxIterations; iteration++) {
0221       int combinations_per_iteration = 0;
0222 
0223       bool easy = (l1track3D.numStubs() < settings_->kalmanMaxStubsEasy());
0224       unsigned int kalmanMaxSkipLayers =
0225           easy ? settings_->kalmanMaxSkipLayersEasy() : settings_->kalmanMaxSkipLayersHard();
0226 
0227       // update each state from previous iteration (or seed) using stubs in next Kalman layer
0228       vector<const KalmanState *>::const_iterator i_state = prev_states.begin();
0229       for (; i_state != prev_states.end(); i_state++) {
0230         const KalmanState *the_state = *i_state;
0231 
0232         unsigned int layer = the_state->nextLayer();  // Get KF layer where stubs to be searched for next
0233         unsigned nSkipped = the_state->nSkippedLayers();
0234 
0235         // If this layer is known to be dead, skip to the next layer (layer+1)
0236         // The next_states_skipped will then look at layer+2
0237         // 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)
0238         // Continue to skip until you reach a functioning layer (or a layer with stubs)
0239         unsigned nSkippedDeadLayers = 0;
0240         unsigned nSkippedAmbiguousLayers = 0;
0241         while (kfDeadLayers.find(layer) != kfDeadLayers.end() && layerStubs[layer].empty()) {
0242           layer += 1;
0243           ++nSkippedDeadLayers;
0244         }
0245         while (this->kalmanAmbiguousLayer(etaReg, layer) && layerStubs[layer].empty()) {
0246           layer += 1;
0247           ++nSkippedAmbiguousLayers;
0248         }
0249 
0250         // containers for updated state+stub combinations
0251         vector<const KalmanState *> next_states;
0252         vector<const KalmanState *> next_states_skipped;
0253 
0254         // find stubs for this layer
0255         // (If layer > 6, this will return empty vector, so safe).
0256         vector<Stub *> thislay_stubs = layerStubs[layer];
0257 
0258         // find stubs for next layer if we skip a layer, except when we are on the penultimate layer,
0259         // or we have exceeded the max skipped layers
0260         vector<Stub *> nextlay_stubs;
0261 
0262         // If the next layer (layer+1) is a dead layer, then proceed to the layer after next (layer+2), if possible
0263         // Also note if we need to increase "skipped" by one more for these states
0264         unsigned nSkippedDeadLayers_nextStubs = 0;
0265         unsigned nSkippedAmbiguousLayers_nextStubs = 0;
0266         if (nSkipped < kalmanMaxSkipLayers) {
0267           if (kfDeadLayers.find(layer + 1) != kfDeadLayers.end() && layerStubs[layer + 1].empty()) {
0268             nextlay_stubs = layerStubs[layer + 2];
0269             nSkippedDeadLayers_nextStubs++;
0270           } else if (this->kalmanAmbiguousLayer(etaReg, layer) && layerStubs[layer + 1].empty()) {
0271             nextlay_stubs = layerStubs[layer + 2];
0272             nSkippedAmbiguousLayers_nextStubs++;
0273           } else {
0274             nextlay_stubs = layerStubs[layer + 1];
0275           }
0276         }
0277 
0278         // If track was not rejected by isGoodState() is previous iteration, failure here usually means the tracker ran out of layers to explore.
0279         // (Due to "kalmanLay" not having unique ID for each layer within a given eta sector).
0280         if (settings_->kalmanDebugLevel() >= 2 && best_state_by_nstubs.empty() && thislay_stubs.empty() &&
0281             nextlay_stubs.empty())
0282           PrintL1trk() << "State is lost by start of iteration " << iteration
0283                        << " : #thislay_stubs=" << thislay_stubs.size() << " #nextlay_stubs=" << nextlay_stubs.size()
0284                        << " layer=" << layer << " eta=" << l1track3D.iEtaReg();
0285 
0286         // If we skipped over a dead layer, only increment "nSkipped" after the stubs in next+1 layer have been obtained
0287         nSkipped += nSkippedDeadLayers;
0288         nSkipped += nSkippedAmbiguousLayers;
0289 
0290         // check to guarantee no fewer than 2PS hits per state at iteration 1
0291         // (iteration 0 will always include a PS hit, but iteration 1 could use 2S hits
0292         // unless we include this)
0293         if (iteration == 1 && !remove2PSCut) {
0294           vector<Stub *> temp_thislaystubs;
0295           vector<Stub *> temp_nextlaystubs;
0296           for (auto stub : thislay_stubs) {
0297             if (stub->psModule())
0298               temp_thislaystubs.push_back(stub);
0299           }
0300           for (auto stub : nextlay_stubs) {
0301             if (stub->psModule())
0302               temp_nextlaystubs.push_back(stub);
0303           }
0304           thislay_stubs = temp_thislaystubs;
0305           nextlay_stubs = temp_nextlaystubs;
0306         }
0307 
0308         combinations_per_iteration += thislay_stubs.size() + nextlay_stubs.size();
0309 
0310         // loop over each stub in this layer and check for compatibility with this state
0311         for (unsigned i = 0; i < thislay_stubs.size(); i++) {
0312           Stub *stub = thislay_stubs[i];
0313 
0314           // Update helix params by adding this stub.
0315           const KalmanState *new_state = kalmanUpdate(nSkipped, layer, stub, the_state, tpa);
0316 
0317           // Cut on track chi2, pt etc.
0318           if (isGoodState(*new_state))
0319             next_states.push_back(new_state);
0320         }
0321 
0322         // loop over each stub in next layer if we skip, and check for compatibility with this state
0323         for (unsigned i = 0; i < nextlay_stubs.size(); i++) {
0324           Stub *stub = nextlay_stubs[i];
0325 
0326           const KalmanState *new_state =
0327               kalmanUpdate(nSkipped + 1 + nSkippedDeadLayers_nextStubs + nSkippedAmbiguousLayers_nextStubs,
0328                            layer + 1 + nSkippedDeadLayers_nextStubs + nSkippedAmbiguousLayers_nextStubs,
0329                            stub,
0330                            the_state,
0331                            tpa);
0332 
0333           if (isGoodState(*new_state))
0334             next_states_skipped.push_back(new_state);
0335         }
0336 
0337         // post Kalman filter local sorting per state
0338         auto orderByChi2 = [](const KalmanState *a, const KalmanState *b) {
0339           return bool(a->chi2scaled() < b->chi2scaled());
0340         };
0341         sort(next_states.begin(), next_states.end(), orderByChi2);
0342         sort(next_states_skipped.begin(), next_states_skipped.end(), orderByChi2);
0343 
0344         new_states.insert(new_states.end(), next_states.begin(), next_states.end());
0345         new_states.insert(new_states.end(), next_states_skipped.begin(), next_states_skipped.end());
0346         /*
0347         i = 0;
0348         for (auto state : next_states) {
0349             new_states.push_back(state);
0350           i++;
0351         }
0352 
0353         i = 0;
0354         for (auto state : next_states_skipped) {
0355             new_states.push_back(state);
0356           i++;
0357         }
0358 */
0359       }  //end of state loop
0360 
0361       // copy new_states into prev_states for next iteration or end if we are on
0362       // last iteration by clearing all states and making final state selection
0363 
0364       auto orderByMinSkipChi2 = [](const KalmanState *a, const KalmanState *b) {
0365         return bool((a->chi2scaled()) * (a->nSkippedLayers() + 1) < (b->chi2scaled()) * (b->nSkippedLayers() + 1));
0366       };
0367       sort(new_states.begin(), new_states.end(), orderByMinSkipChi2);  // Sort by chi2*(skippedLayers+1)
0368 
0369       unsigned int nStubs = iteration + 1;
0370       // Success. We have at least one state that passes all cuts. Save best state found with this number of stubs.
0371       if (nStubs >= settings_->kalmanMinNumStubs() && not new_states.empty())
0372         best_state_by_nstubs[nStubs] = new_states[0];
0373 
0374       if (nStubs == settings_->kalmanMaxNumStubs()) {
0375         // We're done.
0376         prev_states.clear();
0377         new_states.clear();
0378 
0379       } else {
0380         // Continue iterating.
0381         prev_states = new_states;
0382         new_states.clear();
0383       }
0384     }
0385 
0386     if (not best_state_by_nstubs.empty()) {
0387       // Select state with largest number of stubs.
0388       finished_state = best_state_by_nstubs.begin()->second;  // First element has largest number of stubs.
0389       if (settings_->kalmanDebugLevel() >= 1) {
0390         std::stringstream text;
0391         text << std::fixed << std::setprecision(4);
0392         text << "Track found! final state selection: nLay=" << finished_state->nStubLayers()
0393              << " hitPattern=" << std::hex << finished_state->hitPattern() << std::dec
0394              << " phiSec=" << l1track3D.iPhiSec() << " etaReg=" << l1track3D.iEtaReg() << " HT(m,c)=("
0395              << l1track3D.cellLocationHT().first << "," << l1track3D.cellLocationHT().second << ")";
0396         TVectorD y = trackParams(finished_state);
0397         text << " q/pt=" << y[QOVERPT] << " tanL=" << y[T] << " z0=" << y[Z0] << " phi0=" << y[PHI0];
0398         if (nHelixPar_ == 5)
0399           text << " d0=" << y[D0];
0400         text << " chosen from states:";
0401         for (const auto &p : best_state_by_nstubs)
0402           text << " " << p.second->chi2() << "/" << p.second->nStubLayers();
0403         PrintL1trk() << text.str();
0404       }
0405     } else {
0406       if (settings_->kalmanDebugLevel() >= 1) {
0407         PrintL1trk() << "Track lost";
0408       }
0409     }
0410 
0411     return finished_state;
0412   }
0413 
0414   /*--- Update a helix state by adding a stub. */
0415 
0416   const KalmanState *KFbase::kalmanUpdate(
0417       unsigned nSkipped, unsigned int layer, Stub *stub, const KalmanState *state, const TP *tpa) {
0418     if (settings_->kalmanDebugLevel() >= 4) {
0419       PrintL1trk() << "---------------";
0420       PrintL1trk() << "kalmanUpdate";
0421       PrintL1trk() << "---------------";
0422       printStub(stub);
0423     }
0424 
0425     numUpdateCalls_++;  // For monitoring, count calls to updator per track.
0426 
0427     // Helix params & their covariance.
0428     TVectorD vecX = state->vectorX();
0429     TMatrixD matC = state->matrixC();
0430     if (state->barrel() && !stub->barrel()) {
0431       if (settings_->kalmanDebugLevel() >= 4) {
0432         PrintL1trk() << "STATE BARREL TO ENDCAP BEFORE ";
0433         PrintL1trk() << "state : " << vecX[0] << " " << vecX[1] << " " << vecX[2] << " " << vecX[3];
0434         PrintL1trk() << "cov(x): ";
0435         matC.Print();
0436       }
0437       if (settings_->kalmanDebugLevel() >= 4) {
0438         PrintL1trk() << "STATE BARREL TO ENDCAP AFTER ";
0439         PrintL1trk() << "state : " << vecX[0] << " " << vecX[1] << " " << vecX[2] << " " << vecX[3];
0440         PrintL1trk() << "cov(x): ";
0441         matC.Print();
0442       }
0443     }
0444     // Matrix to propagate helix reference point from one layer to next.
0445     TMatrixD matF = matrixF(stub, state);
0446     TMatrixD matFtrans(TMatrixD::kTransposed, matF);
0447     if (settings_->kalmanDebugLevel() >= 4) {
0448       PrintL1trk() << "matF";
0449       matF.Print();
0450     }
0451 
0452     // Multiply matrices to get helix params relative to reference point at next layer.
0453     TVectorD vecXref = matF * vecX;
0454     if (settings_->kalmanDebugLevel() >= 4) {
0455       PrintL1trk() << "vecFref = [";
0456       for (unsigned i = 0; i < nHelixPar_; i++)
0457         PrintL1trk() << vecXref[i] << ", ";
0458       PrintL1trk() << "]";
0459     }
0460 
0461     // Get stub residuals.
0462     TVectorD delta = residual(stub, vecXref, state->candidate().qOverPt());
0463     if (settings_->kalmanDebugLevel() >= 4) {
0464       PrintL1trk() << "delta = " << delta[0] << ", " << delta[1];
0465     }
0466 
0467     // Derivative of predicted (phi,z) intercept with layer w.r.t. helix params.
0468     TMatrixD matH = matrixH(stub);
0469     if (settings_->kalmanDebugLevel() >= 4) {
0470       PrintL1trk() << "matH";
0471       matH.Print();
0472     }
0473 
0474     if (settings_->kalmanDebugLevel() >= 4) {
0475       PrintL1trk() << "previous state covariance";
0476       matC.Print();
0477     }
0478     // Get scattering contribution to helix parameter covariance (currently zero).
0479     TMatrixD matScat(nHelixPar_, nHelixPar_);
0480 
0481     // Get covariance on helix parameters at new reference point including scattering..
0482     TMatrixD matCref = matF * matC * matFtrans + matScat;
0483     if (settings_->kalmanDebugLevel() >= 4) {
0484       PrintL1trk() << "matCref";
0485       matCref.Print();
0486     }
0487     // Get hit position covariance matrix.
0488     TMatrixD matV = matrixV(stub, state);
0489     if (settings_->kalmanDebugLevel() >= 4) {
0490       PrintL1trk() << "matV";
0491       matV.Print();
0492     }
0493 
0494     TMatrixD matRinv = matrixRinv(matH, matCref, matV);
0495     if (settings_->kalmanDebugLevel() >= 4) {
0496       PrintL1trk() << "matRinv";
0497       matRinv.Print();
0498     }
0499 
0500     // Calculate Kalman Gain matrix.
0501     TMatrixD matK = getKalmanGainMatrix(matH, matCref, matRinv);
0502     if (settings_->kalmanDebugLevel() >= 4) {
0503       PrintL1trk() << "matK";
0504       matK.Print();
0505     }
0506 
0507     // Update helix state & its covariance matrix with new stub.
0508     TVectorD new_vecX(nHelixPar_);
0509     TMatrixD new_matC(nHelixPar_, nHelixPar_);
0510     adjustState(matK, matCref, vecXref, matH, delta, new_vecX, new_matC);
0511 
0512     // Update track fit chi2 with new stub.
0513     double new_chi2rphi = 0, new_chi2rz = 0;
0514     this->adjustChi2(state, matRinv, delta, new_chi2rphi, new_chi2rz);
0515 
0516     if (settings_->kalmanDebugLevel() >= 4) {
0517       if (nHelixPar_ == 4)
0518         PrintL1trk() << "adjusted x = " << new_vecX[0] << ", " << new_vecX[1] << ", " << new_vecX[2] << ", "
0519                      << new_vecX[3];
0520       else if (nHelixPar_ == 5)
0521         PrintL1trk() << "adjusted x = " << new_vecX[0] << ", " << new_vecX[1] << ", " << new_vecX[2] << ", "
0522                      << new_vecX[3] << ", " << new_vecX[4];
0523       PrintL1trk() << "adjusted C ";
0524       new_matC.Print();
0525       PrintL1trk() << "adjust chi2rphi=" << new_chi2rphi << " chi2rz=" << new_chi2rz;
0526     }
0527 
0528     const KalmanState *new_state = mkState(
0529         state->candidate(), nSkipped, layer, state, new_vecX, new_matC, matK, matV, stub, new_chi2rphi, new_chi2rz);
0530 
0531     return new_state;
0532   }
0533 
0534   /* Create a KalmanState, containing a helix state & next stub it is to be updated with. */
0535 
0536   const KalmanState *KFbase::mkState(const L1track3D &candidate,
0537                                      unsigned nSkipped,
0538                                      unsigned layer,
0539                                      const KalmanState *last_state,
0540                                      const TVectorD &vecX,
0541                                      const TMatrixD &matC,
0542                                      const TMatrixD &matK,
0543                                      const TMatrixD &matV,
0544                                      Stub *stub,
0545                                      double chi2rphi,
0546                                      double chi2rz) {
0547     auto new_state = std::make_unique<const KalmanState>(
0548         settings_, candidate, nSkipped, layer, last_state, vecX, matC, matK, matV, stub, chi2rphi, chi2rz);
0549 
0550     const KalmanState *p_new_state = new_state.get();
0551     listAllStates_.push_back(std::move(new_state));  // Vector keeps ownership of all states.
0552     return p_new_state;
0553   }
0554 
0555   /* Product of H*C*H(transpose) (where C = helix covariance matrix) */
0556 
0557   TMatrixD KFbase::matrixHCHt(const TMatrixD &matH, const TMatrixD &matC) const {
0558     TMatrixD matHtrans(TMatrixD::kTransposed, matH);
0559     return matH * matC * matHtrans;
0560   }
0561 
0562   /* Get inverted Kalman R matrix: inverse(V + HCHt) */
0563 
0564   TMatrixD KFbase::matrixRinv(const TMatrixD &matH, const TMatrixD &matCref, const TMatrixD &matV) const {
0565     TMatrixD matHCHt = matrixHCHt(matH, matCref);
0566     TMatrixD matR = matV + matHCHt;
0567     TMatrixD matRinv(2, 2);
0568     if (matR.Determinant() > 0) {
0569       matRinv = TMatrixD(TMatrixD::kInverted, matR);
0570     } else {
0571       // Protection against rare maths instability.
0572       const TMatrixD unitMatrix(TMatrixD::kUnit, TMatrixD(nHelixPar_, nHelixPar_));
0573       const double big = 9.9e9;
0574       matRinv = big * unitMatrix;
0575     }
0576     if (settings_->kalmanDebugLevel() >= 4) {
0577       PrintL1trk() << "matHCHt";
0578       matHCHt.Print();
0579       PrintL1trk() << "matR";
0580       matR.Print();
0581     }
0582     return matRinv;
0583   }
0584 
0585   /* Determine Kalman gain matrix K */
0586 
0587   TMatrixD KFbase::getKalmanGainMatrix(const TMatrixD &matH, const TMatrixD &matCref, const TMatrixD &matRinv) const {
0588     TMatrixD matHtrans(TMatrixD::kTransposed, matH);
0589     TMatrixD matCrefht = matCref * matHtrans;
0590     TMatrixD matK = matCrefht * matRinv;
0591     return matK;
0592   }
0593 
0594   /* Calculate stub residual w.r.t. helix */
0595 
0596   TVectorD KFbase::residual(const Stub *stub, const TVectorD &vecX, double candQoverPt) const {
0597     TVectorD vd = vectorM(stub);  // Get (phi relative to sector, z) of hit.
0598     TMatrixD h = matrixH(stub);
0599     TVectorD hx = h * vecX;  // Get intercept of helix with layer (linear approx).
0600     TVectorD delta = vd - hx;
0601 
0602     // Calculate higher order corrections to residuals.
0603 
0604     if (not settings_->kalmanHOfw()) {
0605       TVectorD correction(2);
0606 
0607       float inv2R = (settings_->invPtToInvR()) * 0.5 * candQoverPt;
0608       float tanL = vecX[T];
0609       float z0 = vecX[Z0];
0610 
0611       float deltaS = 0.;
0612       if (settings_->kalmanHOhelixExp()) {
0613         // Higher order correction correction to circle expansion for improved accuracy at low Pt.
0614         double corr = stub->r() * inv2R;
0615 
0616         // N.B. In endcap 2S, this correction to correction[0] is exactly cancelled by the deltaS-dependent correction to it below.
0617         correction[0] += (1. / 6.) * pow(corr, 3);
0618 
0619         deltaS = (1. / 6.) * (stub->r()) * pow(corr, 2);
0620         correction[1] -= deltaS * tanL;
0621       }
0622 
0623       if ((not stub->barrel()) && not(stub->psModule())) {
0624         // These corrections rely on inside --> outside tracking, so r-z track params in 2S modules known.
0625         float rShift = (stub->z() - z0) / tanL - stub->r();
0626 
0627         if (settings_->kalmanHOhelixExp())
0628           rShift -= deltaS;
0629 
0630         if (settings_->kalmanHOprojZcorr() == 1) {
0631           // Add correlation term related to conversion of stub residuals from (r,phi) to (z,phi).
0632           correction[0] += inv2R * rShift;
0633         }
0634 
0635         if (settings_->kalmanHOalpha() == 1) {
0636           // Add alpha correction for non-radial 2S endcap strips..
0637           correction[0] += stub->alpha() * rShift;
0638         }
0639       }
0640 
0641       // Apply correction to residuals.
0642       delta += correction;
0643     }
0644 
0645     delta[0] = reco::deltaPhi(delta[0], 0.);
0646 
0647     return delta;
0648   }
0649 
0650   /* Update helix state & its covariance matrix with new stub */
0651 
0652   void KFbase::adjustState(const TMatrixD &matK,
0653                            const TMatrixD &matCref,
0654                            const TVectorD &vecXref,
0655                            const TMatrixD &matH,
0656                            const TVectorD &delta,
0657                            TVectorD &new_vecX,
0658                            TMatrixD &new_matC) const {
0659     new_vecX = vecXref + matK * delta;
0660     const TMatrixD unitMatrix(TMatrixD::kUnit, TMatrixD(nHelixPar_, nHelixPar_));
0661     TMatrixD tmp = unitMatrix - matK * matH;
0662     new_matC = tmp * matCref;
0663   }
0664 
0665   /* Update track fit chi2 with new stub */
0666 
0667   void KFbase::adjustChi2(const KalmanState *state,
0668                           const TMatrixD &matRinv,
0669                           const TVectorD &delta,
0670                           double &chi2rphi,
0671                           double &chi2rz) const {
0672     // Change in chi2 (with r-phi/r-z correlation term included in r-phi component)
0673     double delChi2rphi = delta[PHI] * delta[PHI] * matRinv[PHI][PHI] + 2 * delta[PHI] * delta[Z] * matRinv[PHI][Z];
0674     double delChi2rz = delta[Z] * delta[Z] * matRinv[Z][Z];
0675 
0676     if (settings_->kalmanDebugLevel() >= 4) {
0677       PrintL1trk() << "delta(chi2rphi)=" << delChi2rphi << " delta(chi2rz)= " << delChi2rz;
0678     }
0679     chi2rphi = state->chi2rphi() + delChi2rphi;
0680     chi2rz = state->chi2rz() + delChi2rz;
0681     return;
0682   }
0683 
0684   /* Reset internal data ready for next track. */
0685 
0686   void KFbase::resetStates() { listAllStates_.clear(); }
0687 
0688   /* Get Kalman layer mapping (i.e. layer order in which stubs should be processed) */
0689 
0690   unsigned int KFbase::kalmanLayer(
0691       unsigned int iEtaReg, unsigned int layerIDreduced, bool barrel, float r, float z) const {
0692     // index across is GP encoded layer ID (where barrel layers=1,2,7,5,4,3 & endcap wheels=3,4,5,6,7 & 0 never occurs)
0693     // index down is eta reg
0694     // element is kalman layer, where 7 is invalid
0695 
0696     // If stub with given GP encoded layer ID can have different KF layer ID depending on whether it
0697     // is barrel or endcap, then in layerMap, the the barrel case is assumed.
0698     // The endcap case is fixed by hand later in this function.
0699 
0700     const unsigned int nEta = 16;
0701     const unsigned int nGPlayID = 7;
0702 
0703     if (nEta != numEtaRegions_)
0704       throw cms::Exception("LogicError")
0705           << "ERROR KFbase::getKalmanLayer hardwired value of nEta differs from NumEtaRegions cfg param";
0706 
0707     // In cases where identical GP encoded layer ID present in this sector from both barrel & endcap, this array filled considering barrel. The endcap is fixed by subsequent code.
0708 
0709     constexpr unsigned layerMap[nEta / 2][nGPlayID + 1] = {
0710         {7, 0, 1, 5, 4, 3, 7, 2},  // B1 B2 B3 B4 B5 B6 -- current FW
0711         {7, 0, 1, 5, 4, 3, 7, 2},  // B1 B2 B3 B4 B5 B6
0712         {7, 0, 1, 5, 4, 3, 7, 2},  // B1 B2 B3 B4 B5 B6
0713         {7, 0, 1, 5, 4, 3, 7, 2},  // B1 B2 B3 B4 B5 B6
0714         {7, 0, 1, 5, 4, 3, 7, 2},  // B1 B2 B3 B4(/D3) B5(/D2) B6(/D1)
0715         {7, 0, 1, 3, 4, 2, 6, 2},  // B1 B2 B3(/D5)+B4(/D3) D1 D2 X D4
0716         {7, 0, 1, 1, 2, 3, 4, 5},  // B1 B2+D1 D2 D3 D5 D6
0717         {7, 0, 7, 1, 2, 3, 4, 5},  // B1 D1 D2 D3 D4 D5
0718     };
0719 
0720     unsigned int kfEtaReg;  // KF VHDL eta sector def: small in barrel & large in endcap.
0721     if (iEtaReg < numEtaRegions_ / 2) {
0722       kfEtaReg = numEtaRegions_ / 2 - 1 - iEtaReg;
0723     } else {
0724       kfEtaReg = iEtaReg - numEtaRegions_ / 2;
0725     }
0726 
0727     unsigned int kalmanLay = layerMap[kfEtaReg][layerIDreduced];
0728 
0729     // Fixes to layermap when "maybe layer" used
0730     if (settings_->kfUseMaybeLayers()) {
0731       switch (kfEtaReg) {
0732         case 5:  //case 5: B1 B2 (B3+B4)* D1 D2 D3+D4 D5+D6  -- B3 is combined with B4 and is flagged as "maybe layer"
0733           if (layerIDreduced == 6) {
0734             kalmanLay = 5;
0735           }
0736           break;
0737         case 6:  //case 6: B1* B2* D1 D2 D3 D4 D5 -- B1 and B2 are flagged as "maybe layer"
0738           if (layerIDreduced > 2) {
0739             kalmanLay++;
0740           }
0741           break;
0742         default:
0743           break;
0744       }
0745     }
0746 
0747     // Fixes to endcap stubs, for cases where identical GP encoded layer ID present in this sector from both barrel & endcap.
0748 
0749     if (not barrel) {
0750       switch (kfEtaReg) {
0751         case 4:  // B1 B2 B3 B4 B5/D1 B6/D2 D3
0752           if (layerIDreduced == 3) {
0753             kalmanLay = 4;
0754           } else if (layerIDreduced == 4) {
0755             kalmanLay = 5;
0756           } else if (layerIDreduced == 5) {
0757             kalmanLay = 6;
0758           }
0759           break;
0760           //case 5:  // B1 B2 B3+B4 D1 D2 D3 D4/D5
0761         case 5:  // B1 B2 B3 D1+B4 D2 D3 D4/D5
0762           if (layerIDreduced == 5) {
0763             kalmanLay = 5;
0764           } else if (layerIDreduced == 7) {
0765             kalmanLay = 6;
0766           }
0767           break;
0768         default:
0769           break;
0770       }
0771     }
0772 
0773     /*
0774   // Fix cases where a barrel layer only partially crosses the eta sector.
0775   // (Logically should work, but actually reduces efficiency -- INVESTIGATE).
0776 
0777   const float barrelHalfLength = 120.;
0778   const float barrel4Radius = 68.8;
0779   const float barrel5Radius = 86.1;
0780   
0781   if ( not barrel) {
0782     switch ( kfEtaReg ) {
0783     case 4:
0784       if (layerIDreduced==3) {  // D1
0785         float disk1_rCut = barrel5Radius*(std::abs(z)/barrelHalfLength); 
0786         if (r > disk1_rCut) kalmanLay++;
0787       }
0788       break;
0789     case 5:
0790       if (layerIDreduced==3) { // D1
0791         float disk1_rCut = barrel4Radius*(std::abs(z)/barrelHalfLength); 
0792         if (r > disk1_rCut) kalmanLay++;
0793       }
0794       if (layerIDreduced==4) { // D2
0795         float disk2_rCut = barrel4Radius*(std::abs(z)/barrelHalfLength); 
0796         if (r > disk2_rCut) kalmanLay++;
0797       }
0798       break;
0799     default:
0800       break;
0801     }           
0802   }
0803   */
0804 
0805     return kalmanLay;
0806   }
0807 
0808   /*=== Check if particles in given eta sector are uncertain to go through the given KF layer. */
0809   /*=== (If so, count layer for numbers of hit layers, but not for number of skipped layers). */
0810 
0811   bool KFbase::kalmanAmbiguousLayer(unsigned int iEtaReg, unsigned int kfLayer) {
0812     // Only helps in extreme forward sector, and there not significantly.
0813     // UNDERSTAND IF CAN BE USED ELSEWHERE.
0814 
0815     const unsigned int nEta = 16;
0816     const unsigned int nKFlayer = 7;
0817     constexpr bool ambiguityMap[nEta / 2][nKFlayer] = {
0818         {false, false, false, false, false, false, false},
0819         {false, false, false, false, false, false, false},
0820         {false, false, false, false, false, false, false},
0821         {false, false, false, false, false, false, false},
0822         {false, false, false, false, false, false, false},
0823         {false, false, true, false, false, false, false},
0824         {true, true, false, false, false, false, false},
0825         {true, false, false, false, false, false, false},
0826     };
0827 
0828     unsigned int kfEtaReg;  // KF VHDL eta sector def: small in barrel & large in endcap.
0829     if (iEtaReg < numEtaRegions_ / 2) {
0830       kfEtaReg = numEtaRegions_ / 2 - 1 - iEtaReg;
0831     } else {
0832       kfEtaReg = iEtaReg - numEtaRegions_ / 2;
0833     }
0834 
0835     bool ambiguous = false;
0836     if (settings_->kfUseMaybeLayers() and kfLayer < nKFlayer)
0837       ambiguous = ambiguityMap[kfEtaReg][kfLayer];
0838 
0839     return ambiguous;
0840   }
0841 
0842   /* Adjust KF algorithm to allow for any dead tracker layers */
0843 
0844   set<unsigned> KFbase::kalmanDeadLayers(bool &remove2PSCut) const {
0845     // Kill scenarios described StubKiller.cc
0846 
0847     // By which Stress Test scenario (if any) are dead modules being emulated?
0848     const StubKiller::KillOptions killScenario = static_cast<StubKiller::KillOptions>(settings_->killScenario());
0849     // Should TMTT tracking be modified to reduce efficiency loss due to dead modules?
0850     const bool killRecover = settings_->killRecover();
0851 
0852     set<pair<unsigned, bool>> deadGPlayers;  // GP layer ID & boolean indicating if in barrel.
0853 
0854     // Range of sectors chosen to cover dead regions from StubKiller.
0855     if (killRecover) {
0856       if (killScenario == StubKiller::KillOptions::layer5) {  // barrel layer 5
0857         if (iEtaReg_ >= 3 && iEtaReg_ <= 7 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0858           deadGPlayers.insert(pair<unsigned, bool>(4, true));
0859         }
0860       } else if (killScenario == StubKiller::KillOptions::layer1) {  // barrel layer 1
0861         if (iEtaReg_ <= 7 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0862           deadGPlayers.insert(pair<unsigned, bool>(1, true));
0863         }
0864         remove2PSCut = true;
0865       } else if (killScenario == StubKiller::KillOptions::layer1layer2) {  // barrel layers 1 & 2
0866         if (iEtaReg_ <= 7 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0867           deadGPlayers.insert(pair<unsigned, bool>(1, true));
0868         }
0869         if (iEtaReg_ >= 1 && iEtaReg_ <= 7 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0870           deadGPlayers.insert(pair<unsigned, bool>(2, true));
0871         }
0872         remove2PSCut = true;
0873       } else if (killScenario == StubKiller::KillOptions::layer1disk1) {  // barrel layer 1 & disk 1
0874         if (iEtaReg_ <= 7 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0875           deadGPlayers.insert(pair<unsigned, bool>(1, true));
0876         }
0877         if (iEtaReg_ <= 3 && iPhiSec_ >= 1 && iPhiSec_ <= 5) {
0878           deadGPlayers.insert(pair<unsigned, bool>(3, false));
0879         }
0880         remove2PSCut = true;
0881       }
0882     }
0883 
0884     set<unsigned> kfDeadLayers;
0885     for (const auto &p : deadGPlayers) {
0886       unsigned int layer = p.first;
0887       bool barrel = p.second;
0888       float r = 0.;  // This fails for r-dependent parts of kalmanLayer(). FIX
0889       float z = 999.;
0890       unsigned int kalmanLay = this->kalmanLayer(iEtaReg_, layer, barrel, r, z);
0891       kfDeadLayers.insert(kalmanLay);
0892     }
0893 
0894     return kfDeadLayers;
0895   }
0896 
0897   //=== Function to calculate approximation for tilted barrel modules (aka B) copied from Stub class.
0898 
0899   float KFbase::approxB(float z, float r) const {
0900     return settings_->bApprox_gradient() * std::abs(z) / r + settings_->bApprox_intercept();
0901   }
0902 
0903   /* Print truth particle */
0904 
0905   void KFbase::printTP(const TP *tp) const {
0906     TVectorD tpParams(5);
0907     bool useForAlgEff(false);
0908     if (tp) {
0909       useForAlgEff = tp->useForAlgEff();
0910       tpParams[QOVERPT] = tp->qOverPt();
0911       tpParams[PHI0] = tp->phi0();
0912       tpParams[Z0] = tp->z0();
0913       tpParams[T] = tp->tanLambda();
0914       tpParams[D0] = tp->d0();
0915     }
0916     std::stringstream text;
0917     text << std::fixed << std::setprecision(4);
0918     if (tp) {
0919       text << "  TP index = " << tp->index() << " useForAlgEff = " << useForAlgEff << " ";
0920       const string helixNames[5] = {"qOverPt", "phi0", "z0", "tanL", "d0"};
0921       for (int i = 0; i < tpParams.GetNrows(); i++) {
0922         text << helixNames[i] << ":" << tpParams[i] << ", ";
0923       }
0924       text << "  inv2R = " << tp->qOverPt() * settings_->invPtToInvR() * 0.5;
0925     } else {
0926       text << "  Fake";
0927     }
0928     PrintL1trk() << text.str();
0929   }
0930 
0931   /* Print tracker layers with stubs */
0932 
0933   void KFbase::printStubLayers(const vector<Stub *> &stubs, unsigned int iEtaReg) const {
0934     std::stringstream text;
0935     text << std::fixed << std::setprecision(4);
0936     if (stubs.empty())
0937       text << "stub layers = []\n";
0938     else {
0939       text << "stub layers = [ ";
0940       for (unsigned i = 0; i < stubs.size(); i++) {
0941         text << stubs[i]->layerId();
0942         if (i != stubs.size() - 1)
0943           text << ", ";
0944       }
0945       text << " ]   ";
0946       text << "KF stub layers = [ ";
0947       for (unsigned j = 0; j < stubs.size(); j++) {
0948         unsigned int kalmanLay =
0949             this->kalmanLayer(iEtaReg, stubs[j]->layerIdReduced(), stubs[j]->barrel(), stubs[j]->r(), stubs[j]->z());
0950         text << kalmanLay;
0951         if (j != stubs.size() - 1)
0952           text << ", ";
0953       }
0954       text << " ]\n";
0955     }
0956     PrintL1trk() << text.str();
0957   }
0958 
0959   /* Print a stub */
0960 
0961   void KFbase::printStub(const Stub *stub) const {
0962     std::stringstream text;
0963     text << std::fixed << std::setprecision(4);
0964     text << "stub ";
0965     text << "index=" << stub->index() << " ";
0966     text << "layerId=" << stub->layerId() << " ";
0967     text << "r=" << stub->r() << " ";
0968     text << "phi=" << stub->phi() << " ";
0969     text << "z=" << stub->z() << " ";
0970     text << "sigmaX=" << stub->sigmaPerp() << " ";
0971     text << "sigmaZ=" << stub->sigmaPar() << " ";
0972     text << "TPids=";
0973     std::set<const TP *> tps = stub->assocTPs();
0974     for (auto tp : tps)
0975       text << tp->index() << ",";
0976     PrintL1trk() << text.str();
0977   }
0978 
0979   /* Print all stubs */
0980 
0981   void KFbase::printStubs(const vector<Stub *> &stubs) const {
0982     for (auto &stub : stubs) {
0983       printStub(stub);
0984     }
0985   }
0986 
0987 }  // namespace tmtt