From 8d6e6aaa366aecb65520bb5abe255c2ff7335a8f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Mon, 21 Jun 2021 08:42:34 +0000 Subject: [PATCH] Revert "Update tdcp.cpp" This reverts commit adc58115fd927c95bdc7f899b92aae03d314b5aa --- src/tdcp.cpp | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/tdcp.cpp b/src/tdcp.cpp index 4004d86..a270b8c 100644 --- a/src/tdcp.cpp +++ b/src/tdcp.cpp @@ -453,19 +453,20 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, r_raim(row_removed) = 0; // solve - Eigen::Vector4d d_raim = d_0 - (A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim; + Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim; + Eigen::Vector4d d_raim = d_0 + delta_d_raim; // no NaN if (!d_raim.array().isNaN().any()) { // residual if (tdcp_params.residual_opt == 0) // RMSE - residual = sqrt((r_raim + A_raim * d_raim).squaredNorm() / (A_raim.rows() - raim_discarded_rows.size() - 1)) / sqrt(var_tdcp); + residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / (A_raim.rows() - raim_discarded_rows.size() - 1)) / sqrt(var_tdcp); else if (tdcp_params.residual_opt == 1) // max error in squared Mahalanobis distance (using measure noise) - residual = (r_raim + A_raim * d_raim).cwiseAbs2().maxCoeff() / var_tdcp; + residual = (r_raim + A_raim * delta_d_raim).cwiseAbs2().maxCoeff() / var_tdcp; else throw std::runtime_error("unknown value for residual_opt"); - // residual = (r_raim + A_raim * d_raim).norm() / (A_raim.rows() - 1); + // residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1); #if GNSS_UTILS_TDCP_DEBUG == 1 std::cout << "RAIM excluding row " << row_removed;// << std::endl; @@ -501,19 +502,20 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, r_raim(worst_sat_row) = 0; // solve - Eigen::Vector4d d_raim = d_0 - (A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim; + Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim; + Eigen::Vector4d d_raim = d_0 + delta_d_raim; // no NaN if (!d_raim.array().isNaN().any()) { // residual if (tdcp_params.residual_opt == 0) // RMSE - residual = sqrt((r_raim + A_raim * d_raim).squaredNorm() / (A_raim.rows() - raim_discarded_rows.size() - 1)) / sqrt(var_tdcp); + residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / (A_raim.rows() - raim_discarded_rows.size() - 1)) / sqrt(var_tdcp); else if (tdcp_params.residual_opt == 1) // max error in squared Mahalanobis distance (using measure noise) - residual = (r_raim + A_raim * d_raim).cwiseAbs2().maxCoeff() / var_tdcp; + residual = (r_raim + A_raim * delta_d_raim).cwiseAbs2().maxCoeff() / var_tdcp; else throw std::runtime_error("unknown value for residual_opt"); - // residual = (r_raim + A_raim * d_raim).norm() / (A_raim.rows() - 1); + // residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1); #if GNSS_UTILS_TDCP_DEBUG == 1 std::cout << "RAIM excluding row " << worst_sat_row;// << std::endl; -- GitLab