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

struct for tdcp output

parent 5aa076b2
No related branches found
No related tags found
3 merge requests!20new tag,!19new tag,!17Resolve "TDCP batch implementation"
Hide whitespace changes
Inline Side-by-side
include/gnss_utils/tdcp.h
+ 40
− 39
View file @ c31b219d
......@@ -17,48 +17,49 @@ struct TdcpBatchParams
int max_iterations;
};
bool Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
const std::set<int>& discarded_sats,
std::set<int>& raim_discarded_sats,
const TdcpBatchParams& tdcp_params,
const Options& opt);
struct TdcpOutput
{
bool success = false;
std::string msg = "";
std::set<int> raim_discarded_sats;
std::set<int> used_sats;
Eigen::Vector4d d = Eigen::Vector4d::Zero();
Eigen::Matrix4d cov_d = Eigen::Matrix4d::Zero();
double dt = 0;
double residual = 0;
};
TdcpOutput Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector4d& d_0,
const std::set<int>& discarded_sats,
const TdcpBatchParams& tdcp_params,
const Options& opt);
bool Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
const std::set<int>& discarded_sats,
std::set<int>& raim_discarded_sats,
const TdcpBatchParams& tdcp_params,
const Options& opt);
TdcpOutput Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r,
const Eigen::Vector4d& d_0,
const std::set<int>& discarded_sats,
const TdcpBatchParams& tdcp_params,
const Options& opt);
bool Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r,
const std::set<int> common_sats,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
std::set<int>& raim_discarded_sats,
const TdcpBatchParams& tdcp_params);
TdcpOutput Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r,
const std::set<int>& common_sats,
const Eigen::Vector4d& d_0,
const TdcpBatchParams& tdcp_params);
bool Tdcp(const Eigen::Vector3d& x_r,
Eigen::Matrix<double, Eigen::Dynamic, 4>& A,
Eigen::VectorXd& r,
Eigen::VectorXd& drho_m,
Eigen::Matrix<double, 3, Eigen::Dynamic>& s_k,
Eigen::Matrix<double, 3, Eigen::Dynamic>& s_r,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
std::set<int>& raim_discarded_sats,
const TdcpBatchParams& tdcp_params);
TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
Eigen::MatrixXd& A,
Eigen::VectorXd& r,
Eigen::VectorXd& drho_m,
Eigen::MatrixXd& s_k,
Eigen::MatrixXd& s_r,
const Eigen::Vector4d& d_0,
std::set<int>& raim_discarded_rows,
const TdcpBatchParams& tdcp_params);
} // namespace GnssUtils
......
src/tdcp.cpp
+ 77
− 75
View file @ c31b219d
......@@ -6,38 +6,29 @@
namespace GnssUtils
{
bool Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
const std::set<int>& discarded_sats,
std::set<int>& raim_discarded_sats,
const TdcpBatchParams& tdcp_params,
const Options& opt)
TdcpOutput Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector4d& d_0,
const std::set<int>& discarded_sats,
const TdcpBatchParams& tdcp_params,
const Options& opt)
{
return Tdcp(snapshot_r,
snapshot_k,
computePos(*snapshot_r->getObservations(), *snapshot_r->getNavigation(), opt).pos,
d,
cov_d,
residual,
d_0,
discarded_sats,
raim_discarded_sats,
tdcp_params,
opt);
}
bool Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
const std::set<int>& discarded_sats,
std::set<int>& raim_discarded_sats,
const TdcpBatchParams& tdcp_params,
const Options& opt)
TdcpOutput Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r,
const Eigen::Vector4d& d_0,
const std::set<int>& discarded_sats,
const TdcpBatchParams& tdcp_params,
const Options& opt)
{
// If use old nav temporary change navigation to (re)compute satellites positions
auto nav_k = snapshot_k->getNavigation();
......@@ -68,15 +59,12 @@ bool Tdcp(SnapshotPtr snapshot_r,
snapshot_k->getRanges());
// COMPUTE TDCP
bool tdcp_ok = Tdcp(snapshot_r,
snapshot_k,
x_r,
common_sats,
d,
cov_d,
residual,
raim_discarded_sats,
tdcp_params);
TdcpOutput output = Tdcp(snapshot_r,
snapshot_k,
x_r,
common_sats,
d_0,
tdcp_params);
// UNDO temporary change navigation
if (tdcp_params.tdcp.use_old_nav)
......@@ -85,25 +73,19 @@ bool Tdcp(SnapshotPtr snapshot_r,
snapshot_k->computeSatellites(opt.sateph);
}
return tdcp_ok;
return output;
}
bool Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r,
const std::set<int> common_sats,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
std::set<int>& raim_discarded_sats,
const TdcpBatchParams& tdcp_params)
TdcpOutput Tdcp(SnapshotPtr snapshot_r,
SnapshotPtr snapshot_k,
const Eigen::Vector3d& x_r,
const std::set<int>& common_sats,
const Eigen::Vector4d& d_0,
const TdcpBatchParams& tdcp_params)
{
// CHECKS
if (not raim_discarded_sats.empty())
{
std::cout << "TDCP: 'raim_discarded_sats' is just an output param, it will be cleared.\n";
raim_discarded_sats.clear();
}
TdcpOutput output;
assert(snapshot_r->satellitesComputed() && "this TDCP assumes satellites already computed");
assert(snapshot_k->satellitesComputed() && "this TDCP assumes satellites already computed");
assert(snapshot_r->rangesComputed() && "this TDCP assumes ranges already computed");
......@@ -120,7 +102,6 @@ bool Tdcp(SnapshotPtr snapshot_r,
assert(snapshot_k->getRanges().count(sat) && "common sat not stored in ranges");
}
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)
......@@ -129,7 +110,9 @@ bool Tdcp(SnapshotPtr snapshot_r,
printf("Tdcp: NOT ENOUGH COMMON SATS");
printf("Tdcp: %i common sats available, %i sats are REQUIRED. [ SKIPPING Tdcp ]", n_common_sats, required_n_sats);
#endif
return false;
output.msg = "Not enough common sats";
output.success = false;
return output;
}
// Times
......@@ -171,11 +154,11 @@ bool Tdcp(SnapshotPtr snapshot_r,
#endif
// FILL SATELLITES POSITIONS AND MEASUREMENTS =======================================================================
Eigen::Matrix<double, Eigen::Dynamic, 4> A(Eigen::Matrix<double, Eigen::Dynamic, 4>::Zero(n_differences, 4));
Eigen::VectorXd r(Eigen::VectorXd::Zero(n_differences));
Eigen::VectorXd drho_m(Eigen::VectorXd::Zero(n_differences));
Eigen::Matrix<double, 3, Eigen::Dynamic> s_k(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3, n_differences));
Eigen::Matrix<double, 3, Eigen::Dynamic> s_r(Eigen::Matrix<double, 3, Eigen::Dynamic>::Zero(3, n_differences));
Eigen::MatrixXd A(Eigen::MatrixXd::Zero(n_differences, 4));
Eigen::VectorXd r(Eigen::VectorXd::Zero(n_differences));
Eigen::VectorXd drho_m(Eigen::VectorXd::Zero(n_differences));
Eigen::MatrixXd s_k(Eigen::MatrixXd::Zero(3, n_differences));
Eigen::MatrixXd s_r(Eigen::MatrixXd::Zero(3, n_differences));
for (auto row_sat_freq_it : row_2_sat_freq)
{
......@@ -209,7 +192,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
if (!tdcp_params.relinearize_jacobian)
{
// Unit vectors from receivers to satellites
Eigen::Vector3d u_k = (s_k.col(row) - x_r - d.head(3)).normalized();
Eigen::Vector3d u_k = (s_k.col(row) - x_r - d_0.head(3)).normalized();
// Fill Jacobian matrix
A(row, 0) = u_k(0);
......@@ -221,34 +204,47 @@ bool Tdcp(SnapshotPtr snapshot_r,
// LEAST SQUARES SOLUTION =======================================================================
std::set<int> raim_discarded_rows;
bool result = Tdcp(x_r, A, r, drho_m, s_k, s_r, d, cov_d, residual, raim_discarded_rows, tdcp_params);
output = Tdcp(x_r, A, r, drho_m, s_k, s_r, d_0, raim_discarded_rows, tdcp_params);
// FILL OUTPUT
output.used_sats = common_sats;
for (auto disc_row : raim_discarded_rows)
raim_discarded_sats.insert(row_2_sat_freq[disc_row].first);
{
output.raim_discarded_sats.insert(row_2_sat_freq[disc_row].first);
output.used_sats.erase(row_2_sat_freq[disc_row].first);
}
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;
cov_d *= sq_sigma_Tdcp;
output.cov_d *= sq_sigma_Tdcp;
return result;
return output;
}
bool Tdcp(const Eigen::Vector3d& x_r,
Eigen::Matrix<double, Eigen::Dynamic, 4>& A,
Eigen::VectorXd& r,
Eigen::VectorXd& drho_m,
Eigen::Matrix<double, 3, Eigen::Dynamic>& s_k,
Eigen::Matrix<double, 3, Eigen::Dynamic>& s_r,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
std::set<int>& raim_discarded_rows,
const TdcpBatchParams& tdcp_params)
TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
Eigen::MatrixXd& A,
Eigen::VectorXd& r,
Eigen::VectorXd& drho_m,
Eigen::MatrixXd& s_k,
Eigen::MatrixXd& s_r,
const Eigen::Vector4d& d_0,
std::set<int>& raim_discarded_rows,
const TdcpBatchParams& tdcp_params)
{
assert(A.cols() == 4);
assert(s_k.rows() == 3);
assert(s_r.rows() == 3);
TdcpOutput output;
Eigen::Vector4d& d(output.d);
Eigen::Matrix4d& cov_d(output.cov_d);
double& residual(output.residual);
// Initial guess
Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero());
Eigen::Vector4d delta_d(Eigen::Vector4d::Zero());
d = d_0;
for (int j = 0; j < tdcp_params.max_iterations; j++)
{
......@@ -298,7 +294,9 @@ bool Tdcp(const Eigen::Vector3d& x_r,
std::cout << "Tdcp: cov_d = \n" << cov_d << std::endl;
printf("Tdcp: dT = %8.3f secs\n", d(3));
#endif
return false;
output.msg = "NaN values in NLS";
output.success = false;
return output;
}
// residual
......@@ -387,7 +385,9 @@ bool Tdcp(const Eigen::Vector3d& x_r,
if (worst_sat_row == -1)
{
printf("Tdcp: LS after RAIM DID NOT WORK. NAN values appeared. ABORTING!!");
return false;
output.msg = "NaN values in NLS after RAIM";
output.success = false;
return output;
}
// store removed sat
......@@ -466,7 +466,9 @@ bool Tdcp(const Eigen::Vector3d& x_r,
// residual = (r - A * d).norm() / A.rows();
residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size()));
return true;
output.success = true;
return output;
}
......
test/gtest_tdcp.cpp
+ 23
− 30
View file @ c31b219d
......@@ -65,7 +65,7 @@ TEST(Tdcp, Tdcp)
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;
Vector4d d_gt;
Matrix4d cov_d;
double range, residual;
......@@ -166,22 +166,18 @@ TEST(Tdcp, Tdcp)
}
// TDCP
d = Vector4d::Zero(); // initialize
bool tdcp_ok = Tdcp(snapshot_r,
snapshot_k,
x_r_ECEF,
common_sats,
d,
cov_d,
residual,
raim_discarded_sats,
tdcp_params);
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_TRUE(tdcp_ok);
ASSERT_LE(residual, 1e-9);
ASSERT_MATRIX_APPROX(d, d_gt, 1e-6);
ASSERT_TRUE(tdcp_out.success);
ASSERT_LE(tdcp_out.residual, 1e-9);
ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-6);
}
}
......@@ -307,26 +303,23 @@ TEST(Tdcp, Tdcp_raim)
int wrong_sat2 = (i + N_sats / 2) % N_sats;
snapshot_r->getRanges().at(wrong_sat2).L_corrected -= 10;
// TDCP
d = Vector4d::Zero(); // initialize
bool tdcp_ok = Tdcp(snapshot_r,
snapshot_k,
x_r_ECEF,
common_sats,
d,
cov_d,
residual,
raim_discarded_sats,
tdcp_params);
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(raim_discarded_sats.size(), 2);
ASSERT_TRUE(raim_discarded_sats.count(wrong_sat1));
ASSERT_TRUE(raim_discarded_sats.count(wrong_sat2));
ASSERT_TRUE(tdcp_ok);
ASSERT_LE(residual, 1e-9);
ASSERT_MATRIX_APPROX(d, d_gt, 1e-6);
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-9);
ASSERT_MATRIX_APPROX(tdcp_out.d, d_gt, 1e-6);
}
}
......
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