Changeset 2558 in ntrip for trunk/BNS

Timestamp:

Aug 10, 2010, 9:37:02 PM (14 years ago)

Author:

mervart

Message:

 

Location:

trunk/BNS

Files:

• bnseph.cpp (modified) (4 diffs)
• bnseph.h (modified) (1 diff)
• bnsutils.cpp (modified) (1 diff)
• bnsutils.h (modified) (1 diff)

trunk/BNS/bnseph.cpp

@@ -634 +634 @@
 // Derivative of the state vector using a simple force model (static)
 ////////////////////////////////////////////////////////////////////////////
-ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv) {
+ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv,
+                                 double* acc) {
 
   // State vector components
@@ -646 +647 @@
   static const double AE    = 6378136.0;
   static const double OMEGA = 7292115.e-11;
-  static const double C20   = -1082.63e-6;
+  static const double C20   = -1082.625e-6;
 
   double rho = rr.norm_Frobenius();
@@ -661 +662 @@
   va(2) = vv(2);
   va(3) = vv(3);
-  va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2); 
-  va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1); 
-  va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho))     ) * rr(3);
+  va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0]; 
+  va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1]; 
+  va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho))     ) * rr(3)            + acc[2];
 
   return va;
@@ -684 +685 @@
   double step = dtPos / nSteps;
 
+  double acc[3];
+  acc[0] = _x_acceleration * 1.e3;
+  acc[1] = _x_acceleration * 1.e3;
+  acc[2] = _x_acceleration * 1.e3;
   for (int ii = 1; ii <= nSteps; ii++) { 
-    _xv = rungeKutta4(_tt, _xv, step, glo_deriv);
+    _xv = rungeKutta4(_tt, _xv, step, acc, glo_deriv);
     _tt += step;
   }

trunk/BNS/bnseph.h

@@ -41 +41 @@
   virtual int  RTCM3(unsigned char *);
  private:
-  static ColumnVector glo_deriv(double /* tt */, const ColumnVector& xv);
+  static ColumnVector glo_deriv(double /* tt */, const ColumnVector& xv,
+                                double* acc);
   mutable double       _tt;  // time in seconds of GPSweek
   mutable ColumnVector _xv;  // status vector (position, velocity) at time _tt

trunk/BNS/bnsutils.cpp

@@ -128 +128 @@
   const ColumnVector& yi, // vector of the initial y-values
   double dx,              // the step size for the integration
-  ColumnVector (*der)(double x, const ColumnVector& y)
+  double* acc,            // aditional acceleration
+  ColumnVector (*der)(double x, const ColumnVector& y, double* acc)
                           // A pointer to a function that computes the 
                           // derivative of a function at a point (x,y)
                          ) {
 
-  ColumnVector k1 = der(xi       , yi       ) * dx;
-  ColumnVector k2 = der(xi+dx/2.0, yi+k1/2.0) * dx;
-  ColumnVector k3 = der(xi+dx/2.0, yi+k2/2.0) * dx;
-  ColumnVector k4 = der(xi+dx    , yi+k3    ) * dx;
+  ColumnVector k1 = der(xi       , yi       , acc) * dx;
+  ColumnVector k2 = der(xi+dx/2.0, yi+k1/2.0, acc) * dx;
+  ColumnVector k3 = der(xi+dx/2.0, yi+k2/2.0, acc) * dx;
+  ColumnVector k4 = der(xi+dx    , yi+k3    , acc) * dx;
 
   ColumnVector yf = yi + k1/6.0 + k2/3.0 + k3/3.0 + k4/6.0;

trunk/BNS/bnsutils.h

@@ -31 +31 @@
 
 ColumnVector rungeKutta4(double xi, const ColumnVector& yi, double dx,
-                         ColumnVector (*der)(double x, const ColumnVector& y));
+                         double* acc,
+           ColumnVector (*der)(double x, const ColumnVector& y, double* acc));
 
 QByteArray waitForLine(QTcpSocket* socket);