From e3b7e7124db8fbb490ce54ad08b9467236897818 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 26 Oct 2022 16:46:13 +0200
Subject: [PATCH] ouptut covariance on clock bias
---
include/gnss_utils/gnss_utils.h | 20 ++++++++++----------
src/utils/rcv_position.cpp | 23 ++++++++++++-----------
2 files changed, 22 insertions(+), 21 deletions(-)
diff --git a/include/gnss_utils/gnss_utils.h b/include/gnss_utils/gnss_utils.h
index c00bf50..f00d1be 100644
--- a/include/gnss_utils/gnss_utils.h
+++ b/include/gnss_utils/gnss_utils.h
@@ -52,16 +52,16 @@ struct ComputePosOutput
{
time_t time;
double sec;
- Eigen::Vector3d pos; // position (m)
- Eigen::Vector3d vel; // velocity (m/s)
- Eigen::Matrix3d pos_covar; // position covariance (m^2)
- // {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx}
- Eigen::VectorXd rcv_bias; // receiver clock bias to time systems (s) 0: receiver clock bias (s), 1: glo-gps time offset (s), 2: gal-gps time offset (s), 3: bds-gps time offset (s)
- int type; // coordinates used (0:xyz-ecef,1:enu-baseline)
- int stat; // solution status (SOLQ_???)
- int ns; // number of valid satellites
- double age; // age of differential (s)
- double ratio; // AR ratio factor for valiation
+ Eigen::Vector3d pos; // position (m)
+ Eigen::Vector3d vel; // velocity (m/s)
+ Eigen::Matrix4d sol_cov; // solution covariance [x, y, z, dt] (m², s²)
+ Eigen::Vector4d rcv_bias; // receiver clock bias to time systems (s) 0: receiver clock bias (s), 1: glo-gps time offset (s), 2: gal-gps time offset (s), 3: bds-gps time offset (s)
+ Eigen::Vector4d rcv_bias_var; // receiver clock bias variances 0: receiver clock bias (s), 1: glo-gps time offset (s), 2: gal-gps time offset (s), 3: bds-gps time offset (s)
+ int type; // coordinates used (0:xyz-ecef,1:enu-baseline)
+ int stat; // solution status (SOLQ_???)
+ int ns; // number of valid satellites
+ double age; // age of differential (s)
+ double ratio; // AR ratio factor for valiation
bool success; // return from pntpos true/false
std::string msg; // message returned (in case of error)
diff --git a/src/utils/rcv_position.cpp b/src/utils/rcv_position.cpp
index b354333..77e66b7 100644
--- a/src/utils/rcv_position.cpp
+++ b/src/utils/rcv_position.cpp
@@ -72,20 +72,21 @@ ComputePosOutput computePos(const GnssUtils::Observations& _observations,
output.sec = sol.time.sec;
output.pos = Eigen::Vector3d(sol.rr);
output.vel = Eigen::Vector3d(&sol.rr[3]);
- output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5], sol.qr[3], sol.qr[1], sol.qr[4], sol.qr[5], sol.qr[4], sol.qr[2];
+ output.sol_cov << sol.qr[0], sol.qr[3], sol.qr[5], sol.qdtr[4],
+ sol.qr[3], sol.qr[1], sol.qr[4], sol.qdtr[5],
+ sol.qr[5], sol.qr[4], sol.qr[2], sol.qdtr[6],
+ sol.qdtr[4], sol.qdtr[5], sol.qdtr[6], sol.qdtr[0];
// std::cout << "Compute pos: " << output.pos.transpose() << "\n";
// std::cout << "Covariance:\n" << output.pos_covar << "\n";
- if (sol.dtr != NULL)
- output.rcv_bias =
- (Eigen::Matrix<double, 6, 1>() << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5])
- .finished();
- output.type = sol.type;
- output.stat = sol.stat;
- output.ns = sol.ns;
- output.age = sol.age;
- output.ratio = sol.ratio;
- output.lat_lon = ecefToLatLonAlt(output.pos);
+ output.rcv_bias = Eigen::Vector4d(sol.dtr); // all 4 elements
+ output.rcv_bias_var << sol.qdtr[0], sol.qdtr[1], sol.qdtr[2], sol.qdtr[3]; // cannot use constructor from reference because it is float)
+ output.type = sol.type;
+ output.stat = sol.stat;
+ output.ns = sol.ns;
+ output.age = sol.age;
+ output.ratio = sol.ratio;
+ output.lat_lon = ecefToLatLonAlt(output.pos);
for (auto i = 0; i < _observations.size(); i++)
output.sat_azel.emplace(_observations.getObservationByIdx(i).sat,
--
GitLab