KFTrackState.cxx

Go to the documentation of this file.

#include "KFTrackState.h"

using namespace trkf;

bool KFTrackState::updateWithHitState(const HitState& hitstate) {
  // if track and hit not on same plane do not update and return false
  if ( (hitstate.plane().position()-fTrackState.plane().position()).Mag2()>10e-6   ) return false;
  if ( (hitstate.plane().direction()-fTrackState.plane().direction()).Mag2()>10e-6 ) return false;
  // Kalman Filter update (simplified case: 1D measurement along the same coordinate as element 0 of the track parameters)
  SMatrixSym55 tmp;
  tmp(0,0) = 1./(hitstate.hitMeasErr2()+fTrackState.covariance()(0,0));
  fTrackState.setParameters( fTrackState.parameters() + fTrackState.covariance()*tmp.Col(0)*(hitstate.hitMeas() - fTrackState.parameters()(0)) );
  fTrackState.setCovariance( (fTrackState.covariance()-ROOT::Math::Similarity(fTrackState.covariance(),tmp)) );
  return true;
}

bool KFTrackState::combineWithTrackState(const TrackState& trackstate) {
  // if tracks not on same plane do not update and return false
  if ( (trackstate.plane().position()-fTrackState.plane().position()).Mag2()>10e-6   ) return false;
  if ( (trackstate.plane().direction()-fTrackState.plane().direction()).Mag2()>10e-6 ) return false;
  // compute the weighted average of the two states
  const SVector5& par1 = fTrackState.parameters();
  const SVector5& par2 = trackstate.parameters();
  const SMatrixSym55& cov1 = fTrackState.covariance();
  const SMatrixSym55& cov2 = trackstate.covariance();
  SMatrixSym55&& cov = cov1 + cov2;
  bool success = cov.Invert();
  if (!success) return false;
  SMatrix55 K = cov1 * cov;
  fTrackState.setParameters( par1 + K*(par2 - par1) );
  K = K*cov2;
  fTrackState.setCovariance( K.LowerBlock() );
  return true;
}