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,
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_ */
......@@ -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;
......
......@@ -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
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