Trk::KalmanUpdator Class Reference

Implementation of Trk::IUpdator based on gain formalism and Eigen. More...

#include <KalmanUpdator.h>

Inheritance diagram for Trk::KalmanUpdator:

Collaboration diagram for Trk::KalmanUpdator:

Public Member Functions
	KalmanUpdator (const std::string &, const std::string &, const IInterface *)
	AlgTool standard constuctor.
	~KalmanUpdator ()
virtual StatusCode	initialize () override final
	AlgTool initialisation.
virtual StatusCode	finalize () override final
	AlgTool termination.
virtual std::unique_ptr< TrackParameters >	addToState (const TrackParameters &, const Amg::Vector2D &, const Amg::MatrixX &) const override final
	measurement updator for the KalmanFitter getting the meas't coord'
virtual std::unique_ptr< TrackParameters >	addToState (const TrackParameters &, const LocalParameters &, const Amg::MatrixX &) const override final
	measurement updator for the KalmanFitter getting the coordinates
virtual std::unique_ptr< TrackParameters >	addToState (const TrackParameters &, const Amg::Vector2D &, const Amg::MatrixX &, FitQualityOnSurface *&) const override final
	measurement updator interface for the KalmanFitter returning the fit quality
virtual std::unique_ptr< TrackParameters >	addToState (const TrackParameters &, const LocalParameters &, const Amg::MatrixX &, FitQualityOnSurface *&) const override final
	measurement updator interface for the KalmanFitter returning the fit quality
virtual std::unique_ptr< TrackParameters >	removeFromState (const TrackParameters &, const Amg::Vector2D &, const Amg::MatrixX &) const override final
	reverse update eg for track property analysis (unbiased residuals)
virtual std::unique_ptr< TrackParameters >	removeFromState (const TrackParameters &, const LocalParameters &, const Amg::MatrixX &) const override final
	reverse update eg for track property analysis (unbiased residuals)
virtual std::unique_ptr< TrackParameters >	removeFromState (const TrackParameters &, const Amg::Vector2D &, const Amg::MatrixX &, FitQualityOnSurface *&) const override final
	reverse updator for the KalmanFitter and other fitters using the
virtual std::unique_ptr< TrackParameters >	removeFromState (const TrackParameters &, const LocalParameters &, const Amg::MatrixX &, FitQualityOnSurface *&) const override final
	reverse updator for the KalmanFitter and other fitters using
virtual std::unique_ptr< TrackParameters >	combineStates (const TrackParameters &, const TrackParameters &) const override final
	trajectory state updator which combines two parts of a trajectory on a common surface.
virtual std::unique_ptr< TrackParameters >	combineStates (const TrackParameters &, const TrackParameters &, FitQualityOnSurface *&) const override final
	trajectory state updator which combines two parts of a trajectory on a common surface and provides the FitQuality.
virtual FitQualityOnSurface	fullStateFitQuality (const TrackParameters &, const Amg::Vector2D &, const Amg::MatrixX &) const override final
	estimator for FitQuality on Surface from a full track state, that is a state
virtual FitQualityOnSurface	fullStateFitQuality (const TrackParameters &, const LocalParameters &, const Amg::MatrixX &) const override final
	estimator for FitQuality on Surface from a full track state, that is
virtual FitQualityOnSurface	predictedStateFitQuality (const TrackParameters &, const Amg::Vector2D &, const Amg::MatrixX &) const override final
	estimator for FitQuality on Surface from a predicted track state, that is a state
virtual FitQualityOnSurface	predictedStateFitQuality (const TrackParameters &, const LocalParameters &, const Amg::MatrixX &) const override final
	estimator for FitQuality on Surface from a predicted track state, that is a state
virtual FitQualityOnSurface	predictedStateFitQuality (const TrackParameters &, const TrackParameters &) const override final
	estimator for FitQuality on Surface for the situation when a track is fitted to
virtual std::pair< AmgVector(5), AmgSymMatrix(5)> *	updateParameterDifference (const AmgVector(5) &, const AmgSymMatrix(5) &, const Amg::VectorX &, const Amg::MatrixX &, int, Trk::FitQualityOnSurface *&, bool) const override final
	interface for reference-track KF, not implemented.
virtual std::vector< double >	initialErrors () const override final
	gives back how updator is configured for inital covariances
ServiceHandle< StoreGateSvc > &	evtStore ()
	The standard StoreGateSvc (event store) Returns (kind of) a pointer to the StoreGateSvc.
const ServiceHandle< StoreGateSvc > &	detStore () const
	The standard StoreGateSvc/DetectorStore Returns (kind of) a pointer to the StoreGateSvc.
virtual StatusCode	sysInitialize () override
	Perform system initialization for an algorithm.
virtual StatusCode	sysStart () override
	Handle START transition.
virtual std::vector< Gaudi::DataHandle * >	inputHandles () const override
	Return this algorithm's input handles.
virtual std::vector< Gaudi::DataHandle * >	outputHandles () const override
	Return this algorithm's output handles.
Gaudi::Details::PropertyBase &	declareProperty (Gaudi::Property< T, V, H > &t)
void	updateVHKA (Gaudi::Details::PropertyBase &)
MsgStream &	msg () const
bool	msgLvl (const MSG::Level lvl) const

Static Public Member Functions
static const InterfaceID &	interfaceID ()
	Algtool infrastructure.

Protected Member Functions
void	renounceArray (SG::VarHandleKeyArray &handlesArray)
	remove all handles from I/O resolution
std::enable_if_t< std::is_void_v< std::result_of_t< decltype(&T::renounce)(T)> > &&!std::is_base_of_v< SG::VarHandleKeyArray, T > &&std::is_base_of_v< Gaudi::DataHandle, T >, void >	renounce (T &h)
void	extraDeps_update_handler (Gaudi::Details::PropertyBase &ExtraDeps)
	Add StoreName to extra input/output deps as needed.

Private Types
typedef ServiceHandle< StoreGateSvc >	StoreGateSvc_t

Private Member Functions
std::unique_ptr< TrackParameters >	calculateFilterStep (const TrackParameters &, const Amg::Vector2D &, const Amg::MatrixX &, const int, FitQualityOnSurface *&, bool) const
	Common maths calculation code for addToState and removeFromState - Amg::Vector2D interface.
std::unique_ptr< TrackParameters >	calculateFilterStep (const TrackParameters &, const LocalParameters &, const Amg::MatrixX &, const int, FitQualityOnSurface *&, bool) const
	Common maths calculation code for addToState and removeFromState - LocalParameters interface.
FitQualityOnSurface	makeChi2Object (const Amg::VectorX &, const Amg::MatrixX &, const Amg::MatrixX &, const int, const int) const
	also the chi2 calculation and FitQuality object creation is combined in an extra method.
Amg::MatrixX	projection (const Amg::MatrixX &, const int) const
	avoid CLHEP's empty math operations (H-matrix) by copying members out
void	logStart (const std::string &, const TrackParameters &) const
	internal structuring: debugging output for start of method.
void	logInputCov (const Amg::MatrixX &, const Amg::VectorX &, const Amg::MatrixX &) const
	internal structuring: common logfile output of the inputs
void	logGainForm (int, const Amg::VectorX &, const Amg::MatrixX &, const Amg::MatrixX &, const Amg::MatrixX &) const
	internal structuring: common logfile output during calculation
void	logResult (const std::string &, const Amg::VectorX &, const Amg::MatrixX &) const
	internal structuring: common logfile output after calculation
bool	consistentParamDimensions (const LocalParameters &, int) const
	method testing correct use of LocalParameters *‍/
bool	thetaPhiWithinRange (const Amg::VectorX &, const int key=31) const
	tests if ranges of abolute phi (-pi, pi) and theta (0, pi) are correct *‍/
bool	diffThetaPhiWithinRange (const Amg::VectorX &, const int key=31) const
	tests if ranges of phi (-pi, pi) and theta (0, pi) residuals are correct *‍/
bool	correctThetaPhiRange (Amg::VectorX &, AmgSymMatrix(5) &, const bool isDifference=false, const int key=31) const
	brings phi/theta back into valid range using 2pi periodicity
bool	correctThetaPhiRange (Amg::VectorX &, Amg::MatrixX &, const bool isDifference=false, const int key=31) const
Gaudi::Details::PropertyBase &	declareGaudiProperty (Gaudi::Property< T, V, H > &hndl, const SG::VarHandleKeyType &)
	specialization for handling Gaudi::Property<SG::VarHandleKey>

Private Attributes
std::vector< double >	m_cov0
	job option: initial covariance matrix
