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
Hide whitespace changes
Inline Side-by-side
include/gnss_utils/tdcp.h
+ 4
− 2
View file @ f99b2689
......@@ -12,9 +12,10 @@ struct TdcpBatchParams
TdcpOptions tdcp;
int min_common_sats;
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;
int max_iterations;
int residual_opt; // 0: RMS of residual vector. 1: Max residual in Mahalanobis squared distance
};
struct TdcpOutput
......@@ -66,7 +67,8 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
Eigen::VectorXd& drho_m,
Eigen::MatrixXd& s_k,
Eigen::MatrixXd& s_r,
const Eigen::Vector4d& d_0,
const Eigen::Vector4d& d_0,
const double& var_tdcp,
std::set<int>& raim_discarded_rows,
const TdcpBatchParams& tdcp_params);
......
src/tdcp.cpp
+ 57
− 49
View file @ f99b2689
......@@ -142,12 +142,11 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
}
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 < required_n_sats)
if (n_common_sats < std::max(tdcp_params.min_common_sats, 4))
{
#if GNSS_UTILS_TDCP_DEBUG == 1
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
output.msg = "Not enough common sats";
output.success = false;
......@@ -277,10 +276,13 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
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 =======================================================================
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
output.used_sats = common_sats;
......@@ -291,11 +293,6 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
}
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;
}
......@@ -306,6 +303,7 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
Eigen::MatrixXd& s_k,
Eigen::MatrixXd& s_r,
const Eigen::Vector4d& d_0,
const double& var_tdcp,
std::set<int>& raim_discarded_rows,
const TdcpBatchParams& tdcp_params)
{
......@@ -376,10 +374,13 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
}
// residual
// residual = sqrt((r - A * delta_d).squaredNorm() / A.rows());
residual = (r + A * delta_d).norm();
// std::cout << "residual = " << residual << std::endl;
// std::cout << "Tdcp2: r-A*delta_d = " << (r + A * delta_d).transpose() << std::endl;
if (tdcp_params.residual_opt == 0) // RMSE
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)
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
std::cout << "Displacement vector =" << d.head<3>().transpose() << std::endl;
......@@ -396,27 +397,32 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
printf("Tdcp: dT = %8.3f secs\n", d(3));
#endif
// RAIM ====================================== (only at first iteration) RAIM WITH SATELLITES, not ROWS
if (j == 0 and tdcp_params.raim_n > 0 and residual > tdcp_params.raim_min_residual)
// RAIM ====================================== (at first iteration)
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;
double best_residual = 1e12;
Eigen::Vector4d best_d;
// remove some satellites
while (raim_discarded_rows.size() < tdcp_params.raim_n)
// remove up to 'raim_n' observations (if enough satellites and residual condition holds)
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 r_raim = r;
// solve removing each satellite
// 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 satellite row
// remove row
A_raim.row(row_removed) = Eigen::Vector4d::Zero().transpose();
r_raim(row_removed) = 0; // not necessary
......@@ -428,8 +434,13 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
if (!d_raim.array().isNaN().any())
{
// residual
residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows() - 1);
// residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows());
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;
......@@ -469,34 +480,11 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
// store removed sat
raim_discarded_rows.insert(worst_sat_row);
// remove row
// remove row (set zero)
A.row(worst_sat_row) = Eigen::Vector4d::Zero().transpose();
r(worst_sat_row) = 0; // not necessary
/*// Remove selected satellite from problem
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
// Choose de best RAIM solution
d = best_d;
cov_d = (A.transpose() * A).inverse();
}
......@@ -534,15 +522,35 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
}
// Computing error on measurement space
std::cout << "r (r-A*delta_d) = " << (r - A * (d-d_0)).transpose() << std::endl;
r.setZero();
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 = (r - A * d).norm() / A.rows();
residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size()));
std::cout << "r (reevaluated) = " << r.transpose() << std::endl;
// 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;
}
......
test/gtest_tdcp.cpp
+ 4
− 2
View file @ f99b2689
......@@ -57,9 +57,10 @@ TEST(Tdcp, Tdcp)
tdcp_params.max_iterations = 10;
tdcp_params.min_common_sats = 6;
tdcp_params.raim_n = 0;
tdcp_params.raim_min_residual = 0;
tdcp_params.max_residual = 0;
tdcp_params.relinearize_jacobian = true;
tdcp_params.tdcp.multi_freq = false;
tdcp_params.residual_opt = 0;
Vector3d sat_ENU, sat_ECEF;
Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
......@@ -193,9 +194,10 @@ TEST(Tdcp, Tdcp_raim)
tdcp_params.max_iterations = 10;
tdcp_params.min_common_sats = 6;
tdcp_params.raim_n = 2;
tdcp_params.raim_min_residual = 0;
tdcp_params.max_residual = 0;
tdcp_params.relinearize_jacobian = true;
tdcp_params.tdcp.multi_freq = false;
tdcp_params.residual_opt = 0;
Vector3d sat_ENU, sat_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