Back to home page

Project CMSSW displayed by LXR
 
 

    

File indexing completed on 2023-03-17 11:12:36 

0001 #include <cmath>
0002 #include "L1Trigger/L1TMuonBarrel/interface/L1TMuonBarrelKalmanAlgo.h"
0003 #include "ap_int.h"
0004 #include "ap_fixed.h"
0005 
0006 L1TMuonBarrelKalmanAlgo::L1TMuonBarrelKalmanAlgo(const edm::ParameterSet& settings)
0007     : verbose_(settings.getParameter<bool>("verbose")),
0008       lutService_(new L1TMuonBarrelKalmanLUTs(settings.getParameter<std::string>("lutFile"))),
0009       initK_(settings.getParameter<std::vector<double> >("initialK")),
0010       initK2_(settings.getParameter<std::vector<double> >("initialK2")),
0011       eLoss_(settings.getParameter<std::vector<double> >("eLoss")),
0012       aPhi_(settings.getParameter<std::vector<double> >("aPhi")),
0013       aPhiB_(settings.getParameter<std::vector<double> >("aPhiB")),
0014       aPhiBNLO_(settings.getParameter<std::vector<double> >("aPhiBNLO")),
0015       bPhi_(settings.getParameter<std::vector<double> >("bPhi")),
0016       bPhiB_(settings.getParameter<std::vector<double> >("bPhiB")),
0017       phiAt2_(settings.getParameter<double>("phiAt2")),
0018 
0019       chiSquare_(settings.getParameter<std::vector<double> >("chiSquare")),
0020       chiSquareCutPattern_(settings.getParameter<std::vector<int> >("chiSquareCutPattern")),
0021       chiSquareCutCurv_(settings.getParameter<std::vector<int> >("chiSquareCutCurvMax")),
0022       chiSquareCut_(settings.getParameter<std::vector<int> >("chiSquareCut")),
0023       trackComp_(settings.getParameter<std::vector<double> >("trackComp")),
0024       trackCompErr1_(settings.getParameter<std::vector<double> >("trackCompErr1")),
0025       trackCompErr2_(settings.getParameter<std::vector<double> >("trackCompErr2")),
0026       trackCompPattern_(settings.getParameter<std::vector<int> >("trackCompCutPattern")),
0027       trackCompCutCurv_(settings.getParameter<std::vector<int> >("trackCompCutCurvMax")),
0028       trackCompCut_(settings.getParameter<std::vector<int> >("trackCompCut")),
0029       chiSquareCutTight_(settings.getParameter<std::vector<int> >("chiSquareCutTight")),
0030       combos4_(settings.getParameter<std::vector<int> >("combos4")),
0031       combos3_(settings.getParameter<std::vector<int> >("combos3")),
0032       combos2_(settings.getParameter<std::vector<int> >("combos2")),
0033       combos1_(settings.getParameter<std::vector<int> >("combos1")),
0034 
0035       useOfflineAlgo_(settings.getParameter<bool>("useOfflineAlgo")),
0036       mScatteringPhi_(settings.getParameter<std::vector<double> >("mScatteringPhi")),
0037       mScatteringPhiB_(settings.getParameter<std::vector<double> >("mScatteringPhiB")),
0038       pointResolutionPhi_(settings.getParameter<double>("pointResolutionPhi")),
0039       pointResolutionPhiB_(settings.getParameter<double>("pointResolutionPhiB")),
0040       pointResolutionPhiBH_(settings.getParameter<std::vector<double> >("pointResolutionPhiBH")),
0041       pointResolutionPhiBL_(settings.getParameter<std::vector<double> >("pointResolutionPhiBL")),
0042       pointResolutionVertex_(settings.getParameter<double>("pointResolutionVertex"))
0043 
0044 {}
0045 
0046 std::pair<bool, uint> L1TMuonBarrelKalmanAlgo::getByCode(const L1MuKBMTrackCollection& tracks, int mask) {
0047   for (uint i = 0; i < tracks.size(); ++i) {
0048     printf("Code=%d, track=%d\n", tracks[i].hitPattern(), mask);
0049     if (tracks[i].hitPattern() == mask)
0050       return std::make_pair(true, i);
0051   }
0052   return std::make_pair(false, 0);
0053 }
0054 
0055 l1t::RegionalMuonCand L1TMuonBarrelKalmanAlgo::convertToBMTF(const L1MuKBMTrack& track) {
0056   //  int  K = fabs(track.curvatureAtVertex());
0057 
0058   //calibration
0059   int sign, signValid;
0060 
0061   if (track.curvatureAtVertex() == 0) {
0062     sign = 0;
0063     signValid = 0;
0064   } else if (track.curvatureAtVertex() > 0) {
0065     sign = 0;
0066     signValid = 1;
0067   } else {
0068     sign = 1;
0069     signValid = 1;
0070   }
0071 
0072   // if (K<22)
0073   //   K=22;
0074 
0075   // if (K>4095)
0076   //   K=4095;
0077 
0078   int pt = ptLUT(track.curvatureAtVertex());
0079 
0080   // int  K2 = fabs(track.curvatureAtMuon());
0081   // if (K2<22)
0082   //   K2=22;
0083 
0084   // if (K2>4095)
0085   //   K2=4095;
0086   int pt2 = ptLUT(track.curvatureAtMuon()) / 2;
0087   int eta = track.hasFineEta() ? track.fineEta() : track.coarseEta();
0088 
0089   //  int phi2 = track.phiAtMuon()>>2;
0090   //  float phi_f = float(phi2);
0091   //double kPhi = 57.2958/0.625/1024.;
0092   //int phi = 24+int(floor(kPhi*phi_f));
0093   //  if (phi >  69) phi =  69;
0094   //  if (phi < -8) phi = -8;
0095   int phi2 = track.phiAtMuon() >> 2;
0096   int tmp = fp_product(0.0895386, phi2, 14);
0097   int phi = 24 + tmp;
0098 
0099   int processor = track.sector();
0100   int HF = track.hasFineEta();
0101 
0102   int quality = 12 | (rank(track) >> 6);
0103 
0104   int dxy = abs(track.dxy()) >> 8;
0105   if (dxy > 3)
0106     dxy = 3;
0107 
0108   int trackAddr;
0109   std::map<int, int> addr = trackAddress(track, trackAddr);
0110 
0111   l1t::RegionalMuonCand muon(pt, phi, eta, sign, signValid, quality, processor, l1t::bmtf, addr);
0112   muon.setHwHF(HF);
0113   muon.setHwPtUnconstrained(pt2);
0114   muon.setHwDXY(dxy);
0115 
0116   //nw the words!
0117   uint32_t word1 = pt;
0118   word1 = word1 | quality << 9;
0119   word1 = word1 | (twosCompToBits(eta)) << 13;
0120   word1 = word1 | HF << 22;
0121   word1 = word1 | (twosCompToBits(phi)) << 23;
0122 
0123   uint32_t word2 = sign;
0124   word2 = word2 | signValid << 1;
0125   word2 = word2 | dxy << 2;
0126   word2 = word2 | trackAddr << 4;
0127   word2 = word2 | (twosCompToBits(track.wheel())) << 20;
0128   word2 = word2 | pt2 << 23;
0129   muon.setDataword(word2, word1);
0130   return muon;
0131 }
0132 
0133 void L1TMuonBarrelKalmanAlgo::addBMTFMuon(int bx,
0134                                           const L1MuKBMTrack& track,
0135                                           std::unique_ptr<l1t::RegionalMuonCandBxCollection>& out) {
0136   out->push_back(bx, convertToBMTF(track));
0137 }
0138 
0139 // std::pair<bool,uint> L1TMuonBarrelKalmanAlgo::match(const L1MuKBMTCombinedStubRef& seed, const L1MuKBMTCombinedStubRefVector& stubs,int step) {
0140 //   L1MuKBMTCombinedStubRefVector selected;
0141 
0142 //   bool found=false;
0143 //   uint best=0;
0144 //   int distance=100000;
0145 //   uint N=0;
0146 //   for (const auto& stub :stubs)  {
0147 //     N=N+1;
0148 //     if (stub->stNum()!=step)
0149 //       continue;
0150 
0151 //     int d = fabs(wrapAround(((correctedPhi(seed,seed->scNum())-correctedPhi(stub,seed->scNum()))>>3),1024));
0152 //     if (d<distance) {
0153 //       distance = d;
0154 //       best=N-1;
0155 //       found=true;
0156 //     }
0157 //   }
0158 //   return std::make_pair(found,best);
0159 // }
0160 
0161 uint L1TMuonBarrelKalmanAlgo::matchAbs(std::map<uint, uint>& info, uint i, uint j) {
0162   if (info[i] < info[j])
0163     return i;
0164   else
0165     return j;
0166 }
0167 
0168 std::pair<bool, uint> L1TMuonBarrelKalmanAlgo::match(const L1MuKBMTCombinedStubRef& seed,
0169                                                      const L1MuKBMTCombinedStubRefVector& stubs,
0170                                                      int step) {
0171   L1MuKBMTCombinedStubRefVector selected;
0172 
0173   std::map<uint, uint> diffInfo;
0174   for (uint i = 0; i < 12; ++i) {
0175     diffInfo[i] = 60000;
0176   }
0177 
0178   std::map<uint, uint> stubInfo;
0179 
0180   int sector = seed->scNum();
0181   int previousSector = sector - 1;
0182   int nextSector = sector + 1;
0183   if (sector == 0) {
0184     previousSector = 11;
0185   }
0186   if (sector == 11) {
0187     nextSector = 0;
0188   }
0189 
0190   int wheel = seed->whNum();
0191   int innerWheel = 0;
0192   if (wheel == -2)
0193     innerWheel = -1;
0194   if (wheel == -1)
0195     innerWheel = 0;
0196   if (wheel == 0)
0197     innerWheel = 1982;
0198   if (wheel == 1)
0199     innerWheel = 0;
0200   if (wheel == 2)
0201     innerWheel = 1;
0202 
0203   //First align the data
0204   uint N = 0;
0205   for (const auto& stub : stubs) {
0206     N = N + 1;
0207 
0208     if (stub->stNum() != step)
0209       continue;
0210 
0211     uint distance =
0212         fabs(wrapAround(((correctedPhi(seed, seed->scNum()) - correctedPhi(stub, seed->scNum())) >> 3), 1024));
0213 
0214     if (stub->scNum() == previousSector) {
0215       if (stub->whNum() == wheel) {
0216         if (!stub->tag()) {
0217           diffInfo[0] = distance;
0218           stubInfo[0] = N - 1;
0219         } else {
0220           diffInfo[1] = distance;
0221           stubInfo[1] = N - 1;
0222         }
0223       } else if (stub->whNum() == innerWheel) {
0224         if (!stub->tag()) {
0225           diffInfo[2] = distance;
0226           stubInfo[2] = N - 1;
0227         } else {
0228           diffInfo[3] = distance;
0229           stubInfo[3] = N - 1;
0230         }
0231       }
0232     } else if (stub->scNum() == sector) {
0233       if (stub->whNum() == wheel) {
0234         if (!stub->tag()) {
0235           diffInfo[4] = distance;
0236           stubInfo[4] = N - 1;
0237         } else {
0238           diffInfo[5] = distance;
0239           stubInfo[5] = N - 1;
0240         }
0241       } else if (stub->whNum() == innerWheel) {
0242         if (!stub->tag()) {
0243           diffInfo[6] = distance;
0244           stubInfo[6] = N - 1;
0245         } else {
0246           diffInfo[7] = distance;
0247           stubInfo[7] = N - 1;
0248         }
0249       }
0250     } else if (stub->scNum() == nextSector) {
0251       if (stub->whNum() == wheel) {
0252         if (!stub->tag()) {
0253           diffInfo[8] = distance;
0254           stubInfo[8] = N - 1;
0255         } else {
0256           diffInfo[9] = distance;
0257           stubInfo[9] = N - 1;
0258         }
0259       } else if (stub->whNum() == innerWheel) {
0260         if (!stub->tag()) {
0261           diffInfo[10] = distance;
0262           stubInfo[10] = N - 1;
0263         } else {
0264           diffInfo[11] = distance;
0265           stubInfo[11] = N - 1;
0266         }
0267       }
0268     }
0269   }
0270 
0271   uint s1_1 = matchAbs(diffInfo, 0, 1);
0272   uint s1_2 = matchAbs(diffInfo, 2, 3);
0273   uint s1_3 = matchAbs(diffInfo, 4, 5);
0274   uint s1_4 = matchAbs(diffInfo, 6, 7);
0275   uint s1_5 = matchAbs(diffInfo, 8, 9);
0276   uint s1_6 = matchAbs(diffInfo, 10, 11);
0277 
0278   uint s2_1 = matchAbs(diffInfo, s1_1, s1_2);
0279   uint s2_2 = matchAbs(diffInfo, s1_3, s1_4);
0280   uint s2_3 = matchAbs(diffInfo, s1_5, s1_6);
0281 
0282   uint s3_1 = matchAbs(diffInfo, s2_1, s2_2);
0283 
0284   uint s4 = matchAbs(diffInfo, s3_1, s2_3);
0285 
0286   if (diffInfo[s4] != 60000)
0287     return std::make_pair(true, stubInfo[s4]);
0288   else
0289     return std::make_pair(false, 0);
0290 }
0291 
0292 int L1TMuonBarrelKalmanAlgo::correctedPhiB(const L1MuKBMTCombinedStubRef& stub) {
0293   //Promote phiB to 12 bits
0294   return 8 * stub->phiB();
0295 }
0296 
0297 int L1TMuonBarrelKalmanAlgo::correctedPhi(const L1MuKBMTCombinedStubRef& stub, int sector) {
0298   if (stub->scNum() == sector) {
0299     return stub->phi();
0300   } else if ((stub->scNum() == sector - 1) || (stub->scNum() == 11 && sector == 0)) {
0301     return stub->phi() - 2144;
0302   } else if ((stub->scNum() == sector + 1) || (stub->scNum() == 0 && sector == 11)) {
0303     return stub->phi() + 2144;
0304   }
0305   return stub->phi();
0306 }
0307 
0308 int L1TMuonBarrelKalmanAlgo::hitPattern(const L1MuKBMTrack& track) {
0309   unsigned int mask = 0;
0310   for (const auto& stub : track.stubs()) {
0311     mask = mask + round(pow(2, stub->stNum() - 1));
0312   }
0313   return mask;
0314 }
0315 
0316 int L1TMuonBarrelKalmanAlgo::customBitmask(unsigned int bit1, unsigned int bit2, unsigned int bit3, unsigned int bit4) {
0317   return bit1 * 1 + bit2 * 2 + bit3 * 4 + bit4 * 8;
0318 }
0319 
0320 bool L1TMuonBarrelKalmanAlgo::getBit(int bitmask, int pos) { return (bitmask & (1 << pos)) >> pos; }
0321 
0322 void L1TMuonBarrelKalmanAlgo::propagate(L1MuKBMTrack& track) {
0323   int K = track.curvature();
0324   int phi = track.positionAngle();
0325   int phiB = track.bendingAngle();
0326   unsigned int step = track.step();
0327 
0328   //energy loss term only for MU->VERTEX
0329   //int offset=int(charge*eLoss_[step-1]*K*K);
0330   //  if (fabs(offset)>4096)
0331   //      offset=4096*offset/fabs(offset);
0332   int charge = 1;
0333   if (K != 0)
0334     charge = K / fabs(K);
0335 
0336   int KBound = K;
0337 
0338   if (KBound > 4095)
0339     KBound = 4095;
0340   if (KBound < -4095)
0341     KBound = -4095;
0342 
0343   int deltaK = 0;
0344   int KNew = 0;
0345   if (step == 1) {
0346     int addr = KBound / 2;
0347     if (addr < 0)
0348       addr = (-KBound) / 2;
0349     deltaK = 2 * addr - int(2 * addr / (1 + eLoss_[step - 1] * addr));
0350 
0351     if (verbose_)
0352       printf("propagate to vertex K=%d deltaK=%d addr=%d\n", K, deltaK, addr);
0353   }
0354 
0355   if (K >= 0)
0356     KNew = K - deltaK;
0357   else
0358     KNew = K + deltaK;
0359 
0360   //phi propagation
0361   ap_fixed<BITSCURV, BITSCURV> phi11 = ap_fixed<BITSPARAM + 1, 2>(aPhi_[step - 1]) * ap_fixed<BITSCURV, BITSCURV>(K);
0362   ap_fixed<BITSPHIB, BITSPHIB> phi12 =
0363       ap_fixed<BITSPARAM + 1, 2>(-bPhi_[step - 1]) * ap_fixed<BITSPHIB, BITSPHIB>(phiB);
0364 
0365   if (verbose_) {
0366     printf("phi prop = %d * %f = %d, %d * %f = %d\n",
0367            K,
0368            ap_fixed<BITSPARAM + 1, 2>(aPhi_[step - 1]).to_float(),
0369            phi11.to_int(),
0370            phiB,
0371            ap_fixed<BITSPARAM + 1, 2>(-bPhi_[step - 1]).to_float(),
0372            phi12.to_int());
0373   }
0374   int phiNew = ap_fixed<BITSPHI, BITSPHI>(phi + phi11 + phi12);
0375 
0376   //phiB propagation
0377   ap_fixed<BITSCURV, BITSCURV> phiB11 = ap_fixed<BITSPARAM, 1>(aPhiB_[step - 1]) * ap_fixed<BITSCURV, BITSCURV>(K);
0378   ap_fixed<BITSPHIB + 1, BITSPHIB + 1> phiB12 =
0379       ap_ufixed<BITSPARAM + 1, 1>(bPhiB_[step - 1]) * ap_fixed<BITSPHIB, BITSPHIB>(phiB);
0380   int phiBNew = ap_fixed<13, 13>(phiB11 + phiB12);
0381   if (verbose_) {
0382     printf("phiB prop = %d * %f = %d, %d * %f = %d\n",
0383            K,
0384            ap_fixed<BITSPARAM + 1, 2>(aPhiB_[step - 1]).to_float(),
0385            phiB11.to_int(),
0386            phiB,
0387            ap_ufixed<BITSPARAM + 1, 1>(bPhiB_[step - 1]).to_float(),
0388            phiB12.to_int());
0389   }
0390 
0391   //Only for the propagation to vertex we use the LUT for better precision and the full function
0392   if (step == 1) {
0393     int addr = KBound / 2;
0394     // Extra steps to mimic firmware for vertex prop
0395     ap_ufixed<11, 11> dxyOffset = (int)fabs(aPhiB_[step - 1] * addr / (1 + charge * aPhiBNLO_[step - 1] * addr));
0396     ap_fixed<12, 12> DXY;
0397     if (addr > 0)
0398       DXY = -dxyOffset;
0399     else
0400       DXY = dxyOffset;
0401     phiBNew = ap_fixed<BITSPHIB, BITSPHIB>(DXY - ap_fixed<BITSPHIB, BITSPHIB>(phiB));
0402     if (verbose_) {
0403       printf("Vertex phiB prop = %d - %d = %d\n", DXY.to_int(), ap_fixed<BITSPHIB, BITSPHIB>(phiB).to_int(), phiBNew);
0404     }
0405   }
0406   ///////////////////////////////////////////////////////
0407   //Rest of the stuff  is for the offline version only
0408   //where we want to check what is happening in the covariaznce matrix
0409 
0410   //Create the transformation matrix
0411   double a[9];
0412   a[0] = 1.;
0413   a[1] = 0.0;
0414   a[2] = 0.0;
0415   a[3] = aPhi_[step - 1];
0416   //  a[3] = 0.0;
0417   a[4] = 1.0;
0418   a[5] = -bPhi_[step - 1];
0419   //a[6]=0.0;
0420   a[6] = aPhiB_[step - 1];
0421   if (step == 1)
0422     a[6] = aPhiB_[step - 1] / 2.0;
0423 
0424   a[7] = 0.0;
0425   a[8] = bPhiB_[step - 1];
0426 
0427   ROOT::Math::SMatrix<double, 3> P(a, 9);
0428 
0429   const std::vector<double>& covLine = track.covariance();
0430   L1MuKBMTrack::CovarianceMatrix cov(covLine.begin(), covLine.end());
0431   cov = ROOT::Math::Similarity(P, cov);
0432 
0433   //Add the multiple scattering
0434   double phiRMS = mScatteringPhi_[step - 1] * K * K;
0435   double phiBRMS = mScatteringPhiB_[step - 1] * K * K;
0436 
0437   std::vector<double> b(6);
0438   b[0] = 0;
0439   b[1] = 0;
0440   b[2] = phiRMS;
0441   b[3] = 0;
0442   b[4] = 0;
0443   b[5] = phiBRMS;
0444 
0445   reco::Candidate::CovarianceMatrix MS(b.begin(), b.end());
0446 
0447   cov = cov + MS;
0448 
0449   if (verbose_) {
0450     printf("Covariance term for phiB = %f\n", cov(2, 2));
0451     printf("Multiple scattering term for phiB = %f\n", MS(2, 2));
0452   }
0453 
0454   track.setCovariance(cov);
0455   track.setCoordinates(step - 1, KNew, phiNew, phiBNew);
0456 }
0457 
0458 bool L1TMuonBarrelKalmanAlgo::update(L1MuKBMTrack& track, const L1MuKBMTCombinedStubRef& stub, int mask, int seedQual) {
0459   updateEta(track, stub);
0460   if (useOfflineAlgo_) {
0461     if (mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12)
0462       return updateOffline(track, stub);
0463     else
0464       return updateOffline1D(track, stub);
0465 
0466   } else
0467     return updateLUT(track, stub, mask, seedQual);
0468 }
0469 
0470 bool L1TMuonBarrelKalmanAlgo::updateOffline(L1MuKBMTrack& track, const L1MuKBMTCombinedStubRef& stub) {
0471   int trackK = track.curvature();
0472   int trackPhi = track.positionAngle();
0473   int trackPhiB = track.bendingAngle();
0474 
0475   int phi = correctedPhi(stub, track.sector());
0476   int phiB = correctedPhiB(stub);
0477 
0478   Vector2 residual;
0479   residual[0] = phi - trackPhi;
0480   residual[1] = phiB - trackPhiB;
0481 
0482   Matrix23 H;
0483   H(0, 0) = 0.0;
0484   H(0, 1) = 1.0;
0485   H(0, 2) = 0.0;
0486   H(1, 0) = 0.0;
0487   H(1, 1) = 0.0;
0488   H(1, 2) = 1.0;
0489 
0490   CovarianceMatrix2 R;
0491   R(0, 0) = pointResolutionPhi_;
0492   R(0, 1) = 0.0;
0493   R(1, 0) = 0.0;
0494   if (stub->quality() < 4)
0495     R(1, 1) = pointResolutionPhiBL_[track.step() - 1];
0496   else
0497     R(1, 1) = pointResolutionPhiBH_[track.step() - 1];
0498 
0499   const std::vector<double>& covLine = track.covariance();
0500   L1MuKBMTrack::CovarianceMatrix cov(covLine.begin(), covLine.end());
0501 
0502   CovarianceMatrix2 S = ROOT::Math::Similarity(H, cov) + R;
0503   if (!S.Invert())
0504     return false;
0505   Matrix32 Gain = cov * ROOT::Math::Transpose(H) * S;
0506 
0507   track.setKalmanGain(
0508       track.step(), fabs(trackK), Gain(0, 0), Gain(0, 1), Gain(1, 0), Gain(1, 1), Gain(2, 0), Gain(2, 1));
0509 
0510   int KNew = (trackK + int(Gain(0, 0) * residual(0) + Gain(0, 1) * residual(1)));
0511   if (fabs(KNew) > 8192)
0512     return false;
0513 
0514   int phiNew = wrapAround(trackPhi + residual(0), 8192);
0515   int phiBNew = wrapAround(trackPhiB + int(Gain(2, 0) * residual(0) + Gain(2, 1) * residual(1)), 4096);
0516 
0517   track.setResidual(stub->stNum() - 1, fabs(phi - phiNew) + fabs(phiB - phiBNew) / 8);
0518 
0519   if (verbose_) {
0520     printf("residual %d - %d = %d %d - %d = %d\n", phi, trackPhi, int(residual[0]), phiB, trackPhiB, int(residual[1]));
0521     printf("Gains offline: %f %f %f %f\n", Gain(0, 0), Gain(0, 1), Gain(2, 0), Gain(2, 1));
0522     printf(" K = %d + %f * %f + %f * %f\n", trackK, Gain(0, 0), residual(0), Gain(0, 1), residual(1));
0523     printf(" phiB = %d + %f * %f + %f * %f\n", trackPhiB, Gain(2, 0), residual(0), Gain(2, 1), residual(1));
0524   }
0525 
0526   track.setCoordinates(track.step(), KNew, phiNew, phiBNew);
0527   Matrix33 covNew = cov - Gain * (H * cov);
0528   L1MuKBMTrack::CovarianceMatrix c;
0529 
0530   c(0, 0) = covNew(0, 0);
0531   c(0, 1) = covNew(0, 1);
0532   c(0, 2) = covNew(0, 2);
0533   c(1, 0) = covNew(1, 0);
0534   c(1, 1) = covNew(1, 1);
0535   c(1, 2) = covNew(1, 2);
0536   c(2, 0) = covNew(2, 0);
0537   c(2, 1) = covNew(2, 1);
0538   c(2, 2) = covNew(2, 2);
0539   if (verbose_) {
0540     printf("Post Fit Covariance Matrix %f %f %f\n", cov(0, 0), cov(1, 1), cov(2, 2));
0541   }
0542 
0543   track.setCovariance(c);
0544   track.addStub(stub);
0545   track.setHitPattern(hitPattern(track));
0546 
0547   return true;
0548 }
0549 
0550 bool L1TMuonBarrelKalmanAlgo::updateOffline1D(L1MuKBMTrack& track, const L1MuKBMTCombinedStubRef& stub) {
0551   int trackK = track.curvature();
0552   int trackPhi = track.positionAngle();
0553   int trackPhiB = track.bendingAngle();
0554 
0555   int phi = correctedPhi(stub, track.sector());
0556 
0557   double residual = phi - trackPhi;
0558 
0559   if (verbose_)
0560     printf("residuals %d - %d = %d\n", phi, trackPhi, int(residual));
0561 
0562   Matrix13 H;
0563   H(0, 0) = 0.0;
0564   H(0, 1) = 1.0;
0565   H(0, 2) = 0.0;
0566 
0567   const std::vector<double>& covLine = track.covariance();
0568   L1MuKBMTrack::CovarianceMatrix cov(covLine.begin(), covLine.end());
0569 
0570   double S = ROOT::Math::Similarity(H, cov)(0, 0) + pointResolutionPhi_;
0571 
0572   if (S == 0.0)
0573     return false;
0574   Matrix31 Gain = cov * ROOT::Math::Transpose(H) / S;
0575 
0576   track.setKalmanGain(track.step(), fabs(trackK), Gain(0, 0), 0.0, Gain(1, 0), 0.0, Gain(2, 0), 0.0);
0577   if (verbose_)
0578     printf("Gains: %f %f\n", Gain(0, 0), Gain(2, 0));
0579 
0580   int KNew = wrapAround(trackK + int(Gain(0, 0) * residual), 8192);
0581   int phiNew = wrapAround(trackPhi + residual, 8192);
0582   int phiBNew = wrapAround(trackPhiB + int(Gain(2, 0) * residual), 4096);
0583   track.setCoordinates(track.step(), KNew, phiNew, phiBNew);
0584   Matrix33 covNew = cov - Gain * (H * cov);
0585   L1MuKBMTrack::CovarianceMatrix c;
0586 
0587   if (verbose_) {
0588     printf("phiUpdate: %d %d\n", int(Gain(0, 0) * residual), int(Gain(2, 0) * residual));
0589   }
0590 
0591   c(0, 0) = covNew(0, 0);
0592   c(0, 1) = covNew(0, 1);
0593   c(0, 2) = covNew(0, 2);
0594   c(1, 0) = covNew(1, 0);
0595   c(1, 1) = covNew(1, 1);
0596   c(1, 2) = covNew(1, 2);
0597   c(2, 0) = covNew(2, 0);
0598   c(2, 1) = covNew(2, 1);
0599   c(2, 2) = covNew(2, 2);
0600   track.setCovariance(c);
0601   track.addStub(stub);
0602   track.setHitPattern(hitPattern(track));
0603 
0604   return true;
0605 }
0606 
0607 bool L1TMuonBarrelKalmanAlgo::updateLUT(L1MuKBMTrack& track,
0608                                         const L1MuKBMTCombinedStubRef& stub,
0609                                         int mask,
0610                                         int seedQual) {
0611   int trackK = track.curvature();
0612   int trackPhi = track.positionAngle();
0613   int trackPhiB = track.bendingAngle();
0614 
0615   int phi = correctedPhi(stub, track.sector());
0616   int phiB = correctedPhiB(stub);
0617 
0618   Vector2 residual;
0619   ap_fixed<BITSPHI + 1, BITSPHI + 1> residualPhi = phi - trackPhi;
0620   ap_fixed<BITSPHIB + 1, BITSPHIB + 1> residualPhiB = phiB - trackPhiB;
0621 
0622   if (verbose_)
0623     printf("residual %d - %d = %d %d - %d = %d\n",
0624            phi,
0625            trackPhi,
0626            residualPhi.to_int(),
0627            phiB,
0628            trackPhiB,
0629            residualPhiB.to_int());
0630 
0631   uint absK = fabs(trackK);
0632   if (absK > 4095)
0633     absK = 4095;
0634 
0635   std::vector<float> GAIN;
0636   //For the three stub stuff use only gains 0 and 4
0637   if (!(mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12)) {
0638     GAIN = lutService_->trackGain(track.step(), track.hitPattern(), absK / 4);
0639     GAIN[1] = 0.0;
0640     GAIN[3] = 0.0;
0641 
0642   } else {
0643     GAIN = lutService_->trackGain2(track.step(), track.hitPattern(), absK / 8, seedQual, stub->quality());
0644   }
0645   if (verbose_) {
0646     printf("Gains (fp): %f %f %f %f\n", GAIN[0], GAIN[1], GAIN[2], GAIN[3]);
0647     if (!(mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12))
0648       printf("Addr=%d   gain0=%f gain4=-%f\n",
0649              absK / 4,
0650              ap_ufixed<GAIN_0, GAIN_0INT>(GAIN[0]).to_float(),
0651              ap_ufixed<GAIN_4, GAIN_4INT>(GAIN[2]).to_float());
0652     else
0653       printf("Addr=%d   %f -%f %f %f\n",
0654              absK / 4,
0655              ap_fixed<GAIN2_0, GAIN2_0INT>(GAIN[0]).to_float(),
0656              ap_ufixed<GAIN2_1, GAIN2_1INT>(GAIN[1]).to_float(),
0657              ap_ufixed<GAIN2_4, GAIN2_4INT>(GAIN[2]).to_float(),
0658              ap_ufixed<GAIN2_5, GAIN2_5INT>(GAIN[3]).to_float());
0659   }
0660 
0661   track.setKalmanGain(track.step(), fabs(trackK), GAIN[0], GAIN[1], 1, 0, GAIN[2], GAIN[3]);
0662 
0663   int KNew;
0664   if (!(mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12)) {
0665     KNew = ap_fixed<BITSPHI + 9, BITSPHI + 9>(ap_fixed<BITSCURV, BITSCURV>(trackK) +
0666                                               ap_ufixed<GAIN_0, GAIN_0INT>(GAIN[0]) * residualPhi);
0667   } else {
0668     ap_fixed<BITSPHI + 7, BITSPHI + 7> k11 = ap_fixed<GAIN2_0, GAIN2_0INT>(GAIN[0]) * residualPhi;
0669     ap_fixed<BITSPHIB + 4, BITSPHIB + 4> k12 = ap_ufixed<GAIN2_1, GAIN2_1INT>(GAIN[1]) * residualPhiB;
0670     KNew = ap_fixed<BITSPHI + 9, BITSPHI + 9>(ap_fixed<BITSCURV, BITSCURV>(trackK) + k11 - k12);
0671   }
0672   if (fabs(KNew) >= 8191)
0673     return false;
0674   KNew = wrapAround(KNew, 8192);
0675   int phiNew = phi;
0676 
0677   //different products for different firmware logic
0678   ap_fixed<BITSPHI + 5, BITSPHI + 5> pbdouble_0 = ap_ufixed<GAIN2_4, GAIN2_4INT>(GAIN[2]) * residualPhi;
0679   ap_fixed<BITSPHIB + 24, BITSPHIB + 4> pb_1 = ap_ufixed<GAIN2_5, GAIN2_5INT>(GAIN[3]) * residualPhiB;
0680   ap_fixed<BITSPHI + 9, BITSPHI + 5> pb_0 = ap_ufixed<GAIN_4, GAIN_4INT>(GAIN[2]) * residualPhi;
0681 
0682   if (verbose_) {
0683     printf("phiupdate %f %f %f\n", pb_0.to_float(), pb_1.to_float(), pbdouble_0.to_float());
0684   }
0685 
0686   int phiBNew;
0687   if (!(mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12)) {
0688     phiBNew = ap_fixed<BITSPHI + 8, BITSPHI + 8>(ap_fixed<BITSPHIB, BITSPHIB>(trackPhiB) -
0689                                                  ap_ufixed<GAIN_4, GAIN_4INT>(GAIN[2]) * residualPhi);
0690 
0691     if (fabs(phiBNew) >= 4095)
0692       return false;
0693   } else {
0694     phiBNew = ap_fixed<BITSPHI + 7, BITSPHI + 7>(ap_fixed<BITSPHIB, BITSPHIB>(trackPhiB) + pb_1 - pbdouble_0);
0695     if (fabs(phiBNew) >= 4095)
0696       return false;
0697   }
0698   track.setCoordinates(track.step(), KNew, phiNew, phiBNew);
0699   track.addStub(stub);
0700   track.setHitPattern(hitPattern(track));
0701   return true;
0702 }
0703 
0704 void L1TMuonBarrelKalmanAlgo::updateEta(L1MuKBMTrack& track, const L1MuKBMTCombinedStubRef& stub) {}
0705 
0706 void L1TMuonBarrelKalmanAlgo::vertexConstraint(L1MuKBMTrack& track) {
0707   if (useOfflineAlgo_)
0708     vertexConstraintOffline(track);
0709   else
0710     vertexConstraintLUT(track);
0711 }
0712 
0713 void L1TMuonBarrelKalmanAlgo::vertexConstraintOffline(L1MuKBMTrack& track) {
0714   double residual = -track.dxy();
0715   Matrix13 H;
0716   H(0, 0) = 0;
0717   H(0, 1) = 0;
0718   H(0, 2) = 1;
0719 
0720   const std::vector<double>& covLine = track.covariance();
0721   L1MuKBMTrack::CovarianceMatrix cov(covLine.begin(), covLine.end());
0722 
0723   double S = (ROOT::Math::Similarity(H, cov))(0, 0) + pointResolutionVertex_;
0724   S = 1.0 / S;
0725   Matrix31 Gain = cov * (ROOT::Math::Transpose(H)) * S;
0726   track.setKalmanGain(track.step(), fabs(track.curvature()), Gain(0, 0), Gain(1, 0), Gain(2, 0));
0727 
0728   if (verbose_) {
0729     printf("sigma3=%f sigma6=%f\n", cov(0, 3), cov(3, 3));
0730     printf(" K = %d + %f * %f\n", track.curvature(), Gain(0, 0), residual);
0731   }
0732 
0733   int KNew = wrapAround(int(track.curvature() + Gain(0, 0) * residual), 8192);
0734   int phiNew = wrapAround(int(track.positionAngle() + Gain(1, 0) * residual), 8192);
0735   int dxyNew = wrapAround(int(track.dxy() + Gain(2, 0) * residual), 8192);
0736   if (verbose_)
0737     printf("Post fit impact parameter=%d\n", dxyNew);
0738   track.setCoordinatesAtVertex(KNew, phiNew, -residual);
0739   Matrix33 covNew = cov - Gain * (H * cov);
0740   L1MuKBMTrack::CovarianceMatrix c;
0741   c(0, 0) = covNew(0, 0);
0742   c(0, 1) = covNew(0, 1);
0743   c(0, 2) = covNew(0, 2);
0744   c(1, 0) = covNew(1, 0);
0745   c(1, 1) = covNew(1, 1);
0746   c(1, 2) = covNew(1, 2);
0747   c(2, 0) = covNew(2, 0);
0748   c(2, 1) = covNew(2, 1);
0749   c(2, 2) = covNew(2, 2);
0750   track.setCovariance(c);
0751   //  track.covariance = track.covariance - Gain*H*track.covariance;
0752 }
0753 
0754 void L1TMuonBarrelKalmanAlgo::vertexConstraintLUT(L1MuKBMTrack& track) {
0755   double residual = -track.dxy();
0756   uint absK = fabs(track.curvature());
0757   if (absK > 2047)
0758     absK = 2047;
0759 
0760   std::pair<float, float> GAIN = lutService_->vertexGain(track.hitPattern(), absK / 2);
0761   track.setKalmanGain(track.step(), fabs(track.curvature()), GAIN.first, GAIN.second, -1);
0762 
0763   ap_fixed<BITSCURV, BITSCURV> k_0 =
0764       -(ap_ufixed<GAIN_V0, GAIN_V0INT>(fabs(GAIN.first))) * ap_fixed<BITSPHIB, BITSPHIB>(residual);
0765   int KNew = ap_fixed<BITSCURV, BITSCURV>(k_0 + ap_fixed<BITSCURV, BITSCURV>(track.curvature()));
0766 
0767   if (verbose_) {
0768     printf("VERTEX GAIN(%d)= -%f * %d = %d\n",
0769            absK / 2,
0770            ap_ufixed<GAIN_V0, GAIN_V0INT>(fabs(GAIN.first)).to_float(),
0771            ap_fixed<BITSPHIB, BITSPHIB>(residual).to_int(),
0772            k_0.to_int());
0773   }
0774 
0775   int p_0 = fp_product(GAIN.second, int(residual), 7);
0776   int phiNew = wrapAround(track.positionAngle() + p_0, 8192);
0777   track.setCoordinatesAtVertex(KNew, phiNew, -residual);
0778 }
0779 
0780 void L1TMuonBarrelKalmanAlgo::setFloatingPointValues(L1MuKBMTrack& track, bool vertex) {
0781   int K, etaINT;
0782 
0783   if (track.hasFineEta())
0784     etaINT = track.fineEta();
0785   else
0786     etaINT = track.coarseEta();
0787 
0788   double lsb = 1.25 / float(1 << 13);
0789   double lsbEta = 0.010875;
0790 
0791   if (vertex) {
0792     int charge = 1;
0793     if (track.curvatureAtVertex() < 0)
0794       charge = -1;
0795     double pt = double(ptLUT(track.curvatureAtVertex())) / 2.0;
0796 
0797     double phi = track.sector() * M_PI / 6.0 + track.phiAtVertex() * M_PI / (6 * 2048.) - 2 * M_PI;
0798 
0799     double eta = etaINT * lsbEta;
0800     track.setPtEtaPhi(pt, eta, phi);
0801     track.setCharge(charge);
0802   } else {
0803     K = track.curvatureAtMuon();
0804     if (K == 0)
0805       K = 1;
0806 
0807     if (fabs(K) < 46)
0808       K = 46 * K / fabs(K);
0809     double pt = 1.0 / (lsb * fabs(K));
0810     if (pt < 1.6)
0811       pt = 1.6;
0812     track.setPtUnconstrained(pt);
0813   }
0814 }
0815 
0816 std::pair<bool, L1MuKBMTrack> L1TMuonBarrelKalmanAlgo::chain(const L1MuKBMTCombinedStubRef& seed,
0817                                                              const L1MuKBMTCombinedStubRefVector& stubs) {
0818   L1MuKBMTrackCollection pretracks;
0819   std::vector<int> combinatorics;
0820   int seedQual;
0821   switch (seed->stNum()) {
0822     case 1:
0823       combinatorics = combos1_;
0824       break;
0825     case 2:
0826       combinatorics = combos2_;
0827       break;
0828 
0829     case 3:
0830       combinatorics = combos3_;
0831       break;
0832 
0833     case 4:
0834       combinatorics = combos4_;
0835       break;
0836 
0837     default:
0838       printf("Something really bad happend\n");
0839   }
0840 
0841   L1MuKBMTrack nullTrack(seed, correctedPhi(seed, seed->scNum()), correctedPhiB(seed));
0842   seedQual = seed->quality();
0843   for (const auto& mask : combinatorics) {
0844     L1MuKBMTrack track(seed, correctedPhi(seed, seed->scNum()), correctedPhiB(seed));
0845     int phiB = correctedPhiB(seed);
0846     int charge;
0847     if (phiB == 0)
0848       charge = 0;
0849     else
0850       charge = phiB / fabs(phiB);
0851 
0852     int address = phiB;
0853     if (track.step() == 4 && (fabs(seed->phiB()) > 15))
0854       address = charge * 15 * 8;
0855 
0856     if (track.step() == 3 && (fabs(seed->phiB()) > 30))
0857       address = charge * 30 * 8;
0858     if (track.step() == 2 && (fabs(seed->phiB()) > 127))
0859       address = charge * 127 * 8;
0860     int initialK = int(initK_[seed->stNum() - 1] * address / (1 + initK2_[seed->stNum() - 1] * charge * address));
0861     if (initialK > 8191)
0862       initialK = 8191;
0863     if (initialK < -8191)
0864       initialK = -8191;
0865 
0866     track.setCoordinates(seed->stNum(), initialK, correctedPhi(seed, seed->scNum()), phiB);
0867     if (seed->quality() < 4) {
0868       track.setCoordinates(seed->stNum(), 0, correctedPhi(seed, seed->scNum()), 0);
0869     }
0870 
0871     track.setHitPattern(hitPattern(track));
0872     //set covariance
0873     L1MuKBMTrack::CovarianceMatrix covariance;
0874 
0875     float DK = 512 * 512.;
0876     covariance(0, 0) = DK;
0877     covariance(0, 1) = 0;
0878     covariance(0, 2) = 0;
0879     covariance(1, 0) = 0;
0880     covariance(1, 1) = float(pointResolutionPhi_);
0881     covariance(1, 2) = 0;
0882     covariance(2, 0) = 0;
0883     covariance(2, 1) = 0;
0884     if (!(mask == 3 || mask == 5 || mask == 9 || mask == 6 || mask == 10 || mask == 12))
0885       covariance(2, 2) = float(pointResolutionPhiB_);
0886     else {
0887       if (seed->quality() < 4)
0888         covariance(2, 2) = float(pointResolutionPhiBL_[seed->stNum() - 1]);
0889       else
0890         covariance(2, 2) = float(pointResolutionPhiBH_[seed->stNum() - 1]);
0891     }
0892     track.setCovariance(covariance);
0893 
0894     //
0895     if (verbose_) {
0896       printf("New Kalman fit staring at step=%d, phi=%d,phiB=%d with curvature=%d\n",
0897              track.step(),
0898              track.positionAngle(),
0899              track.bendingAngle(),
0900              track.curvature());
0901       printf("BITMASK:");
0902       for (unsigned int i = 0; i < 4; ++i)
0903         printf("%d", getBit(mask, i));
0904       printf("\n");
0905       printf("------------------------------------------------------\n");
0906       printf("------------------------------------------------------\n");
0907       printf("------------------------------------------------------\n");
0908       printf("stubs:\n");
0909       for (const auto& stub : stubs)
0910         printf("station=%d phi=%d phiB=%d qual=%d tag=%d sector=%d wheel=%d fineEta= %d %d\n",
0911                stub->stNum(),
0912                correctedPhi(stub, seed->scNum()),
0913                correctedPhiB(stub),
0914                stub->quality(),
0915                stub->tag(),
0916                stub->scNum(),
0917                stub->whNum(),
0918                stub->eta1(),
0919                stub->eta2());
0920       printf("------------------------------------------------------\n");
0921       printf("------------------------------------------------------\n");
0922     }
0923 
0924     int phiAtStation2 = 0;
0925 
0926     while (track.step() > 0) {
0927       // muon station 1
0928       if (track.step() == 1) {
0929         track.setCoordinatesAtMuon(track.curvature(), track.positionAngle(), track.bendingAngle());
0930         phiAtStation2 = phiAt2(track);
0931         bool passed = estimateChiSquare(track);
0932         if (!passed)
0933           break;
0934         calculateEta(track);
0935         setFloatingPointValues(track, false);
0936         //calculate coarse eta
0937         //////////////////////
0938 
0939         if (verbose_)
0940           printf("Unconstrained PT  in Muon System: pt=%f\n", track.ptUnconstrained());
0941       }
0942 
0943       propagate(track);
0944       if (verbose_)
0945         printf("propagated Coordinates step:%d,phi=%d,phiB=%d,K=%d\n",
0946                track.step(),
0947                track.positionAngle(),
0948                track.bendingAngle(),
0949                track.curvature());
0950 
0951       if (track.step() > 0)
0952         if (getBit(mask, track.step() - 1)) {
0953           std::pair<bool, uint> bestStub = match(seed, stubs, track.step());
0954           if ((!bestStub.first) || (!update(track, stubs[bestStub.second], mask, seedQual)))
0955             break;
0956           if (verbose_) {
0957             printf("updated Coordinates step:%d,phi=%d,phiB=%d,K=%d\n",
0958                    track.step(),
0959                    track.positionAngle(),
0960                    track.bendingAngle(),
0961                    track.curvature());
0962           }
0963         }
0964 
0965       if (track.step() == 0) {
0966         track.setCoordinatesAtVertex(track.curvature(), track.positionAngle(), track.bendingAngle());
0967         if (verbose_)
0968           printf(" Coordinates before vertex constraint step:%d,phi=%d,dxy=%d,K=%d\n",
0969                  track.step(),
0970                  track.phiAtVertex(),
0971                  track.dxy(),
0972                  track.curvatureAtVertex());
0973         if (verbose_)
0974           printf("Chi Square = %d\n", track.approxChi2());
0975 
0976         vertexConstraint(track);
0977         estimateCompatibility(track);
0978         if (verbose_) {
0979           printf(" Coordinates after vertex constraint step:%d,phi=%d,dxy=%d,K=%d  maximum local chi2=%d\n",
0980                  track.step(),
0981                  track.phiAtVertex(),
0982                  track.dxy(),
0983                  track.curvatureAtVertex(),
0984                  track.approxChi2());
0985           printf("------------------------------------------------------\n");
0986           printf("------------------------------------------------------\n");
0987         }
0988         setFloatingPointValues(track, true);
0989         //set the coordinates at muon to include phi at station 2
0990         track.setCoordinatesAtMuon(track.curvatureAtMuon(), phiAtStation2, track.phiBAtMuon());
0991         track.setRank(rank(track));
0992         if (verbose_)
0993           printf("Floating point coordinates at vertex: pt=%f, eta=%f phi=%f\n", track.pt(), track.eta(), track.phi());
0994         pretracks.push_back(track);
0995       }
0996     }
0997   }
0998 
0999   //Now for all the pretracks we need only one
1000   L1MuKBMTrackCollection cleaned = clean(pretracks, seed->stNum());
1001 
1002   if (!cleaned.empty()) {
1003     return std::make_pair(true, cleaned[0]);
1004   }
1005   return std::make_pair(false, nullTrack);
1006 }
1007 
1008 bool L1TMuonBarrelKalmanAlgo::estimateChiSquare(L1MuKBMTrack& track) {
1009   //here we have a simplification of the algorithm for the sake of the emulator - rsult is identical
1010   // we apply cuts on the firmware as |u -u'|^2 < a+b *K^2
1011   int K = track.curvatureAtMuon();
1012 
1013   uint chi = 0;
1014 
1015   // const double PHI[4]={0.0,0.249,0.543,0.786};
1016   // const double DROR[4]={0.0,0.182,0.430,0.677};
1017 
1018   int coords = wrapAround((track.phiAtMuon() + track.phiBAtMuon()) >> 4, 512);
1019   for (const auto& stub : track.stubs()) {
1020     int AK = wrapAround(fp_product(-chiSquare_[stub->stNum() - 1], K >> 4, 8), 256);
1021     int stubCoords = wrapAround((correctedPhi(stub, track.sector()) >> 4) + (stub->phiB() >> 1), 512);
1022     int diff1 = wrapAround(stubCoords - coords, 1024);
1023     uint delta = wrapAround(abs(diff1 + AK), 2048);
1024     chi = chi + delta;
1025     if (verbose_)
1026       printf("Chi Square stub for track with pattern=%d coords=%d -> AK=%d stubCoords=%d diff=%d delta=%d\n",
1027              track.hitPattern(),
1028              coords,
1029              AK,
1030              stubCoords,
1031              diff1,
1032              delta);
1033   }
1034 
1035   if (chi > 127)
1036     chi = 127;
1037   // for (const auto& stub: track.stubs()) {
1038   //   int deltaPhi = (correctedPhi(stub,track.sector())-track.phiAtMuon())>>3;
1039   //   int AK =  fp_product(PHI[stub->stNum()-1],K>>3,8);
1040   //   int BPB = fp_product(DROR[stub->stNum()-1],track.phiBAtMuon()>>3,8);
1041   //   chi=chi+abs(deltaPhi-AK-BPB);
1042   // }
1043   // //  }
1044 
1045   track.setApproxChi2(chi);
1046   for (uint i = 0; i < chiSquareCutPattern_.size(); ++i) {
1047     if (track.hitPattern() == chiSquareCutPattern_[i] && fabs(K) < chiSquareCutCurv_[i] &&
1048         track.approxChi2() > chiSquareCut_[i])
1049       return false;
1050   }
1051   return true;
1052 }
1053 
1054 void L1TMuonBarrelKalmanAlgo::estimateCompatibility(L1MuKBMTrack& track) {
1055   int K = track.curvatureAtVertex() >> 4;
1056 
1057   if (track.stubs().size() != 2) {
1058     track.setTrackCompatibility(0);
1059     return;
1060   }
1061 
1062   uint stubSel = 1;
1063   if (track.stubs()[0]->quality() > track.stubs()[1]->quality())
1064     stubSel = 0;
1065   const L1MuKBMTCombinedStubRef& stub = track.stubs()[stubSel];
1066 
1067   if (verbose_) {
1068     printf("stubsel %d phi=%d phiB=%d\n", stubSel, stub->phi(), stub->phiB());
1069   }
1070 
1071   ap_ufixed<BITSCURV - 5, BITSCURV - 5> absK;
1072   if (K < 0)
1073     absK = -K;
1074   else
1075     absK = K;
1076 
1077   ap_fixed<12, 12> diff = ap_int<10>(stub->phiB()) -
1078                           ap_ufixed<5, 1>(trackComp_[stub->stNum() - 1]) * ap_fixed<BITSCURV - 4, BITSCURV - 4>(K);
1079   ap_ufixed<11, 11> delta;
1080   if (diff.is_neg())
1081     delta = -diff;
1082   else
1083     delta = diff;
1084 
1085   ap_ufixed<BITSCURV - 5, BITSCURV - 5> err =
1086       ap_uint<3>(trackCompErr1_[stub->stNum() - 1]) + ap_ufixed<5, 0>(trackCompErr2_[stub->stNum() - 1]) * absK;
1087   track.setTrackCompatibility(((int)delta) / ((int)err));
1088   for (uint i = 0; i < trackCompPattern_.size(); ++i) {
1089     int deltaMax = ap_ufixed<BITSCURV, BITSCURV>(err * trackCompCut_[i]);
1090     if (verbose_) {
1091       if (track.hitPattern() == trackCompPattern_[i]) {
1092         printf("delta = %d = abs(%d - %f*%d\n", delta.to_int(), stub->phiB(), trackComp_[stub->stNum() - 1], K);
1093         printf("err = %d = %f + %f*%d\n",
1094                err.to_int(),
1095                trackCompErr1_[stub->stNum() - 1],
1096                trackCompErr2_[stub->stNum() - 1],
1097                absK.to_int());
1098         printf("deltaMax = %d = %d*%d\n", deltaMax, err.to_int(), trackCompCut_[i]);
1099       }
1100     }
1101     if ((track.hitPattern() == trackCompPattern_[i]) && ((int)absK < trackCompCutCurv_[i]) &&
1102         ((track.approxChi2() > chiSquareCutTight_[i]) || (delta > deltaMax))) {
1103       track.setCoordinatesAtVertex(8191, track.phiAtVertex(), track.dxy());
1104       break;
1105     }
1106   }
1107 }
1108 
1109 int L1TMuonBarrelKalmanAlgo::rank(const L1MuKBMTrack& track) {
1110   //    int offset=0;
1111   uint chi = track.approxChi2() > 127 ? 127 : track.approxChi2();
1112   if (hitPattern(track) == customBitmask(0, 0, 1, 1)) {
1113     return 60;
1114   }
1115   //    return offset+(track.stubs().size()*2+track.quality())*80-track.approxChi2();
1116   return 160 + (track.stubs().size()) * 20 - chi;
1117 }
1118 
1119 int L1TMuonBarrelKalmanAlgo::wrapAround(int value, int maximum) {
1120   if (value > maximum - 1)
1121     return wrapAround(value - 2 * maximum, maximum);
1122   if (value < -maximum)
1123     return wrapAround(value + 2 * maximum, maximum);
1124   return value;
1125 }
1126 
1127 int L1TMuonBarrelKalmanAlgo::encode(bool ownwheel, int sector, bool tag) {
1128   if (ownwheel) {
1129     if (sector == 0) {
1130       if (tag)
1131         return 9;
1132       else
1133         return 8;
1134     } else if (sector == 1) {
1135       if (tag)
1136         return 11;
1137       else
1138         return 10;
1139 
1140     } else {
1141       if (tag)
1142         return 13;
1143       else
1144         return 12;
1145     }
1146 
1147   } else {
1148     if (sector == 0) {
1149       if (tag)
1150         return 1;
1151       else
1152         return 0;
1153     } else if (sector == 1) {
1154       if (tag)
1155         return 3;
1156       else
1157         return 2;
1158 
1159     } else {
1160       if (tag)
1161         return 5;
1162       else
1163         return 4;
1164     }
1165   }
1166   return 15;
1167 }
1168 
1169 std::map<int, int> L1TMuonBarrelKalmanAlgo::trackAddress(const L1MuKBMTrack& track, int& word) {
1170   std::map<int, int> out;
1171 
1172   out[l1t::RegionalMuonCand::kWheelSide] = track.wheel() < 0;
1173   if (track.wheel() == -2)
1174     out[l1t::RegionalMuonCand::kWheelNum] = 2;
1175   else if (track.wheel() == -1)
1176     out[l1t::RegionalMuonCand::kWheelNum] = 1;
1177   else if (track.wheel() == 0)
1178     out[l1t::RegionalMuonCand::kWheelNum] = 0;
1179   else if (track.wheel() == 1)
1180     out[l1t::RegionalMuonCand::kWheelNum] = 1;
1181   else if (track.wheel() == 2)
1182     out[l1t::RegionalMuonCand::kWheelNum] = 2;
1183   else
1184     out[l1t::RegionalMuonCand::kWheelNum] = 0;
1185   out[l1t::RegionalMuonCand::kStat1] = 15;
1186   out[l1t::RegionalMuonCand::kStat2] = 15;
1187   out[l1t::RegionalMuonCand::kStat3] = 15;
1188   out[l1t::RegionalMuonCand::kStat4] = 3;
1189   out[l1t::RegionalMuonCand::kSegSelStat1] = 0;
1190   out[l1t::RegionalMuonCand::kSegSelStat2] = 0;
1191   out[l1t::RegionalMuonCand::kSegSelStat3] = 0;
1192   out[l1t::RegionalMuonCand::kSegSelStat4] = 0;
1193   //out[l1t::RegionalMuonCand::kNumBmtfSubAddr]=0; // This is commented out for better data/MC agreement
1194 
1195   for (const auto& stub : track.stubs()) {
1196     bool ownwheel = stub->whNum() == track.wheel();
1197     int sector = 0;
1198     if ((stub->scNum() == track.sector() + 1) || (stub->scNum() == 0 && track.sector() == 11))
1199       sector = +1;
1200     if ((stub->scNum() == track.sector() - 1) || (stub->scNum() == 11 && track.sector() == 0))
1201       sector = -1;
1202     int addr = encode(ownwheel, sector, stub->tag());
1203 
1204     if (stub->stNum() == 4) {
1205       if (stub->tag())
1206         addr = 1;
1207       else
1208         addr = 2;
1209       out[l1t::RegionalMuonCand::kStat4] = addr;
1210     }
1211     if (stub->stNum() == 3) {
1212       out[l1t::RegionalMuonCand::kStat3] = addr;
1213     }
1214     if (stub->stNum() == 2) {
1215       out[l1t::RegionalMuonCand::kStat2] = addr;
1216     }
1217     if (stub->stNum() == 1) {
1218       out[l1t::RegionalMuonCand::kStat1] = addr;
1219     }
1220   }
1221 
1222   word = 0;
1223   word = word | out[l1t::RegionalMuonCand::kStat4] << 12;
1224   word = word | out[l1t::RegionalMuonCand::kStat3] << 8;
1225   word = word | out[l1t::RegionalMuonCand::kStat2] << 4;
1226   word = word | out[l1t::RegionalMuonCand::kStat1];
1227 
1228   return out;
1229 }
1230 
1231 uint L1TMuonBarrelKalmanAlgo::twosCompToBits(int q) {
1232   if (q >= 0)
1233     return q;
1234   else
1235     return (~q) + 1;
1236 }
1237 
1238 int L1TMuonBarrelKalmanAlgo::fp_product(float a, int b, uint bits) {
1239   //  return long(a*(1<<bits)*b)>>bits;
1240   return (long((a * (1 << bits)) * b)) >> bits;
1241 }
1242 
1243 int L1TMuonBarrelKalmanAlgo::ptLUT(int K) {
1244   int charge = (K >= 0) ? +1 : -1;
1245   float lsb = 1.25 / float(1 << 13);
1246   float FK = fabs(K);
1247 
1248   if (FK > 2047)
1249     FK = 2047.;
1250   if (FK < 26)
1251     FK = 26.;
1252 
1253   FK = FK * lsb;
1254 
1255   //step 1 -material and B-field
1256   FK = .8569 * FK / (1.0 + 0.1144 * FK);
1257   //step 2 - misalignment
1258   FK = FK - charge * 1.23e-03;
1259   //Get to BMTF scale
1260   FK = FK / 1.17;
1261 
1262   int pt = 0;
1263   if (FK != 0)
1264     pt = int(2.0 / FK);
1265 
1266   if (pt > 511)
1267     pt = 511;
1268 
1269   if (pt < 8)
1270     pt = 8;
1271 
1272   return pt;
1273 }
1274 
1275 L1MuKBMTrackCollection L1TMuonBarrelKalmanAlgo::clean(const L1MuKBMTrackCollection& tracks, uint seed) {
1276   L1MuKBMTrackCollection out;
1277 
1278   std::map<uint, int> infoRank;
1279   std::map<uint, L1MuKBMTrack> infoTrack;
1280   for (uint i = 3; i <= 15; ++i) {
1281     if (i == 4 || i == 8)
1282       continue;
1283     infoRank[i] = -1;
1284   }
1285 
1286   for (const auto& track : tracks) {
1287     infoRank[track.hitPattern()] = rank(track);
1288     infoTrack[track.hitPattern()] = track;
1289   }
1290 
1291   int selected = 15;
1292   if (seed == 4)  //station 4 seeded
1293   {
1294     int sel6 = infoRank[10] >= infoRank[12] ? 10 : 12;
1295     int sel5 = infoRank[14] >= infoRank[9] ? 14 : 9;
1296     int sel4 = infoRank[11] >= infoRank[13] ? 11 : 13;
1297     int sel3 = infoRank[sel6] >= infoRank[sel5] ? sel6 : sel5;
1298     int sel2 = infoRank[sel4] >= infoRank[sel3] ? sel4 : sel3;
1299     selected = infoRank[15] >= infoRank[sel2] ? 15 : sel2;
1300   }
1301   if (seed == 3)  //station 3 seeded
1302   {
1303     int sel2 = infoRank[5] >= infoRank[6] ? 5 : 6;
1304     selected = infoRank[7] >= infoRank[sel2] ? 7 : sel2;
1305   }
1306   if (seed == 2)  //station 2 seeded
1307     selected = 3;
1308 
1309   auto search = infoTrack.find(selected);
1310   if (search != infoTrack.end())
1311     out.push_back(search->second);
1312 
1313   return out;
1314 }
1315 
1316 uint L1TMuonBarrelKalmanAlgo::etaStubRank(const L1MuKBMTCombinedStubRef& stub) {
1317   if (stub->qeta1() != 0 && stub->qeta2() != 0) {
1318     return 0;
1319   }
1320   if (stub->qeta1() == 0) {
1321     return 0;
1322   }
1323   //  return (stub->qeta1()*4+stub->stNum());
1324   return (stub->qeta1());
1325 }
1326 
1327 void L1TMuonBarrelKalmanAlgo::calculateEta(L1MuKBMTrack& track) {
1328   uint pattern = track.hitPattern();
1329   int wheel = track.stubs()[0]->whNum();
1330   uint awheel = fabs(wheel);
1331   int sign = 1;
1332   if (wheel < 0)
1333     sign = -1;
1334   uint nstubs = track.stubs().size();
1335   uint mask = 0;
1336   for (unsigned int i = 0; i < track.stubs().size(); ++i) {
1337     if (fabs(track.stubs()[i]->whNum()) != awheel)
1338       mask = mask | (1 << i);
1339   }
1340   mask = (awheel << nstubs) | mask;
1341   track.setCoarseEta(sign * lutService_->coarseEta(pattern, mask));
1342   int sumweights = 0;
1343   int sums = 0;
1344 
1345   for (const auto& stub : track.stubs()) {
1346     uint rank = etaStubRank(stub);
1347     if (rank == 0)
1348       continue;
1349     //    printf("Stub station=%d rank=%d values=%d %d\n",stub->stNum(),rank,stub->eta1(),stub->eta2());
1350     sumweights += rank;
1351     sums += rank * stub->eta1();
1352   }
1353   //0.5  0.332031 0.25 0.199219 0.164063
1354   float factor;
1355   if (sumweights == 1)
1356     factor = 1.0;
1357   else if (sumweights == 2)
1358     factor = 0.5;
1359   else if (sumweights == 3)
1360     factor = 0.332031;
1361   else if (sumweights == 4)
1362     factor = 0.25;
1363   else if (sumweights == 5)
1364     factor = 0.199219;
1365   else if (sumweights == 6)
1366     factor = 0.164063;
1367   else
1368     factor = 0.0;
1369 
1370   int eta = 0;
1371   if (sums > 0)
1372     eta = fp_product(factor, sums, 10);
1373   else
1374     eta = -fp_product(factor, fabs(sums), 10);
1375 
1376   //int eta=int(factor*sums);
1377   //    printf("Eta debug %f *%d=%d\n",factor,sums,eta);
1378 
1379   if (sumweights > 0)
1380     track.setFineEta(eta);
1381 }
1382 
1383 int L1TMuonBarrelKalmanAlgo::phiAt2(const L1MuKBMTrack& track) {
1384   //If there is stub at station 2 use this else propagate from 1
1385   for (const auto& stub : track.stubs())
1386     if (stub->stNum() == 2)
1387       return correctedPhi(stub, track.sector());
1388 
1389   ap_fixed<BITSPHI, BITSPHI> phi = track.phiAtMuon();
1390   ap_fixed<BITSPHIB, BITSPHIB> phiB = track.phiBAtMuon();
1391   ap_fixed<BITSPARAM, 1> phiAt2 = phiAt2_;
1392   int phiNew = ap_fixed<BITSPHI + 1, BITSPHI + 1, AP_TRN_ZERO, AP_SAT>(phi + phiAt2 * phiB);
1393 
1394   if (verbose_)
1395     printf("Phi at second station=%d\n", phiNew);
1396   return phiNew;
1397 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.2.1 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!