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

2 raim methods implemented and gtest working

parent f99b2689
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
Hide whitespace changes
Inline Side-by-side
include/gnss_utils/tdcp.h
+ 1
− 1
View file @ 9fb7e462
...@@ -15,7 +15,7 @@ struct TdcpBatchParams ...@@ -15,7 +15,7 @@ struct TdcpBatchParams
double max_residual; // max allowed residual to be considered good solution. RAIM applied if enabled in this case. double max_residual; // max allowed residual to be considered good solution. RAIM applied if enabled in this case.
bool relinearize_jacobian; bool relinearize_jacobian;
int max_iterations; int max_iterations;
int residual_opt; // 0: RMS of residual vector. 1: Max residual in Mahalanobis squared distance int residual_opt; // 0: Normalized RMS of residual vector. 1: Max residual in Mahalanobis squared distance
}; };
struct TdcpOutput struct TdcpOutput
......
src/tdcp.cpp
+ 125
− 70
View file @ 9fb7e462
...@@ -145,8 +145,8 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, ...@@ -145,8 +145,8 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
if (n_common_sats < std::max(tdcp_params.min_common_sats, 4)) if (n_common_sats < std::max(tdcp_params.min_common_sats, 4))
{ {
#if GNSS_UTILS_TDCP_DEBUG == 1 #if GNSS_UTILS_TDCP_DEBUG == 1
printf("Tdcp: NOT ENOUGH COMMON SATS"); printf("Tdcp: NOT ENOUGH COMMON SATS\n");
printf("Tdcp: %i common sats available, %i sats are REQUIRED. [ SKIPPING Tdcp ]", n_common_sats, std::max(tdcp_params.min_common_sats, 4)); printf("Tdcp: %i common sats available, %i sats are REQUIRED. [ SKIPPING Tdcp ]\n", n_common_sats, std::max(tdcp_params.min_common_sats, 4));
#endif #endif
output.msg = "Not enough common sats"; output.msg = "Not enough common sats";
output.success = false; output.success = false;
...@@ -202,7 +202,7 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, ...@@ -202,7 +202,7 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
#if GNSS_UTILS_TDCP_DEBUG == 1 #if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl; std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl;
std::cout << "d initial guess: " << d.transpose() << std::endl; std::cout << "d initial guess: " << d_0.transpose() << std::endl;
std::cout << "common sats: "; std::cout << "common sats: ";
for (auto row_sat_freq_it : row_2_sat_freq) for (auto row_sat_freq_it : row_2_sat_freq)
std::cout << row_sat_freq_it.second.first << " "; std::cout << row_sat_freq_it.second.first << " ";
...@@ -368,63 +368,130 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -368,63 +368,130 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
std::cout << "Tdcp: cov_d = \n" << cov_d << std::endl; std::cout << "Tdcp: cov_d = \n" << cov_d << std::endl;
printf("Tdcp: dT = %8.3f secs\n", d(3)); printf("Tdcp: dT = %8.3f secs\n", d(3));
#endif #endif
output.msg = "NaN values in NLS"; output.msg = "Na"
"N values in NLS";
output.success = false; output.success = false;
return output; return output;
} }
// residual // residual
if (tdcp_params.residual_opt == 0) // RMSE if (tdcp_params.residual_opt == 0) // RMSE normalized
residual = sqrt((r - A * delta_d).squaredNorm() / A.rows()) / sqrt(var_tdcp); residual = sqrt((r + A * delta_d).squaredNorm() / A.rows()) / sqrt(var_tdcp);
else if (tdcp_params.residual_opt == 1) // max error in squared Mahalanobis distance (using measure noise) else if (tdcp_params.residual_opt == 1) // max error in squared Mahalanobis distance (using measure noise)
residual = (r - A * delta_d).cwiseAbs2().maxCoeff() / var_tdcp; 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();
#if GNSS_UTILS_TDCP_DEBUG == 1 #if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Displacement vector =" << d.head<3>().transpose() << std::endl; if (j == 0)
std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl; {
printf("Ref distance = %7.3f m\n", d_0.norm()); std::cout << "\n//////////////// Initial solution at first iteration ////////////////\n";
printf("Computed distance = %7.3f m\n", d.head<3>().norm()); std::cout << "Displacement vector =" << d.head<3>().transpose() << std::endl;
printf("Tdcp: residual = %13.10f\n", residual); std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl;
printf("Tdcp: rows = %lu\n", r.rows()); printf("Ref distance = %7.3f m\n", d_0.norm());
std::cout << "Tdcp: r = " << r.transpose() << std::endl; printf("Computed distance = %7.3f m\n", d.head<3>().norm());
std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl; printf("Tdcp: residual = %13.10f\n", residual);
std::cout << "Tdcp: A = \n" << A << std::endl; printf("Tdcp: rows = %lu\n", r.rows());
std::cout << "Tdcp: H = \n" << A.transpose() * A << std::endl; std::cout << "Tdcp: r = " << r.transpose() << std::endl;
std::cout << "Tdcp: cov_d = \n" << cov_d << std::endl; std::cout << "Tdcp: r+A*delta_d = " << (r + A * delta_d).transpose() << std::endl;
printf("Tdcp: dT = %8.3f secs\n", d(3)); std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl;
std::cout << "Tdcp: A = \n" << A << std::endl;
std::cout << "Tdcp: H = \n" << A.transpose() * A << std::endl;
std::cout << "Tdcp: cov_d = \n" << cov_d << std::endl;
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 << "\ttdcp_params.raim_n > 0: " << (tdcp_params.raim_n > 0 ? "OK\n":"FAILED\n");
std::cout << "\tresidual > tdcp_params.max_residual: " << (residual > tdcp_params.max_residual ? "OK\n":"FAILED\n");
}
#endif #endif
// RAIM ====================================== (at first iteration) // RAIM ====================================== (after first iteration)
if (j == 0 and if (j == 0 and
A.cols() > tdcp_params.min_common_sats and A.rows() > tdcp_params.min_common_sats and
tdcp_params.raim_n > 0 and tdcp_params.raim_n > 0 and
residual > tdcp_params.max_residual) residual > tdcp_params.max_residual)
{ {
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "//////////////// Performing RAIM ////////////////\n";
#endif
int worst_sat_row = -1; int worst_sat_row = -1;
double best_residual = 1e12; double best_residual = 1e12;
Eigen::Vector4d best_d; Eigen::Vector4d best_d;
// remove up to 'raim_n' observations (if enough satellites and residual 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.cols() - raim_discarded_rows.size() > tdcp_params.min_common_sats and A.rows() - raim_discarded_rows.size() > tdcp_params.min_common_sats and
residual > tdcp_params.max_residual) residual > tdcp_params.max_residual)
{ {
auto A_raim = A; auto A_raim = A;
auto r_raim = r; auto r_raim = r;
// solve removing each row // METHOD A: using normalized RMSE residual
for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++) if (tdcp_params.residual_opt == 0)
{
// solve removing each row
for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
{
// skip already discarded rows
if (raim_discarded_rows.count(row_removed) != 0)
continue;
// remove row
A_raim.row(row_removed) = Eigen::Vector4d::Zero().transpose();
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;
// 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);
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;
else
throw std::runtime_error("unknown value for residual_opt");
// 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;
//std::cout << "\tDisplacement vector =" << d_raim.head<3>().transpose() << std::endl;
printf("\tresidual = %13.10f\n", residual);
//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
if (residual < best_residual)
{
worst_sat_row = row_removed;
best_residual = residual;
best_d = d_raim;
}
}
// restore initial A and r
A_raim.row(row_removed) = A.row(row_removed);
r_raim(row_removed) = r(row_removed);
}
}
// METHOD B: using max residual in Mahalanobis distance
else
{ {
// skip already discarded rows // find index of max residual
if (raim_discarded_rows.count(row_removed) != 0) (r + A * delta_d).cwiseAbs2().maxCoeff(&worst_sat_row);
continue;
// remove row // remove row
A_raim.row(row_removed) = Eigen::Vector4d::Zero().transpose(); A_raim.row(worst_sat_row) = Eigen::Vector4d::Zero().transpose();
r_raim(row_removed) = 0; // not necessary r_raim(worst_sat_row) = 0;
// solve // solve
Eigen::Vector4d delta_d_raim = -(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;
...@@ -435,43 +502,33 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -435,43 +502,33 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
{ {
// residual // residual
if (tdcp_params.residual_opt == 0) // RMSE 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 * 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) 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 * 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() / (A_raim.rows() - 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 " << worst_sat_row;// << std::endl;
std::cout << "Displacement vector =" << d_raim.head<3>().transpose() << std::endl; //std::cout << "\tDisplacement vector =" << d_raim.head<3>().transpose() << std::endl;
std::cout << "Displacement update =" << delta_d_raim.head<3>().transpose() << std::endl; printf("\tresidual = %13.10f\n", residual);
printf("Ref distance = %7.3f m\n", d_0.norm()); //std::cout << "Tdcp: drho = " << r_raim.transpose() << std::endl;
printf("Computed distance = %7.3f m\n", d_raim.head<3>().norm()); //std::cout << "Tdcp: A = \n" << A_raim << std::endl;
printf("Tdcp: residual = %13.10f\n", residual); //std::cout << "Tdcp: H = \n" << A_raim.transpose() * A_raim << std::endl;
std::cout << "Tdcp: drho = " << r_raim.transpose() << std::endl; //printf("Tdcp: dT = %8.3f secs\n", d_raim(3));
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 #endif
// store if best residual // store as best residual
if (residual < best_residual) best_residual = residual;
{ best_d = d_raim;
worst_sat_row = row_removed;
best_residual = residual;
best_d = d_raim;
}
} }
// restore initial A and r
A_raim.row(row_removed) = A.row(row_removed);
r_raim(row_removed) = r(row_removed);
} }
// No successful RAIM solution // No successful RAIM solution
if (worst_sat_row == -1) if (worst_sat_row == -1)
{ {
printf("Tdcp: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!"); printf("Tdcp: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!\n");
output.msg = "NaN values in NLS after RAIM"; output.msg = "NaN values in NLS after RAIM";
output.success = false; output.success = false;
return output; return output;
...@@ -482,15 +539,16 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -482,15 +539,16 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
// remove row (set zero) // remove row (set zero)
A.row(worst_sat_row) = Eigen::Vector4d::Zero().transpose(); A.row(worst_sat_row) = Eigen::Vector4d::Zero().transpose();
r(worst_sat_row) = 0; // not necessary r(worst_sat_row) = 0;
// Choose de best RAIM solution // Choose de best RAIM solution
d = best_d; d = best_d;
cov_d = (A.transpose() * A).inverse(); cov_d = (A.transpose() * A).inverse();
residual = best_residual;
} }
#if GNSS_UTILS_TDCP_DEBUG == 1 #if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Tdcp After RAIM " << std::endl; std::cout << "//////////////// Best solution after RAIM ////////////////" << std::endl;
std::cout << "\tExcluded rows : "; std::cout << "\tExcluded rows : ";
for (auto excl_row : raim_discarded_rows) for (auto excl_row : raim_discarded_rows)
std::cout << excl_row << " "; std::cout << excl_row << " ";
...@@ -506,30 +564,27 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -506,30 +564,27 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
} }
#if GNSS_UTILS_TDCP_DEBUG == 1 #if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Tdcp iteration " << j << std::endl; std::cout << "\n//////////////// Final solution at iteration " << j << "////////////////\n";
std::cout << "\tExcluded rows : "; std::cout << "\tExcluded rows : ";
for (auto excl_row : raim_discarded_rows) for (auto excl_row : raim_discarded_rows)
std::cout << excl_row << " "; std::cout << excl_row << " ";
std::cout << std::endl; std::cout << std::endl;
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 << std::endl; std::cout << "\tResidual = " << 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;
#endif #endif
} }
// Computing error on measurement space // Computing error on measurement space
std::cout << "r (r-A*delta_d) = " << (r - A * (d-d_0)).transpose() << std::endl;
r.setZero(); r.setZero();
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);
std::cout << "r (reevaluated) = " << r.transpose() << std::endl;
// residual // residual
if (tdcp_params.residual_opt == 0) // RMSE if (tdcp_params.residual_opt == 0) // RMSE
residual = sqrt(r.squaredNorm() / (r.rows() - raim_discarded_rows.size())) / sqrt(var_tdcp); residual = sqrt(r.squaredNorm() / (r.rows() - raim_discarded_rows.size())) / sqrt(var_tdcp);
...@@ -542,7 +597,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -542,7 +597,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
// check residual condition // check residual condition
if (residual > tdcp_params.max_residual) if (residual > tdcp_params.max_residual)
{ {
printf("Tdcp: Didn't success. Final residual bigger than max_residual."); printf("Tdcp: Didn't success. Final residual=%f bigger than max_residual=%f.\n", residual, tdcp_params.max_residual);
output.msg = "Residual bigger than max_residual"; output.msg = "Residual bigger than max_residual";
output.success = false; output.success = false;
} }
......
test/gtest_tdcp.cpp
+ 223
− 46
View file @ 9fb7e462
...@@ -7,6 +7,11 @@ ...@@ -7,6 +7,11 @@
using namespace GnssUtils; using namespace GnssUtils;
using namespace Eigen; using namespace Eigen;
int N_sats = 20;
int N_tests = 100;
double distortion1 = -30;
double distortion2 = -20;
Vector3d computeRandomReceiverLatLonAlt() Vector3d computeRandomReceiverLatLonAlt()
{ {
Vector3d receiver_LLA = Vector3d::Random(); // in [-1, 1] Vector3d receiver_LLA = Vector3d::Random(); // in [-1, 1]
...@@ -18,11 +23,12 @@ Vector3d computeRandomReceiverLatLonAlt() ...@@ -18,11 +23,12 @@ Vector3d computeRandomReceiverLatLonAlt()
return receiver_LLA; return receiver_LLA;
} }
void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt, void computeVisibleSatellite(const Vector3d& receiver_latlonalt,
Vector3d& sat_ENU, const int& n_sat,
Vector3d& sat_ECEF, Vector3d& sat_ENU,
Vector2d& sat_azel, Vector3d& sat_ECEF,
double& range) Vector2d& sat_azel,
double& range)
{ {
Vector3d t_ECEF_ENU, t_ENU_ECEF; Vector3d t_ECEF_ENU, t_ENU_ECEF;
Matrix3d R_ENU_ECEF; Matrix3d R_ENU_ECEF;
...@@ -31,10 +37,21 @@ void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt, ...@@ -31,10 +37,21 @@ void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt,
computeEnuEcefFromLatLonAlt(receiver_latlonalt, R_ENU_ECEF, t_ENU_ECEF); computeEnuEcefFromLatLonAlt(receiver_latlonalt, R_ENU_ECEF, t_ENU_ECEF);
// equidistributed azimuth at elevation 45º
if (n_sat < 6)
{
sat_azel(0) = -M_PI + n_sat * 2*M_PI/6 + 0.1; // in [-pi, pi]
sat_azel(1) = M_PI / 4; // in [0, pi/2]
}
// random elevation and azimuth // random elevation and azimuth
sat_azel = Vector2d::Random(); // in [-1, 1] else
sat_azel(0) *= M_PI; // in [-pi, pi] {
sat_azel(1) = (sat_azel(1)/2 + 0.5) * M_PI / 2; // in [0, pi/2] sat_azel = Vector2d::Random(); // in [-1, 1]
sat_azel(0) *= M_PI; // in [-pi, pi]
sat_azel(1) = (sat_azel(1)/2 + 0.5) * M_PI / 2; // in [0, pi/2]
}
// random range
range = VectorXd::Random(1)(0) * 5e2 + 1e3; // in [500, 1500] range = VectorXd::Random(1)(0) * 5e2 + 1e3; // in [500, 1500]
// ENU // ENU
...@@ -51,16 +68,16 @@ void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt, ...@@ -51,16 +68,16 @@ void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt,
TEST(Tdcp, Tdcp) TEST(Tdcp, Tdcp)
{ {
int N_sats = 20;
TdcpBatchParams tdcp_params; TdcpBatchParams tdcp_params;
tdcp_params.max_iterations = 10; tdcp_params.max_iterations = 5;
tdcp_params.min_common_sats = 6; tdcp_params.min_common_sats = 6;
tdcp_params.raim_n = 0; tdcp_params.raim_n = 0;
tdcp_params.max_residual = 0; tdcp_params.residual_opt = 0;
tdcp_params.max_residual = 1e20;
tdcp_params.relinearize_jacobian = true; tdcp_params.relinearize_jacobian = true;
tdcp_params.tdcp.multi_freq = false; tdcp_params.tdcp.multi_freq = false;
tdcp_params.residual_opt = 0; tdcp_params.tdcp.sigma_atm = 1;
tdcp_params.tdcp.sigma_carrier = 1;
Vector3d sat_ENU, sat_ECEF; Vector3d sat_ENU, sat_ECEF;
Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF; Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
...@@ -87,7 +104,7 @@ TEST(Tdcp, Tdcp) ...@@ -87,7 +104,7 @@ TEST(Tdcp, Tdcp)
snapshot_k->getObservations()->addObservation(obs_k); snapshot_k->getObservations()->addObservation(obs_k);
// Random receiver position // Random receiver position
for (auto i = 0; i<100; i++) for (auto i = 0; i<N_tests; i++)
{ {
// clear satellites and ranges // clear satellites and ranges
snapshot_r->getSatellites().clear(); snapshot_r->getSatellites().clear();
...@@ -109,7 +126,7 @@ TEST(Tdcp, Tdcp) ...@@ -109,7 +126,7 @@ TEST(Tdcp, Tdcp)
d_gt.head<3>() = d_ECEF; d_gt.head<3>() = d_ECEF;
d_gt(3) = (clock_k - clock_r) * CLIGHT; d_gt(3) = (clock_k - clock_r) * CLIGHT;
std::cout << "Iteration " << i << ":\n"; std::cout << "Test " << i << ":\n";
std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n"; std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n";
std::cout << "\tclock_r " << clock_r << ":\n"; std::cout << "\tclock_r " << clock_r << ":\n";
std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n"; std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n";
...@@ -124,8 +141,8 @@ TEST(Tdcp, Tdcp) ...@@ -124,8 +141,8 @@ TEST(Tdcp, Tdcp)
{ {
common_sats.insert(j); common_sats.insert(j);
// Satellite r (random) // Satellite r
computeRandomVisibleSatellite(x_r_LLA, sat_ENU, sat_ECEF, azel, range); computeVisibleSatellite(x_r_LLA, j, sat_ENU, sat_ECEF, azel, range);
ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3); ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3);
Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1}); Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
...@@ -143,12 +160,12 @@ TEST(Tdcp, Tdcp) ...@@ -143,12 +160,12 @@ TEST(Tdcp, Tdcp)
snapshot_r->getRanges().emplace(j, range_r); snapshot_r->getRanges().emplace(j, range_r);
std::cout << "\tsat: " << j << "\n"; //std::cout << "\tsat: " << j << "\n";
std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n"; //std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n";
std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n"; //std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n";
// Satellite k (random) // Satellite k (random)
computeRandomVisibleSatellite(x_k_LLA, sat_ENU, sat_ECEF, azel, range); computeVisibleSatellite(x_k_LLA, j, sat_ENU, sat_ECEF, azel, range);
ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3); ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3);
Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1}); Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
...@@ -166,8 +183,8 @@ TEST(Tdcp, Tdcp) ...@@ -166,8 +183,8 @@ TEST(Tdcp, Tdcp)
snapshot_k->getRanges().emplace(j, range_k); snapshot_k->getRanges().emplace(j, range_k);
std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n"; //std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n"; //std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
} }
// TDCP // TDCP
...@@ -179,25 +196,28 @@ TEST(Tdcp, Tdcp) ...@@ -179,25 +196,28 @@ TEST(Tdcp, Tdcp)
tdcp_params); tdcp_params);
// CHECKS // CHECKS
std::cout << "CHECKS iteration " << i << std::endl; std::cout << "CHECKS Test " << i << std::endl;
ASSERT_TRUE(tdcp_out.success); ASSERT_TRUE(tdcp_out.success);
ASSERT_LE(tdcp_out.residual, 1e-9); ASSERT_LE(tdcp_out.residual, 1e-3);
ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-6); ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3);
} }
} }
TEST(Tdcp, Tdcp_raim) TEST(Tdcp, Tdcp_raim_residual_rmse)
{ {
int N_sats = 20; int N_sats = 20;
TdcpBatchParams tdcp_params; TdcpBatchParams tdcp_params;
tdcp_params.max_iterations = 10; tdcp_params.max_iterations = 5;
tdcp_params.min_common_sats = 6; tdcp_params.min_common_sats = 6;
tdcp_params.raim_n = 2; tdcp_params.raim_n = 2;
tdcp_params.max_residual = 0;
tdcp_params.relinearize_jacobian = true; tdcp_params.relinearize_jacobian = true;
tdcp_params.residual_opt = 0; // Normalized RMSE
tdcp_params.max_residual = 0.1; // low threshold to detect outliers...
tdcp_params.tdcp.multi_freq = false; tdcp_params.tdcp.multi_freq = false;
tdcp_params.residual_opt = 0; tdcp_params.tdcp.sigma_atm = 1;
tdcp_params.tdcp.sigma_carrier = 1;
Vector3d sat_ENU, sat_ECEF; Vector3d sat_ENU, sat_ECEF;
Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF; Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
...@@ -224,7 +244,7 @@ TEST(Tdcp, Tdcp_raim) ...@@ -224,7 +244,7 @@ TEST(Tdcp, Tdcp_raim)
snapshot_k->getObservations()->addObservation(obs_k); snapshot_k->getObservations()->addObservation(obs_k);
// Random receiver position // Random receiver position
for (auto i = 0; i<100; i++) for (auto i = 0; i<N_tests; i++)
{ {
// clear satellites and ranges // clear satellites and ranges
snapshot_r->getSatellites().clear(); snapshot_r->getSatellites().clear();
...@@ -246,12 +266,17 @@ TEST(Tdcp, Tdcp_raim) ...@@ -246,12 +266,17 @@ TEST(Tdcp, Tdcp_raim)
d_gt.head<3>() = d_ECEF; d_gt.head<3>() = d_ECEF;
d_gt(3) = (clock_k - clock_r) * CLIGHT; d_gt(3) = (clock_k - clock_r) * CLIGHT;
std::cout << "Iteration " << i << ":\n"; // distorted sats
int wrong_sat1 = i % N_sats;
int wrong_sat2 = (i + N_sats / 2) % N_sats;
std::cout << "Test " << i << ":\n";
std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n"; std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n";
std::cout << "\tclock_r " << clock_r << ":\n"; std::cout << "\tclock_r " << clock_r << ":\n";
std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n"; std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n";
std::cout << "\tclock_k " << clock_k << ":\n"; std::cout << "\tclock_k " << clock_k << ":\n";
std::cout << "\td_gt " << d_gt.transpose() << ":\n"; std::cout << "\td_gt " << d_gt.transpose() << ":\n";
std::cout << "\tdistorted sats: " << wrong_sat1 << ", " << wrong_sat2 << std::endl;
std::set<int> common_sats; std::set<int> common_sats;
std::set<int> raim_discarded_sats; std::set<int> raim_discarded_sats;
...@@ -262,7 +287,7 @@ TEST(Tdcp, Tdcp_raim) ...@@ -262,7 +287,7 @@ TEST(Tdcp, Tdcp_raim)
common_sats.insert(j); common_sats.insert(j);
// Satellite r (random) // Satellite r (random)
computeRandomVisibleSatellite(x_r_LLA, sat_ENU, sat_ECEF, azel, range); computeVisibleSatellite(x_r_LLA, j, sat_ENU, sat_ECEF, azel, range);
ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3); ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3);
Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1}); Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
...@@ -280,12 +305,12 @@ TEST(Tdcp, Tdcp_raim) ...@@ -280,12 +305,12 @@ TEST(Tdcp, Tdcp_raim)
snapshot_r->getRanges().emplace(j, range_r); snapshot_r->getRanges().emplace(j, range_r);
std::cout << "\tsat: " << j << "\n"; //std::cout << "\tsat: " << j << "\n";
std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n"; //std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n";
std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n"; //std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n";
// Satellite k (random) // Satellite k (random)
computeRandomVisibleSatellite(x_k_LLA, sat_ENU, sat_ECEF, azel, range); computeVisibleSatellite(x_k_LLA, j, sat_ENU, sat_ECEF, azel, range);
ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3); ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3);
Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1}); Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
...@@ -303,18 +328,170 @@ TEST(Tdcp, Tdcp_raim) ...@@ -303,18 +328,170 @@ TEST(Tdcp, Tdcp_raim)
snapshot_k->getRanges().emplace(j, range_k); snapshot_k->getRanges().emplace(j, range_k);
std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n"; //std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n"; //std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
} }
// Distort 2 ranges -> to be detected by RAIM // Distort 2 ranges -> to be detected by RAIM
snapshot_k->getRanges().at(wrong_sat1).L_corrected += distortion1;
snapshot_k->getRanges().at(wrong_sat1).L += distortion1;
snapshot_r->getRanges().at(wrong_sat2).L_corrected += distortion2;
snapshot_r->getRanges().at(wrong_sat2).L += distortion2;
// TDCP
auto tdcp_out = Tdcp(snapshot_r,
snapshot_k,
x_r_ECEF,
common_sats,
Vector4d::Zero(),
tdcp_params);
// CHECKS
std::cout << "CHECKS iteration " << i << std::endl;
ASSERT_EQ(tdcp_out.raim_discarded_sats.size(), 2);
ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat1));
ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat2));
ASSERT_TRUE(tdcp_out.success);
ASSERT_LE(tdcp_out.residual, 1e-3);
ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3);
}
}
TEST(Tdcp, Tdcp_raim_residual_max_mah)
{
int N_sats = 20;
TdcpBatchParams tdcp_params;
tdcp_params.max_iterations = 5;
tdcp_params.min_common_sats = 6;
tdcp_params.raim_n = 2;
tdcp_params.relinearize_jacobian = true;
tdcp_params.residual_opt = 1; // Max residual in Mahalanobis distance
tdcp_params.max_residual = 3.84; // 95% of confidence
tdcp_params.tdcp.multi_freq = false;
tdcp_params.tdcp.sigma_atm = 1;
tdcp_params.tdcp.sigma_carrier = 1;
Vector3d sat_ENU, sat_ECEF;
Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
Matrix3d R_ENU_ECEF;
Vector2d azel, azel2;
Vector4d d, d_gt;
Matrix4d cov_d;
double range, residual;
// Snapshots
auto snapshot_r = std::make_shared<Snapshot>();
auto snapshot_k = std::make_shared<Snapshot>();
// Observations (just needed for the GPST)
snapshot_r->setObservations(std::make_shared<Observations>());
snapshot_k->setObservations(std::make_shared<Observations>());
obsd_t obs_r;
obs_r.time.sec = 0;
obs_r.time.time = 0;
snapshot_r->getObservations()->addObservation(obs_r);
obsd_t obs_k;
obs_k.time.sec = 0;
obs_k.time.time = 1;
snapshot_k->getObservations()->addObservation(obs_k);
// Random receiver position
for (auto i = 0; i<N_tests; i++)
{
// clear satellites and ranges
snapshot_r->getSatellites().clear();
snapshot_k->getSatellites().clear();
snapshot_r->getRanges().clear();
snapshot_k->getRanges().clear();
// random setup
x_r_LLA = computeRandomReceiverLatLonAlt();
x_r_ECEF = latLonAltToEcef(x_r_LLA);
d_ECEF = Vector3d::Random() * 10;
x_k_ECEF = x_r_ECEF + d_ECEF;
x_k_LLA = ecefToLatLonAlt(x_k_ECEF);
double clock_r = Vector2d::Random()(0) * 1e-6;
double clock_k = Vector2d::Random()(0) * 1e-6;
// groundtruth
d_gt.head<3>() = d_ECEF;
d_gt(3) = (clock_k - clock_r) * CLIGHT;
// distorted sats
int wrong_sat1 = i % N_sats; int wrong_sat1 = i % N_sats;
snapshot_k->getRanges().at(wrong_sat1).L_corrected += 20;
snapshot_k->getRanges().at(wrong_sat1).L += 20;
int wrong_sat2 = (i + N_sats / 2) % N_sats; int wrong_sat2 = (i + N_sats / 2) % N_sats;
snapshot_r->getRanges().at(wrong_sat2).L_corrected -= 10;
snapshot_r->getRanges().at(wrong_sat2).L -= 10;
std::cout << "\nTest " << i << ":\n";
std::cout << std::setprecision(10) << "\tx_r_ECEF " << x_r_ECEF.transpose() << ":\n";
std::cout << "\tclock_r " << clock_r << ":\n";
std::cout << std::setprecision(10) << "\tx_k_ECEF " << x_k_ECEF.transpose() << ":\n";
std::cout << "\tclock_k " << clock_k << ":\n";
std::cout << "\td_gt " << d_gt.transpose() << ":\n";
std::cout << "\tdistorted sats: " << wrong_sat1 << ", " << wrong_sat2 << std::endl;
std::set<int> common_sats;
std::set<int> raim_discarded_sats;
// random visible satellites
for (auto j = 0; j<N_sats; j++)
{
common_sats.insert(j);
// Satellite r (random)
computeVisibleSatellite(x_r_LLA, j, sat_ENU, sat_ECEF, azel, range);
ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-3);
Satellite sat_r({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
snapshot_r->getSatellites().emplace(j, sat_r);
// Range r
Range range_r;
range_r.sat = j;
range_r.L = range + CLIGHT * clock_r;
range_r.L_corrected = range_r.L;
range_r.L_var = 1;
range_r.L2 = range_r.L;
range_r.L2_corrected = range_r.L;
range_r.L2_var = range_r.L_var;
snapshot_r->getRanges().emplace(j, range_r);
//std::cout << "\tsat: " << j << "\n";
//std::cout << "\t\tsat_r_ECEF " << sat_ECEF.transpose() << ":\n";
//std::cout << "\t\trange_r.L_corrected " << range_r.L_corrected << ":\n";
// Satellite k (random)
computeVisibleSatellite(x_k_LLA, j, sat_ENU, sat_ECEF, azel, range);
ASSERT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-3);
Satellite sat_k({0,j,sat_ECEF, Vector3d::Zero(), 1.0, 0, 0, 1});
snapshot_k->getSatellites().emplace(j, sat_k);
// Range k
Range range_k;
range_k.sat = j;
range_k.L = range + CLIGHT * clock_k;
range_k.L_corrected = range_k.L;
range_k.L_var = 1;
range_k.L2 = range_k.L;
range_k.L2_corrected = range_k.L;
range_k.L2_var = range_k.L_var;
snapshot_k->getRanges().emplace(j, range_k);
//std::cout << "\t\tsat_k_ECEF " << sat_ECEF.transpose() << ":\n";
//std::cout << "\t\trange_k.L_corrected " << range_k.L_corrected << ":\n";
}
// Distort 2 ranges -> to be detected by RAIM
snapshot_k->getRanges().at(wrong_sat1).L_corrected += distortion1;
snapshot_k->getRanges().at(wrong_sat1).L += distortion1;
snapshot_r->getRanges().at(wrong_sat2).L_corrected += distortion2;
snapshot_r->getRanges().at(wrong_sat2).L += distortion2;
// TDCP // TDCP
auto tdcp_out = Tdcp(snapshot_r, auto tdcp_out = Tdcp(snapshot_r,
...@@ -325,13 +502,13 @@ TEST(Tdcp, Tdcp_raim) ...@@ -325,13 +502,13 @@ TEST(Tdcp, Tdcp_raim)
tdcp_params); tdcp_params);
// CHECKS // CHECKS
std::cout << "CHECKS iteration " << i << std::endl; std::cout << "Checks iteration " << i << std::endl;
ASSERT_EQ(tdcp_out.raim_discarded_sats.size(), 2); ASSERT_EQ(tdcp_out.raim_discarded_sats.size(), 2);
ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat1)); ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat1));
ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat2)); ASSERT_TRUE(tdcp_out.raim_discarded_sats.count(wrong_sat2));
ASSERT_TRUE(tdcp_out.success); ASSERT_TRUE(tdcp_out.success);
ASSERT_LE(tdcp_out.residual, 1e-9); ASSERT_LE(tdcp_out.residual, 1e-3);
ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-6); ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-3);
} }
} }
......
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