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

WIP

parent db78648f
No related branches found
No related tags found
3 merge requests!20new tag,!19new tag,!17Resolve "TDCP batch implementation"
Subproject commit 0260d91932df0ca0691842aa99b39acde5d49c45 Subproject commit 4ab9a199ff46b1220fb4fe99b019c8df526e53e9
...@@ -48,6 +48,19 @@ bool Tdcp(SnapshotPtr snapshot_r, ...@@ -48,6 +48,19 @@ bool Tdcp(SnapshotPtr snapshot_r,
double& residual, double& residual,
std::set<int> raim_discarded_sats, std::set<int> raim_discarded_sats,
const TdcpBatchParams& tdcp_params); 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 } // namespace GnssUtils
#endif /* INCLUDE_GNSS_UTILS_Tdcp_H_ */ #endif /* INCLUDE_GNSS_UTILS_Tdcp_H_ */
...@@ -25,6 +25,24 @@ namespace GnssUtils ...@@ -25,6 +25,24 @@ namespace GnssUtils
const Navigation& nav, const Navigation& nav,
const int& eph_opt); // see rtklib.h L396 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 struct Satellite
{ {
int sys; int sys;
......
...@@ -149,6 +149,8 @@ bool Tdcp(SnapshotPtr snapshot_r, ...@@ -149,6 +149,8 @@ bool Tdcp(SnapshotPtr snapshot_r,
{ {
tr = obs_r.time.time + obs_r.time.sec; tr = obs_r.time.time + obs_r.time.sec;
tk = obs_k.time.time + obs_k.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 // Carrier phase
...@@ -171,12 +173,9 @@ bool Tdcp(SnapshotPtr snapshot_r, ...@@ -171,12 +173,9 @@ bool Tdcp(SnapshotPtr snapshot_r,
} }
int n_differences = row_2_sat_freq.size(); 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 #if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl; 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: "; std::cout << "common sats: ";
for (auto row_sat_freq_it : row_2_sat_freq) for (auto row_sat_freq_it : row_2_sat_freq)
std::cout << row_sat_freq_it.second.first << " "; std::cout << row_sat_freq_it.second.first << " ";
...@@ -189,7 +188,6 @@ bool Tdcp(SnapshotPtr snapshot_r, ...@@ -189,7 +188,6 @@ bool Tdcp(SnapshotPtr snapshot_r,
Eigen::VectorXd drho_m(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_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::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) for (auto row_sat_freq_it : row_2_sat_freq)
{ {
...@@ -240,6 +238,36 @@ bool Tdcp(SnapshotPtr snapshot_r, ...@@ -240,6 +238,36 @@ bool Tdcp(SnapshotPtr snapshot_r,
} }
// LEAST SQUARES SOLUTION ======================================================================= // 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++) for (int j = 0; j < tdcp_params.max_iterations; j++)
{ {
// fill A and r // fill A and r
...@@ -301,10 +329,9 @@ bool Tdcp(SnapshotPtr snapshot_r, ...@@ -301,10 +329,9 @@ bool Tdcp(SnapshotPtr snapshot_r,
int worst_sat_row = -1; int worst_sat_row = -1;
double best_residual = 1e12; double best_residual = 1e12;
Eigen::Vector4d best_d; Eigen::Vector4d best_d;
int n_removed_rows = 1;
// remove some satellites // 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 A_raim = A;
auto r_raim = r; auto r_raim = r;
...@@ -312,21 +339,13 @@ bool Tdcp(SnapshotPtr snapshot_r, ...@@ -312,21 +339,13 @@ bool Tdcp(SnapshotPtr snapshot_r,
// solve removing each satellite // solve removing each satellite
for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++) for (int row_removed = 0; row_removed < A_raim.rows(); row_removed++)
{ {
int sat_removed = row_2_sat_freq.at(row_removed).first; // skip already discarded rows
if (raim_discarded_rows.count(row_removed) != 0)
// Multi-freq: some rows for the same satellite continue;
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++;
// remove satellite rows // remove satellite row
for (auto r = 0; r < n_removed_rows; r++) A_raim.row(row_removed) = Eigen::Vector4d::Zero().transpose();
{ r_raim(row_removed) = 0; // not necessary
A_raim.row(row_removed + r) = Eigen::Vector4d::Zero().transpose();
r_raim(row_removed + r) = 0; // not necessary
}
// solve // solve
Eigen::Vector4d delta_d_raim = -(A_raim.transpose() * A_raim).inverse() * A_raim.transpose() * r_raim; 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, ...@@ -340,8 +359,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
// residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows()); // residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows());
#if GNSS_UTILS_TDCP_DEBUG == 1 #if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "RAIM excluding sat " << sat_removed << " - row " << row_removed << "(n_rows " std::cout << "RAIM excluding row " << << row_removed << std::endl;
<< n_removed_rows << ")" << std::endl;
std::cout << "Displacement vector =" << d_raim.head<3>().transpose() << 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; std::cout << "Displacement update =" << delta_d_raim.head<3>().transpose() << std::endl;
printf("Ref distance = %7.3f m\n", d_0.norm()); printf("Ref distance = %7.3f m\n", d_0.norm());
...@@ -363,12 +381,8 @@ bool Tdcp(SnapshotPtr snapshot_r, ...@@ -363,12 +381,8 @@ bool Tdcp(SnapshotPtr snapshot_r,
} }
} }
// restore initial A and r // restore initial A and r
for (auto row = 0; row < n_removed_rows; row++) A_raim.row(row_removed) = A.row(row_removed);
{ r_raim(row_removed) = r(row_removed);
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);
} }
// No successful RAIM solution // No successful RAIM solution
...@@ -379,13 +393,9 @@ bool Tdcp(SnapshotPtr snapshot_r, ...@@ -379,13 +393,9 @@ bool Tdcp(SnapshotPtr snapshot_r,
} }
// store removed sat // store removed sat
raim_discarded_sats.insert(row_2_sat_freq[worst_sat_row].first); raim_discarded_rows.insert(worst_sat_row);
// decrease n_common_sats
n_differences -= n_removed_rows;
n_common_sats--;
// Remove selected satellite from problem /*// Remove selected satellite from problem
std::cout << "resizing problem..." << std::endl; std::cout << "resizing problem..." << std::endl;
auto A_aux = A; auto A_aux = A;
auto r_aux = r; auto r_aux = r;
...@@ -406,7 +416,7 @@ bool Tdcp(SnapshotPtr snapshot_r, ...@@ -406,7 +416,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
s_k.rightCols(back_elements_changing) = s_k_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); r.tail(back_elements_changing) = r_aux.tail(back_elements_changing);
drho_m.tail(back_elements_changing) = drho_m_aux.tail(back_elements_changing); drho_m.tail(back_elements_changing) = drho_m_aux.tail(back_elements_changing);
} }*/
// apply the RAIM solution // apply the RAIM solution
d = best_d; d = best_d;
...@@ -415,9 +425,10 @@ bool Tdcp(SnapshotPtr snapshot_r, ...@@ -415,9 +425,10 @@ bool Tdcp(SnapshotPtr snapshot_r,
#if GNSS_UTILS_TDCP_DEBUG == 1 #if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Tdcp After RAIM " << std::endl; std::cout << "Tdcp After RAIM " << std::endl;
std::cout << "\tExcluded sats : "; std::cout << "\tExcluded rows : ";
std::cout << "\tCommon sats : " << n_common_sats << std::endl; for (auto excl_row : raim_discarded_rows)
std::cout << "\tRows : " << n_differences << std::endl; 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 << "\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 << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
std::cout << "\tClock offset = " << d(3) << std::endl; std::cout << "\tClock offset = " << d(3) << std::endl;
...@@ -430,9 +441,10 @@ bool Tdcp(SnapshotPtr snapshot_r, ...@@ -430,9 +441,10 @@ bool Tdcp(SnapshotPtr snapshot_r,
#if GNSS_UTILS_TDCP_DEBUG == 1 #if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Tdcp iteration " << j << std::endl; std::cout << "Tdcp iteration " << j << std::endl;
std::cout << "\tExcluded sats : "; std::cout << "\tExcluded rows : ";
std::cout << "\tCommon sats : " << common_sats.size() << std::endl; for (auto excl_row : raim_discarded_rows)
std::cout << "\tRows : " << n_differences << std::endl; 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 << "\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 << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
std::cout << "\tClock offset = " << d(3) << std::endl; std::cout << "\tClock offset = " << d(3) << std::endl;
...@@ -443,19 +455,17 @@ bool Tdcp(SnapshotPtr snapshot_r, ...@@ -443,19 +455,17 @@ bool Tdcp(SnapshotPtr snapshot_r,
#endif #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 // Computing error on measurement space
for (int row = 0; row < n_differences; row++) r.setZero();
r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3); 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; return true;
} }
} // namespace GnssUtils } // namespace GnssUtils
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment