From ecf99a0e8122710d77f7f31b43174b107c8f909d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Mon, 5 Jul 2021 12:46:22 +0200
Subject: [PATCH] sagnac 2
---
include/gnss_utils/tdcp.h | 2 +-
src/tdcp.cpp | 35 +++++++++++++++++++++++++++++------
2 files changed, 30 insertions(+), 7 deletions(-)
diff --git a/include/gnss_utils/tdcp.h b/include/gnss_utils/tdcp.h
index 6cd6cde..818fe3f 100644
--- a/include/gnss_utils/tdcp.h
+++ b/include/gnss_utils/tdcp.h
@@ -54,7 +54,7 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
Eigen::MatrixXd& A,
Eigen::VectorXd& r,
- const Eigen::VectorXd& drho_m,
+ Eigen::VectorXd& drho_m,
const Eigen::MatrixXd& s_k,
const Eigen::MatrixXd& s_r,
const Eigen::Vector4d& d_0,
diff --git a/src/tdcp.cpp b/src/tdcp.cpp
index e2e48d9..44b0f24 100644
--- a/src/tdcp.cpp
+++ b/src/tdcp.cpp
@@ -221,12 +221,9 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
if (tdcp_params.sagnac_correction != 0)
{
double sagnac_corr_r = computeSagnacCorrection(x_r,s_r.col(row)); //OMGE*(s_r.col(row)(0)*x_r(1)-s_r.col(row)(1)*x_r(0))/CLIGHT;
- double sagnac_corr_k = computeSagnacCorrection(x_r,s_k.col(row)); //OMGE*(s_k.col(row)(0)*(x_r(1)+d_0(1))-s_k.col(row)(1)*(x_r(0)+d_0(0)))/CLIGHT;
+ double sagnac_corr_k = computeSagnacCorrection(x_r+d_0.head(3),s_k.col(row)); //OMGE*(s_k.col(row)(0)*(x_r(1)+d_0(1))-s_k.col(row)(1)*(x_r(0)+d_0(0)))/CLIGHT;
- if (tdcp_params.sagnac_correction == 1)
- drho_m(row) += -sagnac_corr_k + sagnac_corr_r;
- else
- drho_m(row) -= -sagnac_corr_k + sagnac_corr_r;
+ drho_m(row) += -sagnac_corr_k + sagnac_corr_r;
}
#if GNSS_UTILS_TDCP_DEBUG == 1
@@ -275,7 +272,7 @@ TdcpOutput Tdcp(SnapshotPtr snapshot_r,
TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
Eigen::MatrixXd& A,
Eigen::VectorXd& r,
- const Eigen::VectorXd& drho_m,
+ Eigen::VectorXd& drho_m,
const Eigen::MatrixXd& s_k,
const Eigen::MatrixXd& s_r,
const Eigen::Vector4d& d_0,
@@ -296,6 +293,17 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
Eigen::Vector4d delta_d(Eigen::Vector4d::Zero());
d = d_0;
+ // initial sagnac correction
+ Eigen::VectorXd prev_sagnac_corr(s_k.cols());
+ if (tdcp_params.sagnac_correction == 2 )
+ for (int row = 0; row < A.rows(); row++)
+ {
+ double sagnac_corr_r = computeSagnacCorrection(x_r,s_r.col(row)); //OMGE*(s_r.col(row)(0)*x_r(1)-s_r.col(row)(1)*x_r(0))/CLIGHT;
+ double sagnac_corr_k = computeSagnacCorrection(x_r+d_0.head(3),s_k.col(row)); //OMGE*(s_k.col(row)(0)*(x_r(1)+d_0(1))-s_k.col(row)(1)*(x_r(0)+d_0(0)))/CLIGHT;
+
+ prev_sagnac_corr(row) = -sagnac_corr_k + sagnac_corr_r;
+ }
+
for (int j = 0; j < tdcp_params.max_iterations; j++)
{
// fill A and r
@@ -305,6 +313,21 @@ TdcpOutput Tdcp(const Eigen::Vector3d& x_r,
if (raim_discarded_rows.count(row) != 0)
continue;
+ // update sagnac correction
+ if (tdcp_params.sagnac_correction == 2 and j > 0)
+ {
+ // undo prev_correction
+ drho_m(row) -= prev_sagnac_corr(row);
+
+ // apply new correction
+ double sagnac_corr_r = computeSagnacCorrection(x_r,s_r.col(row)); //OMGE*(s_r.col(row)(0)*x_r(1)-s_r.col(row)(1)*x_r(0))/CLIGHT;
+ double sagnac_corr_k = computeSagnacCorrection(x_r+d.head(3),s_k.col(row)); //OMGE*(s_k.col(row)(0)*(x_r(1)+d_0(1))-s_k.col(row)(1)*(x_r(0)+d_0(0)))/CLIGHT;
+ drho_m(row) += -sagnac_corr_k + sagnac_corr_r;
+
+ // store correction
+ prev_sagnac_corr(row) = -sagnac_corr_k + sagnac_corr_r;
+ }
+
// Evaluate r
r(row) = drho_m(row) - (s_k.col(row) - x_r - d.head(3)).norm() + (s_r.col(row) - x_r).norm() - d(3);
--
GitLab