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

parameters modified in tdcp batch

parent ca9f24c3
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
+ 4
− 2
Edit View file @ f99b2689
...@@ -12,9 +12,10 @@ struct TdcpBatchParams ...@@ -12,9 +12,10 @@ struct TdcpBatchParams
TdcpOptions tdcp; TdcpOptions tdcp;
int min_common_sats; int min_common_sats;
int raim_n; int raim_n;
double raim_min_residual; 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
}; };
struct TdcpOutput struct TdcpOutput
...@@ -66,7 +67,8 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -66,7 +67,8 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
Eigen::VectorXd& drho_m, Eigen::VectorXd& drho_m,
Eigen::MatrixXd& s_k, Eigen::MatrixXd& s_k,
Eigen::MatrixXd& s_r, Eigen::MatrixXd& s_r,
const Eigen::Vector4d& d_0, const Eigen::Vector4d& d_0,
const double& var_tdcp,
std::set<int>& raim_discarded_rows, std::set<int>& raim_discarded_rows,
const TdcpBatchParams& tdcp_params); const TdcpBatchParams& tdcp_params);
......
src/tdcp.cpp
+ 57
− 49
Edit View file @ f99b2689
...@@ -142,12 +142,11 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, ...@@ -142,12 +142,11 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
} }
int n_common_sats = common_sats.size(); int n_common_sats = common_sats.size();
int required_n_sats(std::max(tdcp_params.min_common_sats + tdcp_params.raim_n, 4 + tdcp_params.raim_n)); if (n_common_sats < std::max(tdcp_params.min_common_sats, 4))
if (n_common_sats < required_n_sats)
{ {
#if GNSS_UTILS_TDCP_DEBUG == 1 #if GNSS_UTILS_TDCP_DEBUG == 1
printf("Tdcp: NOT ENOUGH COMMON SATS"); printf("Tdcp: NOT ENOUGH COMMON SATS");
printf("Tdcp: %i common sats available, %i sats are REQUIRED. [ SKIPPING Tdcp ]", n_common_sats, required_n_sats); printf("Tdcp: %i common sats available, %i sats are REQUIRED. [ SKIPPING Tdcp ]", 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;
...@@ -277,10 +276,13 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, ...@@ -277,10 +276,13 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
A(row, 3) = -1.0; A(row, 3) = -1.0;
} }
} }
// Compute TDCP measurement noise variance (proportional to time)
double var_tdcp = (tk - tr) * tdcp_params.tdcp.sigma_atm * tdcp_params.tdcp.sigma_atm +
2 * tdcp_params.tdcp.sigma_carrier * tdcp_params.tdcp.sigma_carrier;
// LEAST SQUARES SOLUTION ======================================================================= // LEAST SQUARES SOLUTION =======================================================================
std::set<int> raim_discarded_rows; std::set<int> raim_discarded_rows;
output = Tdcp(x_r, A, r, drho_m, s_k, s_r, d_0, raim_discarded_rows, tdcp_params); output = Tdcp(x_r, A, r, drho_m, s_k, s_r, d_0, var_tdcp, raim_discarded_rows, tdcp_params);
// FILL OUTPUT // FILL OUTPUT
output.used_sats = common_sats; output.used_sats = common_sats;
...@@ -291,11 +293,6 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r, ...@@ -291,11 +293,6 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
} }
output.dt = tk - tr; output.dt = tk - tr;
// weight covariance with the measurement noise (proportional to time)
double sq_sigma_Tdcp = (tk - tr) * tdcp_params.tdcp.sigma_atm * tdcp_params.tdcp.sigma_atm +
2 * tdcp_params.tdcp.sigma_carrier * tdcp_params.tdcp.sigma_carrier;
output.cov_d *= sq_sigma_Tdcp;
return output; return output;
} }
...@@ -306,6 +303,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -306,6 +303,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
Eigen::MatrixXd& s_k, Eigen::MatrixXd& s_k,
Eigen::MatrixXd& s_r, Eigen::MatrixXd& s_r,
const Eigen::Vector4d& d_0, const Eigen::Vector4d& d_0,
const double& var_tdcp,
std::set<int>& raim_discarded_rows, std::set<int>& raim_discarded_rows,
const TdcpBatchParams& tdcp_params) const TdcpBatchParams& tdcp_params)
{ {
...@@ -376,10 +374,13 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -376,10 +374,13 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
} }
// residual // residual
// residual = sqrt((r - A * delta_d).squaredNorm() / A.rows()); if (tdcp_params.residual_opt == 0) // RMSE
residual = (r + A * delta_d).norm(); residual = sqrt((r - A * delta_d).squaredNorm() / A.rows()) / sqrt(var_tdcp);
// std::cout << "residual = " << residual << std::endl; else if (tdcp_params.residual_opt == 1) // max error in squared Mahalanobis distance (using measure noise)
// std::cout << "Tdcp2: r-A*delta_d = " << (r + A * delta_d).transpose() << std::endl; 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();
#if GNSS_UTILS_TDCP_DEBUG == 1 #if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Displacement vector =" << d.head<3>().transpose() << std::endl; std::cout << "Displacement vector =" << d.head<3>().transpose() << std::endl;
...@@ -396,27 +397,32 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -396,27 +397,32 @@ 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));
#endif #endif
// RAIM ====================================== (only at first iteration) RAIM WITH SATELLITES, not ROWS // RAIM ====================================== (at first iteration)
if (j == 0 and tdcp_params.raim_n > 0 and residual > tdcp_params.raim_min_residual) if (j == 0 and
A.cols() > tdcp_params.min_common_sats and
tdcp_params.raim_n > 0 and
residual > tdcp_params.max_residual)
{ {
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 some satellites // remove up to 'raim_n' observations (if enough satellites and residual condition holds)
while (raim_discarded_rows.size() < tdcp_params.raim_n) while (raim_discarded_rows.size() < tdcp_params.raim_n and
A.cols() - raim_discarded_rows.size() > tdcp_params.min_common_sats and
residual > tdcp_params.max_residual)
{ {
auto A_raim = A; auto A_raim = A;
auto r_raim = r; auto r_raim = r;
// solve removing each satellite // solve removing each row
for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++) for (int row_removed = 0; row_removed < A_raim.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)
continue; continue;
// remove satellite row // remove row
A_raim.row(row_removed) = Eigen::Vector4d::Zero().transpose(); A_raim.row(row_removed) = Eigen::Vector4d::Zero().transpose();
r_raim(row_removed) = 0; // not necessary r_raim(row_removed) = 0; // not necessary
...@@ -428,8 +434,13 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -428,8 +434,13 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
if (!d_raim.array().isNaN().any()) if (!d_raim.array().isNaN().any())
{ {
// residual // residual
residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1); if (tdcp_params.residual_opt == 0) // RMSE
// residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows()); 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 #if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "RAIM excluding row " << row_removed << std::endl; std::cout << "RAIM excluding row " << row_removed << std::endl;
...@@ -469,34 +480,11 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -469,34 +480,11 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
// store removed sat // store removed sat
raim_discarded_rows.insert(worst_sat_row); raim_discarded_rows.insert(worst_sat_row);
// remove row // 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; // not necessary
/*// Remove selected satellite from problem // Choose de best RAIM solution
std::cout << "resizing problem..." << std::endl;
auto A_aux = A;
auto r_aux = r;
auto drho_m_aux = drho_m;
auto s_r_aux = s_r;
auto s_k_aux = s_k;
A.conservativeResize(n_differences, Eigen::NoChange);
r.conservativeResize(n_differences);
drho_m.conservativeResize(n_differences);
s_r.conservativeResize(Eigen::NoChange, n_differences);
s_k.conservativeResize(Eigen::NoChange, n_differences);
int back_elements_changing = A_aux.rows() - worst_sat_row - n_removed_rows;
if (back_elements_changing != 0) // if last satelite removed, conservative resize did the job
{
A.bottomRows(back_elements_changing) = A_aux.bottomRows(back_elements_changing);
s_r.rightCols(back_elements_changing) = s_r_aux.rightCols(back_elements_changing);
s_k.rightCols(back_elements_changing) = s_k_aux.rightCols(back_elements_changing);
r.tail(back_elements_changing) = r_aux.tail(back_elements_changing);
drho_m.tail(back_elements_changing) = drho_m_aux.tail(back_elements_changing);
}*/
// apply the RAIM solution
d = best_d; d = best_d;
cov_d = (A.transpose() * A).inverse(); cov_d = (A.transpose() * A).inverse();
} }
...@@ -534,15 +522,35 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r, ...@@ -534,15 +522,35 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
} }
// 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);
// residual = (r - A * d).norm() / A.rows(); std::cout << "r (reevaluated) = " << r.transpose() << std::endl;
residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size()));
// residual
if (tdcp_params.residual_opt == 0) // RMSE
residual = sqrt(r.squaredNorm() / (r.rows() - raim_discarded_rows.size())) / sqrt(var_tdcp);
else if (tdcp_params.residual_opt == 1) // max error in squared Mahalanobis distance (using measure noise)
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 condition
if (residual > tdcp_params.max_residual)
{
printf("Tdcp: Didn't success. Final residual bigger than max_residual.");
output.msg = "Residual bigger than max_residual";
output.success = false;
}
else
output.success = true;
output.success = true; // covariance
cov_d *= var_tdcp;
return output; return output;
} }
......
test/gtest_tdcp.cpp
+ 4
− 2
Edit View file @ f99b2689
...@@ -57,9 +57,10 @@ TEST(Tdcp, Tdcp) ...@@ -57,9 +57,10 @@ TEST(Tdcp, Tdcp)
tdcp_params.max_iterations = 10; tdcp_params.max_iterations = 10;
tdcp_params.min_common_sats = 6; tdcp_params.min_common_sats = 6;
tdcp_params.raim_n = 0; tdcp_params.raim_n = 0;
tdcp_params.raim_min_residual = 0; tdcp_params.max_residual = 0;
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;
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;
...@@ -193,9 +194,10 @@ TEST(Tdcp, Tdcp_raim) ...@@ -193,9 +194,10 @@ TEST(Tdcp, Tdcp_raim)
tdcp_params.max_iterations = 10; tdcp_params.max_iterations = 10;
tdcp_params.min_common_sats = 6; tdcp_params.min_common_sats = 6;
tdcp_params.raim_n = 2; tdcp_params.raim_n = 2;
tdcp_params.raim_min_residual = 0; tdcp_params.max_residual = 0;
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;
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;
......
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