Skip to content
Snippets Groups Projects
Commit cf502f3f authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

residual and residual_ci

parent 30c544fd
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
This commit is part of merge request !20. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
include/gnss_utils/tdcp.h
+ 1
− 0
Edit View file @ cf502f3f
......@@ -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;
};
......
src/tdcp.cpp
+ 67
− 44
Edit View file @ cf502f3f
......@@ -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
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment