Main Page | Namespace List | Class Hierarchy | Class List | Directories | File List | Namespace Members | Class Members | File Members

TrackKalmanFilter Class Reference

#include <TrackKalmanFilter.h>

Inheritance diagram for TrackKalmanFilter:

ITrackFitter List of all members.

Public Member Functions

 TrackKalmanFilter (const std::string &type, const std::string &name, const IInterface *parent)
 Standard constructor.
virtual ~TrackKalmanFilter ()
 Destructor.
StatusCode initialize ()
StatusCode fit (Track &track)
 fit the track (filter and smoother)
StatusCode fit (Track &track, const State &seed)
 fit the track (filter and smoother)
StatusCode filter (Track &track, const State &seed)
 filter the track (only filter)
StatusCode filter (State &state, Measurement &meas)
 filter/update state with this measurement

Protected Member Functions

StatusCode iniKalman (Track &track)
 initialize the filter to fit/filter his track
StatusCode predict (FitNode &node, State &state)
 Predict the state to this node.
StatusCode filter (FitNode &node)
 filter this node
StatusCode smoother (Track &track)
 smooth the track
StatusCode smooth (FitNode &node0, const FitNode &node1)
 smooth 2 nodes
void computeChiSq (Track &track)
 compute the chi2
StatusCode checkInvertMatrix (const HepSymMatrix &mat)
StatusCode checkPositiveMatrix (HepSymMatrix &mat)
StatusCode invertMatrix (HepSymMatrix &mat)
StatusCode cToG3 (HepSymMatrix &C)
 change of units to facilitate inversion
StatusCode cToG4 (HepSymMatrix &invC)
 change of units to facilitate inversion

Protected Attributes

Statem_state
 internal clone of the state (pointer onwer by Kalman)
std::vector< FitNode * > m_nodes
HepVector m_K
ITrackExtrapolatorm_extrapolator
 extrapolator
ITrackProjectorm_projector
 projector

Private Member Functions

StatusCode failure (const std::string &comment)
 print comment of failure

Private Attributes

std::string m_extrapolatorName
 name of the extrapolator in Gaudi
std::string m_projectorName
 name of the projector in Gaudi
std::vector< double > m_zPositions
 z-positions at which to determine the states
bool m_statesAtMeasZPos
 store states at measurement-positions?
bool m_stateAtBeamLine
 add state closest to the beam-line?
int m_numFitIter
 number of fit iterations to perform
double m_chi2Outliers
 chi2 of outliers to be removed
double m_storeTransport
 store the transport of the extrapolator
bool m_debugLevel

Detailed Description

Author:
Jose Angel Hernando Morata
Date:
2005-04-15 reusing the previous code
Author:
Rutger van der Eijk 07-04-1999

Mattiew Needham

Definition at line 29 of file TrackKalmanFilter.h.


Constructor & Destructor Documentation

TrackKalmanFilter::TrackKalmanFilter const std::string &  type,
const std::string &  name,
const IInterface *  parent
 

Standard constructor.

Definition at line 30 of file TrackKalmanFilter.cpp.

References m_chi2Outliers, m_extrapolatorName, m_K, m_nodes, m_numFitIter, m_projectorName, m_state, m_stateAtBeamLine, m_statesAtMeasZPos, m_storeTransport, and m_zPositions.

00032                                                                 :
00033   GaudiTool( type, name, parent)
00034 {
00035   declareInterface<ITrackFitter>( this );
00036 
00037   // Define the fixed z-positions
00038   m_zPositions.push_back(   990.0*mm );
00039   m_zPositions.push_back(  2165.0*mm );
00040   m_zPositions.push_back(  9450.0*mm );
00041   m_zPositions.push_back( 11900.0*mm );
00042 
00043   declareProperty( "Extrapolator"     , m_extrapolatorName = "TrackFirstCleverExtrapolator" );
00044   declareProperty( "Projector"        , m_projectorName = "TrackMasterProjector" );
00045   declareProperty( "ZPositions"       , m_zPositions               );
00046   declareProperty( "StatesAtMeasZPos" , m_statesAtMeasZPos = false );
00047   declareProperty( "StateAtBeamLine"  , m_stateAtBeamLine  = true  );
00048   declareProperty( "NumberFitIter"    , m_numFitIter       = 5     );
00049   declareProperty( "Chi2Outliers"     , m_chi2Outliers     = 9.0   );
00050   declareProperty( "StoreTransport"   , m_storeTransport   = true  );
00051 
00052   m_state = NULL;
00053   m_nodes.clear();
00054   m_K = HepVector(5,0);
00055 }

