// ------------------------------------------------------------------------- // ----- CbmLitKalmanImp source file ----- // ----- Created 14/08/06 by A. Lebedev ----- // ------------------------------------------------------------------------- #include "CbmGeoNode.h" #include "CbmRunAna.h" #include "CbmField.h" #include "CbmBaseParSet.h" #include "CbmGeoStsPar.h" #include "CbmGeoRichPar.h" #include "CbmGeoTrdPar.h" #include "CbmGeoPassivePar.h" #include "CbmTrackParam.h" #include "CbmLitKalmanImp.h" //constructors CbmLitKalmanImp::CbmLitKalmanImp() { // fExtrapolator = new CbmLitExtrapolator(); fFms = 1.; fThickWall = 5.; SetPid(211); fEnergyLoss = 0.03; } //Destructor CbmLitKalmanImp::~CbmLitKalmanImp() { // delete fExtrapolator; } void CbmLitKalmanImp::SetMethod(Int_t Method) { fExtrapolator->SetMethod(Method); } void CbmLitKalmanImp::SetParContainers() { CbmRunAna* ana = CbmRunAna::Instance(); CbmRuntimeDb* rtdb=ana->GetRuntimeDb(); rtdb->getContainer("CbmBaseParSet"); rtdb->getContainer("CbmGeoPassivePar"); rtdb->getContainer("CbmGeoStsPar"); rtdb->getContainer("CbmGeoRichPar"); rtdb->getContainer("CbmGeoTrdPar"); } InitStatus CbmLitKalmanImp::ReInit() { SetParContainers(); return kSUCCESS; } void CbmLitKalmanImp::ReadMaterials() { ReadTrdMaterials(); ReadStsMaterials(); ReadRichMaterials(); sort(fvAllMaterials.begin(), fvAllMaterials.end(), CbmLitMaterial::CmpUp); cout << "-I- CbmLitKalmanImp::ReadMaterials(): " << endl; for (UInt_t i = 0; i < fvAllMaterials.size(); i++) { cout << " Add material : [ " << " Z = " << fvAllMaterials[i].GetZ() << " Thickness = " << fvAllMaterials[i].GetThickness() << " RadLength = " << fvAllMaterials[i].GetRadLength() << " ]" << endl; } } void CbmLitKalmanImp::ReadTrdMaterials(TObjArray *Nodes) { for(int i = 0; i < Nodes->GetEntries(); i++){ CbmGeoNode *node = dynamic_cast (Nodes->At(i)); if (!node) continue; TString SName = node->getShapePointer()->GetName(); TArrayD *P = node->getParameters(); CbmGeoMedium *material = node->getMedium(); Int_t NofComponents = material->getNComponents(); //cout << " NofComponents = " << NofComponents << endl; Double_t Zeff = 0.; Double_t Aeff = 0.; Double_t W = 0.; for (Int_t iComp = 0; iComp < NofComponents; iComp++) { Double_t p[3]; material->getComponent(iComp, p); W += p[2]; } for (Int_t iComp = 0; iComp < NofComponents; iComp++) { Double_t p[3]; material->getComponent(iComp, p); Aeff += p[0] * p[2]; Zeff += p[1] * p[2]; } Aeff /= W; Zeff /= W; cout << " Zeff = " << Zeff << ", Aeff = " << Aeff << ", Zeff/Aeff = " << Zeff/Aeff << endl; CbmLitMaterial m; m.SetDensity(material->getDensity()); m.SetAeff(Aeff); m.SetZeff(Zeff); material->calcRadiationLength(); m.SetRadLength(material->getRadiationLength()); if(SName == "PGON") m.SetThickness(-2 * P->At(4));else if(SName == "BOX") m.SetThickness(2 * P->At(2)); else continue; CbmGeoVector nodeV = node->getLabTransform()->getTranslation(); //in cm CbmGeoVector centerV = node->getCenterPosition().getTranslation(); Double_t z; //cout << "nodeV.Z() = " << nodeV.Z() << " centerV.Z() = " << centerV.Z() << endl; if (m.GetThickness() < fThickWall) z = nodeV.Z() + centerV.Z(); else z = nodeV.Z()+ centerV.Z() + m.GetThickness() / 2.0; m.SetZ(z); if(m.GetRadLength() > 10e10) continue; fvAllMaterials.push_back(m); cout << " Add material " << SName << " : [ Z = " << m.GetZ() << " Thickness = " << m.GetThickness() << " RadLength = " << m.GetRadLength() << " ]" << std::endl; } } void CbmLitKalmanImp::ReadTrdMaterials() { cout << "CbmLitKalmanImp::ReadTrdMaterials: " << endl; cout << " reading TRD materials..." << endl; CbmRunAna *Run = CbmRunAna::Instance(); CbmRuntimeDb *RunDB = Run->GetRuntimeDb(); CbmGeoTrdPar *TrdPar = (CbmGeoTrdPar*) (RunDB->getContainer("CbmGeoTrdPar")); TObjArray *NodesActive = TrdPar->GetGeoSensitiveNodes(); TObjArray *NodesPassive = TrdPar->GetGeoPassiveNodes(); cout << " TRD Active Nodes:" << endl; ReadTrdMaterials(NodesActive); cout << " TRD Passive Nodes:" << endl; ReadTrdMaterials(NodesPassive); } void CbmLitKalmanImp::ReadStsMaterials(TObjArray *Nodes) { for (int i = 0; i < Nodes->GetEntries(); i++){ CbmGeoNode *node = dynamic_cast (Nodes->At(i)); if ( !node ) break; TArrayD *P = node->getParameters(); CbmGeoMedium *material = node->getMedium(); Int_t NofComponents = material->getNComponents(); //cout << " NofComponents = " << NofComponents << endl; Double_t Zeff = 0.; Double_t Aeff = 0.; Double_t W = 0.; for (Int_t iComp = 0; iComp < NofComponents; iComp++) { Double_t p[3]; material->getComponent(iComp, p); W += p[2]; } for (Int_t iComp = 0; iComp < NofComponents; iComp++) { Double_t p[3]; material->getComponent(iComp, p); Aeff += p[0] * p[2]; Zeff += p[1] * p[2]; } Aeff /= W; Zeff /= W; cout << " Zeff = " << Zeff << ", Aeff = " << Aeff << ", Zeff/Aeff = " << Zeff/Aeff << endl; CbmLitMaterial m; m.SetDensity(material->getDensity()); m.SetAeff(Aeff); m.SetZeff(Zeff); material->calcRadiationLength(); m.SetRadLength(material->getRadiationLength()); m.SetThickness(P->At(2)); m.SetRmin(P->At(0)); m.SetRmax(P->At(1)); CbmGeoVector nodeV = node->getLabTransform()->getTranslation(); //in cm CbmGeoVector centerV = node->getCenterPosition().getTranslation(); Double_t z; if (m.GetThickness() < fThickWall) z = nodeV.Z() + centerV.Z(); else z = nodeV.Z()+ centerV.Z() + m.GetThickness() / 2.0; m.SetZ(z); if(m.GetRadLength() > 10e10) continue; fvAllMaterials.push_back(m); } } void CbmLitKalmanImp::ReadStsMaterials() { std::cout << "CbmLitKalmanImp::ReadStsMaterials: " << std::endl; cout << " reading STS materials..." << endl; CbmRunAna *Run = CbmRunAna::Instance(); CbmRuntimeDb *RunDB = Run->GetRuntimeDb(); CbmGeoStsPar* StsPar = (CbmGeoStsPar*) (RunDB->getContainer("CbmGeoStsPar")); TObjArray *NodesActive = StsPar->GetGeoSensitiveNodes(); TObjArray *NodesPassive = StsPar->GetGeoPassiveNodes(); if( StsPar ){ //=== Sts stations === cout << " STS Active Nodes: " << NodesActive->GetEntries() << endl; ReadStsMaterials(NodesActive); cout << " STS Passive Nodes: " << NodesPassive->GetEntries() << endl; ReadStsMaterials(NodesPassive); } } void CbmLitKalmanImp::ReadRichMaterials() { std::cout << "CbmLitKalmanImp::ReadRichMaterials: " << std::endl; CbmRunAna *Run = CbmRunAna::Instance(); CbmRuntimeDb *RunDB = Run->GetRuntimeDb(); CbmGeoRichPar *RichPar = (CbmGeoRichPar*) (RunDB->getContainer("CbmGeoRichPar")); RichPar->Print(); RichPar->printParams(); if( RichPar ) { TObjArray *NodesActive = RichPar->GetGeoSensitiveNodes(); TObjArray *NodesPassive = RichPar->GetGeoPassiveNodes(); cout << " Rich Active Nodes:" << NodesActive->GetEntries() << endl; for (int i = 0; i < NodesActive->GetEntries(); i++) { CbmGeoNode *node = dynamic_cast (NodesActive->At(i)); if ( !node ) continue; } cout << " Rich Passive Nodes:" << NodesPassive->GetEntries() << endl; for (int i = 0; i < NodesPassive->GetEntries(); i++) { CbmGeoNode *node = dynamic_cast (NodesPassive->At(i)); if ( !node ) continue; } } } void CbmLitKalmanImp::SetPid(Int_t Pid) { fElectron = kFALSE; switch (TMath::Abs(Pid)) { case 211 : fMass = 0.139; break; case 11 : fMass = 0.0003; fElectron = kTRUE; break; default: fMass = 0.139; break; } } void CbmLitKalmanImp::KalmanPrediction(CbmLitState &State, Double_t z_out) { Double_t z_in = State.GetZ(); if (z_out > z_in) fDownstream = kTRUE; else fDownstream = kFALSE; // CbmLitState S1; // S1.SetStateVector(State.GetStateVector()); // S1.SetZ(z_in); // fExtrapolator->Propagate(State, z_out, kTRUE); //matrix with process noise // Double_t Q[25]; // for (Int_t i = 0; i < 25; ++i) Q[i] = 0.; //Add noise from materials vector::iterator it_min, it_max; CbmLitMaterial m; m.SetZ(z_in); it_min = lower_bound(fvAllMaterials.begin(), fvAllMaterials.end(), m, CbmLitMaterial::CmpFindZ); m.SetZ(z_out); it_max = lower_bound(fvAllMaterials.begin(), fvAllMaterials.end(), m, CbmLitMaterial::CmpFindZ); for(vector::iterator i = it_min; i != it_max; ) { Double_t Z = (*i).GetZ(); Double_t Thickness = (*i).GetThickness(); fExtrapolator->Propagate(State, Z, kTRUE); // fExtrapolator->Propagate(S1, Z, kFALSE); Double_t RadLength = Thickness / (*i).GetRadLength(); if (Thickness < fThickWall) AddThinScatter(State, RadLength, NULL); else AddThickScatter(State, RadLength, Thickness, NULL); // if (!fElectron) AddEnergyLoss(State, (*i)); // else AddElectronEnergyLoss(State, (*i)); //AddThinScatter(S1, RadLength, Q); //AddThickScatter(S1, RadLength, (*i).GetThickness(), Q); if (fDownstream) i++; else i--; } fExtrapolator->Propagate(State, z_out, kTRUE); //add process noise to the covariance matrix //Double_t *C = State.GetCovMatrix(); //for (Int_t i = 0; i < 25; i++) C[i] += Q[i]; } void CbmLitKalmanImp::KalmanFilter(CbmLitNode &Node) { Double_t *X_in = Node.GetPredictedState().GetStateVector(); Double_t *C_in = Node.GetPredictedState().GetCovMatrix(); Double_t *X_out = Node.GetFilteredState().GetStateVector(); Double_t *C_out = Node.GetFilteredState().GetCovMatrix(); Double_t *M = Node.GetMeas().GetMeas(); Double_t *V = Node.GetMeas().GetCovMatrix(); double t1 = V[3] + C_in[6]; double t7 = C_in[5] * C_in[5]; double t9 = 0.1e1 / (V[0] * V[3] + V[0] * C_in[6] + C_in[0] * V[3] + C_in[0] * C_in[6] - t7); double t11 = t7 * t9; double t13 = M[0] - X_in[0]; double t17 = V[0] + C_in[0]; double t21 = M[1] - X_in[1]; //Calculate Filtered State vector X_out[0] = X_in[0] + (C_in[0] * t1 * t9 - t11) * t13 + (-C_in[0] * C_in[5] * t9 + C_in[5] * t17 * t9) * t21; X_out[1] = X_in[1] + (C_in[5] * t1 * t9 - C_in[6] * C_in[5] * t9) * t13 + (-t11 + C_in[6] * t17 * t9) * t21; X_out[2] = X_in[2] + (C_in[10] * t1 * t9 - C_in[11] * C_in[5] * t9) * t13 + (-C_in[10] * C_in[5] * t9 + C_in[11] * t17 * t9) * t21; X_out[3] = X_in[3] + (C_in[15] * t1 * t9 - C_in[16] * C_in[5] * t9) * t13 + (-C_in[15] * C_in[5] * t9 + C_in[16] * t17 * t9) * t21; X_out[4] = X_in[4] + (C_in[20] * t1 * t9 - C_in[21] * C_in[5] * t9) * t13 + (-C_in[20] * C_in[5] * t9 + C_in[21] * t17 * t9) * t21; //double t1 = V[1][1] + C_in[1][1]; //double t7 = C_in[1][0] * C_in[1][0]; //double t9 = 0.1e1 / (V[0][0] * V[1][1] + V[0][0] * C_in[1][1] + // C_in[0][0 * V[1][1] + C_in[0][0] * C_in[1][1] - t7); //double t11 = t7 * t9; double t12 = 1 - C_in[0] * t1 * t9 + t11; double t16 = V[0] + C_in[0]; double t19 = C_in[0] * C_in[5] * t9 - C_in[5] * t16 * t9; double t38 = -C_in[5] * t1 * t9 + C_in[6] * C_in[5] * t9; double t42 = 1 + t11 - C_in[6] * t16 * t9; double t61 = -C_in[10] * t1 * t9 + C_in[11] * C_in[5] * t9; double t67 = C_in[10] * C_in[5] * t9 - C_in[11] * t16 * t9; double t86 = -C_in[15] * t1 * t9 + C_in[16] * C_in[5] * t9; double t92 = C_in[15] * C_in[5] * t9 - C_in[16] * t16 * t9; double t111 = -C_in[20] * t1 * t9 + C_in[21] * C_in[5] * t9; double t117 = C_in[20] * C_in[5] * t9 - C_in[21] * t16 * t9; //Calculate Filtered covariance matrix C_out[0] = t12 * C_in[0] + t19 * C_in[5]; C_out[1] = t12 * C_in[5] + t19 * C_in[6]; C_out[2] = t12 * C_in[10] + t19 * C_in[11]; C_out[3] = t12 * C_in[15] + t19 * C_in[16]; C_out[4] = t12 * C_in[20] + t19 * C_in[21]; C_out[5] = C_out[1]; C_out[6] = t38 * C_in[5] + t42 * C_in[6]; C_out[7] = t38 * C_in[10] + t42 * C_in[11]; C_out[8] = t38 * C_in[15] + t42 * C_in[16]; C_out[9] = t38 * C_in[20] + t42 * C_in[21]; C_out[10] = C_out[2]; C_out[11] = C_out[7]; C_out[12] = t61 * C_in[10] + t67 * C_in[11] + C_in[12]; C_out[13] = t61 * C_in[15] + t67 * C_in[16] + C_in[17]; C_out[14] = t61 * C_in[20] + t67 * C_in[21] + C_in[22]; C_out[15] = C_out[3]; C_out[16] = C_out[8]; C_out[17] = C_out[13]; C_out[18] = t86 * C_in[15] + t92 * C_in[16] + C_in[18]; C_out[19] = t86 * C_in[20] + t92 * C_in[21] + C_in[23]; C_out[20] = C_out[4]; C_out[21] = C_out[9]; C_out[22] = C_out[14]; C_out[23] = C_out[19]; C_out[24] = t111 * C_in[20] + t117 * C_in[21] + C_in[24]; //double t1 = V[1][1] + C_in[1][1]; //double t7 = C_in[1][0] * C_in[1][0]; //double t9 = 0.1e1 / (V[0][0] * V[1][1] + V[0][0] * C_in[1][1] + // C_in[0][0] * V[1][1] + C_in[0][0] * C_in[1][1] - t7); //double t11 = t7 * t9; //double t13 = m[0] - X_in[0]; //double t17 = V[0][0] + C_in[0][0]; //double t21 = m[1] - X_in[1]; //Calculate Filtered Chi2 double t23 = M[0] - X_out[0]; //X_in[0] - (C_in[0][0] * t1 * t9 - t11) * // t13 - (-C_in[0][0] * C_in[1][0] * t9+ C_in[1][0] * t17 * t9) * t21; double t25 = V[0] * V[0]; double t381 = M[1] - X_out[1];//X_in[1] - (C_in[1][0] * t1 * t9 - C_in[1][1] * // C_in[1][0] * t9) * t13 - (-t11 + C_in[1][1] * t17 * t9) * t21; double t421 = 0.1e1 / V[0] / V[3]; double t49 = V[3] * V[3]; double t54 = t23 * (t23 * t17 / t25 + t381 * C_in[5] * t421) + t381 * (t23 * C_in[5] * t421 + t381 * t1 / t49); Node.SetFilteredChi2(t54); } void CbmLitKalmanImp::AddThickScatter(CbmLitState& State, Double_t RadLength, Double_t Thickness, Double_t Q[]) { //TODO momentum too small Double_t tx = State.GetStateVector()[2]; Double_t ty = State.GetStateVector()[3]; Double_t qp = State.GetStateVector()[4]; Double_t t3 = 0.; if (RadLength > 1e-20) { Double_t norm = 1 + tx * tx + ty * ty; Double_t RadThick = RadLength * TMath::Sqrt(norm); Double_t t1 = 1 + 0.038 * log(RadThick); Double_t t2 = fFms * 0.0136 * qp * t1; t3 = ( 1 + fMass * fMass * qp * qp ) * RadThick * t2 * t2 * norm; } Double_t Q33 = ( 1 + tx * tx ) * t3; Double_t Q44 = ( 1 + ty * ty ) * t3; Double_t Q34 = tx * ty * t3; Double_t T23 = (Thickness * Thickness) / 3.0; Double_t T2 = Thickness / 2.0; Double_t D = (fDownstream) ? 1. : -1. ; if(!Q) { Double_t *C = State.GetCovMatrix(); C[0] += Q33 * T23; C[1] = C[5] += Q34 * T23; C[2] = C[10] += Q33 * D * T2; C[3] = C[15] += Q34 * D * T2; C[6] += Q44 * T23; C[7] = C[11] += Q34 * D * T2; C[8] = C[16] += Q44 * D * T2; C[12] += Q33; C[13] = C[17] += Q34; C[18] += Q44; // //C[24] += 0.0001; } else { Q[0] += Q33 * T23; Q[1] = Q[5] += Q34 * T23; Q[2] = Q[10] += Q33 * D * T2; Q[3] = Q[15] += Q34 * D * T2; Q[6] += Q44 * T23; Q[7] = Q[11] += Q34 * D * T2; Q[8] = Q[16] += Q44 * D * T2; Q[12] += Q33; Q[13] = Q[17] += Q34; Q[18] += Q44; // //Q[24] += 0.0001; } } void CbmLitKalmanImp::AddThinScatter(CbmLitState& State, Double_t RadLength, Double_t Q[]) { //TODO momentum too small Double_t tx = State.GetStateVector()[2]; Double_t ty = State.GetStateVector()[3]; Double_t qp = State.GetStateVector()[4]; Double_t t3 = 0.; if (RadLength > 1e-20) { Double_t norm = 1 + tx * tx + ty * ty; Double_t RadThick = RadLength * TMath::Sqrt(norm); Double_t t1 = 1 + 0.038 * log(RadThick); Double_t t2 = fFms * 0.0136 * qp * t1; t3 = ( 1 + fMass * fMass * qp * qp ) * RadThick * t2 * t2 * norm; } // Double_t Q33 = ( 1 + tx * tx ) * t3; // Double_t Q44 = ( 1 + ty * ty ) * t3; // Double_t Q34 = tx * ty * t3; if(!Q) { Double_t *C = State.GetCovMatrix(); C[12] += ( 1 + tx * tx ) * t3;//Q33; C[18] += ( 1 + ty * ty ) * t3;//Q44; C[13] = C[17] += tx * ty * t3;//Q34; } else { Q[12] += ( 1 + tx * tx ) * t3;//Q33; Q[18] += ( 1 + ty * ty ) * t3;//Q44; Q[13] = Q[17] += tx * ty * t3;//Q34; } } void CbmLitKalmanImp::AddEnergyLoss( CbmLitState& State, CbmLitMaterial& Material) { // dE/dx energy loss (Bethe-Block) Double_t tx = State.GetStateVector()[2]; Double_t ty = State.GetStateVector()[3]; Double_t qp = State.GetStateVector()[4]; Double_t norm = TMath::Sqrt(1 + tx * tx + ty * ty); Double_t bbLoss = Material.GetThickness() * norm * Material.GetDensity() * fEnergyLoss * Material.GetZeff() / Material.GetAeff(); //TODO bbEloss too big if (fDownstream) { bbLoss *= -1.0; } if (qp > 0.) { qp = 1./((1./qp) + bbLoss); } else{ qp = 1./((1./qp) - bbLoss); } State.GetStateVector()[4] = qp; } void CbmLitKalmanImp::AddElectronEnergyLoss(CbmLitState& State, CbmLitMaterial& Material) { //energy loss for electrons Double_t RadThick; Double_t tx = State.GetStateVector()[2]; Double_t ty = State.GetStateVector()[3]; Double_t qp = State.GetStateVector()[4]; Double_t norm = TMath::Sqrt(1 + tx * tx + ty * ty); if (fDownstream){ RadThick = - (Material.GetThickness() / Material.GetRadLength()) * norm; } else{ RadThick = (Material.GetThickness() / Material.GetRadLength()) * norm; } // TODO protect against t too big State.GetCovMatrix()[24] += qp * qp * (TMath::Exp(-RadThick * TMath::Log(3.0)/TMath::Log(2.0)) - TMath::Exp(-2.0 * RadThick)); State.GetStateVector()[4] *= TMath::Exp(-RadThick); } ClassImp(CbmLitKalmanImp)