From cf502f3fca6b1ed37a61cf7f1570e432bb46a800 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 29 Jul 2021 18:37:09 +0200
Subject: [PATCH] residual and residual_ci
---
include/gnss_utils/tdcp.h | 1 +
src/tdcp.cpp | 111 +++++++++++++++++++++++---------------
2 files changed, 68 insertions(+), 44 deletions(-)
diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index 28cef3d..00ca035 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -18,6 +18,7 @@ struct TdcpOutput
Eigen::Vector4d d = Eigen::Vector4d::Zero();
Eigen::Matrix4d cov_d = Eigen::Matrix4d::Zero();
double dt = 0;
+ double residual = 0;
double residual_ci = 0;
};
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index 6093631..aa3720f 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -288,8 +288,11 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
TdcpOutput output;
Eigen::Vector4d& d(output.d);
Eigen::Matrix4d& cov_d(output.cov_d);
+ double& residual(output.residual);
double& residual_ci(output.residual_ci);
+ const int n_rows = A.rows();
+
// Initial guess
Eigen::Vector4d delta_d(Eigen::Vector4d::Zero());
d = d_0;
@@ -297,7 +300,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
// initial sagnac correction
Eigen::VectorXd prev_sagnac_corr(s_k.cols());
if (tdcp_params.sagnac_correction == 2 )
- for (int row = 0; row < A.rows(); row++)
+ for (int row = 0; row < n_rows; row++)
{
double sagnac_corr_r = computeSagnacCorrection(x_r,s_r.col(row)); //OMGE*(s_r.col(row)(0)*x_r(1)-s_r.col(row)(1)*x_r(0))/CLIGHT;
double sagnac_corr_k = computeSagnacCorrection(x_r+d_0.head(3),s_k.col(row)); //OMGE*(s_k.col(row)(0)*(x_r(1)+d_0(1))-s_k.col(row)(1)*(x_r(0)+d_0(0)))/CLIGHT;
@@ -308,7 +311,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
for (int j = 0; j < tdcp_params.max_iterations; j++)
{
// fill A and r
- for (int row = 0; row < A.rows(); row++)
+ for (int row = 0; row < n_rows; row++)
{
// skip discarded rows
if (raim_discarded_rows.count(row) != 0)
@@ -360,7 +363,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl;
printf("Ref distance = %7.3f m\n", d_0.norm());
printf("Computed distance = %7.3f m\n", d.head<3>().norm());
- printf("Tdcp: rows = %lu\n", r.rows());
+ printf("Tdcp: rows = %lu\n", n_rows);
std::cout << "Tdcp: r = " << r.transpose() << std::endl;
std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl;
std::cout << "Tdcp: A = \n" << A << std::endl;
@@ -374,15 +377,18 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
return output;
}
- // residual_ci
+ // residual
if (tdcp_params.residual_opt == 0) // all problem
- residual_ci = chisq2ci((r + A * delta_d).squaredNorm() / var_tdcp, A.rows());
+ residual = (r + A * delta_d).squaredNorm() / var_tdcp;
else if (tdcp_params.residual_opt == 1) // worst sat
- residual_ci = chisq2ci((r + A * delta_d).cwiseAbs2().maxCoeff() / var_tdcp, 1);
+ residual = (r + A * delta_d).cwiseAbs2().maxCoeff() / var_tdcp;
else
throw std::runtime_error("unknown value for residual_opt");
// residual = (r + A * delta_d).norm();
+ // residual_ci
+ residual_ci = chisq2ci(residual, tdcp_params.residual_opt == 0 ? n_rows : 1);
+
#if GNSS_UTILS_TDCP_DEBUG == 1
if (j == 0)
{
@@ -391,8 +397,9 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl;
printf("Ref distance = %7.3f m\n", d_0.norm());
printf("Computed distance = %7.3f m\n", d.head<3>().norm());
- printf("Tdcp: residual_ci = %13.10f\n", residual_ci);
- printf("Tdcp: rows = %lu\n", r.rows());
+ printf("Tdcp: residual = %13.10f\n", residual);
+ printf("Tdcp: residual_ci = %13.10f\n", residual_ci);
+ printf("Tdcp: rows = %lu\n", n_rows);
std::cout << "Tdcp: r = " << r.transpose() << std::endl;
std::cout << "Tdcp: r+A*delta_d = " << (r + A * delta_d).transpose() << std::endl;
std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl;
@@ -402,15 +409,15 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
printf("Tdcp: dT = %8.3f secs\n", d(3));
std::cout << "Check RAIM conditions...\n";
std::cout << "\titeration == 0: " << (j==0 ? "OK\n":"FAILED\n");
- std::cout << "\tA.rows() > tdcp_params.min_common_sats: " << (A.rows() > tdcp_params.min_common_sats ? "OK\n":"FAILED\n");
+ std::cout << "\tn_rows > tdcp_params.min_common_sats: " << (n_rows > tdcp_params.min_common_sats ? "OK\n":"FAILED\n");
std::cout << "\ttdcp_params.raim_n > 0: " << (tdcp_params.raim_n > 0 ? "OK\n":"FAILED\n");
- std::cout << "\tresidual_ci > tdcp_params.max_residual_ci: " << (residual_ci > tdcp_params.max_residual_ci ? "OK\n":"FAILED\n");
+ std::cout << "\ttdcp_params.max_residual_ci: " << tdcp_params.max_residual_ci;
}
#endif
// RAIM ====================================== (after first iteration)
if (j == 0 and
- A.rows() > tdcp_params.min_common_sats and
+ n_rows > tdcp_params.min_common_sats and
tdcp_params.raim_n > 0 and
residual_ci > tdcp_params.max_residual_ci)
{
@@ -419,22 +426,22 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
#endif
int worst_sat_row = -1;
- double best_residual_ci = 1e12;
+ double best_residual = 1e12;
Eigen::Vector4d best_d;
- // remove up to 'raim_n' observations (if enough satellites and residual_ci condition holds)
+ // remove up to 'raim_n' observations (if enough satellites and residual condition holds)
while (raim_discarded_rows.size() < tdcp_params.raim_n and
- A.rows() - raim_discarded_rows.size() > tdcp_params.min_common_sats and
+ n_rows - raim_discarded_rows.size() > tdcp_params.min_common_sats and
residual_ci > tdcp_params.max_residual_ci)
{
auto A_raim = A;
auto r_raim = r;
- // METHOD A: using normalized RMSE residual_ci
+ // METHOD A: using normalized RMSE residual
if (tdcp_params.residual_opt == 0)
{
// solve removing each row
- for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
+ for (int row_removed = 0; row_removed < n_rows; row_removed++)
{
// skip already discarded rows
if (raim_discarded_rows.count(row_removed) != 0)
@@ -451,30 +458,34 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
// no NaN
if (!d_raim.array().isNaN().any())
{
- // residual_ci
+ // residual
if (tdcp_params.residual_opt == 0) // all problem
- residual_ci = chisq2ci((r_raim + A_raim * delta_d_raim).squaredNorm() / var_tdcp, A_raim.rows() - raim_discarded_rows.size());
+ residual = (r_raim + A_raim * delta_d_raim).squaredNorm() / var_tdcp;
else if (tdcp_params.residual_opt == 1) // worst sat
- residual_ci = chisq2ci((r_raim + A_raim * delta_d_raim).cwiseAbs2().maxCoeff() / var_tdcp, 1);
+ 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 * delta_d_raim).norm() / (A_raim.rows() - 1);
+ // residual = (r_raim + A_raim * delta_d_raim).norm() / (n_rows - 1);
+
+ // residual_ci
+ residual_ci = chisq2ci(residual, tdcp_params.residual_opt == 0 ? n_rows - raim_discarded_rows.size() : 1);
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "RAIM excluding row " << row_removed;// << std::endl;
//std::cout << "\tDisplacement vector =" << d_raim.head<3>().transpose() << std::endl;
- printf("\tresidual_ci = %13.10f\n", residual_ci);
+ printf("\tresidual = %13.10f\n", residual);
+ printf("\tresidual_ci = %13.10f\n", residual_ci);
//std::cout << "Tdcp: drho = " << r_raim.transpose() << std::endl;
//std::cout << "Tdcp: A = \n" << A_raim << std::endl;
//std::cout << "Tdcp: H = \n" << A_raim.transpose() * A_raim << std::endl;
//printf("Tdcp: dT = %8.3f secs\n", d_raim(3));
#endif
- // store if best residual_ci
- if (residual_ci < best_residual_ci)
+ // store if best residual
+ if (residual < best_residual)
{
worst_sat_row = row_removed;
- best_residual_ci = residual_ci;
+ best_residual = residual;
best_d = d_raim;
}
}
@@ -483,10 +494,10 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
r_raim(row_removed) = r(row_removed);
}
}
- // METHOD B: using max residual_ci in Mahalanobis distance
+ // METHOD B: using max residual in Mahalanobis distance
else if (tdcp_params.residual_opt == 1)
{
- // find index of max residual_ci
+ // find index of max residual
(r + A * delta_d).cwiseAbs2().maxCoeff(&worst_sat_row);
// remove row
@@ -500,18 +511,22 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
// no NaN
if (!d_raim.array().isNaN().any())
{
- // residual_ci
+ // residual
if (tdcp_params.residual_opt == 0) // all problem
- residual_ci = chisq2ci((r_raim + A_raim * delta_d_raim).squaredNorm() / var_tdcp, A_raim.rows() - raim_discarded_rows.size());
+ residual = (r_raim + A_raim * delta_d_raim).squaredNorm() / var_tdcp;
else if (tdcp_params.residual_opt == 1) // worst sat
- residual_ci = chisq2ci((r_raim + A_raim * delta_d_raim).cwiseAbs2().maxCoeff() / var_tdcp, 1);
+ 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 * delta_d_raim).norm() / (A_raim.rows() - 1);
+ // residual = (r_raim + A_raim * delta_d_raim).norm() / (n_rows - 1);
+
+ // residual_ci
+ residual_ci = chisq2ci(residual, tdcp_params.residual_opt == 0 ? n_rows - raim_discarded_rows.size() : 1);
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "RAIM excluding row " << worst_sat_row;// << std::endl;
//std::cout << "\tDisplacement vector =" << d_raim.head<3>().transpose() << std::endl;
+ printf("\tresidual = %13.10f\n", residual);
printf("\tresidual_ci = %13.10f\n", residual_ci);
//std::cout << "Tdcp: drho = " << r_raim.transpose() << std::endl;
//std::cout << "Tdcp: A = \n" << A_raim << std::endl;
@@ -519,8 +534,8 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
//printf("Tdcp: dT = %8.3f secs\n", d_raim(3));
#endif
- // store as best residual_ci
- best_residual_ci = residual_ci;
+ // store as best residual
+ best_residual = residual;
best_d = d_raim;
}
}
@@ -544,9 +559,9 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
r(worst_sat_row) = 0;
// Choose the best RAIM solution
- d = best_d;
- cov_d = (A.transpose() * A).inverse();
- residual_ci = best_residual_ci;
+ d = best_d;
+ cov_d = (A.transpose() * A).inverse();
+ residual= best_residual;
}
#if GNSS_UTILS_TDCP_DEBUG == 1
@@ -558,7 +573,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
std::cout << "\tClock offset = " << d(3) << std::endl;
- std::cout << "\tResidual = " << best_residual_ci << std::endl;
+ std::cout << "\tResidual = " << best_residual << std::endl;
std::cout << "\tA = \n" << A << std::endl;
std::cout << "\tH = \n" << A.transpose() * A << std::endl;
std::cout << "\tcov_d = \n" << cov_d << std::endl;
@@ -574,7 +589,8 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
std::cout << "\tClock offset = " << d(3) << std::endl;
- std::cout << "\tResidual = " << residual_ci << std::endl;
+ std::cout << "\tResidual = " << residual << std::endl;
+ std::cout << "\tResidual_ci = " << residual_ci << std::endl;
std::cout << "\tA = \n" << A << std::endl;
std::cout << "\tH = \n" << A.transpose() * A << std::endl;
std::cout << "\tcov_d = \n" << cov_d << std::endl;
@@ -586,20 +602,27 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
for (int row = 0; row < r.size(); row++)
if (raim_discarded_rows.count(row) == 0)
r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
- // residual_ci
+ // residual
if (tdcp_params.residual_opt == 0) // all problem
- residual_ci = chisq2ci(r.squaredNorm() / var_tdcp, r.rows() - raim_discarded_rows.size());
+ residual = r.squaredNorm() / var_tdcp;
else if (tdcp_params.residual_opt == 1) // worst sat
- residual_ci = chisq2ci(r.cwiseAbs2().maxCoeff() / var_tdcp, 1);
+ residual = r.cwiseAbs2().maxCoeff() / var_tdcp;
else
throw std::runtime_error("unknown value for residual_opt");
//residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size()));
- // check residual_ci condition
- if (tdcp_params.validate_residual and residual_ci > tdcp_params.max_residual_ci)
+ // residual_ci
+ residual_ci = chisq2ci(residual, tdcp_params.residual_opt == 0 ? n_rows - raim_discarded_rows.size() : 1);
+
+ // check residual condition
+ if (tdcp_params.validate_residual and
+ residual_ci > tdcp_params.max_residual_ci)
{
- printf("Tdcp: Didn't success. Final residual_ci=%f bigger than max_residual_ci=%f.\n", residual_ci, tdcp_params.max_residual_ci);
- output.msg = "Residual bigger than max_residual_ci";
+ printf("Tdcp: Didn't success. Final residual=%f (CI %f) bigger than max_residual_ci=%f.\n",
+ residual,
+ residual_ci,
+ tdcp_params.max_residual_ci);
+ output.msg = "residual_ci bigger than max_residual_ci";
output.success = false;
}
else
--
GitLab