From 7f8b25edceb419d1359d7c807b3bc2091024c227 Mon Sep 17 00:00:00 2001 From: Valerio Pia Date: Tue, 15 Oct 2024 12:13:22 +0200 Subject: [PATCH] Updated kf functions to work inside the manager --- include/STTKFKalmanFilter.h | 34 +- include/STTKFTrack.h | 6 +- src/SANDMeasurementsBuilder.cpp | 235 +-------- src/STTKFKalmanFilter.cpp | 837 ++++++++++++++------------------ 4 files changed, 424 insertions(+), 688 deletions(-) diff --git a/include/STTKFKalmanFilter.h b/include/STTKFKalmanFilter.h index e58a516..2d78fe0 100644 --- a/include/STTKFKalmanFilter.h +++ b/include/STTKFKalmanFilter.h @@ -4,16 +4,26 @@ #include "TGeoManager.h" #include "TMatrixD.h" -#include "SANDTrackerCluster.h" #include "SANDTrackerClusterCollection.h" #include "STTKFTrack.h" #include "SANDTrackerUtils.h" #include "STTKFGeoManager.h" +struct SParticleInfo { + int charge; + double mass; + int pdg_code; + int id; + + TVector3 pos; + TVector3 mom; +}; + class STTKFChecks; using STTKFStateCovarianceMatrix = TMatrixD; using STTKFMeasurement = TMatrixD; +using TrackletMap = std::map>; class STTKFKalmanFilterManager { @@ -22,7 +32,9 @@ class STTKFKalmanFilterManager { STTKFTrackStep::STTKFTrackStateStage fCurrentStage; // forward or backward int fCurrentStep; // index of the STTKFTrackStep in STTKFTrack - + double fCurrentZ; + TrackletMap* z_to_tracklets_; + SParticleInfo particleInfo_; public: @@ -53,6 +65,7 @@ class STTKFKalmanFilterManager { public: Orientation fCurrentOrientation = Orientation::kVertical; + STTKFMeasurement GetMeasurementFromTracklet(const TVectorD& tracklet); double DeltaRadius(const STTKFStateVector& stateVector, double nextPhi, double dZ, double dE, double particle_mass) const; inline double DEDTanl(const STTKFStateVector& stateVector, double nextPhi, double dZ, double dE, double particle_mass) const { auto tan = stateVector.TanLambda(); return dE * tan / (1 + tan*tan); }; inline double DEDPhi(const STTKFStateVector& stateVector, double nextPhi, double dZ, double dE, double particle_mass) const { @@ -191,14 +204,15 @@ class STTKFKalmanFilterManager { STTKFMeasurement GetPrediction(Orientation orientation, const STTKFStateVector& stateVector); - // void Propagate(double dE, const STTPlaneID& nextPlaneID); - double EvalChi2(const STTKFMeasurement& observation, const STTKFMeasurement& prediction, const TMatrixD& measurementNoiseMatrix); - // int FindBestMatch(const std::vector& clusterIDs, const STTKFMeasurement& prediction, const TMatrixD& measurementNoiseMatrix); - // void Filter(const STTKFMeasurement& observation, const STTKFMeasurement& prediction, Orientation orientation); - // void Smooth(); - // void Init(const STTPlaneID& plane, int clusterID); - // void Run(); - // const STTKFTrack& GetTrack() {return fThisTrack; }; + void Propagate(double& dE, double& dZ, double& beta); + double EvalChi2(const STTKFMeasurement& measurement, const STTKFMeasurement& prediction, const TMatrixD& measurementNoiseMatrix); + int FindBestMatch(double& nextZ, const STTKFMeasurement& prediction, const TMatrixD& measurementNoiseMatrix); + void SetNextOrientation(); + void Filter(const STTKFMeasurement& measurement, const STTKFMeasurement& prediction); + void Smooth(); + void InitFromMC(TrackletMap* z_to_tracklets, const SParticleInfo& particloInfo); + void Run(); + const STTKFTrack& GetTrack() {return fThisTrack; }; friend class STTKFChecks; }; diff --git a/include/STTKFTrack.h b/include/STTKFTrack.h index c5a7c1a..bef3778 100644 --- a/include/STTKFTrack.h +++ b/include/STTKFTrack.h @@ -111,7 +111,7 @@ class STTKFTrackStep { STTKFState fSmoothed; // the propagation that bring the vector in this state - // TMatrixD fPropagatorMatrix; + TMatrixD fPropagatorMatrix; // TMatrixD fProjectionMatrix; // TMatrixD fProcessNoiseMatrix; // TMatrixD fMeasurementNoiseMatrix; @@ -125,7 +125,7 @@ class STTKFTrackStep { int fClusterID; public: - // STTKFTrackStep(): fPropagatorMatrix(5,5), + STTKFTrackStep(): fPropagatorMatrix(5,5) {}; // fProjectionMatrix(2,5), // fProcessNoiseMatrix(5,5), // fMeasurementNoiseMatrix(2,2), @@ -137,6 +137,8 @@ class STTKFTrackStep { int GetClusterIDForThisState() const { return fClusterID; } void SetStage(STTKFTrackStateStage stage, STTKFState state); const STTKFState& GetStage(STTKFTrackStateStage stage) const; + void SetPropagatorMatrix(TMatrixD pMatrix) { fPropagatorMatrix = pMatrix; }; + const TMatrixD GetPropagatorMatrix() { return fPropagatorMatrix; }; }; class STTKFTrack { diff --git a/src/SANDMeasurementsBuilder.cpp b/src/SANDMeasurementsBuilder.cpp index 04edcbf..03f6896 100644 --- a/src/SANDMeasurementsBuilder.cpp +++ b/src/SANDMeasurementsBuilder.cpp @@ -22,15 +22,23 @@ #include "EDEPTree.h" -struct SParticleInfo { - int charge; - double mass; - int pdg_code; - int id; +void TryCompleteManager(TrackletMap z_to_tracklets, SParticleInfo particleInfo) { + STTKFKalmanFilterManager manager; + manager.InitFromMC(&z_to_tracklets, particleInfo); + manager.Run(); - TVector3 pos; - TVector3 mom; -}; + auto track = manager.GetTrack(); + + auto step = track.GetSteps().back(); + auto reco_state = + step.GetStage(STTKFTrackStep::STTKFTrackStateStage::kSmoothing).GetStateVector(); + auto reco_mom = SANDTrackerUtils::GetMomentumInMeVFromRadiusInMM( + reco_state.Radius(), reco_state.TanLambda()); + + std::cout << "Initial Smoothed Reco Momentum " << reco_mom << std::endl; + + return; +} void ProcessEventWithKF(SANDGeoManager* sand_geo, TG4Event* mc_event, std::vector* digits) { @@ -39,9 +47,10 @@ void ProcessEventWithKF(SANDGeoManager* sand_geo, TG4Event* mc_event, std::vecto SANDTrackerDigitCollection::FillMap(digits); auto digit_map = SANDTrackerDigitCollection::GetDigits(); + if (SANDTrackerDigitCollection::GetDigits().empty()) { + return; + } std::string tracker_name = SANDTrackerDigitCollection::GetDigits().begin()->det; - // std::cout << "TRACKER: " << tracker_name << std::endl; - SANDTrackerClusterCollection clusters(sand_geo, SANDTrackerDigitCollection::GetDigits(), SANDTrackerClusterCollection::ClusteringMethod::kCellAdjacency); TrackletFinder traklet_finder; @@ -76,10 +85,8 @@ void ProcessEventWithKF(SANDGeoManager* sand_geo, TG4Event* mc_event, std::vecto int sum = 0; for (auto el:z_to_tracklets) { - // std::cout << "At z = " << el.first << " there are " << el.second.size() << " tracklets" << std::endl; sum += el.second.size(); } - std::cout << "Total tracklets: " << sum << std::endl; if (sum == 0) { return; } @@ -91,9 +98,6 @@ void ProcessEventWithKF(SANDGeoManager* sand_geo, TG4Event* mc_event, std::vecto tree.Filter(std::back_insert_iterator>(primaryTrj), [](const EDEPTrajectory& trj) { return trj.GetParentId() == -1;} ); - // std::string print; - // for (auto trj:primaryTrj) trj.Print(print); - TDatabasePDG pdg_db; std::vector particleInfos; for (auto trj:primaryTrj) { @@ -109,10 +113,7 @@ void ProcessEventWithKF(SANDGeoManager* sand_geo, TG4Event* mc_event, std::vecto pi.mom = trj.GetTrajectoryPoints().at(string_to_component[tracker_name]).back().GetMomentum(); particleInfos.push_back(pi); - // std::cout << pi.pdg_code << " " << pi.id << " " << pi.mass << " " << pi.charge << std::endl; std::cout << "Initial Momentum " << trj.GetInitialMomentum().Vect().Mag() << std::endl; - // pi.pos.Print(); - // pi.mom.Print(); } int nParticles = particleInfos.size(); @@ -124,202 +125,8 @@ void ProcessEventWithKF(SANDGeoManager* sand_geo, TG4Event* mc_event, std::vecto } for (int ip = 0; ip < nParticles; ip++) { - int charge = particleInfos[ip].charge; - double particle_mass = particleInfos[ip].mass; - int pdg = particleInfos[ip].pdg_code; - - STTKFTrack this_track; - STTKFStateVector current_state; - STTKFKalmanFilterManager manager; - STTKFKalmanFilterManager::Orientation current_orientation = manager.GetOrientation(); - - // To Do: find a smarter algorithm to compute this - TMatrixD initial_cov_matrix(5, 5); - initial_cov_matrix[0][0] = pow(200E-6, 2); - initial_cov_matrix[1][1] = pow(200E-6, 2); - initial_cov_matrix[2][2] = pow(0.1, 2); - initial_cov_matrix[3][3] = pow(0.01, 2); - initial_cov_matrix[4][4] = pow(0.01, 2); - - - // To Do: implment a seeding algorithm - STTKFStateVector initial_state_vector = STTKFCheck::get_state_vector(particleInfos[ip].mom * 1E-3, // GeV - particleInfos[ip].pos * 1E-3, // m - particleInfos[ip].charge); - - - // initial_cov_matrix.Print(); - - - std::vector propagator_matrices; - - STTKFTrackStep trackStep; - trackStep.SetStage(STTKFTrackStep::STTKFTrackStateStage::kPrediction, - STTKFState(initial_state_vector, initial_cov_matrix)); - trackStep.SetStage(STTKFTrackStep::STTKFTrackStateStage::kFiltering, - STTKFState(initial_state_vector, initial_cov_matrix)); - - propagator_matrices.push_back(initial_cov_matrix); - - this_track.AddStep(trackStep); - - - double previous_z = particleInfos[ip].pos.Z(); - auto current_z_it = std::prev(z_to_tracklets.lower_bound(particleInfos[ip].pos.Z())); - for ( ; current_z_it != z_to_tracklets.begin(); current_z_it--) { - - // Get previous kf step - auto previous_step = - this_track.GetStep(this_track.GetSteps().size() - 1); - auto previousStateVector = - previous_step - .GetStage(STTKFTrackStep::STTKFTrackStateStage::kFiltering) - .GetStateVector(); - auto previousCovMatrix = - previous_step - .GetStage(STTKFTrackStep::STTKFTrackStateStage::kFiltering) - .GetStateCovMatrix(); - - - // Compute energy loss for the new step - auto dir = -1. * manager.GetDirectiveCosinesFromStateVector( - previousStateVector); - - // To Do: check if this is still valid and add a real fix if needed - if (dir.Z() > 0) { - dir *= -1; - } - - auto current_mom = SANDTrackerUtils::GetMomentumInMeVFromRadiusInMM( - previousStateVector.Radius(), - previousStateVector.TanLambda()) / 1000; - double gamma = sqrt(current_mom * current_mom + particle_mass * particle_mass) / - particle_mass; - double beta_from_gamma = sqrt(1 - pow(1 / gamma, 2)); - - // std::cout << previous_z << " " - // << previousStateVector.X() << " " - // << previousStateVector.Y() << " " - // << current_z_it->first << " " << std::endl; - // dir.Print(); - - // To Do: check all units - auto de_step = STTKFGeoManager::GetDE( - current_z_it->first, - 1000 * previousStateVector.X(), 1000 * previousStateVector.Y(), previous_z, - dir.X(), dir.Y(), dir.Z(), - beta_from_gamma, particle_mass, charge) / 1000; - // std::cout << "de_step " << de_step << std::endl; - - double dz = (current_z_it->first - previous_z) / 1000; - // std::cout << dz << std::endl; - - // Propagation of state and cov matrix - auto predictedStateVector = manager.PropagateState( - previousStateVector, dz, de_step, particle_mass); - auto nextPhi = predictedStateVector.Phi(); - TMatrixD covariance_noise(5, 5); - covariance_noise = manager.GetProcessNoiseMatrix( - previousStateVector, nextPhi, dz, de_step, previous_z, - particle_mass); - - auto propagatorMatrix = manager.GetPropagatorMatrix( - previousStateVector, nextPhi, dz, de_step, particle_mass); - - auto predictedCovMatrix = manager.PropagateCovMatrix( - previousCovMatrix, propagatorMatrix, covariance_noise); - - - // Filling prediction step of track - STTKFTrackStep currentTrackState; - currentTrackState.SetStage( - STTKFTrackStep::STTKFTrackStateStage::kPrediction, - STTKFState(predictedStateVector, predictedCovMatrix)); - - // previousStateVector().Print(); - // predictedStateVector().Print(); - - // Prediction and filtering on the next plane - STTKFMeasurement prediction = - manager.GetPrediction(current_orientation, predictedStateVector); - auto projectionMatrix = - manager.GetProjectionMatrix(current_orientation, predictedStateVector); - auto measurementNoiseMatrix = manager.GetMeasurementNoiseMatrix(); - - auto kalmanGainMatrix = manager.GetKalmanGainMatrix( - predictedCovMatrix, projectionMatrix, measurementNoiseMatrix); - - TMatrixD projectionMatrixTransposed(TMatrixD::kTransposed, - projectionMatrix); - auto Sk = measurementNoiseMatrix + projectionMatrix * - predictedCovMatrix * - projectionMatrixTransposed; - - - // prediction.Print(); - double best_chi = 1E9; - STTKFMeasurement best_measurement(2, 1); - for (const auto& tracklet:current_z_it->second) { - STTKFMeasurement measurement(2, 1); - // To Do: vertical and horizontal are outdated and confusing. Replace with something more meaningful. - // Notice: vertical planes means horizontal measurements and the opposite - if (current_orientation == STTKFKalmanFilterManager::Orientation::kVertical) { - measurement[0][0] = tracklet[0] / 1000.; - measurement[1][0] = M_PI_2 - tracklet[2]; - } else { - measurement[0][0] = tracklet[1] / 1000.; - measurement[1][0] = tracklet[3]; - } - // measurement[0][0] += rand->Gaus(0, sigmaPos); - // measurement[1][0] += rand->Gaus(0, sigmaAng); - // measurement.Print(); - // std::cout << tracklet[0] << " " << tracklet[1] << " " << tracklet[2] << " " << tracklet[3] << std::endl; - auto chi2 = manager.EvalChi2(measurement, prediction, Sk); - if (chi2 < best_chi) { - best_chi = chi2; - best_measurement = measurement; - } - } - if (best_chi > 1.5) { - continue; - } - propagator_matrices.push_back(propagatorMatrix); - - // std::cout << "chi2 " << best_chi << std::endl; - // best_measurement.Print(); - - auto filteredStateVector = - manager.FilterState(predictedStateVector, kalmanGainMatrix, - best_measurement, prediction); - // filteredStateVector().Print(); - - auto filteredCovMatrix = manager.FilterCovMatrix( - predictedCovMatrix, projectionMatrix, measurementNoiseMatrix); - - currentTrackState.SetStage( - STTKFTrackStep::STTKFTrackStateStage::kFiltering, - STTKFState(filteredStateVector, filteredCovMatrix)); - this_track.AddStep(currentTrackState); - - - if (current_orientation == STTKFKalmanFilterManager::Orientation::kVertical) { - current_orientation = STTKFKalmanFilterManager::Orientation::kHorizontal; - } else { - current_orientation = STTKFKalmanFilterManager::Orientation::kVertical; - } - - previous_z = current_z_it->first; - } - - auto reco_state = - this_track.GetSteps()[this_track.GetSteps().size() - 1] - .GetStage(STTKFTrackStep::STTKFTrackStateStage::kFiltering) - .GetStateVector(); - auto reco_mom = SANDTrackerUtils::GetMomentumInMeVFromRadiusInMM( - reco_state.Radius(), reco_state.TanLambda()); - std::cout << "Initial Reco Momentum " << reco_mom << std::endl; + TryCompleteManager(z_to_tracklets, particleInfos[ip]); } - } int main(int argc, char* argv[]) @@ -353,7 +160,7 @@ int main(int argc, char* argv[]) SANDGeoManager sand_geo; sand_geo.init(geo); - for (int i = 2; i < 3; i++) { + for (int i = 0; i < 30; i++) { t_h->GetEntry(i); t->GetEntry(i); diff --git a/src/STTKFKalmanFilter.cpp b/src/STTKFKalmanFilter.cpp index 3f2d3c3..3bec5fe 100644 --- a/src/STTKFKalmanFilter.cpp +++ b/src/STTKFKalmanFilter.cpp @@ -1,6 +1,5 @@ #include "STTKFKalmanFilter.h" #include "SANDTrackerClusterCollection.h" -#include "STTKFClusterManager.h" #include "SANDTrackerUtils.h" #include @@ -99,51 +98,51 @@ TMatrixD STTKFKalmanFilterManager::GetProcessNoiseMatrix( const STTKFStateVector& stateVector, double nextPhi, double dZ, double dE, double z, double particle_mass) { - // TVectorD processNoiseTanlDerivative(5); - // TVectorD processNoisePhiDerivative(5); - - // processNoiseTanlDerivative[0] = DxDTanl(stateVector, nextPhi, dZ, dE, particle_mass); - // processNoiseTanlDerivative[1] = DyDTanl(stateVector, nextPhi, dZ, dE, particle_mass); - // processNoiseTanlDerivative[2] = DInvCRDTanl(stateVector, nextPhi, dZ, dE, particle_mass); - // processNoiseTanlDerivative[3] = DTanlDTanl(stateVector, nextPhi, dZ, dE, particle_mass); - // processNoiseTanlDerivative[4] = DPhiDTanl(stateVector, nextPhi, dZ, dE, particle_mass); - - // processNoisePhiDerivative[0] = DxDPhi(stateVector, nextPhi, dZ, dE, particle_mass); - // processNoisePhiDerivative[1] = DyDPhi(stateVector, nextPhi, dZ, dE, particle_mass); - // processNoisePhiDerivative[2] = DInvCRDPhi(stateVector, nextPhi, dZ, dE, particle_mass); - // processNoisePhiDerivative[3] = DTanlDPhi(stateVector, nextPhi, dZ, dE, particle_mass); - // processNoisePhiDerivative[4] = DPhiDPhi(stateVector, nextPhi, dZ, dE, particle_mass); - - // auto momentumInMeV = SANDTrackerUtils::GetMomentumInMeVFromRadiusInMM(stateVector.Radius(), stateVector.TanLambda()); - - // auto dir = -1. * GetDirectiveCosinesFromStateVector(stateVector); - // if (dir.Z() > 0) dir *= -1; - // auto pathLengthInX0 = STTKFGeoManager::GetPathLengthInX0( - // (z + dZ)*1000, stateVector.X()*1000, stateVector.Y()*1000, z*1000, dir.X(), dir.Y(), dir.Z()); - - // // MCS angle - // double radius = stateVector.Radius(); - // double tan = stateVector.TanLambda(); - // double mom = SANDTrackerUtils::GetMomentumInMeVFromRadiusInMM(radius, tan) / 1000; + TVectorD processNoiseTanlDerivative(5); + TVectorD processNoisePhiDerivative(5); + + processNoiseTanlDerivative[0] = DxDTanl(stateVector, nextPhi, dZ, dE, particle_mass); + processNoiseTanlDerivative[1] = DyDTanl(stateVector, nextPhi, dZ, dE, particle_mass); + processNoiseTanlDerivative[2] = DInvCRDTanl(stateVector, nextPhi, dZ, dE, particle_mass); + processNoiseTanlDerivative[3] = DTanlDTanl(stateVector, nextPhi, dZ, dE, particle_mass); + processNoiseTanlDerivative[4] = DPhiDTanl(stateVector, nextPhi, dZ, dE, particle_mass); + + processNoisePhiDerivative[0] = DxDPhi(stateVector, nextPhi, dZ, dE, particle_mass); + processNoisePhiDerivative[1] = DyDPhi(stateVector, nextPhi, dZ, dE, particle_mass); + processNoisePhiDerivative[2] = DInvCRDPhi(stateVector, nextPhi, dZ, dE, particle_mass); + processNoisePhiDerivative[3] = DTanlDPhi(stateVector, nextPhi, dZ, dE, particle_mass); + processNoisePhiDerivative[4] = DPhiDPhi(stateVector, nextPhi, dZ, dE, particle_mass); + + auto momentumInMeV = SANDTrackerUtils::GetMomentumInMeVFromRadiusInMM(stateVector.Radius(), stateVector.TanLambda()); + + auto dir = -1. * GetDirectiveCosinesFromStateVector(stateVector); + if (dir.Z() > 0) dir *= -1; + auto pathLengthInX0 = STTKFGeoManager::GetPathLengthInX0( + (z + dZ)*1000, stateVector.X()*1000, stateVector.Y()*1000, z*1000, dir.X(), dir.Y(), dir.Z()); + + // MCS angle + double radius = stateVector.Radius(); + double tan = stateVector.TanLambda(); + double mom = SANDTrackerUtils::GetMomentumInMeVFromRadiusInMM(radius, tan) / 1000; - // double constant = SANDTrackerUtils::GetRadiusInMMToMomentumInGeVConstant() * SANDTrackerUtils::GetMagneticField() / sqrt(1 + pow(tan, 2)); + double constant = SANDTrackerUtils::GetRadiusInMMToMomentumInGeVConstant() * SANDTrackerUtils::GetMagneticField() / sqrt(1 + pow(tan, 2)); - // double gamma = sqrt(mom*mom + particle_mass*particle_mass) / particle_mass; - // double beta = sqrt( 1 - pow(1/gamma, 2)); - // auto sigmaMCSAngle = SANDTrackerUtils::GetMCSSigmaAngleFromMomentumInMeV( - // momentumInMeV, beta, pathLengthInX0); - // auto sigmaMCSAngleSquared = sigmaMCSAngle * sigmaMCSAngle; + double gamma = sqrt(mom*mom + particle_mass*particle_mass) / particle_mass; + double beta = sqrt( 1 - pow(1/gamma, 2)); + auto sigmaMCSAngle = SANDTrackerUtils::GetMCSSigmaAngleFromMomentumInMeV( + momentumInMeV, beta, pathLengthInX0); + auto sigmaMCSAngleSquared = sigmaMCSAngle * sigmaMCSAngle; - // auto factor = pow(1 + pow(stateVector.TanLambda(), 2), 2); + auto factor = pow(1 + pow(stateVector.TanLambda(), 2), 2); TMatrixD processNoiseMatrix(5, 5); - // for (int i = 0; i < 5; i++) - // for (int j = 0; j < 5; j++) - // processNoiseMatrix[i][j] = - // sigmaMCSAngleSquared * - // (processNoisePhiDerivative[i] * processNoisePhiDerivative[j] + - // factor * processNoiseTanlDerivative[i] * - // processNoiseTanlDerivative[j]); + for (int i = 0; i < 5; i++) + for (int j = 0; j < 5; j++) + processNoiseMatrix[i][j] = + sigmaMCSAngleSquared * + (processNoisePhiDerivative[i] * processNoisePhiDerivative[j] + + factor * processNoiseTanlDerivative[i] * + processNoiseTanlDerivative[j]); return processNoiseMatrix; } @@ -151,10 +150,10 @@ TMatrixD STTKFKalmanFilterManager::GetProcessNoiseMatrix( TMatrixD STTKFKalmanFilterManager::GetMeasurementNoiseMatrix() { TMatrixD measurementNoiseMatrix(2, 2); - // measurementNoiseMatrix[0][0] = SANDTrackerUtils::GetSigmaPositionMeasurement() * - // SANDTrackerUtils::GetSigmaPositionMeasurement(); - // measurementNoiseMatrix[1][1] = SANDTrackerUtils::GetSigmaAngleMeasurement() * - // SANDTrackerUtils::GetSigmaAngleMeasurement(); + measurementNoiseMatrix[0][0] = SANDTrackerUtils::GetSigmaPositionMeasurement() * + SANDTrackerUtils::GetSigmaPositionMeasurement(); + measurementNoiseMatrix[1][1] = SANDTrackerUtils::GetSigmaAngleMeasurement() * + SANDTrackerUtils::GetSigmaAngleMeasurement(); return measurementNoiseMatrix; } @@ -320,87 +319,61 @@ STTKFStateCovarianceMatrix STTKFKalmanFilterManager::FilterCovMatrix( return TMatrixD(TMatrixD::kInverted, nextCovarianceMatrixInverted); } -// STTKFStateVector STTKFKalmanFilterManager::smoothState( -// const STTKFStateVector& stateVectorFiltered, -// const STTKFStateVector& stateVectorPreviousSmoothed, -// const STTKFStateVector& stateVectorPreviousPredicted, -// const TMatrixD& theAMatrix) -// { -// return STTKFStateVector(stateVectorFiltered() + -// theAMatrix * (stateVectorPreviousSmoothed() - -// stateVectorPreviousPredicted())); -// } +STTKFStateVector STTKFKalmanFilterManager::smoothState( + const STTKFStateVector& stateVectorFiltered, + const STTKFStateVector& stateVectorPreviousSmoothed, + const STTKFStateVector& stateVectorPreviousPredicted, + const TMatrixD& theAMatrix) +{ + return STTKFStateVector(stateVectorFiltered() + + theAMatrix * (stateVectorPreviousSmoothed() - + stateVectorPreviousPredicted())); +} -// STTKFStateCovarianceMatrix STTKFKalmanFilterManager::smoothCovMatrix( -// const TMatrixD& covarianceMatrixFiltered, -// const TMatrixD& covarianceMatrixPreviousSmoothed, -// const TMatrixD& covarianceMatrixPreviousPredicted, -// const TMatrixD& theAMatrix) -// { -// TMatrixD theAMatrixTransposed(TMatrixD::kTransposed, theAMatrix); -// return covarianceMatrixFiltered + theAMatrix * -// (covarianceMatrixPreviousSmoothed - -// covarianceMatrixPreviousPredicted) * -// theAMatrixTransposed; -// } +STTKFStateCovarianceMatrix STTKFKalmanFilterManager::smoothCovMatrix( + const TMatrixD& covarianceMatrixFiltered, + const TMatrixD& covarianceMatrixPreviousSmoothed, + const TMatrixD& covarianceMatrixPreviousPredicted, + const TMatrixD& theAMatrix) +{ + TMatrixD theAMatrixTransposed(TMatrixD::kTransposed, theAMatrix); + return covarianceMatrixFiltered + theAMatrix * + (covarianceMatrixPreviousSmoothed - + covarianceMatrixPreviousPredicted) * + theAMatrixTransposed; +} -// void STTKFKalmanFilterManager::Propagate(double dE, -// const STTPlaneID& nextPlaneID) -// { +void STTKFKalmanFilterManager::Propagate(double& dE, + double& dZ, + double& beta) +{ + auto currentState = fThisTrack.GetStep(fCurrentStep); + auto currentStage = + currentState.GetStage(STTKFTrackStep::STTKFTrackStateStage::kFiltering); -// // Notice: BETA = 1 INSTEAD OF REAL BETA - - - -// auto currentState = fThisTrack.GetStep(fCurrentStep); -// auto currentStage = -// currentState.GetStage(STTKFTrackStep::STTKFTrackStateStage::kFiltering); - -// auto currentPlaneID = currentState.GetPlaneID(); -// auto currentZ = STTStrawTubeTracker::GetZPlane(currentPlaneID); -// auto nextZ = STTStrawTubeTracker::GetZPlane(nextPlaneID); -// auto dZ = nextZ - currentZ; -// std::cout << "dZZZZ " << dZ << std::endl; -// auto previousStateVector = currentStage.GetStateVector(); -// auto predictedStateVector = PropagateState(previousStateVector, dZ, dE, 1); -// auto nextPhi = predictedStateVector.Phi(); -// auto processNoiseMatrix = -// GetProcessNoiseMatrix(previousStateVector, nextPhi, dZ, dE, nextZ, 1); -// STTTRACKRECO_LOG( -// "INFO", TString::Format("Process Noise Matrix: %s", -// SANDTrackerUtils::PrintMatrix(processNoiseMatrix).Data()) -// .Data()); - -// auto propagatorMatrix = -// GetPropagatorMatrix(previousStateVector, nextPhi, dZ, dE, 1); -// STTTRACKRECO_LOG( -// "INFO", TString::Format("Propagator Matrix: %s", -// SANDTrackerUtils::PrintMatrix(propagatorMatrix).Data()) -// .Data()); -// auto predictedCovMatrix = PropagateCovMatrix( -// currentStage.GetStateCovMatrix(), propagatorMatrix, processNoiseMatrix); - -// STTKFTrackStep predictedTrackState; -// predictedTrackState.SetPlaneID(nextPlaneID); -// predictedTrackState.SetStage( -// STTKFTrackStep::STTKFTrackStateStage::kPrediction, -// STTKFState(predictedStateVector, predictedCovMatrix)); -// fThisTrack.AddStep(predictedTrackState); - -// fCurrentStep++; -// fCurrentStage = STTKFTrackStep::STTKFTrackStateStage::kPrediction; - -// STTTRACKRECO_LOG( -// "INFO", -// TString::Format("State Vector : %s", -// SANDTrackerUtils::PrintStateVector(predictedStateVector).Data()) -// .Data()); -// STTTRACKRECO_LOG( -// "INFO", TString::Format("Covariance Matrix: %s", -// SANDTrackerUtils::PrintMatrix(predictedCovMatrix).Data()) -// .Data()); -// } + auto currentStateVector = currentStage.GetStateVector(); + auto predictedStateVector = PropagateState(currentStateVector, dZ, dE, particleInfo_.mass); + auto nextPhi = predictedStateVector.Phi(); + + auto processNoiseMatrix = + GetProcessNoiseMatrix(currentStateVector, nextPhi, dZ, dE, fCurrentZ, particleInfo_.mass); + auto propagatorMatrix = + GetPropagatorMatrix(currentStateVector, nextPhi, dZ, dE, particleInfo_.mass); + auto predictedCovMatrix = PropagateCovMatrix( + currentStage.GetStateCovMatrix(), propagatorMatrix, processNoiseMatrix); + + STTKFTrackStep predictedTrackState; + predictedTrackState.SetStage( + STTKFTrackStep::STTKFTrackStateStage::kPrediction, + STTKFState(predictedStateVector, predictedCovMatrix)); + predictedTrackState.SetPropagatorMatrix(propagatorMatrix); + + fThisTrack.AddStep(predictedTrackState); + + fCurrentStep++; + fCurrentStage = STTKFTrackStep::STTKFTrackStateStage::kPrediction; +} double STTKFKalmanFilterManager::EvalChi2( const STTKFMeasurement& observation, const STTKFMeasurement& prediction, @@ -415,361 +388,301 @@ double STTKFKalmanFilterManager::EvalChi2( return chi2Matrix[0][0]; } -// int STTKFKalmanFilterManager::FindBestMatch( -// const std::vector& clusterIDs, const STTKFMeasurement& prediction, -// const TMatrixD& measurementNoiseMatrix) -// { -// // criterio per decidere se รจ buono o no -// std::vector chi2Values; -// for (auto clusterID : clusterIDs) { -// auto measurement = GetMeasurementFromCluster(clusterID); +STTKFMeasurement STTKFKalmanFilterManager::GetMeasurementFromTracklet(const TVectorD& tracklet) +{ + STTKFMeasurement measurement(2, 1); + // To Do: vertical and horizontal are outdated and confusing. Replace with something more meaningful. + // Notice: vertical planes means horizontal measurements and the opposite + if (fCurrentOrientation == Orientation::kVertical) { + // To Do: Check units! + measurement[0][0] = tracklet[0] / 1000.; + measurement[1][0] = M_PI_2 - tracklet[2]; + } else { + measurement[0][0] = tracklet[1] / 1000.; + measurement[1][0] = tracklet[3]; + } -// chi2Values.push_back( -// EvalChi2(measurement, prediction, measurementNoiseMatrix)); -// } + return measurement; +} -// if (chi2Values.size() > 0) -// return clusterIDs.at(std::distance( -// chi2Values.begin(), std::min(chi2Values.begin(), chi2Values.end()))); -// else -// return -1; -// } +int STTKFKalmanFilterManager::FindBestMatch(double& nextZ, const STTKFMeasurement& prediction, + const TMatrixD& Sk) +{ + double best_chi = 1E9; + auto& next_tracklets = z_to_tracklets_->at(nextZ); + auto best_tracklet_index = -1; -// void STTKFKalmanFilterManager::Filter(const STTKFMeasurement& observation, -// const STTKFMeasurement& prediction, -// STTPlane::EOrientation orientation) -// { + for (int i = 0; i < next_tracklets.size(); i++) { + STTKFMeasurement measurement = GetMeasurementFromTracklet(next_tracklets[i]); -// auto currentState = fThisTrack.GetStep(fCurrentStep); -// auto predictedStage = -// currentState.GetStage(STTKFTrackStep::STTKFTrackStateStage::kPrediction); -// auto predictedStateVector = predictedStage.GetStateVector(); -// auto predictedCovMatrix = predictedStage.GetStateCovMatrix(); -// auto measurementNoiseMatrix = GetMeasurementNoiseMatrix(); -// auto projectionMatrix = -// GetProjectionMatrix(orientation, predictedStateVector); -// auto kalmanGainMatrix = GetKalmanGainMatrix( -// predictedCovMatrix, projectionMatrix, measurementNoiseMatrix); -// auto filteredStateVector = FilterState(predictedStateVector, kalmanGainMatrix, -// observation, prediction); -// auto filteredCovMatrix = FilterCovMatrix(predictedCovMatrix, projectionMatrix, -// measurementNoiseMatrix); - -// fThisTrack.SetStage(fCurrentStep, -// STTKFTrackStep::STTKFTrackStateStage::kFiltering, -// STTKFState(filteredStateVector, filteredCovMatrix)); -// fCurrentStage = STTKFTrackStep::STTKFTrackStateStage::kFiltering; - -// STTTRACKRECO_LOG( -// "INFO", TString::Format("Current Step : %d", fCurrentStep).Data()); -// STTTRACKRECO_LOG( -// "INFO", -// TString::Format("State Vector : %s", -// SANDTrackerUtils::PrintStateVector(filteredStateVector).Data()) -// .Data()); -// STTTRACKRECO_LOG( -// "INFO", -// TString::Format( -// "Check state Vector: %s", -// SANDTrackerUtils::PrintStateVector( -// fThisTrack.GetStep(fCurrentStep) -// .GetStage(STTKFTrackStep::STTKFTrackStateStage::kFiltering) -// .GetStateVector()) -// .Data()) -// .Data()); -// STTTRACKRECO_LOG( -// "INFO", TString::Format("Covariance Matrix : %s", -// SANDTrackerUtils::PrintMatrix(filteredCovMatrix).Data()) -// .Data()); -// } + auto chi2 = EvalChi2(measurement, prediction, Sk); + if (chi2 < best_chi) { + best_chi = chi2; + best_tracklet_index = i; + } + } + if (best_chi < 1.5) { + return best_tracklet_index; + } else { + return -1; + } +} -// void STTKFKalmanFilterManager::Smooth() -// { +void STTKFKalmanFilterManager::SetNextOrientation() +{ + if (fCurrentOrientation == Orientation::kVertical) { + fCurrentOrientation = Orientation::kHorizontal; + } else { + fCurrentOrientation = Orientation::kVertical; + } +} -// // Notice: BETA = 1 INSTEAD OF REAL BETA +void STTKFKalmanFilterManager::Filter(const STTKFMeasurement& measurement, + const STTKFMeasurement& prediction) +{ + auto currentState = fThisTrack.GetStep(fCurrentStep); + auto predictedStage = + currentState.GetStage(STTKFTrackStep::STTKFTrackStateStage::kPrediction); + auto predictedStateVector = predictedStage.GetStateVector(); + auto predictedCovMatrix = predictedStage.GetStateCovMatrix(); + + auto measurementNoiseMatrix = GetMeasurementNoiseMatrix(); + auto projectionMatrix = GetProjectionMatrix(fCurrentOrientation, predictedStateVector); + auto kalmanGainMatrix = GetKalmanGainMatrix( + predictedCovMatrix, projectionMatrix, measurementNoiseMatrix); + auto filteredStateVector = FilterState(predictedStateVector, kalmanGainMatrix, + measurement, prediction); + auto filteredCovMatrix = FilterCovMatrix(predictedCovMatrix, projectionMatrix, + measurementNoiseMatrix); + + fThisTrack.SetStage(fCurrentStep, + STTKFTrackStep::STTKFTrackStateStage::kFiltering, + STTKFState(filteredStateVector, filteredCovMatrix)); + fCurrentStage = STTKFTrackStep::STTKFTrackStateStage::kFiltering; + + SetNextOrientation(); +} +void STTKFKalmanFilterManager::Smooth() +{ + auto currentState = fThisTrack.GetStep(fCurrentStep); + auto filteredState = + currentState.GetStage(STTKFTrackStep::STTKFTrackStateStage::kFiltering); + auto filteredStateVector = filteredState.GetStateVector(); + auto filteredCovMatrix = filteredState.GetStateCovMatrix(); -// auto currentState = fThisTrack.GetStep(fCurrentStep); -// auto filteredState = -// currentState.GetStage(STTKFTrackStep::STTKFTrackStateStage::kFiltering); -// auto filteredStateVector = filteredState.GetStateVector(); -// auto filteredCovMatrix = filteredState.GetStateCovMatrix(); + if (fCurrentStep == int(fThisTrack.GetSteps().size()) - 1) { + fThisTrack.SetStage(fCurrentStep, + STTKFTrackStep::STTKFTrackStateStage::kSmoothing, + STTKFState(filteredStateVector, filteredCovMatrix)); + } else { + // previous state + auto previousState = fThisTrack.GetStep(fCurrentStep + 1); + + // previous smoothed + auto previousSmoothedState = previousState.GetStage( + STTKFTrackStep::STTKFTrackStateStage::kSmoothing); + auto previousSmoothedStateVector = previousSmoothedState.GetStateVector(); + auto previousSmoothedCovMatrix = previousSmoothedState.GetStateCovMatrix(); + + // previous predicted + auto previousPredictedState = previousState.GetStage( + STTKFTrackStep::STTKFTrackStateStage::kPrediction); + auto previousPredictedStateVector = previousPredictedState.GetStateVector(); + auto previousPredictedCovMatrix = + previousPredictedState.GetStateCovMatrix(); + + // current predicted + auto predictedState = currentState.GetStage( + STTKFTrackStep::STTKFTrackStateStage::kPrediction); + auto predictedStateVector = predictedState.GetStateVector(); + auto predictedCovMatrix = predictedState.GetStateCovMatrix(); + + auto nextPhi = previousPredictedStateVector.Phi(); + auto propagatorMatrix = currentState.GetPropagatorMatrix(); + + auto theAMatrix = GetAMatrix(filteredCovMatrix, previousPredictedCovMatrix, + propagatorMatrix); + + auto smoothedStateVector = + smoothState(filteredStateVector, previousSmoothedStateVector, + previousPredictedStateVector, theAMatrix); + auto smoothedCovMatrix = + smoothCovMatrix(filteredCovMatrix, previousSmoothedCovMatrix, + previousPredictedCovMatrix, theAMatrix); + + fThisTrack.SetStage(fCurrentStep, + STTKFTrackStep::STTKFTrackStateStage::kSmoothing, + STTKFState(smoothedStateVector, smoothedCovMatrix)); + // currentState.SetStage(STTKFTrackStep::STTKFTrackStateStage::kSmoothing, + // STTKFState(smoothedStateVector, smoothedCovMatrix)); -// if (fCurrentStep == int(fThisTrack.GetSteps().size()) - 1) { -// fThisTrack.SetStage(fCurrentStep, -// STTKFTrackStep::STTKFTrackStateStage::kSmoothing, -// STTKFState(filteredStateVector, filteredCovMatrix)); -// // currentState.SetStage(STTKFTrackStep::STTKFTrackStateStage::kSmoothing, -// // STTKFState(filteredStateVector, filteredCovMatrix)); + } + fCurrentStage = STTKFTrackStep::STTKFTrackStateStage::kFiltering; + fCurrentStep--; +} -// STTTRACKRECO_LOG( -// "INFO", -// TString::Format("State Vector : %s", -// SANDTrackerUtils::PrintStateVector(filteredStateVector).Data()) -// .Data()); -// STTTRACKRECO_LOG( -// "INFO", TString::Format("Covariance Matrix: %s", -// SANDTrackerUtils::PrintMatrix(filteredCovMatrix).Data()) -// .Data()); -// } else { -// // previous state -// auto previousState = fThisTrack.GetStep(fCurrentStep + 1); -// // previous smoothed -// auto previousSmoothedState = previousState.GetStage( -// STTKFTrackStep::STTKFTrackStateStage::kSmoothing); -// auto previousSmoothedStateVector = previousSmoothedState.GetStateVector(); -// auto previousSmoothedCovMatrix = previousSmoothedState.GetStateCovMatrix(); -// // previous predicted -// auto previousPredictedState = previousState.GetStage( -// STTKFTrackStep::STTKFTrackStateStage::kPrediction); -// auto previousPredictedStateVector = previousPredictedState.GetStateVector(); -// auto previousPredictedCovMatrix = -// previousPredictedState.GetStateCovMatrix(); - -// auto predictedState = currentState.GetStage( -// STTKFTrackStep::STTKFTrackStateStage::kPrediction); -// auto predictedStateVector = predictedState.GetStateVector(); -// auto predictedCovMatrix = predictedState.GetStateCovMatrix(); - -// auto nextPhi = previousPredictedStateVector.Phi(); -// auto nextZ = STTStrawTubeTracker::GetZPlane(previousState.GetPlaneID()); -// auto currentZ = STTStrawTubeTracker::GetZPlane(currentState.GetPlaneID()); -// auto dZ = nextZ - currentZ; -// auto dir = -1. * GetDirectiveCosinesFromStateVector(predictedStateVector); -// auto dE = SANDTrackerUtils::GetDEInMeV(STTKFGeoManager::GetCrossedMaterialInGCM2( -// nextZ, predictedStateVector.X(), predictedStateVector.Y(), currentZ, -// dir.X(), dir.Y(), dir.Z())); - -// auto propagatorMatrix = -// GetPropagatorMatrix(predictedStateVector, nextPhi, dZ, dE, 1); -// auto theAMatrix = GetAMatrix(filteredCovMatrix, previousPredictedCovMatrix, -// propagatorMatrix); - -// auto smoothedStateVector = -// smoothState(filteredStateVector, previousSmoothedStateVector, -// previousPredictedStateVector, theAMatrix); -// auto smoothedCovMatrix = -// smoothCovMatrix(filteredCovMatrix, previousSmoothedCovMatrix, -// previousPredictedCovMatrix, theAMatrix); - -// fThisTrack.SetStage(fCurrentStep, -// STTKFTrackStep::STTKFTrackStateStage::kSmoothing, -// STTKFState(smoothedStateVector, smoothedCovMatrix)); -// // currentState.SetStage(STTKFTrackStep::STTKFTrackStateStage::kSmoothing, -// // STTKFState(smoothedStateVector, smoothedCovMatrix)); - -// STTTRACKRECO_LOG( -// "INFO", -// TString::Format("State Vector : %s", -// SANDTrackerUtils::PrintStateVector(smoothedStateVector).Data()) -// .Data()); -// STTTRACKRECO_LOG( -// "INFO", TString::Format("Covariance Matrix: %s", -// SANDTrackerUtils::PrintMatrix(smoothedCovMatrix).Data()) -// .Data()); -// } -// fCurrentStage = STTKFTrackStep::STTKFTrackStateStage::kFiltering; -// fCurrentStep--; -// } +void STTKFKalmanFilterManager::InitFromMC(TrackletMap* z_to_tracklets, const SParticleInfo& particleInfo) +{ -// void STTKFKalmanFilterManager::Init(const STTPlaneID& planeID, int clusterID) -// { + TMatrixD initial_cov_matrix(5, 5); + initial_cov_matrix[0][0] = pow(200E-6, 2); + initial_cov_matrix[1][1] = pow(200E-6, 2); + initial_cov_matrix[2][2] = pow(0.1, 2); + initial_cov_matrix[3][3] = pow(0.01, 2); + initial_cov_matrix[4][4] = pow(0.01, 2); -// auto cluster = STTKFClusterManager::GetCluster(clusterID); -// auto trkParameter = cluster.GetRecoParameters().at(0).trk; - -// double x, y, invR, tanL, phi; -// auto plane = STTStrawTubeTracker::GetPlane(planeID); -// auto defaultCharge = -1; -// auto planeOrientation = plane.GetOrientation(); - -// if (planeOrientation == STTPlane::EOrientation::kHorizontal) { -// x = SANDTrackerUtils::GetSANDInnerVolumeCenterPosition()[0]; -// y = trkParameter.m * plane.GetZ() + trkParameter.q; -// invR = defaultCharge / -// SANDTrackerUtils::GetRadiusInMMFromPerpMomentumInGeV(1. /*GeV*/); -// tanL = 0.; -// phi = GetPhiFromTheta(atan(trkParameter.m), defaultCharge); -// } else { -// x = trkParameter.m * plane.GetZ() + trkParameter.q; -// ; -// y = SANDTrackerUtils::GetSANDInnerVolumeCenterPosition()[1]; -// invR = defaultCharge / -// SANDTrackerUtils::GetRadiusInMMFromPerpMomentumInGeV(1. /*GeV*/); -// tanL = trkParameter.m; -// phi = 0.5 * TMath::Pi(); -// } + STTKFStateVector initial_state_vector = STTKFCheck::get_state_vector(particleInfo.mom * 1E-3, // GeV + particleInfo.pos * 1E-3, // m + particleInfo.charge); -// STTKFTrackStep trackStep; -// trackStep.SetPlaneID(planeID); -// trackStep.SetClusterIDForThisState(clusterID); -// STTKFStateVector stateVector(x, y, invR, tanL, phi); - -// auto initialCovMatrix = GetInitialCovMatrix(stateVector, planeOrientation); - -// trackStep.SetStage(STTKFTrackStep::STTKFTrackStateStage::kPrediction, -// STTKFState(stateVector, initialCovMatrix)); -// trackStep.SetStage(STTKFTrackStep::STTKFTrackStateStage::kFiltering, -// STTKFState(stateVector, initialCovMatrix)); -// fThisTrack.AddStep(trackStep); - -// fCurrentStage = STTKFTrackStep::STTKFTrackStateStage::kFiltering; -// fCurrentStep = 0u; - -// STTTRACKRECO_LOG( -// "INFO", TString::Format("State Vector : %s", -// SANDTrackerUtils::PrintStateVector(stateVector).Data()) -// .Data()); -// STTTRACKRECO_LOG( -// "INFO", TString::Format("Covariance Matrix: %s", -// SANDTrackerUtils::PrintMatrix(initialCovMatrix).Data()) -// .Data()); -// } + STTKFTrackStep trackStep; + trackStep.SetStage(STTKFTrackStep::STTKFTrackStateStage::kPrediction, + STTKFState(initial_state_vector, initial_cov_matrix)); + trackStep.SetStage(STTKFTrackStep::STTKFTrackStateStage::kFiltering, + STTKFState(initial_state_vector, initial_cov_matrix)); -// void STTKFKalmanFilterManager::Run() -// { -// // criterio per quando fermare la ricerca - -// int stepLength = 1; - -// auto currentPlaneID = fThisTrack.GetStep(fCurrentStep).GetPlaneID(); -// auto currentPlaneIndex = STTStrawTubeTracker::GetPlaneIndex(currentPlaneID); - -// while (stepLength < 100 && currentPlaneIndex - stepLength >= 0) { -// // 1- propagate to [currentPlaneID - step] -// auto nextPlane = STTStrawTubeTracker::Prev(currentPlaneID, stepLength); -// auto nextPlaneID = nextPlane->GetId(); - -// auto currentZ = STTStrawTubeTracker::GetZPlane(currentPlaneID); -// auto nextZ = nextPlane->GetZ(); - -// auto currentStep = fThisTrack.GetStep(fCurrentStep); -// auto filteredStateVector = -// currentStep.GetStage(STTKFTrackStep::STTKFTrackStateStage::kFiltering) -// .GetStateVector(); - -// STTTRACKRECO_LOG("INFO", -// TString::Format("Current Step: %d", fCurrentStep).Data()); -// STTTRACKRECO_LOG( -// "INFO", -// TString::Format( -// "Filtered State Vector: %s", -// SANDTrackerUtils::PrintStateVector( -// fThisTrack.GetStep(fCurrentStep) -// .GetStage(STTKFTrackStep::STTKFTrackStateStage::kFiltering) -// .GetStateVector()) -// .Data()) -// .Data()); - -// auto dir = -1. * GetDirectiveCosinesFromStateVector(filteredStateVector); -// auto dL = STTKFGeoManager::GetCrossedMaterialInGCM2( -// nextZ, filteredStateVector.X(), filteredStateVector.Y(), currentZ, -// dir.X(), dir.Y(), dir.Z()); -// auto dE = SANDTrackerUtils::GetDEInGeV(dL); - -// STTTRACKRECO_LOG( -// "INFO", TString::Format( -// "Expected dE: %f (GeV) for dL: %f (g/cm2) from: (%f, %f, " -// "%f) with direction: (%f, %f, %f) to z: %f", -// dE, dL, filteredStateVector.X(), filteredStateVector.Y(), -// currentZ, dir.X(), dir.Y(), dir.Z(), nextZ) -// .Data()); -// STTTRACKRECO_LOG( -// "INFO", -// TString::Format( -// "Propagate for %d step(s) from plane %u [Z=%f] to plane %u [z=%f]", -// stepLength, currentPlaneID(), currentZ, nextPlaneID(), nextZ) -// .Data()); - -// Propagate(dE, nextPlaneID); - -// // 2- Search best match -// auto clusterIDs = -// STTKFClusterManager::GetClusterIDsForPlane(nextPlane->GetIndex()); -// auto predictionStateVector = -// fThisTrack.GetStep(fCurrentStep) -// .GetStage(STTKFTrackStep::STTKFTrackStateStage::kPrediction) -// .GetStateVector(); -// auto prediction = -// GetPrediction(nextPlane->GetOrientation(), predictionStateVector); - -// STTTRACKRECO_LOG("INFO", -// TString::Format("Prediction: %s", -// SANDTrackerUtils::PrintMatrix(prediction).Data()) -// .Data()); - -// auto measurementNoiseMatrix = GetMeasurementNoiseMatrix(); -// auto clusterID = -// FindBestMatch(clusterIDs, prediction, measurementNoiseMatrix); - -// // 3- If it is found: step = 1 -// // else step++ -// if (clusterID >= 0) { -// fThisTrack.SetClusterIDForState(fCurrentStep, clusterID); -// auto cluster = STTKFClusterManager::GetCluster(clusterID); -// auto trk = cluster.GetRecoParameters().front().trk; -// STTTRACKRECO_LOG( -// "INFO", -// TString::Format( -// "Cluster with ID: %d (m: %f, q: %f, x: %f, angle: %f) is the " -// "best cluster in plane %u [z=%f] orient: %s", -// clusterID, trk.m, trk.q, trk.m * cluster.GetZ() + trk.q, -// atan(trk.m), nextPlaneID(), nextZ, -// cluster.GetOrientation() == STTPlane::EOrientation::kHorizontal -// ? "hor" -// : "ver") -// .Data()); -// stepLength = 1; -// auto observation = GetMeasurementFromCluster(clusterID); -// Filter(observation, prediction, nextPlane->GetOrientation()); -// } else { -// STTTRACKRECO_LOG( -// "INFO", TString::Format("No compatible cluster in plane %u [z=%f]", -// nextPlaneID(), nextZ) -// .Data()); -// stepLength++; -// fThisTrack.RemoveLastStep(); -// fCurrentStep--; -// fCurrentStage = STTKFTrackStep::STTKFTrackStateStage::kFiltering; -// } - -// currentPlaneID = fThisTrack.GetStep(fCurrentStep).GetPlaneID(); -// currentPlaneIndex = STTStrawTubeTracker::GetPlaneIndex(currentPlaneID); -// } -// STTTRACKRECO_LOG("INFO", "Doing smoothing"); -// while (fCurrentStep >= 0) Smooth(); - -// auto lastStep = fThisTrack.GetSteps().back(); -// auto lastZ = STTStrawTubeTracker::GetPlane(lastStep.GetPlaneID()).GetZ(); -// auto lastStateVector = -// lastStep.GetStage(STTKFTrackStep::STTKFTrackStateStage::kSmoothing) -// .GetStateVector(); -// auto pPerp = -// SANDTrackerUtils::GetPerpMomentumInGeVFromRadiusInMM(lastStateVector.Radius()); -// auto pX = pPerp * lastStateVector.TanLambda(); -// auto pY = pPerp * sin(lastStateVector.Phi()); -// auto pZ = pPerp * cos(lastStateVector.Phi()); - -// STTTRACKRECO_LOG( -// "INFO", -// TString::Format("Last State Vector: %s", -// SANDTrackerUtils::PrintStateVector(lastStateVector).Data()) -// .Data()); -// STTTRACKRECO_LOG( -// "INFO", TString::Format("Last Reco Parameter at Z=%f", lastZ).Data()); -// STTTRACKRECO_LOG("INFO", -// TString::Format(" X: %f", lastStateVector.X()).Data()); -// STTTRACKRECO_LOG("INFO", -// TString::Format(" Y: %f", lastStateVector.Y()).Data()); -// STTTRACKRECO_LOG("INFO", TString::Format(" PX: %f", pX).Data()); -// STTTRACKRECO_LOG("INFO", TString::Format(" PY: %f", pY).Data()); -// STTTRACKRECO_LOG("INFO", TString::Format(" PZ: %f", pZ).Data()); -// STTTRACKRECO_LOG( -// "INFO", -// TString::Format(" Charge: %d", lastStateVector.Charge()).Data()); - -// STTTRACKRECO_LOG("INFO", "End of KalmanFilter Run"); + trackStep.SetPropagatorMatrix(initial_cov_matrix); + + fThisTrack.AddStep(trackStep); + + particleInfo_ = particleInfo; + z_to_tracklets_ = z_to_tracklets; + fCurrentStage = STTKFTrackStep::STTKFTrackStateStage::kFiltering; + fCurrentStep = 0u; + fCurrentZ = particleInfo.pos.Z(); //Notice: UNITS!! mm, why? + fCurrentOrientation = Orientation::kVertical; +} + +// To Do: implment a seeding algorithm +// void STTKFKalmanFilterManager::Init(const STTPlaneID& planeID, int clusterID) + // { + // auto cluster = STTKFClusterManager::GetCluster(clusterID); + // auto trkParameter = cluster.GetRecoParameters().at(0).trk; + + // double x, y, invR, tanL, phi; + // auto plane = STTStrawTubeTracker::GetPlane(planeID); + // auto defaultCharge = -1; + // auto planeOrientation = plane.GetOrientation(); + + // if (planeOrientation == STTPlane::EOrientation::kHorizontal) { + // x = SANDTrackerUtils::GetSANDInnerVolumeCenterPosition()[0]; + // y = trkParameter.m * plane.GetZ() + trkParameter.q; + // invR = defaultCharge / + // SANDTrackerUtils::GetRadiusInMMFromPerpMomentumInGeV(1. /*GeV*/); + // tanL = 0.; + // phi = GetPhiFromTheta(atan(trkParameter.m), defaultCharge); + // } else { + // x = trkParameter.m * plane.GetZ() + trkParameter.q; + // ; + // y = SANDTrackerUtils::GetSANDInnerVolumeCenterPosition()[1]; + // invR = defaultCharge / + // SANDTrackerUtils::GetRadiusInMMFromPerpMomentumInGeV(1. /*GeV*/); + // tanL = trkParameter.m; + // phi = 0.5 * TMath::Pi(); + // } + + // STTKFTrackStep trackStep; + // trackStep.SetPlaneID(planeID); + // trackStep.SetClusterIDForThisState(clusterID); + // STTKFStateVector stateVector(x, y, invR, tanL, phi); + + // auto initialCovMatrix = GetInitialCovMatrix(stateVector, planeOrientation); + + // trackStep.SetStage(STTKFTrackStep::STTKFTrackStateStage::kPrediction, + // STTKFState(stateVector, initialCovMatrix)); + // trackStep.SetStage(STTKFTrackStep::STTKFTrackStateStage::kFiltering, + // STTKFState(stateVector, initialCovMatrix)); + // fThisTrack.AddStep(trackStep); + + // fCurrentStage = STTKFTrackStep::STTKFTrackStateStage::kFiltering; + // fCurrentStep = 0u; + + // STTTRACKRECO_LOG( + // "INFO", TString::Format("State Vector : %s", + // SANDTrackerUtils::PrintStateVector(stateVector).Data()) + // .Data()); + // STTTRACKRECO_LOG( + // "INFO", TString::Format("Covariance Matrix: %s", + // SANDTrackerUtils::PrintMatrix(initialCovMatrix).Data()) + // .Data()); // } + +void STTKFKalmanFilterManager::Run() +{ + // criterio per quando fermare la ricerca + int stepLength = 1; + + // Notice: if currentZ is not in the map, the second condition is always true + while (stepLength < 100 && std::distance(z_to_tracklets_->begin(), z_to_tracklets_->find(fCurrentZ)) >= stepLength) { + // 1- propagate to [currentPlaneID - step] + auto nextZ = std::prev(z_to_tracklets_->lower_bound(fCurrentZ), stepLength)->first; + + auto currentStep = fThisTrack.GetStep(fCurrentStep); + auto filteredStateVector = + currentStep.GetStage(STTKFTrackStep::STTKFTrackStateStage::kFiltering) + .GetStateVector(); + + auto dir = -1. * GetDirectiveCosinesFromStateVector(filteredStateVector); + + // To Do: check if this is still valid and add a real fix if needed + if (dir.Z() > 0) { + dir *= -1; + } + + auto current_mom = SANDTrackerUtils::GetMomentumInMeVFromRadiusInMM( + filteredStateVector.Radius(), + filteredStateVector.TanLambda()) / 1000; + double gamma = sqrt(current_mom * current_mom + particleInfo_.mass * particleInfo_.mass) / + particleInfo_.mass; + double beta = sqrt(1 - pow(1 / gamma, 2)); + + // To Do: check all units + auto dE = STTKFGeoManager::GetDE( + nextZ, + 1000 * filteredStateVector.X(), 1000 * filteredStateVector.Y(), fCurrentZ, + dir.X(), dir.Y(), dir.Z(), + beta, particleInfo_.mass, particleInfo_.charge) / 1000; + + double dZ = (nextZ - fCurrentZ) / 1000; + + Propagate(dE, dZ, beta); + + // 2- Search best match + auto predictionStateVector = fThisTrack.GetStep(fCurrentStep) + .GetStage(STTKFTrackStep::STTKFTrackStateStage::kPrediction).GetStateVector(); + auto predictionStateCovMatrix = fThisTrack.GetStep(fCurrentStep).GetStage(STTKFTrackStep::STTKFTrackStateStage::kPrediction) + .GetStateCovMatrix(); + auto prediction = GetPrediction(fCurrentOrientation, predictionStateVector); + + auto measurementNoiseMatrix = GetMeasurementNoiseMatrix(); + auto projectionMatrix = GetProjectionMatrix(fCurrentOrientation, + predictionStateVector); + TMatrixD projectionMatrixTransposed(TMatrixD::kTransposed, + projectionMatrix); + auto Sk = measurementNoiseMatrix + projectionMatrix * + predictionStateCovMatrix * + projectionMatrixTransposed; + + int tracklet_index = FindBestMatch(nextZ, prediction, Sk); + + // // 3- If it is found: step = 1 + // // else step++ + if (tracklet_index != -1) { + stepLength = 1; + auto measurement = GetMeasurementFromTracklet(z_to_tracklets_->at(nextZ)[tracklet_index]); + Filter(measurement, prediction); + fCurrentZ = nextZ; + } else { + stepLength++; + fThisTrack.RemoveLastStep(); + fCurrentStep--; + fCurrentStage = STTKFTrackStep::STTKFTrackStateStage::kFiltering; + } + } + + while (fCurrentStep >= 0) Smooth(); +}