- #include "Event/TbKalmanPixelMeasurement.h"
- #include "Event/TbKalmanTrack.h"
-
- namespace {
- /// Helper function to filter one hit
- inline double filterX(double& x, double& tx, double& covXX, double& covXTx,
- double& covTxTx, const double xhit,
- const double xhitcov) {
- const double predcovXX = covXX;
- const double predcovXTx = covXTx;
- const double predcovTxTx = covTxTx;
- const double predx = x;
- const double predtx = tx;
-
- // compute the gain matrix
- const double R = xhitcov + predcovXX;
- const double Kx = predcovXX / R;
- const double KTx = predcovXTx / R;
- // update the state vector
- double r = xhit - predx;
- x = predx + Kx * r;
- tx = predtx + KTx * r;
- // update the covariance matrix. we can write it in many ways ...
- covXX /*= predcovXX - Kx * predcovXX */ = (1 - Kx) * predcovXX;
- covXTx /*= predcovXTx - predcovXX * predcovXTx / R */ = (1 - Kx) * predcovXTx;
- covTxTx = predcovTxTx - KTx * predcovXTx;
- // return the chi2
- return r * r / R;
- }
- }
-
- namespace LHCb {
-
- LHCb::ChiSquare TbKalmanPixelMeasurement::filter(State& state) const {
- // filter X and Y independently. could easily be complicated if needed.
-
- double chi2(0);
- Gaudi::SymMatrix4x4& stateCov = state.covariance();
- Gaudi::Vector4& stateVec = state.parameters();
-
- // first X
- chi2 += filterX(stateVec(0), stateVec(2), stateCov(0, 0), stateCov(0, 2),
- stateCov(2, 2), m_x, m_covx);
-
- // then Y
- chi2 += filterX(stateVec(1), stateVec(3), stateCov(1, 1), stateCov(1, 3),
- stateCov(3, 3), m_y, m_covy);
- // chi2 has 2 dofs
-
- return LHCb::ChiSquare(chi2, 2);
- }
-
- // compute residual with respect to given (smoothed) state
- void TbKalmanPixelMeasurement::updateResidual(const State& state) {
- m_residualX = m_x - state.x();
- m_residualY = m_y - state.y();
- int sign = m_isactive ? -1 : +1;
- m_residualCovX = m_covx + sign * state.covariance()(0, 0);
- m_residualCovY = m_covy + sign * state.covariance()(1, 1);
- }
-
- void TbKalmanPixelMeasurement::deactivateMeasurement(bool deactivate) {
- // only do something if this is actually an active hit
- if ((deactivate && m_isactive) || (!deactivate && !m_isactive)) {
- // set type to outlier
- m_isactive = !deactivate;
- // this will take care of upstream and downstream nodes as well:
- // they will be reset to initialized. we need to check this
- // carefully
- resetFilterStatus(Predicted);
- // make sure the KalmanFitResult knows something has changed
- if (parent()) parent()->resetCache();
- // now make sure others do not rely on this one anymore
- if (!hasInfoUpstream(Forward)) resetHasInfoUpstream(Forward);
- if (!hasInfoUpstream(Backward)) resetHasInfoUpstream(Backward);
- }
- }
-
- }