diff --git a/deps/RTKLIB b/deps/RTKLIB
index 0260d91932df0ca0691842aa99b39acde5d49c45..4ab9a199ff46b1220fb4fe99b019c8df526e53e9 160000
--- a/deps/RTKLIB
+++ b/deps/RTKLIB
@@ -1 +1 @@
-Subproject commit 0260d91932df0ca0691842aa99b39acde5d49c45
+Subproject commit 4ab9a199ff46b1220fb4fe99b019c8df526e53e9
diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index 143fa67b4d775ae123fafffbb43df65700445946..5cc391964f820d6588753b1578381c0e3cd7b664 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -48,6 +48,19 @@ bool Tdcp(SnapshotPtr snapshot_r,
double& residual,
std::set<int> raim_discarded_sats,
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);
+
} // namespace GnssUtils
#endif /* INCLUDE_GNSS_UTILS_Tdcp_H_ */
diff --git a/include/gnss_utils/utils/satellite.h b/include/gnss_utils/utils/satellite.h
index 491f331cba40bf7de69909cc7a712909afae87c5..2f8dac50b926408d12507a77b8d45fd96c332c7f 100644
--- a/include/gnss_utils/utils/satellite.h
+++ b/include/gnss_utils/utils/satellite.h
@@ -25,6 +25,24 @@ namespace GnssUtils
const Navigation& nav,
const int& eph_opt); // see rtklib.h L396
+ Eigen::Vector2d computeAzel(const Satellite& sat, const Eigen::Vector3d& receiver_ecef)
+ {
+ Eigen::Vector3d unit_vector;
+ geodist(sat.pose.data(), receiver_ecef.data(), unit_vector.data());
+
+ Eigen::Vector2d azel;
+ Eigen::Vector3d pos_enu; // TODO
+ satazel(pos_enu.data(), unit_vector.data(), azel.data());
+ }
+
+ Azels computeAzels(const Satellites& sats, const Eigen::Vector3d& receiver_ecef)
+ {
+ Azels azels;
+ for (auto sat : sats)
+ azels.insert(sat.first, computeAzel(sat, receiver_ecef));
+ return azels;
+ }
+
struct Satellite
{
int sys;
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index 1bea6dab33bd705a8c5fee41bfe052b8bade5953..c67052f2f53d8f3d7f9a757f9afac4d9cbf86513 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -149,6 +149,8 @@ bool Tdcp(SnapshotPtr snapshot_r,
{
tr = obs_r.time.time + obs_r.time.sec;
tk = obs_k.time.time + obs_k.time.sec;
+ std::cout << "tr: " << tr << std::endl;
+ std::cout << "tk: " << tk << std::endl;
}
// Carrier phase
@@ -171,12 +173,9 @@ bool Tdcp(SnapshotPtr snapshot_r,
}
int n_differences = row_2_sat_freq.size();
- // Initial guess
- Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero());
-
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl;
- std::cout << "d initial guess: " << d_0.transpose() << std::endl;
+ std::cout << "d initial guess: " << d.transpose() << std::endl;
std::cout << "common sats: ";
for (auto row_sat_freq_it : row_2_sat_freq)
std::cout << row_sat_freq_it.second.first << " ";
@@ -189,7 +188,6 @@ bool Tdcp(SnapshotPtr snapshot_r,
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));
- int sat_i = 0;
for (auto row_sat_freq_it : row_2_sat_freq)
{
@@ -240,6 +238,36 @@ 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);
+
+ for (auto disc_row : raim_discarded_rows)
+ raim_discarded_sats.insert(row_2_sat_freq[disc_row].first);
+
+ // 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;
+
+ return result;
+}
+
+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)
+{
+
+ // Initial guess
+ Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero());
+
for (int j = 0; j < tdcp_params.max_iterations; j++)
{
// fill A and r
@@ -301,10 +329,9 @@ bool Tdcp(SnapshotPtr snapshot_r,
int worst_sat_row = -1;
double best_residual = 1e12;
Eigen::Vector4d best_d;
- int n_removed_rows = 1;
// remove some satellites
- while (raim_discarded_sats.size() < tdcp_params.raim_n)
+ while (raim_discarded_rows.size() < tdcp_params.raim_n)
{
auto A_raim = A;
auto r_raim = r;
@@ -312,21 +339,13 @@ bool Tdcp(SnapshotPtr snapshot_r,
// solve removing each satellite
for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
{
- int sat_removed = row_2_sat_freq.at(row_removed).first;
-
- // Multi-freq: some rows for the same satellite
- n_removed_rows = 1;
- if (tdcp_params.tdcp.multi_freq)
- while (row_removed + n_removed_rows < A_raim.rows() and
- row_2_sat_freq.at(row_removed + n_removed_rows).first == sat_removed)
- n_removed_rows++;
+ // skip already discarded rows
+ if (raim_discarded_rows.count(row_removed) != 0)
+ continue;
- // remove satellite rows
- for (auto r = 0; r < n_removed_rows; r++)
- {
- A_raim.row(row_removed + r) = Eigen::Vector4d::Zero().transpose();
- r_raim(row_removed + r) = 0; // not necessary
- }
+ // remove satellite row
+ A_raim.row(row_removed) = Eigen::Vector4d::Zero().transpose();
+ r_raim(row_removed) = 0; // not necessary
// solve
Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim;
@@ -340,8 +359,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
// residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows());
#if GNSS_UTILS_TDCP_DEBUG == 1
- std::cout << "RAIM excluding sat " << sat_removed << " - row " << row_removed << "(n_rows "
- << n_removed_rows << ")" << 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());
@@ -363,12 +381,8 @@ bool Tdcp(SnapshotPtr snapshot_r,
}
}
// restore initial A and r
- for (auto row = 0; row < n_removed_rows; row++)
- {
- A_raim.row(row_removed + row) = A.row(row_removed + row);
- r_raim(row_removed + row) = r(row_removed + row);
- }
- row_removed += (n_removed_rows - 1);
+ A_raim.row(row_removed) = A.row(row_removed);
+ r_raim(row_removed) = r(row_removed);
}
// No successful RAIM solution
@@ -379,13 +393,9 @@ bool Tdcp(SnapshotPtr snapshot_r,
}
// store removed sat
- raim_discarded_sats.insert(row_2_sat_freq[worst_sat_row].first);
-
- // decrease n_common_sats
- n_differences -= n_removed_rows;
- n_common_sats--;
+ raim_discarded_rows.insert(worst_sat_row);
- // Remove selected satellite from problem
+ /*// Remove selected satellite from problem
std::cout << "resizing problem..." << std::endl;
auto A_aux = A;
auto r_aux = r;
@@ -406,7 +416,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
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;
@@ -415,9 +425,10 @@ bool Tdcp(SnapshotPtr snapshot_r,
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Tdcp After RAIM " << std::endl;
- std::cout << "\tExcluded sats : ";
- std::cout << "\tCommon sats : " << n_common_sats << std::endl;
- std::cout << "\tRows : " << n_differences << std::endl;
+ std::cout << "\tExcluded rows : ";
+ for (auto excl_row : raim_discarded_rows)
+ std::cout << excl_row << " ";
+ std::cout << std::endl;
std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
std::cout << "\tClock offset = " << d(3) << std::endl;
@@ -430,9 +441,10 @@ bool Tdcp(SnapshotPtr snapshot_r,
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Tdcp iteration " << j << std::endl;
- std::cout << "\tExcluded sats : ";
- std::cout << "\tCommon sats : " << common_sats.size() << std::endl;
- std::cout << "\tRows : " << n_differences << std::endl;
+ std::cout << "\tExcluded rows : ";
+ for (auto excl_row : raim_discarded_rows)
+ std::cout << excl_row << " ";
+ std::cout << std::endl;
std::cout << "\tInitial guess = " << d_0.transpose() << " (" << d_0.head<3>().norm() << "m)" << std::endl;
std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
std::cout << "\tClock offset = " << d(3) << std::endl;
@@ -443,19 +455,17 @@ bool Tdcp(SnapshotPtr snapshot_r,
#endif
}
- // 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;
- // residual = (r - A * d).norm() / A.rows();
-
// Computing error on measurement space
- for (int row = 0; row < n_differences; row++)
- 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.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 = sqrt(r.squaredNorm() / r.size());
+ // residual = (r - A * d).norm() / A.rows();
+ residual = sqrt(r.squaredNorm() / (r.size() - raim_discarded_rows.size()));
return true;
}
+
} // namespace GnssUtils