backup
authorBenoit Papillault <benoit.papillault@free.fr>
Thu, 29 Dec 2011 07:49:42 +0000 (08:49 +0100)
committerBenoit Papillault <benoit.papillault@free.fr>
Thu, 29 Dec 2011 07:49:42 +0000 (08:49 +0100)
Makefile
gpstk-solution2.cpp
gpstk-solution6.cpp
position.cpp [new file with mode: 0644]
sirf2.cpp

index af8253f..943c10a 100644 (file)
--- a/Makefile
+++ b/Makefile
@@ -10,7 +10,8 @@ CPPFLAGS      += -Wall -g
 LDLIBS         += -lgps
 
 # for gpstk
-GPSTK_HOME     = $(HOME)/gpstk
+#GPSTK_HOME    = $(HOME)/gpstk
+GPSTK_HOME     = /usr/local
 CPPFLAGS       += -I$(GPSTK_HOME)/include/gpstk/
 LDFLAGS                += -L$(GPSTK_HOME)/lib
 LDLIBS         += -lprocframe -lgpstk
index 782b41d..5335e0a 100644 (file)
@@ -96,7 +96,7 @@ int main() {
                SimpleFilter filter;
 
                /* FIXME : what is the role of nominalPos ? */
-               Position nominalPos(4205741.0, 166208.0, 4776301.0);
+               Position nominalPos(4205000.0, 166000.0, 4776000.0);
 
                double ionAlpha[4] = { 0.0 , 0.0, 0.0, 0.0 };
                double ionBeta[4]  = { 149500.0, -131100.0, 0.0, -131100.0 };
@@ -109,7 +109,7 @@ int main() {
 
                MOPSTropModel mopsTM( nominalPos.getAltitude(),
                                      nominalPos.getGeodeticLatitude(),
-                                     30 );
+                                     dayTime.DOYday());
 
 
                ModelObs model(/* IonoModelStore */ ionoStore,
index 9180f88..e80d6c2 100644 (file)
@@ -6,6 +6,8 @@
 #include <iostream>
 #include <iomanip>
 
+#include <math.h>
+
 // Class for handling satellite observation parameters RINEX files
 #include "RinexObsStream.hpp"
 
 using namespace std;
 using namespace gpstk;
 
+// various global initialisation
+
+// mobile station nominal position
+//Position nominalPos_true(4201577.209,        189859.856,     4779064.567);
+//Position nominalPos_true(48.803125, 2.263235, 20, Position::Geodetic);
+Position nominalPos_true(48.803403, 2.263000, 133.0, Position::Geodetic);
+
 // class for weight 
 
 class ComputeElevationWeights : public WeightBase, public ProcessingClass
@@ -115,9 +124,56 @@ public:
        };
 };
 
+class ComputeSNRWeights : public WeightBase, public ProcessingClass
+{
+public:
+       ComputeSNRWeights() {};
+
+       double getWeight(const SatID& sat,
+                        typeValueMap& tvMap) {
+               double snr = tvMap(TypeID::S1);
+               return snr;
+       };
+
+       virtual satTypeValueMap& Process(const DayTime& time,
+                                        satTypeValueMap& gData) {
+               
+               // Loop through all the satellites
+               satTypeValueMap::iterator it;
+               for (it = gData.begin(); it != gData.end(); ++it ) {
+                       (*it).second[TypeID::weight] =
+                               getWeight((*it).first, (*it).second);
+               }
+
+               return gData;
+       };
+
+       virtual gnssSatTypeValue& Process(gnssSatTypeValue& gData) {
+               Process(gData.header.epoch, gData.body); return gData;
+       };
+
+       virtual gnssRinex& Process(gnssRinex& gData) {
+               Process(gData.header.epoch, gData.body); return gData;
+       };
+
+       virtual int getIndex(void) const {
+               return 7; /* FIXME */
+       };
+
+       virtual string getClassName(void) const {
+               return "ComputeSNRWeights";
+       };
+};
+
 void display_rinex(const gnssRinex& gRin) {
+       //      cout << gRin.header.epoch
+       //           << " " << gRin.header.epoch.second() << endl;
        cout << gRin.header.epoch
-            << " " << gRin.header.epoch.second() << endl;
+            << " GPS week " << gRin.header.epoch.GPSfullweek()
+            << setprecision(9)
+            << " GPS seconds " << gRin.header.epoch.GPSsow()
+            << setprecision(6)
+            << endl;
        gRin.body.dump(cout, 1);
 }
 