TrackKalmanFilter::~TrackKalmanFilter  )  [virtual]
 

Destructor.

Definition at line 60 of file TrackKalmanFilter.cpp.

00060                                       {
00061 }


Member Function Documentation

StatusCode TrackKalmanFilter::checkInvertMatrix const HepSymMatrix &  mat  )  [protected]
 

Definition at line 472 of file TrackKalmanFilter.cpp.

Referenced by smooth().

00473 {
00474   int nParam = (int) mat.num_row();
00475   for (int i=0; i<nParam; ++i){
00476     for (int j=0; j<nParam; ++j){
00477       if (mat[i][j] > 1e20)
00478         return Warning( "Covariance errors too big to invert!");
00479     } // j
00480   }   // i
00481   return StatusCode::SUCCESS;
00482 }

StatusCode TrackKalmanFilter::checkPositiveMatrix HepSymMatrix &  mat  )  [protected]
 

Definition at line 487 of file TrackKalmanFilter.cpp.

Referenced by smooth().

00488 {
00489   int nParam = (int) mat.num_row();
00490   for (int i=0; i<nParam; ++i){
00491     if (mat[i][i] <= 0.)
00492       return Warning( "Covariance matrix has non-positive elements!" );
00493   }
00494 
00495  return StatusCode::SUCCESS;
00496 }

void TrackKalmanFilter::computeChiSq Track track  )  [protected]
 

compute the chi2

Definition at line 444 of file TrackKalmanFilter.cpp.

References Node::chi2(), m_debugLevel, m_nodes, m_state, and State::nParameters().

Referenced by filter(), and smoother().

00445 {
00446   double chi2 = 0.;
00447   for (std::vector<FitNode*>::iterator it = m_nodes.begin();
00448        it != m_nodes.end(); it++) 
00449     chi2 += (*it)->chi2();
00450   int ndof = -(m_state->nParameters());
00451   ndof += m_nodes.size();
00452 
00453   debug() << "NDoF = " << m_nodes.size() << "-" << m_state->nParameters()
00454           << " = " << ndof << endreq;
00455 
00456   double chi2Norma = 0.;
00457   if (ndof != 0) chi2Norma = chi2 /((double) (ndof));
00458   track.setChi2PerDoF(chi2Norma);
00459   track.setNDoF(ndof);
00460 
00461   if ( m_debugLevel ) {
00462     debug() << " track ndof \t" << track.nDoF() << endreq
00463             << " track chi2, chi2/ndof \t " << track.chi2()  << ", "
00464             << track.chi2PerDoF() << endreq;
00465   }
00466   
00467 }

StatusCode TrackKalmanFilter::cToG3 HepSymMatrix &  C  )  [protected]
 

change of units to facilitate inversion

Definition at line 534 of file TrackKalmanFilter.cpp.

Referenced by invertMatrix().

00534                                                   {
00535 
00536   // cov matrix
00537   C[0][0] /= cm2;
00538   C[0][1] /= cm2;
00539   C[0][2] /= cm;
00540   C[0][3] /= cm;
00541   C[0][4]  = C[0][4]*GeV/cm;
00542 
00543   C[1][1] /= cm2;
00544   C[1][2] /= cm;
00545   C[1][3] /= cm;
00546   C[1][4]  = C[1][4]*GeV/cm;
00547 
00548   C[2][4]  = C[2][4]*GeV;
00549 
00550   C[3][4]  = C[3][4]*GeV;
00551 
00552   C[4][4]  = C[4][4]*GeV*GeV;
00553 
00554   return StatusCode::SUCCESS;
00555 
00556 }