ProjectionMatricesSet	m_projectionMatrices
	get the correct projection matrix
bool	m_useFruehwirth8a
	job option: formula for cov update
int	m_outputlevel
	MsgStream output level cached.
StoreGateSvc_t	m_evtStore
	Pointer to StoreGate (event store by default)
StoreGateSvc_t	m_detStore
	Pointer to StoreGate (detector store by default)
std::vector< SG::VarHandleKeyArray * >	m_vhka
bool	m_varHandleArraysDeclared

Detailed Description

Implementation of Trk::IUpdator based on gain formalism and Eigen.

Tool to provide calculations for Kalman filtering, i.e. to add or remove a measured hit to the state vector. Implemented fully in Eigen using dynamic matrices As a drawback due to using dynamic matrices this tool is generally slower than alternatives that are written based on fixed size objects.

Author

M. Elsing, W. Liebig http://consult.cern.ch/xwho

Definition at line 43 of file KalmanUpdator.h.

Member Typedef Documentation

StoreGateSvc_t

typedef ServiceHandle<StoreGateSvc> AthCommonDataStore< AthCommonMsg< AlgTool > >::StoreGateSvc_t

privateinherited

Definition at line 388 of file AthCommonDataStore.h.

Constructor & Destructor Documentation

KalmanUpdator()

Trk::KalmanUpdator::KalmanUpdator	(	const std::string &	t,
		const std::string &	n,
		const IInterface *	p )

AlgTool standard constuctor.

Definition at line 24 of file KalmanUpdator.cxx.

                                                                                         :
    AthAlgTool (t,n,p),
    m_cov0{250., 250.,0.25, 0.25, 0.000001}, // set defaults _before_ reading from job options
    m_projectionMatrices(5),
    m_outputlevel(1)
{
    // AlgTool stuff
  declareProperty("InitialCovariances",m_cov0,"default covariance to be used at start of filter");
  declareProperty("FastTrackStateCovCalculation",m_useFruehwirth8a=false,"toggles which formula to use for updated cov");
  declareInterface<IUpdator>( this );
}

~KalmanUpdator()

Trk::KalmanUpdator::~KalmanUpdator ( )

default

Member Function Documentation

addToState() [1/4]

std::unique_ptr< Trk::TrackParameters > Trk::KalmanUpdator::addToState ( const TrackParameters & trkPar, const Amg::Vector2D & measmtPos, const Amg::MatrixX & measmtErr ) const

finaloverridevirtual

measurement updator for the KalmanFitter getting the meas't coord'

Implements Trk::IUpdator.

Definition at line 59 of file KalmanUpdator.cxx.

                                                                                              {
    if (m_outputlevel <= 0) logStart("addToState(TP,LPOS,ERR)",trkPar);
    FitQualityOnSurface*    fitQoS = nullptr;
    return calculateFilterStep (trkPar, measmtPos, measmtErr,1,fitQoS,false);
}

addToState() [2/4]

std::unique_ptr< Trk::TrackParameters > Trk::KalmanUpdator::addToState ( const TrackParameters & trkPar, const Amg::Vector2D & measmtPos, const Amg::MatrixX & measmtErr, FitQualityOnSurface *& fitQoS ) const

finaloverridevirtual

measurement updator interface for the KalmanFitter returning the fit quality

Implements Trk::IUpdator.

Definition at line 76 of file KalmanUpdator.cxx.

                                                                                            {
    if (m_outputlevel <= 0) logStart("addToState(TP,LPOS,ERR,FQ)",trkPar);
    if (fitQoS) {
      ATH_MSG_WARNING( "expect nil FitQuality pointer, refuse operation to avoid mem leak!"  );
      return nullptr;
    }
      return calculateFilterStep (trkPar, measmtPos, measmtErr, 1, fitQoS, true);
 
}

addToState() [3/4]

std::unique_ptr< Trk::TrackParameters > Trk::KalmanUpdator::addToState ( const TrackParameters & trkPar, const LocalParameters & measmtPar, const Amg::MatrixX & measmtErr ) const

finaloverridevirtual

measurement updator for the KalmanFitter getting the coordinates

Implements Trk::IUpdator.

Definition at line 68 of file KalmanUpdator.cxx.

                                                                                                    {
    if (m_outputlevel <= 0) logStart("addToState(TP,LPAR,ERR)",trkPar);
    FitQualityOnSurface*    fitQoS = nullptr;
    return calculateFilterStep (trkPar, measmtPar, measmtErr,1,fitQoS, false);
}

addToState() [4/4]

std::unique_ptr< Trk::TrackParameters > Trk::KalmanUpdator::addToState ( const TrackParameters & trkPar, const LocalParameters & measmtPar, const Amg::MatrixX & measmtErr, FitQualityOnSurface *& fitQoS ) const

finaloverridevirtual

measurement updator interface for the KalmanFitter returning the fit quality

Implements Trk::IUpdator.

Definition at line 90 of file KalmanUpdator.cxx.

                                                                                            {
    if (m_outputlevel <= 0) logStart("addToState(TP,LPAR,ERR,FQ)",trkPar);
    if (fitQoS) {
      ATH_MSG_WARNING( "expect nil FitQuality pointer, refuse operation to avoid mem leak!"  );
      return nullptr;
    }
      return calculateFilterStep (trkPar, measmtPar, measmtErr, 1, fitQoS, true);
 
}

calculateFilterStep() [1/2]

std::unique_ptr< Trk::TrackParameters > Trk::KalmanUpdator::calculateFilterStep ( const TrackParameters & trkPar, const Amg::Vector2D & locPos, const Amg::MatrixX & covRio, const int sign, Trk::FitQualityOnSurface *& fitQoS, bool createFQoS ) const

private

Common maths calculation code for addToState and removeFromState - Amg::Vector2D interface.

Definition at line 434 of file KalmanUpdator.cxx.

{
 
  // try if Track Parameters are measured ones ?
  AmgSymMatrix(5) covTrk;
  if (!trkPar.covariance()) {
    if (sign<0) {
      ATH_MSG_WARNING( "MeasuredTrackParameters == Null, can not calculate updated parameter state"  );
      return nullptr;
    }
      // no error given - use a huge error matrix for the time
      // covTrk = Amg::MatrixX(5, 1) * 1000.f;
      ATH_MSG_VERBOSE( "-U- no covTrk at input -  assign large error matrix for the time being."  );
      covTrk(0,0) = m_cov0[0];
      covTrk(1,1) = m_cov0[1];
      covTrk(2,2) = m_cov0[2];
      covTrk(3,3) = m_cov0[3];
      covTrk(4,4) = m_cov0[4];
 
  }else {
    covTrk = (*trkPar.covariance());
  }
 
 
  // covariance matrix and parameters of track
  Amg::VectorX parTrk = trkPar.parameters();
  if (!thetaPhiWithinRange(parTrk)) {
    ATH_MSG_WARNING( (sign>0?"addToState(TP,LPOS,ERR..)":"removeFromState(TP,LPOS,ERR..)")
                     << ": undefined phi,theta range in input parameters."  );
    return nullptr;
  }
 
  // measurement vector of RIO_OnTrack - needs more care for # local par?
  int nLocCoord = covRio.cols();
  if ( (nLocCoord < 1) || (nLocCoord > 2 ) ) {
    ATH_MSG_WARNING( " number of local coordinates must be 1 or 2, but it is "
                     << nLocCoord  );
  }
  Amg::VectorX rioPar(nLocCoord);
  for (int iLocCoord=0; iLocCoord < nLocCoord; iLocCoord++) {
    rioPar[iLocCoord] = locPos[iLocCoord];
  }
  if (m_outputlevel<0) logInputCov(covTrk,rioPar,covRio);
 
  // measurement Matrix ( n x m )
  Amg::MatrixX H(covRio.cols(),covTrk.cols());
  H.setZero();
  for (int i=0; i < nLocCoord; i++) H(i,i)=1.f;
 
  // residual from reconstructed hit wrt. predicted state
  Amg::VectorX r = rioPar - H * parTrk;
 
  // compute covariance on of residual R = +/- covRIO + H * covTrk * H.T()
  Amg::MatrixX R = (sign * covRio) + projection(covTrk,(nLocCoord==1 ? 1 : 3) ); // .similarity(H);
  // compute Kalman gain matrix
  Amg::MatrixX K   = covTrk * H.transpose() * R.inverse();
  AmgSymMatrix(5) I;  // 5x5 unit matrix
  I.setIdentity();
  AmgSymMatrix(5) M = I - K * H;
  if (m_outputlevel<0) {logGainForm (nLocCoord,r,R.inverse(),K,M);}
 
  // compute local filtered state
  Amg::VectorX par = parTrk + K * r;
 
  // compute covariance matrix of local filteres state:
  AmgSymMatrix(5) covPar;
  if (m_useFruehwirth8a) {
    // one can use as well: covPar = M * covTrk; see A.Gelb why.
    covPar = AmgSymMatrix(5)(M*covTrk);
  } else {
    //   C = M*covTrk*M.T() +/- K*covRio*K.T(), supposedly more robust Gelb form.
    // bad_alloc:  covPar = new AmgSymMatrix(5)(covTrk.similarity(M) + (sign * covRio.similarity(K)));
    covPar = AmgSymMatrix(5)(M*covTrk*M.transpose() + sign*K*covRio*K.transpose());
  }
  if ( (!thetaPhiWithinRange(par)) ? !correctThetaPhiRange(par,covPar) : false ) {
    ATH_MSG_WARNING( "calculateFS(TP,LPOS,ERR): bad angles in filtered state!"  );
    return nullptr;
  }
  if (m_outputlevel<=0) logResult(createFQoS?
                  (sign>0?"addToState(TP,LPOS,ERR,FQ)":"removeFromState(TP,LPOS,ERR,FQ)"):
                  (sign>0?"addToState(TP,LPOS,ERR)":"removeFromState(TP,LPOS,ERR)"),
                  par,covPar);
 
  // if a pointer is given, compute chi2
  if (createFQoS) {
    if (sign<0) {
      // when removing, the input are updated par
      fitQoS = new Trk::FitQualityOnSurface(makeChi2Object(r,covTrk,covRio,-1,(nLocCoord==1?1:3)));
    } else {
      // when adding chi2 is made from the updated par
      const Amg::VectorX r_upd = rioPar - H * par;
      fitQoS = new FitQualityOnSurface(makeChi2Object(r_upd,covPar,covRio,-1,(nLocCoord==1?1:3)));
    }
  }
  // return cloned version of Track Parameters (MeasuredPerigee, MeasuredAtA...)
  auto updated =
    trkPar.associatedSurface().createUniqueTrackParameters(par[Trk::loc1],
                                                           par[Trk::loc2],
                                                           par[Trk::phi],
                                                           par[Trk::theta],
                                                           par[Trk::qOverP],
                                                           std::move(covPar));
  return updated;
}

