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

residual computation bug

parent a3b283f8
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
Hide whitespace changes
Inline Side-by-side
src/single_differences.cpp
+ 40
− 16
View file @ 21014dac
......@@ -133,7 +133,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
int n_differences = row_2_sat_freq.size();
// Initial guess
Eigen::Vector4d d_0(d);
Eigen::Vector4d d_0(d), delta_d(Eigen::Vector4d::Zero());
#if GNSS_UTILS_SD_DEBUG == 1
std::cout << "Receiver at t_r: " << x_r.transpose() << std::endl;
......@@ -142,6 +142,10 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
for (auto row_sat_freq_it : row_2_sat_freq)
std::cout << row_sat_freq_it.second.first << " ";
std::cout << std::endl;
std::cout << "discarded_sats: ";
for (auto sat : discarded_sats)
std::cout << sat << " ";
std::cout << std::endl;
#endif
// FILL SATELLITES POSITIONS AND MEASUREMENTS =======================================================================
......@@ -182,7 +186,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
drho_m(row) = obs_k.P[freq] - obs_r.P[freq];
#if GNSS_UTILS_SD_DEBUG == 1
std::cout << "\tsat " << sat_number << " frequency " << freq << " wavelength = " << nav_r.getNavigation().lam[sat_number-1][freq] << std::endl;
//std::cout << "\tsat " << sat_number << " frequency " << freq << " wavelength = " << nav_r.getNavigation().lam[sat_number-1][freq] << std::endl;
//std::cout << "\tpositions:\n\t\tt_r: " << s_r.col(row).transpose() << "\n\t\tt_k: " << s_k.col(row).transpose() << std::endl;
//std::cout << "\tobs_r.L: " << obs_r.L[freq] << std::endl;
//std::cout << "\tobs_k.L: " << obs_k.L[freq] << std::endl;
......@@ -231,7 +235,8 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
// Solve
cov_d = (A.transpose()*A).inverse();
d -= cov_d*A.transpose()*r;
delta_d = -cov_d*A.transpose()*r;
d += delta_d;
// wrong solution
if(d.array().isNaN().any())
......@@ -241,13 +246,14 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
}
// residual
//residual = sqrt((r - A * d).squaredNorm() / A.rows());
residual = (r + A * d).norm();
//residual = sqrt((r - A * delta_d).squaredNorm() / A.rows());
residual = (r + A * delta_d).norm();
//std::cout << "residual = " << residual << std::endl;
//std::cout << "SD2: r-A*d = " << (r + A * d).transpose() << std::endl;
//std::cout << "SD2: r-A*delta_d = " << (r + A * delta_d).transpose() << std::endl;
#if GNSS_UTILS_SD_DEBUG == 1
std::cout << "Displacement vector = %s" << d.head<3>().transpose() << std::endl;
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("SD: residual = %13.10f\n", residual);
......@@ -271,16 +277,19 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
// remove some satellites
while(discarded_sats.size() - input_discarded_sats < sd_params.raim_n)
{
auto A_raim = A;
auto r_raim = 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 (sd_params.use_multi_freq)
while (row_removed + n_removed_rows < A_raim.rows() and row_2_sat_freq.at(row_removed+n_removed_rows) == row_2_sat_freq.at(row_removed))
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
......@@ -291,15 +300,28 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
}
// solve
Eigen::Vector4d d_raim = d_0-(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;
Eigen::Vector4d d_raim = d_0+delta_d_raim;
// no NaN
if (!d_raim.array().isNaN().any())
{
// residual
residual = (r - A * d_raim).norm() / A.rows(); //FIXME: evaluate with XX_raim or not? I think next line is the good one
//residual = sqrt((r_raim - A_raim * d_raim).squaredNorm() / A_raim.rows());
residual = (r_raim + A_raim * delta_d_raim).norm() / (A_raim.rows()-1);
//residual = sqrt((r_raim + A_raim * delta_d_raim).squaredNorm() / A_raim.rows());
#if GNSS_UTILS_SD_DEBUG == 1
std::cout << "RAIM excluding sat " << sat_removed << " - row " << row_removed << "(n_rows " << n_removed_rows << ")" << 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("SD: residual = %13.10f\n", residual);
std::cout << "SD: drho = " << r_raim.transpose() << std::endl;
std::cout << "SD: H = \n" << A_raim << std::endl;
printf("SD: dT = %8.3f secs\n", d_raim(3));
#endif
// store if best residual
if (residual < best_residual)
{
......@@ -324,14 +346,16 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
return false;
}
// store removed sat
discarded_sats.insert(row_2_sat_freq[worst_sat_row].first);
// decrease n_common_sats
n_differences=-n_removed_rows;
n_differences-=n_removed_rows;
n_common_sats--;
// Remove selected satellite from problem
std::cout << "resizing problem..." << std::endl;
auto A_aux = A;
auto r_aux = r;
auto drho_m_aux = drho_m;
......@@ -343,7 +367,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
s_r.conservativeResize(Eigen::NoChange, n_differences);
s_k.conservativeResize(Eigen::NoChange, n_differences);
int back_elements_changing = A.rows() - worst_sat_row - n_removed_rows;
int back_elements_changing = A_aux.rows() - worst_sat_row - n_removed_rows;
if (back_elements_changing != 0) //if last satelite removed, conservative resize did the job
{
A .bottomRows(back_elements_changing) = A_aux .bottomRows(back_elements_changing);
......@@ -368,7 +392,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
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;
std::cout << "\tResidual = " << residual << std::endl;
std::cout << "\tResidual = " << best_residual << std::endl;
#endif
}
......@@ -383,7 +407,7 @@ bool singleDifferences(const Observations& common_obs_r, const Navigation& nav_r
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;
std::cout << "\tResidual = " << residual << std::endl;
std::cout << "\tResidual = " << best_residual << std::endl;
#endif
}
......
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