core/src/MeasuredStateOnPlane.cc

0001 /* Copyright 2008-2010, Technische Universitaet Muenchen,
0002    Authors: Christian Hoeppner & Sebastian Neubert & Johannes Rauch
0003
0004    This file is part of GENFIT.
0005
0006    GENFIT is free software: you can redistribute it and/or modify
0007    it under the terms of the GNU Lesser General Public License as published
0008    by the Free Software Foundation, either version 3 of the License, or
0009    (at your option) any later version.
0010
0011    GENFIT is distributed in the hope that it will be useful,
0012    but WITHOUT ANY WARRANTY; without even the implied warranty of
0013    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0014    GNU Lesser General Public License for more details.
0015
0016    You should have received a copy of the GNU Lesser General Public License
0017    along with GENFIT.  If not, see <http://www.gnu.org/licenses/>.
0018 */
0019
0020 #include "MeasuredStateOnPlane.h"
0021 #include "AbsTrackRep.h"
0022 #include "Exception.h"
0023 #include "Tools.h"
0024 #include "IO.h"
0025
0026 #include <cassert>
0027
0028 #include "TDecompChol.h"
0029
0030 namespace genfit {
0031
0032 void MeasuredStateOnPlane::Print(Option_t*) const {
0033   printOut << "genfit::MeasuredStateOnPlane ";
0034   printOut << "my address " << this << " my plane's address " << this->sharedPlane_.get() << "; use count: " << sharedPlane_.use_count() << std::endl;
0035   printOut << " state vector: "; state_.Print();
0036   printOut << " covariance matrix: "; cov_.Print();
0037   if (sharedPlane_ != nullptr) {
0038     printOut << " defined in plane "; sharedPlane_->Print();
0039     TVector3 pos, mom;
0040     TMatrixDSym cov(6,6);
0041     getRep()->getPosMomCov(*this, pos, mom, cov);
0042     printOut << " 3D position: "; pos.Print();
0043     printOut << " 3D momentum: "; mom.Print();
0044     //printOut << " 6D covariance: "; cov.Print();
0045   }
0046 }
0047
0048 void MeasuredStateOnPlane::blowUpCov(double blowUpFac, bool resetOffDiagonals, double maxVal) {
0049
0050   unsigned int dim = cov_.GetNcols();
0051
0052   if (resetOffDiagonals) {
0053     for (unsigned int i=0; i<dim; ++i) {
0054       for (unsigned int j=0; j<dim; ++j) {
0055         if (i != j)
0056           cov_(i,j) = 0; // reset off-diagonals
0057         else
0058           cov_(i,j) *= blowUpFac; // blow up diagonals
0059       }
0060     }
0061   }
0062   else
0063     cov_ *= blowUpFac;
0064
0065   // limit
0066   if (maxVal > 0.)
0067     for (unsigned int i=0; i<dim; ++i) {
0068       for (unsigned int j=0; j<dim; ++j) {
0069         cov_(i,j) = std::min(cov_(i,j), maxVal);
0070       }
0071     }
0072
0073 }
0074
0075
0076 MeasuredStateOnPlane calcAverageState(const MeasuredStateOnPlane& forwardState, const MeasuredStateOnPlane& backwardState) {
0077   // check if both states are defined in the same plane
0078   if (forwardState.getPlane() != backwardState.getPlane()) {
0079     Exception e("KalmanFitterInfo::calcAverageState: forwardState and backwardState are not defined in the same plane.", __LINE__,__FILE__);
0080     throw e;
0081   }
0082
0083   // This code is called a lot, so some effort has gone into reducing
0084   // the number of temporary objects being constructed.
0085
0086 #if 0
0087   // For ease of understanding, here's a very explicit implementation
0088   // that uses the textbook algorithm:
0089   TMatrixDSym fCovInv, bCovInv, smoothed_cov;
0090   tools::invertMatrix(forwardState.getCov(), fCovInv);
0091   tools::invertMatrix(backwardState.getCov(), bCovInv);
0092
0093   tools::invertMatrix(fCovInv + bCovInv, smoothed_cov);  // one temporary TMatrixDSym
0094
0095   MeasuredStateOnPlane retVal(forwardState);
0096   retVal.setState(smoothed_cov*(fCovInv*forwardState.getState() + bCovInv*backwardState.getState())); // four temporary TVectorD's
0097   retVal.setCov(smoothed_cov);
0098   return retVal;
0099 #endif
0100
0101   // This is a numerically stable implementation of the averaging
0102   // process.  We write S1, S2 for the upper diagonal square roots
0103   // (Cholesky decompositions) of the covariance matrices, such that
0104   // C1 = S1' S1 (transposition indicated by ').
0105   //
0106   // Then we can write
0107   //  (C1^-1 + C2^-1)^-1 = (S1inv' S1inv + S2inv' S2inv)^-1
0108   //                     = ( (S1inv', S2inv') . ( S1inv ) )^-1
0109   //                       (                    ( S2inv ) )
0110   //                     = ( (R', 0) . Q . Q' . ( R ) )^-1
0111   //                       (                    ( 0 ) )
0112   // where Q is an orthogonal matrix chosen such that R is upper diagonal.
0113   // Since Q'.Q = 1, this reduces to
0114   //                     = ( R'.R )^-1
0115   //                     = R^-1 . (R')^-1.
0116   // This gives the covariance matrix of the average and its Cholesky
0117   // decomposition.
0118   //
0119   // In order to get the averaged state (writing x1 and x2 for the
0120   // states) we proceed from
0121   //  C1^-1.x1 + C2^-1.x2 = (S1inv', S2inv') . ( S1inv.x1 )
0122   //                                           ( S2inv.x2 )
0123   // which by the above can be written as
0124   //                      =  (R', 0) . Q . ( S1inv.x1 )
0125   //                                       ( S2inv.x2 )
0126   // with the same (R, Q) as above.
0127   //
0128   // The average is then after obvious simplification
0129   //   average = R^-1 . Q . (S1inv.x1)
0130   //                        (S2inv.x2)
0131   //
0132   // This is what's implemented below, where we make use of the
0133   // tridiagonal shapes of the various matrices when multiplying or
0134   // inverting.
0135   //
0136   // This turns out not only more numerically stable, but because the
0137   // matrix operations are simpler, it is also faster than the
0138   // straightoforward implementation.
0139   //
0140   // This is an application of the technique of Golub, G.,
0141   // Num. Math. 7, 206 (1965) to the least-squares problem underlying
0142   // averaging.
0143   TDecompChol d1(forwardState.getCov());
0144   bool success = d1.Decompose();
0145   TDecompChol d2(backwardState.getCov());
0146   success &= d2.Decompose();
0147
0148   if (!success) {
0149     Exception e("KalmanFitterInfo::calcAverageState: ill-conditioned covariance matrix.", __LINE__,__FILE__);
0150     throw e;
0151   }
0152
0153   int nRows = d1.GetU().GetNrows();
0154   assert(nRows == d2.GetU().GetNrows());
0155   TMatrixD S1inv, S2inv;
0156   tools::transposedInvert(d1.GetU(), S1inv);
0157   tools::transposedInvert(d2.GetU(), S2inv);
0158
0159   TMatrixD A(2*nRows, nRows);
0160   TVectorD b(2 * nRows);
0161   double *const bk = b.GetMatrixArray();
0162   double *const Ak = A.GetMatrixArray();
0163   const double* S1invk = S1inv.GetMatrixArray();
0164   const double* S2invk = S2inv.GetMatrixArray();
0165   // S1inv and S2inv are lower triangular.
0166   for (int i = 0; i < nRows; ++i) {
0167     double sum1 = 0;
0168     double sum2 = 0;
0169     for (int j = 0; j <= i; ++j) {
0170       Ak[i*nRows + j] = S1invk[i*nRows + j];
0171       Ak[(i + nRows)*nRows + j] = S2invk[i*nRows + j];
0172       sum1 += S1invk[i*nRows + j]*forwardState.getState().GetMatrixArray()[j];
0173       sum2 += S2invk[i*nRows + j]*backwardState.getState().GetMatrixArray()[j];
0174     }
0175     bk[i] = sum1;
0176     bk[i + nRows] = sum2;
0177   }
0178
0179   tools::QR(A, b);
0180   A.ResizeTo(nRows, nRows);
0181
0182   TMatrixD inv;
0183   tools::transposedInvert(A, inv);
0184   b.ResizeTo(nRows);
0185   for (int i = 0; i < nRows; ++i) {
0186     double sum = 0;
0187     for (int j = i; j < nRows; ++j) {
0188       sum += inv.GetMatrixArray()[j*nRows+i] * b[j];
0189     }
0190     b.GetMatrixArray()[i] = sum;
0191   }
0192   return MeasuredStateOnPlane(b,
0193                   TMatrixDSym(TMatrixDSym::kAtA, inv),
0194                   forwardState.getPlane(),
0195                   forwardState.getRep(),
0196                   forwardState.getAuxInfo());
0197 }
0198
0199
0200 } /* End of namespace genfit */