@@ -125,9 +181,9 @@ void display_position(const Position& p,
                      const Position& nominalPos) {
        Position pos = p;
 
-       double dx = pos.asECEF().X() - nominalPos.X();
-       double dy = pos.asECEF().Y() - nominalPos.Y();
-       double dz = pos.asECEF().Z() - nominalPos.Z();
+       double dx = pos.X() - nominalPos.X();
+       double dy = pos.Y() - nominalPos.Y();
+       double dz = pos.Z() - nominalPos.Z();
        double distance = sqrt(dx*dx+dy*dy+dz*dz);
 
        cout << " X = " << pos.asECEF().X()
@@ -140,6 +196,63 @@ void display_position(const Position& p,
             << endl;
 }
 
+void display_solution(const gnssRinex& gData,
+                     const ModelObsFixedStation& model,
+                     const SolverLMS& solver) {
+
+       static double sumX = 0;
+       static double sumY = 0;
+       static double sumZ = 0;
+       static int count   = 0;
+
+       Position pos(model.rxPos.X()+solver.getSolution(TypeID::dx),
+                    model.rxPos.Y()+solver.getSolution(TypeID::dy),
+                    model.rxPos.Z()+solver.getSolution(TypeID::dz));
+
+       DayTime T = gData.header.epoch - solver.getSolution(TypeID::cdt) / C_GPS_M;
+
+       double dx = pos.X() - nominalPos_true.X();
+       double dy = pos.Y() - nominalPos_true.Y();
+       double dz = pos.Z() - nominalPos_true.Z();
+       double distance = sqrt(dx*dx+dy*dy+dz*dz);
+       double altitude = pos.asGeodetic().getAltitude();
+
+       cout << setprecision(1)
+            << " X= " << pos.X()
+            << " Y= " << pos.Y()
+            << " Z= " << pos.Z()
+            << setprecision(9)
+            << " T= " << T.GPSfullweek() << "/" << T.GPSsecond()
+            << setprecision(6)
+            << " lon= " << pos.asGeodetic().getLongitude()
+            << " lat= " << pos.asGeodetic().geodeticLatitude()
+            << setprecision(0)
+            << " alt= " << altitude
+            << " cdt= " << solver.getSolution(TypeID::cdt)
+            << setprecision(9)
+            << " (" << solver.getSolution(TypeID::cdt) / C_GPS_M << ")"
+            << setprecision(6)
+            << " d= " << distance              
+            << endl;
+
+
+       if ((0<=altitude) && (altitude<=300)) {
+               sumX += pos.X();
+               sumY += pos.Y();
+               sumZ += pos.Z();
+               count ++;
+
+               if (count > 10) {
+                       cout << "adjusting nominalPos_true" << endl;
+
+                       nominalPos_true = Position(sumX/count,
+                                                  sumY/count,
+                                                  sumZ/count,
+                                                  Position::Cartesian);
+               }
+       }
+}
+
 /* keep only GPS satellites (removing Glonass for instance, ... )*/
 gnssRinex keepOnlySatGPS(const gnssRinex& rin) {
        gnssRinex result;
@@ -165,18 +278,15 @@ int main(int argc, const char *argv[])
 
        // second set : from rgpdata.ign.fr:/pub/data/2011/361/data_1
        const char obsReference[] = "crei361o.11o";
-       const char obsMobile[] = "4.ooo";
-       const char navReference[] = "crei361o.11n";
+       const char obsMobile[] = "out6.ooo";
+       const char navReference[] = "brdc363z.11n";
        const char sp3File[] = "igu16682_12.sp3";
        
        // reference station nominal position
        Position nominalPosRef  (4166409.705,   182770.997,     4809802.953);
-       // mobile station nominal position
-       Position nominalPos_true(4201577.209,   189859.856,     4779064.567);   
-       Position nominalPos     (4201000.000,   189000.000,     4770000.000);   
-
        // initial set to nominalPos
-       Position lastPosition = nominalPos;
+       Position lastPosition = nominalPosRef;
+       bool lastPositionFound = false;
 
        ////////// Initialization phase //////////
 
@@ -226,6 +336,7 @@ int main(int argc, const char *argv[])
        // Declare a simple filter object. By default, it filters C1 with
        // default limits
        SimpleFilter filter;
+       filter.setMaxLimit(100000000.0);
 
        // This is the GNSS data structure that will hold all the
        // GNSS-related information
@@ -281,16 +392,6 @@ int main(int argc, const char *argv[])
 
       ////////////////////////////////////////
 
-
-      //////////// CASE #9 OBJECTS ////////////
-
-      // Declare a new Kalman solver
-   CodeKalmanSolver solverK9;
-
-      ////////////////////////////////////////
-
-
-
       //////////// CASE #10 OBJECTS ////////////
 
       // This is the GNSS data structure that will hold the
@@ -319,8 +420,8 @@ int main(int argc, const char *argv[])
    ModelObs model;
    model.setDefaultIonoModel(ionoStore);
    model.setDefaultTropoModel(mopsTM);
-   model.setDefaultEphemeris(sp3Store /*bceStore*/);
-   model.setMinElev(1.0);
+   model.setDefaultEphemeris(/*sp3Store*/bceStore);
+   //model.setMinElev(1.0);
 
       // Declare the appropriate modeler object for a reference station
    //ModelObsFixedStation modelRef(nominalPosRef,
@@ -330,7 +431,7 @@ int main(int argc, const char *argv[])
    ModelObs modelRef;
    modelRef.setDefaultIonoModel(ionoStore);
    modelRef.setDefaultTropoModel(mopsTMRef);
-   modelRef.setDefaultEphemeris(sp3Store /*bceStore*/);
+   modelRef.setDefaultEphemeris(/*sp3Store*/ bceStore);
    modelRef.Prepare(nominalPosRef);
 
    // use simple model without iono & tropo corrections
@@ -356,6 +457,7 @@ int main(int argc, const char *argv[])
    CodeKalmanSolver solverK12;
 
    ComputeElevationWeights elevationW;
+   ComputeSNRWeights snrW;
 
       ////////////////////////////////////////
 
@@ -378,30 +480,52 @@ int main(int argc, const char *argv[])
       // Loop over all data epochs
    while (rin >> gOriginal) {
 
-          // remove glonass satellite
-          gOriginal = keepOnlySatGPS(gOriginal);
-          gOriginal.keepOnlyTypeID(TypeID::C1);
-
-          // shift pseudorange by 37900000 m
-          /*
-          for (satTypeValueMap::iterator it = gOriginal.body.begin();
-               it != gOriginal.body.end(); it++) {
-                  (*it).second[TypeID::C1] -= 37900000.0;
-          }
-          */
           cout << "--- " << ++i << " ---" << endl;
           cout << "  Ref.   :";
           display_position(nominalPosRef, nominalPos_true);
           cout << "  Mobile :";
           display_position(nominalPos_true, nominalPos_true);
-          cout << "  Approx :";
-          display_position(nominalPos, nominalPos_true);
-          cout << "  Last   :";
+          cout << "  Last " << lastPositionFound << " :";
           display_position(lastPosition, nominalPos_true);
-
           cout << "gOriginal : " << endl;
           display_rinex(gOriginal);
 
+          // keep only GPS satellites (removing GLONASS)
+          gOriginal = keepOnlySatGPS(gOriginal);
+
+          // keep only C1 & S1 (since that what we can record now)
+          TypeIDSet set;
+          set.insert(TypeID::C1);
+          set.insert(TypeID::S1);
+          gOriginal.keepOnlyTypeID(set);
+
+          // adjust time to the nearest millisecond
+          /*
+          DayTime old_t = gOriginal.header.epoch;
+          double clock_bias = old_t.GPSsecond() - floor(1.0*old_t.GPSsecond())/1.0;
+          DayTime new_t = old_t - clock_bias;
+          gOriginal.header.epoch = new_t;
+          printf("  clock_bias %.9f\n", clock_bias);
+          */
+
+          // adjust pseudorange
+          /*
+          double clock_bias = 0.001;
+          for (satTypeValueMap::iterator it = gOriginal.body.begin();
+               it != gOriginal.body.end(); ++it) {
+                  double old_val = (*it).second[TypeID::C1];
+                  double new_val = old_val - clock_bias * C_GPS_M;
+                  cout << (*it).first << " adjusted " << old_val << " to " << new_val << endl;
+                  (*it).second[TypeID::C1] = new_val;
+          }
+          */
+
+          // based on SimpleFilter.hpp and Bancroft.hpp
+          // pseudorange should be within [15000000 ... 30000000]
+
+          cout << "gOriginal(adjust):" << endl;
+          display_rinex(gOriginal);
+
           // update MOPSTropModel
           mopsTMRef.setReceiverLatitude(nominalPosRef.getGeodeticLatitude());
           mopsTMRef.setReceiverHeight(nominalPosRef.getAltitude());
@@ -413,31 +537,50 @@ int main(int argc, const char *argv[])
 
           // update mopsW
           mopsW.setPosition(lastPosition);
-          mopsW.setDefaultEphemeris(sp3Store /*bceStore*/);
+          mopsW.setDefaultEphemeris(/*sp3Store*/bceStore);
 
-          myModelRef.setDefaultEphemeris(sp3Store /*bceStore*/);
-          myModel.setDefaultEphemeris(sp3Store /*bceStore*/);
+          myModelRef.setDefaultEphemeris(/*sp3Store*/ bceStore);
+          myModel.setDefaultEphemeris(/*sp3Store*/bceStore);
    
-          model.Prepare(lastPosition);
           myModelRef.Prepare(nominalPosRef);
-          myModel.Prepare(lastPosition);
+          if (lastPositionFound) {
+                  model.Prepare(lastPosition);
+                  myModel.Prepare(lastPosition);
+          } else {
+                  model.setModelPrepared(false);
+                  myModel.setModelPrepared(false);
+                  //model.Prepare(nominalPosRef);
+                  //myModel.Prepare(nominalPosRef);
+          }
 
           // CASE #0
           // We are using LMS + C1 + no tropo/iono model
           gnssRinex gRin0(gOriginal);
           try {
-                  gRin0 >> filter >> myModel;
-                  cout << "gRin0 : " << endl;
+                  myModel.setModelPrepared(false);
+
+                  gRin0 >> filter;
+                  //cout << "gRin0(filter) : " << endl;
+                  //display_rinex(gRin0);
+                  gRin0 >> myModel;
+                  cout << "gRin0(myModel) : " << endl;
                   display_rinex(gRin0);
                   gRin0 >> solver;
-                  display_rinex(gRin0);
+                  //cout << "gRin0(solver) : " << endl;
+                  //display_rinex(gRin0);
 
           Position pos(myModel.rxPos.X()+solver.getSolution(TypeID::dx),
                        myModel.rxPos.Y()+solver.getSolution(TypeID::dy),
                        myModel.rxPos.Z()+solver.getSolution(TypeID::dz));
              
-                  cout << "  Case 0 :";
-                  display_position(pos, nominalPos_true);
+          //cout << "  Case 0 :";
+                  //display_solution(gRin0, myModel, solver);
+
+                  double altitude = pos.asGeodetic().getAltitude();
+                  if ((0.0 < altitude) && (altitude < 300.0))  {
+                          lastPosition = pos;
+                          lastPositionFound = true;
+                  }
           }
           catch (Exception e) {
                   cout << "Case 0. Exception : " << e.what() << endl;
@@ -467,8 +610,8 @@ int main(int argc, const char *argv[])
                           model.rxPos.Y() + solver.getSolution(TypeID::dy),
                           model.rxPos.Z() + solver.getSolution(TypeID::dz));
              
-             cout << "  Case 1 :";
-             display_position(pos, nominalPos_true);
+             //cout << "  Case 1 :";
+             //display_solution(gRin1, model, solver);
       }
       catch(...)
       {
@@ -499,10 +642,12 @@ int main(int argc, const char *argv[])
                           (model.rxPos.Z()+solverWMS.getSolution(TypeID::dz)));
              
              cout << "  Case 3 :";
-             display_position(pos, nominalPos_true);
+             display_solution(gRin3, model, solverWMS);
              double altitude = pos.asGeodetic().getAltitude();
-             if ((0.0 < altitude) && (altitude < 300.0))
+             if ((0.0 < altitude) && (altitude < 300.0))  {
                      lastPosition = pos;
+                     lastPositionFound = true;
+             }
       }
       catch (Exception e)
       {
@@ -522,8 +667,22 @@ int main(int argc, const char *argv[])
                           (model.rxPos.Y()+solverWMS.getSolution(TypeID::dy)),
                           (model.rxPos.Z()+solverWMS.getSolution(TypeID::dz)));
              
-             cout << "  Case 4 :";
-             display_position(pos, nominalPos_true);
+             //cout << "  Case 4 :";
+             //display_solution(gRin4, model, solverWMS);
+      }
+      catch (Exception e)
+      {
+             cout << "Exception : " << e.what() << endl;
+      }
+
+
+      // Case #5 : use SNR as weight
+      gnssRinex gRin5(gOriginal);
+
+      try  {
+             gRin5 >> filter >> model >> snrW >> solverWMS;
+             //cout << "  Case 5 :";
+             //display_solution(gRin5, model, solverWMS);
       }
       catch (Exception e)
       {
@@ -584,8 +743,9 @@ at epoch: " << gRef.header.epoch << endl;
                           (myModel.rxPos.Z()+solver.getSolution(TypeID::dz)));
              
              cout << "  Case 10:";
