Changeset 2063 in ntrip for trunk

Timestamp:

Dec 1, 2009, 1:54:49 PM (15 years ago)

Author:

mervart

Message:

* empty log message *

Location:

trunk/BNC

Files:

• bncconst.cpp (modified) (1 diff)
• bncconst.h (modified) (1 diff)
• bncmodel.cpp (modified) (4 diffs)
• bncmodel.h (modified) (2 diffs)
• bncutils.cpp (modified) (1 diff)
• bncutils.h (modified) (2 diffs)

trunk/BNC/bncconst.cpp

@@ -31 +31 @@
 const double t_CST::lambda2 = c / freq2;
 const double t_CST::omega   = 7292115.1467e-11;
+const double t_CST::aell    = 6378137.000;
+const double t_CST::fInv    = 298.2572236;

trunk/BNC/bncconst.h

@@ -36 +36 @@
     static const double lambda2;
     static const double omega;
+    static const double aell;
+    static const double fInv;
 };
 

trunk/BNC/bncmodel.cpp

@@ -40 +40 @@
 
 #include <iomanip>
+#include <cmath>
 #include <newmatio.h>
 
@@ -45 +46 @@
 #include "bncpppclient.h"
 #include "bancroft.h"
+#include "bncutils.h"
 
 using namespace std;
@@ -138 +140 @@
   }
 
+  // Set Station Height
+  // ------------------
+  ColumnVector ell(3);
+  xyz2ell(_xcBanc.data(), ell.data());
+  _height = ell(3);
+
+  cout << "height = " << _height << endl;
+
   return success;
 }
@@ -155 +165 @@
   satData->rho = (satData->xx - xRec).norm_Frobenius();
 
-  double tropDelay = 0.0;
+  double tropDelay = delay_saast();
+
+  cout << "tropDelay " << tropDelay << endl;
 
   return satData->rho + _xcBanc(4) - satData->clk + tropDelay;
+}
+
+// Tropospheric Model (Saastamoinen)
+////////////////////////////////////////////////////////////////////////////
+double bncModel::delay_saast() {
+
+  double Ele = M_PI/2.0;
+
+  double pp =  1013.25 * pow(1.0 - 2.26e-5 * _height, 5.225);
+  double TT =  18.0 - _height * 0.0065 + 273.15;
+  double hh =  50.0 * exp(-6.396e-4 * _height);
+  double ee =  hh / 100.0 * exp(-37.2465 + 0.213166*TT - 0.000256908*TT*TT);
+
+  double h_km = _height / 1000.0;
+  
+  if (h_km < 0.0) h_km = 0.0;
+  if (h_km > 5.0) h_km = 5.0;
+  int    ii   = int(h_km + 1);
+  double href = ii - 1;
+  
+  double bCor[6]; 
+  bCor[0] = 1.156;
+  bCor[1] = 1.006;
+  bCor[2] = 0.874;
+  bCor[3] = 0.757;
+  bCor[4] = 0.654;
+  bCor[5] = 0.563;
+  
+  double BB = bCor[ii-1] + (bCor[ii]-bCor[ii-1]) * (h_km - href);
+  
+  double zen  = M_PI/2.0 - Ele;
+
+  return (0.002277/cos(zen)) * (pp + ((1255.0/TT)+0.05)*ee - BB*(tan(zen)*tan(zen)));
 }
 

trunk/BNC/bncmodel.h

@@ -55 +55 @@
  private:
   double cmpValueP3(t_satData* satData);
+  double delay_saast();
+
   QList<bncParam*> _params;
   SymmetricMatrix  _QQ;
@@ -62 +64 @@
   ColumnVector     _xx;
   ColumnVector     _xcBanc;
+  double           _height;
 };
 

trunk/BNC/bncutils.cpp

@@ -185 +185 @@
   xyz = RR * rsw;
 }
+
+// Rectangular Coordinates -> Ellipsoidal Coordinates
+////////////////////////////////////////////////////////////////////////////
+t_irc xyz2ell(const double* XYZ, double* Ell) {
+
+  const double bell = t_CST::aell*(1.0-1.0/t_CST::fInv) ;
+  const double e2   = (t_CST::aell*t_CST::aell-bell*bell)/(t_CST::aell*t_CST::aell) ;
+  const double e2c  = (t_CST::aell*t_CST::aell-bell*bell)/(bell*bell) ;
+  
+  double nn, ss, zps, hOld, phiOld, theta, sin3, cos3;
+
+  ss    = sqrt(XYZ[0]*XYZ[0]+XYZ[1]*XYZ[1]) ;
+  zps   = XYZ[2]/ss ;
+  theta = atan( (XYZ[2]*t_CST::aell) / (ss*bell) );
+  sin3  = sin(theta) * sin(theta) * sin(theta);
+  cos3  = cos(theta) * cos(theta) * cos(theta);
+
+  // Closed formula
+  Ell[0] = atan( (XYZ[2] + e2c * bell * sin3) / (ss - e2 * t_CST::aell * cos3) );  
+  Ell[1] = atan2(XYZ[1],XYZ[0]) ;
+  nn = t_CST::aell/sqrt(1.0-e2*sin(Ell[0])*sin(Ell[0])) ;
+  Ell[2] = ss / cos(Ell[0]) - nn;
+
+  const int MAXITER = 100;
+  for (int ii = 1; ii <= MAXITER; ii++) {
+    nn     = t_CST::aell/sqrt(1.0-e2*sin(Ell[0])*sin(Ell[0])) ;
+    hOld   = Ell[2] ;
+    phiOld = Ell[0] ;
+    Ell[2] = ss/cos(Ell[0])-nn ;
+    Ell[0] = atan(zps/(1.0-e2*nn/(nn+Ell[2]))) ;
+    if ( fabs(phiOld-Ell[0]) <= 1.0e-11 && fabs(hOld-Ell[2]) <= 1.0e-5 ) {
+      return success;
+    }
+  }
+
+  return failure;
+}

trunk/BNC/bncutils.h

@@ -30 +30 @@
 
 #include <newmat.h>
+#include <bncconst.h>
 
 void expandEnvVar(QString& str);
@@ -46 +47 @@
                 const ColumnVector& rsw, ColumnVector& xyz);
 
+t_irc xyz2ell(const double* XYZ, double* Ell);
+
 #endif