From ace123175a50f33bebe5507fbfe3de414bfa8576 Mon Sep 17 00:00:00 2001
From: Yann Meneroux <yann.meneroux@ign.fr>
Date: Tue, 21 Jun 2022 19:36:35 +0200
Subject: [PATCH] GNSS solve ambiguities on ref point

---
 src/Yann/ExcludeSats.cpp     | 36 ++++++++++++++++------------
 src/Yann/gnss/Algorithms.cpp | 46 ++++++++++++++++++++++++++++++++++++
 src/Yann/gnss/Algorithms.h   |  1 +
 3 files changed, 68 insertions(+), 15 deletions(-)

diff --git a/src/Yann/ExcludeSats.cpp b/src/Yann/ExcludeSats.cpp
index 2d5b67cc09..51ccc5bae6 100755
--- a/src/Yann/ExcludeSats.cpp
+++ b/src/Yann/ExcludeSats.cpp
@@ -1092,6 +1092,9 @@ cAppli_YannSkyMask::cAppli_YannSkyMask(int argc, char ** argv){
 		// Down-grading h5py to earlier version
 		System("pip install 'h5py==2.10.0' --force-reinstall");
 		
+		// Down-grading protobuf version
+		System("pip install 'protobuf~=3.19.0'");
+		
 		return;
 		
 	}
@@ -1188,25 +1191,28 @@ cAppli_YannScript::cAppli_YannScript(int argc, char ** argv){
         LArgMain()  <<  EAM(mOut,"Ref", "file.o", "Rinex base station observation file"));
 	
 	
-	ObservationData rover = RinexReader::readObsFile(ImPattern);
-    ObservationData base = RinexReader::readObsFile(mOut);
+	ObservationData rov = RinexReader::readObsFile(ImPattern);
+    ObservationData bas = RinexReader::readObsFile(mOut);
     NavigationData nav  = RinexReader::readNavFile(aPostIn);
 
-    rover.removeSatellite("G24");
-    base.removeSatellite("G24");
-    base.removeSatellite("G10");
-    base.removeSatellite("G19");
-
-    NavigationDataSet eph = NavigationDataSet();
-    eph.addGpsEphemeris(nav);
-
-    Solution solution = Algorithms::triple_difference_kalman(rover, base, eph, base.getApproxAntennaPosition());
+	ObservationSlot slot = rov.getObservationSlots().at(0);
+	GPSTime time = slot.getTimestamp();
+	
+	std::vector<std::string> SATS = slot.getSatellites();
+	
+	std::string sat1 = "G16";
 
-    std::cout << "------------------------------------------------------------------------------" << std::endl;
-    std::cout << rover.getApproxAntennaPosition() << std::endl;
-    std::cout << solution.getPosition() - rover.getApproxAntennaPosition() << std::endl;
-    std::cout << "------------------------------------------------------------------------------" << std::endl;
 
+	std::cout << std::endl;
+	std::cout << "-----------------------------" << std::endl;
+	
+	for (unsigned i=0; i<SATS.size(); i++){
+		std::string sat2 = SATS.at(i);
+		double N = Algorithms::solve_ambiguity_ref(rov, bas, nav, time, sat1, sat2);
+		std::cout << "DD " << sat1 << "-" << sat2 << "     " << N << std::endl;	
+	}
+	
+	std::cout << "-----------------------------" << std::endl;
 
 	/*
 	std::string prn;
diff --git a/src/Yann/gnss/Algorithms.cpp b/src/Yann/gnss/Algorithms.cpp
index bcfbd44c77..9eb7b5f54a 100755
--- a/src/Yann/gnss/Algorithms.cpp
+++ b/src/Yann/gnss/Algorithms.cpp
@@ -880,3 +880,49 @@ Solution Algorithms::triple_difference_kalman(ObservationData rover,
     return solution;
 
 }
+
+// -------------------------------------------------------------------------------
+// Algorithme de résolution des ambiüités sur une époque où la position de la
+// station mobile (et de la station de base) sont connues exactement.
+// -------------------------------------------------------------------------------
+double Algorithms::solve_ambiguity_ref(ObservationData rov, ObservationData bas, NavigationData nav, GPSTime time, std::string sat1, std::string sat2){
+
+
+    double L1 = Utils::C/L1_FREQ;
+
+    ECEFCoords pos_r = rov.getApproxAntennaPosition();
+    ECEFCoords pos_b = bas.getApproxAntennaPosition();
+
+    ObservationSlot slot_b = bas.lookForEpoch(time);
+    ObservationSlot slot_r = rov.lookForEpoch(time);
+
+    double diff_b = slot_b.getTimestamp()-time;
+    double diff_r = slot_r.getTimestamp()-time;
+
+    if (diff_b != 0){
+        std::cout << "Warning: base station not synchronized. Time diff = " << diff_b*1e6 << " us" << std::endl;
+    }
+    if (diff_r != 0){
+        std::cout << "Warning: rover station not synchronized. Time diff = " << diff_b*1e6 << " us" << std::endl;
+    }
+
+    // Observations
+    double C1b = slot_b.getObservation(sat1).getC1(); double L1b = slot_b.getObservation(sat1).getL1();
+    double C2b = slot_b.getObservation(sat2).getC1(); double L2b = slot_b.getObservation(sat2).getL1();
+    double C1r = slot_r.getObservation(sat1).getC1(); double L1r = slot_r.getObservation(sat1).getL1();
+    double C2r = slot_r.getObservation(sat2).getC1(); double L2r = slot_r.getObservation(sat2).getL1();
+
+    // Positions et distances théoriques
+    ECEFCoords pos_sat1b = nav.computeSatellitePos(sat1, slot_b.getTimestamp(), C1b); double D1b = pos_b.distanceTo(pos_sat1b);
+    ECEFCoords pos_sat2b = nav.computeSatellitePos(sat2, slot_b.getTimestamp(), C2b); double D2b = pos_b.distanceTo(pos_sat2b);
+    ECEFCoords pos_sat1r = nav.computeSatellitePos(sat1, slot_r.getTimestamp(), C1r); double D1r = pos_r.distanceTo(pos_sat1r);
+    ECEFCoords pos_sat2r = nav.computeSatellitePos(sat2, slot_r.getTimestamp(), C2r); double D2r = pos_r.distanceTo(pos_sat2r);
+
+    // Doubles différences observées et théoriques
+    double dd_obs = (L1b - L2b) - (L1r - L2r);
+    double dd_thq = ((D1b - D2b) - (D1r - D2r))/L1;
+
+    // Résolution de l'ambigüité entière
+    return  dd_obs - dd_thq;
+
+}
diff --git a/src/Yann/gnss/Algorithms.h b/src/Yann/gnss/Algorithms.h
index 54414a188b..3f23be1fdc 100755
--- a/src/Yann/gnss/Algorithms.h
+++ b/src/Yann/gnss/Algorithms.h
@@ -45,6 +45,7 @@ class Algorithms{
         static Trajectory estimateTrajectory(ObservationData, NavigationData);
 	
 		// Calcul de la position par la phase
+		static double solve_ambiguity_ref(ObservationData rov, ObservationData bas, NavigationData nav, GPSTime time, std::string sat1, std::string sat2);
         static Solution triple_difference_kalman(ObservationData, ObservationData, NavigationDataSet, ECEFCoords);
 		static ElMatrix<REAL> makeTripleDifferenceMatrix(int, int);