KalmanFilter Class Reference

#include <KalmanFilter.h>

Inheritance diagram for KalmanFilter:

List of all members.

Public Member Functions
	KalmanFilter (const std::string &type, const std::string &name, const IInterface *parent)
	Standard constructor.
virtual	~KalmanFilter ()
	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 fit7/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, FitNode &node1)
	smooth 2 nodes
void	computeChiSq (Track &track)
	compute the chi2
StatusCode	failure (const std::string &comment)
	print comment of failure
StatusCode	checkInvertMatrix (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
State *	m_state
	internal clone of the state (pointer onwer by Kalman)
std::vector< FitNode * >	m_nodes
HepVector	m_K
ITrackExtrapolator *	m_extrapolator
	extrapolator
ITrackProjector *	m_projector
	projector
std::string	m_extrapolatorName
	name of the extrapolator in Gaudi
std::string	m_projectorName
	name of the projector in Gaudi

---

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 25 of file KalmanFilter.h.

---

Constructor & Destructor Documentation

KalmanFilter::KalmanFilter (  const std::string &  type, const std::string &  name, const IInterface *  parent )

Standard constructor. Definition at line 24 of file KalmanFilter.cpp. References m_extrapolatorName, m_K, m_nodes, m_projectorName, and m_state. : GaudiTool( type, name, parent) { declareInterface<ITrackFitter>( this ); declareProperty("Extrapolator",m_extrapolatorName = "MasterExtrapolator"); declareProperty("Projector",m_projectorName = "MasterProjector"); m_state = NULL; m_nodes.clear(); m_K = HepVector(5,0); }

KalmanFilter::~KalmanFilter (   )  [virtual]

Destructor. Definition at line 40 of file KalmanFilter.cpp. { }

---

Member Function Documentation

StatusCode KalmanFilter::checkInvertMatrix (  HepSymMatrix &  mat  )  [protected]

Definition at line 363 of file KalmanFilter.cpp. Referenced by smooth(). { int nParam = (int) mat.num_row(); for (int i=0; i<nParam; ++i){ for (int j=0; j<nParam; ++j){ if (mat[i][j] > 1e20) return Warning("covariance errors too big to invert"); } // j } // i return StatusCode::SUCCESS; }

StatusCode KalmanFilter::checkPositiveMatrix (  HepSymMatrix &  mat  )  [protected]

Definition at line 375 of file KalmanFilter.cpp. Referenced by smooth(). { int nParam = (int) mat.num_row(); for (int i=0; i<nParam; ++i){ if (mat[i][i] <= 0.) return Warning("covariance errors too big to convert"); } return StatusCode::SUCCESS; }

void KalmanFilter::computeChiSq (  Track &  track  )  [protected]

compute the chi2 Definition at line 151 of file KalmanFilter.cpp. References Node::chi2(), m_nodes, m_state, and State::nParameters(). Referenced by filter(), and smoother(). { double chi2 = 0.; for (std::vector<FitNode*>::iterator it = m_nodes.begin(); it != m_nodes.end(); it++) chi2 += (*it)->chi2(); int ndof = -(m_state->nParameters()); ndof += m_nodes.size(); double chi2Norma = 0.; if (ndof != 0) chi2Norma = chi2 /((float) (ndof)); track.setChi2PerDoF(chi2Norma); track.setNDoF(ndof); if (msgLevel(MSG::DEBUG)) { debug() << " track ndof \t" << track.chi2() << endreq; debug() << " track chi2, chi2/ndof \t " << track.chi2() << ", " << track.chi2PerDoF() << endreq; } }

StatusCode KalmanFilter::cToG3 (  HepSymMatrix &  C  )  [protected]

change of units to facilitate inversion Definition at line 423 of file KalmanFilter.cpp. Referenced by invertMatrix(). { // cov matrix C[0][0] /= cm2; C[0][1] /= cm2; C[0][2] /= cm; C[0][3] /= cm; C[0][4] = C[0][4]*GeV/cm; C[1][1] /= cm2; C[1][2] /= cm; C[1][3] /= cm; C[1][4] = C[1][4]*GeV/cm; C[2][4] = C[2][4]*GeV; C[3][4] = C[3][4]*GeV; C[4][4] = C[4][4]*GeV*GeV; return StatusCode::SUCCESS; }

StatusCode KalmanFilter::cToG4 (  HepSymMatrix &  invC  )  [protected]

change of units to facilitate inversion Definition at line 450 of file KalmanFilter.cpp. Referenced by invertMatrix(). { // cov matrix invC[0][0] /= cm2; invC[0][1] /= cm2; invC[0][2] /= cm; invC[0][3] /= cm; invC[0][4] = invC[0][4]*GeV/cm; invC[1][1] /= cm2; invC[1][2] /= cm; invC[1][3] /= cm; invC[1][4] = invC[1][4]*GeV/cm; invC[2][4] = invC[2][4]*GeV; invC[3][4] = invC[3][4]*GeV; invC[4][4] = invC[4][4]*GeV*GeV; return StatusCode::SUCCESS; }

StatusCode KalmanFilter::failure (  const std::string &  comment  )  [protected]

print comment of failure Definition at line 52 of file KalmanFilter.cpp. Referenced by filter(), fit(), predict(), smooth(), and smoother(). { info() << " Kalman Filter failure: " << comment << endreq; return StatusCode::FAILURE; }

StatusCode KalmanFilter::filter (  FitNode &  node  )  [protected]

filter this node Definition at line 208 of file KalmanFilter.cpp. References Measurement::errMeasure(), ITrackProjector::errResidual(), failure(), filter(), m_K, m_projector, ITrackProjector::projectionMatrix(), and ITrackProjector::residual(). { Measurement& meas = node.measurement(); State& state = node.state(); StatusCode sc = filter(state, meas); if (sc.isFailure()) return failure("Not able to filter the node!"); double tMeasureError = meas.errMeasure(); double res = m_projector->residual(); const HepVector& H = m_projector->projectionMatrix(); double HK = dot ( H, m_K); res *= (1 - HK); double errorRes = (1 - HK) * pow(tMeasureError, 2.0); node.setResidual(res); node.setErrResidual(sqrt(errorRes)); // save predicted state node.setFilteredState(state); if (msgLevel(MSG::DEBUG)) { debug() << " filtered residual and error \t" << node.residual() << ", " <<node.errResidual() << endreq;; } return StatusCode::SUCCESS; }

StatusCode KalmanFilter::filter (  State &  state, Measurement &  meas )  [virtual]

filter/update state with this measurement Implements ITrackFitter. Definition at line 244 of file KalmanFilter.cpp. References ITrackProjector::errResidual(), failure(), m_K, m_projector, ITrackProjector::project(), ITrackProjector::projectionMatrix(), and ITrackProjector::residual(). { // check z position if ( fabs(meas.z() - state.z()) > 1e-6) { warning() << "z position of TrState (=" << state.z() << ") and TrStateMeasurement (= " << meas.z() << ") are not equal." << endreq; } // get reference to the state vector and cov HepVector& tX = state.stateVector(); HepSymMatrix& tC = state.covariance(); // project the state into the measurement StatusCode sc = m_projector->project(state,meas); if (sc.isFailure()) return failure(" Not able to project a state into a measurement"); // calculate predicted residuals double errorMeas = meas.errMeasure(); double res = m_projector->residual(); double errorRes = m_projector->errResidual(); double errorRes2 = (errorRes*errorRes); const HepVector& H = m_projector->projectionMatrix(); // calculate gain matrix K m_K = (tC * H)/errorRes2; // update state vector tX += m_K * res; HepMatrix B = HepDiagMatrix(tC.num_row(), 1) - ( m_K * H.T()); tC.assign( B * tC * B.T() + ( m_K * pow(errorMeas, 2.0) * m_K.T())); if (msgLevel(MSG::DEBUG)) { debug() << " measure and error \t" << meas.measure() << ", " << errorMeas << endreq; debug() << " residual and error \t" << res << ", " << errorRes << endreq; debug() << " projection matrix \t" << H << endreq; debug() << " gain matrix \t" << m_K << endreq; debug() << " filter state vector \t" << tX << endreq; debug() << " filter state covariance \t"<< tC << endreq; } return sc; }

StatusCode KalmanFilter::filter (  Track &  track, const State &  seed )  [virtual]

filter the track (only filter) Implements ITrackFitter. Definition at line 103 of file KalmanFilter.cpp. References State::clone(), computeChiSq(), failure(), iniKalman(), m_extrapolator, m_nodes, m_state, predict(), ITrackExtrapolator::propagate(), and State::z(). Referenced by filter(), and fit(). { StatusCode sc = StatusCode::SUCCESS; sc = iniKalman(track); if (sc.isFailure()) return failure(" Not able to create Nodes"); m_state = state.clone(); std::vector<FitNode*>::iterator iNode = m_nodes.begin(); double z = (*iNode)->measurement().z(); // sc = m_extrapolator->propagate(*m_state,z,m_particleID); sc = m_extrapolator->propagate(*m_state,z); if (sc.isFailure()) return failure(" Not able to extrapolate to 1st measurement"); // TODO: handle different iterations for (iNode = m_nodes.begin(); iNode != m_nodes.end(); ++iNode) { FitNode& node = *(*iNode); sc = predict(node,*m_state); if (sc.isFailure()) return failure(" not able to predict at node"); sc = filter(node); if (sc.isFailure()) return failure(" not able to filter node "); } computeChiSq(track); return sc; }

StatusCode KalmanFilter::fit (  Track &  track, const State &  seed )  [virtual]

fit the track (filter and smoother) Implements ITrackFitter. Definition at line 61 of file KalmanFilter.cpp. References failure(), filter(), and smoother(). { StatusCode sc = filter(track,state); if (sc.isFailure()) return failure(" No able to filter the track"); return sc = smoother(track); if (sc.isFailure()) return failure(" No able to smooth the track"); return sc; }

StatusCode KalmanFilter::fit (  Track &  track  )  [inline, virtual]

fit the track (filter and smoother) Implements ITrackFitter. Definition at line 38 of file KalmanFilter.h. { info() << " not implemented yet!" << track.nMeasurements() << endreq; return StatusCode::SUCCESS; }

StatusCode KalmanFilter::iniKalman (  Track &  track  )  [protected]

initialize the filter to fit7/filter his track Definition at line 72 of file KalmanFilter.cpp. References State::covariance(), m_nodes, m_state, and State::stateVector(). Referenced by filter(). { // destroy the previous state if not null if (m_state != NULL) delete m_state; // Kalman has private copy of the nodes to acelerate the computations // remember that the track owns the Nodes, so it is responsability of the // Track destructor to delete them! m_nodes.clear(); std::vector<Measurement*>& measures = track.measurements(); std::vector<Node*>& nodes = track.nodes(); if (nodes.size() > 0) for (std::vector<Node*>::iterator it = nodes.begin(); it != nodes.end(); it++) delete *it; nodes.clear(); for (std::vector<Measurement*>::iterator it = measures.begin(); it != measures.end(); it++) { Measurement& meas = *(*it); FitNode* node = new FitNode(meas); m_nodes.push_back(node); track.addToNodes(node); } if (msgLevel(MSG::DEBUG)) { debug() << " seed state vector \t" << m_state->stateVector() << endreq; debug() << " seed state cov \t" << m_state->covariance() << endreq; debug() << " track nodes size \t" << m_nodes.size() << endreq; } return StatusCode::SUCCESS; }

StatusCode KalmanFilter::initialize (   )

Definition at line 43 of file KalmanFilter.cpp. References m_extrapolator, m_extrapolatorName, m_projector, and m_projectorName. { m_extrapolator = get<ITrackExtrapolator>(m_extrapolatorName); m_projector = get<ITrackProjector>(m_projectorName); return StatusCode::SUCCESS; }

StatusCode KalmanFilter::invertMatrix (  HepSymMatrix &  mat  )  [protected]

Definition at line 397 of file KalmanFilter.cpp. References cToG3(), and cToG4(). Referenced by smooth(). { StatusCode sc = StatusCode::SUCCESS; // G3 units cToG3(invPrevNodeC); HepSymMatrix tempMatrix = invPrevNodeC; int ifail; invPrevNodeC.invert(ifail); //G4 units cToG4(invPrevNodeC); if (ifail !=0){ warning() <<"failed to invert covariance matrix, failure code " <<ifail<<endreq; sc = StatusCode::FAILURE; } return sc; }

StatusCode KalmanFilter::predict (  FitNode &  node, State &  state )  [protected]

predict the state to this node Definition at line 175 of file KalmanFilter.cpp. References failure(), m_extrapolator, Node::measurement(), ITrackExtrapolator::propagate(), ITrackExtrapolator::transportMatrix(), and Measurement::z(). Referenced by filter(). { Measurement& thisMeasure = aNode.measurement(); // only use extrapolator in first iteration; else use stored parameters HepVector prevStateVec = aState.stateVector(); HepSymMatrix prevStateCov = aState.covariance(); double z = thisMeasure.z(); // StatusCode sc = m_extrapolator->propagate(state,z,m_particleID); StatusCode sc = m_extrapolator->propagate(aState,z); if (sc.isFailure()) return failure(" No able to predit state at next measurement"); const HepMatrix& F = m_extrapolator->transportMatrix(); aNode.setTransportMatrix( F ); aNode.setTransportVector( aState.stateVector() - F * prevStateVec ); aNode.setNoiseMatrix( aState.covariance() - prevStateCov.similarity(F) ); // save predicted state aNode.setPredictedState(aState); if (msgLevel(MSG::DEBUG)) { debug() << " predicted state at z \t" << z << endreq; debug() << " predicted state vector \t" << aState.stateVector() << endreq; debug() << " predicted state cov \t" << aState.covariance() << endreq; debug() << " transport matrix \t" << aNode.transportMatrix() << endreq; debug() << " transport vector \t" << aNode.transportVector() << endreq; debug() << " noise matrix \t" << aNode.noiseMatrix() << endreq; } return StatusCode::SUCCESS; }

StatusCode KalmanFilter::smooth (  FitNode &  node0, FitNode &  node1 )  [protected]

smooth 2 nodes Definition at line 298 of file KalmanFilter.cpp. References checkInvertMatrix(), checkPositiveMatrix(), ITrackProjector::chi2(), ITrackProjector::errResidual(), failure(), invertMatrix(), m_projector, ITrackProjector::project(), ITrackProjector::projectionMatrix(), and ITrackProjector::residual(). Referenced by smoother(). { // preliminaries, first we need to invert the _predicted_ covariance // matrix at k+1 HepVector& prevNodeX = prevNode.predictedState().stateVector(); HepSymMatrix& prevNodeC = prevNode.predictedState().covariance(); // check that the elements are not too large else dsinv will crash StatusCode sc = checkInvertMatrix(prevNodeC);; if ( sc.isFailure() ) return failure(" not valid matrix in smoother"); //invert the covariance matrix HepSymMatrix invPrevNodeC = prevNodeC; sc = invertMatrix(invPrevNodeC); if ( sc.isFailure() ) return failure(" inverting matrix in smoother"); //references to _predicted_ state + cov of this node from the first step HepVector& thisNodeX = thisNode.state().stateVector(); HepSymMatrix& thisNodeC = thisNode.state().covariance(); //transport const HepMatrix& F = prevNode.transportMatrix(); //calculate gain matrix A HepMatrix A = thisNodeC * F.T() * invPrevNodeC; // best = smoothed state at prev Node HepVector& prevNodeSmoothedX = prevNode.state().stateVector(); HepSymMatrix& prevNodeSmoothedC = prevNode.state().covariance(); //smooth state thisNodeX += A * (prevNodeSmoothedX - prevNodeX); // smooth covariance matrix HepSymMatrix covDiff = prevNodeSmoothedC- prevNodeC; HepSymMatrix covUpDate = covDiff.similarity(A); thisNodeC += covUpDate; // check that the cov matrix is possitive sc = checkPositiveMatrix(thisNodeC); if (sc.isFailure()) return Warning(" not postive Matrix in smoother"); // update = smooth the residuals const HepVector& H = thisNode.projectionMatrix(); sc = m_projector->project(thisNode.state(),thisNode.measurement()); if (sc.isFailure()) Warning(" unable to project the smooth node"); double res = m_projector->residual(); thisNode.setResidual(res); double errRes = thisNode.errResidual(); double errRes2 = errRes*errRes - covUpDate.similarity(H); if (errRes2 >0.) errRes = sqrt(errRes2); else return Warning(" negative error residuals in smoother"); thisNode.setErrResidual(sqrt(errRes)); if (msgLevel(MSG::DEBUG)) { debug() << " A matrix \t" << A << endreq;; debug() << " smooth state vector \t" << thisNodeX << endreq; debug() << " smooth state covariance \t"<< thisNodeC << endreq; debug() << " smooth residual, error and chi2 \t" << res << ", " << errRes << ", " << thisNode.chi2() << endreq; } return StatusCode::SUCCESS; }

StatusCode KalmanFilter::smoother (  Track &  track  )  [protected]

smooth the track Definition at line 131 of file KalmanFilter.cpp. References computeChiSq(), failure(), m_nodes, and smooth(). Referenced by fit(). { StatusCode sc = StatusCode::SUCCESS; // smoother loop - runs in opposite direction to filter std::vector<FitNode*>::reverse_iterator riNode = m_nodes.rbegin(); FitNode* oldNode = *riNode; ++riNode; while (riNode != m_nodes.rend()) { sc = smooth( *(*riNode), *oldNode ); if (sc.isFailure()) return failure(" not able to smooth node"); oldNode = *riNode; ++riNode; } computeChiSq(track); return StatusCode::SUCCESS; }

---

Member Data Documentation

ITrackExtrapolator* KalmanFilter::m_extrapolator [protected]

extrapolator Definition at line 106 of file KalmanFilter.h. Referenced by filter(), initialize(), and predict().

std::string KalmanFilter::m_extrapolatorName [protected]

name of the extrapolator in Gaudi Definition at line 112 of file KalmanFilter.h. Referenced by initialize(), and KalmanFilter().

HepVector KalmanFilter::m_K [protected]

Definition at line 101 of file KalmanFilter.h. Referenced by filter(), and KalmanFilter().

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

Definition at line 98 of file KalmanFilter.h. Referenced by computeChiSq(), filter(), iniKalman(), KalmanFilter(), and smoother().

ITrackProjector* KalmanFilter::m_projector [protected]

projector Definition at line 109 of file KalmanFilter.h. Referenced by filter(), initialize(), and smooth().

std::string KalmanFilter::m_projectorName [protected]

name of the projector in Gaudi Definition at line 115 of file KalmanFilter.h. Referenced by initialize(), and KalmanFilter().

State* KalmanFilter::m_state [protected]

internal clone of the state (pointer onwer by Kalman) Definition at line 95 of file KalmanFilter.h. Referenced by computeChiSq(), filter(), iniKalman(), and KalmanFilter().

---

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

• KalmanFilter.h
• KalmanFilter.cpp

---