KalmanUpdators/src/KFUpdator.cc

0001 #include "TrackingTools/KalmanUpdators/interface/KFUpdator.h"
0002 #include "TrackingTools/PatternTools/interface/MeasurementExtractor.h"
0003 #include "TrackingTools/TransientTrackingRecHit/interface/TransientTrackingRecHit.h"
0004 #include "DataFormats/GeometrySurface/interface/Plane.h"
0005 #include "DataFormats/TrackingRecHit/interface/KfComponentsHolder.h"
0006 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0007 #include "DataFormats/Math/interface/invertPosDefMatrix.h"
0008 #include "DataFormats/Math/interface/ProjectMatrix.h"
0009
0010 // test of joseph form
0011 #ifdef KU_JF_TEST
0012
0013 #include <atomic>
0014 #include <iostream>
0015 namespace {
0016   struct Stat {
0017     Stat() : tot(0), nopd(0), jnopd(0), fnopd(0), inopd(0), ijnopd(0), ifnopd(0), dtot(0) {
0018       for (int i = 0; i < 5; ++i)
0019         idmm[i] = 0;
0020     }
0021
0022     std::atomic<long long> tot;
0023     std::atomic<long long> nopd;
0024     std::atomic<long long> jnopd;
0025     std::atomic<long long> fnopd;
0026     std::atomic<long long> inopd;
0027     std::atomic<long long> ijnopd;
0028     std::atomic<long long> ifnopd;
0029
0030     std::atomic<unsigned long long> dtot;
0031     std::atomic<unsigned long long> idmm[5];
0032     double mm = 0;
0033     double dmm[5] = {0};
0034     bool ok = true;
0035     ~Stat() {
0036       double n = 1.e-3 / double(dtot);
0037       std::cout << "KF " << tot * 1.e-9 << "G " << mm << " " << dmm[0] << '/' << dmm[1] << '/' << dmm[2] << '/'
0038                 << dmm[3] << '/' << dmm[4] << '\n'
0039                 << dtot << ' ' << idmm[0] << '/' << idmm[1] << '/' << idmm[2] << '/' << idmm[3] << '/' << idmm[4]
0040                 << '\n'
0041                 << std::sqrt(idmm[0] * n) << '/' << std::sqrt(idmm[1] * n) << '/' << std::sqrt(idmm[2] * n) << '/'
0042                 << std::sqrt(idmm[3] * n) << '/' << std::sqrt(idmm[4] * n) << " " << nopd << " " << jnopd << " "
0043                 << fnopd << " " << inopd << " " << ijnopd << " " << ifnopd << std::endl;
0044     }
0045   };
0046
0047   Stat stat;
0048
0049   bool isNopd(AlgebraicSymMatrix55 const& m) {
0050     return m(0, 0) < 0 || m(1, 1) < 0 || m(2, 2) < 0 || m(3, 3) < 0 || m(4, 4) < 0;
0051   }
0052 }  // namespace
0053 #endif
0054
0055 namespace {
0056
0057   template <unsigned int D>
0058   TrajectoryStateOnSurface lupdate(const TrajectoryStateOnSurface& tsos, const TrackingRecHit& aRecHit) {
0059     typedef typename AlgebraicROOTObject<D, 5>::Matrix MatD5;
0060     typedef typename AlgebraicROOTObject<5, D>::Matrix Mat5D;
0061     typedef typename AlgebraicROOTObject<D, D>::SymMatrix SMatDD;
0062     typedef typename AlgebraicROOTObject<D>::Vector VecD;
0063     using ROOT::Math::SMatrixNoInit;
0064     double pzSign = tsos.localParameters().pzSign();
0065
0066     auto&& x = tsos.localParameters().vector();
0067     auto&& C = tsos.localError().matrix();
0068
0069     // projection matrix (assume element of "H" to be just 0 or 1)
0070     ProjectMatrix<double, 5, D> pf;
0071
0072     // Measurement matrix
0073     VecD r, rMeas;
0074     SMatDD V(SMatrixNoInit{}), VMeas(SMatrixNoInit{});
0075
0076     KfComponentsHolder holder;
0077     holder.template setup<D>(&r, &V, &pf, &rMeas, &VMeas, x, C);
0078     aRecHit.getKfComponents(holder);
0079
0080     r -= rMeas;
0081
0082     // and covariance matrix of residuals
0083     SMatDD R = V + VMeas;
0084     bool ok = invertPosDefMatrix(R);
0085
0086     // Compute Kalman gain matrix
0087     AlgebraicMatrix55 M = AlgebraicMatrixID();
0088     Mat5D K = C * pf.project(R);
0089     pf.projectAndSubtractFrom(M, K);
0090
0091     // Compute local filtered state vector
0092     AlgebraicVector5 fsv = x + K * r;
0093
0094     // Compute covariance matrix of local filtered state vector
0095 #define KU_JosephForm
0096 #ifdef KU_JosephForm
0097     AlgebraicSymMatrix55 fse = ROOT::Math::Similarity(M, C) + ROOT::Math::Similarity(K, V);
0098 #else
0099     AlgebraicSymMatrix55 fse;
0100     ROOT::Math::AssignSym::Evaluate(fse, M * C);
0101 #endif
0102
0103 // test of joseph form
0104 #ifdef KU_JF_TEST
0105
0106     AlgebraicSymMatrix55 fse2;
0107     ROOT::Math::AssignSym::Evaluate(fse2, M * C);
0108
0109     // std::cout << "Joseph Form \n" << fse << std::endl;
0110     // std::cout << "Fast Form \n"  << fse2 << std::endl;
0111
0112     stat.tot++;
0113     auto n1 = isNopd(fse);
0114     auto n2 = isNopd(fse2);
0115     if (n1 && n2)
0116       stat.nopd++;
0117     if (n1)
0118       stat.jnopd++;
0119     if (n2)
0120       stat.fnopd++;
0121
0122     AlgebraicSymMatrix55 dd = fse2 - fse;
0123     auto dda = dd.Array();
0124     auto fsa = fse.Array();
0125     double ddd[15];
0126     for (int i = 0; i < 15; ++i)
0127       ddd[i] = std::abs(dda[i]) / std::abs(fsa[i]);
0128     auto mm = *std::max_element(ddd, ddd + 15);
0129     stat.mm = std::max(stat.mm, mm);
0130     if (stat.ok && !(n1 || n2)) {
0131       stat.dtot++;
0132       for (int i = 0; i < 5; ++i) {
0133         auto dmm = std::sqrt(fse2(i, i)) - std::sqrt(fse(i, i));
0134         stat.dmm[i] = std::max(stat.dmm[i], std::abs(dmm));
0135         stat.idmm[i] += (unsigned long long)(1000. * dmm * dmm);
0136         if (stat.idmm[i] > std::numeric_limits<long long>::max())
0137           stat.ok = false;
0138       }
0139     }
0140
0141     AlgebraicSymMatrix55 ifse = fse;
0142     invertPosDefMatrix(ifse);
0143     AlgebraicSymMatrix55 ifse2 = fse2;
0144     invertPosDefMatrix(ifse2);
0145     n1 = isNopd(ifse);
0146     n2 = isNopd(ifse2);
0147     if (n1 && n2)
0148       stat.inopd++;
0149     if (n1)
0150       stat.ijnopd++;
0151     if (n2)
0152       stat.ifnopd++;
0153 #endif
0154
0155     /*
0156   // expanded similariy
0157   AlgebraicSymMatrix55 fse;
0158   ROOT::Math::AssignSym::Evaluate(fse, (M* C) * ( ROOT::Math::Transpose(M)));
0159   AlgebraicSymMatrix55 tmp;
0160   ROOT::Math::AssignSym::Evaluate(tmp, (K*V) * (ROOT::Math::Transpose(K)));
0161   fse +=  tmp;
0162   */
0163
0164     if (ok) {
0165       return TrajectoryStateOnSurface(LocalTrajectoryParameters(fsv, pzSign),
0166                                       LocalTrajectoryError(fse),
0167                                       tsos.surface(),
0168                                       &(tsos.globalParameters().magneticField()),
0169                                       tsos.surfaceSide());
0170     } else {
0171       edm::LogError("KFUpdator") << " could not invert martix:\n" << (V + VMeas);
0172       return TrajectoryStateOnSurface();
0173     }
0174   }
0175 }  // namespace
0176
0177 TrajectoryStateOnSurface KFUpdator::update(const TrajectoryStateOnSurface& tsos, const TrackingRecHit& aRecHit) const {
0178   switch (aRecHit.dimension()) {
0179     case 1:
0180       return lupdate<1>(tsos, aRecHit);
0181     case 2:
0182       return lupdate<2>(tsos, aRecHit);
0183     case 3:
0184       return lupdate<3>(tsos, aRecHit);
0185     case 4:
0186       return lupdate<4>(tsos, aRecHit);
0187     case 5:
0188       return lupdate<5>(tsos, aRecHit);
0189   }
0190   throw cms::Exception("Rec hit of invalid dimension (not 1,2,3,4,5)")
0191       << "The value was " << aRecHit.dimension() << ", type is " << typeid(aRecHit).name() << "\n";
0192 }