...

include/mcc/mcc_pcm_construct.h

@@ -117,7 +117,6 @@ public:
         MccDefaultPCMType pcm_type;
         MccError error{};  // final model computation error
 
-        size_t max_iter{100};  // max number of iterations for robust linear regression method
         size_t final_iter{};  // number of final iteration
 
         std::vector<double> model_colon{}, model_colat{};  // fitting model values
@@ -132,6 +131,12 @@ public:
 #endif
     };
 
+    struct compute_params_t {
+        size_t max_iter{100};      // max number of iterations for robust linear regression method
+        double c{4.685};           // Tukey's parameter
+        double tolerance{1.0E-6};  //
+    };
+
     MccError addPoint(mcc_skypoint_c auto target_coords, mcc_coord_pair_c auto const& hw_counts)
         requires(decltype(hw_counts)::pairKind == MccCoordPairKind::COORDS_KIND_XY)
     {
@@ -209,7 +214,8 @@ public:
     //
     // WARNING: the input knots for inverse B-spline are ignored so the direct and inverse B-spline coefficients are
     //          calculated on the same mesh!
-    compute_result_t computeModel(MccDefaultPCM<MOUNT_TYPE>::pcm_data_t& pcm_data)
+    compute_result_t computeModel(MccDefaultPCM<MOUNT_TYPE>::pcm_data_t& pcm_data,
+                                  compute_params_t const& comp_params = {})
     {
         compute_result_t result{.pcm_type = pcm_data.type, .error = MccDefaultPCMConstructorErrorCode::ERROR_OK};
 
@@ -232,6 +238,7 @@ public:
             }
 
             // robust linear regression with Tukey's loss function
+            result = robustLinearRegress(pcm_data, comp_params);
         } else {
             if (_table.size() < min_data_size) {
                 result.error = MccDefaultPCMConstructorErrorCode::ERROR_NOT_ENOUGH_DATA;
@@ -247,13 +254,33 @@ public:
             // the fitting for geometrical coefficients is already done above so
             // one must fit residuals by bivariate B-splines
 
-            std::vector<double> xres(numberOfPoints()), yres(numberOfPoints());
+            std::vector<double> xres = _table.colon_res, yres = _table.colat_res;
             for (size_t i = 0; i < numberOfPoints(); ++i) {
-                xres = _table.target_colon[i] - result.model_colon;
+                _table.colon_res = _table.target_colon[i] - result.model_colon;
-                yres = _table.target_colat[i] - result.model_colat;
+                _table.colat_res = _table.target_colat[i] - result.model_colat;
             }
+
+            auto r = bsplineFitting(pcm_data);
+            result.error = r.error;
+            result.bspline_fit_err = r.bspline_fit_err;
+
+            if (!r.error) {
+                for (size_t i = 0; i < numberOfPoints(); ++i) {
+                    result.model_colon += r.model_colon;
+                    result.model_colat += r.model_colat;
+
+                    result.colon_res = _table.target_colon[i] - result.model_colon;
+                    result.colat_res = _table.target_colat[i] - result.model_colat;
+                }
+            }
+
+            // restore original residuals
+            _table.colon_res = xres;
+            _table.colat_res = yres;
         }
 #endif
+
+        return result;
     }
 
 protected:
@@ -373,7 +400,8 @@ protected:
     }
 
 