StatusCode TrackKalmanFilter::cToG4 HepSymMatrix &  invC  )  [protected]
 

change of units to facilitate inversion

Definition at line 561 of file TrackKalmanFilter.cpp.

Referenced by invertMatrix().

00561                                                      {
00562 
00563   // cov matrix
00564   invC[0][0] /= cm2;
00565   invC[0][1] /= cm2;
00566   invC[0][2] /= cm;
00567   invC[0][3] /= cm;
00568   invC[0][4]  = invC[0][4]*GeV/cm;
00569 
00570   invC[1][1] /= cm2;
00571   invC[1][2] /= cm;
00572   invC[1][3] /= cm;
00573   invC[1][4]  = invC[1][4]*GeV/cm;
00574 
00575   invC[2][4]  = invC[2][4]*GeV;
00576 
00577   invC[3][4]  = invC[3][4]*GeV;
00578 
00579   invC[4][4]  = invC[4][4]*GeV*GeV;
00580 
00581   return StatusCode::SUCCESS;
00582 }

StatusCode TrackKalmanFilter::failure const std::string &  comment  )  [private]
 

print comment of failure

Definition at line 80 of file TrackKalmanFilter.cpp.

Referenced by filter(), fit(), predict(), smooth(), and smoother().

00080                                                               {
00081   info() << "TrackKalmanFilter failure: " << comment << endreq;
00082   return StatusCode::FAILURE;
00083 }

StatusCode TrackKalmanFilter::filter FitNode node  )  [protected]
 

filter this node

Definition at line 226 of file TrackKalmanFilter.cpp.

References Measurement::errMeasure(), ITrackProjector::errResidual(), failure(), filter(), m_debugLevel, m_K, m_projector, ITrackProjector::projectionMatrix(), ITrackProjector::residual(), Measurement::z(), and State::z().

00227 {
00228   
00229   Measurement& meas = node.measurement();
00230   State& state      = node.state();
00231 
00232   debug() << "z-position of State = " << state.z() << endreq;
00233   debug() << "z-position of Meas  = " << meas.z() << endreq;
00234   debug() << "-> calling filter(state,meas) ..." << endreq;
00235   StatusCode sc = filter(state, meas);
00236   if (sc.isFailure()) return failure("Unable to filter the node!");
00237 
00238   debug() << "z-position of State = " << state.z() << endreq;
00239   debug() << "z-position of Meas  = " << meas.z() << endreq;
00240 
00241   double tMeasureError = meas.errMeasure();
00242   debug() << "getting info from projection ..." << endreq;
00243   double res = m_projector->residual();  // projection just made in filter(,)
00244   const HepVector& H = m_projector->projectionMatrix();
00245 
00246   // save it for later use
00247   node.setProjectionMatrix( H );
00248 
00249   double HK = dot ( H, m_K);
00250   res     *= (1 - HK);
00251   double errorRes = (1 - HK) * (tMeasureError*tMeasureError);
00252 
00253   debug() << "res/errRes = " << res << " / " << errorRes << endreq;
00254 
00255   debug() << "setting info in node ..." << endreq;
00256   node.setResidual(res);
00257   node.setErrResidual(sqrt(errorRes));
00258 
00259   // save predicted state
00260   debug() << "setting filteredState" << endreq;
00261   node.setFilteredState(state);
00262   debug() << "just set filteredState" << endreq;
00263 
00264   if ( m_debugLevel ) {
00265     debug() << " bef print" << endreq;  
00266     debug() << " filtered residual and error \t" << node.residual() << ", "
00267             <<node.errResidual() << endreq;;
00268     debug() << " aft print" << endreq;  
00269   }
00270  
00271   return StatusCode::SUCCESS;
00272 }

StatusCode TrackKalmanFilter::filter State state,
Measurement meas
[virtual]
 

