KalmanGhostTrackUpdater.cc

Go to the documentation of this file.

#include <vector>
#include <cmath>

#include <Math/SVector.h>
#include <Math/SMatrix.h>
#include <Math/MatrixFunctions.h>

#include "DataFormats/GeometryVector/interface/GlobalPoint.h"
#include "DataFormats/GeometryVector/interface/GlobalVector.h"
#include "DataFormats/GeometryCommonDetAlgo/interface/GlobalError.h"

#include "RecoVertex/GhostTrackFitter/interface/GhostTrackPrediction.h"
#include "RecoVertex/GhostTrackFitter/interface/GhostTrackState.h"

#include "RecoVertex/GhostTrackFitter/interface/KalmanGhostTrackUpdater.h"

using namespace reco;

namespace {
#ifndef __clang__
  static inline double sqr(double arg) { return arg * arg; }
#endif
  using namespace ROOT::Math;

  typedef SVector<double, 4> Vector4;
  typedef SVector<double, 2> Vector2;

  typedef SMatrix<double, 4, 4, MatRepSym<double, 4> > Matrix4S;
  typedef SMatrix<double, 3, 3, MatRepSym<double, 3> > Matrix3S;
  typedef SMatrix<double, 2, 2, MatRepSym<double, 2> > Matrix2S;
  typedef SMatrix<double, 4, 4> Matrix44;
  typedef SMatrix<double, 4, 2> Matrix42;
  typedef SMatrix<double, 2, 4> Matrix24;
  typedef SMatrix<double, 2, 3> Matrix23;
  typedef SMatrix<double, 2, 2> Matrix22;

  struct KalmanState {
    KalmanState(const GhostTrackPrediction &pred, const GhostTrackState &state);

    Vector2 residual;
    Matrix2S measErr, measPredErr;
    Matrix24 h;
  };
}  // namespace

KalmanState::KalmanState(const GhostTrackPrediction &pred, const GhostTrackState &state) {
  using namespace ROOT::Math;

  GlobalPoint point = state.globalPosition();

  // precomputed values
  double x = std::cos(pred.phi());
  double y = std::sin(pred.phi());
  double dz = pred.cotTheta();
  double lip = x * point.x() + y * point.y();
  double tip = x * point.y() - y * point.x();

  // jacobian of global -> local
  Matrix23 measToLocal;
  measToLocal(0, 0) = -dz * x;
  measToLocal(0, 1) = -dz * y;
  measToLocal(0, 2) = 1.;
  measToLocal(1, 0) = y;
  measToLocal(1, 1) = -x;

  // measurement error on the 2d plane projection
  measErr = Similarity(measToLocal, state.cartesianCovariance());

  // jacobian of representation to measurement transformation
  h(0, 0) = 1.;
  h(0, 2) = lip;
  h(0, 3) = dz * tip;
  h(1, 1) = -1.;
  h(1, 3) = -lip;

  // error on prediction
  measPredErr = Similarity(h, pred.covariance());

  // residual
  residual[0] = point.z() - pred.z() - dz * lip;
  residual[1] = pred.ip() - tip;
}

GhostTrackPrediction KalmanGhostTrackUpdater::update(const GhostTrackPrediction &pred,
                                                     const GhostTrackState &state,
                                                     double &ndof,
                                                     double &chi2) const {
  using namespace ROOT::Math;

  KalmanState kalmanState(pred, state);

  if (state.weight() < 1.0e-3)
    return pred;

  // inverted combined error
  Matrix2S invErr = kalmanState.measPredErr + (1.0 / state.weight()) * kalmanState.measErr;
  if (!invErr.Invert())
    return pred;

  // gain
  Matrix42 gain = pred.covariance() * Transpose(kalmanState.h) * invErr;

  // new prediction
  Vector4 newPred = pred.prediction() + (gain * kalmanState.residual);
  Matrix44 tmp44 = SMatrixIdentity();
  tmp44 = (tmp44 - gain * kalmanState.h) * pred.covariance();
  Matrix4S newError(tmp44.LowerBlock());

  // filtered residuals
  Matrix22 tmp22 = SMatrixIdentity();
  tmp22 = tmp22 - kalmanState.h * gain;
  Vector2 filtRes = tmp22 * kalmanState.residual;
  tmp22 *= kalmanState.measErr;
  Matrix2S filtResErr(tmp22.LowerBlock());
  if (!filtResErr.Invert())
    return pred;

  ndof += state.weight() * 2.;
  chi2 += state.weight() * Similarity(filtRes, filtResErr);

  return GhostTrackPrediction(newPred, newError);
}

void KalmanGhostTrackUpdater::contribution(const GhostTrackPrediction &pred,
                                           const GhostTrackState &state,
                                           double &ndof,
                                           double &chi2,
                                           bool withPredError) const {
  using namespace ROOT::Math;

  KalmanState kalmanState(pred, state);

  // this is called on the full predicted state,
  // so the residual is already with respect to the filtered state

  // inverted error
  Matrix2S invErr = kalmanState.measErr;
  if (withPredError)
    invErr += kalmanState.measPredErr;
  if (!invErr.Invert()) {
    ndof = 0.;
    chi2 = 0.;
  }

  ndof = 2.;
  chi2 = Similarity(kalmanState.residual, invErr);
}