-    compute_result_t robustLinearRegress(MccDefaultPCM<MOUNT_TYPE>::pcm_data_t& pcm_data, size_t max_iter = 100)
+    compute_result_t robustLinearRegress(MccDefaultPCM<MOUNT_TYPE>::pcm_data_t& pcm_data,
+                                         compute_params_t const& comp_params)
     {
         auto tukey_weight = [](Eigen::VectorXd const& resi, double tukey_c, Eigen::VectorXd& tukey_weight) {
             Eigen::ArrayXd u = (resi / tukey_c).cwiseAbs();
@@ -396,13 +424,14 @@ protected:
                    1.0E-8;
         };
 
-        compute_result_t result{
+        compute_result_t result{.pcm_type = pcm_data.type,
-            .pcm_type = pcm_data.type, .error = MccDefaultPCMConstructorErrorCode::ERROR_OK, .max_iter = max_iter};
+                                .error = MccDefaultPCMConstructorErrorCode::ERROR_OK,
+                                .max_iter = comp_params.max_iter};
 
         const size_t n_coeffs = pcm_data.type == MOUNT_TYPE == MccMountType::FORK_TYPE ? 9 : 8;
 
         Eigen::MatrixXd X(2 * numberOfPoints(), n_coeffs);
-        Eigen::VectorXd y(2 * numberOfPoints()), beta(n_coeffs);
+        Eigen::VectorXd y(2 * numberOfPoints()), beta(n_coeffs), new_beta(n_coeffs);
 
         Eigen::RowVectorXd row(n_coeffs);
         double tgY, cosY, rcosY, cosX, sinX, cosPhi = std::cos(pcm_data.siteLatitude),
@@ -417,21 +446,34 @@ protected:
             sinX = std::sin(_table.target_colon[i]);
 
 
-            // [X0, Y0, collim, nonperp, misal1, misal2, tubefl, DEcaxfl, forkfl] or
+            // [X0, Y0, collim, nonperp, misal1, misal2, tubefl, DECaxfl, forkfl] or
-            // [X0, Y0, collim, nonperp, misal1, misal2, tubefl, DEcaxfl]
+            // [X0, Y0, collim, nonperp, misal1, misal2, tubefl, DECaxfl]
             if constexpr (MOUNT_TYPE == MccMountType::FORK_TYPE) {
+                if (utils::isEqual(cosY, 0.0)) {
+                    row << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+                } else {
                     row << 1.0, 0.0, rcosY, tgY, -cosX * tgY, sinX * tgY, cosPhi * sinX * rcosY,
                         -(cosPhi * cosX + sinPhi * tgY), 0.0;
+                }
+            } else {
+                if (utils::isEqual(cosY, 0.0)) {
+                    row << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
                 } else {
                     row << 1.0, 0.0, rcosY, tgY, -cosX * tgY, sinX * tgY, cosPhi * sinX * rcosY,
                         -(cosPhi * cosX + sinPhi * tgY);
                 }
+            }
 
             X.row(i) = row;
 
             if constexpr (MOUNT_TYPE == MccMountType::FORK_TYPE) {
-                row << 0.0, 1.0, 0.0, 0.0, sinX, cosX, cosPhi * cosX * std::sin(_table.target_colat[i]) - sinPhi * cosY,
+                if (utils::isEqual(cosX, 0.0)) {
-                    0.0, 1.0 / cosX;
+                    row << 0.0, 1.0, 0.0, 0.0, sinX, cosX,
+                        cosPhi * cosX * std::sin(_table.target_colat[i]) - sinPhi * cosY, 0.0, 0.0;
+                } else {
+                    row << 0.0, 1.0, 0.0, 0.0, sinX, cosX,
+                        cosPhi * cosX * std::sin(_table.target_colat[i]) - sinPhi * cosY, 0.0, 1.0 / cosX;
+                }
             } else {
                 row << 0.0, 1.0, 0.0, 0.0, sinX, cosX, cosPhi * cosX * std::sin(_table.target_colat[i]) - sinPhi * cosY,
                     0.0;
@@ -448,9 +490,48 @@ protected:
 
         double scale;
 
-        for (result.final_iter = 1; result.final_iter <= max_iter; ++result.final_iter) {
+        for (result.final_iter = 1; result.final_iter <= comp_params.max_iter; ++result.final_iter) {
             scale = gastwirth_est(resi);
+
+            tukey_weight(resi / scale, comp_params.c, weights);
+
+            Eigen::MatrixXd XtW = X.transpose() * weights.asDiagonal();
+            Eigen::MatrixXd XtWX = XtW * X;
+            Eigen::VectorXd XtWy = XtW * y;
+
+            new_beta = XtWX.ldlt().solve(XtWy);
+
+            if ((new_beta - beta).norm() < comp_params.tolerance) {
+                break;
             }
+
+            beta = new_beta;
+        }
+
+        pcm_data.geomCoefficients.zeroPointX = new_beta(0);
+        pcm_data.geomCoefficients.zeroPointY = new_beta(1);
+        pcm_data.geomCoefficients.collimationErr = new_beta(2);
+        pcm_data.geomCoefficients.nonperpendErr = new_beta(3);
+        pcm_data.geomCoefficients.misalignErr1 = new_beta(4);
+        pcm_data.geomCoefficients.misalignErr2 = new_beta(5);
+        pcm_data.geomCoefficients.tubeFlexure = new_beta(6);
+        pcm_data.geomCoefficients.DECaxisFlexure = new_beta(7);
+        if constexpr (MOUNT_TYPE == MccMountType::FORK_TYPE) {
+            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()};
+
+        for (size_t i = 0; i < numberOfPoints(); ++i) {
+            result.colon_res[i] = _table.colon_res[i] - result.model_colon[i];  // = target - model
+            result.colat_res[i] = _table.colat_res[i] - result.model_colat[i];  // = target - model
+            result.model_colon[i] += _table.hw_colon[i];                        // == hw + fitted_pcmX
+            result.model_colat[i] += _table.hw_colat[i];                        // == hw + fitted_pcmY
+        }
+
+        return result;
     }
 };