filter/update state with this measurement

Implements ITrackFitter.

Definition at line 279 of file TrackKalmanFilter.cpp.

References ITrackProjector::errResidual(), failure(), m_debugLevel, m_K, m_projector, ITrackProjector::project(), ITrackProjector::projectionMatrix(), and ITrackProjector::residual().

00279                                                                      {
00280   
00281   debug() << "In filter(state,meas)" << endreq;
00282   debug() << "z-position of State = " << state.z() << endreq;
00283   debug() << "z-position of Meas  = " << meas.z() << endreq;
00284   // check z position
00285   if ( fabs(meas.z() - state.z()) > 1e-6) {
00286     warning() << "z-position of State (=" << state.z() 
00287               << ") and Measurement (= " << meas.z() 
00288               << ") are not equal." << endreq;
00289   }
00290 
00291   // get reference to the state vector and cov
00292   HepVector&    tX = state.stateVector();
00293   HepSymMatrix& tC = state.covariance();
00294 
00295   debug() << "filter(state,meas): projecting ..." << endreq;
00296   // project the state into the measurement 
00297   StatusCode sc = m_projector->project(state,meas);
00298   if (sc.isFailure()) 
00299     return failure(" Not able to project a state into a measurement");
00300   debug() << "filter(state,meas): projection done" << endreq;
00301 
00302   // calculate predicted residuals
00303   double errorMeas = meas.errMeasure();
00304   double res       = m_projector->residual();
00305   double errorRes  = m_projector->errResidual();
00306   double errorRes2 = (errorRes*errorRes);
00307   const HepVector& H = m_projector->projectionMatrix();
00308 
00309   // calculate gain matrix K
00310   m_K = (tC * H)/errorRes2;
00311 
00312   // update state vector 
00313   tX += m_K * res;
00314 
00315   HepMatrix B  = HepDiagMatrix(tC.num_row(), 1) - ( m_K * H.T());
00316   tC.assign( B * tC * B.T() + ( m_K * pow(errorMeas, 2.0) * m_K.T()));
00317 
00318  if ( m_debugLevel ) {
00319     debug() << " measure and error \t" << meas.measure() << ", " 
00320             << errorMeas << endreq
00321             << " residual and error \t" << res << ", " 
00322             << errorRes << endreq
00323             << " projection matrix \t" << H << endreq
00324             << " gain matrix \t" << m_K << endreq
00325             << " filter state vector \t" << tX << endreq
00326             << " filter state covariance \t"<< tC << endreq;
00327   }
00328   
00329   return sc;
00330 }

StatusCode TrackKalmanFilter::filter Track track,
const State seed
[virtual]
 

filter the track (only filter)

Implements ITrackFitter.

Definition at line 142 of file TrackKalmanFilter.cpp.

References State::clone(), computeChiSq(), State::covariance(), failure(), iniKalman(), m_debugLevel, m_extrapolator, m_nodes, m_state, predict(), ITrackExtrapolator::propagate(), State::stateVector(), and State::z().

Referenced by filter(), and fit().

00143 {
00144   StatusCode sc = StatusCode::SUCCESS;
00145   sc = iniKalman(track);
00146   if (sc.isFailure()) return failure(" not able to create Nodes");
00147 
00148   m_state = state.clone();
00149 
00150   if ( m_debugLevel ) {
00151     debug() << " seed state at z \t " << m_state->z() << endreq
00152             << " seed state vector \t" << m_state->stateVector() << endreq    
00153             << " seed state cov \t" << m_state->covariance() << endreq
00154             << " track nodes size \t" << m_nodes.size() << endreq;
00155   }
00156 
00157   std::vector<FitNode*>::iterator iNode = m_nodes.begin();
00158   double z = (*iNode)->measurement().z();
00159 
00160   debug() << "Z-positions of seed state/meas.begin = "
00161           << state.z() << " / " << z << endreq;
00162 
00163   debug() << "- propagating the state ..." << endreq;
00164   // sc = m_extrapolator->propagate(*m_state,z,m_particleID);
00165   sc = m_extrapolator->propagate(*m_state,z);
00166   if (sc.isFailure()) 
00167     return failure(" unable to extrapolate to 1st measurement");
00168 
00169   // TODO: handle different iterations
00170   unsigned int nNodes = 0;
00171   debug() << "- looping over nodes ..." << endreq;
00172   for (iNode = m_nodes.begin(); iNode != m_nodes.end(); ++iNode) {
00173     FitNode& node = *(*iNode);
00174     debug() << "-> predict(...) for node # " << nNodes << endreq;
00175     sc = predict(node,*m_state);
00176     if (sc.isFailure()) return failure(" unable to predict at node");
00177     debug() << "-> filter(...) for node # " << nNodes++ << endreq;
00178     sc = filter(node);
00179     if (sc.isFailure()) return failure(" unable to filter node ");
00180   }  
00181 
00182   computeChiSq(track);
00183 
00184   return sc;
00185 }

