JLine1ZEstimator.hh

Go to the documentation of this file.

#ifndef __JFIT__JLINE1ZESTIMATOR__
#define __JFIT__JLINE1ZESTIMATOR__

#include "JPhysics/JConstants.hh"         
#include "JMath/JMatrix3S.hh"
#include "JMath/JSVD3D.hh"
#include "JFit/JMatrixNZ.hh"
#include "JFit/JLine1Z.hh"
#include "JFit/JEstimator.hh"



/**
 * \file
 * Linear fit of JFIT::JLine1Z.
 * \author mdejong
 */
namespace JFIT {}
namespace JPP { using namespace JFIT; }

namespace JFIT {

  /**
   * Linear fit of straight line parallel to z-axis to set of hits (objects with position and time).
   *
   \f{center}\setlength{\unitlength}{0.7cm}\begin{picture}(8,12)

   \put( 1.0, 0.5){\vector(0,1){9}}
   \put( 1.0,10.0){\makebox(0,0){$z$}}
   \put( 1.0, 0.0){\makebox(0,0){muon}}

   \put( 1.0, 8.0){\line(1,0){6}}
   \put( 4.0, 8.5){\makebox(0,0)[c]{$R$}}

   \multiput( 1.0, 2.0)(0.5, 0.5){12}{\qbezier(0.0,0.0)(-0.1,0.35)(0.25,0.25)\qbezier(0.25,0.25)(0.6,0.15)(0.5,0.5)}
   \put( 4.5, 4.5){\makebox(0,0)[l]{photon}}

   \put( 1.0, 2.0){\circle*{0.2}}
   \put( 0.5, 2.0){\makebox(0,0)[r]{$(x_{0},y_{0},z_{0},t_{0})$}}

   \put( 1.0, 8.0){\circle*{0.2}}
   \put( 0.5, 8.0){\makebox(0,0)[r]{$(x_{0},y_{0},z_{j})$}}

   \put( 7.0, 8.0){\circle*{0.2}}
   \put( 7.0, 9.0){\makebox(0,0)[c]{PMT}}
   \put( 7.5, 8.0){\makebox(0,0)[l]{$(x_{j},y_{j},z_{j},t_{j})$}}

   \put( 1.1, 2.1){
   \put(0.0,1.00){\vector(-1,0){0.1}}
   \qbezier(0.0,1.0)(0.25,1.0)(0.5,0.75)
   \put(0.5,0.75){\vector(1,-1){0.1}}
   \put(0.4,1.5){\makebox(0,0){$\theta_{c}$}}
   }

   \end{picture}
   \f}

   \f[
         ct_j  =  ct_0  +  (z_j - z_0)  +  \tan(\theta_{c}) \sqrt((x_j - x_0)^2 + (y_j - y_0)^2)
   \f]
   *
   * where:
   *
   \f{eqnarray*}{
          x_0 & = & \textrm{x position of track       (fit parameter)}            \\
          y_0 & = & \textrm{y position of track       (fit parameter)}            \\
          z_0 & = & \textrm{z position of track}                                  \\
          t_0 & = & \textrm{time of track at } z = z_0 \textrm{ (fit parameter)}  \\
                                                                                  \\
          c            & = & \textrm{speed of light in vacuum}                    \\
          \theta_{c}   & = & \textrm{Cherenkov angle}                             \\
   \f}
   *
   * Defining:
   *
   \f{eqnarray*}{
         t_j'  & \equiv &  ct_j / \tan(\theta_{c})  -  (z_j - z_0)/\tan(\theta_{c})   \\
         t_0'  & \equiv &  ct_0 / \tan(\theta_{c})                                    \\
   \f}
   *
   \f[
   \Rightarrow (t_j' - t_0')^2 = (x_j - x_0)^2  +  (y_j - y_0)^2
   \f]
   *
   * The parameters \f$ \{x_0, y_0, t_0\} \f$ are estimated in the constructor of this class based on 
   * consecutive pairs of equations by which the quadratic terms in \f$ x_0 \f$, \f$ y_0 \f$ and \f$ t_0 \f$ are eliminated.
   */
  template<>
  class JEstimator<JLine1Z> : 
    public JLine1Z 
  {
  public:
    /**
     * Default constructor.
     */
    JEstimator() :
      JLine1Z()
    {}


    /**
     * Constructor.
     *
     * \param  __begin    begin of data
     * \param  __end      end   of data
     */
    template<class T>
    JEstimator(T __begin, T __end) :
      JLine1Z()
    {
      (*this)(__begin, __end);
    }


    /**
     * Fit.
     *
     * The template argument <tt>T</tt> refers to an iterator of a data structure which should have the following member methods:
     *   - double %getX();       // [m]
     *   - double %getY();       // [m]
     *   - double %getZ();       // [m]
     *   - double %getT();       // [ns]
     *
     * \param  __begin    begin of data
     * \param  __end      end   of data
     */
    template<class T>
    const JEstimator<JLine1Z>& operator()(T __begin, T __end)
    {
      using namespace std;
      using namespace JPP;

      const int N = distance(__begin, __end);

      if (N >= NUMBER_OF_PARAMETERS) {

        __x = 0.0;
        __y = 0.0;
        __z = 0.0;
        __t = 0.0;

        double t0 = 0.0;

        for (T i = __begin; i != __end; ++i) {
          __x += i->getX();
          __y += i->getY();
          __z += i->getZ();
          t0  += i->getT();
        }
        
        const double W = 1.0/N;

        __x *= W;
        __y *= W;
        __z *= W;
        t0  *= W;

        V.reset();

        t0 *= getSpeedOfLight();

        double y0 = 0.0;
        double y1 = 0.0;
        double y2 = 0.0;

        T j = __begin;

        double xi =  j->getX() - getX();
        double yi =  j->getY() - getY();
        double ti = (j->getT() * getSpeedOfLight()  -  t0  -  j->getZ() + getZ()) / getKappaC();

        for (bool done = false; !done; ) {

          if ((done = (++j == __end))) {
            j = __begin;
          }

          double xj =  j->getX() - getX();
          double yj =  j->getY() - getY();
          double tj = (j->getT() * getSpeedOfLight()  -  t0  -  j->getZ() + getZ()) / getKappaC();
            
          double dx = xj - xi;
          double dy = yj - yi;
          double dt = ti - tj;    // opposite sign!
            
          const double y  = ((xj + xi) * dx +
                             (yj + yi) * dy +
                             (tj + ti) * dt);

          dx *= 2;
          dy *= 2;
          dt *= 2;
            
          V.a00 += dx * dx;
          V.a01 += dx * dy;
          V.a02 += dx * dt;
          V.a11 += dy * dy;
          V.a12 += dy * dt;
          V.a22 += dt * dt;

          y0 += dx * y;
          y1 += dy * y;
          y2 += dt * y;

          xi = xj;
          yi = yj;
          ti = tj;
        }

        t0 *= getInverseSpeedOfLight();

        V.a10 = V.a01;
        V.a20 = V.a02;
        V.a21 = V.a12;

        svd.decompose(V);

        if (fabs(svd.S.a11) < MINIMAL_SVD_WEIGHT * fabs(svd.S.a00)) {
          THROW(JValueOutOfRange, "JEstimator<JLine1Z>::JEstimator(): singular value " << svd.S.a11 << ' ' << svd.S.a00);
        }

        V = svd.invert(MINIMAL_SVD_WEIGHT);
        
        __x += V.a00 * y0  +  V.a01 * y1  +  V.a02 * y2;
        __y += V.a10 * y0  +  V.a11 * y1  +  V.a12 * y2;
        __t  = V.a20 * y0  +  V.a21 * y1  +  V.a22 * y2;
        
        __t *= getKappaC() * getInverseSpeedOfLight();
        __t += t0;

      } else {
        throw JValueOutOfRange("JEstimator<JLine1Z>::JEstimator(): Not enough data points.");
      }

      return *this;
    }


    /**
     * Update track parameters using updated co-variance matrix (e.g.\ matrix with one hit switched off).
     *
     * In this, it is assumed that the changes in x and y positions are small
     * compared to the actual distances between the track and the hits.
     *
     * The template argument <tt>T</tt> refers to an iterator of a data structure which should have the following member methods:
     *   - double %getX();       // [m]
     *   - double %getY();       // [m]
     *   - double %getZ();       // [m]
     *   - double %getT();       // [ns]
     *
     * \param  __begin    begin of data
     * \param  __end      end   of data
     * \param  A          co-variance matrix of hits
     */
    template<class T>
    void update(T __begin, T __end, const JMatrixNZ& A)
    {
      using namespace std;
      using namespace JPP;

      const int N = distance(__begin, __end);

      if (N != (int) A.size()) {
        THROW(JValueOutOfRange, "JEstimator<JLine1Z>::update(): Wrong number of points " << N << " != " << A.size());
      }

      if (N >= NUMBER_OF_PARAMETERS) {

        double x1 = 0.0;
        double y1 = 0.0;
        double t1 = 0.0;

        V.reset();
        
        T i = __begin;
        
        for (size_t row = 0; row != A.size(); ++row, ++i) {
          
          const double dx = i->getX() - getX();
          const double dy = i->getY() - getY();
          
          const double rt = sqrt(dx*dx + dy*dy);
          
          double xr = getKappaC() * getInverseSpeedOfLight();
          double yr = getKappaC() * getInverseSpeedOfLight();
          double tr = 1.0;
          
          if (rt != 0.0) {
            xr *= -dx / rt;
            yr *= -dy / rt;
          }

          T j = __begin;

          for (size_t col = 0; col != A.size(); ++col, ++j) {
            
            const double dx = j->getX() - getX();
            const double dy = j->getY() - getY();
            const double dz = j->getZ() - getZ();
            
            const double rt = sqrt(dx*dx + dy*dy);
            
            double xc = getKappaC() * getInverseSpeedOfLight();
            double yc = getKappaC() * getInverseSpeedOfLight();
            double tc = 1.0;

            if (rt != 0.0) {
              xc *= -dx / rt;
              yc *= -dy / rt;
            }
            
            const double ts = j->getT() - (dz + rt * getKappaC()) * getInverseSpeedOfLight();
            
            const double vs = A(row,col);

            x1 += xr * vs * ts;
            y1 += yr * vs * ts;
            t1 += tr * vs * ts;
                    
            V.a00 += xr * vs * xc;
            V.a01 += xr * vs * yc;
            V.a02 += xr * vs * tc;
            V.a11 += yr * vs * yc;
            V.a12 += yr * vs * tc;
            V.a22 += tr * vs * tc;
          }
        }
        
        V.a10 = V.a01;
        V.a20 = V.a02;
        V.a21 = V.a12;
        
        svd.decompose(V);

        if (fabs(svd.S.a11) < MINIMAL_SVD_WEIGHT * fabs(svd.S.a00)) {
          THROW(JValueOutOfRange, "JEstimator<JLine1Z>::update(): singular value " << svd.S.a11 << ' ' << svd.S.a00);
        }

        V = svd.invert(MINIMAL_SVD_WEIGHT);
        
        __x += V.a00 * x1  +  V.a01 * y1  +  V.a02 * t1;
        __y += V.a10 * x1  +  V.a11 * y1  +  V.a12 * t1;
        __t  = V.a20 * x1  +  V.a21 * y1  +  V.a22 * t1;

      } else {
        THROW(JValueOutOfRange, "JEstimator<JLine1Z>::update(): Not enough data points " << N);
      }
    }


    static const int NUMBER_OF_PARAMETERS  =  3;  //!< number of parameters of fit
    JMATH::JMatrix3S V;                           //!< co-variance matrix of fit parameters
    JMATH::JSVD3D    svd;
    static double    MINIMAL_SVD_WEIGHT;          //!< minimal SVD weight.
  };

  /**
   * Set default minimal SVD weight.
   */
  double JEstimator<JLine1Z>::MINIMAL_SVD_WEIGHT = 1.0e-4;
}

#endif