calculateFilterStep() [2/2]

std::unique_ptr< Trk::TrackParameters > Trk::KalmanUpdator::calculateFilterStep ( const TrackParameters & trkPar, const LocalParameters & rioPar, const Amg::MatrixX & covRio, const int sign, Trk::FitQualityOnSurface *& fitQoS, bool createFQoS ) const

private

Common maths calculation code for addToState and removeFromState - LocalParameters interface.

Definition at line 546 of file KalmanUpdator.cxx.

{
 
  // try if Track Parameters are measured ones ?
  AmgSymMatrix(5) covTrk;
  if (!trkPar.covariance()) {
    if (sign<0) {
      ATH_MSG_WARNING( "MeasuredTrackParameters == Null, can not calculate "
                       << "updated parameter state."  );
      return nullptr;
    }
      // no error given - use a huge error matrix for the time
      // covTrk = Amg::MatrixX(5, 1) * 1000.f;
      ATH_MSG_VERBOSE( "-U- no covTrk at input - "
                       << "assign large error matrix for the time being."  );
      covTrk(0,0) = m_cov0[0];
      covTrk(1,1) = m_cov0[1];
      covTrk(2,2) = m_cov0[2];
      covTrk(3,3) = m_cov0[3];
      covTrk(4,4) = m_cov0[4];
 
  } else {
    covTrk = (*trkPar.covariance());
  }
 
  //    LocalParameters  parTrk = trkPar.parameters();
  Amg::VectorX parTrk = trkPar.parameters();
  if (!thetaPhiWithinRange(parTrk)) {
    ATH_MSG_WARNING( (sign>0?"addToState(TP,LPAR,ERR..)":"removeFromState(TP,LPAR,ERR..)")
                     << ": undefined phi,theta range in input parameters."  );
    return nullptr;
  }
  if (!thetaPhiWithinRange(rioPar,rioPar.parameterKey())) {
    ATH_MSG_WARNING( (sign>0?"addToState(TP,LPAR,ERR..)":"removeFromState(TP,LPAR,ERR..)")
                     << ": undefined phi,theta range in input measurement !!"  );
    return nullptr;
  }
 
  // measurement vector of RIO_OnTrack - needs more care for # local par?
  int nLocCoord = covRio.cols();
  if ( ! consistentParamDimensions(rioPar,covRio.cols()) ) return nullptr;
  if (m_outputlevel<0) logInputCov(covTrk,rioPar,covRio);
 
  // State to measurement dimensional reduction Matrix ( n x m )
  Amg::MatrixX H(rioPar.expansionMatrix());
 
  // residual from reconstructed hit wrt. predicted state
  Amg::VectorX r = rioPar - H * parTrk;
  // compute covariance of residual R = +/- covRIO + H * covTrk * H.T()
  Amg::MatrixX R = (sign * covRio) + projection(covTrk,rioPar.parameterKey()); // .similarity(H);
  // catch [-pi,pi] phi boundary problems to keep chi2 under control
  if (!diffThetaPhiWithinRange(r,rioPar.parameterKey()) )
    correctThetaPhiRange(r,R,true,rioPar.parameterKey());
 
  // compute Kalman gain matrix
  if (R.determinant()==0.) return nullptr;
  Amg::MatrixX K   = covTrk * H.transpose() * R.inverse();
  AmgSymMatrix(5) I;  // 5x5 unit matrix
  I.setIdentity();
  AmgSymMatrix(5) M  = I - K * H;
  if (m_outputlevel<0) {logGainForm (nLocCoord,r,R.inverse(),K,M);}
  // compute local filtered state
  Amg::VectorX par = parTrk + K * r;
 
  // compute covariance matrix of local filteres state
  AmgSymMatrix(5) covPar;
  if (m_useFruehwirth8a) {
    // one can use as well: covPar = M * covTrk; see A.Gelb why.
    covPar = AmgSymMatrix(5)(M*covTrk);
  } else {
    covPar = AmgSymMatrix(5)(M*covTrk*M.transpose() + sign*K*covRio*K.transpose());
  }
  if ( (!thetaPhiWithinRange(par)) ? !correctThetaPhiRange(par,covPar) : false ) {
    ATH_MSG_WARNING( "calculateFS(TP,LPAR,ERR): bad angles in filtered state!"  );
    return nullptr;
  }
  if (m_outputlevel<=0) logResult(createFQoS?
                  (sign>0?"addToState(TP,LPAR,ERR,FQ)":"removeFromState(TP,LPAR,ERR,FQ)"):
                  (sign>0?"addToState(TP,LPAR,ERR)":"removeFromState(TP,LPAR,ERR)"),
                  par,covPar);
 
  // if a pointer is given, compute chi2
  if (createFQoS) {
    if (sign<0) {
      // when removing, the input are updated par
      fitQoS = new Trk::FitQualityOnSurface(makeChi2Object(r,covTrk,covRio,-1,rioPar.parameterKey()));
    } else {
      // when adding chi2 is made from the updated par
      Amg::VectorX r_upd = rioPar - H * par;
      fitQoS = new Trk::FitQualityOnSurface(makeChi2Object(r_upd,covPar,covRio,-1,rioPar.parameterKey()));
    }
  }
  // return cloned version of Track Parameters (MeasuredPerigee, MeasuredAtA...)
  return trkPar.associatedSurface().createUniqueTrackParameters(
    par[Trk::loc1],
    par[Trk::loc2],
    par[Trk::phi],
    par[Trk::theta],
    par[Trk::qOverP],
    std::move(covPar));
}

combineStates() [1/2]

std::unique_ptr< Trk::TrackParameters > Trk::KalmanUpdator::combineStates ( const TrackParameters & one, const TrackParameters & two ) const

finaloverridevirtual

trajectory state updator which combines two parts of a trajectory on a common surface.

Make sure that the TPs' surfaces are identical and that the local hit is not duplicated in both trajectories!

Implements Trk::IUpdator.

Definition at line 152 of file KalmanUpdator.cxx.