StatusCode TrackKalmanFilter::fit Track track,
const State seed
[virtual]
 

fit the track (filter and smoother)

Implements ITrackFitter.

Definition at line 126 of file TrackKalmanFilter.cpp.

References failure(), filter(), and smoother().

00127 {
00128   debug() << "-> calling filter(track,state) ..." << endreq;
00129   StatusCode sc = filter(track,state);
00130   if (sc.isFailure()) return failure(" unable to filter the track");
00131 
00132   debug() << "-> calling smoother(track) ..." << endreq;
00133   sc = smoother(track);
00134   if (sc.isFailure()) return failure(" unable to smooth the track");
00135   
00136   return sc;
00137 }

StatusCode TrackKalmanFilter::fit Track track  )  [inline, virtual]
 

fit the track (filter and smoother)

Implements ITrackFitter.

Definition at line 42 of file TrackKalmanFilter.h.

00042                                {
00043     //track.setHistoryFit( TrackKeys::Kalman );
00044     info() << " not implemented yet!" << track.nMeasurements() << endreq;
00045     return StatusCode::SUCCESS;
00046   }

StatusCode TrackKalmanFilter::iniKalman Track track  )  [protected]
 

initialize the filter to fit/filter his track

Definition at line 88 of file TrackKalmanFilter.cpp.

References m_debugLevel, m_nodes, and m_state.

Referenced by filter().

00089 {
00090   // destroy the previous state if not null
00091   if (m_state != NULL) { delete m_state; m_state = 0; }
00092 
00093   // Kalman has private copy of the nodes to acelerate the computations
00094   // remember that the track owns the Nodes, so it is responsability of the
00095   // Track destructor to delete them!
00096   m_nodes.clear();
00097   debug() << "- iniKalman: getting track measurements ..." << endreq;
00098   std::vector<Measurement*>& measures = track.measurements();
00099   debug() << "- iniKalman: getting track nodes ..." << endreq;
00100   std::vector<Node*>& nodes = track.nodes();
00101   if (nodes.size() > 0) 
00102     for (std::vector<Node*>::iterator it = nodes.begin(); it != nodes.end();
00103          it++) delete *it;  
00104   nodes.clear();
00105   debug() << "- iniKalman: private nodes vector cleared ..." << endreq;
00106   // reserve some space in node vector
00107   m_nodes.reserve(measures.size());
00108   for (std::vector<Measurement*>::iterator it = measures.begin(); 
00109        it != measures.end(); it++) {
00110     Measurement& meas = *(*it);
00111     FitNode* node = new FitNode(meas);
00112     m_nodes.push_back(node);
00113     track.addToNodes(node);
00114   }
00115   debug() << "- iniKalman: private nodes vector rebuilt ..." << endreq;
00116 
00117   if ( m_debugLevel )
00118     debug() << " track nodes size \t" << m_nodes.size() << endreq;
00119 
00120   return StatusCode::SUCCESS;
00121 }

StatusCode TrackKalmanFilter::initialize  ) 
 

Definition at line 66 of file TrackKalmanFilter.cpp.

