Changeset 802 in ntrip for trunk

Timestamp:

Apr 9, 2008, 12:24:17 PM (17 years ago)

Author:

mervart

Message:

* empty log message *

Location:

trunk/BNS

Files:

• bns.cpp (modified) (1 diff)
• bnsutils.cpp (modified) (2 diffs)
• bnsutils.h (modified) (1 diff)

trunk/BNS/bns.cpp

@@ -235 +235 @@
 
   ColumnVector xB(4);
-
-  satellitePosition(GPSweeks, ep, xB(1), xB(2), xB(3), xB(4));
+  ColumnVector vv(3);
+
+  satellitePosition(GPSweeks, ep, xB(1), xB(2), xB(3), xB(4), 
+                    vv(1), vv(2), vv(3));
 
   ColumnVector dx = xx - xB;

trunk/BNS/bnsutils.cpp

@@ -97 +97 @@
 ////////////////////////////////////////////////////////////////////////////
 void satellitePosition(double GPSweeks, const gpsEph* ep, 
-                       double& X, double& Y, double& Z, double& dt) {
+                       double& X, double& Y, double& Z, double& dt,
+                       double& vX, double& vY, double& vZ) {
 
   const static double omegaEarth = 7292115.1467e-11;
@@ -142 +143 @@
   dt = ep->clock_bias + ep->clock_drift*tc + ep->clock_driftrate*tc*tc 
        - 4.442807633e-10 * ep->e * sqrt(a0) *sin(E);
+
+  // Velocity
+  // --------
+  double tanv2 = tan(v/2);
+  double dEdM  = 1 / (1 - ep->e*cos(E));
+  double dotv  = sqrt((1.0 + ep->e)/(1.0 - ep->e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2) 
+               * dEdM * n;
+  double dotu  = dotv + (-ep->Cuc*sin2u0 + ep->Cus*cos2u0)*2*dotv;
+  double dotom = ep->OMEGADOT - omegaEarth;
+  double doti  = ep->IDOT + (-ep->Cic*sin2u0 + ep->Cis*cos2u0)*2*dotv;
+  double dotr  = a0 * ep->e*sin(E) * dEdM * n 
+                + (-ep->Crc*sin2u0 + ep->Crs*cos2u0)*2*dotv;
+  double dotx  = dotr*cos(u) - r*sin(u)*dotu;
+  double doty  = dotr*sin(u) + r*cos(u)*dotu;
+
+  vX  = cosom   *dotx  - cosi*sinom   *doty      // dX / dr
+      - xp*sinom*dotom - yp*cosi*cosom*dotom     // dX / dOMEGA
+                       + yp*sini*sinom*doti;     // dX / di
+
+  vY  = sinom   *dotx  + cosi*cosom   *doty
+      + xp*cosom*dotom - yp*cosi*sinom*dotom
+                       - yp*sini*cosom*doti;
+
+  vZ  = sini    *doty  + yp*cosi      *doti;
 }

trunk/BNS/bnsutils.h

@@ -18 +18 @@
 
 void satellitePosition(double GPSweeks, const gpsEph* ep, 
-                       double& X, double& Y, double& Z, double& dt);
+                       double& X, double& Y, double& Z, double&,
+                       double& vX, double& vY, double& vZ);
 #endif