{
  // remember, either one OR two might have no error, but not both !
  if (!one.covariance() && !two.covariance()) {
    ATH_MSG_WARNING( "both parameters have no errors, invalid "
                     << "use of Updator::combineStates()"  );
    return nullptr;
  }
  // if only one of two has an error, return that one
  if (!one.covariance()) {
    if (m_outputlevel<=0) logResult("combineStates(TP,TP)",two.parameters(),
                    *two.covariance());
    return std::unique_ptr<Trk::TrackParameters>(two.clone());
  }
  if (!two.covariance()) {
    if (m_outputlevel<=0) logResult("combineStates(TP,TP)",one.parameters(),
                    *one.covariance());
    return std::unique_ptr<Trk::TrackParameters>(one.clone());
  }
 
  // ... FIXME - TRT is so difficult, need to check that both parameters are in the same frame
  //               otherwise go into frame of one !
  // ok, normal, let's combine using gain matrix formalism
  const AmgSymMatrix(5)& covTrkOne = (*one.covariance());
  const AmgSymMatrix(5)& covTrkTwo = (*two.covariance());
 
  AmgSymMatrix(5)  sumCov = covTrkOne + covTrkTwo;
  AmgSymMatrix(5)  K      = covTrkOne * sumCov.inverse();
 
  Amg::VectorX         r   = two.parameters() - one.parameters();
  // catch [-pi,pi] phi boundary problems
  if (!diffThetaPhiWithinRange(r)) correctThetaPhiRange(r,sumCov,true);
  Amg::VectorX        par = one.parameters() + K * r;
  AmgSymMatrix(5) covPar = AmgSymMatrix(5)( K * covTrkTwo );
  if ( (!thetaPhiWithinRange(par)) ? !correctThetaPhiRange(par,covPar,false) : false ) {
    ATH_MSG_WARNING( "combineStates(TP,TP): could not combine angular values."  );
    return nullptr;
  }
 
  // return cloned version of Track Parameters (MeasuredPerigee, MeasuredAtA...)
  auto comb =
    one.associatedSurface().createUniqueTrackParameters(par[Trk::loc1],
                                                        par[Trk::loc2],
                                                        par[Trk::phi],
                                                        par[Trk::theta],
                                                        par[Trk::qOverP],
                                                        covPar);
  if (m_outputlevel <= 0){
    logResult("combineStates(TP,TP)", par, covPar);
  }
  return comb;
}

combineStates() [2/2]

std::unique_ptr< Trk::TrackParameters > Trk::KalmanUpdator::combineStates ( const TrackParameters & one, const TrackParameters & two, FitQualityOnSurface *& fitQoS ) const

finaloverridevirtual

trajectory state updator which combines two parts of a trajectory on a common surface and provides the FitQuality.

Make sure that the TPs' surfaces are identical and that the local hit is not duplicated!

Implements Trk::IUpdator.

Definition at line 207 of file KalmanUpdator.cxx.

                                                                                             {
    // try if both Track Parameters are measured ones ?
  // remember, either one OR two might have no error, but not both !
  if (!one.covariance() && !two.covariance()) {
    ATH_MSG_WARNING( "both parameters have no errors, invalid use of Updator::combineStates()"  );
    return nullptr;
  }
  if (fitQoS) {
    ATH_MSG_WARNING( "expect nil FitQuality pointer, refuse operation to avoid mem leak!"  );
    return nullptr;
  }
    // if only one of two has an error, return that one
    if (!one.covariance()) {
      fitQoS =  new FitQualityOnSurface(0.f, 5);
      if (m_outputlevel<=0) logResult("combineStates(TP,TP,FQ)", one.parameters(),
                      (*two.covariance()));
      return std::unique_ptr<Trk::TrackParameters>(two.clone());
    }
    if (!two.covariance()) {
      fitQoS =  new FitQualityOnSurface(0.f, 5);
      if (m_outputlevel<=0) logResult("combineStates(TP,TP,FQ)", one.parameters(),
                      (*one.covariance()));
      return std::unique_ptr<Trk::TrackParameters>(one.clone());
    }
 
    // covariance matrix for prediction and the state to be added
    const AmgSymMatrix(5)& covTrkOne = (*one.covariance());
    const AmgSymMatrix(5)& covTrkTwo = (*two.covariance());
 
    // chi2 calculation and Kalman Gain preparation
    Amg::VectorX    r = two.parameters() - one.parameters();
    AmgSymMatrix(5) R = covTrkOne + covTrkTwo;
    // catch [-pi,pi] phi boundary problems to keep chi2 under control
    if (!diffThetaPhiWithinRange(r)) correctThetaPhiRange(r,R,true);
    AmgSymMatrix(5) R_inv = R.inverse();
    AmgSymMatrix(5) K   = covTrkOne * R_inv;
    Amg::VectorX      par = one.parameters() + K * r;
    AmgSymMatrix(5) covPar = AmgSymMatrix(5)(K * covTrkTwo);
    if ( (!thetaPhiWithinRange(par)) ? !correctThetaPhiRange(par,covPar) : false ) {
      ATH_MSG_WARNING( "combineStates(TP,TP,FQ): could not combine angular values."  );
      return nullptr;
    }
 
    // compute fit quality
    fitQoS =  new FitQualityOnSurface(Amg::chi2(R_inv, r), 5);
 
    // return cloned version of Track Parameters (MeasuredPerigee, MeasuredAtA...)
    auto comb =
      one.associatedSurface().createUniqueTrackParameters(par[Trk::loc1],
                                                          par[Trk::loc2],
                                                          par[Trk::phi],
                                                          par[Trk::theta],
                                                          par[Trk::qOverP],
                                                          covPar);
    if (m_outputlevel <= 0){
      logResult("combineStates(TP,TP,FQ)", par, covPar);
    }
    return comb;
 
}

consistentParamDimensions()

bool Trk::KalmanUpdator::consistentParamDimensions ( const LocalParameters & P, int dimCov ) const

private

method testing correct use of LocalParameters *‍/

Definition at line 666 of file KalmanUpdator.cxx.

                                                                     {
  if (P.dimension() != dimCov ) {
    ATH_MSG_WARNING( "Inconsistency in dimension of local coord - problem with LocalParameters object?"  );
    ATH_MSG_WARNING( "dim of local parameters: "<< P.dimension()<< " vs. dim of error matrix: "<<dimCov  );
    return false;
  }
  if ( (dimCov < 1) || (dimCov > 5 ) ) {
    ATH_MSG_WARNING( "invalid dimension for local coordinates: " << dimCov  );
    return false;
  }
  return true;
}

correctThetaPhiRange() [1/2]

bool Trk::KalmanUpdator::correctThetaPhiRange ( Amg::VectorX & V, Amg::MatrixX & C, const bool isDifference = false, const int key = 31 ) const

private

Definition at line 763 of file KalmanUpdator.cxx.