References m_debugLevel, m_extrapolator, m_extrapolatorName, m_projector, and m_projectorName.

00067 {
00068   m_extrapolator = tool<ITrackExtrapolator>(m_extrapolatorName);
00069 
00070   m_projector    = tool<ITrackProjector>(m_projectorName);
00071 
00072   m_debugLevel   = msgLevel( MSG::DEBUG );
00073   
00074   return StatusCode::SUCCESS;
00075 }

StatusCode TrackKalmanFilter::invertMatrix HepSymMatrix &  mat  )  [protected]
 

Definition at line 508 of file TrackKalmanFilter.cpp.

References cToG3(), and cToG4().

Referenced by smooth().

00508                                                                     {
00509 
00510   StatusCode sc = StatusCode::SUCCESS; 
00511 
00512   // G3 units
00513   cToG3(invPrevNodeC);
00514 
00515   HepSymMatrix tempMatrix = invPrevNodeC;
00516 
00517   int ifail;
00518   invPrevNodeC.invert(ifail);
00519 
00520   //G4 units
00521   cToG4(invPrevNodeC);
00522 
00523   if (ifail !=0){
00524     warning()
00525        <<"failed to invert covariance matrix, failure code " <<ifail<<endreq;
00526     sc = StatusCode::FAILURE;
00527   }
00528   return sc;
00529 }

StatusCode TrackKalmanFilter::predict FitNode node,
State state
[protected]
 

Predict the state to this node.

Definition at line 190 of file TrackKalmanFilter.cpp.

References failure(), m_debugLevel, m_extrapolator, ITrackExtrapolator::propagate(), ITrackExtrapolator::transportMatrix(), and Measurement::z().

Referenced by filter().

00190                                                                    {
00191 
00192   Measurement& thisMeasure = aNode.measurement();
00193 
00194   // only use extrapolator in first iteration; else use stored parameters
00195   HepVector prevStateVec = aState.stateVector();
00196   HepSymMatrix prevStateCov = aState.covariance();
00197   double z = thisMeasure.z();
00198   // StatusCode sc = m_extrapolator->propagate(state,z,m_particleID);
00199   StatusCode sc = m_extrapolator->propagate(aState,z);
00200   if (sc.isFailure()) 
00201     return failure(" Unable to predit state at next measurement");
00202 
00203   const HepMatrix& F = m_extrapolator->transportMatrix();
00204   aNode.setTransportMatrix( F );
00205   aNode.setTransportVector( aState.stateVector() - F * prevStateVec );
00206   aNode.setNoiseMatrix( aState.covariance() - prevStateCov.similarity(F) );
00207 
00208   // save predicted state
00209   aNode.setPredictedState(aState);
00210 
00211   if ( m_debugLevel ) {
00212     debug() << " predicted  state at z \t" << z << endreq;
00213     debug() << " predicted  state vector  \t" << aState.stateVector() << endreq;
00214     debug() << " predicted  state cov \t" << aState.covariance() << endreq;
00215     debug() << " transport matrix \t" << aNode.transportMatrix() << endreq;
00216     debug() << " transport vector \t" << aNode.transportVector() << endreq;
00217     debug() << " noise matrix \t" << aNode.noiseMatrix() << endreq;
00218   } 
00219 
00220   return StatusCode::SUCCESS;
00221 }

StatusCode TrackKalmanFilter::smooth FitNode node0,
const FitNode node1
[protected]
 

smooth 2 nodes

Definition at line 338 of file TrackKalmanFilter.cpp.

References checkInvertMatrix(), checkPositiveMatrix(), ITrackProjector::chi2(), ITrackProjector::errResidual(), failure(), invertMatrix(), m_debugLevel, m_projector, ITrackProjector::project(), ITrackProjector::projectionMatrix(), and ITrackProjector::residual().

Referenced by smoother().

