| 1 | /* -------------------------------------------------------------------------
|
|---|
| 2 | * BKG NTRIP Server
|
|---|
| 3 | * -------------------------------------------------------------------------
|
|---|
| 4 | *
|
|---|
| 5 | * Function: glo_deriv
|
|---|
| 6 | *
|
|---|
| 7 | * Purpose: Derivative of the state vector of a Galileo satellite
|
|---|
| 8 | * using its position, velocity and a simplified force model
|
|---|
| 9 | *
|
|---|
| 10 | * Author: L. Mervart
|
|---|
| 11 | *
|
|---|
| 12 | * Created: 07-Mai-2008
|
|---|
| 13 | *
|
|---|
| 14 | * Changes:
|
|---|
| 15 | *
|
|---|
| 16 | /******************************************************************************/
|
|---|
| 17 |
|
|---|
| 18 | #include "glonass.h"
|
|---|
| 19 |
|
|---|
| 20 | // Derivative of the state vector
|
|---|
| 21 | ////////////////////////////////////////////////////////////////////////////
|
|---|
| 22 | void glo_deriv(double /* tt */, const ColumnVector& yy,
|
|---|
| 23 | ColumnVector& yp, void* /* pVoid */) {
|
|---|
| 24 |
|
|---|
| 25 | // State vector components
|
|---|
| 26 | // -----------------------
|
|---|
| 27 | ColumnVector rr = yy.rows(1,3);
|
|---|
| 28 | ColumnVector vv = yy.rows(4,6);
|
|---|
| 29 |
|
|---|
| 30 | // Acceleration
|
|---|
| 31 | // ------------
|
|---|
| 32 | const static double GM = 398.60044e12;
|
|---|
| 33 | const static double AE = 6378136.0;
|
|---|
| 34 | const static double OMEGA = 7292115.e-11;
|
|---|
| 35 | const static double C20 = -1082.63e-6;
|
|---|
| 36 |
|
|---|
| 37 | double rho = rr.norm_Frobenius();
|
|---|
| 38 | double t1 = -GM/(rho*rho*rho);
|
|---|
| 39 | double t2 = 3.0/2.0 * C20 * (GM*AE*AE) / (rho*rho*rho*rho*rho);
|
|---|
| 40 | double t3 = OMEGA * OMEGA;
|
|---|
| 41 | double t4 = 2.0 * OMEGA;
|
|---|
| 42 | double z2 = rr(3) * rr(3);
|
|---|
| 43 |
|
|---|
| 44 | ColumnVector aa(3);
|
|---|
| 45 | aa(1) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2);
|
|---|
| 46 | aa(2) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1);
|
|---|
| 47 | aa(3) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3);
|
|---|
| 48 |
|
|---|
| 49 | // State vector derivative
|
|---|
| 50 | // -----------------------
|
|---|
| 51 | yp = vv &
|
|---|
| 52 | aa ;
|
|---|
| 53 | }
|
|---|