{
  // get phi and theta coordinate
  int jphi = -1;
  int jtheta = -1;
  double thetaMin = (isDifference ? -M_PI : 0);
  if (key == 31) jphi = Trk::phi;
  else if (key & 4) { // phi is within localParameter and a measured coordinate
    for (int itag = 0, ipos=1 ; itag<Trk::phi; ++itag, ipos*=2) if (key & ipos) ++jphi;
  }
  if (key == 31) jtheta = Trk::theta;
  else if (key & 8) { // theta is within localParameter and a measured coordinate
    for (int itag = 0, ipos=1 ; itag<Trk::theta; ++itag, ipos*=2) if (key & ipos) ++jtheta;
  }
 
  // correct theta and phi coordinate
  if ((jtheta>=0) && (V[jtheta]<thetaMin || V[jtheta]> M_PI) ) {
    if (m_outputlevel <= 0 ) {
      if (key !=31) ATH_MSG_WARNING( "-U- key: "<<key << " jphi: "<<jphi << " jtheta: "<<jtheta  );
      msg() << MSG::WARNING << "-U- " << (isDifference?"diff. ":" ") << "angles out of range,   phi = ";
      if (jphi>=0) msg() << V[jphi]; else msg() <<"free";
      msg() << " theta =  " << V[jtheta] <<endmsg;
    }
    // absolute theta: repair if between -pi and +2pi.
    // differential theta: repair if between -pi and +pi
    if ( ( V(jtheta) < -M_PI ) ||
         ( V(jtheta) > (isDifference? M_PI : 2*M_PI) )
         ) {
      ATH_MSG_WARNING( "-U- track theta too far from defined range, stop update."  );
      msg() << MSG::WARNING << "-U- " << (isDifference?"diff. ":" ") << "angles out of range,   phi = ";
      if (jphi>=0) msg() << V[jphi]; else msg() <<"free";
      msg() << " theta =  " << V[jtheta] <<endmsg;
      return false;
    }
    if (V[jtheta] > M_PI) {
      V[jtheta] = 2*M_PI - V[jtheta];
      if (jphi>=0) V[jphi]   += (V[jphi]>0.0) ? -M_PI : M_PI;
    }
    if (V[jtheta] < 0.0) {
      V[jtheta] = -V[jtheta];
      if (jphi>=0) V[jphi]   += (V[jphi]>0.0) ? -M_PI : M_PI;
    }
    if (jtheta==0 && key==24) C(jtheta,1) = -C(jtheta,1);
    if (jtheta==1)  {
      C(0,jtheta) = -C(0,jtheta);
      if (key>15) C(2,1) = -C(2,1);
    }
    if (jtheta==2)  {
      C(0,jtheta) = -C(0,jtheta);
      C(1,jtheta) = -C(1,jtheta);
      if (key>15) C(3,jtheta) = -C(3,jtheta);
    }
    if (jtheta==3)  {
      C(0,jtheta) = -C(0,jtheta);
      C(1,jtheta) = -C(1,jtheta);
      C(2,jtheta) = -C(2,jtheta);
      if (key>15) C(4,jtheta) = -C(4,jtheta);
    }
 
    if (m_outputlevel <=0) {
      msg() << MSG::DEBUG << "-U- now use corrected " << (isDifference?"diff. ":" ") << "value phi= ";
      if (jphi>=0) msg() << V[jphi]; else msg() <<"free";
      msg() << " theta =  " << V[jtheta] <<endmsg;
    }
  }
 
  // correct phi coordinate if necessary
  if ((jphi>=0) && (V[jphi] > M_PI) ) {
    if (m_outputlevel <=0) msg() << MSG::DEBUG << "-U- phi= " << V[jphi];
    V[jphi] = std::fmod(V[jphi]+M_PI,2*M_PI)-M_PI;
    if (m_outputlevel <=0) ATH_MSG_DEBUG( " out of range, now "
                                          << "corrected to " << V[jphi]  );
  } else if ((jphi>=0) && (V[jphi] < -M_PI) ) {
    if (m_outputlevel <=0) msg() << MSG::DEBUG << "-U- phi= " << V[jphi];
    V[jphi] = std::fmod(V[jphi]-M_PI,2*M_PI)+M_PI;
    if (m_outputlevel <=0) ATH_MSG_DEBUG( " out of range, now "
                                          << "corrected to " << V[jphi]  );
  }
  return true;
}

correctThetaPhiRange() [2/2]

bool Trk::KalmanUpdator::correctThetaPhiRange ( Amg::VectorX & , AmgSymMatrix(5) & , const bool isDifference = false, const int key = 31 ) const

private

brings phi/theta back into valid range using 2pi periodicity

declareGaudiProperty()

Gaudi::Details::PropertyBase & AthCommonDataStore< AthCommonMsg< AlgTool > >::declareGaudiProperty ( Gaudi::Property< T, V, H > & hndl, const SG::VarHandleKeyType &  )

inlineprivateinherited

specialization for handling Gaudi::Property<SG::VarHandleKey>

Definition at line 156 of file AthCommonDataStore.h.

  {
    return *AthCommonDataStore<PBASE>::declareProperty(hndl.name(),
                                                       hndl.value(), 
                                                       hndl.documentation());
 
  }

declareProperty()

Gaudi::Details::PropertyBase & AthCommonDataStore< AthCommonMsg< AlgTool > >::declareProperty ( Gaudi::Property< T, V, H > & t )

inlineinherited

Definition at line 145 of file AthCommonDataStore.h.

                                                                     {
    typedef typename SG::HandleClassifier<T>::type htype;
    return AthCommonDataStore<PBASE>::declareGaudiProperty(t, htype());
  }

detStore()

const ServiceHandle< StoreGateSvc > & AthCommonDataStore< AthCommonMsg< AlgTool > >::detStore ( ) const

inlineinherited

The standard StoreGateSvc/DetectorStore Returns (kind of) a pointer to the StoreGateSvc.

Definition at line 95 of file AthCommonDataStore.h.

{ return m_detStore; }

diffThetaPhiWithinRange()

bool Trk::KalmanUpdator::diffThetaPhiWithinRange ( const Amg::VectorX & V, const int key = 31 ) const

inlineprivate

tests if ranges of phi (-pi, pi) and theta (0, pi) residuals are correct *‍/

Definition at line 291 of file KalmanUpdator.h.

{
  if (!(key & 4 || key & 8))
    return true; // in case no angles measured.
  if (key == 31)
    return ((std::abs(V[Trk::phi]) <= M_PI) && (V[Trk::theta] >= -M_PI) &&
            (V[Trk::theta] <= M_PI));
 
  else { // if vector is compressed (i.e. localParameters) need to extract
         // phi,theta first.
    bool okay = true;
    if (key & 4) { // phi is being measured
      int jphi = 0;
      for (int itag = 0, ipos = 1; itag < Trk::phi; ++itag, ipos *= 2)
        if (key & ipos)
          ++jphi;
      okay = okay && (std::abs(V[jphi]) <= M_PI);
    }
    if (key & 8) { // theta is being measured
      int jtheta = 0;
      for (int itag = 0, ipos = 1; itag <= Trk::theta; ++itag, ipos *= 2)
        if (key & ipos)
          ++jtheta;
      okay = okay && (std::abs(V[jtheta]) <= M_PI);
    }
    return okay;
  }
}

evtStore()

ServiceHandle< StoreGateSvc > & AthCommonDataStore< AthCommonMsg< AlgTool > >::evtStore ( )

inlineinherited

The standard StoreGateSvc (event store) Returns (kind of) a pointer to the StoreGateSvc.

Definition at line 85 of file AthCommonDataStore.h.

{ return m_evtStore; }

extraDeps_update_handler()

void AthCommonDataStore< AthCommonMsg< AlgTool > >::extraDeps_update_handler ( Gaudi::Details::PropertyBase & ExtraDeps )

protectedinherited

Add StoreName to extra input/output deps as needed.

use the logic of the VarHandleKey to parse the DataObjID keys supplied via the ExtraInputs and ExtraOuputs Properties to add the StoreName if it's not explicitly given

finalize()

StatusCode Trk::KalmanUpdator::finalize ( )

finaloverridevirtual

AlgTool termination.

Definition at line 53 of file KalmanUpdator.cxx.

{
    return StatusCode::SUCCESS;
}

fullStateFitQuality() [1/2]

Trk::FitQualityOnSurface Trk::KalmanUpdator::fullStateFitQuality ( const TrackParameters & trkPar, const Amg::Vector2D & locPos, const Amg::MatrixX & rioErr ) const

finaloverridevirtual

estimator for FitQuality on Surface from a full track state, that is a state

Implements Trk::IUpdator.

Definition at line 273 of file KalmanUpdator.cxx.

                                                                         {
    ATH_MSG_DEBUG( "--> entered KalmanUpdator::fullStateFitQuality(TP,LPOS,ERR)"  );
 
    // try if Track Parameters are measured ones ?
    if (!trkPar.covariance()) {
      ATH_MSG_ERROR( "updated smoother/trajectory has no error matrix"  );
      return {};
    }
    // covariance matrix for prediction
    const AmgSymMatrix(5)& covTrk = (*trkPar.covariance());
 
    // For the Amg::Vector2D version, need to know # meas. coord. from covariance matrix.
    int nLocCoord = rioErr.cols();
    Amg::VectorX rioPar(nLocCoord);
    for (int iLocCoord=0; iLocCoord < nLocCoord; iLocCoord++) {
      rioPar[iLocCoord] = locPos[iLocCoord];
    }
 
    // measurement Matrix ( n x m )
    Amg::MatrixX H(rioErr.cols(),covTrk.cols());
    H.setZero();
    for (int i=0; i < nLocCoord; i++) H(i,i)=1.f;
 
    // residuals
    Amg::VectorX    r = rioPar - H * trkPar.parameters();
 
    // all the rest is outsourced to a common chi2 routine
    return makeChi2Object(r,covTrk,rioErr,-1,(nLocCoord==1?1:3));
}

fullStateFitQuality() [2/2]

Trk::FitQualityOnSurface Trk::KalmanUpdator::fullStateFitQuality ( const TrackParameters & trkPar, const LocalParameters & rioPar, const Amg::MatrixX & rioErr ) const

finaloverridevirtual

estimator for FitQuality on Surface from a full track state, that is

Implements Trk::IUpdator.

