diff --git a/algo/kf/core/KfTrackKalmanFilter.cxx b/algo/kf/core/KfTrackKalmanFilter.cxx
index 7230a22f136d33fea3e1a0945b7d2024cb3a54a2..17801e77bf36f06c9ee3f4047b142a910d48bd32 100644
--- a/algo/kf/core/KfTrackKalmanFilter.cxx
+++ b/algo/kf/core/KfTrackKalmanFilter.cxx
@@ -990,9 +990,7 @@ namespace cbm::algo::kf
     DataT tyty  = ty * ty;
     DataT txtx1 = DataT(1.) + txtx;
     DataT tyty1 = DataT(1.) + tyty;
-    DataT h     = txtx + tyty;
     DataT t     = sqrt(txtx1 + tyty);
-    DataT h2    = h * h;
     DataT qpt   = qp * t;
 
     DataT lg = DataT(.0136) * (DataT(1.) + DataT(0.038) * log(radThick * t));
@@ -1000,7 +998,11 @@ namespace cbm::algo::kf
 
     DataT s0 = lg * qp * t;
     DataT a  = (DataT(1.) + fMass2 * qp * qp) * s0 * s0 * t * radThick;
+    
+    // Approximate formula
 
+    // DataT h     = txtx + tyty;
+    // DataT h2    = h * h;
     //cnst c1 = 0.0136f, c2 = c1 * 0.038f, c3 = c2 * 0.5f, c4 = -c3 / 2.0f, c5 = c3 / 3.0f, c6 = -c3 / 4.0f;
     //DataT s0 = (c1 + c2 * log(radThick) + c3 * h + h2 * (c4 + c5 * h + c6 * h2)) * qp0t;
     //DataT a = ( (kONE+mass2*qp0*qp0t)*radThick*s0*s0 );
diff --git a/reco/KF/CbmKfTrackFitter.cxx b/reco/KF/CbmKfTrackFitter.cxx
index fe974c9d069d80d20d803ab5b60fa4f747eb6814..b29c924b31cfb7438046bb1ac44e8ed66abe4b35 100644
--- a/reco/KF/CbmKfTrackFitter.cxx
+++ b/reco/KF/CbmKfTrackFitter.cxx
@@ -428,16 +428,27 @@ void CbmKfTrackFitter::FilterFirstMeasurement(const FitNode& n)
 
   auto& tr = fFit.Tr();
 
+  if (n.fIsFitted && (fabs(tr.GetZ() - n.fZ) > 1.e-10)) {
+    LOG(fatal) << "CbmKfTrackFitter: Z mismatch: fitted track " << tr.GetZ() << " != node " << n.fZ;
+  }
+
   tr.ResetErrors(mxy.Dx2(), mxy.Dy2(), 100., 100., 10., 1.e4, 1.e2);
   tr.SetC10(mxy.Dxy());
+  tr.SetX(mxy.X());
+  tr.SetY(mxy.Y());
+  tr.SetZ(n.fZ);
 
-  if (!(fSkipUnmeasuredCoordinates && mxy.NdfX() == 0)) {
-    tr.SetX(mxy.X());
-  }
-  if (!(fSkipUnmeasuredCoordinates && mxy.NdfY() == 0)) {
-    tr.SetY(mxy.Y());
+  if (fSkipUnmeasuredCoordinates && n.fIsFitted) {
+    if (mxy.NdfX() == 0) {
+      tr.SetX(n.fTrack.GetX());
+      tr.SetC00(1.e4);
+    }
+    if (mxy.NdfY() == 0) {
+      tr.SetY(n.fTrack.GetY());
+      tr.SetC11(1.e4);
+      tr.SetC10(0.);
+    }
   }
-  tr.SetZ(n.fZ);
 
   tr.SetChiSq(0.);
   tr.SetChiSqTime(0.);