#include <SingleTrajectoryUpdator.h>

Inheritance diagram for SingleTrajectoryUpdator:

Public Member Functions
virtual SingleTrajectoryUpdator *	clone (void) const
virtual void	process (const ReferenceTrajectoryPtr &trajectory, AlignmentParameterStore store, AlignableNavigator navigator, KalmanAlignmentMetricsUpdator metrics, const MagneticField magField=0)
	Calculate the improved estimate.
	SingleTrajectoryUpdator (const edm::ParameterSet &config)
virtual	~SingleTrajectoryUpdator (void)
Private Member Functions
bool	checkCovariance (const AlgebraicSymMatrix &cov) const
Private Attributes
bool	theCovCheckFlag
double	theExternalPredictionWeight
double	theExtraWeight
unsigned int	theMinNumberOfHits
unsigned int	theNumberOfPreAlignmentEvts
unsigned int	theNumberOfProcessedEvts

Detailed Description

Definition at line 15 of file SingleTrajectoryUpdator.h.

Constructor & Destructor Documentation

SingleTrajectoryUpdator::SingleTrajectoryUpdator ( const edm::ParameterSet & config )

Definition at line 19 of file SingleTrajectoryUpdator.cc.

References gather_cfg::cout, edm::ParameterSet::getParameter(), theCovCheckFlag, theExternalPredictionWeight, theExtraWeight, theMinNumberOfHits, theNumberOfPreAlignmentEvts, and theNumberOfProcessedEvts.

Referenced by clone().

                                                                                 :
  KalmanAlignmentUpdator( config )
{
  theMinNumberOfHits = config.getParameter< unsigned int >( "MinNumberOfHits" );
  theExtraWeight = config.getParameter< double >( "ExtraWeight" );
  theExternalPredictionWeight = config.getParameter< double >( "ExternalPredictionWeight" );
  theCovCheckFlag = config.getParameter< bool >( "CheckCovariance" );

  theNumberOfPreAlignmentEvts = config.getParameter< unsigned int >( "NumberOfPreAlignmentEvts" );
  theNumberOfProcessedEvts = 0;

  std::cout << "[SingleTrajectoryUpdator] Use " << theNumberOfPreAlignmentEvts << "events for pre-alignment" << std::endl;
}

SingleTrajectoryUpdator::~SingleTrajectoryUpdator ( void ) [virtual]

Definition at line 34 of file SingleTrajectoryUpdator.cc.

{}

Member Function Documentation

bool SingleTrajectoryUpdator::checkCovariance ( const AlgebraicSymMatrix & cov ) const [private]

Definition at line 211 of file SingleTrajectoryUpdator.cc.

References i.

Referenced by process().

{
  for ( int i = 0; i < cov.num_row(); ++i )
  {
    if ( cov[i][i] < 0. ) return false;
  }

  return true;
}

virtual SingleTrajectoryUpdator* SingleTrajectoryUpdator::clone ( void ) const [inline, virtual]

Implements KalmanAlignmentUpdator.

Definition at line 30 of file SingleTrajectoryUpdator.h.

References SingleTrajectoryUpdator().

{ return new SingleTrajectoryUpdator( *this ); }

void SingleTrajectoryUpdator::process	(	const ReferenceTrajectoryPtr &	trajectory,
		AlignmentParameterStore *	store,
		AlignableNavigator *	navigator,
		KalmanAlignmentMetricsUpdator *	metrics,
		const MagneticField *	magField = `0`
	)		`[virtual]`

Calculate the improved estimate.

Implements KalmanAlignmentUpdator.

Definition at line 37 of file SingleTrajectoryUpdator.cc.

References KalmanAlignmentMetricsUpdator::additionalAlignables(), KalmanAlignmentUpdator::alignablesFromAlignableDets(), AlignableNavigator::alignablesFromHits(), checkCovariance(), CompositeAlignmentParameters::clone(), CompositeAlignmentParameters::components(), CompositeAlignmentDerivativesExtractor::correctionTerm(), gather_cfg::cout, CompositeAlignmentParameters::covarianceSubset(), Exception, KalmanAlignmentDataCollector::fillGraph(), i, KalmanAlignmentUpdator::nDifferentAlignables(), AlignmentParameterStore::numCorrelations(), CompositeAlignmentParameters::parameters(), AlignmentParameterStore::selectParameters(), theExternalPredictionWeight, theExtraWeight, theMinNumberOfHits, theNumberOfPreAlignmentEvts, theNumberOfProcessedEvts, KalmanAlignmentMetricsUpdator::update(), AlignmentParameterStore::updateParameters(), and KalmanAlignmentUpdator::updateUserVariables().

