diff --git a/source/include/HelixClassT.h b/source/include/HelixClassT.h
index cb0e327..e3dc1b1 100644
--- a/source/include/HelixClassT.h
+++ b/source/include/HelixClassT.h
@@ -1,7 +1,6 @@
 #ifndef HELIXCLASST_H
 #define HELIXCLASST_H 1
 
-
 #include <vector>
 #include <cmath>
 
@@ -108,6 +107,23 @@ class HelixClassT {
  */
     void Initialize_Canonical(FloatT phi0, FloatT d0, FloatT z0, FloatT omega,
 			      FloatT tanlambda, FloatT B);
+
+/**
+ *  Canonical (LEP-wise) parameterisation with the following parameters<br>
+ *     - phi0 - Phi angle of momentum vector at the point of<br>
+ *       closest approach to IP in R-Phi plane;
+ *     - d0 - signed distance of closest approach in R-Phi plane;<br>
+ *     - z0 - z coordinate of the point of closest approach to IP
+ *       in R-Phi plane;<br>
+ *     - omega - signed curvature;<br>
+ *     - tanlambda - tangent of dip angle;<br>
+ *     - B - magnetic field (in Tesla)<br>
+ *	   - pos - reference point at which the helix parameteres are given
+ *	     (by default PCA is used as the reference point);<br>
+ */
+  	void Initialize_Canonical(FloatT phi0, FloatT d0, FloatT z0,
+  				  FloatT omega, FloatT tanLambda, FloatT B, FloatT * pos);
+
     /**
      *  Returns momentum of particle at the point of closest approach <br>
      *  to IP <br>
@@ -163,6 +179,16 @@ class HelixClassT {
      */
     FloatT getYC() const { return _yCentre; }
 
+    /**
+     *  Returns x coordinate of point of closest approach
+     */
+    FloatT getXPCA() const { return _xAtPCA; }
+
+    /**
+     *  Returns y coordinate of point of closest approach
+     */
+    FloatT getYPCA() const { return _yAtPCA; }
+
 
      /**
      *  Returns radius of circumference
diff --git a/source/include/HelixClassT.ipp b/source/include/HelixClassT.ipp
index 78b2768..d67cd3b 100644
--- a/source/include/HelixClassT.ipp
+++ b/source/include/HelixClassT.ipp
@@ -3,6 +3,8 @@
 #include <iostream>
 #include "ced_cli.h"
 
+#include <streamlog/streamlog.h>
+
 template<typename FloatT>
 void HelixClassT<FloatT>::Initialize_VP(FloatT * pos, FloatT * mom, FloatT q, FloatT B) {
     _referencePoint[0] = pos[0];
@@ -112,6 +114,34 @@ void HelixClassT<FloatT>::Initialize_Canonical(FloatT phi0, FloatT d0, FloatT z0
     _bField = B;
 }
 
+template<typename FloatT>
+void HelixClassT<FloatT>::Initialize_Canonical(FloatT phi0, FloatT d0, FloatT z0,
+				      FloatT omega, FloatT tanLambda, FloatT B, FloatT* pos) {
+    _omega = omega;
+    _d0 = d0;
+    _phi0 = phi0;
+    _z0 = z0;
+    _tanLambda = tanLambda;
+    _charge = omega/fabs(omega);
+    _radius = 1./fabs(omega);
+    _referencePoint[0] = pos[0];
+    _referencePoint[1] = pos[1];
+    _referencePoint[2] = pos[2];
+    _xAtPCA = -_d0*sin(_phi0) + _referencePoint[0];
+    _yAtPCA = _d0*cos(_phi0) + _referencePoint[1];
+    _pxy = _FCT*B*_radius;
+    _momentum[0] = _pxy*cos(_phi0);
+    _momentum[1] = _pxy*sin(_phi0);
+    _momentum[2] = _tanLambda * _pxy;
+    _pxAtPCA = _momentum[0];
+    _pyAtPCA = _momentum[1];
+    _phiMomRefPoint = atan2(_momentum[1],_momentum[0]);
+    _xCentre = _xAtPCA + _radius*cos(_phi0-_const_pi2*_charge);
+    _yCentre = _yAtPCA + _radius*sin(_phi0-_const_pi2*_charge);
+    _phiAtPCA = atan2(_referencePoint[1]-_yCentre,_referencePoint[0]-_xCentre);
+    _phiRefPoint =  _phiAtPCA ;
+    _bField = B;
+}
 
 template<typename FloatT>
 void HelixClassT<FloatT>::Initialize_BZ(FloatT xCentre, FloatT yCentre, FloatT radius,
@@ -248,10 +278,9 @@ FloatT HelixClassT<FloatT>::getPointInXY(FloatT x0, FloatT y0, FloatT ax, FloatT
   tt2 = -_charge*dphi2*_radius/_pxy;
 
   if (tt1 < 0. )
-    std::cout << "WARNING " << tt1 << std::endl;
+    streamlog_out(WARNING) << "Time : " << tt1 << " < 0!" << std::endl;
   if (tt2 < 0. )
-    std::cout << "WARNING " << tt2 << std::endl;
-
+    streamlog_out(WARNING) << "Time : " << tt2 << " < 0!" << std::endl;
 
   if (tt1 < tt2) {
     point[0] = xx1;
@@ -341,9 +370,9 @@ FloatT HelixClassT<FloatT>::getPointOnCircle(FloatT Radius, FloatT * ref, FloatT
   tt2 = -_charge*dphi2*_radius/_pxy;
 
   if (tt1 < 0. )
-    std::cout << "WARNING " << tt1 << std::endl;
+    streamlog_out(WARNING) << "Time : " << tt1 << " < 0!" << std::endl;
   if (tt2 < 0. )
-    std::cout << "WARNING " << tt2 << std::endl;
+    streamlog_out(WARNING) << "Time : " << tt2 << " < 0!" << std::endl;
 
 
   FloatT time2;
@@ -594,9 +623,6 @@ FloatT HelixClassT<FloatT>::getDistanceToHelix(HelixClassT * helix, FloatT * pos
     FloatT xSect2 = x02 + rad2*cos(phi2);
     FloatT ySect2 = y02 + rad2*sin(phi2);
 
-//     std::cout << "(xSect1,ySect1)=(" << xSect1 << "," << ySect1 << ")" << std::endl;
-//     std::cout << "(xSect2,ySect2)=(" << xSect2 << "," << ySect2 << ")" << std::endl;
-
     FloatT temp21[3];
     FloatT temp22[3];
 
@@ -608,25 +634,46 @@ FloatT HelixClassT<FloatT>::getDistanceToHelix(HelixClassT * helix, FloatT * pos
     FloatT charge2 = helix->getCharge();
     FloatT pxy2 = helix->getPXY();
     FloatT pz2   = helix->getMomentum()[2];
-    if (deltaPhi21 < 0 && charge2 < 0) {
-      deltaPhi21 += _const_2pi;
-    }
-    else if (deltaPhi21 > 0 && charge2 > 0) {
-      deltaPhi21 -= _const_2pi;
-    }
 
-    if (deltaPhi22 < 0 && charge2 < 0) {
-      deltaPhi22 += _const_2pi;
+    float xAtPCA2 = helix->getXPCA();
+    float yAtPCA2 = helix->getYPCA();
+
+
+    // if canonical initialization with no input ref. point (for backward compatibility)
+    if ( ref2[0]==xAtPCA2 && ref2[1]==yAtPCA2 && ref2[2]==helix->getZ0() ) {
+      if (deltaPhi21 < 0 && charge2 < 0)
+        deltaPhi21 += _const_2pi;
+      else if (deltaPhi21 > 0 && charge2 > 0)
+        deltaPhi21 -= _const_2pi;
+
+      if (deltaPhi22 < 0 && charge2 < 0)
+        deltaPhi22 += _const_2pi;
+      else if (deltaPhi22 > 0 && charge2 > 0)
+        deltaPhi22 -= _const_2pi;
     }
-    else if (deltaPhi22 > 0 && charge2 > 0) {
-      deltaPhi22 -= _const_2pi;
+
+    else { // for non-zero ref. points given as an input
+      if (fabs(deltaPhi21) > M_PI) {
+        if (deltaPhi21 < 0 && charge2 > 0)
+          deltaPhi21 += _const_2pi;
+        else if (deltaPhi21 > 0 && charge2 < 0)
+          deltaPhi21 -= _const_2pi;
+      }
+      if (fabs(deltaPhi22) > M_PI) {
+        if (deltaPhi22 < 0 && charge2 > 0)
+          deltaPhi22 += _const_2pi;
+        else if (deltaPhi22 > 0 && charge2 < 0)
+          deltaPhi22 -= _const_2pi;
+      }
     }
 
     FloatT time21 = -charge2*deltaPhi21*rad2/ pxy2;
     FloatT time22 = -charge2 * deltaPhi22 * rad2 / pxy2;
 
-    FloatT Z21 = ref2[2] + time21 * pz2;
-    FloatT Z22 = ref2[2] + time22 * pz2;
+    FloatT tanLambda2 = helix->getTanLambda();
+
+    FloatT Z21 = ref2[2] - charge2*deltaPhi21*rad2*tanLambda2;
+    FloatT Z22 = ref2[2] - charge2*deltaPhi22*rad2*tanLambda2;
 
     temp21[0] = xSect1;
     temp21[1] = ySect1;
@@ -635,9 +682,6 @@ FloatT HelixClassT<FloatT>::getDistanceToHelix(HelixClassT * helix, FloatT * pos
     temp22[1] = ySect2;
     temp22[2] = Z22;
 
-    //     std::cout << "temp21 = " << temp21[0] << " " << temp21[1] << " " <<
-    //     temp21[2] << std::endl; std::cout << "temp22 = " << temp22[0] << " "
-    //     << temp22[1] << " " << temp22[2] << std::endl;
 
     FloatT temp11[3];
     FloatT temp12[3];
@@ -650,23 +694,45 @@ FloatT HelixClassT<FloatT>::getDistanceToHelix(HelixClassT * helix, FloatT * pos
     FloatT charge1 = _charge;
     FloatT pxy1 = _pxy;
     FloatT pz1 = _momentum[2];
-    if (deltaPhi11 < 0 && charge1 < 0) {
-      deltaPhi11 += _const_2pi;
-    } else if (deltaPhi11 > 0 && charge1 > 0) {
-      deltaPhi11 -= _const_2pi;
+
+    FloatT xAtPCA1 = _xAtPCA;
+    FloatT yAtPCA1 = _yAtPCA;
+
+    // if canonical initialization with no input ref. point (for backward compatibility)
+    if ( ref1[0]==xAtPCA1 && ref1[1]==yAtPCA1 && ref1[2]==_z0 ) {
+      if (deltaPhi11 < 0 && charge1 < 0)
+        deltaPhi11 += _const_2pi;
+      else if (deltaPhi11 > 0 && charge1 > 0)
+        deltaPhi11 -= _const_2pi;
+      if (deltaPhi12 < 0 && charge1 < 0)
+        deltaPhi12 += _const_2pi;
+      else if (deltaPhi12 > 0 && charge1 > 0)
+        deltaPhi12 -= _const_2pi;
     }
 
-    if (deltaPhi12 < 0 && charge1 < 0) {
-      deltaPhi12 += _const_2pi;
-    } else if (deltaPhi12 > 0 && charge1 > 0) {
-      deltaPhi12 -= _const_2pi;
+    else { // for non-zero ref. points given as an input
+      if (fabs(deltaPhi11) > M_PI) {
+        if (deltaPhi11 < 0 && charge1 > 0)
+          deltaPhi11 += _const_2pi;
+        else if (deltaPhi11 > 0 && charge1 < 0)
+          deltaPhi11 -= _const_2pi;
+      }
+      if (fabs(deltaPhi12) > M_PI) {
+        if (deltaPhi12 < 0 && charge1 > 0)
+          deltaPhi12 += _const_2pi;
+        else if (deltaPhi12 > 0 && charge1 < 0)
+          deltaPhi12 -= _const_2pi;
+      }
     }
 
+
     FloatT time11 = -charge1 * deltaPhi11 * rad1 / pxy1;
     FloatT time12 = -charge1 * deltaPhi12 * rad1 / pxy1;
 
-    FloatT Z11 = ref1[2] + time11 * pz1;
-    FloatT Z12 = ref1[2] + time12 * pz1;
+    FloatT tanLambda1 = getTanLambda();
+
+    FloatT Z11 = ref1[2] - charge1*deltaPhi11*rad1*tanLambda1;
+    FloatT Z12 = ref1[2] - charge1*deltaPhi12*rad1*tanLambda1;
 
     temp11[0] = xSect1;
     temp11[1] = ySect1;
@@ -675,10 +741,6 @@ FloatT HelixClassT<FloatT>::getDistanceToHelix(HelixClassT * helix, FloatT * pos
     temp12[1] = ySect2;
     temp12[2] = Z12;
 
-    //     std::cout << "temp11 = " << temp11[0] << " " << temp11[1] << " " <<
-    //     temp11[2] << std::endl; std::cout << "temp12 = " << temp12[0] << " "
-    //     << temp12[1] << " " << temp12[2] << std::endl;
-
     FloatT Dist1 = 0;
     FloatT Dist2 = 0;
 
diff --git a/source/tests/unittests/TestHelixClass.cpp b/source/tests/unittests/TestHelixClass.cpp
index 6094325..a2d2cba 100644
--- a/source/tests/unittests/TestHelixClass.cpp
+++ b/source/tests/unittests/TestHelixClass.cpp
@@ -6,6 +6,8 @@
 #include <catch2/catch_approx.hpp>
 #include <catch2/catch_template_test_macros.hpp>
 #include <catch2/catch_test_macros.hpp>
+#include <catch2/matchers/catch_matchers_floating_point.hpp>
+#include <catch2/matchers/catch_matchers_quantifiers.hpp>
 
 #include <array>
 #include <cmath>
@@ -31,9 +33,11 @@ TEMPLATE_LIST_TEST_CASE("Initialize_VP", "[helix-init]", HelixTypes) {
 
   helix.Initialize_VP(position.data(), momentum.data(), charge, magField);
 
-  REQUIRE(helix.getRadius() == Catch::Approx(pt / (c_helix * magField)));
-  REQUIRE(helix.getOmega() == Catch::Approx(charge / helix.getRadius()));
-  REQUIRE(helix.getTanLambda() == Catch::Approx(momentum[2] / pt));
+  const auto eps = 0.1;
+
+  REQUIRE_THAT(helix.getRadius(), Catch::Matchers::WithinAbs(pt / (c_helix * magField), eps));
+  REQUIRE_THAT(helix.getOmega(), Catch::Matchers::WithinAbs(charge / helix.getRadius(), eps));
+  REQUIRE_THAT(helix.getTanLambda(), Catch::Matchers::WithinAbs(momentum[2] / pt, eps));
   // etc...
 }
 
@@ -47,3 +51,168 @@ TEMPLATE_LIST_TEST_CASE("Initialize_VP", "[helix-init]", HelixTypes) {
 // - Other usage examples as found in the "real-world"
 // - Also add things that work slightly unexpectedly at the moment, e.g. things
 // that are listed in https://github.com/iLCSoft/MarlinUtil/issues/24
+
+TEMPLATE_LIST_TEST_CASE("Initialize_Canonical", "[helix-init]", HelixTypes) {
+  TestType helix;
+  using FloatT = typename TestType::float_type;
+  /*const*/ std::array<FloatT, 3> position = {0, 0, 0};
+  /*const*/ std::array<FloatT, 3> momentum = {-2.322, -11.684, 98.288};
+
+  const auto pt = std::hypot(momentum[0], momentum[1]);
+  const auto magField = 3.5;
+  const auto charge = -1.0;
+
+  const auto omega = c_helix * magField * charge / pt;
+  const auto tanLambda = momentum[2] / pt;
+
+  FloatT phi0 = atan2( momentum[1],momentum[0] );
+  if (phi0 < 0) phi0 += 2 * M_PI;
+
+  helix.Initialize_Canonical(-1.767, -9.163e-03, -2.103e-01, -8.808e-05, 8.251,
+                              magField, position.data());
+
+  const auto eps = 0.1;
+
+  // omega is very small radius is large here
+  CHECK_THAT(helix.getRadius(), Catch::Matchers::WithinAbs(pt / (c_helix * magField), 1));
+  CHECK_THAT(helix.getOmega(), Catch::Matchers::WithinAbs(omega, 1.0e-8));
+  CHECK_THAT(helix.getTanLambda(), Catch::Matchers::WithinAbs(tanLambda, eps));
+  CHECK_THAT(helix.getPhi0(), Catch::Matchers::WithinAbs(phi0, eps));
+  CHECK_THAT(helix.getD0(), Catch::Matchers::WithinAbs(-9.163e-03, 1.0e-4));
+  CHECK_THAT(helix.getZ0(), Catch::Matchers::WithinAbs(-2.103e-01, eps));
+}
+
+TEMPLATE_LIST_TEST_CASE("getDistance-Initialize_VP",
+                        "[helix-init][helix-distance]", HelixTypes) {
+  TestType helix1, helix2;
+  using FloatT = typename TestType::float_type;
+  /*const*/ std::array<FloatT, 3> momentum1 = {10., 20., 30.};
+  /*const*/ std::array<FloatT, 3> momentum2 = {70., 30., 10.};
+  /*const*/ std::array<FloatT, 3> momentum;
+  for (int i=0; i<3; i++) momentum[i] = momentum1[i] + momentum2[i];
+
+  const auto magField = 3.5;
+  const auto charge1 = -1.0;
+  const auto charge2 = 1.0;
+
+  FloatT vtx_momentum1[3];
+  FloatT vtx_momentum2[3];
+  FloatT vertex1[3];
+  FloatT vertex2[3];
+
+  SECTION("non-zero reference point") {
+    // use ref. point in a "true" vertex for simplicity
+    /*const*/ std::array<FloatT, 3> position = {214, 360, -700};
+
+    helix1.Initialize_VP(position.data(), momentum1.data(), charge1, magField);
+    helix2.Initialize_VP(position.data(), momentum2.data(), charge2, magField);
+
+    FloatT dist1 = helix1.getDistanceToHelix(&helix2, vertex1, vtx_momentum1);
+    FloatT dist2 = helix2.getDistanceToHelix(&helix1, vertex2, vtx_momentum2);
+
+    const auto eps = 0.1;
+
+    CHECK_THAT(0.0, Catch::Matchers::WithinAbs(dist1, eps));
+    CHECK_THAT(0.0, Catch::Matchers::WithinAbs(dist2, eps));
+
+    for (int i=0; i<3; i++) {
+      CHECK_THAT(position[i], Catch::Matchers::WithinAbs(vertex1[i], eps));
+      CHECK_THAT(position[i], Catch::Matchers::WithinAbs(vertex2[i], eps));
+
+      CHECK_THAT(momentum[i], Catch::Matchers::WithinAbs(vtx_momentum1[i], eps));
+      CHECK_THAT(momentum[i], Catch::Matchers::WithinAbs(vtx_momentum2[i], eps));
+    }
+  }
+
+}
+
+TEMPLATE_LIST_TEST_CASE("getDistance-Initialize_Canonical",
+                        "[helix-init][helix-distance]", HelixTypes) {
+  TestType helix1, helix2;
+  using FloatT = typename TestType::float_type;
+  // "real-world" numbers (BSM event)
+  /*const*/ std::array<FloatT, 3> momentum1 = {2.63e-01,-1.17e-01,-1.46e-01};
+  /*const*/ std::array<FloatT, 3> momentum2 = {1.69e-01, 4.80e-02, 6.35e-01};
+  /*const*/ std::array<FloatT, 3> momentum;
+  for (int i=0; i<3; i++) momentum[i] = momentum1[i] + momentum2[i];
+
+  const auto magField = 3.5;
+  const auto charge1 = -1.0;
+  const auto charge2 = 1.0;
+  const auto pt1 = std::hypot(momentum1[0], momentum1[1]);
+  const auto pt2 = std::hypot(momentum2[0], momentum2[1]);
+
+  const auto omega1 = c_helix * magField * charge1 / pt1;
+  const auto omega2 = c_helix * magField * charge2 / pt2;
+  const auto tanLambda1 = momentum1[2] / pt1;
+  const auto tanLambda2 = momentum2[2] / pt2;
+  const auto phi01 = atan2( momentum1[1],momentum1[0] );
+  const auto phi02 = atan2( momentum2[1],momentum2[0] );
+
+
+  FloatT vtx_momentum1[3];
+  FloatT vtx_momentum2[3];
+  FloatT vertex1[3];
+  FloatT vertex2[3];
+
+  SECTION("no reference point given and helices passing through zero") {
+    /*const*/ std::array<FloatT, 3> position = {0, 0, 0};
+
+    // passing through ref. point
+    const auto d0 = 1.0e-5;
+    const auto z0 = 1.0e-5;
+
+    helix1.Initialize_Canonical(phi01, d0, z0, omega1, tanLambda1, magField);
+    helix2.Initialize_Canonical(phi02, -d0, z0, omega2, tanLambda2, magField);
+
+    CHECK_THAT(helix1.getRadius(),
+                Catch::Matchers::WithinAbs(pt1 / (c_helix * magField), 0.1));
+    CHECK_THAT(helix2.getRadius(),
+                Catch::Matchers::WithinAbs(pt2 / (c_helix * magField), 0.1));
+
+
+    FloatT dist1 = helix1.getDistanceToHelix(&helix2, vertex1, vtx_momentum1);
+    FloatT dist2 = helix2.getDistanceToHelix(&helix1, vertex2, vtx_momentum2);
+
+    const auto eps = 0.1;
+
+    CHECK_THAT(0.0, Catch::Matchers::WithinAbs(dist1, eps));
+    CHECK_THAT(0.0, Catch::Matchers::WithinAbs(dist2, eps));
+
+    for (int i=0; i<3; i++) {
+      CHECK_THAT(position[i], Catch::Matchers::WithinAbs(vertex1[i], eps));
+      CHECK_THAT(position[i], Catch::Matchers::WithinAbs(vertex2[i], eps));
+
+      CHECK_THAT(momentum[i], Catch::Matchers::WithinAbs(vtx_momentum1[i], eps));
+      CHECK_THAT(momentum[i], Catch::Matchers::WithinAbs(vtx_momentum2[i], eps));
+    }
+  }
+
+  SECTION("input reference point and helices with small d0, z0") {
+    // "real-world" numbers (BSM event)
+    std::array<FloatT, 3> position = {242.5, -358.5, 171.6}; // true vtx
+    std::array<FloatT, 3> ref1 = {244.944, -359.603, 169.052};
+    std::array<FloatT, 3> ref2 = {249.609, -356.381, 198.409};
+
+    helix1.Initialize_Canonical(phi01, 0.103768967092, 1.39691245556, omega1,
+                                tanLambda1, magField, ref1.data());
+    helix2.Initialize_Canonical(phi02, 0.275440096855, -1.55072879791, omega2,
+                                tanLambda2, magField, ref2.data());
+
+    FloatT dist1 = helix1.getDistanceToHelix(&helix2, vertex1, vtx_momentum1);
+    FloatT dist2 = helix2.getDistanceToHelix(&helix1, vertex2, vtx_momentum2);
+
+    // distance should be small but nonzero
+    CHECK_THAT(0.0, Catch::Matchers::WithinAbs(dist1, 2));
+    CHECK_THAT(0.0, Catch::Matchers::WithinAbs(dist2, 2));
+
+    for (int i=0; i<3; i++) {
+      // 1 mm precision is good enough for very far vertices
+      CHECK_THAT(position[i], Catch::Matchers::WithinAbs(vertex1[i], 1));
+      CHECK_THAT(position[i], Catch::Matchers::WithinAbs(vertex2[i], 1));
+
+      CHECK_THAT(momentum[i], Catch::Matchers::WithinAbs(vtx_momentum1[i], 0.1));
+      CHECK_THAT(momentum[i], Catch::Matchers::WithinAbs(vtx_momentum2[i], 0.1));
+    }
+  }
+}