diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index 7005a84596a41a9fb6a4cd491b8d7b6f546f1111..5bcd6add6289ee2e128a7615f0b67fa6e1112840 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -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);
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index 995489cf4b66e4d75ea5f19127e7ec269133cd13..23f981a77d2d3d5ed2b711e06dc83f150d35f6d8 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -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;
}
diff --git a/test/gtest_tdcp.cpp b/test/gtest_tdcp.cpp
index cdb90c91ac14a642e40a7a3f75cd8c549cc1de2e..f022cd9340b27d731a1858b122464cd3ab977388 100644
--- a/test/gtest_tdcp.cpp
+++ b/test/gtest_tdcp.cpp
@@ -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;