Back to home page

Project CMSSW displayed by LXR

 
 

    


File indexing completed on 2022-10-14 01:43:54

0001 #include "L1Trigger/TrackerTFP/interface/KalmanFilter.h"
0002 
0003 #include <numeric>
0004 #include <algorithm>
0005 #include <iterator>
0006 #include <deque>
0007 #include <vector>
0008 #include <set>
0009 #include <utility>
0010 #include <cmath>
0011 
0012 using namespace std;
0013 using namespace edm;
0014 using namespace tt;
0015 
0016 namespace trackerTFP {
0017 
0018   KalmanFilter::KalmanFilter(const ParameterSet& iConfig,
0019                              const Setup* setup,
0020                              const DataFormats* dataFormats,
0021                              KalmanFilterFormats* kalmanFilterFormats,
0022                              int region)
0023       : enableTruncation_(iConfig.getParameter<bool>("EnableTruncation")),
0024         setup_(setup),
0025         dataFormats_(dataFormats),
0026         kalmanFilterFormats_(kalmanFilterFormats),
0027         region_(region),
0028         input_(dataFormats_->numChannel(Process::kf)),
0029         layer_(0),
0030         x0_(&kalmanFilterFormats_->format(VariableKF::x0)),
0031         x1_(&kalmanFilterFormats_->format(VariableKF::x1)),
0032         x2_(&kalmanFilterFormats_->format(VariableKF::x2)),
0033         x3_(&kalmanFilterFormats_->format(VariableKF::x3)),
0034         H00_(&kalmanFilterFormats_->format(VariableKF::H00)),
0035         H12_(&kalmanFilterFormats_->format(VariableKF::H12)),
0036         m0_(&kalmanFilterFormats_->format(VariableKF::m0)),
0037         m1_(&kalmanFilterFormats_->format(VariableKF::m1)),
0038         v0_(&kalmanFilterFormats_->format(VariableKF::v0)),
0039         v1_(&kalmanFilterFormats_->format(VariableKF::v1)),
0040         r0_(&kalmanFilterFormats_->format(VariableKF::r0)),
0041         r1_(&kalmanFilterFormats_->format(VariableKF::r1)),
0042         S00_(&kalmanFilterFormats_->format(VariableKF::S00)),
0043         S01_(&kalmanFilterFormats_->format(VariableKF::S01)),
0044         S12_(&kalmanFilterFormats_->format(VariableKF::S12)),
0045         S13_(&kalmanFilterFormats_->format(VariableKF::S13)),
0046         K00_(&kalmanFilterFormats_->format(VariableKF::K00)),
0047         K10_(&kalmanFilterFormats_->format(VariableKF::K10)),
0048         K21_(&kalmanFilterFormats_->format(VariableKF::K21)),
0049         K31_(&kalmanFilterFormats_->format(VariableKF::K31)),
0050         R00_(&kalmanFilterFormats_->format(VariableKF::R00)),
0051         R11_(&kalmanFilterFormats_->format(VariableKF::R11)),
0052         R00Rough_(&kalmanFilterFormats_->format(VariableKF::R00Rough)),
0053         R11Rough_(&kalmanFilterFormats_->format(VariableKF::R11Rough)),
0054         invR00Approx_(&kalmanFilterFormats_->format(VariableKF::invR00Approx)),
0055         invR11Approx_(&kalmanFilterFormats_->format(VariableKF::invR11Approx)),
0056         invR00Cor_(&kalmanFilterFormats_->format(VariableKF::invR00Cor)),
0057         invR11Cor_(&kalmanFilterFormats_->format(VariableKF::invR11Cor)),
0058         invR00_(&kalmanFilterFormats_->format(VariableKF::invR00)),
0059         invR11_(&kalmanFilterFormats_->format(VariableKF::invR11)),
0060         C00_(&kalmanFilterFormats_->format(VariableKF::C00)),
0061         C01_(&kalmanFilterFormats_->format(VariableKF::C01)),
0062         C11_(&kalmanFilterFormats_->format(VariableKF::C11)),
0063         C22_(&kalmanFilterFormats_->format(VariableKF::C22)),
0064         C23_(&kalmanFilterFormats_->format(VariableKF::C23)),
0065         C33_(&kalmanFilterFormats_->format(VariableKF::C33)) {
0066     C00_->updateRangeActual(pow(dataFormats_->base(Variable::inv2R, Process::kfin), 2));
0067     C11_->updateRangeActual(pow(dataFormats_->base(Variable::phiT, Process::kfin), 2));
0068     C22_->updateRangeActual(pow(dataFormats_->base(Variable::cot, Process::kfin), 2));
0069     C33_->updateRangeActual(pow(dataFormats_->base(Variable::zT, Process::kfin), 2));
0070   }
0071 
0072   // read in and organize input product (fill vector input_)
0073   void KalmanFilter::consume(const StreamsTrack& streamsTrack, const StreamsStub& streamsStub) {
0074     auto valid = [](const auto& frame) { return frame.first.isNonnull(); };
0075     auto acc = [](int& sum, const auto& frame) { return sum += (frame.first.isNonnull() ? 1 : 0); };
0076     int nTracks(0);
0077     int nStubs(0);
0078     const int offset = region_ * dataFormats_->numChannel(Process::kf);
0079     for (int channel = 0; channel < dataFormats_->numChannel(Process::kf); channel++) {
0080       const int channelTrack = offset + channel;
0081       const StreamTrack& streamTracks = streamsTrack[channelTrack];
0082       nTracks += accumulate(streamTracks.begin(), streamTracks.end(), 0, acc);
0083       for (int layer = 0; layer < setup_->numLayers(); layer++) {
0084         const int channelStub = channelTrack * setup_->numLayers() + layer;
0085         const StreamStub& streamStubs = streamsStub[channelStub];
0086         nStubs += accumulate(streamStubs.begin(), streamStubs.end(), 0, acc);
0087       }
0088     }
0089     tracks_.reserve(nTracks);
0090     stubs_.reserve(nStubs);
0091     // N.B. One input stream for track & one for its stubs in each layer. If a track has N stubs in one layer, and fewer in all other layers, then next valid track will be N frames later
0092     for (int channel = 0; channel < dataFormats_->numChannel(Process::kf); channel++) {
0093       const int channelTrack = offset + channel;
0094       const StreamTrack& streamTracks = streamsTrack[channelTrack];
0095       vector<TrackKFin*>& tracks = input_[channel];
0096       tracks.reserve(streamTracks.size());
0097       for (int frame = 0; frame < (int)streamTracks.size(); frame++) {
0098         const FrameTrack& frameTrack = streamTracks[frame];
0099         // Select frames with valid track
0100         if (frameTrack.first.isNull()) {
0101           if (dataFormats_->hybrid())
0102             tracks.push_back(nullptr);
0103           continue;
0104         }
0105         auto endOfTrk = find_if(next(streamTracks.begin(), frame + 1), streamTracks.end(), valid);
0106         if (dataFormats_->hybrid())
0107           endOfTrk = next(streamTracks.begin(), frame + 1);
0108         // No. of frames before next track indicates gives max. no. of stubs this track had in any layer
0109         const int maxStubsPerLayer = distance(next(streamTracks.begin(), frame), endOfTrk);
0110         tracks.insert(tracks.end(), maxStubsPerLayer - 1, nullptr);
0111         deque<StubKFin*> stubs;
0112         for (int layer = 0; layer < setup_->numLayers(); layer++) {
0113           const int channelStub = channelTrack * setup_->numLayers() + layer;
0114           const StreamStub& streamStubs = streamsStub[channelStub];
0115           // Get stubs on this track
0116           for (int i = frame; i < frame + maxStubsPerLayer; i++) {
0117             const FrameStub& frameStub = streamStubs[i];
0118             if (frameStub.first.isNull())
0119               break;
0120             // Store input stubs, so remainder of KF algo can work with pointers to them (saves CPU)
0121             stubs_.emplace_back(frameStub, dataFormats_, layer);
0122             stubs.push_back(&stubs_.back());
0123           }
0124         }
0125         // Store input tracks, so remainder of KF algo can work with pointers to them (saves CPU)
0126         tracks_.emplace_back(frameTrack, dataFormats_, vector<StubKFin*>(stubs.begin(), stubs.end()));
0127         tracks.push_back(&tracks_.back());
0128       }
0129     }
0130   }
0131 
0132   // fill output products
0133   void KalmanFilter::produce(StreamsStub& acceptedStubs,
0134                              StreamsTrack& acceptedTracks,
0135                              StreamsStub& lostStubs,
0136                              StreamsTrack& lostTracks,
0137                              int& numAcceptedStates,
0138                              int& numLostStates) {
0139     auto put = [this](
0140                    const deque<State*>& states, StreamsStub& streamsStubs, StreamsTrack& streamsTracks, int channel) {
0141       const int streamId = region_ * dataFormats_->numChannel(Process::kf) + channel;
0142       const int offset = streamId * setup_->numLayers();
0143       StreamTrack& tracks = streamsTracks[streamId];
0144       tracks.reserve(states.size());
0145       for (int layer = 0; layer < setup_->numLayers(); layer++)
0146         streamsStubs[offset + layer].reserve(states.size());
0147       for (State* state : states) {
0148         tracks.emplace_back(state->frame());
0149         for (const StubKF& stub : state->stubs())
0150           streamsStubs[offset + stub.layer()].emplace_back(stub.frame());
0151         // adding a gap to all layer without a stub
0152         for (int layer : state->hitPattern().ids(false))
0153           streamsStubs[offset + layer].emplace_back(FrameStub());
0154       }
0155     };
0156     auto count = [this](int& sum, const State* state) {
0157       return sum += state && state->hitPattern().count() >= setup_->kfMinLayers() ? 1 : 0;
0158     };
0159     for (int channel = 0; channel < dataFormats_->numChannel(Process::kf); channel++) {
0160       deque<State*> stream;
0161       deque<State*> lost;
0162       // proto state creation
0163       int trackId(0);
0164       for (TrackKFin* track : input_[channel]) {
0165         State* state = nullptr;
0166         if (track) {
0167           // Store states, so remainder of KF algo can work with pointers to them (saves CPU)
0168           states_.emplace_back(dataFormats_, track, trackId++);
0169           state = &states_.back();
0170         }
0171         stream.push_back(state);
0172       }
0173       // Propagate state to each layer in turn, updating it with all viable stub combinations there, using KF maths
0174       for (layer_ = 0; layer_ < setup_->numLayers(); layer_++)
0175         addLayer(stream);
0176       // calculate number of states before truncating
0177       const int numUntruncatedStates = accumulate(stream.begin(), stream.end(), 0, count);
0178       // untruncated best state selection
0179       deque<State*> untruncatedStream = stream;
0180       accumulator(untruncatedStream);
0181       // apply truncation
0182       if (enableTruncation_ && (int)stream.size() > setup_->numFrames())
0183         stream.resize(setup_->numFrames());
0184       // calculate number of states after truncating
0185       const int numTruncatedStates = accumulate(stream.begin(), stream.end(), 0, count);
0186       // best state per candidate selection
0187       accumulator(stream);
0188       deque<State*> truncatedStream = stream;
0189       // storing of best states missed due to truncation
0190       sort(untruncatedStream.begin(), untruncatedStream.end());
0191       sort(truncatedStream.begin(), truncatedStream.end());
0192       set_difference(untruncatedStream.begin(),
0193                      untruncatedStream.end(),
0194                      truncatedStream.begin(),
0195                      truncatedStream.end(),
0196                      back_inserter(lost));
0197       // store found tracks
0198       put(stream, acceptedStubs, acceptedTracks, channel);
0199       // store lost tracks
0200       put(lost, lostStubs, lostTracks, channel);
0201       // store number of states which got taken into account
0202       numAcceptedStates += numTruncatedStates;
0203       // store number of states which got not taken into account due to truncation
0204       numLostStates += numUntruncatedStates - numTruncatedStates;
0205     }
0206   }
0207 
0208   // adds a layer to states
0209   void KalmanFilter::addLayer(deque<State*>& stream) {
0210     // Latency of KF Associator block firmware
0211     static constexpr int latency = 5;
0212     // dynamic state container for clock accurate emulation
0213     deque<State*> streamOutput;
0214     // Memory stack used to handle combinatorics
0215     deque<State*> stack;
0216     // static delay container
0217     vector<State*> delay(latency, nullptr);
0218     // each trip corresponds to a f/w clock tick
0219     // done if no states to process left, taking as much time as needed
0220     while (!stream.empty() || !stack.empty() ||
0221            !all_of(delay.begin(), delay.end(), [](const State* state) { return state == nullptr; })) {
0222       State* state = pop_front(stream);
0223       // Process a combinatoric state if no (non-combinatoric?) state available
0224       if (!state)
0225         state = pop_front(stack);
0226       streamOutput.push_back(state);
0227       // The remainder of the code in this loop deals with combinatoric states.
0228       if (state != nullptr)
0229         // Assign next combinatoric stub to state
0230         comb(state);
0231       delay.push_back(state);
0232       state = pop_front(delay);
0233       if (state != nullptr)
0234         stack.push_back(state);
0235     }
0236     stream = streamOutput;
0237     for (State*& state : stream) {
0238       if (!state || !state->stub() || state->layer() != layer_)
0239         continue;
0240       // Update state with next stub using KF maths
0241       update(state);
0242     }
0243   }
0244 
0245   // Assign next combinatoric (i.e. not first in layer) stub to state
0246   void KalmanFilter::comb(State*& state) {
0247     const TrackKFin* track = state->track();
0248     const StubKFin* stub = state->stub();
0249     const vector<StubKFin*>& stubs = track->layerStubs(layer_);
0250     const TTBV& hitPattern = state->hitPattern();
0251     StubKFin* stubNext = nullptr;
0252     // Get first stub on this layer if state reached min layers
0253     if (!stub) {
0254       if (hitPattern.count() < setup_->kfMaxLayers() && track->hitPattern(layer_))
0255         stubNext = track->layerStub(layer_);
0256     } else if (stub->layer() == layer_) {
0257       // Get next unused stub on this layer
0258       const int pos = distance(stubs.begin(), find(stubs.begin(), stubs.end(), stub)) + 1;
0259       if (pos != (int)stubs.size())
0260         stubNext = stubs[pos];
0261       // picks next stub on different layer, nullifies state if skipping layer is not valid
0262       else {
0263         bool valid(true);
0264         // having already maximum number of added layers
0265         if (hitPattern.count() == setup_->kfMaxLayers())
0266           valid = false;
0267         // Impossible for this state to ever get enough layers to form valid track
0268         if (hitPattern.count() + track->hitPattern().count(stub->layer() + 1, setup_->numLayers()) <
0269             setup_->kfMinLayers())
0270           valid = false;
0271         if (layer_ == setup_->numLayers() - 1)
0272           valid = false;
0273         if (valid) {
0274           // pick next stub on next populated layer
0275           for (int nextLayer = layer_ + 1; nextLayer < setup_->numLayers(); nextLayer++) {
0276             if (track->hitPattern(nextLayer)) {
0277               stubNext = track->layerStub(nextLayer);
0278               break;
0279             }
0280           }
0281         }
0282       }
0283     }
0284     if (stubNext) {
0285       // create combinatoric state
0286       states_.emplace_back(state, stubNext);
0287       state = &states_.back();
0288     } else
0289       state = nullptr;
0290   }
0291 
0292   // best state selection
0293   void KalmanFilter::accumulator(deque<State*>& stream) {
0294     // accumulator delivers contigious stream of best state per track
0295     // remove gaps and not final states
0296     stream.erase(
0297         remove_if(stream.begin(),
0298                   stream.end(),
0299                   [this](State* state) { return !state || state->hitPattern().count() < setup_->kfMinLayers(); }),
0300         stream.end());
0301     // Determine quality of completed state
0302     for (State* state : stream)
0303       state->finish();
0304     // sort in number of skipped layers
0305     auto lessSkippedLayers = [](State* lhs, State* rhs) { return lhs->numSkippedLayers() < rhs->numSkippedLayers(); };
0306     stable_sort(stream.begin(), stream.end(), lessSkippedLayers);
0307     // sort in number of consistent stubs
0308     auto moreConsistentLayers = [](State* lhs, State* rhs) {
0309       return lhs->numConsistentLayers() > rhs->numConsistentLayers();
0310     };
0311     stable_sort(stream.begin(), stream.end(), moreConsistentLayers);
0312     // sort in track id
0313     stable_sort(stream.begin(), stream.end(), [](State* lhs, State* rhs) { return lhs->trackId() < rhs->trackId(); });
0314     // keep first state (best due to previous sorts) per track id
0315     stream.erase(
0316         unique(stream.begin(), stream.end(), [](State* lhs, State* rhs) { return lhs->track() == rhs->track(); }),
0317         stream.end());
0318   }
0319 
0320   // updates state
0321   void KalmanFilter::update(State*& state) {
0322     // 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.
0323     // stub phi residual wrt input helix
0324     const double m0 = m0_->digi(state->m0());
0325     // stub z residual wrt input helix
0326     const double m1 = m1_->digi(state->m1());
0327     // stub projected phi uncertainty squared);
0328     const double v0 = v0_->digi(state->v0());
0329     // stub projected z uncertainty squared
0330     const double v1 = v1_->digi(state->v1());
0331     // helix inv2R wrt input helix
0332     double x0 = x0_->digi(state->x0());
0333     // helix phi at radius ChosenRofPhi wrt input helix
0334     double x1 = x1_->digi(state->x1());
0335     // helix cot(Theta) wrt input helix
0336     double x2 = x2_->digi(state->x2());
0337     // helix z at radius chosenRofZ wrt input helix
0338     double x3 = x3_->digi(state->x3());
0339     // Derivative of predicted stub coords wrt helix params: stub radius minus chosenRofPhi
0340     const double H00 = H00_->digi(state->H00());
0341     // Derivative of predicted stub coords wrt helix params: stub radius minus chosenRofZ
0342     const double H12 = H12_->digi(state->H12());
0343     // cov. matrix
0344     double C00 = C00_->digi(state->C00());
0345     double C01 = C01_->digi(state->C01());
0346     double C11 = C11_->digi(state->C11());
0347     double C22 = C22_->digi(state->C22());
0348     double C23 = C23_->digi(state->C23());
0349     double C33 = C33_->digi(state->C33());
0350     // stub phi residual wrt current state
0351     const double r0C = x1_->digi(m0 - x1);
0352     const double r0 = r0_->digi(r0C - x0 * H00);
0353     // stub z residual wrt current state
0354     const double r1C = x3_->digi(m1 - x3);
0355     const double r1 = r1_->digi(r1C - x2 * H12);
0356     // matrix S = H*C
0357     const double S00 = S00_->digi(C01 + H00 * C00);
0358     const double S01 = S01_->digi(C11 + H00 * C01);
0359     const double S12 = S12_->digi(C23 + H12 * C22);
0360     const double S13 = S13_->digi(C33 + H12 * C23);
0361     // Cov. matrix of predicted residuals R = V+HCHt = C+H*St
0362     const double R00C = S01_->digi(v0 + S01);
0363     const double R00 = R00_->digi(R00C + H00 * S00);
0364     const double R11C = S13_->digi(v1 + S13);
0365     const double R11 = R11_->digi(R11C + H12 * S12);
0366     // imrpoved dynamic cancelling
0367     const int msb0 = max(0, (int)ceil(log2(R00 / R00_->base())));
0368     const int msb1 = max(0, (int)ceil(log2(R11 / R11_->base())));
0369     const double R00Rough = R00Rough_->digi(R00 * pow(2., 16 - msb0));
0370     const double invR00Approx = invR00Approx_->digi(1. / R00Rough);
0371     const double invR00Cor = invR00Cor_->digi(2. - invR00Approx * R00Rough);
0372     const double invR00 = invR00_->digi(invR00Approx * invR00Cor * pow(2., 16 - msb0));
0373     const double R11Rough = R11Rough_->digi(R11 * pow(2., 16 - msb1));
0374     const double invR11Approx = invR11Approx_->digi(1. / R11Rough);
0375     const double invR11Cor = invR11Cor_->digi(2. - invR11Approx * R11Rough);
0376     const double invR11 = invR11_->digi(invR11Approx * invR11Cor * pow(2., 16 - msb1));
0377     // Kalman gain matrix K = S*R(inv)
0378     const double K00 = K00_->digi(S00 * invR00);
0379     const double K10 = K10_->digi(S01 * invR00);
0380     const double K21 = K21_->digi(S12 * invR11);
0381     const double K31 = K31_->digi(S13 * invR11);
0382     // Updated helix params & their cov. matrix
0383     x0 = x0_->digi(x0 + r0 * K00);
0384     x1 = x1_->digi(x1 + r0 * K10);
0385     x2 = x2_->digi(x2 + r1 * K21);
0386     x3 = x3_->digi(x3 + r1 * K31);
0387     C00 = C00_->digi(C00 - S00 * K00);
0388     C01 = C01_->digi(C01 - S01 * K00);
0389     C11 = C11_->digi(C11 - S01 * K10);
0390     C22 = C22_->digi(C22 - S12 * K21);
0391     C23 = C23_->digi(C23 - S13 * K21);
0392     C33 = C33_->digi(C33 - S13 * K31);
0393     // create updated state
0394     states_.emplace_back(State(state, (initializer_list<double>){x0, x1, x2, x3, C00, C11, C22, C33, C01, C23}));
0395     state = &states_.back();
0396     // update variable ranges to tune variable granularity
0397     m0_->updateRangeActual(m0);
0398     m1_->updateRangeActual(m1);
0399     v0_->updateRangeActual(v0);
0400     v1_->updateRangeActual(v1);
0401     H00_->updateRangeActual(H00);
0402     H12_->updateRangeActual(H12);
0403     r0_->updateRangeActual(r0);
0404     r1_->updateRangeActual(r1);
0405     S00_->updateRangeActual(S00);
0406     S01_->updateRangeActual(S01);
0407     S12_->updateRangeActual(S12);
0408     S13_->updateRangeActual(S13);
0409     R00_->updateRangeActual(R00);
0410     R11_->updateRangeActual(R11);
0411     R00Rough_->updateRangeActual(R00Rough);
0412     invR00Approx_->updateRangeActual(invR00Approx);
0413     invR00Cor_->updateRangeActual(invR00Cor);
0414     invR00_->updateRangeActual(invR00);
0415     R11Rough_->updateRangeActual(R11Rough);
0416     invR11Approx_->updateRangeActual(invR11Approx);
0417     invR11Cor_->updateRangeActual(invR11Cor);
0418     invR11_->updateRangeActual(invR11);
0419     K00_->updateRangeActual(K00);
0420     K10_->updateRangeActual(K10);
0421     K21_->updateRangeActual(K21);
0422     K31_->updateRangeActual(K31);
0423     x0_->updateRangeActual(x0);
0424     x1_->updateRangeActual(x1);
0425     x2_->updateRangeActual(x2);
0426     x3_->updateRangeActual(x3);
0427     C00_->updateRangeActual(C00);
0428     C01_->updateRangeActual(C01);
0429     C11_->updateRangeActual(C11);
0430     C22_->updateRangeActual(C22);
0431     C23_->updateRangeActual(C23);
0432     C33_->updateRangeActual(C33);
0433   }
0434 
0435   // remove and return first element of deque, returns nullptr if empty
0436   template <class T>
0437   T* KalmanFilter::pop_front(deque<T*>& ts) const {
0438     T* t = nullptr;
0439     if (!ts.empty()) {
0440       t = ts.front();
0441       ts.pop_front();
0442     }
0443     return t;
0444   }
0445 
0446   // remove and return first element of vector, returns nullptr if empty
0447   template <class T>
0448   T* KalmanFilter::pop_front(vector<T*>& ts) const {
0449     T* t = nullptr;
0450     if (!ts.empty()) {
0451       t = ts.front();
0452       ts.erase(ts.begin());
0453     }
0454     return t;
0455   }
0456 
0457 }  // namespace trackerTFP