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
0057
0058
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
0073
0074
0075
0076
0077
0078 int pt = ptLUT(track.curvatureAtVertex());
0079
0080
0081
0082
0083
0084
0085
0086 int pt2 = ptLUT(track.curvatureAtMuon()) / 2;
0087 int eta = track.hasFineEta() ? track.fineEta() : track.coarseEta();
0088
0089
0090
0091
0092
0093
0094
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
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
0140
0141
0142
0143
0144
0145
0146
0147
0148
0149
0150
0151
0152
0153
0154
0155
0156
0157
0158
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
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
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
0329
0330
0331
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
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
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
0392 if (step == 1) {
0393 int addr = KBound / 2;
0394
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
0408
0409
0410
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
0417 a[4] = 1.0;
0418 a[5] = -bPhi_[step - 1];
0419
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
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
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
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
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
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
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
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
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
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
1010
1011 int K = track.curvatureAtMuon();
1012
1013 uint chi = 0;
1014
1015
1016
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
1038
1039
1040
1041
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
1111 uint chi = track.approxChi2() > 127 ? 127 : track.approxChi2();
1112 if (hitPattern(track) == customBitmask(0, 0, 1, 1)) {
1113 return 60;
1114 }
1115
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
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
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
1256 FK = .8569 * FK / (1.0 + 0.1144 * FK);
1257
1258 FK = FK - charge * 1.23e-03;
1259
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)
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)
1302 {
1303 int sel2 = infoRank[5] >= infoRank[6] ? 5 : 6;
1304 selected = infoRank[7] >= infoRank[sel2] ? 7 : sel2;
1305 }
1306 if (seed == 2)
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
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
1350 sumweights += rank;
1351 sums += rank * stub->eta1();
1352 }
1353
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
1377
1378
1379 if (sumweights > 0)
1380 track.setFineEta(eta);
1381 }
1382
1383 int L1TMuonBarrelKalmanAlgo::phiAt2(const L1MuKBMTrack& track) {
1384
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 }