Back to home page

Project CMSSW displayed by LXR
 
 

    

File indexing completed on 2025-06-03 00:12:23 

0001 #include "L1Trigger/TrackFindingTracklet/interface/KalmanFilter.h"
0002 #include "L1Trigger/TrackFindingTMTT/interface/L1track3D.h"
0003 #include "L1Trigger/TrackFindingTMTT/interface/Stub.h"
0004 #include "L1Trigger/TrackFindingTMTT/interface/L1fittedTrack.h"
0005 
0006 #include <numeric>
0007 #include <algorithm>
0008 #include <iterator>
0009 #include <deque>
0010 #include <vector>
0011 #include <set>
0012 #include <cmath>
0013 
0014 namespace trklet {
0015 
0016   KalmanFilter::KalmanFilter(const tt::Setup* setup,
0017                              const DataFormats* dataFormats,
0018                              KalmanFilterFormats* kalmanFilterFormats,
0019                              tmtt::Settings* settings,
0020                              tmtt::KFParamsComb* tmtt,
0021                              int region,
0022                              tt::TTTracks& ttTracks)
0023       : setup_(setup),
0024         dataFormats_(dataFormats),
0025         kalmanFilterFormats_(kalmanFilterFormats),
0026         settings_(settings),
0027         tmtt_(tmtt),
0028         region_(region),
0029         ttTracks_(ttTracks),
0030         layer_(0) {
0031     zTs_.reserve(settings_->etaRegions().size());
0032     for (double eta : settings_->etaRegions())
0033       zTs_.emplace_back(std::sinh(eta) * settings_->chosenRofZ());
0034   }
0035 
0036   // read in and organize input tracks and stubs
0037   void KalmanFilter::consume(const tt::StreamsTrack& streamsTrack, const tt::StreamsStub& streamsStub) {
0038     const int offset = region_ * setup_->numLayers();
0039     const tt::StreamTrack& streamTrack = streamsTrack[region_];
0040     const int numTracks =
0041         std::accumulate(streamTrack.begin(), streamTrack.end(), 0, [](int sum, const tt::FrameTrack& f) {
0042           return sum + (f.first.isNull() ? 0 : 1);
0043         });
0044     int numStubs(0);
0045     for (int layer = 0; layer < setup_->numLayers(); layer++) {
0046       const tt::StreamStub& streamStub = streamsStub[offset + layer];
0047       numStubs += std::accumulate(streamStub.begin(), streamStub.end(), 0, [](int sum, const tt::FrameStub& f) {
0048         return sum + (f.first.isNull() ? 0 : 1);
0049       });
0050     }
0051     tracks_.reserve(numTracks);
0052     stubs_.reserve(numStubs);
0053     int trackId(0);
0054     for (int frame = 0; frame < static_cast<int>(streamTrack.size()); frame++) {
0055       const tt::FrameTrack& frameTrack = streamTrack[frame];
0056       if (frameTrack.first.isNull()) {
0057         stream_.push_back(nullptr);
0058         continue;
0059       }
0060       tracks_.emplace_back(frameTrack, dataFormats_);
0061       TrackDR* track = &tracks_.back();
0062       std::vector<Stub*> stubs(setup_->numLayers(), nullptr);
0063       TTBV hitPattern(0, setup_->numLayers());
0064       for (int layer = 0; layer < setup_->numLayers(); layer++) {
0065         const tt::FrameStub& frameStub = streamsStub[offset + layer][frame];
0066         if (frameStub.first.isNull())
0067           continue;
0068         stubs_.emplace_back(kalmanFilterFormats_, frameStub);
0069         stubs[layer] = &stubs_.back();
0070         hitPattern.set(layer);
0071       }
0072       if (hitPattern.count(0, setup_->kfMaxSeedingLayer()) < setup_->kfNumSeedStubs()) {
0073         stream_.push_back(nullptr);
0074         continue;
0075       }
0076       states_.emplace_back(kalmanFilterFormats_, track, stubs, trackId++);
0077       stream_.push_back(&states_.back());
0078       if (setup_->enableTruncation() && trackId == setup_->kfMaxTracks())
0079         break;
0080     }
0081   }
0082 
0083   // call old KF
0084   void KalmanFilter::simulate(tt::StreamsStub& streamsStub, tt::StreamsTrack& streamsTrack) {
0085     // prep finals_ where old KF tracks will be stored
0086     finals_.reserve(states_.size());
0087     for (const State& state : states_) {
0088       // convert tracks to by old KF expected data formats
0089       TrackDR* trackFound = state.track();
0090       const TTTrackRef& ttTrackRef = trackFound->frame().first;
0091       const double qOverPt = -trackFound->inv2R() / setup_->invPtToDphi();
0092       const double phi0 = tt::deltaPhi(trackFound->phiT() - setup_->chosenRofPhi() * trackFound->inv2R() +
0093                                        region_ * setup_->baseRegion());
0094       const double tanLambda = trackFound->zT() / setup_->chosenRofZ();
0095       static constexpr double z0 = 0;
0096       static constexpr double helixD0 = 0.;
0097       // convert stubs to by old KF expected data formats
0098       std::vector<tmtt::Stub> stubs;
0099       std::vector<tmtt::Stub*> stubsFound;
0100       stubs.reserve(state.trackPattern().count());
0101       stubsFound.reserve(state.trackPattern().count());
0102       const std::vector<Stub*>& stubsState = state.stubs();
0103       for (int layer = 0; layer < setup_->numLayers(); layer++) {
0104         if (!stubsState[layer])
0105           continue;
0106         const StubDR& stub = stubsState[layer]->stubDR_;
0107         const TTStubRef& ttStubRef = stub.frame().first;
0108         tt::SensorModule* sensorModule = setup_->sensorModule(ttStubRef);
0109         // convert position
0110         double r, phi, z;
0111         if (setup_->kfUseTTStubResiduals()) {
0112           const GlobalPoint gp = setup_->stubPos(ttStubRef);
0113           r = gp.perp();
0114           phi = gp.phi();
0115           z = gp.z();
0116         } else {
0117           r = stub.r() + setup_->chosenRofPhi();
0118           phi = tt::deltaPhi(stub.phi() + trackFound->phiT() + stub.r() * trackFound->inv2R() +
0119                              region_ * setup_->baseRegion());
0120           z = stub.z() + trackFound->zT() + (r - setup_->chosenRofZ()) * tanLambda;
0121         }
0122         // convert stub layer id (barrel: 1 - 6, endcap: 11 - 15) to reduced layer id (0 - 6)
0123         int layerId = setup_->layerId(ttStubRef);
0124         if (layerId > 10 && z < 0.)
0125           layerId += 10;
0126         int layerIdReduced = setup_->layerId(ttStubRef);
0127         if (layerIdReduced == 6)
0128           layerIdReduced = 11;
0129         else if (layerIdReduced == 5)
0130           layerIdReduced = 12;
0131         else if (layerIdReduced == 4)
0132           layerIdReduced = 13;
0133         else if (layerIdReduced == 3)
0134           layerIdReduced = 15;
0135         if (layerIdReduced > 10)
0136           layerIdReduced -= 8;
0137         const double stripPitch = sensorModule->pitchRow();
0138         const double stripLength = sensorModule->pitchCol();
0139         const bool psModule = sensorModule->psModule();
0140         const bool barrel = sensorModule->barrel();
0141         const bool tiltedBarrel = sensorModule->tilted();
0142         stubs.emplace_back(
0143             ttStubRef, r, phi, z, layerId, layerIdReduced, stripPitch, stripLength, psModule, barrel, tiltedBarrel);
0144         stubsFound.push_back(&stubs.back());
0145       }
0146       // determine phi and eta region
0147       const int iPhiSec = region_;
0148       const double zTtrack = ttTrackRef->z0() + settings_->chosenRofZ() * ttTrackRef->tanL();
0149       int iEtaReg = 0;
0150       for (; iEtaReg < 15; iEtaReg++)
0151         if (zTtrack < zTs_[iEtaReg + 1])
0152           break;
0153       const tmtt::L1track3D l1track3D(settings_, stubsFound, qOverPt, phi0, z0, tanLambda, helixD0, iPhiSec, iEtaReg);
0154       // perform fit
0155       const tmtt::L1fittedTrack trackFitted(tmtt_->fit(l1track3D));
0156       if (!trackFitted.accepted())
0157         continue;
0158       // convert olf kf fitted track format into emulator format
0159       static constexpr int trackId = 0;
0160       static constexpr int numConsistent = 0;
0161       static constexpr int numConsistentPS = 0;
0162       const double inv2R = -trackFitted.qOverPt() * setup_->invPtToDphi();
0163       const double phiT =
0164           tt::deltaPhi(trackFitted.phi0() + inv2R * setup_->chosenRofPhi() - region_ * setup_->baseRegion());
0165       const double cot = trackFitted.tanLambda();
0166       const double zT = trackFitted.z0() + cot * setup_->chosenRofZ();
0167       // check for bit overflows
0168       if (!dataFormats_->format(Variable::inv2R, Process::kf).inRange(inv2R, true))
0169         continue;
0170       if (!dataFormats_->format(Variable::phiT, Process::kf).inRange(phiT, true))
0171         continue;
0172       if (!dataFormats_->format(Variable::cot, Process::kf).inRange(cot, true))
0173         continue;
0174       if (!dataFormats_->format(Variable::zT, Process::kf).inRange(zT, true))
0175         continue;
0176       const double d0 = trackFitted.d0();
0177       const double x0 = inv2R - trackFound->inv2R();
0178       const double x1 = phiT - trackFound->phiT();
0179       const double x2 = cot - tanLambda;
0180       const double x3 = zT - trackFound->zT();
0181       const double x4 = d0;
0182       // get intput stubs and convert to emulator format
0183       TTBV hitPattern(0, setup_->numLayers());
0184       std::vector<StubKF> stubsKF;
0185       stubsKF.reserve(setup_->numLayers());
0186       for (tmtt::Stub* stub : trackFitted.stubs()) {
0187         if (!stub)
0188           continue;
0189         // get stub
0190         const auto it = std::find_if(stubsState.begin(), stubsState.end(), [stub](Stub* state) {
0191           return state && (stub->ttStubRef() == state->stubDR_.frame().first);
0192         });
0193         const StubDR& s = (*it)->stubDR_;
0194         // convert position relative to track
0195         const double r = s.r();
0196         const double r0 = r + setup_->chosenRofPhi();
0197         const double phi = s.phi() - (x1 + r * x0 + x4 / r0);
0198         const double z = s.z() - (x3 + (r0 - setup_->chosenRofZ()) * x2);
0199         const double dPhi = s.dPhi();
0200         const double dZ = s.dZ();
0201         const int layer = std::distance(stubsState.begin(), it);
0202         // check for bit overflows
0203         if (!dataFormats_->format(Variable::phi, Process::kf).inRange(phi, true))
0204           continue;
0205         if (!dataFormats_->format(Variable::z, Process::kf).inRange(z, true))
0206           continue;
0207         hitPattern.set(layer);
0208         stubsKF.emplace_back(s, r, phi, z, dPhi, dZ);
0209       }
0210       // check if enough stubs left to form a track
0211       if (hitPattern.count() < setup_->kfMinLayers())
0212         continue;
0213       // store track
0214       const TrackKF trackKF(*trackFound, inv2R, phiT, cot, zT);
0215       finals_.emplace_back(trackId, numConsistent, numConsistentPS, d0, hitPattern, trackKF, stubsKF);
0216     }
0217     conv(streamsStub, streamsTrack);
0218   }
0219 
0220   // fill output products
0221   void KalmanFilter::produce(tt::StreamsStub& streamsStub,
0222                              tt::StreamsTrack& streamsTrack,
0223                              int& numAcceptedStates,
0224                              int& numLostStates) {
0225     if (setup_->kfUseSimmulation())
0226       return simulate(streamsStub, streamsTrack);
0227     // 5 parameter fit simulation
0228     if (setup_->kfUse5ParameterFit()) {
0229       // Propagate state to each layer in turn, updating it with all viable stub combinations there, using KF maths
0230       for (layer_ = 0; layer_ < setup_->numLayers(); layer_++)
0231         addLayer();
0232     } else {  // 4 parameter fit emulation
0233       // seed building
0234       for (layer_ = 0; layer_ < setup_->kfMaxSeedingLayer(); layer_++)
0235         addLayer(true);
0236       // calulcate seed parameter
0237       calcSeeds();
0238       // Propagate state to each layer in turn, updating it with all viable stub combinations there, using KF maths
0239       for (layer_ = setup_->kfNumSeedStubs(); layer_ < setup_->numLayers(); layer_++)
0240         addLayer();
0241     }
0242     // count total number of final states
0243     const int nStates =
0244         std::accumulate(stream_.begin(), stream_.end(), 0, [](int sum, State* state) { return sum + (state ? 1 : 0); });
0245     // apply truncation
0246     if (setup_->enableTruncation() && static_cast<int>(stream_.size()) > setup_->numFramesHigh())
0247       stream_.resize(setup_->numFramesHigh());
0248     // cycle event, remove gaps
0249     stream_.erase(std::remove(stream_.begin(), stream_.end(), nullptr), stream_.end());
0250     // store number of states which got taken into account
0251     numAcceptedStates += stream_.size();
0252     // store number of states which got not taken into account due to truncation
0253     numLostStates += nStates - stream_.size();
0254     // apply final cuts
0255     finalize();
0256     // best track per candidate selection
0257     accumulator();
0258     // Transform States into output products
0259     conv(streamsStub, streamsTrack);
0260   }
0261 
0262   // apply final cuts
0263   void KalmanFilter::finalize() {
0264     finals_.reserve(stream_.size());
0265     for (State* state : stream_) {
0266       int numConsistent(0);
0267       int numConsistentPS(0);
0268       TTBV hitPattern = state->hitPattern();
0269       std::vector<StubKF> stubsKF;
0270       stubsKF.reserve(setup_->numLayers());
0271       // stub residual cut
0272       State* s = state;
0273       while ((s = s->parent())) {
0274         const double dPhi = state->x1() + s->H00() * state->x0() + state->x4() / s->H04();
0275         const double dZ = state->x3() + s->H12() * state->x2();
0276         const double phi = digi(VariableKF::m0, s->m0() - dPhi);
0277         const double z = digi(VariableKF::m1, s->m1() - dZ);
0278         const bool validPhi = dataFormats_->format(Variable::phi, Process::kf).inRange(phi);
0279         const bool validZ = dataFormats_->format(Variable::z, Process::kf).inRange(z);
0280         if (validPhi && validZ) {
0281           const double r = s->H00();
0282           const double dPhi = s->d0();
0283           const double dZ = s->d1();
0284           const StubDR& stubDR = s->stub()->stubDR_;
0285           stubsKF.emplace_back(stubDR, r, phi, z, dPhi, dZ);
0286           if (abs(phi) <= dPhi && abs(z) <= dZ) {
0287             numConsistent++;
0288             if (setup_->psModule(stubDR.frame().first))
0289               numConsistentPS++;
0290           }
0291         } else
0292           hitPattern.reset(s->layer());
0293       }
0294       std::reverse(stubsKF.begin(), stubsKF.end());
0295       // layer cut
0296       bool validLayers = hitPattern.count() >= setup_->kfMinLayers();
0297       // track parameter cuts
0298       const double cotTrack =
0299           dataFormats_->format(Variable::cot, Process::kf).digi(state->track()->zT() / setup_->chosenRofZ());
0300       const double inv2R = state->x0() + state->track()->inv2R();
0301       const double phiT = state->x1() + state->track()->phiT();
0302       const double cot = state->x2() + cotTrack;
0303       const double zT = state->x3() + state->track()->zT();
0304       const double d0 = state->x4();
0305       // pt cut
0306       const bool validX0 = dataFormats_->format(Variable::inv2R, Process::kf).inRange(inv2R);
0307       // cut on phi sector boundaries
0308       const bool validX1 = abs(phiT) < setup_->baseRegion() / 2.;
0309       // cot cut
0310       const bool validX2 = dataFormats_->format(Variable::cot, Process::kf).inRange(cot);
0311       // zT cut
0312       const bool validX3 = dataFormats_->format(Variable::zT, Process::kf).inRange(zT);
0313       if (!validLayers || !validX0 || !validX1 || !validX2 || !validX3)
0314         continue;
0315       const int trackId = state->trackId();
0316       const TrackKF trackKF(*state->track(), inv2R, phiT, cot, zT);
0317       finals_.emplace_back(trackId, numConsistent, numConsistentPS, d0, hitPattern, trackKF, stubsKF);
0318     }
0319   }
0320 
0321   // best state selection
0322   void KalmanFilter::accumulator() {
0323     // create container of pointer to make sorts less CPU intense
0324     std::vector<Track*> finals;
0325     finals.reserve(finals_.size());
0326     std::transform(finals_.begin(), finals_.end(), std::back_inserter(finals), [](Track& track) { return &track; });
0327     // prepare arrival order
0328     std::vector<int> trackIds;
0329     trackIds.reserve(tracks_.size());
0330     for (Track* track : finals) {
0331       const int trackId = track->trackId_;
0332       if (std::find_if(trackIds.begin(), trackIds.end(), [trackId](int id) { return id == trackId; }) == trackIds.end())
0333         trackIds.push_back(trackId);
0334     }
0335     // sort in number of consistent stubs
0336     auto moreConsistentLayers = [](Track* lhs, Track* rhs) { return lhs->numConsistent_ > rhs->numConsistent_; };
0337     std::stable_sort(finals.begin(), finals.end(), moreConsistentLayers);
0338     // sort in number of consistent ps stubs
0339     auto moreConsistentLayersPS = [](Track* lhs, Track* rhs) { return lhs->numConsistentPS_ > rhs->numConsistentPS_; };
0340     std::stable_sort(finals.begin(), finals.end(), moreConsistentLayersPS);
0341     // sort in track id as arrived
0342     auto order = [&trackIds](auto lhs, auto rhs) {
0343       const auto l = find(trackIds.begin(), trackIds.end(), lhs->trackId_);
0344       const auto r = find(trackIds.begin(), trackIds.end(), rhs->trackId_);
0345       return std::distance(r, l) < 0;
0346     };
0347     std::stable_sort(finals.begin(), finals.end(), order);
0348     // keep first state (best due to previous sorts) per track id
0349     const auto it = std::unique(
0350         finals.begin(), finals.end(), [](Track* lhs, Track* rhs) { return lhs->trackId_ == rhs->trackId_; });
0351     finals.erase(it, finals.end());
0352     // apply to actual track container
0353     int i(0);
0354     for (Track* track : finals)
0355       finals_[i++] = *track;
0356     finals_.resize(i);
0357   }
0358 
0359   // Transform States into output products
0360   void KalmanFilter::conv(tt::StreamsStub& streamsStub, tt::StreamsTrack& streamsTrack) {
0361     const int offset = region_ * setup_->numLayers();
0362     tt::StreamTrack& streamTrack = streamsTrack[region_];
0363     streamTrack.reserve(stream_.size());
0364     for (int layer = 0; layer < setup_->numLayers(); layer++)
0365       streamsStub[offset + layer].reserve(stream_.size());
0366     for (const Track& track : finals_) {
0367       streamTrack.emplace_back(track.trackKF_.frame());
0368       const TTBV& hitPattern = track.hitPattern_;
0369       const std::vector<StubKF>& stubsKF = track.stubsKF_;
0370       int i(0);
0371       for (int layer = 0; layer < setup_->numLayers(); layer++)
0372         streamsStub[offset + layer].emplace_back(hitPattern.test(layer) ? stubsKF[i++].frame() : tt::FrameStub());
0373       // store d0 in copied TTTracks
0374       if (setup_->kfUse5ParameterFit()) {
0375         const TTTrackRef& ttTrackRef = track.trackKF_.frame().first;
0376         ttTracks_.emplace_back(ttTrackRef->rInv(),
0377                                ttTrackRef->phi(),
0378                                ttTrackRef->tanL(),
0379                                ttTrackRef->z0(),
0380                                track.d0_,
0381                                ttTrackRef->chi2XY(),
0382                                ttTrackRef->chi2Z(),
0383                                ttTrackRef->trkMVA1(),
0384                                ttTrackRef->trkMVA2(),
0385                                ttTrackRef->trkMVA3(),
0386                                ttTrackRef->hitPattern(),
0387                                5,
0388                                setup_->bField());
0389         ttTracks_.back().setPhiSector(ttTrackRef->phiSector());
0390         ttTracks_.back().setEtaSector(ttTrackRef->etaSector());
0391         ttTracks_.back().setTrackSeedType(ttTrackRef->trackSeedType());
0392         ttTracks_.back().setStubPtConsistency(ttTrackRef->stubPtConsistency());
0393         ttTracks_.back().setStubRefs(ttTrackRef->getStubRefs());
0394       }
0395     }
0396   }
0397 
0398   // calculates the helix params & their cov. matrix from a pair of stubs
0399   void KalmanFilter::calcSeeds() {
0400     auto update = [this](State* s) {
0401       updateRangeActual(VariableKF::m0, s->m0());
0402       updateRangeActual(VariableKF::m1, s->m1());
0403       updateRangeActual(VariableKF::v0, s->v0());
0404       updateRangeActual(VariableKF::v1, s->v1());
0405       updateRangeActual(VariableKF::H00, s->H00());
0406       updateRangeActual(VariableKF::H12, s->H12());
0407     };
0408     for (State*& state : stream_) {
0409       if (!state)
0410         continue;
0411       State* s1 = state->parent();
0412       State* s0 = s1->parent();
0413       update(s0);
0414       update(s1);
0415       const double dH = digi(VariableKF::dH, s1->H00() - s0->H00());
0416       const double invdH = digi(VariableKF::invdH, 1.0 / dH);
0417       const double invdH2 = digi(VariableKF::invdH2, 1.0 / dH / dH);
0418       const double H12 = digi(VariableKF::H2, s1->H00() * s1->H00());
0419       const double H02 = digi(VariableKF::H2, s0->H00() * s0->H00());
0420       const double H32 = digi(VariableKF::H2, s1->H12() * s1->H12());
0421       const double H22 = digi(VariableKF::H2, s0->H12() * s0->H12());
0422       const double H1m0 = digi(VariableKF::Hm0, s1->H00() * s0->m0());
0423       const double H0m1 = digi(VariableKF::Hm0, s0->H00() * s1->m0());
0424       const double H3m2 = digi(VariableKF::Hm1, s1->H12() * s0->m1());
0425       const double H2m3 = digi(VariableKF::Hm1, s0->H12() * s1->m1());
0426       const double H1v0 = digi(VariableKF::Hv0, s1->H00() * s0->v0());
0427       const double H0v1 = digi(VariableKF::Hv0, s0->H00() * s1->v0());
0428       const double H3v2 = digi(VariableKF::Hv1, s1->H12() * s0->v1());
0429       const double H2v3 = digi(VariableKF::Hv1, s0->H12() * s1->v1());
0430       const double H12v0 = digi(VariableKF::H2v0, H12 * s0->v0());
0431       const double H02v1 = digi(VariableKF::H2v0, H02 * s1->v0());
0432       const double H32v2 = digi(VariableKF::H2v1, H32 * s0->v1());
0433       const double H22v3 = digi(VariableKF::H2v1, H22 * s1->v1());
0434       const double x0 = digi(VariableKF::x0, (s1->m0() - s0->m0()) * invdH);
0435       const double x2 = digi(VariableKF::x2, (s1->m1() - s0->m1()) * invdH);
0436       const double x1 = digi(VariableKF::x1, (H1m0 - H0m1) * invdH);
0437       const double x3 = digi(VariableKF::x3, (H3m2 - H2m3) * invdH);
0438       const double C00 = digi(VariableKF::C00, (s1->v0() + s0->v0()) * invdH2);
0439       const double C22 = digi(VariableKF::C22, (s1->v1() + s0->v1()) * invdH2);
0440       const double C01 = -digi(VariableKF::C01, (H1v0 + H0v1) * invdH2);
0441       const double C23 = -digi(VariableKF::C23, (H3v2 + H2v3) * invdH2);
0442       const double C11 = digi(VariableKF::C11, (H12v0 + H02v1) * invdH2);
0443       const double C33 = digi(VariableKF::C33, (H32v2 + H22v3) * invdH2);
0444       // create updated state
0445       states_.emplace_back(State(s1, {x0, x1, x2, x3, 0., C00, C11, C22, C33, C01, C23, 0., 0., 0.}));
0446       state = &states_.back();
0447       updateRangeActual(VariableKF::x0, x0);
0448       updateRangeActual(VariableKF::x1, x1);
0449       updateRangeActual(VariableKF::x2, x2);
0450       updateRangeActual(VariableKF::x3, x3);
0451       updateRangeActual(VariableKF::C00, C00);
0452       updateRangeActual(VariableKF::C01, C01);
0453       updateRangeActual(VariableKF::C11, C11);
0454       updateRangeActual(VariableKF::C22, C22);
0455       updateRangeActual(VariableKF::C23, C23);
0456       updateRangeActual(VariableKF::C33, C33);
0457     }
0458   }
0459 
0460   // adds a layer to states
0461   void KalmanFilter::addLayer(bool seed) {
0462     auto comb = [seed, this](State*& s) {
0463       if (s)
0464         s = seed ? s->combSeed(states_, layer_) : s->comb(states_, layer_);
0465     };
0466     auto param = [this](State*& s) { setup_->kfUse5ParameterFit() ? this->update5(s) : this->update4(s); };
0467     auto update = [seed, param, this](State*& s) {
0468       if (s && (seed || s->hitPattern().pmEncode() == layer_)) {
0469         if (seed)
0470           s = s->update(states_, layer_);
0471         else
0472           param(s);
0473       }
0474     };
0475     // Latency of KF Associator block firmware
0476     static constexpr int latency = 5;
0477     // dynamic state container for clock accurate emulation
0478     std::deque<State*> streamOutput;
0479     // Memory stack used to handle combinatorics
0480     std::deque<State*> stack;
0481     // static delay container
0482     std::deque<State*> delay(latency, nullptr);
0483     // each trip corresponds to a f/w clock tick
0484     // done if no states to process left, taking as much time as needed
0485     while (!stream_.empty() || !stack.empty() ||
0486            !std::all_of(delay.begin(), delay.end(), [](const State* state) { return state == nullptr; })) {
0487       State* state = pop_front(stream_);
0488       // Process a combinatoric state if no (non-combinatoric?) state available
0489       if (!state)
0490         state = pop_front(stack);
0491       streamOutput.push_back(state);
0492       // The remainder of the code in this loop deals with combinatoric states.
0493       comb(state);
0494       delay.push_back(state);
0495       state = pop_front(delay);
0496       if (state)
0497         stack.push_back(state);
0498     }
0499     stream_ = streamOutput;
0500     // Update state with next stub using KF maths
0501     for (State*& state : stream_)
0502       update(state);
0503   }
0504 
0505   // updates state
0506   void KalmanFilter::update4(State*& state) {
0507     // All variable names & equations come from Fruhwirth KF paper http://dx.doi.org/10.1016/0168-9002%2887%2990887-4", where F taken as unit matrix. Stub uncertainties projected onto (phi,z), assuming no correlations between r-phi & r-z planes.
0508     // stub phi residual wrt input helix
0509     const double m0 = state->m0();
0510     // stub z residual wrt input helix
0511     const double m1 = state->m1();
0512     // stub projected phi uncertainty squared);
0513     const double v0 = state->v0();
0514     // stub projected z uncertainty squared
0515     const double v1 = state->v1();
0516     // Derivative of predicted stub coords wrt helix params: stub radius minus chosenRofPhi
0517     const double H00 = state->H00();
0518     // Derivative of predicted stub coords wrt helix params: stub radius minus chosenRofZ
0519     const double H12 = state->H12();
0520     updateRangeActual(VariableKF::m0, m0);
0521     updateRangeActual(VariableKF::m1, m1);
0522     updateRangeActual(VariableKF::v0, v0);
0523     updateRangeActual(VariableKF::v1, v1);
0524     updateRangeActual(VariableKF::H00, H00);
0525     updateRangeActual(VariableKF::H12, H12);
0526     // helix inv2R wrt input helix
0527     double x0 = state->x0();
0528     // helix phi at radius ChosenRofPhi wrt input helix
0529     double x1 = state->x1();
0530     // helix cot(Theta) wrt input helix
0531     double x2 = state->x2();
0532     // helix z at radius chosenRofZ wrt input helix
0533     double x3 = state->x3();
0534     // cov. matrix
0535     double C00 = state->C00();
0536     double C01 = state->C01();
0537     double C11 = state->C11();
0538     double C22 = state->C22();
0539     double C23 = state->C23();
0540     double C33 = state->C33();
0541     // stub phi residual wrt current state
0542     const double r0C = digi(VariableKF::x1, m0 - x1);
0543     const double r0 = digi(VariableKF::r0, r0C - x0 * H00);
0544     // stub z residual wrt current state
0545     const double r1C = digi(VariableKF::x3, m1 - x3);
0546     const double r1 = digi(VariableKF::r1, r1C - x2 * H12);
0547     // matrix S = H*C
0548     const double S00 = digi(VariableKF::S00, C01 + H00 * C00);
0549     const double S01 = digi(VariableKF::S01, C11 + H00 * C01);
0550     const double S12 = digi(VariableKF::S12, C23 + H12 * C22);
0551     const double S13 = digi(VariableKF::S13, C33 + H12 * C23);
0552     // Cov. matrix of predicted residuals R = V+HCHt = C+H*St
0553     const double R00 = digi(VariableKF::R00, v0 + S01 + H00 * S00);
0554     const double R11 = digi(VariableKF::R11, v1 + S13 + H12 * S12);
0555     // improved dynamic cancelling
0556     const int msb0 = std::max(0, static_cast<int>(std::ceil(std::log2(R00 / base(VariableKF::R00)))));
0557     const int msb1 = std::max(0, static_cast<int>(std::ceil(std::log2(R11 / base(VariableKF::R11)))));
0558     const int shift0 = width(VariableKF::R00) - msb0;
0559     const int shift1 = width(VariableKF::R11) - msb1;
0560     const double R00Shifted = R00 * std::pow(2., shift0);
0561     const double R11Shifted = R11 * std::pow(2., shift1);
0562     const double R00Rough = digi(VariableKF::R00Rough, R00Shifted);
0563     const double R11Rough = digi(VariableKF::R11Rough, R11Shifted);
0564     const double invR00Approx = digi(VariableKF::invR00Approx, 1. / R00Rough);
0565     const double invR11Approx = digi(VariableKF::invR11Approx, 1. / R11Rough);
0566     const double invR00Cor = digi(VariableKF::invR00Cor, 2. - invR00Approx * R00Shifted);
0567     const double invR11Cor = digi(VariableKF::invR11Cor, 2. - invR11Approx * R11Shifted);
0568     const double invR00 = digi(VariableKF::invR00, invR00Approx * invR00Cor);
0569     const double invR11 = digi(VariableKF::invR11, invR11Approx * invR11Cor);
0570     // shift S to "undo" shifting of R
0571     auto digiShifted = [](double val, double base) { return std::floor(val / base * 2. + 1.e-11) * base / 2.; };
0572     const double S00Shifted = digiShifted(S00 * std::pow(2., shift0), base(VariableKF::S00Shifted));
0573     const double S01Shifted = digiShifted(S01 * std::pow(2., shift0), base(VariableKF::S01Shifted));
0574     const double S12Shifted = digiShifted(S12 * std::pow(2., shift1), base(VariableKF::S12Shifted));
0575     const double S13Shifted = digiShifted(S13 * std::pow(2., shift1), base(VariableKF::S13Shifted));
0576     // Kalman gain matrix K = S*R(inv)
0577     const double K00 = digi(VariableKF::K00, S00Shifted * invR00);
0578     const double K10 = digi(VariableKF::K10, S01Shifted * invR00);
0579     const double K21 = digi(VariableKF::K21, S12Shifted * invR11);
0580     const double K31 = digi(VariableKF::K31, S13Shifted * invR11);
0581     // Updated helix params, their cov. matrix
0582     x0 = digi(VariableKF::x0, x0 + r0 * K00);
0583     x1 = digi(VariableKF::x1, x1 + r0 * K10);
0584     x2 = digi(VariableKF::x2, x2 + r1 * K21);
0585     x3 = digi(VariableKF::x3, x3 + r1 * K31);
0586     C00 = digi(VariableKF::C00, C00 - S00 * K00);
0587     C01 = digi(VariableKF::C01, C01 - S01 * K00);
0588     C11 = digi(VariableKF::C11, C11 - S01 * K10);
0589     C22 = digi(VariableKF::C22, C22 - S12 * K21);
0590     C23 = digi(VariableKF::C23, C23 - S13 * K21);
0591     C33 = digi(VariableKF::C33, C33 - S13 * K31);
0592     // update variable ranges to tune variable granularity
0593     updateRangeActual(VariableKF::r0, r0);
0594     updateRangeActual(VariableKF::r1, r1);
0595     updateRangeActual(VariableKF::S00, S00);
0596     updateRangeActual(VariableKF::S01, S01);
0597     updateRangeActual(VariableKF::S12, S12);
0598     updateRangeActual(VariableKF::S13, S13);
0599     updateRangeActual(VariableKF::S00Shifted, S00Shifted);
0600     updateRangeActual(VariableKF::S01Shifted, S01Shifted);
0601     updateRangeActual(VariableKF::S12Shifted, S12Shifted);
0602     updateRangeActual(VariableKF::S13Shifted, S13Shifted);
0603     updateRangeActual(VariableKF::R00, R00);
0604     updateRangeActual(VariableKF::R11, R11);
0605     updateRangeActual(VariableKF::R00Rough, R00Rough);
0606     updateRangeActual(VariableKF::R11Rough, R11Rough);
0607     updateRangeActual(VariableKF::invR00Approx, invR00Approx);
0608     updateRangeActual(VariableKF::invR11Approx, invR11Approx);
0609     updateRangeActual(VariableKF::invR00Cor, invR00Cor);
0610     updateRangeActual(VariableKF::invR11Cor, invR11Cor);
0611     updateRangeActual(VariableKF::invR00, invR00);
0612     updateRangeActual(VariableKF::invR11, invR11);
0613     updateRangeActual(VariableKF::K00, K00);
0614     updateRangeActual(VariableKF::K10, K10);
0615     updateRangeActual(VariableKF::K21, K21);
0616     updateRangeActual(VariableKF::K31, K31);
0617     // create updated state
0618     states_.emplace_back(State(state, {x0, x1, x2, x3, 0., C00, C11, C22, C33, C01, C23, 0., 0., 0.}));
0619     state = &states_.back();
0620     updateRangeActual(VariableKF::x0, x0);
0621     updateRangeActual(VariableKF::x1, x1);
0622     updateRangeActual(VariableKF::x2, x2);
0623     updateRangeActual(VariableKF::x3, x3);
0624     updateRangeActual(VariableKF::C00, C00);
0625     updateRangeActual(VariableKF::C01, C01);
0626     updateRangeActual(VariableKF::C11, C11);
0627     updateRangeActual(VariableKF::C22, C22);
0628     updateRangeActual(VariableKF::C23, C23);
0629     updateRangeActual(VariableKF::C33, C33);
0630   }
0631 
0632   // updates state
0633   void KalmanFilter::update5(State*& state) {
0634     const double m0 = state->m0();
0635     const double m1 = state->m1();
0636     const double v0 = state->v0();
0637     const double v1 = state->v1();
0638     const double H00 = state->H00();
0639     const double H12 = state->H12();
0640     const double H04 = state->H04();
0641     double x0 = state->x0();
0642     double x1 = state->x1();
0643     double x2 = state->x2();
0644     double x3 = state->x3();
0645     double x4 = state->x4();
0646     double C00 = state->C00();
0647     double C01 = state->C01();
0648     double C11 = state->C11();
0649     double C22 = state->C22();
0650     double C23 = state->C23();
0651     double C33 = state->C33();
0652     double C44 = state->C44();
0653     double C40 = state->C40();
0654     double C41 = state->C41();
0655     const double r0 = m0 - x1 - x0 * H00 - x4 / H04;
0656     const double r1 = m1 - x3 - x2 * H12;
0657     const double S00 = C01 + H00 * C00 + C40 / H04;
0658     const double S01 = C11 + H00 * C01 + C41 / H04;
0659     const double S12 = C23 + H12 * C22;
0660     const double S13 = C33 + H12 * C23;
0661     const double S04 = C41 + H00 * C40 + C44 / H04;
0662     const double R00 = v0 + S01 + H00 * S00 + S04 / H04;
0663     const double R11 = v1 + S13 + H12 * S12;
0664     const double K00 = S00 / R00;
0665     const double K10 = S01 / R00;
0666     const double K21 = S12 / R11;
0667     const double K31 = S13 / R11;
0668     const double K40 = S04 / R00;
0669     x0 += r0 * K00;
0670     x1 += r0 * K10;
0671     x2 += r1 * K21;
0672     x3 += r1 * K31;
0673     x4 += r0 * K40;
0674     C00 -= S00 * K00;
0675     C01 -= S01 * K00;
0676     C11 -= S01 * K10;
0677     C22 -= S12 * K21;
0678     C23 -= S13 * K21;
0679     C33 -= S13 * K31;
0680     C44 -= S04 * K40;
0681     C40 -= S04 * K00;
0682     C41 -= S04 * K10;
0683     states_.emplace_back(State(state, {x0, x1, x2, x3, x4, C00, C11, C22, C33, C01, C23, C44, C40, C41}));
0684     state = &states_.back();
0685   }
0686 
0687   // remove and return first element of deque, returns nullptr if empty
0688   template <class T>
0689   T* KalmanFilter::pop_front(std::deque<T*>& ts) const {
0690     T* t = nullptr;
0691     if (!ts.empty()) {
0692       t = ts.front();
0693       ts.pop_front();
0694     }
0695     return t;
0696   }
0697 
0698 }  // namespace trklet
[ 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!