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
...@@ -18,6 +18,7 @@ struct TdcpOutput ...@@ -18,6 +18,7 @@ struct TdcpOutput
Eigen::Vector4d d = Eigen::Vector4d::Zero(); Eigen::Vector4d d = Eigen::Vector4d::Zero();
Eigen::Matrix4d cov_d = Eigen::Matrix4d::Zero(); Eigen::Matrix4d cov_d = Eigen::Matrix4d::Zero();
double dt = 0; double dt = 0;
double residual = 0;
double residual_ci = 0; double residual_ci = 0;
}; };
......
...@@ -288,8 +288,11 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -288,8 +288,11 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
TdcpOutput output; TdcpOutput output;
Eigen::Vector4d& d(output.d); Eigen::Vector4d& d(output.d);
Eigen::Matrix4d& cov_d(output.cov_d); Eigen::Matrix4d& cov_d(output.cov_d);
double& residual(output.residual);
double& residual_ci(output.residual_ci); double& residual_ci(output.residual_ci);
const int n_rows = A.rows();
// Initial guess // Initial guess
Eigen::Vector4d delta_d(Eigen::Vector4d::Zero()); Eigen::Vector4d delta_d(Eigen::Vector4d::Zero());
d = d_0; d = d_0;
...@@ -297,7 +300,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -297,7 +300,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
// initial sagnac correction // initial sagnac correction
Eigen::VectorXd prev_sagnac_corr(s_k.cols()); Eigen::VectorXd prev_sagnac_corr(s_k.cols());
if (tdcp_params.sagnac_correction == 2 ) 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_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; 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, ...@@ -308,7 +311,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
for (int j = 0; j < tdcp_params.max_iterations; j++) for (int j = 0; j < tdcp_params.max_iterations; j++)
{ {
// fill A and r // fill A and r
for (int row = 0; row < A.rows(); row++) for (int row = 0; row < n_rows; row++)
{ {
// skip discarded rows // skip discarded rows
if (raim_discarded_rows.count(row) != 0) if (raim_discarded_rows.count(row) != 0)
...@@ -360,7 +363,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -360,7 +363,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl; std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl;
printf("Ref distance = %7.3f m\n", d_0.norm()); printf("Ref distance = %7.3f m\n", d_0.norm());
printf("Computed distance = %7.3f m\n", d.head<3>().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: r = " << r.transpose() << std::endl;
std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl; std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl;
std::cout << "Tdcp: A = \n" << A << std::endl; std::cout << "Tdcp: A = \n" << A << std::endl;
...@@ -374,15 +377,18 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -374,15 +377,18 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
return output; return output;
} }
// residual_ci // residual
if (tdcp_params.residual_opt == 0) // all problem 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 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 else
throw std::runtime_error("unknown value for residual_opt"); throw std::runtime_error("unknown value for residual_opt");
// residual = (r + A * delta_d).norm(); // 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 GNSS_UTILS_TDCP_DEBUG == 1
if (j == 0) if (j == 0)
{ {
...@@ -391,8 +397,9 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -391,8 +397,9 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl; std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl;
printf("Ref distance = %7.3f m\n", d_0.norm()); printf("Ref distance = %7.3f m\n", d_0.norm());
printf("Computed distance = %7.3f m\n", d.head<3>().norm()); printf("Computed distance = %7.3f m\n", d.head<3>().norm());
printf("Tdcp: residual_ci = %13.10f\n", residual_ci); printf("Tdcp: residual = %13.10f\n", residual);
printf("Tdcp: rows = %lu\n", r.rows()); 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 = " << r.transpose() << std::endl;
std::cout << "Tdcp: r+A*delta_d = " << (r + A * delta_d).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; std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl;
...@@ -402,15 +409,15 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -402,15 +409,15 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
printf("Tdcp: dT = %8.3f secs\n", d(3)); printf("Tdcp: dT = %8.3f secs\n", d(3));
std::cout << "Check RAIM conditions...\n"; std::cout << "Check RAIM conditions...\n";
std::cout << "\titeration == 0: " << (j==0 ? "OK\n":"FAILED\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 << "\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 #endif
// RAIM ====================================== (after first iteration) // RAIM ====================================== (after first iteration)
if (j == 0 and 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 tdcp_params.raim_n > 0 and
residual_ci > tdcp_params.max_residual_ci) residual_ci > tdcp_params.max_residual_ci)
{ {
...@@ -419,22 +426,22 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -419,22 +426,22 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
#endif #endif
int worst_sat_row = -1; int worst_sat_row = -1;
double best_residual_ci = 1e12; double best_residual = 1e12;
Eigen::Vector4d best_d; 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 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) residual_ci > tdcp_params.max_residual_ci)
{ {
auto A_raim = A; auto A_raim = A;
auto r_raim = r; auto r_raim = r;
// METHOD A: using normalized RMSE residual_ci // METHOD A: using normalized RMSE residual
if (tdcp_params.residual_opt == 0) if (tdcp_params.residual_opt == 0)
{ {
// solve removing each row // 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 // skip already discarded rows
if (raim_discarded_rows.count(row_removed) != 0) if (raim_discarded_rows.count(row_removed) != 0)
...@@ -451,30 +458,34 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -451,30 +458,34 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
// no NaN // no NaN
if (!d_raim.array().isNaN().any()) if (!d_raim.array().isNaN().any())
{ {
// residual_ci // residual
if (tdcp_params.residual_opt == 0) // all problem 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 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 else
throw std::runtime_error("unknown value for residual_opt"); 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 #if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "RAIM excluding row " << row_removed;// << std::endl; std::cout << "RAIM excluding row " << row_removed;// << std::endl;
//std::cout << "\tDisplacement vector =" << d_raim.head<3>().transpose() << 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: drho = " << r_raim.transpose() << std::endl;
//std::cout << "Tdcp: A = \n" << A_raim << std::endl; //std::cout << "Tdcp: A = \n" << A_raim << std::endl;
//std::cout << "Tdcp: H = \n" << A_raim.transpose() * 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)); //printf("Tdcp: dT = %8.3f secs\n", d_raim(3));
#endif #endif
// store if best residual_ci // store if best residual
if (residual_ci < best_residual_ci) if (residual < best_residual)
{ {
worst_sat_row = row_removed; worst_sat_row = row_removed;
best_residual_ci = residual_ci; best_residual = residual;
best_d = d_raim; best_d = d_raim;
} }
} }
...@@ -483,10 +494,10 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -483,10 +494,10 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
r_raim(row_removed) = r(row_removed); 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) 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); (r + A * delta_d).cwiseAbs2().maxCoeff(&worst_sat_row);
// remove row // remove row
...@@ -500,18 +511,22 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -500,18 +511,22 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
// no NaN // no NaN
if (!d_raim.array().isNaN().any()) if (!d_raim.array().isNaN().any())
{ {
// residual_ci // residual
if (tdcp_params.residual_opt == 0) // all problem 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 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 else
throw std::runtime_error("unknown value for residual_opt"); 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 #if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "RAIM excluding row " << worst_sat_row;// << std::endl; std::cout << "RAIM excluding row " << worst_sat_row;// << std::endl;
//std::cout << "\tDisplacement vector =" << d_raim.head<3>().transpose() << 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); printf("\tresidual_ci = %13.10f\n", residual_ci);
//std::cout << "Tdcp: drho = " << r_raim.transpose() << std::endl; //std::cout << "Tdcp: drho = " << r_raim.transpose() << std::endl;
//std::cout << "Tdcp: A = \n" << A_raim << std::endl; //std::cout << "Tdcp: A = \n" << A_raim << std::endl;
...@@ -519,8 +534,8 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -519,8 +534,8 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
//printf("Tdcp: dT = %8.3f secs\n", d_raim(3)); //printf("Tdcp: dT = %8.3f secs\n", d_raim(3));
#endif #endif
// store as best residual_ci // store as best residual
best_residual_ci = residual_ci; best_residual = residual;
best_d = d_raim; best_d = d_raim;
} }
} }
...@@ -544,9 +559,9 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -544,9 +559,9 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
r(worst_sat_row) = 0; r(worst_sat_row) = 0;
// Choose the best RAIM solution // Choose the best RAIM solution
d = best_d; d = best_d;
cov_d = (A.transpose() * A).inverse(); cov_d = (A.transpose() * A).inverse();
residual_ci = best_residual_ci; residual= best_residual;
} }
#if GNSS_UTILS_TDCP_DEBUG == 1 #if GNSS_UTILS_TDCP_DEBUG == 1
...@@ -558,7 +573,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -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 << "\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 << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
std::cout << "\tClock offset = " << d(3) << 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 << "\tA = \n" << A << std::endl;
std::cout << "\tH = \n" << A.transpose() * A << std::endl; std::cout << "\tH = \n" << A.transpose() * A << std::endl;
std::cout << "\tcov_d = \n" << cov_d << std::endl; std::cout << "\tcov_d = \n" << cov_d << std::endl;
...@@ -574,7 +589,8 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -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 << "\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 << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
std::cout << "\tClock offset = " << d(3) << 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 << "\tA = \n" << A << std::endl;
std::cout << "\tH = \n" << A.transpose() * A << std::endl; std::cout << "\tH = \n" << A.transpose() * A << std::endl;
std::cout << "\tcov_d = \n" << cov_d << std::endl; std::cout << "\tcov_d = \n" << cov_d << std::endl;
...@@ -586,20 +602,27 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -586,20 +602,27 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
for (int row = 0; row < r.size(); row++) for (int row = 0; row < r.size(); row++)
if (raim_discarded_rows.count(row) == 0) 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); 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 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 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 else
throw std::runtime_error("unknown value for residual_opt"); throw std::runtime_error("unknown value for residual_opt");
//residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size())); //residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size()));
// check residual_ci condition // residual_ci
if (tdcp_params.validate_residual and residual_ci > tdcp_params.max_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); printf("Tdcp: Didn't success. Final residual=%f (CI %f) bigger than max_residual_ci=%f.\n",
output.msg = "Residual bigger than max_residual_ci"; residual,
residual_ci,
tdcp_params.max_residual_ci);
output.msg = "residual_ci bigger than max_residual_ci";
output.success = false; output.success = false;
} }
else else
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment