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

working!

parent 7338f7b3
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
+ 2
− 2
View file @ b7f35fa4
......@@ -46,7 +46,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
std::set<int> raim_discarded_sats,
std::set<int>& raim_discarded_sats,
const TdcpBatchParams& tdcp_params);
bool Tdcp(const Eigen::Vector3d& x_r,
......@@ -58,7 +58,7 @@ bool Tdcp(const Eigen::Vector3d& x_r,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
std::set<int> raim_discarded_sats,
std::set<int>& raim_discarded_sats,
const TdcpBatchParams& tdcp_params);
} // namespace GnssUtils
......
src/tdcp.cpp
+ 28
− 9
View file @ b7f35fa4
......@@ -95,7 +95,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
std::set<int> raim_discarded_sats,
std::set<int>& raim_discarded_sats,
const TdcpBatchParams& tdcp_params)
{
// CHECKS
......@@ -137,7 +137,6 @@ bool Tdcp(SnapshotPtr snapshot_r,
double tk = snapshot_k->getGPST();
// MULTI-FREQUENCY
// TODO: change obs -> ranges
std::map<int, std::pair<int, int>> row_2_sat_freq;
int row = 0;
for (auto sat : common_sats)
......@@ -197,7 +196,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
drho_m(row) = range_k.L2_corrected - range_r.L2_corrected;
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "\tsat " << sat_number
std::cout << "\tsat " << sat_number;
std::cout << std::setprecision(10)
<< "\tpositions:" << std::endl
<< "\t\ts_r: " << s_r.col(row).transpose() << std::endl
......@@ -244,7 +243,7 @@ bool Tdcp(const Eigen::Vector3d& x_r,
Eigen::Vector4d& d,
Eigen::Matrix4d& cov_d,
double& residual,
std::set<int> raim_discarded_rows,
std::set<int>& raim_discarded_rows,
const TdcpBatchParams& tdcp_params)
{
......@@ -256,6 +255,10 @@ bool Tdcp(const Eigen::Vector3d& x_r,
// fill A and r
for (int row = 0; row < A.rows(); row++)
{
// skip discarded rows
if (raim_discarded_rows.count(row) != 0)
continue;
// Evaluate r
r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
......@@ -282,6 +285,19 @@ bool Tdcp(const Eigen::Vector3d& x_r,
if (d.array().isNaN().any())
{
std::cout << "Tdcp: NLS DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl;
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Displacement vector =" << d.head<3>().transpose() << std::endl;
std::cout << "Displacement update =" << delta_d.head<3>().transpose() << std::endl;
printf("Ref distance = %7.3f m\n", d_0.norm());
printf("Computed distance = %7.3f m\n", d.head<3>().norm());
printf("Tdcp: rows = %lu\n", r.rows());
std::cout << "Tdcp: r = " << r.transpose() << std::endl;
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));
#endif
return false;
}
......@@ -297,7 +313,7 @@ bool Tdcp(const Eigen::Vector3d& x_r,
printf("Ref distance = %7.3f m\n", d_0.norm());
printf("Computed distance = %7.3f m\n", d.head<3>().norm());
printf("Tdcp: residual = %13.10f\n", residual);
printf("Tdcp: row = %lu\n", r.rows());
printf("Tdcp: rows = %lu\n", r.rows());
std::cout << "Tdcp: r = " << r.transpose() << std::endl;
std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl;
std::cout << "Tdcp: A = \n" << A << std::endl;
......@@ -342,16 +358,15 @@ bool Tdcp(const Eigen::Vector3d& x_r,
// residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows());
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "RAIM excluding row " << << row_removed << std::endl;
std::cout << "RAIM excluding row " << row_removed << std::endl;
std::cout << "Displacement vector =" << d_raim.head<3>().transpose() << std::endl;
std::cout << "Displacement update =" << delta_d_raim.head<3>().transpose() << std::endl;
printf("Ref distance = %7.3f m\n", d_0.norm());
printf("Computed distance = %7.3f m\n", d_raim.head<3>().norm());
printf("Tdcp: residual = %13.10f\n", residual);
std::cout << "Tdcp: drho = " << r_raim.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;
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
......@@ -378,6 +393,10 @@ bool Tdcp(const Eigen::Vector3d& x_r,
// store removed sat
raim_discarded_rows.insert(worst_sat_row);
// remove row
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;
......
test/gtest_tdcp.cpp
+ 87
− 18
View file @ b7f35fa4
......@@ -22,7 +22,7 @@ void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt,
Vector3d& sat_ENU,
Vector3d& sat_ECEF,
Vector2d& sat_azel,
double range)
double& range)
{
Vector3d t_ECEF_ENU, t_ENU_ECEF;
Matrix3d R_ENU_ECEF;
......@@ -44,13 +44,23 @@ void computeRandomVisibleSatellite(const Vector3d& receiver_latlonalt,
// ECEF
sat_ECEF = t_ECEF_ENU + R_ENU_ECEF.transpose() * sat_ENU;
// Range
range = (sat_ECEF - t_ECEF_ENU).norm();
}
TEST(Tdcp, Tdcp)
{
int N_sats = 20;
TdcpBatchParams tdcp_params;
// TODO: fill params
tdcp_params.max_iterations = 10;
tdcp_params.min_common_sats = 6;
tdcp_params.raim_n = 2;
tdcp_params.raim_min_residual = 0;
tdcp_params.relinearize_jacobian = true;
tdcp_params.tdcp.multi_freq = false;
Vector3d sat_ENU, sat_ECEF;
Vector3d x_r_LLA, x_r_ECEF, x_k_LLA, x_k_ECEF, d_ECEF;
......@@ -58,58 +68,112 @@ TEST(Tdcp, Tdcp)
Vector2d azel, azel2;
Vector4d d, d_gt;
Matrix4d cov_d;
double residual;
double range;
Satellite sat_def({0,0,Vector3d::Zero(),Vector3d::Zero(),1.0,0,0,1});
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<100; 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);
// TODO: random clock biases
double clock_r, clock_k;
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;
d_gt(3) = (clock_k - clock_r) * CLIGHT;
std::cout << "Iteration " << 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::set<int> common_sats;
std::set<int> raim_discarded_sats;
// random visible satellites
for (auto j = 0; j<10; j++)
for (auto j = 0; j<N_sats; j++)
{
common_sats.insert(j);
// Satellite r (random)
computeRandomVisibleSatellite(x_r_LLA, sat_ENU, sat_ECEF, azel, range);
EXPECT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_r_ECEF), 1e-6);
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);
// TODO: compute range and add random clock bias
// Range r
Range range_r;
range_r.sat = j;
range_r.L_corrected = range + CLIGHT * clock_r;
range_r.L_var = 1;
range_r.L2_corrected = range_r.L_corrected;
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)
computeRandomVisibleSatellite(x_k_LLA, sat_ENU, sat_ECEF, azel, range);
EXPECT_MATRIX_APPROX(azel, computeAzel(sat_ECEF, x_k_ECEF), 1e-6);
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);
// TODO: compute range and add random clock bias
// Range k
Range range_k;
range_k.sat = j;
range_k.L_corrected = range + CLIGHT * clock_k;
range_k.L_var = 1;
range_k.L2_corrected = range_k.L_corrected;
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";
}
// TODO: randomly distort 1 satellite -> to be detected by RAIM
// Distort one range -> to be detected by RAIM
int wrong_sat1 = i % N_sats;
snapshot_k->getRanges().at(wrong_sat1).L_corrected += 20;
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,
......@@ -120,9 +184,14 @@ TEST(Tdcp, Tdcp)
raim_discarded_sats,
tdcp_params);
EXPECT_TRUE(tdcp_ok);
EXPECT_MATRIX_APPROX(d, d_gt, 1e-9);
EXPECT_LE(residual, 1e-9);
// 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);
}
}
......
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