From c33c62d5a6a7412ce10943d7df7f8bcc2a9c40fa Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 23 Jun 2020 20:46:42 +0200
Subject: [PATCH] added batch to tdcp params
---
include/gnss_utils/gnss_utils.h | 5 +--
include/gnss_utils/tdcp.h | 2 +-
src/tdcp.cpp | 57 +++++++++++++++++----------------
3 files changed, 34 insertions(+), 30 deletions(-)
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index 66228d6..16dbcce 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -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
diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index 7885eb4..143fa67 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -1,7 +1,7 @@
#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"
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index 783f738..1bea6da 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -1,3 +1,4 @@
+#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
}
--
GitLab