diff --git a/include/mcc/mcc_pcm_construct.h b/include/mcc/mcc_pcm_construct.h index 1697822..e70d756 100644 --- a/include/mcc/mcc_pcm_construct.h +++ b/include/mcc/mcc_pcm_construct.h @@ -117,8 +117,7 @@ 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 + size_t final_iter{}; // number of final iteration std::vector model_colon{}, model_colat{}; // fitting model values std::vector colon_res{}, colat_res{}; // target - model @@ -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::pcm_data_t& pcm_data) + compute_result_t computeModel(MccDefaultPCM::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 xres(numberOfPoints()), yres(numberOfPoints()); + std::vector xres = _table.colon_res, yres = _table.colat_res; for (size_t i = 0; i < numberOfPoints(); ++i) { - xres = _table.target_colon[i] - result.model_colon; - yres = _table.target_colat[i] - result.model_colat; + _table.colon_res = _table.target_colon[i] - result.model_colon; + _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::pcm_data_t& pcm_data, size_t max_iter = 100) + compute_result_t robustLinearRegress(MccDefaultPCM::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{ - .pcm_type = pcm_data.type, .error = MccDefaultPCMConstructorErrorCode::ERROR_OK, .max_iter = max_iter}; + compute_result_t result{.pcm_type = pcm_data.type, + .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] + // [X0, Y0, collim, nonperp, misal1, misal2, tubefl, DECaxfl, forkfl] or + // [X0, Y0, collim, nonperp, misal1, misal2, tubefl, DECaxfl] if constexpr (MOUNT_TYPE == MccMountType::FORK_TYPE) { - row << 1.0, 0.0, rcosY, tgY, -cosX * tgY, sinX * tgY, cosPhi * sinX * rcosY, - -(cosPhi * cosX + sinPhi * tgY), 0.0; + 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 { - row << 1.0, 0.0, rcosY, tgY, -cosX * tgY, sinX * tgY, cosPhi * sinX * rcosY, - -(cosPhi * cosX + sinPhi * tgY); + 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, - 0.0, 1.0 / cosX; + if (utils::isEqual(cosX, 0.0)) { + 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; } };