-             display_position(pos, nominalPos_true);
+             display_solution(gRin10, myModel, solver);
              lastPosition = pos;
+             lastPositionFound = true;
       }
       catch(...)
       {
@@ -612,14 +772,11 @@ at epoch: " << gRef.header.epoch << endl;
          // obtained from Case #10.
 
     try {
-           gRin11 >> filter >> myModel >> delta >> mopsW  >> solverWMS;
            // Like case #10, but now with "mopsW" and "solverWMS"
-           Position pos(myModel.rxPos.X()+solverWMS.getSolution(TypeID::dx),
-                        myModel.rxPos.Y()+solverWMS.getSolution(TypeID::dy),
-                        myModel.rxPos.Z()+solverWMS.getSolution(TypeID::dz));
-           
+           gRin11 >> filter >> myModel >> delta >> mopsW  >> solverWMS;
            cout << "  Case 11:";
-           display_position(pos, nominalPos_true);
+           //display_position(pos, nominalPos_true);
+           display_solution(gRin11, myModel, solverWMS);
       }
       catch(...)
       {
@@ -650,17 +807,8 @@ at epoch: " << gRef.header.epoch << endl;
              if (i > 1) {
                      gRin12 >> filter >> myModel >> delta
                             >> mopsW  >> solverK12;
-            // Like case #11, but now with "solverK12"
-            // VERY IMPORTANT: Note that in this case the coordinates are
-            // handled as constants, whereas the receiver clock is modeled as
-            // white noise.
-
-             Position pos(myModel.rxPos.X()+solverK12.getSolution(TypeID::dx),
-                          myModel.rxPos.Y()+solverK12.getSolution(TypeID::dy),
-                          myModel.rxPos.Z()+solverK12.getSolution(TypeID::dz));
-
-             cout << "  Case 12:";
-             display_position(pos, nominalPos_true);
+                     cout << "  Case 12:";
+                     display_solution(gRin12, myModel, solverK12);
              }
       }
       catch(...)
@@ -678,13 +826,8 @@ at epoch: " << gRef.header.epoch << endl;
 
       try {
              gRin13 >> filter >> myModel >> delta >> elevationW >> solverWMS;
-
-             Position pos(myModel.rxPos.X()+solverWMS.getSolution(TypeID::dx),
-                          myModel.rxPos.Y()+solverWMS.getSolution(TypeID::dy),
-                          myModel.rxPos.Z()+solverWMS.getSolution(TypeID::dz));
-           
              cout << "  Case 13:";
-             display_position(pos, nominalPos_true);
+             display_solution(gRin13, myModel, solverWMS);
       }
       catch(...)
       {
@@ -697,13 +840,8 @@ at epoch: " << gRef.header.epoch << endl;
 
       try {
              gRin14 >> filter >> model >> delta >> mopsW >> solverWMS;
-
-             Position pos(model.rxPos.X()+solverWMS.getSolution(TypeID::dx),
-                          model.rxPos.Y()+solverWMS.getSolution(TypeID::dy),
-                          model.rxPos.Z()+solverWMS.getSolution(TypeID::dz));
-           
              cout << "  Case 14:";
-             display_position(pos, nominalPos_true);
+             display_solution(gRin14, model, solverWMS);
       }
       catch(...)
       {
diff --git a/position.cpp b/position.cpp
new file mode 100644 (file)
index 0000000..f65d00f
--- /dev/null
@@ -0,0 +1,23 @@
+#include <iostream>
+#include "Position.hpp"
+
+using namespace std;
+using namespace gpstk;
+
+int main() {
+
+       /* x,y,z,Position::Cartesian
+          lat,lon,alt,Position::Geodetic
+       */
+       Position pos(2.26, ....);
+
+       cout << " X = " << pos.asECEF().X()
+            << " Y = " << pos.asECEF().Y()
+            << " Z = " << pos.asECEF().Z()
+            << " lon = " << pos.asGeodetic().getLongitude()
+            << " lat = " << pos.asGeodetic().geodeticLatitude()
+            << " alt = " << pos.asGeodetic().getAltitude()
+            << endl;
+       
+       return 0;
+}
index c83c57e..f3a3f6b 100644 (file)
--- a/sirf2.cpp
+++ b/sirf2.cpp
@@ -33,6 +33,12 @@ RinexObsStream       r_out;
 RinexObsHeader r_header;
 RinexObsData   r_data;
 
+double prev_clock_bias = 0.0;
+double prev_gps_seconds = 0.0;
+double prev_gps_drift = 0.0;
+int this_tt = 0, prev_tt = 0;
+int prev_tt_drift = 0;
+
 int rinex_open(const char *filename) {
        r_out.open(filename, ios::out|ios::trunc);
 
@@ -143,7 +149,7 @@ int rinex_data_add_pseudorange_snr(int satNR,
                                   double gps_seconds,
                                   double pseudorange,
                                   double snr) {
-       printf("RINEX range\t| GPS seconds %6f SatID %3u C1 %f\n",
+       printf("RINEX range\t| GPS seconds %6.9f SatID %3u C1 %f (\n",
               gps_seconds, satNR, pseudorange);
 
        // compute ssi
@@ -171,24 +177,58 @@ int rinex_data_add_pseudorange_snr(int satNR,
 /* from Message ID 7 */
 int rinex_data_add_time_clock_bias(int ext_gps_week,
                                   double gps_seconds, double clock_bias) {
-       printf("RINEX time\t| GPS week %5u GPS seconds %6f\n",
-              ext_gps_week, gps_seconds);
+       printf("RINEX time\t| GPS week %5u GPS seconds %6.2f Clock bias %.9f\n",
+              ext_gps_week, gps_seconds, clock_bias);
        
        // set extended GPS week only
        last_ext_gps_week = ext_gps_week;
        
        // adjust using clock bias
+       printf("RINEX : clock adjust %.9f => %.9f\n",
+              last_gps_seconds, last_gps_seconds - clock_bias);
+
+
+       double gps_drift = last_gps_seconds - prev_gps_seconds;
+       double freq = 0.0;
+       if ((prev_gps_drift != 0.0)
+           && (prev_gps_drift != gps_drift))
+               freq = 1.0 / (gps_drift - prev_gps_drift);
+
+       printf("RINEX : clock_bias += %.9f Meas. drift %.12f ClockD %.12f ClockF %f\n", 
+              clock_bias - prev_clock_bias,
+              last_gps_seconds - prev_gps_seconds,
+              gps_drift - prev_gps_drift, freq);
+
+       prev_clock_bias = clock_bias;
+       prev_gps_seconds = last_gps_seconds;
+       prev_gps_drift = gps_drift;
+
+       int tt_drift = this_tt - prev_tt;
+       printf("RINEX : time tag %u => %u drift %d (+%.9f s) +%d\n",
+              prev_tt, this_tt,
+              tt_drift, (double)tt_drift/16368000.0, 
+              tt_drift - prev_tt_drift);
+       prev_tt = this_tt;
+       prev_tt_drift = tt_drift;
+
+       // adjust pseudorange
+       last_gps_seconds = floor(gps_seconds) + clock_bias;
+
+       /*
        last_gps_seconds -= clock_bias;
-       
+       printf("RINEX : using clock_bias %.9f\n", clock_bias);
        RinexObsData::RinexSatMap::iterator it;
        for (it = r_data.obs.begin(); it != r_data.obs.end(); ++it) {
                double old_val = (*it).second[RinexObsHeader::C1].data;
                double new_val = old_val - clock_bias * C_GPS_M;
                (*it).second[RinexObsHeader::C1].data = new_val;
 
-               printf("RINEX satID %3u : pseudo range %f => %f\n",
-                      (*it).first.id, old_val, new_val);
+               printf("RINEX satID %3u : pseudorange adjust %f (%.9f s) => %f (%.9f s)\n",
+                      (*it).first.id,
+                      old_val, old_val / C_GPS_M,
+                      new_val, new_val / C_GPS_M);
        }
+       */
 
        /* write data */
        rinex_data_write();
@@ -196,6 +236,99 @@ int rinex_data_add_time_clock_bias(int ext_gps_week,
        return 0;
 }
 
+/* GPS clock management */
+
+double alpha = 1.0 / 16368000, beta = 0.0;
+int gps_clock_count = 0;
+
+unsigned long long prev_gps_clock = 0;
+unsigned long long last_gps_clock = 0;
+
+double prev_bias;
+double last_bias;
+
+int prev_gps_second = 0;
+int last_gps_second = 0;
+
+/* called on every MID 28 */
+void gps_clock_record_tt(unsigned int clock) {
+#define GPS_CLOCK_MASK ((unsigned long long)0xffffffff)
+       unsigned long long new_gps_clock = (last_gps_clock & ~GPS_CLOCK_MASK) 
+               | (clock & GPS_CLOCK_MASK);
+       while (new_gps_clock < last_gps_clock) {
+               new_gps_clock += (unsigned long long)0x100000000;
+               printf("rollover adjustment in GPS clock : 0x%llx\n", new_gps_clock);
+       }
+
+       last_gps_clock = new_gps_clock;
+}
+
+/* called on every MID 7 */
+void gps_clock_record_bias(double gps_second, double bias) {
+       last_bias = bias;
+       last_gps_second = (int) gps_second;
+}
+
+void gps_clock_adjust() {
+
+       static int first = 1;
+
+       if (first) {
+               first = 0;
+
+               prev_gps_clock = last_gps_clock;
+               prev_bias = last_bias;
+               prev_gps_second = last_gps_second;
+
+               alpha = 1.0 / 16368000.0;
+               beta = (double)last_gps_second + last_bias - (double)last_gps_clock * alpha;
+
+               printf("  alpha %.9g beta %.9f\n", alpha, beta);
+
+       }
+       
+       printf("gps_clock_count = %d\n", gps_clock_count);
+       
+       if (gps_clock_count >= 100) {
+
+               printf("  Computing alpha & beta with count=%d\n"
+                      "  last_gps_second=%d prev_gps_second=%d\n"
+                      "  last_bias=%.9f prev_bias=%.9f\n"
+                      "  last_gps_clock=0x%llx prev_gps_clock=0x%llx\n",
+                      gps_clock_count, last_gps_second, prev_gps_second,
+                      last_bias, prev_bias,
+                      last_gps_clock, prev_gps_clock);
+
+               alpha = ((double)(last_gps_second - prev_gps_second) + (last_bias - prev_bias )) /
+                        (double)(last_gps_clock - prev_gps_clock);
+               beta  = ((double)last_gps_second + last_bias - (double)last_gps_clock * alpha);
+
+               printf("  alpha %.9g beta %.9f\n", alpha, beta);
+               
+               prev_gps_clock = last_gps_clock;
+               prev_bias = last_bias;
+               prev_gps_second = last_gps_second;
+
+               gps_clock_count = 0;
+       }
+
+       gps_clock_count ++ ;
+}
+
+double gps_clock_convert(unsigned int clock) {
+
+       unsigned long long new_gps_clock = (last_gps_clock & ~GPS_CLOCK_MASK) 
+               | ((unsigned long long)clock & GPS_CLOCK_MASK);
+       while (new_gps_clock < last_gps_clock) {
+               new_gps_clock += 0x100000000;
+               printf("rollover adjustment in GPS clock : 0x%llx\n", new_gps_clock);
+       }
+
+       printf("%s: clock=0x%llx 1/alpha=%f beta=%.9f\n",
+              __func__, new_gps_clock, 1.0/alpha, beta);
+       return (double) new_gps_clock * alpha + beta;
+}
+
 /* return 1 if message is a valid SiRF message, 0 if invalid */
 
 int check_sirf_msg(unsigned char *buf, int n) {
@@ -402,7 +535,8 @@ int decode_sirf_msg_4(unsigned char *buf, int n)
        p += 1;
        printf("  GPS Week : %d\n", get_sirf_2s(buf + p));
        p += 2;
-       printf("  GPS Time of Week : %u\n", get_sirf_4u(buf + p));
+       printf("  GPS Time of Week : %.3f\n",
+              (double)get_sirf_4u(buf + p) / 100.0);
        p += 4;
        printf("  Channels : %d\n", buf[p]);
        p += 1;
@@ -446,6 +580,9 @@ int decode_sirf_msg_7(unsigned char *buf, int n)
        printf("  Estimated GPS Time : %u ms\n", get_sirf_4u(buf + p));
        p += 4;
 
+       gps_clock_record_bias(gps_seconds, clock_bias);
+       gps_clock_adjust();
+
        rinex_data_add_time_clock_bias(ext_gps_week, gps_seconds, clock_bias);
 
        return 0;
@@ -510,16 +647,29 @@ int decode_sirf_msg_28(unsigned char *buf, int n)
        double snr_avg = 0.0;
        int i;
        int tit;
+       unsigned int tt;
+       double tt_in_seconds;
+       //const int gps_clock = 16369000; /* Hz */
+         const int gps_clock = 16368000; // Hz
+       int snr_was_zero = 0;
 
        printf("Navigation Library Measurement Data - Channel %d\n", buf[p+1]);
        p += 2;
-       printf("  Time Tag = 0x%08x\n", get_sirf_4u(buf+p));
+       tt=get_sirf_4u(buf+p);
+       gps_clock_record_tt(tt);
+       tt_in_seconds = gps_clock_convert(tt);
+       /* divided by GPS clock freq */
+       printf("  Time Tag = 0x%08x (%.9f s)\n", tt, tt_in_seconds);
+       if (this_tt != tt)
+               this_tt = tt;
        p += 4;
        printf("  SatID : %3u\n", satID=buf[p]);
        p += 1;
-       printf("  GPS SW Time : %f s\n", gps_seconds=get_sirf_dbl(buf+p));
+       gps_seconds=get_sirf_dbl(buf+p);
+       printf("  GPS SW Time : %.9f s [delta with TT : %.9f s]\n",
+              gps_seconds, gps_seconds - tt_in_seconds);
        p += 8;
-       printf("  Pseudorange = %f m\n", pseudorange = get_sirf_dbl(buf+p));
+       printf("  Pseudorange = %.3f m\n", pseudorange = get_sirf_dbl(buf+p));
        p += 8;
        printf("  Carrier frequency = %f m/s\n", get_sirf_sgl(buf+p));
        p += 4;
@@ -534,12 +684,16 @@ int decode_sirf_msg_28(unsigned char *buf, int n)
 
                printf("  SNR %3u dB-Hz\n", snr = buf[p+i]);
                snr_avg += 0.1 * snr;
+               if (snr == 0) snr_was_zero = 1;
        }
+       /* it seems that pseudo range is incorrect (too small) if snr was 0
+          at least once */
+       if (snr_was_zero) snr_avg = 0.0;
        p += 10;
        /* ... */
 
-       /* pseudorange is only valid if time in track is > 0 */
-       if (tit > 0) 
+       /* pseudorange is only valid if time in track is > 1s */
+       if (tit > 1000)
                rinex_data_add_pseudorange_snr(satID, gps_seconds,
                                               pseudorange, snr_avg);
 
@@ -554,6 +708,26 @@ int decode_sirf_msg_31(unsigned char *buf, int n)
        return 0;
 }
 
+int decode_sirf_msg_41(unsigned char *buf, int n)
+{
+       int p = 0;
+
+       printf("Geodetic Navigation Data\n");
+       p += 1;
+       p += 2;
+       p += 2;
+       printf("  GPS week : %u\n", get_sirf_2u(buf + p));
+       p += 2;
+       printf("  GPS seconds : %.3f\n", (double)get_sirf_4u(buf + p) * 0.001);
+       p += 4;
+               
+       printf("  Y-M-D : %04d-%02d-%02d H:M:S : %02d:%02d:%02.3f\n",
+              get_sirf_2u(buf+p), buf[p+2], buf[p+3],
+              buf[p+4], buf[p+5], (double)get_sirf_2u(buf+p+6) * 0.001);
+
+       return 0;
+}
+
 int decode_sirf_msg_50(unsigned char *buf, int n)
 {
        int p = 0;
@@ -697,16 +871,7 @@ int decode_sirf_msg(unsigned char *buf, int n, int *ack) {
                break;
                
        case 41: /* Geodetic Navigation Data */
-               printf("Geodetic Navigation Data\n");
-               p += 1;
-               p += 2;
-               p += 2;
-               p += 2;
-               p += 4;
-               
-               printf("Y-M-D : %04d-%02d-%02d H:M:S : %02d:%02d:%02.3f\n",
-                      (buf[p]<<8)|buf[p+1], buf[p+2], buf[p+3],
-                      buf[p+4], buf[p+5], ((buf[p+6]<<8)|buf[p+7])/1000.0);
+               decode_sirf_msg_41(buf + 4, n - 8);
                break;
 
        case 50: