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

added batch to tdcp params

parent d887b670
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
Hide whitespace changes
Inline Side-by-side
include/gnss_utils/gnss_utils.h
+ 3
− 2
View file @ c33c62d5
......@@ -99,6 +99,7 @@ const prcopt_t default_prcopt = {
struct TdcpOptions
{
bool enabled; // TDCP enabled
bool batch; // precompute global displacement between 2 epochs
bool corr_iono; // apply correction also in TDCP
bool corr_tropo; // apply correction also in TDCP
bool corr_clock;
......@@ -116,7 +117,7 @@ struct Options
int ionoopt; // ionosphere option: IONOOPT_OFF(0):correction off, IONOOPT_BRDC(1):broadcast mode, IONOOPT_SBAS(2):SBAS model, IONOOPT_IFLC(3):L1/L2 or L1/L5, IONOOPT_EST(4):estimation, IONOOPT_TEC(5):IONEX TEC model, IONOOPT_QZS(6):QZSS broadcast, IONOOPT_LEX(7):QZSS LEX ionosphere, IONOOPT_STEC(8):SLANT TEC mode
int tropopt; // troposphere option: TROPOPT_OFF(0):correction off, TROPOPT_SAAS(1):Saastamoinen model, TROPOPT_SBAS(2):SBAS model, TROPOPT_EST(3):troposphere option: ZTD estimation, TROPOPT_ESTG(4):ZTD+grad estimation, TROPOPT_ZTD(5):ZTD correction,6:ZTD+grad correction
int sbascorr; // SBAS correction options (can be added): SBSOPT_LCORR(1): long term correction, SBSOPT_FCORR(2): fast correction, SBSOPT_ICORR(4): ionosphere correction, SBSOPT_RANGE(8): ranging
bool raim; // RAIM enabled
int raim; // RAIM removed sats
double elmin; // min elevation (degrees)
double maxgdop; // maxgdop: reject threshold of gdop
bool GPS,SBS,GLO,GAL,QZS,CMP,IRN,LEO; // constellations used
......@@ -155,7 +156,7 @@ const Options default_options =
IONOOPT_BRDC, // ionosphere option: IONOOPT_OFF(0):correction off, IONOOPT_BRDC(1):broadcast mode, IONOOPT_SBAS(2):SBAS model, IONOOPT_IFLC(3):L1/L2 or L1/L5, IONOOPT_EST(4):estimation, IONOOPT_TEC(5):IONEX TEC model, IONOOPT_QZS(6):QZSS broadcast, IONOOPT_LEX(7):QZSS LEX ionosphere, IONOOPT_STEC(8):SLANT TEC mode
TROPOPT_SAAS, // troposphere option: TROPOPT_OFF(0):correction off, TROPOPT_SAAS(1):Saastamoinen model, TROPOPT_SBAS(2):SBAS model, TROPOPT_EST(3):troposphere option: ZTD estimation, TROPOPT_ESTG(4):ZTD+grad estimation, TROPOPT_ZTD(5):ZTD correction,6:ZTD+grad correction
0, // SBAS correction options (can be added): SBSOPT_LCORR(1): long term correction, SBSOPT_FCORR(2): fast correction, SBSOPT_ICORR(4): ionosphere correction, SBSOPT_RANGE(8): ranging
true, // RAIM enabled
1, // RAIM enabled
D2R*15.0, // min elevation (degrees)
30.0, // maxgdop: reject threshold of gdop
true, false, true, true, false, false, false, false, //GPS,SBS,GLO,GAL,QZS,CMP,IRN,LEO; // constellations used
......
include/gnss_utils/tdcp.h
+ 1
− 1
View file @ c33c62d5
#ifndef INCLUDE_GNSS_UTILS_TDCP_H_
#define INCLUDE_GNSS_UTILS_TDCP_H_
#define GNSS_UTILS_TDCP_DEBUG 1
#define GNSS_UTILS_TDCP_DEBUG 0
#include "gnss_utils/gnss_utils.h"
......
src/tdcp.cpp
+ 30
− 27
View file @ c33c62d5
#include <iomanip>
#include "gnss_utils/tdcp.h"
#include "gnss_utils/utils/rcv_position.h"
#include "gnss_utils/utils/satellite.h"
......@@ -203,23 +204,26 @@ bool Tdcp(SnapshotPtr snapshot_r,
// Satellite's positions
s_r.col(row) = snapshot_r->getSatellites().at(sat_number).pos;
s_k.col(row) = snapshot_r->getSatellites().at(sat_number).pos;
nav_k.ion_gps;
s_k.col(row) = snapshot_k->getSatellites().at(sat_number).pos;
//nav_k.ion_gps;
drho_m(row) = (obs_k.L[freq] * nav_k.lam[sat_number - 1][freq]) -
(obs_r.L[freq] * nav_r.lam[sat_number - 1][freq]);
#if GNSS_UTILS_TDCP_DEBUG == 1
// 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; std::cout <<
// "\tnav_r.getNavigation().lam[sat_number-1][freq]: " << nav_r.getNavigation().lam[sat_number-1][freq] <<
// std::endl; std::cout << "\tnav_k.getNavigation().lam[sat_number-1][freq]: " <<
// nav_k.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\trho_r: " << obs_r.L[freq] *
// nav_r.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\trho_k: " << obs_k.L[freq] *
// nav_k.getNavigation().lam[sat_number-1][freq] << std::endl; std::cout << "\tdrho_m: " << drho_m(row) <<
// std::endl;
std::cout << "\tsat " << sat_number
<< " frequency " << freq
<< " wavelength = " << nav_r.lam[sat_number-1][freq] << std::endl;
std::cout << std::setprecision(10) << "\tpositions:\n\t\ts_r: " << s_r.col(row).transpose()
<< "\n\t\ts_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;
std::cout << "\tnav_r.getNavigation().lam[sat_number-1][freq]: " << nav_r.lam[sat_number-1][freq] << std::endl;
std::cout << "\tnav_k.getNavigation().lam[sat_number-1][freq]: " << nav_k.lam[sat_number-1][freq] << std::endl;
std::cout << "\trho_r: " << obs_r.L[freq] * nav_r.lam[sat_number-1][freq] << std::endl;
std::cout << "\trho_k: " << obs_k.L[freq] * nav_k.lam[sat_number-1][freq] << std::endl;
std::cout << "\tdrho_m: " << drho_m(row) << std::endl;
#endif
if (!tdcp_params.relinearize_jacobian)
......@@ -266,7 +270,7 @@ bool Tdcp(SnapshotPtr snapshot_r,
// wrong solution
if (d.array().isNaN().any())
{
std::cout << "Tdcp: LSM DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl;
std::cout << "Tdcp: NLS DID NOT WORK. NAN values appeared. ABORTING!!" << std::endl;
return false;
}
......@@ -283,9 +287,11 @@ bool Tdcp(SnapshotPtr snapshot_r,
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());
std::cout << "Tdcp: drho = " << r.transpose() << std::endl;
std::cout << "Tdcp: r = " << r.transpose() << std::endl;
std::cout << "Tdcp: drho_m = " << drho_m.transpose() << std::endl;
std::cout << "Tdcp: H = \n" << A << 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
......@@ -342,7 +348,9 @@ bool Tdcp(SnapshotPtr snapshot_r,
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: H = \n" << A_raim << 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_raim(3));
#endif
......@@ -403,17 +411,6 @@ bool Tdcp(SnapshotPtr snapshot_r,
// apply the RAIM solution
d = best_d;
cov_d = (A.transpose() * A).inverse();
#if GNSS_UTILS_TDCP_DEBUG == 1
std::cout << "Tdcp After RAIM iteration" << std::endl;
std::cout << "\tExcluded sats : ";
std::cout << "\tCommon sats : " << n_common_sats << std::endl;
std::cout << "\tRows : " << n_differences << 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;
std::cout << "\tResidual = " << best_residual << std::endl;
#endif
}
#if GNSS_UTILS_TDCP_DEBUG == 1
......@@ -425,6 +422,9 @@ bool Tdcp(SnapshotPtr snapshot_r,
std::cout << "\tSolution = " << d.transpose() << " (" << d.head<3>().norm() << "m)" << std::endl;
std::cout << "\tClock offset = " << d(3) << std::endl;
std::cout << "\tResidual = " << best_residual << std::endl;
std::cout << "\tA = \n" << A << std::endl;
std::cout << "\tH = \n" << A.transpose() * A << std::endl;
std::cout << "\tcov_d = \n" << cov_d << std::endl;
#endif
}
......@@ -437,6 +437,9 @@ bool Tdcp(SnapshotPtr snapshot_r,
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 << "\tA = \n" << A << std::endl;
std::cout << "\tH = \n" << A.transpose() * A << std::endl;
std::cout << "\tcov_d = \n" << cov_d << 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