diff --git a/reco/KF/CbmKfFitTracksTask.cxx b/reco/KF/CbmKfFitTracksTask.cxx
index 75229257502dbe5d8e885d33f37a43059e27a228..07467a4a6c5b395de671f95bb12d06062e8c9bd9 100644
--- a/reco/KF/CbmKfFitTracksTask.cxx
+++ b/reco/KF/CbmKfFitTracksTask.cxx
@@ -107,14 +107,14 @@ void CbmKfFitTracksTask::Exec(Option_t* /*opt*/)
       }
       fFitter.FitTrack(t);
       {
-        const auto& parV = t.fNodes[t.fFirstHitNode].fTrack;
+        const auto& parV = t.fNodes.front().fTrack;
         cbm::algo::kf::TrackParamD parD;
         parD.Set(parV, 0);
         FairTrackParam trackFirst = cbm::L1Util::ConvertTrackParam(parD);
         stsTrack->SetParamFirst(&trackFirst);
       }
       {
-        const auto& parV = t.fNodes[t.fLastHitNode].fTrack;
+        const auto& parV = t.fNodes.back().fTrack;
         cbm::algo::kf::TrackParamD parD;
         parD.Set(parV, 0);
         FairTrackParam trackLast = cbm::L1Util::ConvertTrackParam(parD);
@@ -125,6 +125,9 @@ void CbmKfFitTracksTask::Exec(Option_t* /*opt*/)
 
   if (FitMode::kMcbm == fFitMode && fGlobalTracks) {
 
+    fFitter.SetDefaultMomentumForMs(0.1);  // 0.1 GeV/c
+    fFitter.FixMomentumForMs(true);        // fix the momentum for the Multiple Scattering calculation
+
     for (int iTr = 0; iTr < fGlobalTracks->GetEntriesFast(); iTr++) {
       CbmGlobalTrack* globalTrack = dynamic_cast<CbmGlobalTrack*>(fGlobalTracks->At(iTr));
       if (!globalTrack) {
@@ -136,19 +139,16 @@ void CbmKfFitTracksTask::Exec(Option_t* /*opt*/)
         LOG(fatal) << "CbmKfFitTracksTask: can not create the global track for the fit! ";
         return;
       }
-      t.fMsQp0                              = 1. / 0.1;
-      t.fIsMsQp0Set                         = true;
-      t.fNodes[t.fFirstHitNode].fTrack.Qp() = 0.;
       fFitter.FitTrack(t);
       {
-        const auto& parV = t.fNodes[t.fFirstHitNode].fTrack;
+        const auto& parV = t.fNodes.front().fTrack;
         cbm::algo::kf::TrackParamD parD;
         parD.Set(parV, 0);
         FairTrackParam trackFirst = cbm::L1Util::ConvertTrackParam(parD);
         globalTrack->SetParamFirst(&trackFirst);
       }
       {
-        const auto& parV = t.fNodes[t.fLastHitNode].fTrack;
+        const auto& parV = t.fNodes.back().fTrack;
         cbm::algo::kf::TrackParamD parD;
         parD.Set(parV, 0);
         FairTrackParam trackLast = cbm::L1Util::ConvertTrackParam(parD);
diff --git a/reco/KF/CbmKfTrackFitter.cxx b/reco/KF/CbmKfTrackFitter.cxx
index 382ed9ccdadea3dbf898b34153788a1e658025fc..811412fcf685ae6e5eea6ad2432dca739cd6e87d 100644
--- a/reco/KF/CbmKfTrackFitter.cxx
+++ b/reco/KF/CbmKfTrackFitter.cxx
@@ -44,20 +44,10 @@ namespace
   using namespace cbm::algo;
 }
 
-void CbmKfTrackFitter::Track::MakeConsistent()
+void CbmKfTrackFitter::Track::OrderNodesInZ()
 {
   // sort the nodes in z
   std::sort(fNodes.begin(), fNodes.end(), [](const FitNode& a, const FitNode& b) { return a.fZ < b.fZ; });
-
-  // set the first and last hit nodes
-  fFirstHitNode = fNodes.size() - 1;
-  fLastHitNode  = 0;
-  for (int i = 0; i < (int) fNodes.size(); i++) {
-    if (fNodes[i].fIsXySet) {
-      fFirstHitNode = std::min(fFirstHitNode, i);
-      fLastHitNode  = std::max(fLastHitNode, i);
-    }
-  }
 }
 
 
@@ -200,7 +190,7 @@ bool CbmKfTrackFitter::CreateGlobalTrack(CbmKfTrackFitter::Track& kfTrack, const
       n.fMaterialLayer             = i;
       n.fZ                         = caPar.GetStation(i).GetZ<float>();
       n.fRadThick                  = 0.;
-      n.fIsRadThickSet             = false;
+      n.fIsRadThickFixed           = false;
       n.fIsFitted                  = false;
       n.fIsXySet                   = false;
       n.fIsTimeSet                 = false;
@@ -208,8 +198,6 @@ bool CbmKfTrackFitter::CreateGlobalTrack(CbmKfTrackFitter::Track& kfTrack, const
       n.fHitAddress                = -1;
       n.fHitIndex                  = -1;
     }
-    t.fFirstHitNode = nStations - 1;
-    t.fLastHitNode  = 0;
   }
 
   // lambda to set the node from the pixel hit
@@ -232,12 +220,12 @@ bool CbmKfTrackFitter::CreateGlobalTrack(CbmKfTrackFitter::Track& kfTrack, const
     n.fMt.SetT(h.GetTime());
     n.fMt.SetDt2(h.GetTimeError() * h.GetTimeError());
     n.fMt.SetNdfT(1);
-    n.fIsTimeSet     = (detId != ca::EDetectorID::kMvd);
-    n.fRadThick      = 0.;
-    n.fIsRadThickSet = false;
-    n.fHitSystemId   = cbm::L1Util::ConvertDetSystemId(detId);
-    n.fHitAddress    = h.GetAddress();
-    n.fHitIndex      = hitIdx;
+    n.fIsTimeSet       = (detId != ca::EDetectorID::kMvd);
+    n.fRadThick        = 0.;
+    n.fIsRadThickFixed = false;
+    n.fHitSystemId     = cbm::L1Util::ConvertDetSystemId(detId);
+    n.fHitAddress      = h.GetAddress();
+    n.fHitIndex        = hitIdx;
     return &n;
   };
 
@@ -417,12 +405,12 @@ bool CbmKfTrackFitter::CreateGlobalTrack(CbmKfTrackFitter::Track& kfTrack, const
     }
   }
 
-  t.MakeConsistent();
+  t.OrderNodesInZ();
 
-  kf::TrackParamD tmp = cbm::L1Util::ConvertTrackParam(*globalTrack.GetParamFirst());
-
-  t.fNodes[t.fFirstHitNode].fTrack.Set(tmp);
-  t.fNodes[t.fFirstHitNode].fIsFitted = 1;
+  // set initial track parameters
+  // kf::TrackParamD tmp = cbm::L1Util::ConvertTrackParam(*globalTrack.GetParamFirst());
+  // t.fNodes[0].fTrack.Set(tmp);
+  // t.fNodes[0].fIsFitted = 1;
 
   kfTrack = t;
   return true;
@@ -436,11 +424,14 @@ void CbmKfTrackFitter::FilterFirstMeasurement(const FitNode& n)
   // a special routine to filter the first measurement.
   // the measurement errors are simply copied to the track covariance matrix
 
+  assert(n.fIsXySet);
+
   const auto& mxy = n.fMxy;
   const auto& mt  = n.fMt;
 
   auto& tr = fFit.Tr();
-  tr.ResetErrors(mxy.Dx2(), mxy.Dy2(), 1., 1., 1., 1.e4, 1.e2);
+
+  tr.ResetErrors(mxy.Dx2(), mxy.Dy2(), 100., 100., 1., 1.e4, 1.e2);
   tr.SetC10(mxy.Dxy());
 
   if (!(fSkipUnmeasuredCoordinates && mxy.NdfX()[0] == 0)) {
@@ -460,12 +451,12 @@ void CbmKfTrackFitter::FilterFirstMeasurement(const FitNode& n)
   else {
     tr.SetNdfTime(-2);
   }
-  tr.SetVi(0.);
+  tr.Vi() = constants::phys::SpeedOfLightInv;
+  tr.InitVelocityRange(0.5);
 }
 
 
-void CbmKfTrackFitter::AddMaterialEffects(const CbmKfTrackFitter::Track& t, CbmKfTrackFitter::FitNode& n,
-                                          bool upstreamDirection)
+void CbmKfTrackFitter::AddMaterialEffects(CbmKfTrackFitter::FitNode& n, CbmKfTrackFitter::Direction direction)
 {
   // add material effects
   if (n.fMaterialLayer < 0) {
@@ -475,16 +466,16 @@ void CbmKfTrackFitter::AddMaterialEffects(const CbmKfTrackFitter::Track& t, CbmK
   auto& tr = n.fIsFitted ? n.fTrack : fFit.Tr();
 
   // calculate the radiation thickness from the current track
-  if (!n.fIsRadThickSet) {
+  if (!n.fIsRadThickFixed) {
     n.fRadThick =
       CbmL1::Instance()->fpAlgo->GetParameters().GetMaterialThicknessScal(n.fMaterialLayer, tr.GetX()[0], tr.GetY()[0]);
   }
 
-  fvec msQp0 = t.fIsMsQp0Set ? t.fMsQp0 : tr.GetQp();
+  fvec msQp0 = fIsQpForMsFixed ? fDefaultQpForMs : fFit.Qp0();
 
   fFit.MultipleScattering(n.fRadThick, msQp0);
-  if (!t.fIsMsQp0Set) {
-    fFit.EnergyLossCorrection(n.fRadThick, upstreamDirection ? fvec::One() : fvec::Zero());
+  if (!fIsQpForMsFixed) {
+    fFit.EnergyLossCorrection(n.fRadThick, (direction == kUpstream) ? fvec::One() : fvec::Zero());
   }
 }
 
@@ -492,13 +483,49 @@ void CbmKfTrackFitter::AddMaterialEffects(const CbmKfTrackFitter::Track& t, CbmK
 bool CbmKfTrackFitter::FitTrack(CbmKfTrackFitter::Track& t)
 {
   // fit the track
-
+  if (fVerbosityLevel > 0) {
+    std::cout << "FitTrack ... " << std::endl;
+  }
   bool ok = true;
 
   // ensure that the fitter is initialized
   Init();
 
-  t.MakeConsistent();
+  int nNodes = t.fNodes.size();
+
+  if (nNodes <= 0) {
+    LOG(warning) << "CbmKfTrackFitter: no nodes found!";
+    return false;
+  }
+
+  int firstHitNode = -1;
+  int lastHitNode  = -1;
+  bool isOrdered   = true;
+
+  {  // find the first and the last hit nodes. Check if the nodes are ordered in Z
+    double zOld = -1.e10;
+    for (int i = 0; i < nNodes; i++) {
+      if (t.fNodes[i].fIsXySet) {
+        if (firstHitNode < 0) {
+          firstHitNode = i;
+        }
+        lastHitNode = i;
+      }
+      if (t.fNodes[i].fZ < zOld) {
+        isOrdered = false;
+      }
+      zOld = t.fNodes[i].fZ;
+    }
+  }
+
+  if (firstHitNode < 0 || lastHitNode < 0) {
+    LOG(warning) << "CbmKfTrackFitter: no hit nodes found!";
+    return false;
+  }
+
+  if (!isOrdered) {
+    LOG(warning) << "CbmKfTrackFitter: the nodes are not ordered in Z!";
+  }
 
   fFit.SetMask(fmask::One());
   fFit.SetParticleMass(fMass);
@@ -506,77 +533,160 @@ bool CbmKfTrackFitter::FitTrack(CbmKfTrackFitter::Track& t)
   ca::FieldRegion<ca::fvec> field _fvecalignment;
   field.SetUseOriginalField();
 
-  int nNodes = t.fNodes.size();
+  {  // fit downstream
 
-  // fit downstream. The approximation is taken from the first hit node
-  {
-    FitNode& n = t.fNodes[t.fFirstHitNode];
-    fFit.SetTrack(n.fTrack);
-    FilterFirstMeasurement(n);
-    n.fTrack    = fFit.Tr();
-    n.fIsFitted = false;
-  }
+    {  // initialisation at the first measurement
+      FitNode& n = t.fNodes[firstHitNode];
+      assert(n.fIsXySet);
 
-  for (int iNode = t.fFirstHitNode + 1; iNode < nNodes; iNode++) {
-    FitNode& n = t.fNodes[iNode];
-    fFit.Extrapolate(n.fZ, field);
-    if (n.fIsFitted) {
-      fFit.SetQp0(n.fTrack.GetQp());
-    }
-    AddMaterialEffects(t, n, false);
+      if (n.fIsFitted) {  // The initial parameters are taken from the previous fit
+                          // Linearisation at the previously fitted momentum
+        assert(n.fZ == n.fTrack.GetZ()[0]);
+        fFit.SetTrack(n.fTrack);
+      }
+      else {  // The initial parameters are calculated from the first and the last measurements
+        auto& tr  = fFit.Tr();
+        auto& n1  = t.fNodes[lastHitNode];
+        tr.X()    = n.fMxy.X();
+        tr.Y()    = n.fMxy.Y();
+        tr.Z()    = n.fZ;
+        double dz = n1.fZ - n.fZ;
+        tr.Tx()   = (n1.fMxy.X() - n.fMxy.X()) / dz;
+        tr.Ty()   = (n1.fMxy.Y() - n.fMxy.Y()) / dz;
+        tr.Qp()   = 0.;
+        tr.Time() = 0.;
+        tr.Vi()   = constants::phys::SpeedOfLightInv;
+        fFit.SetQp0(0.);  // Linearisation at the infinite momentum
+      }
 
-    // store track fitted at the previous node and extrapolated to this node
-    n.fTrack    = fFit.Tr();
-    n.fIsFitted = false;
-    if (n.fIsXySet) {
-      fFit.FilterXY(n.fMxy, fSkipUnmeasuredCoordinates);
-    }
-    if (n.fIsTimeSet) {
-      fFit.FilterTime(n.fMt);
+      FilterFirstMeasurement(n);
+
+      if (fVerbosityLevel > 0) {
+        std::cout << " fit downstream: node " << firstHitNode << " chi2 " << fFit.Tr().GetChiSq()[0] << " x "
+                  << fFit.Tr().GetX()[0] << " y " << fFit.Tr().GetY()[0] << " tx " << fFit.Tr().GetTx()[0] << " ty "
+                  << fFit.Tr().GetTy()[0] << std::endl;
+      }
     }
-    if (iNode >= t.fLastHitNode) {  // store track fitted at the last node
-      n.fTrack    = fFit.Tr();
-      n.fIsFitted = true;
+
+    for (int iNode = firstHitNode + 1; iNode < nNodes; iNode++) {
+
+      FitNode& n = t.fNodes[iNode];
+
+      fFit.Extrapolate(n.fZ, field);
+
+      if (fDoSmooth) {  // store the partially fitted track
+        n.fTrack    = fFit.Tr();
+        n.fIsFitted = false;  // the stored track is not fully fitted
+      }
+
+      if (n.fIsFitted && (iNode <= lastHitNode)) {  // linearisation is taken from the previous fit
+        fFit.SetQp0(n.fTrack.GetQp());
+      }
+
+      AddMaterialEffects(n, kDownstream);
+
+      if (n.fIsXySet) {
+        fFit.FilterXY(n.fMxy, fSkipUnmeasuredCoordinates);
+      }
+      if (n.fIsTimeSet) {
+        fFit.FilterTime(n.fMt);
+      }
+
+      if (fVerbosityLevel > 0) {
+        std::cout << " fit downstream: node " << iNode << " chi2 " << fFit.Tr().GetChiSq()[0] << " x "
+                  << fFit.Tr().GetX()[0] << " y " << fFit.Tr().GetY()[0] << " tx " << fFit.Tr().GetTx()[0] << " ty "
+                  << fFit.Tr().GetTy()[0] << std::endl;
+      }
+      if (iNode >= lastHitNode) {  // the track is fully fitted, store it
+        n.fTrack    = fFit.Tr();
+        n.fIsFitted = true;
+        fFit.SetQp0(fFit.Tr().GetQp());  // set the linearisation of q/p to the fitted value
+      }
     }
   }
 
-  // fit upstream
-  {
-    FitNode& n = t.fNodes[t.fLastHitNode];
-    fFit.SetTrack(n.fTrack);
-    FilterFirstMeasurement(n);
-    n.fIsFitted = true;
-  }
+  double dnChi2 = fFit.Tr().GetChiSq()[0];
+  double dnNdf  = fFit.Tr().GetNdf()[0];
 
-  for (int iNode = t.fLastHitNode - 1; iNode >= 0; iNode--) {
-    FitNode& n = t.fNodes[iNode];
-    fFit.Extrapolate(n.fZ, field);
-    if (n.fIsXySet) {
-      fFit.FilterXY(n.fMxy, fSkipUnmeasuredCoordinates);
-    }
-    if (n.fIsTimeSet) {
-      fFit.FilterTime(n.fMt);
+  {  // fit upstream
+
+    {
+      FitNode& n = t.fNodes[lastHitNode];
+      assert(n.fIsFitted);
+      fFit.SetTrack(n.fTrack);
+      FilterFirstMeasurement(n);
+      if (fVerbosityLevel > 0) {
+        std::cout << "\n\n up node " << lastHitNode << " chi2 " << fFit.Tr().GetChiSq()[0] << " x "
+                  << fFit.Tr().GetX()[0] << " y " << fFit.Tr().GetY()[0] << " tx " << fFit.Tr().GetTx()[0] << " ty "
+                  << fFit.Tr().GetTy()[0] << std::endl;
+      }
     }
 
-    // combine partially fitted downstream and upstream tracks
-    n.fIsFitted = true;
-    if (iNode > t.fFirstHitNode) {
-      n.fIsFitted = Smooth(n.fTrack, fFit.Tr());
+    for (int iNode = lastHitNode - 1; iNode >= 0; iNode--) {
+      FitNode& n = t.fNodes[iNode];
+
+      fFit.Extrapolate(n.fZ, field);
+
+      AddMaterialEffects(n, kUpstream);
+
+      if (n.fIsXySet) {
+        fFit.FilterXY(n.fMxy, fSkipUnmeasuredCoordinates);
+      }
+      if (n.fIsTimeSet) {
+        fFit.FilterTime(n.fMt);
+      }
+      if (fVerbosityLevel > 0) {
+        std::cout << " up node " << iNode << " chi2 " << fFit.Tr().GetChiSq()[0] << " x " << fFit.Tr().GetX()[0]
+                  << " y " << fFit.Tr().GetY()[0] << " tx " << fFit.Tr().GetTx()[0] << " ty " << fFit.Tr().GetTy()[0]
+                  << std::endl;
+      }
+
+      // combine partially fitted downstream and upstream tracks
+      n.fIsFitted = true;
+
+      if (iNode > firstHitNode) {
+        n.fIsFitted = false;
+        if (fDoSmooth) {
+          n.fIsFitted = Smooth(n.fTrack, fFit.Tr());
+        }
+      }
+      else {
+        n.fIsFitted = true;
+        n.fTrack    = fFit.Tr();
+      }
+      if (fDoSmooth && !n.fIsFitted) {
+        ok = false;
+      }
+      if (n.fIsFitted) {
+        fFit.SetQp0(n.fTrack.GetQp());
+      }
+      AddMaterialEffects(n, kUpstream);
     }
-    else {
-      n.fTrack = fFit.Tr();
+  }
+
+  double upChi2 = fFit.Tr().GetChiSq()[0];
+  double upNdf  = fFit.Tr().GetNdf()[0];
+
+  if (!fDoSmooth) {
+    if (upNdf != dnNdf) {
+      LOG(fatal) << "CbmKfTrackFitter: ndf mismatch: dn " << dnNdf << " != up " << upNdf;
     }
-    if (!n.fIsFitted) {
-      ok = false;
+    if (fabs(upChi2 - dnChi2) > 1.e-2) {
+      LOG(fatal) << "CbmKfTrackFitter: chi2 mismatch: dn " << dnChi2 << " != up " << upChi2 << " first node "
+                 << firstHitNode << " last node " << lastHitNode << " of " << nNodes;
     }
-    fFit.SetQp0(n.fTrack.GetQp());
-    AddMaterialEffects(t, n, true);
   }
-
   // distribute the final chi2, ndf to all nodes
 
-  const auto& tt = t.fNodes[t.fFirstHitNode].fTrack;
-  for (auto& n : t.fNodes) {
+  const auto& tt = fFit.Tr();
+  for (int iNode = 0; iNode < nNodes; iNode++) {
+    FitNode& n = t.fNodes[iNode];
+    if (iNode == lastHitNode) {
+      //continue;
+    }
+    if (!fDoSmooth && iNode != lastHitNode) {
+      //n.fIsFitted = false;
+    }
     n.fTrack.SetNdf(tt.GetNdf());
     n.fTrack.SetNdfTime(tt.GetNdfTime());
     n.fTrack.SetChiSq(tt.GetChiSq());
@@ -586,7 +696,6 @@ bool CbmKfTrackFitter::FitTrack(CbmKfTrackFitter::Track& t)
   return ok;
 }
 
-
 bool CbmKfTrackFitter::Smooth(kf::TrackParamV& t1, const kf::TrackParamV& t2)
 {
   // TODO: move to the CaTrackFit class
diff --git a/reco/KF/CbmKfTrackFitter.h b/reco/KF/CbmKfTrackFitter.h
index aa12abeed73a1b17f488ea2e8ab9e8f34dfb9c67..b610d2a748ade45b351689152439560239772500 100644
--- a/reco/KF/CbmKfTrackFitter.h
+++ b/reco/KF/CbmKfTrackFitter.h
@@ -45,8 +45,13 @@ class CbmKfTrackFitter {
   /// a) measured and / or
   /// b) scattered and / or
   /// c) need to be estimated
-  /// The nodes must be ordered by increasing Z
+  /// The nodes are expected to be ordered by increasing Z.
+  /// It can be done via CbmKfTrackFitter::Track::MakeConsistent() method.
   ///
+  /// When fIsFitted flag is set (e.g. by the fitter), the node track parameters are used
+  /// for the trajectory linearisation during the fit.
+  ///
+  // TODO: proper interface
   struct FitNode {
 
     double fZ{0.};  ///< Z coordinate
@@ -54,10 +59,13 @@ class CbmKfTrackFitter {
     kf::TrackParamV fTrack{};  ///< fitted track
 
     /// == Material information (if present)
+
     // TODO: change to the material layer index when the material layer is implemented
     int fMaterialLayer{-1};  ///< index of the material layer. Currently equal to the active tracking station index
 
-    double fRadThick{0.};  ///< material radiation thickness at fZ
+    /// radiation thickness of the material associated with the node
+    /// - taken from the material map or set externally
+    double fRadThick{0.};
 
     /// == Hit information ( if present )
 
@@ -65,33 +73,26 @@ class CbmKfTrackFitter {
 
     ca::MeasurementTime<ca::fvec> fMt{};  ///< time measurement at fZ
 
+    /// == Flags etc
+    bool fIsXySet{false};          ///< true if the XY measurement is set
+    bool fIsTimeSet{false};        ///< true if the time measurement is set
+    bool fIsRadThickFixed{false};  ///< true if the radiation thickness is fixed to the fRadThick value
+    bool fIsFitted{false};         ///< true if the track parameters at the node are fitted
+
+    /// == External references
     ECbmModuleId fHitSystemId{ECbmModuleId::kNotExist};  ///< detector system ID of the hit
     int fHitAddress{-1};                                 ///< detector ID of the hit
     int fHitIndex{-1};                                   ///< hit index in the detector hit array
 
-    /// == Flags etc
-    bool fIsXySet{false};        ///< true if the XY measurement is set
-    bool fIsTimeSet{false};      ///< true if the time measurement is set
-    bool fIsFitted{false};       ///< true if the node is fitted, false if the fit failed
-    bool fIsRadThickSet{false};  ///< true if the radiation thickness is set
-    int fReference1{-1};         ///< some reference can be set by the user
-    int fReference2{-1};         ///< some reference can be set by the user
+    int fReference1{-1};  ///< some reference can be set by the user
+    int fReference2{-1};  ///< some reference can be set by the user
   };
 
   /// A track to be fitted
+  // TODO: proper interface
   struct Track {
     std::vector<FitNode> fNodes;  ///< nodes on the track
-    int fFirstHitNode{-1};        ///< index of the first node with the XY measurement
-    int fLastHitNode{-1};         ///< index of the last node with the XY measurement
-
-    /// externally defined inverse momentum for the Multiple Scattering calculation.
-    /// It is used for the tracks in field-free regions.
-    /// When the momentum can be fitted, the fitted value is used.
-    /// the default value is set to 0.1 GeV/c
-    double fMsQp0{1. / 0.1};
-    bool fIsMsQp0Set{false};
-
-    void MakeConsistent();  // make the structure fields consistent
+    void OrderNodesInZ();         // sort the nodes in Z and set fFirstHitNode and fLastHitNode
   };
 
   CbmKfTrackFitter();
@@ -113,6 +114,12 @@ class CbmKfTrackFitter {
   /// skip unmeasured coordinates
   void SetSkipUnmeasuredCoordinates(bool skip = true) { fSkipUnmeasuredCoordinates = skip; }
 
+  /// set the default inverse momentum for the Multiple Scattering calculation
+  void SetDefaultMomentumForMs(double p) { fDefaultQpForMs = 1. / p; }
+
+  /// fix the inverse momentum for the Multiple Scattering calculation
+  void FixMomentumForMs(bool fix = true) { fIsQpForMsFixed = fix; }
+
   /// set the input data arrays
   bool CreateMvdStsTrack(Track& kfTrack, int stsTrackIndex);
   bool CreateGlobalTrack(Track& kfTrack, int globalTrackIndex);
@@ -121,12 +128,22 @@ class CbmKfTrackFitter {
   /// fit the track
   bool FitTrack(CbmKfTrackFitter::Track& t);
 
-  /// fit sts tracks
-  // void FitStsTracks(vector<CbmStsTrack>& Tracks, const vector<int>& pidHypo);
+  /// do the KF-smoothing to define track pars at all the nodes
+  void SetDoSmooth(bool doSmooth) { fDoSmooth = doSmooth; }
+
+  /// set verbosity level
+  void SetVerbosityLevel(int level) { fVerbosityLevel = level; }
 
  private:
   void FilterFirstMeasurement(const FitNode& n);
-  void AddMaterialEffects(const Track& t, FitNode& n, bool upstreamDirection);
+
+  enum Direction
+  {
+    kUpstream,
+    kDownstream
+  };
+  void AddMaterialEffects(FitNode& n, Direction direction);
+
   // combine two tracks
   bool Smooth(kf::TrackParamV& t1, const kf::TrackParamV& t2);
 
@@ -150,6 +167,15 @@ class CbmKfTrackFitter {
   bool fSkipUnmeasuredCoordinates{false};
   ca::TrackFit fFit;  // track fit object
 
+  /// externally defined inverse momentum for the Multiple Scattering calculation.
+  /// It is used for the tracks in field-free regions.
+  /// When the momentum can be fitted, the fitted value is used.
+  /// the default value is set to 0.1 GeV/c
+  double fDefaultQpForMs{1. / 0.1};
+  bool fIsQpForMsFixed{false};
+
   double fMass{ca::constants::phys::PionMass};  // mass hypothesis for the fit
   bool fIsElectron{false};                      // fit track as an electron (with the bermsstrallung effect)
+  bool fDoSmooth{true};                         // do the KF-smoothing to define track pars at all the nodes
+  int fVerbosityLevel{0};                       // verbosity level
 };
diff --git a/reco/alignment/CbmBbaAlignmentTask.cxx b/reco/alignment/CbmBbaAlignmentTask.cxx
index 8684ec9aaab480513473115adf71dbe23fec3c95..fb0a71ea5e2db9b65c7bcd459bb6d18848edfa11 100644
--- a/reco/alignment/CbmBbaAlignmentTask.cxx
+++ b/reco/alignment/CbmBbaAlignmentTask.cxx
@@ -18,6 +18,7 @@
 #include "FairRootManager.h"
 #include "TClonesArray.h"
 #include "TFile.h"
+#include "TGeoPhysicalNode.h"
 #include "TH1F.h"
 #include "TRandom.h"
 
@@ -135,7 +136,7 @@ InitStatus CbmBbaAlignmentTask::Init()
     fNthreads = 1;
   }
 
-  //fNthreads = 1;
+  // fNthreads = 1; // enforce n threads to one
 
   fFitter.Init();
   fFitter.SetSkipUnmeasuredCoordinates(true);
@@ -207,7 +208,7 @@ InitStatus CbmBbaAlignmentTask::Init()
     fHistoDir->mkdir(n1);
     fHistoDir->cd(n1);
 
-    int nBins = 100;
+    int nBins = 301;
 
     double sizeX  = 0.1;
     double sizeY  = 0.1;
@@ -269,6 +270,13 @@ InitStatus CbmBbaAlignmentTask::Init()
       new TH1F(Form("PullAfterAli%s_Y", n1), Form("Y-Pulls After Alignment%s", n2), nBins, -sizePY, sizePY));
   }
 
+  for (int i = 0; i < fNtrackingStations + 1; i++) {
+    hResidualsAfterAlignmentX[i]->SetLineColor(kRed);
+    hResidualsAfterAlignmentY[i]->SetLineColor(kRed);
+    hPullsAfterAlignmentX[i]->SetLineColor(kRed);
+    hPullsAfterAlignmentY[i]->SetLineColor(kRed);
+  }
+
   gDirectory = curDirectory;
 
   return kSUCCESS;
@@ -291,6 +299,8 @@ void CbmBbaAlignmentTask::Exec(Option_t* /*opt*/)
   // select tracks for alignment and store them
   if (fTrackingMode == kSts && fInputStsTracks) {
 
+    fFitter.SetDefaultMomentumForMs(0.1);
+
     for (int iTr = 0; iTr < fInputStsTracks->GetEntriesFast(); iTr++) {
       if (static_cast<int>(fTracks.size()) >= fMaxNtracks) {
         break;
@@ -300,8 +310,6 @@ void CbmBbaAlignmentTask::Exec(Option_t* /*opt*/)
       TrackContainer t;
       if (!fFitter.CreateMvdStsTrack(t.fUnalignedTrack, iTr)) continue;
       t.MakeConsistent();
-      t.fUnalignedTrack.fMsQp0      = 0.;
-      t.fUnalignedTrack.fIsMsQp0Set = false;
 
       for (auto& n : t.fUnalignedTrack.fNodes) {
         n.fIsTimeSet = false;
@@ -312,10 +320,7 @@ void CbmBbaAlignmentTask::Exec(Option_t* /*opt*/)
         continue;
       }
 
-      const auto& par = t.fUnalignedTrack.fNodes[t.fUnalignedTrack.fFirstHitNode].fTrack;
-
-      t.fUnalignedTrack.fMsQp0 = par.GetQp()[0];
-      //t.fUnalignedTrack.fIsMsQp0Set = true;
+      const auto& par = t.fUnalignedTrack.fNodes.front().fTrack;
 
       if (t.fNstsHits < l1Par.GetNstationsActive(ca::EDetectorID::kSts)) continue;
       if (!std::isfinite((ca::fscal) par.GetQp()[0])) continue;
@@ -326,6 +331,11 @@ void CbmBbaAlignmentTask::Exec(Option_t* /*opt*/)
     }
   }
   else if (fTrackingMode == kMcbm && fInputGlobalTracks) {
+
+    fFitter.SetDefaultMomentumForMs(0.5);
+    fFitter.FixMomentumForMs(true);
+    fFitter.SetDoSmooth(true);
+
     for (int iTr = 0; iTr < fInputGlobalTracks->GetEntriesFast(); iTr++) {
       if (static_cast<int>(fTracks.size()) >= fMaxNtracks) {
         break;
@@ -341,8 +351,6 @@ void CbmBbaAlignmentTask::Exec(Option_t* /*opt*/)
         break;
       }
       t.MakeConsistent();
-      t.fUnalignedTrack.fMsQp0      = 1. / 0.5;
-      t.fUnalignedTrack.fIsMsQp0Set = true;
 
       for (auto& n : t.fUnalignedTrack.fNodes) {
         n.fIsTimeSet = false;
@@ -357,14 +365,67 @@ void CbmBbaAlignmentTask::Exec(Option_t* /*opt*/)
         n.fTrack.SetQp(0.);
       }
 
-      //if (t.fNstsHits < 1) continue;
-      //if (t.fNtrdHits < 2) continue;
-      if (t.fNstsHits + t.fNmuchHits + t.fNtrd1dHits + t.fNtrd2dHits + t.fNtofHits < fNtrackingStations - 1) continue;
-      //if (t.fNtrdHits < 3) continue;
       if (t.fNstsHits < 2) continue;
       if (t.fNtofHits < 2) continue;
+      if (t.fNtrd1dHits + t.fNtrd2dHits < 2) continue;
+      if (!fFitter.FitTrack(t.fUnalignedTrack)) {
+        LOG(warning) << "failed to fit the track! ";
+        continue;
+      }
+
+      // fix the material radiation thickness to avoid the chi2 rattling at the material map bin boundaries
+      // (to be improved)
+      {
+        bool isGood = true;
+        for (unsigned int in = 0; in < t.fUnalignedTrack.fNodes.size(); in++) {
+          CbmKfTrackFitter::FitNode& node = t.fUnalignedTrack.fNodes[in];
+          if (!node.fIsFitted) {
+            isGood = false;
+            break;
+          }
+          node.fIsRadThickFixed = true;
+          if (node.fRadThick > 0.01) {
+            LOG(warning) << "CbmBbaAlignmentTask: radiation thickness is too large: " << node.fRadThick;
+            node.fRadThick = 0.01;
+          }
+          if (node.fRadThick < 0.001) {
+            LOG(warning) << "CbmBbaAlignmentTask: radiation thickness is too small: " << node.fRadThick;
+            node.fRadThick = 0.001;
+          }
+        }
+        if (!isGood) continue;
+      }
+
+      // ensure that all the hits are not too much deviated from the trajectory
+      // (to be improved)
+      {
+        bool isGood       = true;
+        const double cutX = 8.;
+        const double cutY = 8.;
+        for (unsigned int in = 0; in < t.fUnalignedTrack.fNodes.size(); in++) {
+          CbmKfTrackFitter::Track tr      = t.fUnalignedTrack;
+          CbmKfTrackFitter::FitNode& node = tr.fNodes[in];
+          if (!node.fIsXySet) {
+            continue;
+          }
+          node.fIsXySet = false;  // exclude the node from the fit
+          if (!fFitter.FitTrack(tr)) {
+            isGood = false;
+            break;
+          }
+          double x_residual = node.fMxy.X()[0] - node.fTrack.X()[0];  // this should be the difference vector
+          double y_residual = node.fMxy.Y()[0] - node.fTrack.Y()[0];
+          double x_pull     = x_residual / sqrt(node.fMxy.Dx2()[0] + node.fTrack.GetCovariance(0, 0)[0]);
+          double y_pull     = y_residual / sqrt(node.fMxy.Dy2()[0] + node.fTrack.GetCovariance(1, 1)[0]);
+          if (!node.fIsFitted || (node.fMxy.NdfX()[0] > 0. && fabs(x_pull) > cutX)
+              || (node.fMxy.NdfY()[0] > 0. && fabs(y_pull) > cutY)) {
+            isGood = false;
+            break;
+          }
+        }
+        if (!isGood) continue;
+      }
 
-      fFitter.FitTrack(t.fUnalignedTrack);
       t.fAlignedTrack = t.fUnalignedTrack;
 
       fTracks.push_back(t);
@@ -386,14 +447,52 @@ void CbmBbaAlignmentTask::Exec(Option_t* /*opt*/)
 }
 
 
+void CbmBbaAlignmentTask::ConstrainStation(std::vector<double>& par, int iSta, int ixyz)
+{
+  // set overall shifts of the station to zero
+
+  double mean = 0.;
+  int n       = 0;
+  for (unsigned int i = 0; i < fAlignmentBodies.size(); i++) {
+    if (fAlignmentBodies[i].fTrackingStation == iSta) {
+      mean += par[3 * i + ixyz];
+      n++;
+    }
+  }
+  if (n <= 0) return;
+  mean /= n;
+  for (unsigned int i = 0; i < fAlignmentBodies.size(); i++) {
+    if (fAlignmentBodies[i].fTrackingStation == iSta) {
+      par[3 * i + ixyz] -= mean;
+    }
+  }
+}
+
+void CbmBbaAlignmentTask::ApplyConstraints(std::vector<double>& par)
+{
+  // apply alignment parameters to the hits
+  ConstrainStation(par, 0, 2);
+  ConstrainStation(par, fNtrackingStations - 1, 0);
+  ConstrainStation(par, fNtrackingStations - 1, 1);
+  ConstrainStation(par, fNtrackingStations - 1, 2);
+}
+
+
 void CbmBbaAlignmentTask::ApplyAlignment(const std::vector<double>& par)
 {
   // apply alignment parameters to the hits
 
+  std::vector<double> parConstrained = par;
+
+  ApplyConstraints(parConstrained);
+
   for (TrackContainer& t : fTracks) {
 
     for (unsigned int in = 0; in < t.fUnalignedTrack.fNodes.size(); in++) {
-      CbmKfTrackFitter::FitNode& node        = t.fUnalignedTrack.fNodes[in];
+      CbmKfTrackFitter::FitNode& node = t.fUnalignedTrack.fNodes[in];
+      if (!node.fIsXySet) {
+        continue;
+      }
       CbmKfTrackFitter::FitNode& nodeAligned = t.fAlignedTrack.fNodes[in];
       int iSensor                            = node.fReference1;
       assert(iSensor >= 0 && iSensor < (int) fSensors.size());
@@ -402,15 +501,27 @@ void CbmBbaAlignmentTask::ApplyAlignment(const std::vector<double>& par)
       Sensor& s = fSensors[iSensor];
       if (s.fAlignmentBody < 0) continue;
 
-      double dx = par[3 * s.fAlignmentBody + 0];
-      double dy = par[3 * s.fAlignmentBody + 1];
-      double dz = par[3 * s.fAlignmentBody + 2];
+      double dx = parConstrained[3 * s.fAlignmentBody + 0];
+      double dy = parConstrained[3 * s.fAlignmentBody + 1];
+      double dz = parConstrained[3 * s.fAlignmentBody + 2];
 
       nodeAligned.fMxy.SetX(node.fMxy.X() + ca::fvec(dx));
       nodeAligned.fMxy.SetY(node.fMxy.Y() + ca::fvec(dy));
       nodeAligned.fZ = node.fZ + dz;
     }
   }
+
+  static int statNCalls = 0;
+  statNCalls++;
+  if (statNCalls % 100 == 0) {
+    std::vector<double>& r = parConstrained;
+    LOG(info) << "Current parameters: ";
+    for (int is = 0; is < fNalignmentBodies; is++) {
+      auto& b = fAlignmentBodies[is];
+      LOG(info) << "Body " << is << " sta " << b.fTrackingStation << ": x " << r[3 * is + 0] << " y " << r[3 * is + 1]
+                << " z " << r[3 * is + 2];
+    }
+  }
 }
 
 
@@ -427,15 +538,19 @@ double CbmBbaAlignmentTask::CostFunction(const std::vector<double>& par)
   std::vector<double> chi2Thread(fNthreads, 0.);
   std::vector<long> ndfThread(fNthreads, 0);
 
+  fFitter.SetDoSmooth(false);
+
   auto fitThread = [&](int iThread) {
     CbmKfTrackFitter fit(fFitter);
     for (unsigned int itr = iThread; itr < fTracks.size(); itr += fNthreads) {
       auto& t = fTracks[itr];
       if (!t.fIsActive) continue;
-      fit.FitTrack(t.fAlignedTrack);
-      double chi2 = t.fAlignedTrack.fNodes[t.fAlignedTrack.fFirstHitNode].fTrack.GetChiSq()[0];
-      double ndf  = t.fAlignedTrack.fNodes[t.fAlignedTrack.fFirstHitNode].fTrack.GetNdf()[0];
-      if (!std::isfinite(chi2) || chi2 <= 0. || ndf <= 0.) {
+      bool ok     = fit.FitTrack(t.fAlignedTrack);
+      double chi2 = t.fAlignedTrack.fNodes.back().fTrack.GetChiSq()[0];
+      double ndf  = t.fAlignedTrack.fNodes.back().fTrack.GetNdf()[0];
+      if (!ok || !std::isfinite(chi2) || chi2 <= 0. || ndf <= 0.) {
+        LOG(fatal) << "BBA: fit failed! ";
+        assert(0);
         chi2 = 0.;
         ndf  = 0.;
       }
@@ -468,8 +583,8 @@ double CbmBbaAlignmentTask::CostFunction(const std::vector<double>& par)
   if (fFixedNdf > 0 && fNdfTotal != fFixedNdf) {
     cost = 1.e30;
   }
-  LOG(info) << "BBA: calculate cost function:  ndf " << fNdfTotal << ", cost " << cost
-            << ", diff to ideal cost: " << cost - fCostIdeal << ", diff to initial cost: " << cost - fCostInitial;
+  //LOG(info) << "BBA: calculate cost function:  ndf " << fNdfTotal << ", cost " << cost
+  //          << ", diff to ideal cost: " << cost - fCostIdeal << ", diff to initial cost: " << cost - fCostInitial;
 
   return cost;
 }
@@ -489,6 +604,9 @@ void CbmBbaAlignmentTask::Finish()
   std::set<Sensor> sensorSet;
   for (auto& t : fTracks) {
     for (auto& n : t.fUnalignedTrack.fNodes) {
+      if (!n.fIsXySet) {
+        continue;
+      }
       Sensor s;
       s.fSystemId = n.fHitSystemId;
       s.fSensorId = n.fHitAddress;
@@ -518,9 +636,13 @@ void CbmBbaAlignmentTask::Finish()
   for (auto& s : sensorSet) {  // convert set to a vector
     fSensors.push_back(s);
   }
+  std::sort(fSensors.begin(), fSensors.end(), [](const Sensor& a, const Sensor& b) { return a < b; });
 
   for (auto& t : fTracks) {
     for (auto& n : t.fUnalignedTrack.fNodes) {
+      if (!n.fIsXySet) {
+        continue;
+      }
       Sensor s;
       s.fSystemId        = n.fHitSystemId;
       s.fSensorId        = n.fHitAddress;
@@ -531,9 +653,11 @@ void CbmBbaAlignmentTask::Finish()
       else if (s.fSystemId == ECbmModuleId::kTof) {
         s.fSensorId = CbmTofAddress::GetRpcFullId(n.fHitAddress);
       }
-      auto iter = sensorSet.find(s);
-      assert(iter != sensorSet.end());
-      int iSensor   = std::distance(sensorSet.begin(), iter);
+      auto iter = std::lower_bound(  // find the sensor in the vector
+        fSensors.begin(), fSensors.end(), s, [](const Sensor& a, const Sensor& b) { return a < b; });
+      assert(iter != fSensors.end());
+      assert(*iter == s);
+      int iSensor   = std::distance(fSensors.begin(), iter);
       n.fReference1 = iSensor;
     }
     t.fAlignedTrack = t.fUnalignedTrack;
@@ -542,7 +666,10 @@ void CbmBbaAlignmentTask::Finish()
   if (0) {  // one alignment body per tracking station
 
     fNalignmentBodies = fNtrackingStations;
-
+    fAlignmentBodies.resize(fNalignmentBodies);
+    for (int i = 0; i < fNalignmentBodies; i++) {
+      fAlignmentBodies[i].fTrackingStation = i;
+    }
     for (auto& s : fSensors) {
       s.fAlignmentBody = s.fTrackingStation;
     }
@@ -550,14 +677,32 @@ void CbmBbaAlignmentTask::Finish()
   else {  // one alignment body per sensor
 
     fNalignmentBodies = fSensors.size();
-
+    fAlignmentBodies.resize(fNalignmentBodies);
     for (int unsigned i = 0; i < fSensors.size(); i++) {
-      fSensors[i].fAlignmentBody = i;
+      fSensors[i].fAlignmentBody           = i;
+      fAlignmentBodies[i].fTrackingStation = fSensors[i].fTrackingStation;
     }
   }
 
   LOG(info) << "BBA: " << fSensors.size() << " sensors, " << fNalignmentBodies << " alignment bodies";
 
+  LOG(info) << "\n Sensors: ";
+  {
+    for (unsigned int is = 0; is < fSensors.size(); is++) {
+      auto& s = fSensors[is];
+      LOG(info) << "Sensor " << is << " sys " << s.fSystemId << " sta " << s.fTrackingStation << " body "
+                << s.fAlignmentBody;
+    }
+  }
+
+  LOG(info) << "\n Alignment bodies: ";
+  {
+    for (int is = 0; is < fNalignmentBodies; is++) {
+      auto& b = fAlignmentBodies[is];
+      LOG(info) << "Body " << is << " sta " << b.fTrackingStation;
+    }
+  }
+
   int nParameters = 3 * fNalignmentBodies;  // x, y, z
 
   std::vector<bba::Parameter> par(nParameters);
@@ -566,34 +711,49 @@ void CbmBbaAlignmentTask::Finish()
     bba::Parameter& p = par[ip];
     p.SetActive(0);
     p.SetValue(0.);
-    p.SetBoundaryMin(-2);  // +-20 cm alignment range
-    p.SetBoundaryMax(2);
-    p.SetStepMin(1.e-2);
+    p.SetBoundaryMin(-20.);  // +-20 cm alignment range
+    p.SetBoundaryMax(20.);
+    p.SetStepMin(1.e-4);
     p.SetStepMax(0.1);
     p.SetStep(1.e-2);
   }
 
   // set active parameters
 
-  for (auto& s : fSensors) {
-    if (s.fAlignmentBody < 0) {
-      continue;
+  for (unsigned int ib = 0; ib < fAlignmentBodies.size(); ib++) {
+    auto& b = fAlignmentBodies[ib];
+    if (b.fTrackingStation == 0) {  // the first station
+      //continue;
     }
-    if (s.fTrackingStation == 0) {  // the first station
-      continue;
+    if (b.fTrackingStation == fNtrackingStations - 1) {  // the last station
+      //continue;
     }
-    if (s.fTrackingStation == fNtrackingStations - 1) {  // the last station
-      continue;
-    }
-    if (fTrackingMode == kSts && s.fTrackingStation == fNtrackingStations / 2) {  // station in the middle for STS mode
-      par[3 * s.fAlignmentBody + 0].SetActive(0);
-      par[3 * s.fAlignmentBody + 1].SetActive(1);
-      par[3 * s.fAlignmentBody + 2].SetActive(0);
+    if (fTrackingMode == kSts && b.fTrackingStation == fNtrackingStations / 2) {  // station in the middle for STS mode
+      par[3 * ib + 0].SetActive(0);
+      par[3 * ib + 1].SetActive(1);
+      par[3 * ib + 2].SetActive(0);
     }
     else {
-      par[3 * s.fAlignmentBody + 0].SetActive(1);
-      par[3 * s.fAlignmentBody + 1].SetActive(1);
-      par[3 * s.fAlignmentBody + 2].SetActive(1);
+      par[3 * ib + 0].SetActive(1);
+      par[3 * ib + 1].SetActive(1);
+      par[3 * ib + 2].SetActive(1);
+    }
+  }
+
+  // set parameter ranges for different subsystems
+  for (const auto& s : fSensors) {
+    int ib = s.fAlignmentBody;
+    if (ib < 0 || ib >= fNalignmentBodies) continue;
+    //auto& b = fAlignmentBodies[ib];
+    for (int j = 0; j < 3; j++) {
+      bba::Parameter& p = par[ib * 3 + j];
+      p.SetStepMin(1.e-4);
+      if (s.fSystemId == ECbmModuleId::kTrd || s.fSystemId == ECbmModuleId::kTrd2d) {
+        p.SetStepMin(10.e-4);
+      }
+      else if (s.fSystemId == ECbmModuleId::kTof) {
+        p.SetStepMin(10.e-4);
+      }
     }
   }
 
@@ -617,6 +777,26 @@ void CbmBbaAlignmentTask::Finish()
     }
   }
 
+  if (0) {  //  test
+    LOG(info) << "STS sensor paths: ";
+    for (auto& s : fSensors) {
+      if (s.fSystemId != ECbmModuleId::kSts) {
+        continue;
+      }
+      const CbmStsElement* el = CbmStsSetup::Instance()->GetElement(s.fSensorId, kStsSensor);
+      if (!el) {
+        LOG(error) << "Element not found: " << s.fSensorId;
+        continue;
+      }
+      el->Print();
+      const auto* n = el->GetPnode();
+      if (n) {
+        LOG(info) << "Node: ";
+        n->Print();
+      }
+    }
+  }
+
   bba::BBA alignment;
 
   alignment.setParameters(par);
@@ -642,8 +822,8 @@ void CbmBbaAlignmentTask::Finish()
     fCostInitial = CostFunction(parMisaligned);
 
     for (auto& t : fTracks) {
-      double ndf  = t.fAlignedTrack.fNodes[t.fAlignedTrack.fFirstHitNode].fTrack.GetNdf()[0];
-      double chi2 = t.fAlignedTrack.fNodes[t.fAlignedTrack.fFirstHitNode].fTrack.GetChiSq()[0];
+      double ndf  = t.fAlignedTrack.fNodes.back().fTrack.GetNdf()[0];
+      double chi2 = t.fAlignedTrack.fNodes.back().fTrack.GetChiSq()[0];
       if (ndf < 0. || chi2 < 0. || !std::isfinite(ndf) || !std::isfinite(chi2)) {
         t.fIsActive = false;
       }
@@ -659,9 +839,10 @@ void CbmBbaAlignmentTask::Finish()
       for (unsigned int in = 0; in < t.fAlignedTrack.fNodes.size(); in++) {
         CbmKfTrackFitter::Track tr      = t.fAlignedTrack;
         CbmKfTrackFitter::FitNode& node = tr.fNodes[in];
-        if (!node.fIsXySet) continue;
+        if (!node.fIsXySet) {
+          continue;
+        }
         node.fIsXySet = false;
-        tr.MakeConsistent();
         fFitter.FitTrack(tr);
 
         Sensor& s = fSensors[node.fReference1];
@@ -767,16 +948,40 @@ void CbmBbaAlignmentTask::Finish()
   }
 
   LOG(info) << "\n Alignment results: ";
-
-  for (int is = 0; is < fNalignmentBodies; is++) {
+  {
     const std::vector<double>& r = alignment.getResult();
-    LOG(info) << "Body " << is << ": x " << r[3 * is + 0] << " y " << r[3 * is + 1] << " z " << r[3 * is + 2];
+    for (int is = 0; is < fNalignmentBodies; is++) {
+      auto& b = fAlignmentBodies[is];
+      LOG(info) << "Body " << is << " sta " << b.fTrackingStation << ": x " << r[3 * is + 0] << " y " << r[3 * is + 1]
+                << " z " << r[3 * is + 2];
+    }
+  }
+
+  LOG(info) << "\n";
+
+  LOG(info) << " Cost function for the true parameters: " << fCostIdeal;
+  LOG(info) << " Cost function for the initial parameters: " << fCostInitial;
+  LOG(info) << " Cost function for the aligned parameters: " << costResult;
+
+  LOG(info) << "\n Alignment results, constrained: ";
+  {
+    std::vector<double> r = alignment.getResult();
+    ApplyConstraints(r);
+    for (int is = 0; is < fNalignmentBodies; is++) {
+      auto& b = fAlignmentBodies[is];
+      LOG(info) << "Body " << is << " sta " << b.fTrackingStation << ": x " << r[3 * is + 0] << " y " << r[3 * is + 1]
+                << " z " << r[3 * is + 2];
+    }
   }
 
 
   LOG(info) << "\n";
 
-  LOG(info) << "Bba: " << fTracks.size() << " tracks used for alignment";
+  LOG(info) << "Bba: " << fTracks.size() << " tracks used for alignment\n";
+
+  LOG(info) << " Cost function for the true parameters: " << fCostIdeal;
+  LOG(info) << " Cost function for the initial parameters: " << fCostInitial;
+  LOG(info) << " Cost function for the aligned parameters: " << costResult;
 
   for (auto& t : fTracks) {
     if (!t.fIsActive) continue;
@@ -784,9 +989,10 @@ void CbmBbaAlignmentTask::Finish()
     for (unsigned int in = 0; in < t.fAlignedTrack.fNodes.size(); in++) {
       CbmKfTrackFitter::Track tr      = t.fAlignedTrack;
       CbmKfTrackFitter::FitNode& node = tr.fNodes[in];
-      if (!node.fIsXySet) continue;
+      if (!node.fIsXySet) {
+        continue;
+      }
       node.fIsXySet = false;
-      tr.MakeConsistent();
       fFitter.FitTrack(tr);
 
       Sensor& s = fSensors[node.fReference1];
diff --git a/reco/alignment/CbmBbaAlignmentTask.h b/reco/alignment/CbmBbaAlignmentTask.h
index f57fe502ea04205a3c1fb85e60c98dd1b7a8762a..e89dd022dac1bae2bc5bb87daf6298394bb6f590 100644
--- a/reco/alignment/CbmBbaAlignmentTask.h
+++ b/reco/alignment/CbmBbaAlignmentTask.h
@@ -79,6 +79,10 @@ class CbmBbaAlignmentTask : public FairTask {
     void MakeConsistent();
   };
 
+  struct AlignmentBody {
+    int fTrackingStation{-1};
+  };
+
   struct Sensor {
     ECbmModuleId fSystemId{0};
     int fSensorId{-1};
@@ -89,8 +93,11 @@ class CbmBbaAlignmentTask : public FairTask {
     {
       if (fSystemId < other.fSystemId) return true;
       if (fSystemId > other.fSystemId) return false;
+      if (fTrackingStation < other.fTrackingStation) return true;
+      if (fTrackingStation > other.fTrackingStation) return false;
       return (fSensorId < other.fSensorId);
     };
+
     bool operator==(const Sensor& other) const
     {
       return (fSystemId == other.fSystemId) && (fSensorId == other.fSensorId);
@@ -107,6 +114,11 @@ class CbmBbaAlignmentTask : public FairTask {
 
   double CostFunction(const std::vector<double>& par);
 
+  void ApplyConstraints(std::vector<double>& par);
+
+  void ConstrainStation(std::vector<double>& par, int iSta, int ixyz);
+
+
   TrackingMode fTrackingMode = TrackingMode::kMcbm;
 
   // input data arrays
@@ -147,6 +159,7 @@ class CbmBbaAlignmentTask : public FairTask {
   long fFixedNdf{-1};
 
   std::vector<Sensor> fSensors;
+  std::vector<AlignmentBody> fAlignmentBodies;
 
   //histograms
 
diff --git a/reco/qa/CbmRecoQaTask.cxx b/reco/qa/CbmRecoQaTask.cxx
index d2daa4fa967fa388ee68210a80dacfb24c6d8b40..655ab5de0fb0e9b16aaafea71e3711b83a7ad782 100644
--- a/reco/qa/CbmRecoQaTask.cxx
+++ b/reco/qa/CbmRecoQaTask.cxx
@@ -324,6 +324,9 @@ void CbmRecoQaTask::Exec(Option_t*)
   LOG(info) << GetName() << "::Exec : Evs[" << (fEvents ? fEvents->GetEntriesFast() : 0) << "] Trks["
             << (fGTracks ? fGTracks->GetEntriesFast() : 0) << "]";
 
+  fFitter.SetDefaultMomentumForMs(0.5);
+  fFitter.FixMomentumForMs(true);  // consider only mcbm case for a moment
+
   int iev(0), itrack(0), nnodes(0);
   if (fEvents) {
     for (auto evObj : *fEvents) {  // EVENT LOOP
@@ -379,8 +382,6 @@ void CbmRecoQaTask::Exec(Option_t*)
           LOG(fatal) << GetName() << "::Exec: can not create the track for the fit! ";
           break;
         }
-        trkKf.fMsQp0      = 1. / 0.5;
-        trkKf.fIsMsQp0Set = true;
 
         // minimalistic QA tool for tracks used for target projection
         int nhit[(size_t) ECbmModuleId::kLastModule] = {0};
@@ -400,7 +401,6 @@ void CbmRecoQaTask::Exec(Option_t*)
 
           n.fIsTimeSet = false;
           n.fIsXySet   = false;
-          trkKf.MakeConsistent();
           fFitter.FitTrack(trkKf);
           view->Load(&n);
           nhit[(int) n.fHitSystemId]++;
@@ -418,7 +418,7 @@ void CbmRecoQaTask::Exec(Option_t*)
           n.fReference1               = iprj++;
           trkKf.fNodes.push_back(n);
         }
-        trkKf.MakeConsistent();
+        trkKf.OrderNodesInZ();
         fFitter.FitTrack(trkKf);
         for (auto& n : trkKf.fNodes) {
           if (n.fReference1 < 0) continue;
@@ -492,8 +492,7 @@ void CbmRecoQaTask::Exec(Option_t*)
       LOG(fatal) << GetName() << "::Exec: can not create the track for the fit! ";
       break;
     }
-    trkKf.fMsQp0      = 1. / 0.5;
-    trkKf.fIsMsQp0Set = true;
+
     if (!nnodes) nnodes = (int) trkKf.fNodes.size();
 
     // minimalistic QA tool for tracks used for target projection
@@ -514,7 +513,6 @@ void CbmRecoQaTask::Exec(Option_t*)
 
       n.fIsTimeSet = false;
       n.fIsXySet   = false;
-      trkKf.MakeConsistent();
       fFitter.FitTrack(trkKf);
       view->Load(&n);
       nhit[(int) n.fHitSystemId]++;
@@ -531,7 +529,7 @@ void CbmRecoQaTask::Exec(Option_t*)
       n.fReference1               = iprj++;
       trkKf.fNodes.push_back(n);
     }
-    trkKf.MakeConsistent();
+    trkKf.OrderNodesInZ();
     fFitter.FitTrack(trkKf);
     for (auto& n : trkKf.fNodes) {
       if (n.fReference1 < 0) continue;