loon: MatrixCalculator.cxx Source File

00001 //_____________________________________________________________________________
00010 
00011 #include "TMatrixD.h"
00012 #include "TDecompChol.h"
00013 
00014 #include "MessageService/MsgService.h"
00015 #include "MessageService/MsgFormat.h"
00016 
00017 #include "CandFitTrackSA/ConstFT.h"
00018 #include "CandFitTrackSA/DataFT.h"
00019 #include "CandFitTrackSA/FitResult.h"
00020 #include "CandFitTrackSA/MatrixCalculator.h"
00021 
00022 #include <cmath>
00023 
00024 using namespace ConstFT;
00025 
00026 CVSID("$Id: MatrixCalculator.cxx,v 1.9 2009/02/11 20:49:34 asousa Exp $");
00027 
00031 MatrixCalculator::MatrixCalculator() : 
00032         fFitCovM(NTrackParams, NTrackParams), 
00033         fFitErrM(NTrackParams, NTrackParams),
00034         fTrackIn(NTrackParams), fTrackOut(NTrackParams)
00035 {}
00036 
00037 
00041 MatrixCalculator::MatrixCalculator(const AlgConfig& /*ac*/, const TrackContext& /*tc*/) :
00042         fFitCovM(NTrackParams, NTrackParams), 
00043         fFitErrM(NTrackParams, NTrackParams),
00044         fTrackIn(NTrackParams), fTrackOut(NTrackParams)
00045 {}
00046 
00047 
00051 MatrixCalculator::~MatrixCalculator()
00052 {}
00053 
00057 FitResult MatrixCalculator::GetFitResult() const
00058 {
00059     return FitResult(   fFitErrM, fTrackOut, fChi2, 
00060                         fDChi2, fNPlanesUsed, fV.GetNcols() );
00061 }
00062 
00063 
00091 Int_t MatrixCalculator::Solve(const DataFT& data) 
00092 {
00093     fTrackIn = data.GetTrack();
00094     
00095     fNPlanesUsed = data.GetNPlanesUsed();
00096         
00097     MakePropagatorMatrix(data);
00098     
00099     TMatrixD At(TMatrixD::kTransposed, fA);
00100 
00101     MakeCovarianceMatrix(data);
00102 
00103     //fW.ResizeTo(fV.GetNrows(), fV.GetNcols());
00104 
00105 #if ROOT_VERSION_CODE >= ROOT_VERSION(4,0,0)
00106     // using Cholesky decomposition here - works on symmetric
00107     // matrices TMatrixDSym only and should be faster than
00108     // regular inversion
00109     // TDecompChol chol(fV); 
00110     // chol.Invert(fW);
00111     
00112     // fW = TMatrixD(TMatrixD::kInverted, fV);
00113     
00114     TMatrixD W(TMatrixD::kInverted, fV);
00115 #else
00116     // fW = TMatrixD(TMatrixD::kInvertedPosDef, fV);
00117     
00118     TMatrixD W(TMatrixD::kInverted, fV);
00119 #endif
00120         
00121     fFitCovM = TMatrixD( TMatrixD(At,TMatrixD::kMult,W), TMatrixD::kMult, fA);
00122 
00123 #if ROOT_VERSION_CODE >= ROOT_VERSION(4,0,0)
00124     fFitErrM = TMatrixD(TMatrixD::kInverted, fFitCovM);
00125 #else    
00126     fFitErrM = TMatrixD(TMatrixD::kInvertedPosDef, fFitCovM);
00127 #endif
00128 
00129     MsgStream *mftsa = &MSGSTREAM("FitTrackSA", Msg::kVerbose);
00130     (*mftsa) << "Solution Error Matrix:\n";
00131     MsgFormat efmt("%10.2e");
00132     for (Int_t i = 0; i<fFitErrM.GetNrows(); i++) {
00133         for (Int_t j = 0; j<fFitErrM.GetNcols(); j++) {
00134             (*mftsa) << efmt(fFitErrM(i,j));
00135         }
00136         (*mftsa) << "\n";
00137     }
00138 
00139     data.FillVectorC(fC);
00140 
00141     TMatrixD solution( fFitErrM, TMatrixD::kMult, 
00142         TMatrixD(At, TMatrixD::kMult, TMatrixD(W,TMatrixD::kMult,fC)) );
00143 
00144     fTrackOut = TMatrixDColumn(solution, 0);
00145 
00146     TMatrixD  vRes(1,1);
00147     data.FillVectorRes(vRes);
00148     TMatrixD chi2(TMatrixD(TMatrixD::kTransposed,vRes), 
00149                     TMatrixD::kMult, TMatrixD(W, TMatrixD::kMult, vRes));
00150     fChi2 = chi2(0,0);
00151 
00152     TVectorD vres(fTrackOut);
00153     for (Int_t i = 0; i<NTrackParams; i++) {
00154         vres(i) -= fTrackIn(i);
00155     }
00156     TVectorD vresT(vres);
00157     vresT *= fFitCovM;
00158     
00159     fDChi2 = vresT*vres;
00160     
00161     return 0;
00162 }
00163 
00167 Int_t MatrixCalculator::MakePropagatorMatrix(const DataFT& data) 
00168 {
00169     Int_t nuhits, nvhits;
00170     nuhits = data.GetNUHitsUsed();
00171     nvhits = data.GetNVHitsUsed();
00172 
00173     fA.ResizeTo(nuhits+nvhits, NTrackParams);
00174 
00175     Int_t nplanes = data.GetNPlanesUsed();
00176     Int_t ihits_u = 0;
00177     //Protect against rare arithmetic exception
00178     Double_t epsilon = 0.00001;
00179     if (TMath::Abs(fTrackIn(kQoverP))<1e-6) 
00180       fTrackIn(kQoverP) = epsilon;
00181 
00182     for (Int_t i=0; i<nplanes; i++) {
00183         if ( data.UHitUse(i) ) {
00184             fA(ihits_u,kU) = 1.;
00185             fA(ihits_u,kdUdZ) = data.GetZ(i) - data.GetZ(0);
00186             fA(ihits_u,kV) = 0.;
00187             fA(ihits_u,kdVdZ) = 0.;
00188             fA(ihits_u,kQoverP) = (data.GetUf(i) - fTrackIn(kU) -
00189                fTrackIn(kdUdZ)*(data.GetZ(i)-data.GetZ(0)))/fTrackIn(kQoverP);
00190             ihits_u++;
00191         }
00192     }
00193 
00194     Int_t ihits_v = 0;
00195     for (Int_t i = 0; i<nplanes; i++) {
00196         if (data.VHitUse(i) ) {
00197             fA(ihits_v+nuhits,kV) = 1.;
00198             fA(ihits_v+nuhits,kdVdZ) = data.GetZ(i) - data.GetZ(0);
00199             fA(ihits_v+nuhits,kU) = 0.;
00200             fA(ihits_v+nuhits,kdUdZ) = 0.;
00201             fA(ihits_v+nuhits,kQoverP) = (data.GetVf(i) - fTrackIn(kV) -
00202                fTrackIn(kdVdZ)*(data.GetZ(i)-data.GetZ(0)))/fTrackIn(kQoverP);
00203             ihits_v++;
00204         }
00205     }
00206     
00207     return 0;
00208 }
00209 
00213 Int_t MatrixCalculator::MakeCovarianceMatrix(const DataFT& data) 
00214 {
00215     Int_t nplanes;
00216     nplanes = data.GetNPlanesUsed();
00217 
00218     Int_t nuhits, nvhits, nhits;
00219     nuhits = data.GetNUHitsUsed();
00220     nvhits = data.GetNVHitsUsed();
00221     nhits = nuhits+nvhits;
00222     fV.ResizeTo(nhits, nhits);
00223     fV.Zero();
00224 
00225     // Calculate MCS scattering angles for each plane
00226     TVectorD theta_i(nplanes);
00227     for (Int_t i = 0; i<nplanes; i++) {
00228         theta_i(i) = data.ThetaMCS(i);
00229     }
00230 
00231     // Calculate MCS covariance matrix
00232     Double_t diag, non_diag;
00233     Int_t index1, index2;
00234     // U part
00235     index1 = 0;
00236     for (Int_t n = 0; n<nplanes; n++) {
00237         if ( data.UHitUse(n) ) {
00238             // calculate diagonal elements
00239             diag = 0.0;
00240             for (Int_t i = 0; i<=n; i++) {
00241                 diag += DiagonalElement(i, n, theta_i, data);
00242             }
00243             fV(index1,index1) = diag;
00244 
00245             // non-diagonal elements
00246             index2 = index1+1;
00247             for (Int_t k = n+1; k<nplanes; k++) {
00248                 if ( data.UHitUse(k) ) {
00249                     non_diag = 0.0;
00250                     for (Int_t i = 0; i<=n; i++) {
00251                         non_diag += NonDiagonalElement(i, k, n, theta_i, data);
00252                     }
00253                     fV(index1,index2) = non_diag;
00254                     fV(index2,index1) = non_diag;
00255                     index2++;
00256                 }
00257             }
00258             index1++;
00259         }
00260     }
00261     // V part
00262     index1 = 0;
00263     for (Int_t n = 0; n<nplanes; n++) {
00264         if ( data.VHitUse(n) ) {
00265             // calculate diagonal elements
00266             diag = 0.0;
00267             for (Int_t i = 0; i<=n; i++) {
00268                 diag += DiagonalElement(i, n, theta_i, data);
00269             }
00270             fV(index1+nuhits,index1+nuhits) = diag;
00271 
00272             // non-diagonal elements
00273             index2 = index1+1;
00274             for (Int_t k = n+1; k<nplanes; k++) {
00275                 if ( data.VHitUse(k) ) {
00276                     non_diag = 0.0;
00277                     for (Int_t i = 0; i<=n; i++) {
00278                         non_diag += NonDiagonalElement(i, k, n, theta_i, data);
00279                     }
00280                     fV(index1+nuhits,index2+nuhits) = non_diag;
00281                     fV(index2+nuhits,index1+nuhits) = non_diag;
00282                     index2++;
00283                 }
00284             }
00285             index1++;
00286         }
00287     }
00288     
00289     // Add resolutions
00290     Int_t i_hit = 0;
00291     for (Int_t i = 0; i<nplanes; i++) {
00292         if ( data.UHitUse(i) ) {
00293             fV(i_hit,i_hit) += pow(data.GetSigmaU(i),2);
00294             i_hit++;
00295         }
00296     }
00297     for (Int_t i = 0; i<nplanes; i++) {
00298         if ( data.VHitUse(i) ) {
00299             fV(i_hit,i_hit) += pow(data.GetSigmaV(i),2);
00300             i_hit++;
00301         }
00302     }
00303 
00304 //     MsgStream *mftsa = &MSGSTREAM("FitTrackSA", Msg::kVerbose);
00305 //     (*mftsa) << "Covariance Matrix: \n";
00306 //     MsgFormat ffmt("%10.3e");
00307 // 
00308 //     for (Int_t i=0; i<nhits; i++) {
00309 //         for (Int_t j=0; j<nhits; j++) {
00310 //             (*mftsa) <<  ffmt(fV(i,j));
00311 //         }
00312 //         (*mftsa) << "\n";
00313 //     }
00314 
00315     return 0;
00316 }
00317 
00321 Double_t MatrixCalculator::DiagonalElement(Int_t i, Int_t n,
00322                             const TVectorD& theta_i, const DataFT& data) const
00323 {
00324     return  pow(theta_i(i),2) *
00325         ( pow(data.GetdZSteel(i)/data.GetCos(i),2)/3.+
00326           data.GetdZSteel(i)/data.GetCos(i)*data.T(i,n) +
00327           pow(data.T(i,n),2)                              );
00328 }
00329 
00333 Double_t MatrixCalculator::NonDiagonalElement(Int_t i, Int_t k, Int_t n,
00334                              const TVectorD& theta_i, const DataFT& data) const
00335 {
00336     return pow(theta_i(i),2) *
00337         (  pow(data.GetdZSteel(i)/data.GetCos(i),2)/3.+
00338            data.GetdZSteel(i)/data.GetCos(i)*data.T(i,n) +
00339            pow(data.T(i,n),2) +
00340            TMath::Abs(data.GetZ(k)-data.GetZ(n))*
00341            (data.GetdZSteel(i)/data.GetCos(i)/2.+data.T(i,n))  );
00342 }