TrackerTFP/src/KalmanFilter.cc

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         vector<StubKF> stubs;
0150         state->fill(stubs);
0151         for (const StubKF& stub : stubs)
0152           streamsStubs[offset + stub.layer()].emplace_back(stub.frame());
0153         // adding a gap to all layer without a stub
0154         for (int layer : state->hitPattern().ids(false))
0155           streamsStubs[offset + layer].emplace_back(FrameStub());
0156       }
0157     };
0158     auto count = [this](int sum, const State* state) {
0159       return sum + ((state && state->hitPattern().count() >= setup_->kfMinLayers()) ? 1 : 0);
0160     };
0161     for (int channel = 0; channel < dataFormats_->numChannel(Process::kf); channel++) {
0162       deque<State*> stream;
0163       deque<State*> lost;
0164       // proto state creation
0165       int trackId(0);
0166       for (TrackKFin* track : input_[channel]) {
0167         State* state = nullptr;
0168         if (track) {
0169           // Store states, so remainder of KF algo can work with pointers to them (saves CPU)
0170           states_.emplace_back(dataFormats_, track, trackId++);
0171           state = &states_.back();
0172         }
0173         stream.push_back(state);
0174       }
0175       // Propagate state to each layer in turn, updating it with all viable stub combinations there, using KF maths
0176       for (layer_ = 0; layer_ < setup_->numLayers(); layer_++)
0177         addLayer(stream);
0178       // calculate number of states before truncating
0179       const int numUntruncatedStates = accumulate(stream.begin(), stream.end(), 0, count);
0180       // untruncated best state selection
0181       deque<State*> untruncatedStream = stream;
0182       accumulator(untruncatedStream);
0183       // apply truncation
0184       if (enableTruncation_ && (int)stream.size() > setup_->numFrames())
0185         stream.resize(setup_->numFrames());
0186       // calculate number of states after truncating
0187       const int numTruncatedStates = accumulate(stream.begin(), stream.end(), 0, count);
0188       // best state per candidate selection
0189       accumulator(stream);
0190       deque<State*> truncatedStream = stream;
0191       // storing of best states missed due to truncation
0192       sort(untruncatedStream.begin(), untruncatedStream.end());
0193       sort(truncatedStream.begin(), truncatedStream.end());
0194       set_difference(untruncatedStream.begin(),
0195                      untruncatedStream.end(),
0196                      truncatedStream.begin(),
0197                      truncatedStream.end(),
0198                      back_inserter(lost));
0199       // store found tracks
0200       put(stream, acceptedStubs, acceptedTracks, channel);
0201       // store lost tracks
0202       put(lost, lostStubs, lostTracks, channel);
0203       // store number of states which got taken into account
0204       numAcceptedStates += numTruncatedStates;
0205       // store number of states which got not taken into account due to truncation
0206       numLostStates += numUntruncatedStates - numTruncatedStates;
0207     }
0208   }
0209
0210   // adds a layer to states
0211   void KalmanFilter::addLayer(deque<State*>& stream) {
0212     // Latency of KF Associator block firmware
0213     static constexpr int latency = 5;
0214     // dynamic state container for clock accurate emulation
0215     deque<State*> streamOutput;
0216     // Memory stack used to handle combinatorics
0217     deque<State*> stack;
0218     // static delay container
0219     vector<State*> delay(latency, nullptr);
0220     // each trip corresponds to a f/w clock tick
0221     // done if no states to process left, taking as much time as needed
0222     while (!stream.empty() || !stack.empty() ||
0223            !all_of(delay.begin(), delay.end(), [](const State* state) { return state == nullptr; })) {
0224       State* state = pop_front(stream);
0225       // Process a combinatoric state if no (non-combinatoric?) state available
0226       if (!state)
0227         state = pop_front(stack);
0228       streamOutput.push_back(state);
0229       // The remainder of the code in this loop deals with combinatoric states.
0230       if (state)
0231         // Assign next combinatoric stub to state
0232         comb(state);
0233       delay.push_back(state);
0234       state = pop_front(delay);
0235       if (state)
0236         stack.push_back(state);
0237     }
0238     stream = streamOutput;
0239     // Update state with next stub using KF maths
0240     for (State*& state : stream)
0241       if (state && state->stub() && state->layer() == layer_)
0242         update(state);
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     bool valid = state->stub() && state->layer() == layer_;
0253     if (valid) {
0254       // Get next unused stub on this layer
0255       const int pos = distance(stubs.begin(), find(stubs.begin(), stubs.end(), stub)) + 1;
0256       if (pos != (int)stubs.size())
0257         stubNext = stubs[pos];
0258       // picks next stub on different layer, nullifies state if skipping layer is not valid
0259       else {
0260         // having already maximum number of added layers
0261         if (hitPattern.count() == setup_->kfMaxLayers())
0262           valid = false;
0263         // Impossible for this state to ever get enough layers to form valid track
0264         if (hitPattern.count() + track->hitPattern().count(stub->layer() + 1, setup_->numLayers()) <
0265             setup_->kfMinLayers())
0266           valid = false;
0267         // not diffrent layers left
0268         if (layer_ == setup_->numLayers() - 1)
0269           valid = false;
0270         if (valid) {
0271           // pick next stub on next populated layer
0272           for (int nextLayer = layer_ + 1; nextLayer < setup_->numLayers(); nextLayer++) {
0273             if (track->hitPattern(nextLayer)) {
0274               stubNext = track->layerStub(nextLayer);
0275               break;
0276             }
0277           }
0278         }
0279       }
0280     }
0281     if (valid) {
0282       // create combinatoric state
0283       states_.emplace_back(state, stubNext);
0284       state = &states_.back();
0285     } else
0286       state = nullptr;
0287   }
0288
0289   // best state selection
0290   void KalmanFilter::accumulator(deque<State*>& stream) {
0291     // accumulator delivers contigious stream of best state per track
0292     // remove gaps and not final states
0293     stream.erase(
0294         remove_if(stream.begin(),
0295                   stream.end(),
0296                   [this](State* state) { return !state || state->hitPattern().count() < setup_->kfMinLayers(); }),
0297         stream.end());
0298     // Determine quality of completed state
0299     for (State* state : stream)
0300       state->finish();
0301     // sort in number of skipped layers
0302     auto lessSkippedLayers = [](State* lhs, State* rhs) { return lhs->numSkippedLayers() < rhs->numSkippedLayers(); };
0303     stable_sort(stream.begin(), stream.end(), lessSkippedLayers);
0304     // sort in number of consistent stubs
0305     auto moreConsistentLayers = [](State* lhs, State* rhs) {
0306       return lhs->numConsistentLayers() > rhs->numConsistentLayers();
0307     };
0308     stable_sort(stream.begin(), stream.end(), moreConsistentLayers);
0309     // sort in track id
0310     stable_sort(stream.begin(), stream.end(), [](State* lhs, State* rhs) { return lhs->trackId() < rhs->trackId(); });
0311     // keep first state (best due to previous sorts) per track id
0312     stream.erase(
0313         unique(stream.begin(), stream.end(), [](State* lhs, State* rhs) { return lhs->track() == rhs->track(); }),
0314         stream.end());
0315   }
0316
0317   // updates state
0318   void KalmanFilter::update(State*& state) {
0319     // 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.
0320     // stub phi residual wrt input helix
0321     const double m0 = m0_->digi(state->m0());
0322     // stub z residual wrt input helix
0323     const double m1 = m1_->digi(state->m1());
0324     // stub projected phi uncertainty squared);
0325     const double v0 = v0_->digi(state->v0());
0326     // stub projected z uncertainty squared
0327     const double v1 = v1_->digi(state->v1());
0328     // helix inv2R wrt input helix
0329     double x0 = x0_->digi(state->x0());
0330     // helix phi at radius ChosenRofPhi wrt input helix
0331     double x1 = x1_->digi(state->x1());
0332     // helix cot(Theta) wrt input helix
0333     double x2 = x2_->digi(state->x2());
0334     // helix z at radius chosenRofZ wrt input helix
0335     double x3 = x3_->digi(state->x3());
0336     // Derivative of predicted stub coords wrt helix params: stub radius minus chosenRofPhi
0337     const double H00 = H00_->digi(state->H00());
0338     // Derivative of predicted stub coords wrt helix params: stub radius minus chosenRofZ
0339     const double H12 = H12_->digi(state->H12());
0340     // cov. matrix
0341     double C00 = C00_->digi(state->C00());
0342     double C01 = C01_->digi(state->C01());
0343     double C11 = C11_->digi(state->C11());
0344     double C22 = C22_->digi(state->C22());
0345     double C23 = C23_->digi(state->C23());
0346     double C33 = C33_->digi(state->C33());
0347     // stub phi residual wrt current state
0348     const double r0C = x1_->digi(m0 - x1);
0349     const double r0 = r0_->digi(r0C - x0 * H00);
0350     // stub z residual wrt current state
0351     const double r1C = x3_->digi(m1 - x3);
0352     const double r1 = r1_->digi(r1C - x2 * H12);
0353     // matrix S = H*C
0354     const double S00 = S00_->digi(C01 + H00 * C00);
0355     const double S01 = S01_->digi(C11 + H00 * C01);
0356     const double S12 = S12_->digi(C23 + H12 * C22);
0357     const double S13 = S13_->digi(C33 + H12 * C23);
0358     // Cov. matrix of predicted residuals R = V+HCHt = C+H*St
0359     const double R00C = S01_->digi(v0 + S01);
0360     const double R00 = R00_->digi(R00C + H00 * S00);
0361     const double R11C = S13_->digi(v1 + S13);
0362     const double R11 = R11_->digi(R11C + H12 * S12);
0363     // imrpoved dynamic cancelling
0364     const int msb0 = max(0, (int)ceil(log2(R00 / R00_->base())));
0365     const int msb1 = max(0, (int)ceil(log2(R11 / R11_->base())));
0366     const double R00Rough = R00Rough_->digi(R00 * pow(2., 16 - msb0));
0367     const double invR00Approx = invR00Approx_->digi(1. / R00Rough);
0368     const double invR00Cor = invR00Cor_->digi(2. - invR00Approx * R00Rough);
0369     const double invR00 = invR00_->digi(invR00Approx * invR00Cor * pow(2., 16 - msb0));
0370     const double R11Rough = R11Rough_->digi(R11 * pow(2., 16 - msb1));
0371     const double invR11Approx = invR11Approx_->digi(1. / R11Rough);
0372     const double invR11Cor = invR11Cor_->digi(2. - invR11Approx * R11Rough);
0373     const double invR11 = invR11_->digi(invR11Approx * invR11Cor * pow(2., 16 - msb1));
0374     // Kalman gain matrix K = S*R(inv)
0375     const double K00 = K00_->digi(S00 * invR00);
0376     const double K10 = K10_->digi(S01 * invR00);
0377     const double K21 = K21_->digi(S12 * invR11);
0378     const double K31 = K31_->digi(S13 * invR11);
0379     // Updated helix params & their cov. matrix
0380     x0 = x0_->digi(x0 + r0 * K00);
0381     x1 = x1_->digi(x1 + r0 * K10);
0382     x2 = x2_->digi(x2 + r1 * K21);
0383     x3 = x3_->digi(x3 + r1 * K31);
0384     C00 = C00_->digi(C00 - S00 * K00);
0385     C01 = C01_->digi(C01 - S01 * K00);
0386     C11 = C11_->digi(C11 - S01 * K10);
0387     C22 = C22_->digi(C22 - S12 * K21);
0388     C23 = C23_->digi(C23 - S13 * K21);
0389     C33 = C33_->digi(C33 - S13 * K31);
0390     // create updated state
0391     states_.emplace_back(State(state, (initializer_list<double>){x0, x1, x2, x3, C00, C11, C22, C33, C01, C23}));
0392     state = &states_.back();
0393     // update variable ranges to tune variable granularity
0394     m0_->updateRangeActual(m0);
0395     m1_->updateRangeActual(m1);
0396     v0_->updateRangeActual(v0);
0397     v1_->updateRangeActual(v1);
0398     H00_->updateRangeActual(H00);
0399     H12_->updateRangeActual(H12);
0400     r0_->updateRangeActual(r0);
0401     r1_->updateRangeActual(r1);
0402     S00_->updateRangeActual(S00);
0403     S01_->updateRangeActual(S01);
0404     S12_->updateRangeActual(S12);
0405     S13_->updateRangeActual(S13);
0406     R00_->updateRangeActual(R00);
0407     R11_->updateRangeActual(R11);
0408     R00Rough_->updateRangeActual(R00Rough);
0409     invR00Approx_->updateRangeActual(invR00Approx);
0410     invR00Cor_->updateRangeActual(invR00Cor);
0411     invR00_->updateRangeActual(invR00);
0412     R11Rough_->updateRangeActual(R11Rough);
0413     invR11Approx_->updateRangeActual(invR11Approx);
0414     invR11Cor_->updateRangeActual(invR11Cor);
0415     invR11_->updateRangeActual(invR11);
0416     K00_->updateRangeActual(K00);
0417     K10_->updateRangeActual(K10);
0418     K21_->updateRangeActual(K21);
0419     K31_->updateRangeActual(K31);
0420     x0_->updateRangeActual(x0);
0421     x1_->updateRangeActual(x1);
0422     x2_->updateRangeActual(x2);
0423     x3_->updateRangeActual(x3);
0424     C00_->updateRangeActual(C00);
0425     C01_->updateRangeActual(C01);
0426     C11_->updateRangeActual(C11);
0427     C22_->updateRangeActual(C22);
0428     C23_->updateRangeActual(C23);
0429     C33_->updateRangeActual(C33);
0430   }
0431
0432   // remove and return first element of deque, returns nullptr if empty
0433   template <class T>
0434   T* KalmanFilter::pop_front(deque<T*>& ts) const {
0435     T* t = nullptr;
0436     if (!ts.empty()) {
0437       t = ts.front();
0438       ts.pop_front();
0439     }
0440     return t;
0441   }
0442
0443   // remove and return first element of vector, returns nullptr if empty
0444   template <class T>
0445   T* KalmanFilter::pop_front(vector<T*>& ts) const {
0446     T* t = nullptr;
0447     if (!ts.empty()) {
0448       t = ts.front();
0449       ts.erase(ts.begin());
0450     }
0451     return t;
0452   }
0453
0454 }  // namespace trackerTFP