{
  if ( !( *trajectory ).isValid() ) return;

//   std::cout << "[SingleTrajectoryUpdator::process] START" << std::endl;

  vector< AlignableDetOrUnitPtr > currentAlignableDets = navigator->alignablesFromHits( ( *trajectory ).recHits() );
  vector< Alignable* > currentAlignables = alignablesFromAlignableDets( currentAlignableDets, store );

  if ( nDifferentAlignables( currentAlignables ) < 2 ) return;
  if ( currentAlignables.size() < theMinNumberOfHits ) return;

  ++theNumberOfProcessedEvts;
  bool includeCorrelations = ( theNumberOfPreAlignmentEvts < theNumberOfProcessedEvts );
  
  metrics->update( currentAlignables );

  vector< Alignable* > additionalAlignables;
  if ( includeCorrelations ) additionalAlignables = metrics->additionalAlignables( currentAlignables );

  vector< Alignable* > allAlignables;
  allAlignables.reserve( currentAlignables.size() + additionalAlignables.size() );
  allAlignables.insert( allAlignables.end(), currentAlignables.begin(), currentAlignables.end() );
  allAlignables.insert( allAlignables.end(), additionalAlignables.begin(), additionalAlignables.end() );

  CompositeAlignmentParameters alignmentParameters = store->selectParameters( allAlignables );

  const AlgebraicVector& allAlignmentParameters = alignmentParameters.parameters();
  AlgebraicSymMatrix currentAlignmentCov = alignmentParameters.covarianceSubset( currentAlignables );
  AlgebraicSymMatrix additionalAlignmentCov = alignmentParameters.covarianceSubset( additionalAlignables );
  AlgebraicMatrix mixedAlignmentCov = alignmentParameters.covarianceSubset( additionalAlignables, currentAlignables );
  AlgebraicMatrix alignmentCovSubset = alignmentParameters.covarianceSubset( alignmentParameters.components(), currentAlignables );

  CompositeAlignmentDerivativesExtractor extractor( currentAlignables, currentAlignableDets, trajectory->trajectoryStates() );
  AlgebraicVector correctionTerm = extractor.correctionTerm();
  AlgebraicMatrix alignmentDeriv = extractor.derivatives();
  AlgebraicSymMatrix alignmentCov = currentAlignmentCov.similarity( alignmentDeriv );

  AlgebraicVector allMeasurements = trajectory->measurements();
  AlgebraicSymMatrix measurementCov = trajectory->measurementErrors();
  AlgebraicVector referenceTrajectory = trajectory->trajectoryPositions();
  AlgebraicMatrix derivatives = trajectory->derivatives();

  measurementCov += theExtraWeight*AlgebraicSymMatrix( measurementCov.num_row(), 1 );

  AlgebraicSymMatrix misalignedCov = measurementCov + alignmentCov;

  int checkInversion = 0;

  AlgebraicSymMatrix weightMatrix;
  AlgebraicVector residuals;

  if ( trajectory->parameterErrorsAvailable() ) // Make an update using an external prediction for the track parameters.
  {
    const AlgebraicSymMatrix& externalParamCov = trajectory->parameterErrors();
    AlgebraicSymMatrix externalTrackCov = theExternalPredictionWeight*externalParamCov.similarity( derivatives );
    AlgebraicSymMatrix fullCov = misalignedCov + externalTrackCov;
    measurementCov += externalTrackCov;

    weightMatrix = fullCov.inverse( checkInversion );
    if ( checkInversion != 0 )
    {
      cout << "[KalmanAlignment] WARNING: 'AlgebraicSymMatrix fullCov' not invertible." << endl;
      return;
    }

    //const AlgebraicVector& trackParameters = trajectory->parameters();
    //const AlgebraicVector& externalTrackParameters = trajectory->externalPrediction();
    //AlgebraicVector trackCorrectionTerm = derivatives*( externalTrackParameters - trackParameters );
    residuals = allMeasurements - referenceTrajectory - correctionTerm;// - trackCorrectionTerm;
  }
  else // No external prediction for the track parameters available --> give the track parameters weight 0.
  {
    AlgebraicSymMatrix invMisalignedCov = misalignedCov.inverse( checkInversion );
    if ( checkInversion != 0 )
    {
      cout << "[KalmanAlignment] WARNING: 'AlgebraicSymMatrix misalignedCov' not invertible." << endl;
      return;
    }
    AlgebraicSymMatrix weightMatrix1 = ( invMisalignedCov.similarityT( derivatives ) ).inverse( checkInversion );
    if ( checkInversion != 0 )
    {
      cout << "[KalmanAlignment] WARNING: 'AlgebraicSymMatrix weightMatrix1' not computed." << endl;
      return;
    }
    AlgebraicSymMatrix weightMatrix2 = weightMatrix1.similarity( invMisalignedCov*derivatives );

    weightMatrix = invMisalignedCov - weightMatrix2;
    residuals = allMeasurements - referenceTrajectory - correctionTerm;
  }

//   AlgebraicVector deltaR = allMeasurements - referenceTrajectory;
//   for ( int i = 0; i < deltaR.num_row()/2; ++i )
//     KalmanAlignmentDataCollector::fillHistogram( "DeltaR_", i, deltaR[2*i] );
//   return;

  AlgebraicMatrix fullCovTimesDeriv = alignmentCovSubset*alignmentDeriv.T();
  AlgebraicMatrix fullGainMatrix = fullCovTimesDeriv*weightMatrix;
  AlgebraicMatrix covTimesDeriv = currentAlignmentCov*alignmentDeriv.T();
  AlgebraicMatrix gainMatrix = covTimesDeriv*weightMatrix;

  // make updates for the kalman-filter
  // update of parameters
  AlgebraicVector updatedAlignmentParameters = allAlignmentParameters + fullGainMatrix*residuals;

  // update of covariance
  int nCRow = currentAlignmentCov.num_row();
  int nARow = additionalAlignmentCov.num_row();

  AlgebraicSymMatrix updatedAlignmentCov( nCRow + nARow );

  AlgebraicMatrix gTimesDeriv = weightMatrix*alignmentDeriv;
  AlgebraicMatrix simMat = AlgebraicMatrix( nCRow, nCRow, 1 ) - covTimesDeriv*gTimesDeriv;
  AlgebraicSymMatrix updatedCurrentAlignmentCov = currentAlignmentCov.similarity( simMat ) + measurementCov.similarity( gainMatrix );

  AlgebraicMatrix mixedUpdateMat = simMat.T()*simMat.T() + measurementCov.similarity( gTimesDeriv.T() )*currentAlignmentCov;
  AlgebraicMatrix updatedMixedAlignmentCov = mixedAlignmentCov*mixedUpdateMat;

  AlgebraicSymMatrix additionalUpdateMat = misalignedCov.similarity( gTimesDeriv.T() ) - 2.*weightMatrix.similarity( alignmentDeriv.T() );
  AlgebraicSymMatrix updatedAdditionalAlignmentCov = additionalAlignmentCov + additionalUpdateMat.similarity( mixedAlignmentCov );

  for ( int nRow=0; nRow<nCRow; nRow++ )
  {
    for ( int nCol=0; nCol<=nRow; nCol++ ) updatedAlignmentCov[nRow][nCol] = updatedCurrentAlignmentCov[nRow][nCol];
  }

  for ( int nRow=0; nRow<nARow; nRow++ )
  {
     for ( int nCol=0; nCol<=nRow; nCol++ ) updatedAlignmentCov[nRow+nCRow][nCol+nCRow] = updatedAdditionalAlignmentCov[nRow][nCol];
  }

  for ( int nRow=0; nRow<nARow; nRow++ )
  {
    for ( int nCol=0; nCol<nCRow; nCol++ ) updatedAlignmentCov[nRow+nCRow][nCol] = updatedMixedAlignmentCov[nRow][nCol];
  }


  // update in alignment-interface
  CompositeAlignmentParameters* updatedParameters;
  updatedParameters = alignmentParameters.clone( updatedAlignmentParameters, updatedAlignmentCov );

  if ( !checkCovariance( updatedAlignmentCov ) )
  {
    if ( includeCorrelations ) throw cms::Exception( "BadCovariance" );

    delete updatedParameters;
    return;
  }

  store->updateParameters( *updatedParameters, includeCorrelations );
  delete updatedParameters;


  // update user variables for debugging
  //updateUserVariables( alignmentParameters.components() );

  //std::cout << "update user variables now" << std::endl;
  updateUserVariables( currentAlignables );
  //std::cout << "done." << std::endl;

  static int i = 0;
  if ( i%100 == 0 ) KalmanAlignmentDataCollector::fillGraph( "correlation_entries", i, store->numCorrelations() );
  ++i;

  //std::cout << "[SingleTrajectoryUpdator::process] DONE" << std::endl;

  return;
}