Definition at line 308 of file KalmanUpdator.cxx.

                                                                             {
  ATH_MSG_VERBOSE( "--> entered KalmanUpdator::fullStateFitQuality(TP,LPAR,ERR)"  );
 
  // try if Track Parameters are measured ones ?
  if (!trkPar.covariance()) {
    ATH_MSG_ERROR( "updated smoother/trajectory has no error matrix"  );
    return {};
  }
  if ( !consistentParamDimensions(rioPar,rioErr.cols()) ) return {};
 
  // covariance matrix for prediction
  const AmgSymMatrix(5)& covTrk = (*trkPar.covariance());
 
  // State to measurement dimensional reduction Matrix ( n x m )
  const Amg::MatrixX& H(rioPar.expansionMatrix());
 
  // residuals
  Amg::VectorX    r = rioPar;
  if( rioPar.parameterKey()==31 ) r -= trkPar.parameters();
  else                            r -= H*trkPar.parameters();
 
  // all the rest is outsourced to a common chi2 routine
  return makeChi2Object(r,covTrk,rioErr,-1,rioPar.parameterKey());
}

initialErrors()

std::vector< double > Trk::KalmanUpdator::initialErrors ( ) const

finaloverridevirtual

gives back how updator is configured for inital covariances

Implements Trk::IUpdator.

Definition at line 426 of file KalmanUpdator.cxx.

                                                        {
  std::vector<double> E(5);
  for (int i=0; i<5; ++i) E[i] = std::sqrt(m_cov0[i]);
  return E;
}

initialize()

StatusCode Trk::KalmanUpdator::initialize ( )

finaloverridevirtual

AlgTool initialisation.

Definition at line 41 of file KalmanUpdator.cxx.

{
    // pass individual outputlevel to message stream
  m_outputlevel = msg().level()-MSG::DEBUG;
  if (m_cov0.size() < 5) {
    ATH_MSG_WARNING( "Wrong-sized initial covariance given, so set to default: "  );
    m_cov0 = {250.,250,0.25,0.25, 0.000001};
  }
  return StatusCode::SUCCESS;
}

inputHandles()

virtual std::vector< Gaudi::DataHandle * > AthCommonDataStore< AthCommonMsg< AlgTool > >::inputHandles ( ) const

overridevirtualinherited

Return this algorithm's input handles.

We override this to include handle instances from key arrays if they have not yet been declared. See comments on updateVHKA.

interfaceID()

const InterfaceID & Trk::IUpdator::interfaceID ( )

inlinestaticinherited

Algtool infrastructure.

Definition at line 227 of file IUpdator.h.

{
  return IID_IUpdator;
}

logGainForm()

void Trk::KalmanUpdator::logGainForm ( int nc, const Amg::VectorX & r, const Amg::MatrixX & R, const Amg::MatrixX & K, const Amg::MatrixX & M ) const

private

internal structuring: common logfile output during calculation

Definition at line 882 of file KalmanUpdator.cxx.

{
  // again some verbose debug output showing internals of updating
  msg() << MSG::VERBOSE << "-U- residual: r=("<<r[0];
  for (int i=1; i<nc; i++) msg() <<","<<r[i];
  msg() << ")" << endmsg;
  msg() << MSG::VERBOSE << "-U- inv. sigmaR=("<< R(0,0);
  for (int i=1; i<nc; i++) msg() << "," << R(i,i);
  msg() << ")" << endmsg;
  for (int i=0; i<nc; i++)  msg() << MSG::VERBOSE // K is a row x col = 5 x nc matrix.
    << ( i==0 ? "-U- gain mtx  K=(" : "                (" )
        << std::setiosflags(std::ios::fixed | std::ios::showpoint | std::ios::right )
        << std::setw(7) << std::setprecision(4) << K(0,i)<<", "
        << std::setw(7) << std::setprecision(4) << K(1,i)<<", "
        << std::setw(7) << std::setprecision(4) << K(2,i)<<", "
        << std::setw(7) << std::setprecision(4) << K(3,i)<<", "
        << std::setw(7) << std::setprecision(4) << K(4,i)<<")"
        << std::resetiosflags(std::ios::fixed) << endmsg;
  msg() << MSG::VERBOSE << "-U- matrix M: diag=("
        << M(0,0)<<"," << M(1,1)<<","
        << M(2,2)<<"," << M(3,3)<<","
        << M(4,4)                <<")" << endmsg;
}

logInputCov()

void Trk::KalmanUpdator::logInputCov ( const Amg::MatrixX & covTrk, const Amg::VectorX & rioPar, const Amg::MatrixX & covRio ) const

private

internal structuring: common logfile output of the inputs

Definition at line 855 of file KalmanUpdator.cxx.

{
  msg() << MSG::VERBOSE << "-U- cov    "<<std::setiosflags(std::ios::right)<<std::setprecision(3)
        << std::setw(9)<<covTrk(0,0)<<" "<< std::setw(9)<<covTrk(0,1)<<" "
        << std::setw(9)<<covTrk(0,2)<<" "<< std::setw(9)<<covTrk(0,3)<<" "
        << std::setw(9)<<covTrk(0,4)<<"\n";
  msg() << "                                      " << "          " << "           "
        << std::setw(9)<<covTrk(1,1)<<" "<< std::setw(9)<<covTrk(1,2)<<" "
        << std::setw(9)<<covTrk(1,3)<<" "<< std::setw(9)<<covTrk(1,4)<<"\n";
  msg() << "  covariance matrix                   " << "          " << "                     "
        << std::setw(9)<<covTrk(2,2)<<" "<< std::setw(9)<<covTrk(2,3)<<" "
        << std::setw(9)<<covTrk(2,4)<< "\n"  ;
  msg() << "  of the PREDICTED track pars         " << "          " << "                               "
        << std::setw(9)<<covTrk(3,3)<<" "<< std::setw(9)<<covTrk(3,4)<<"\n"  ;
  msg() << "                                      " << "          " << "                                         "
        << std::setw(9)<<covTrk(4,4)<<std::setprecision(6)<< endmsg;
 
  int nLocCoord = covRio.cols();
  msg() << MSG::VERBOSE << "-U- measurement locPos: ";
  for (int i=0; i<nLocCoord; i++) msg() << rioPar[i] << " ";
  msg() << endmsg;
  msg() << MSG::VERBOSE << "-U- measurement (err)^2: " <<std::setprecision(4)<<covRio(0,0);
  for (int i=1; i<nLocCoord; i++) msg() << ", "<<covRio(i,i);
  msg() << std::setprecision(6)<<endmsg;
}

logResult()

void Trk::KalmanUpdator::logResult ( const std::string & methodName, const Amg::VectorX & par, const Amg::MatrixX & covPar ) const

private

internal structuring: common logfile output after calculation

Definition at line 907 of file KalmanUpdator.cxx.

{
    // again some verbose debug output
    msg() << MSG::VERBOSE << "-U- ==> result for KalmanUpdator::"<<methodName<<endmsg;
    msg() << MSG::VERBOSE << "-U- new par"<<std::setiosflags(std::ios::right)<<std::setprecision(4)
          << std::setw( 9)<<par[0]<< std::setw(10)<<par[1]<<std::setprecision(5)
          << std::setw(10)<<par[2]<< std::setw(10)<<par[3]<<std::setprecision(4)
          << std::setw(10)<<par[4]                <<endmsg;
    msg() << MSG::VERBOSE << "-U- new cov" <<std::setiosflags(std::ios::right)<<std::setprecision(3)
          << std::setw(9)<<(covPar)(0,0)<<" "<< std::setw(9)<<(covPar)(0,1)<<" "
          << std::setw(9)<<(covPar)(0,2)<<" "<< std::setw(9)<<(covPar)(0,3)
          << " " << std::setw(9)<<(covPar)(0,4)<< "\n";
    msg() << "                                      " << "           " << "          "
          << std::setw(9)<<(covPar)(1,1)<<" "<< std::setw(9)<<(covPar)(1,2)<<" "
          << std::setw(9)<<(covPar)(1,3)<<" "<< std::setw(9)<<(covPar)(1,4)<< "\n";
    msg() << "  covariance matrix                   " << "           " << "                    "
          << std::setw(9)<<(covPar)(2,2)<<" "<< std::setw(9)<<(covPar)(2,3)<<" "
          << std::setw(9)<<(covPar)(2,4)<< "\n";
    msg() << "  of the UPDATED   track pars         " << "           "
          << "                              " <<std::setw(9)<<(covPar)(3,3)<< " "
          << std::setw(9)<<(covPar)(3,4)<< "\n";
    msg() << "                                      " << "           "
          << "                                        "
          << std::setw(9)<<(covPar)(4,4)<<std::setprecision(6)<< endmsg;
}

logStart()