00340 {
00341   debug() << "just entered smooth(node0,node1)" << endreq;  
00342   // preliminaries, first we need to invert the _predicted_ covariance
00343   // matrix at k+1
00344   const HepVector&    prevNodeX = prevNode.predictedState().stateVector();
00345   const HepSymMatrix& prevNodeC = prevNode.predictedState().covariance();
00346 
00347   debug() << "calling checkInvertMatrix ..." << endreq;  
00348   // check that the elements are not too large else dsinv will crash
00349   StatusCode sc = checkInvertMatrix(prevNodeC);
00350   if ( sc.isFailure() ) return failure(" not valid matrix in smoother"); 
00351 
00352   debug() << "inverting matrix ..." << endreq;  
00353   //invert the covariance matrix
00354   HepSymMatrix invPrevNodeC = prevNodeC;
00355   sc = invertMatrix(invPrevNodeC);
00356   if ( sc.isFailure() ) return failure(" inverting matrix in smoother");
00357   debug() << "matrix inverted ..." << endreq;  
00358 
00359   //references to _predicted_ state + cov of this node from the first step
00360   HepVector&    thisNodeX = thisNode.state().stateVector();
00361   HepSymMatrix& thisNodeC = thisNode.state().covariance();
00362 
00363   //transport
00364   const HepMatrix& F = prevNode.transportMatrix();
00365 
00366   debug() << "calculation of gain matrix A ..." << endreq;  
00367   //calculate gain matrix A
00368   HepMatrix A = thisNodeC * F.T() * invPrevNodeC;
00369 
00370   // best = smoothed state at prev Node
00371   const HepVector&    prevNodeSmoothedX = prevNode.state().stateVector();
00372   const HepSymMatrix& prevNodeSmoothedC = prevNode.state().covariance();
00373 
00374   //smooth state
00375   thisNodeX += A * (prevNodeSmoothedX - prevNodeX);
00376 
00377   // smooth covariance  matrix
00378   HepSymMatrix covDiff = prevNodeSmoothedC- prevNodeC;
00379   HepSymMatrix covUpDate = covDiff.similarity(A);
00380   thisNodeC += covUpDate;
00381 
00382   debug() << "calling checkPositiveMatrix ..." << endreq;  
00383   // check that the cov matrix is positive
00384   sc = checkPositiveMatrix(thisNodeC);
00385   if (sc.isFailure()) return Warning("non-positive Matrix in smoother");
00386   debug() << "checkPositiveMatrix done" << endreq;  
00387 
00388   // update = smooth the residuals
00389   debug() << "dims of H is" 
00390           << thisNode.projectionMatrix().num_row() << "x"
00391           << thisNode.projectionMatrix().num_col() << endreq;
00392   const HepVector& H = thisNode.projectionMatrix();
00393   debug() << "projecting ..." << endreq;
00394   sc = m_projector->project(thisNode.state(),thisNode.measurement());
00395   if (sc.isFailure()) Warning(" unable to project the smooth node");
00396   double res = m_projector->residual();
00397   thisNode.setResidual(res);
00398   double errRes = thisNode.errResidual();
00399   double errRes2 = errRes*errRes -  covUpDate.similarity(H);
00400   debug() << "errRes2 = " << errRes2 << endreq;
00401   if (errRes2 >0.) errRes = sqrt(errRes2);
00402   else return Warning( "Negative residual error in smoother!" );
00403   thisNode.setErrResidual(sqrt(errRes));
00404 
00405   if ( m_debugLevel ) {
00406     debug() << " A matrix \t" << A << endreq
00407             << " smooth state vector \t" << thisNodeX << endreq
00408             << " smooth state covariance \t"<< thisNodeC << endreq
00409             << " smooth residual, error and chi2 \t" << res << ", "
00410             << errRes << ", " << thisNode.chi2() << endreq;      
00411   }
00412 
00413   return StatusCode::SUCCESS;
00414 }

StatusCode TrackKalmanFilter::smoother Track track  )  [protected]
 

smooth the track

Definition at line 419 of file TrackKalmanFilter.cpp.

References computeChiSq(), failure(), m_nodes, and smooth().

