diff --git a/include/mcc/mcc_pcm_construct.h b/include/mcc/mcc_pcm_construct.h
index 41698f4..04e3aa2 100644
--- a/include/mcc/mcc_pcm_construct.h
+++ b/include/mcc/mcc_pcm_construct.h
@@ -412,7 +412,13 @@ protected:
             tukey_weight = r.array() * r.array();
         };
 
-        auto gastwirth_est = [](Eigen::VectorXd const& resi) {
+        auto gastwirth_est = [](Eigen::VectorXd const& r) {
+            // auto gastwirth_est = [](Eigen::VectorXd const& resi) {
+            auto resi = r;
+            std::nth_element(resi.begin(), resi.begin() + resi.size() / 2, resi.end());
+            double md = resi(resi.size() / 2);
+            resi = resi.array() - md;
+
             // Gastwirth’s estimation: 0.3*Q(1/3) + 0.4*Q(1/2) + 0.3*Q(2/3)
 
             Eigen::ArrayXd arr = resi.cwiseAbs();
@@ -434,6 +440,7 @@ protected:
 
         Eigen::MatrixXd X(2 * numberOfPoints(), n_coeffs);
         Eigen::VectorXd y(2 * numberOfPoints()), beta = Eigen::VectorXd::Zero(n_coeffs), new_beta(n_coeffs);
+        Eigen::VectorXd model;
 
         Eigen::RowVectorXd row(n_coeffs);
         double tgY, sinY, cosY, rcosY, cosX, sinX, cosPhi = std::cos(pcm_data.siteLatitude),
@@ -490,6 +497,16 @@ protected:
 
         double scale;
 
+        Eigen::MatrixXd WX;
+        Eigen::VectorXd Wy;
+
+        // initial parameters estimation (least squares)
+        beta = X.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(y);
+        model = X * beta;
+        resi = y - model;
+
+        resi = y;
+
         for (result.final_iter = 1; result.final_iter <= comp_params.max_iter; ++result.final_iter) {
             scale = gastwirth_est(resi);
 
@@ -501,13 +518,18 @@ protected:
 
             new_beta = XtWX.ldlt().solve(XtWy);
 
+            // WX = weights.asDiagonal() * X;
+            // Wy = weights.asDiagonal() * y;
+            // new_beta = WX.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Wy);
+
             if ((new_beta - beta).norm() < comp_params.tolerance) {
                 break;
             }
 
             beta = new_beta;
 
-            resi = y - X * beta;
+            model = X * beta;
+            resi = y - model;
         }
 
         pcm_data.geomCoefficients.zeroPointX = new_beta(0);
@@ -522,7 +544,6 @@ protected:
             pcm_data.geomCoefficients.forkFlexure = new_beta(8);
         }
 
-        Eigen::VectorXd model = X * new_beta;
         result.model_colon = {model.begin(), model.begin() + numberOfPoints()};
         result.model_colat = {model.begin() + numberOfPoints(), model.end()};
         result.colon_res.resize(numberOfPoints());
diff --git a/tests/mcc_pcm_test.cpp b/tests/mcc_pcm_test.cpp
index 3a852dd..eb21077 100644
--- a/tests/mcc_pcm_test.cpp
+++ b/tests/mcc_pcm_test.cpp
@@ -33,9 +33,27 @@ int main()
                                                     .forkFlexure = 9.342354_arcsecs}};
 
 
+    size_t haM = 10;   // number of B-spline inner knots along HA-axis
+    size_t decM = 10;  // number of B-spline inner knots along DEC-axis
+
+    double ha_step = 360.0_degs / (haM - 1);
+    pcm_data.bspline.knotsX.resize(haM);  // [0, 360]
+    for (size_t i = 0; i < haM; ++i) {
+        pcm_data.bspline.knotsX[i] = i * ha_step;
+    }
+
+    double dec_start = -35.0_degs;
+    double dec_step = (90.0_degs - dec_start) / (decM - 1);
+    pcm_data.bspline.knotsY.resize(decM);
+    for (size_t i = 0; i < decM; ++i) {
+        pcm_data.bspline.knotsY[i] = dec_start + i * dec_step;
+    }
+
+
     pcm.setPCMData(pcm_data);
 
-    size_t N = 100;
+
+    size_t N = 100;  // number of measurements
 
     std::vector<mcc::impl::MccAngle> encX(N), encY(N), ha(N), dec(N), pcmX(N), pcmY(N);
     std::vector<mcc::impl::MccAngle> resX(N), resY(N);
@@ -71,6 +89,7 @@ int main()
         encY[i] = start_encY + i * y_step;
     }
 
+
     comp_hadec();
 
     std::println("X Y HA DEC (pcmX pcmY):");
@@ -147,6 +166,7 @@ int main()
 
 
     std::println("\n\n{}", r.colon_res);
+    std::println("\n\n{}", r.colat_res);
 
     return 0;
 }
\ No newline at end of file