void Trk::KalmanUpdator::logStart ( const std::string & IDstring, const TrackParameters & tp ) const

private

internal structuring: debugging output for start of method.

Definition at line 845 of file KalmanUpdator.cxx.

{
  ATH_MSG_DEBUG( "--> entered KalmanUpdator::" << IDstring  );
  ATH_MSG_VERBOSE( "-U- TrkPar:" << std::setiosflags(std::ios::right)<<std::setprecision(4)
                   << std::setw( 9)<<tp.parameters()[0]<< std::setw(10)<<tp.parameters()[1]<<std::setprecision(5)
                   << std::setw(10)<<tp.parameters()[2]<< std::setw(10)<<tp.parameters()[3]<<std::setprecision(4)
                   << std::setw(10)<<tp.parameters()[4] );
}

makeChi2Object()

FitQualityOnSurface Trk::KalmanUpdator::makeChi2Object ( const Amg::VectorX & residual, const Amg::MatrixX & covTrk, const Amg::MatrixX & covRio, const int sign, const int key ) const

inlineprivate

also the chi2 calculation and FitQuality object creation is combined in an extra method.

It is called by ___FitQuality() and calculateFilterStep() The sign controls the calculation in case a predicted input track state (sign=+1) or smoothed/updated input track state (sign=-1).

Definition at line 322 of file KalmanUpdator.h.

{ // sign: -1 = updated, +1 = predicted parameters.
  Amg::MatrixX R = covRio + sign * projection(covTrk, key); // .similarity(H);
  if (R.determinant() == 0) {
    ATH_MSG_DEBUG("matrix inversion failed");
    return FitQualityOnSurface(0.0, (int)covRio.cols());
  }
  // get chi2 = r.T() * R^-1 * r
  const double chiSquared = Amg::chi2(R.inverse(), residual);
  ATH_MSG_VERBOSE("-U- fitQuality of " << (sign > 0 ? "predicted" : "updated")
                                       << " state, chi2 :" << chiSquared
                                       << " / ndof= " << covRio.cols());
  // return the FitQualityOnSurface object
  return  FitQualityOnSurface(chiSquared, int(covRio.cols()));
}

msg()

MsgStream & AthCommonMsg< AlgTool >::msg ( ) const

inlineinherited

Definition at line 24 of file AthCommonMsg.h.

                                {
    return this->msgStream();
  }

msgLvl()

bool AthCommonMsg< AlgTool >::msgLvl ( const MSG::Level lvl ) const

inlineinherited

Definition at line 30 of file AthCommonMsg.h.

                                               {
    return this->msgLevel(lvl);
  }

outputHandles()

virtual std::vector< Gaudi::DataHandle * > AthCommonDataStore< AthCommonMsg< AlgTool > >::outputHandles ( ) const

overridevirtualinherited

Return this algorithm's output handles.

We override this to include handle instances from key arrays if they have not yet been declared. See comments on updateVHKA.

predictedStateFitQuality() [1/3]

Trk::FitQualityOnSurface Trk::KalmanUpdator::predictedStateFitQuality ( const TrackParameters & predPar, const Amg::Vector2D & rioLocPos, const Amg::MatrixX & rioLocErr ) const

finaloverridevirtual

estimator for FitQuality on Surface from a predicted track state, that is a state

Implements Trk::IUpdator.

Definition at line 337 of file KalmanUpdator.cxx.

                                                                                 {
  ATH_MSG_VERBOSE( "--> entered KalmanUpdator::predictedStateFitQuality(TP,LPOS,ERR)"  );
  // try if Track Parameters are measured ones ?
  if (predPar.covariance() == nullptr) {
      ATH_MSG_WARNING( "input state has no error matrix in predictedStateFitQuality()"  );
    return {};
  }
  // covariance matrix for prediction
  const AmgSymMatrix(5)& covPred = (*predPar.covariance());
 
  // For the Amg::Vector2D version, need to know # meas. coord. from covariance matrix.
  int nLocCoord = rioLocErr.cols();
  Amg::VectorX rioPar(nLocCoord);
  for (int iLocCoord=0; iLocCoord < nLocCoord; iLocCoord++) {
    rioPar[iLocCoord] = rioLocPos[iLocCoord];
  }
 
  // measurement Matrix ( n x m )
  Amg::MatrixX H(rioLocErr.cols(),covPred.cols());
  H.setZero();
  for (int i=0; i < nLocCoord; i++) H(i,i)=1.f;
 
  // residuals
  Amg::VectorX    r = rioPar - H * predPar.parameters();
 
  // all the rest is outsourced to a common chi2 routine
  return makeChi2Object(r,covPred,rioLocErr,+1,(nLocCoord==1?1:3));
}

predictedStateFitQuality() [2/3]

Trk::FitQualityOnSurface Trk::KalmanUpdator::predictedStateFitQuality ( const TrackParameters & predPar, const LocalParameters & rioPar, const Amg::MatrixX & rioErr ) const

finaloverridevirtual

estimator for FitQuality on Surface from a predicted track state, that is a state

Implements Trk::IUpdator.

Definition at line 370 of file KalmanUpdator.cxx.

                                                                                  {
  ATH_MSG_VERBOSE( "--> entered KalmanUpdator::predictedStateFitQuality(TP,LPAR,ERR)"  );
 
  // try if Track Parameters are measured ones ?
  if (predPar.covariance() == nullptr) {
      ATH_MSG_WARNING( "input state has no error matrix in predictedStateFitQuality()"  );
    return {};
  }
 
  if ( ! consistentParamDimensions(rioPar,rioErr.cols()) ) return {};
 
  // covariance matrix for prediction
  const AmgSymMatrix(5)& covPred = (*predPar.covariance());
 
  // State to measurement dimensional reduction Matrix ( n x m )
  const Amg::MatrixX& H(rioPar.expansionMatrix());
 
  // residuals
  Amg::VectorX    r = rioPar;
  if(rioPar.parameterKey()==31) r -= predPar.parameters();
  else                          r -= H * predPar.parameters();
 
  // all the rest is outsourced to a common chi2 routine
  return makeChi2Object(r,covPred,rioErr,+1,rioPar.parameterKey());
}

predictedStateFitQuality() [3/3]

Trk::FitQualityOnSurface Trk::KalmanUpdator::predictedStateFitQuality ( const TrackParameters & one, const TrackParameters & two ) const

finaloverridevirtual

estimator for FitQuality on Surface for the situation when a track is fitted to

Implements Trk::IUpdator.

Definition at line 400 of file KalmanUpdator.cxx.

                                                                                   {
  ATH_MSG_VERBOSE( "--> entered KalmanUpdator::predictedStateFitQuality(TP,TP)"  );
  // remember, either one OR two might have no error, but not both !
  if (!one.covariance() && !two.covariance()) {
    ATH_MSG_WARNING( "both parameters have no errors, invalid "
                     << "use of Updator::fitQuality()"  );
    return {};
  }
  // if only one of two has an error, place a message.
  if (!one.covariance() || !two.covariance()) {
    ATH_MSG_DEBUG( "One parameter does not have uncertainties, "
                   << "assume initial state and return chi2=0.0"  );
    return {0.f, 5};
  }
 
  // covariance matrix for prediction and the state to be added
  const AmgSymMatrix(5)& covTrkOne = (*one.covariance());
  const AmgSymMatrix(5)& covTrkTwo = (*two.covariance());
  // residuals
  Amg::VectorX r = two.parameters() - one.parameters();
  AmgSymMatrix(5) R = (covTrkOne + covTrkTwo).inverse();
  // chi2 calculation
  return {Amg::chi2(R, r), 5};
}

projection()

Amg::MatrixX Trk::KalmanUpdator::projection ( const Amg::MatrixX & M, const int key ) const

private

avoid CLHEP's empty math operations (H-matrix) by copying members out

Definition at line 653 of file KalmanUpdator.cxx.

{
  //  ATH_MSG_DEBUG( "projection("<<M[0][0]<<", "<<key<<")" );
  if (key == 31) return M;
  // reduction matrix
  const Amg::MatrixX& redMatrix = m_projectionMatrices.reductionMatrix(key);
  Amg::MatrixX R = redMatrix.transpose()*M*redMatrix;
  return R;
 
}

removeFromState() [1/4]

std::unique_ptr< Trk::TrackParameters > Trk::KalmanUpdator::removeFromState ( const TrackParameters & trkPar, const Amg::Vector2D & measmtPos, const Amg::MatrixX & measmtErr ) const

finaloverridevirtual

reverse update eg for track property analysis (unbiased residuals)

Implements Trk::IUpdator.

