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.10 2010/09/11 21:49:24 kasahara 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& , const TrackContext& ) :
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
00104
00105 #if ROOT_VERSION_CODE >= ROOT_VERSION(4,0,0)
00106
00107
00108
00109
00110
00111
00112
00113
00114 TMatrixD W(TMatrixD::kInverted, fV);
00115 #else
00116
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
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
00226 TVectorD theta_i(nplanes);
00227 for (Int_t i = 0; i<nplanes; i++) {
00228 theta_i(i) = data.ThetaMCS(i);
00229 }
00230
00231
00232 Double_t diag, non_diag;
00233 Int_t index1, index2;
00234
00235 index1 = 0;
00236 for (Int_t n = 0; n<nplanes; n++) {
00237 if ( data.UHitUse(n) ) {
00238
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
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
00262 index1 = 0;
00263 for (Int_t n = 0; n<nplanes; n++) {
00264 if ( data.VHitUse(n) ) {
00265
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
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
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
00305
00306
00307
00308
00309
00310
00311
00312
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
00337 double ratiodzsteelcos = data.GetdZSteel(i)/data.GetCos(i);
00338 double dataT = data.T(i,n);
00339 double thetaI = theta_i(i);
00340 return thetaI*thetaI *
00341 ( ratiodzsteelcos*ratiodzsteelcos/3.+
00342 ratiodzsteelcos*dataT +
00343 dataT*dataT +
00344 TMath::Abs(data.GetZ(k)-data.GetZ(n))*
00345 (ratiodzsteelcos/2.+dataT) );
00346 }