/* Compute the atmospheric density using an exponential atmosphere model.
* @param utc Time reference object.
* @param rb Reference body object.
* @param r ECI position vector in meters.
* @param v ECI velocity vector in m/s
* @return Atmospheric density in kg/m^3.
*/
double CiraExponentialDrag::computeDensity(UTCTime utc,
EarthBody& rb,
Vector<double> r,
Vector<double> v)
{
// Get the J2000 to TOD transformation
Matrix<double> N = ReferenceFrames::J2kToTODMatrix(utc);
// Transform r from J2000 to TOD
Vector<double> r_tod = N*r;
Position geoidPos(r_tod(0),r_tod(1),r_tod(3),Position::Cartesian);
double height = geoidPos.getAltitude()/1000.0; // convert to [km]
// check to see if too low
if (height < h0[0])
{
string msg = "CiraExponentialDrag is valid for 50.0 km t0 1000.0 km"
+ string("the altitude you try is ")
+ StringUtils::asString(height) + " km!";
Exception e(msg);
GPSTK_THROW(e);
}
// find the right height bracket
int n = CIRA_SIZE; //h0.length;
int bracket = 0;
if (height >= h0[n-1])
{
bracket = n - 1;
}
else
{
for (int i = 0; i < (n-1); i++)
{
if ((height >= h0[i]) && (height < h0[i+1]))
{
bracket = i;
}
}
}
// compute the density
this->brack = bracket;
double rho = rho_0[bracket] * std::exp((h0[bracket] - height)/H[bracket]);
return rho;
} // End of method 'CiraExponentialDrag::computeDensity()'
// this is the real one
void AtmosphericDrag::doCompute(UTCTime utc, EarthBody& rb, Spacecraft& sc)
{
// To consist with STK
double omega_e = 7.292115E-05; // IERS 1996 conventions
//double omega_e = rb.getSpinRate(utc);
Vector<double> r = sc.R(); // satellite position in m
Vector<double> v = sc.V(); // satellite velocity in m/s
const double cd = sc.getDragCoeff();
const double area = sc.getDragArea();
const double mass = sc.getDryMass();
double rmag = norm(r);
double beta = cd * area / mass; // [m^2/kg]
// compute the atmospheric density
double rho = computeDensity(utc, rb, r, v); // [kg/m^3]
// debuging...
//rho = 6.3097802844338E-12;
// compute the relative velocity vector and magnitude
Vector<double> we(3,0.0);
we(2)= omega_e;
Vector<double> wxr = cross(we,r);
Vector<double> vr = v - wxr;
double vrmag = norm(vr);
// form -1/2 (Cd*A/m) rho
double coeff = -0.5 * beta * rho;
double coeff2 = coeff * vrmag;
// compute the acceleration in ECI frame (km/s^2)
a = vr * coeff2; ///////// a
// Partial reference: Montenbruck,P248
// form partial of drag wrt v
// da_dv = -0.5*Cd*(A/M)*p*(vr*transpose(vr)/vr+vr1)
Matrix<double> tr(3,1,0.0);
tr(0,0)=vr(0);
tr(1,0)=vr(1);
tr(2,0)=vr(2);
Matrix<double> vrvrt = tr*transpose(tr);
vrvrt = vrvrt / vrmag;
double eye3[3*3] = {1,0,0,0,1,0,0,0,1};
Matrix<double> vrm(3,3,0.0);
vrm = eye3;
vrm = vrm * vrmag;
da_dv = (vrvrt + vrm) * coeff; //////// da_dv
// da_dr
// da_dr = -0.5*Cd*(A/M)*vr*dp_dr-da_dv*X(w)
da_dr.resize(3,3,0.0);
Matrix<double> X(3,3,0.0);
X(0,1) = -we(2); // -wz
X(0,2) = +we(1); // wy
X(1,0) = +we(2); // +wz
X(1,2) = -we(0); // -wx
X(2,0) = -we(1); // -wy
X(2,1) = +we(0); // +wx
Matrix<double> part1(3,3,0.0);
Matrix<double> part2(3,3,0.0);
// Get the J2000 to TOD transformation
Matrix<double> N = ReferenceFrames::J2kToTODMatrix(utc);
// Transform r from J2000 to TOD
Vector<double> r_tod = N * r;
Position geoidPos(r_tod(0),r_tod(1),r_tod(3));
// Satellite height
double height = geoidPos.getAltitude()/1000.0; // convert to [km]
const int n = CIRA_SIZE; ;
int bracket = 0;
if (height >= h0[n-1])
{
bracket = n - 1;
}
else
{
for (int i = 0; i < (n-1); i++)
{
if ((height >= h0[i]) && (height < h0[i+1]))
{
bracket = i;
}
}
} // End 'if (height >= h0[n-1]) '
double Hh = H[bracket];
double coeff4 = -1.0 / (Hh * rmag);
Vector<double> drhodr = r*coeff4;
Matrix<double> tr2(3,1,0.0);
tr2(0,0) = drhodr(0);
tr2(1,0) = drhodr(1);
tr2(2,0) = drhodr(2);
part1 = tr*transpose(tr2); // //Matrix part1 = vr.outerProduct(drhodr);
part1 = part1*coeff2;
//part1 = dp_dr*a/rho;
part2 =-da_dv*X;
da_dr = part1-part2;
// form partial of drag wrt cd
double coeff3 = coeff2 / cd;
this->dadcd = vr*coeff3; //////// da_dcd
this->da_dcd(0,0) = dadcd(0);
this->da_dcd(1,0) = dadcd(1);
this->da_dcd(2,0) = dadcd(2);
} // End of method 'AtmosphericDrag::doCompute()'