Definition at line 104 of file KalmanUpdator.cxx.

                                                                                                   {
    if (m_outputlevel<=0) logStart("removeFromState(TP,LPOS,ERR)",trkPar);
    FitQualityOnSurface*    fitQoS = nullptr;
    return calculateFilterStep (trkPar, measmtPos, measmtErr,-1,fitQoS, false);
}

removeFromState() [2/4]

std::unique_ptr< Trk::TrackParameters > Trk::KalmanUpdator::removeFromState ( const TrackParameters & trkPar, const Amg::Vector2D & measmtPos, const Amg::MatrixX & measmtErr, FitQualityOnSurface *& fitQoS ) const

finaloverridevirtual

reverse updator for the KalmanFitter and other fitters using the

Implements Trk::IUpdator.

Definition at line 122 of file KalmanUpdator.cxx.

                                                                                                  {
    if (m_outputlevel<=0) logStart("removeFromState(TP,LPOS,ERR,FQ)",trkPar);
    if (fitQoS) {
      ATH_MSG_WARNING( "expect nil FitQuality pointer, refuse operation to"
                       << " avoid mem leak!"  );
      return nullptr;
    }
      return calculateFilterStep (trkPar, measmtPos, measmtErr, -1, fitQoS, true);
 
}

removeFromState() [3/4]

std::unique_ptr< Trk::TrackParameters > Trk::KalmanUpdator::removeFromState ( const TrackParameters & trkPar, const LocalParameters & measmtPar, const Amg::MatrixX & measmtErr ) const

finaloverridevirtual

reverse update eg for track property analysis (unbiased residuals)

Implements Trk::IUpdator.

Definition at line 113 of file KalmanUpdator.cxx.

                                                                                                   {
    if (m_outputlevel) logStart("removeFromState(TP,LPAR,ERR)",trkPar);
    FitQualityOnSurface*    fitQoS = nullptr;
    return calculateFilterStep (trkPar, measmtPar, measmtErr,-1,fitQoS, false);
}

removeFromState() [4/4]

std::unique_ptr< Trk::TrackParameters > Trk::KalmanUpdator::removeFromState ( const TrackParameters & trkPar, const LocalParameters & measmtPar, const Amg::MatrixX & measmtErr, FitQualityOnSurface *& fitQoS ) const

finaloverridevirtual

reverse updator for the KalmanFitter and other fitters using

Implements Trk::IUpdator.

Definition at line 137 of file KalmanUpdator.cxx.

                                                                                                  {
    if (m_outputlevel<=0) logStart("removeFromState(TP,LPAR,ERR,FQ)",trkPar);
    if (fitQoS) {
      ATH_MSG_WARNING( "expect nil FitQuality pointer, refuse operation to"
                       << " avoid mem leak!"  );
      return nullptr;
    }
    return calculateFilterStep (trkPar, measmtPar, measmtErr, -1, fitQoS, true);
}

renounce()

std::enable_if_t< std::is_void_v< std::result_of_t< decltype(&T::renounce)(T)> > &&!std::is_base_of_v< SG::VarHandleKeyArray, T > &&std::is_base_of_v< Gaudi::DataHandle, T >, void > AthCommonDataStore< AthCommonMsg< AlgTool > >::renounce ( T & h )

inlineprotectedinherited

Definition at line 380 of file AthCommonDataStore.h.

  {
    h.renounce();
    PBASE::renounce (h);
  }

renounceArray()

void AthCommonDataStore< AthCommonMsg< AlgTool > >::renounceArray ( SG::VarHandleKeyArray & handlesArray )

inlineprotectedinherited

remove all handles from I/O resolution

Definition at line 364 of file AthCommonDataStore.h.

                                                          {
    handlesArray.renounce();
  }

sysInitialize()

virtual StatusCode AthCommonDataStore< AthCommonMsg< AlgTool > >::sysInitialize ( )

overridevirtualinherited

Perform system initialization for an algorithm.

We override this to declare all the elements of handle key arrays at the end of initialization. See comments on updateVHKA.

Reimplemented in asg::AsgMetadataTool, AthCheckedComponent< AthAlgTool >, AthCheckedComponent<::AthAlgTool >, and DerivationFramework::CfAthAlgTool.

sysStart()

virtual StatusCode AthCommonDataStore< AthCommonMsg< AlgTool > >::sysStart ( )

overridevirtualinherited

Handle START transition.

We override this in order to make sure that conditions handle keys can cache a pointer to the conditions container.

thetaPhiWithinRange()

bool Trk::KalmanUpdator::thetaPhiWithinRange ( const Amg::VectorX & V, const int key = 31 ) const

inlineprivate

tests if ranges of abolute phi (-pi, pi) and theta (0, pi) are correct *‍/

Definition at line 258 of file KalmanUpdator.h.

{
  if (!(key & 4 || key & 8))
    return true; // in case no angles measured.
  if (key == 31)
    return ((std::abs(V[Trk::phi]) <= M_PI) && (V[Trk::theta] >= 0.0) &&
            (V[Trk::theta] <= M_PI));
  else { // if vector is compressed (i.e. localParameters) need to extract
         // phi,theta first.
    bool okay = true;
    if (key & 4) { // phi is being measured
      int jphi = 0;
      for (int itag = 0, ipos = 1; itag < Trk::phi; ++itag, ipos *= 2)
        if (key & ipos)
          ++jphi;
      okay = okay && (std::abs(V[jphi]) <= M_PI);
    }
    if (key & 8) { // theta is being measured
      int jtheta = 0;
      for (int itag = 0, ipos = 1; itag <= Trk::theta; ++itag, ipos *= 2)
        if (key & ipos)
          ++jtheta;
      okay = okay && (std::abs(V[jtheta] - M_PI_2) <= M_PI_2);
    }
    return okay;
  }
}

updateParameterDifference()

virtual std::pair< AmgVector(5), AmgSymMatrix(5)> * Trk::KalmanUpdator::updateParameterDifference ( const AmgVector(5) & , const AmgSymMatrix(5) & , const Amg::VectorX & , const Amg::MatrixX & , int , Trk::FitQualityOnSurface *& , bool  ) const

inlinefinaloverridevirtual

interface for reference-track KF, not implemented.

Implements Trk::IUpdator.

Definition at line 169 of file KalmanUpdator.h.

  {
    return nullptr;
  }

updateVHKA()

void AthCommonDataStore< AthCommonMsg< AlgTool > >::updateVHKA ( Gaudi::Details::PropertyBase & )

inlineinherited

Definition at line 308 of file AthCommonDataStore.h.

                                               {
    // debug() << "updateVHKA for property " << p.name() << " " << p.toString() 
    //         << "  size: " << m_vhka.size() << endmsg;
    for (auto &a : m_vhka) {
      std::vector<SG::VarHandleKey*> keys = a->keys();
      for (auto k : keys) {
        k->setOwner(this);
      }
    }
  }

Member Data Documentation

m_cov0

std::vector<double> Trk::KalmanUpdator::m_cov0

private

job option: initial covariance matrix

Definition at line 250 of file KalmanUpdator.h.

m_detStore

StoreGateSvc_t AthCommonDataStore< AthCommonMsg< AlgTool > >::m_detStore

privateinherited

Pointer to StoreGate (detector store by default)

Definition at line 393 of file AthCommonDataStore.h.

m_evtStore

StoreGateSvc_t AthCommonDataStore< AthCommonMsg< AlgTool > >::m_evtStore

privateinherited

Pointer to StoreGate (event store by default)

Definition at line 390 of file AthCommonDataStore.h.

m_outputlevel

int Trk::KalmanUpdator::m_outputlevel

private

MsgStream output level cached.

Definition at line 254 of file KalmanUpdator.h.

m_projectionMatrices

ProjectionMatricesSet Trk::KalmanUpdator::m_projectionMatrices

private

get the correct projection matrix

Definition at line 252 of file KalmanUpdator.h.

m_useFruehwirth8a

bool Trk::KalmanUpdator::m_useFruehwirth8a

private

job option: formula for cov update

Definition at line 253 of file KalmanUpdator.h.

m_varHandleArraysDeclared

bool AthCommonDataStore< AthCommonMsg< AlgTool > >::m_varHandleArraysDeclared

privateinherited

Definition at line 399 of file AthCommonDataStore.h.

m_vhka

std::vector<SG::VarHandleKeyArray*> AthCommonDataStore< AthCommonMsg< AlgTool > >::m_vhka

privateinherited

Definition at line 398 of file AthCommonDataStore.h.

---

The documentation for this class was generated from the following files:

• KalmanUpdator.h
• KalmanUpdator.cxx