From adc58115fd927c95bdc7f899b92aae03d314b5aa 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:40:40 +0000
Subject: [PATCH] Update tdcp.cpp
---
src/tdcp.cpp | 18 ++++++++----------
1 file changed, 8 insertions(+), 10 deletions(-)
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index a270b8c..4004d86 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -453,20 +453,19 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
r_raim(row_removed) = 0;
// solve
- 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;
+ Eigen::Vector4d d_raim = d_0 - (A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
// no NaN
if (!d_raim.array().isNaN().any())
{
// residual
if (tdcp_params.residual_opt == 0) // RMSE
- residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / (A_raim.rows() - raim_discarded_rows.size() - 1)) / sqrt(var_tdcp);
+ residual = sqrt((r_raim + A_raim * 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 * delta_d_raim).cwiseAbs2().maxCoeff() / var_tdcp;
+ residual = (r_raim + A_raim * d_raim).cwiseAbs2().maxCoeff() / var_tdcp;
else
throw std::runtime_error("unknown value for residual_opt");
- // residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1);
+ // residual = (r_raim + A_raim * d_raim).norm() / (A_raim.rows() - 1);
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "RAIM excluding row " << row_removed;// << std::endl;
@@ -502,20 +501,19 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
r_raim(worst_sat_row) = 0;
// solve
- 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;
+ Eigen::Vector4d d_raim = d_0 - (A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
// no NaN
if (!d_raim.array().isNaN().any())
{
// residual
if (tdcp_params.residual_opt == 0) // RMSE
- residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / (A_raim.rows() - raim_discarded_rows.size() - 1)) / sqrt(var_tdcp);
+ residual = sqrt((r_raim + A_raim * 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 * delta_d_raim).cwiseAbs2().maxCoeff() / var_tdcp;
+ residual = (r_raim + A_raim * d_raim).cwiseAbs2().maxCoeff() / var_tdcp;
else
throw std::runtime_error("unknown value for residual_opt");
- // residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1);
+ // residual = (r_raim + A_raim * d_raim).norm() / (A_raim.rows() - 1);
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "RAIM excluding row " << worst_sat_row;// << std::endl;
--
GitLab