Referenced by fit().

00420 {
00421   StatusCode sc = StatusCode::SUCCESS;
00422   // smoother loop - runs in opposite direction to filter
00423   std::vector<FitNode*>::reverse_iterator riNode = m_nodes.rbegin();
00424   FitNode* oldNode = *riNode;
00425   ++riNode;
00426 
00427   debug() << "starting loop over nodes..." << endreq;  
00428   while (riNode != m_nodes.rend()) {
00429     debug() << " call to smooth(node0,node1)..." << endreq;  
00430     sc = smooth( *(*riNode), *oldNode );
00431     if ( sc.isFailure() ) return failure("Unable to smooth node!");      
00432     oldNode = *riNode;
00433     ++riNode;
00434   }
00435 
00436   computeChiSq(track);
00437 
00438   return StatusCode::SUCCESS;
00439 }


Member Data Documentation

double TrackKalmanFilter::m_chi2Outliers [private]
 

chi2 of outliers to be removed

Definition at line 122 of file TrackKalmanFilter.h.

Referenced by TrackKalmanFilter().

bool TrackKalmanFilter::m_debugLevel [private]
 

Definition at line 128 of file TrackKalmanFilter.h.

Referenced by computeChiSq(), filter(), iniKalman(), initialize(), predict(), and smooth().

ITrackExtrapolator* TrackKalmanFilter::m_extrapolator [protected]
 

extrapolator

Definition at line 108 of file TrackKalmanFilter.h.

Referenced by filter(), initialize(), and predict().

std::string TrackKalmanFilter::m_extrapolatorName [private]
 

name of the extrapolator in Gaudi

Definition at line 116 of file TrackKalmanFilter.h.

Referenced by initialize(), and TrackKalmanFilter().

HepVector TrackKalmanFilter::m_K [protected]
 

Definition at line 103 of file TrackKalmanFilter.h.

Referenced by filter(), and TrackKalmanFilter().

std::vector<FitNode*> TrackKalmanFilter::m_nodes [protected]
 

Definition at line 100 of file TrackKalmanFilter.h.

Referenced by computeChiSq(), filter(), iniKalman(), smoother(), and TrackKalmanFilter().

int TrackKalmanFilter::m_numFitIter [private]
 

number of fit iterations to perform

Definition at line 121 of file TrackKalmanFilter.h.

Referenced by TrackKalmanFilter().

ITrackProjector* TrackKalmanFilter::m_projector [protected]
 

projector

Definition at line 111 of file TrackKalmanFilter.h.

Referenced by filter(), initialize(), and smooth().

std::string TrackKalmanFilter::m_projectorName [private]
 

name of the projector in Gaudi

Definition at line 117 of file TrackKalmanFilter.h.

Referenced by initialize(), and TrackKalmanFilter().

State* TrackKalmanFilter::m_state [protected]
 

internal clone of the state (pointer onwer by Kalman)

Definition at line 97 of file TrackKalmanFilter.h.

Referenced by computeChiSq(), filter(), iniKalman(), and TrackKalmanFilter().

bool TrackKalmanFilter::m_stateAtBeamLine [private]
 

add state closest to the beam-line?

Definition at line 120 of file TrackKalmanFilter.h.

Referenced by TrackKalmanFilter().

bool TrackKalmanFilter::m_statesAtMeasZPos [private]
 

store states at measurement-positions?

Definition at line 119 of file TrackKalmanFilter.h.

Referenced by TrackKalmanFilter().

double TrackKalmanFilter::m_storeTransport [private]
 

store the transport of the extrapolator

Definition at line 123 of file TrackKalmanFilter.h.

Referenced by TrackKalmanFilter().

std::vector<double> TrackKalmanFilter::m_zPositions [private]
 

z-positions at which to determine the states

Definition at line 118 of file TrackKalmanFilter.h.

Referenced by TrackKalmanFilter().


The documentation for this class was generated from the following files:
Generated on Mon Jul 4 13:54:50 2005 for New Track Event Model by doxygen 1.4.1