//------------------------------------------------------------------------------
void Estimator::PlotResiduals()
{
#ifdef DEBUG_RESIDUAL_PLOTS
MessageInterface::ShowMessage("Entered PlotResiduals\n");
MessageInterface::ShowMessage("Processing plot with %d Residuals\n",
measurementResiduals.size());
#endif
std::vector<RealArray*> dataBlast;
RealArray epochs;
RealArray values;
RealArray hiErrors;
RealArray lowErrors;
RealArray *hi = NULL, *low = NULL;
for (UnsignedInt i = 0; i < residualPlots.size(); ++i)
{
dataBlast.clear();
epochs.clear();
values.clear();
if (showErrorBars)
{
hiErrors.clear();
lowErrors.clear();
if (hiLowData.size() > 0)
{
hi = hiLowData[0];
if (hiLowData.size() > 1)
{
low = hiLowData[1];
}
}
}
// Collect residuals by plot
for (UnsignedInt j = 0; j < measurementResiduals.size(); ++j)
{
if (residualPlots[i]->UsesData(measurementResidualID[j]) >= 0)
{
epochs.push_back(measurementEpochs[j]);
values.push_back(measurementResiduals[j]);
if (hi && showErrorBars)
{
hiErrors.push_back((*hi)[j]);
}
if (low && showErrorBars)
lowErrors.push_back((*low)[j]);
}
}
if (epochs.size() > 0)
{
dataBlast.push_back(&epochs);
dataBlast.push_back(&values);
residualPlots[i]->TakeAction("ClearData");
residualPlots[i]->Deactivate();
residualPlots[i]->SetData(dataBlast, hiErrors, lowErrors);
residualPlots[i]->TakeAction("Rescale");
residualPlots[i]->Activate();
}
#ifdef DEBUG_RESIDUALS
// Dump the data to screen
MessageInterface::ShowMessage("DataDump for residuals plot %d:\n", i);
for (UnsignedInt k = 0; k < epochs.size(); ++k)
{
MessageInterface::ShowMessage(" %.12lf %.12lf", epochs[k], values[k]);
if (hi)
if (hi->size() > k)
if (low)
MessageInterface::ShowMessage(" + %.12lf", (*hi)[k]);
else
MessageInterface::ShowMessage(" +/- %.12lf", (*hi)[k]);
if (low)
if (low->size() > k)
MessageInterface::ShowMessage(" - %.12lf", (*low)[k]);
MessageInterface::ShowMessage("\n");
}
#endif
}
}
//------------------------------------------------------------------------------
RealArray Troposphere::Correction()
{
// Determine Re value
if (!solarSystem)
{
MessageInterface::ShowMessage("Troposphere::Correction: Solar System is NULL; Cannot obtain Earth radius\n");
throw MeasurementException("Troposphere::Correction: Solar System is NULL; Cannot obtain Earth radius\n");
}
CelestialBody *earth= solarSystem->GetBody(GmatSolarSystemDefaults::EARTH_NAME);
if (!earth)
{
MessageInterface::ShowMessage("Troposphere::Correction: Cannot obtain Earth radius\n");
throw MeasurementException("Troposphere::Correction: Cannot obtain Earth radius\n");
}
Real Re = earth->GetEquatorialRadius()*GmatMathConstants::KM_TO_M; // get Erath radius in meters
#ifdef DEBUG_TROPOSPHERE_CORRECTION
MessageInterface::ShowMessage("Troposphere::Correction():\n");
MessageInterface::ShowMessage(" temperature = %f K , pressure = %f hPa, humidity = %f\n", temperature, pressure, humidityFraction);
MessageInterface::ShowMessage(" range = %lfm , elevationAngle = %lf radian, waveLenght = %lfm\n", range, elevationAngle, waveLength);
MessageInterface::ShowMessage(" earth radius = %lf m\n",Re);
#endif
// Specify Ce and Crho:
double lambda = waveLength;
double lp2_inv = 1.0 / ((lambda * 1.0E+06)*(lambda * 1.0E+06));
double denom = (173.3 - lp2_inv);
double term1 = 170.2649 / denom;
double Ce = term1*term2;
double term3 = (173.3 + lp2_inv)/denom;
double Crho = Ce * term3;
#ifdef DEBUG_TROPOSPHERE_CORRECTION
MessageInterface::ShowMessage(" Ce = %lf , Crho = %lf\n", Ce, Crho);
#endif
// Specify inputs:
double p = pressure;
double T = temperature;
double fh = humidityFraction;
double E = elevationAngle;
double rho = range;
// refractivities
double N[2];
// compute dry component refractivity
N[0] = 77.624 * p / T;
// compute wet component refractivity
double Tc = T + GmatPhysicalConstants::ABSOLUTE_ZERO_C;
double e = 6.10 * fh * exp(17.15 * Tc /(234.7 + Tc));
N[1] = 371900.0 * e / (T*T) - 12.92 * e/T;
// troposphere heights
double h[2];
// compute dry troposphere height
h[0] = 5.0 * 0.002277 * p / (N[0] * 1.0E-06);
// compute wet troposphere height
h[1] = 5.0 * 0.002277 * e * (1255.0/T + 0.05) / (N[1] * 1.0E-06);
// distances to top of the troposphere
double r[2];
double alpha[9][2];
double beta[7][2];
double cosE = cos(E);
double cosE2 = cosE * cosE;
double sinE = sin(E);
for (int j = 0; j < 2; j++)
{
r[j] = sqrt((Re + h[j])*(Re + h[j]) - (Re*Re*cosE2)) - Re*sinE;
double aj = -1.0 * sinE/h[j];
double bj = - 1.0 * cosE2/(2.0 * h[j] * Re);
alpha[0][j] = 1.0;
alpha[1][j] = 4.0*aj;
alpha[2][j] = 6.0*aj*aj + 4.0*bj;
alpha[3][j] = 4.0*aj*(aj*aj + 3.0*bj);
alpha[4][j] = pow(aj, 4) + 12.0*aj*aj*bj + 6.0*bj*bj;
alpha[5][j] = 4.0*aj*bj*(aj*aj + 3.0*bj);
alpha[6][j] = bj*bj*(6.0*aj*aj + 4.0*bj);
alpha[7][j] = 4.0 * aj * bj*bj*bj;
alpha[8][j] = pow(bj,4);
beta[0][j] = 1.0;
beta[1][j] = 3.0*aj;
beta[2][j] = 3.0*(aj*aj + bj);
beta[3][j] = aj*(aj*aj + 6.0*bj);
beta[4][j] = 3.0*bj*(aj*aj + bj);
beta[5][j] = 3.0 * aj * bj*bj;
beta[6][j] = pow(bj,3);
}
double drho = 0.0;
double dE = 0.0;
for (int j = 0; j < 2; j++)
{
double sum1 = 0.0;
for (int i = 0; i < 9; i++)
{
double ii = (double)i;
double temp1 = alpha[i][j]*pow(r[j],(i+1))/(ii+1.0);
sum1 = sum1 + temp1;
}
double sum2 = 0.0;
for (int k = 0; k < 7; k++)
{
double kk = (double)k;
double temp2 = beta[k][j]*pow(r[j],(k+2))/((kk+1.0)*(kk+2.0)) + beta[k][j]*pow(r[j],(k+1))*(rho - r[j])/(kk+1);
sum2 = sum2 + temp2;
}
drho = drho + N[j] * 1.0E-06 * sum1;
dE = dE + N[j] * 1.0E-06 * sum2 / h[j];
}
drho = Crho * drho;
dE = Ce * 4.0 * cosE * dE/ rho;
dE = dE / GmatMathConstants::RAD_PER_ARCSEC;
RealArray out;
out.push_back(drho);
out.push_back(dE);
out.push_back(drho/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM);
#ifdef DEBUG_TROPOSPHERE_CORRECTION
MessageInterface::ShowMessage(" Troposphere correction result:\n");
MessageInterface::ShowMessage(" Range correction = %f m\n", drho);
MessageInterface::ShowMessage(" Elevation angle correction = %f arcsec", dE);
MessageInterface::ShowMessage(" Time correction = %f sec\n", drho/GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM);
#endif
return out;
}
//------------------------------------------------------------------------------
const std::vector<RealArray>& TDRSSTwoWayRange::CalculateMeasurementDerivatives(
GmatBase *obj, Integer id)
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("TDRSSTwoWayRange::CalculateMeasurement"
"Derivatives(%s, %d) called\n", obj->GetName().c_str(), id);
#endif
if (!initialized)
InitializeMeasurement();
GmatBase *objPtr = NULL;
Integer size = obj->GetEstimationParameterSize(id);
Integer objNumber = -1;
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" ParameterSize = %d\n", size);
#endif
if (size <= 0)
throw MeasurementException("The derivative parameter on derivative "
"object " + obj->GetName() + "is not recognized");
// Check to see if obj is a participant
for (UnsignedInt i = 0; i < this->participants.size(); ++i)
{
if (participants[i] == obj)
{
objPtr = participants[i];
objNumber = i + 1;
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Participant %s found\n",
objPtr->GetName().c_str());
#endif
break;
}
}
// Or if it is the measurement model for this object
if (obj->IsOfType(Gmat::MEASUREMENT_MODEL))
if (obj->GetRefObject(Gmat::CORE_MEASUREMENT, "") == this)
{
objPtr = obj;
objNumber = 0;
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" The measurement is the object\n",
objPtr->GetName().c_str());
#endif
}
if (objNumber == -1)
throw MeasurementException(
"TDRSSTwoWayRange error - object is neither participant nor "
"measurement model.");
RealArray oneRow;
oneRow.assign(size, 0.0);
currentDerivatives.clear();
currentDerivatives.push_back(oneRow);
Integer parameterID = GetParmIdFromEstID(id, obj);
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Looking up id %d\n", parameterID);
#endif
if (objPtr != NULL)
{
if (objNumber == 1) // participant number 1, either a GroundStation or a Spacecraft
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Deriv is w.r.t. %s of Participant"
" 1\n", objPtr->GetParameterText(parameterID).c_str());
#endif
if (objPtr->GetParameterText(parameterID) == "Position")
{
throw MeasurementException("Derivative w.r.t. " +
participants[0]->GetName() + " Position is not yet implemented");
}
else if (objPtr->GetParameterText(parameterID) == "Velocity")
{
throw MeasurementException("Derivative w.r.t. " +
participants[0]->GetName() + " Velocity is not yet implemented");
}
else if (objPtr->GetParameterText(parameterID) == "CartesianX")
{
throw MeasurementException("Derivative w.r.t. " +
participants[0]->GetName() + " CartesianState is not yet implemented");
}
else if (objPtr->GetParameterText(parameterID) == "Bias")
{
for (Integer i = 0; i < size; ++i)
currentDerivatives[0][i] = 1.0;
}
else
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Deriv is w.r.t. something "
"independent, so zero\n");
#endif
for (UnsignedInt i = 0; i < 3; ++i)
currentDerivatives[0][i] = 0.0;
}
}
else if (objNumber == 2) // participant 2, should be a TDRSS Spacecraft
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Deriv is w.r.t. %s of Participant"
" 1\n", objPtr->GetParameterText(parameterID).c_str());
#endif
if (objPtr->GetParameterText(parameterID) == "Position")
{
throw MeasurementException("Derivative w.r.t. " +
participants[0]->GetName() + " Position is not yet implemented");
}
else if (objPtr->GetParameterText(parameterID) == "Velocity")
{
throw MeasurementException("Derivative w.r.t. " +
participants[0]->GetName() + " Velocity is not yet implemented");
}
else if (objPtr->GetParameterText(parameterID) == "CartesianX")
{
throw MeasurementException("Derivative w.r.t. " +
participants[0]->GetName() + " CartesianState is not yet implemented");
}
else if (objPtr->GetParameterText(parameterID) == "Bias")
{
for (Integer i = 0; i < size; ++i)
currentDerivatives[0][i] = 1.0;
}
else
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Deriv is w.r.t. something "
"independent, so zero\n");
#endif
for (UnsignedInt i = 0; i < 3; ++i)
currentDerivatives[0][i] = 0.0;
}
}
else if (objNumber == 3) // participant 3, always a Spacecraft
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Deriv is w.r.t. %s of Participant"
" 3\n", objPtr->GetParameterText(parameterID).c_str());
#endif
if (objPtr->GetParameterText(parameterID) == "Position")
{
// Get the inverse of the orbit STM at the measurement epoch
// Will need adjustment if stm changes
Rmatrix stmInv(6,6);
GetInverseSTM(obj, stmInv);
Rvector3 forwardlinkRderiv;
GetRangeDerivative(forwardlinkLeg, stmInv, forwardlinkRderiv, false,
1, 2, true, false);
// Downlink leg
Rvector3 backlinkRderiv;
GetRangeDerivative(backlinkLeg, stmInv, backlinkRderiv, false, 1, 2,
true, false);
// Add 'em up per eq tbd
for (Integer i = 0; i < 3; ++i)
currentDerivatives[0][i] =
0.5 * (forwardlinkRderiv[i] + backlinkRderiv[i]);
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("Position Derivative: [%.12lf "
"%.12lf %.12lf]\n", currentDerivatives[0][0],
currentDerivatives[0][1], currentDerivatives[0][2]);
#endif
}
else if (objPtr->GetParameterText(parameterID) == "Velocity")
{
// Get the inverse of the orbit STM at the measurement epoch
// Will need adjustment if stm changes
Rmatrix stmInv(6,6);
GetInverseSTM(obj, stmInv);
Rvector3 forwardlinkVderiv;
GetRangeDerivative(forwardlinkLeg, stmInv, forwardlinkVderiv, false,
1, 2, false);
// Downlink leg
Rvector3 backlinkVderiv;
GetRangeDerivative(backlinkLeg, stmInv, backlinkVderiv, false, 1, 2,
false);
// Add 'em up per eq tbd
for (Integer i = 0; i < 3; ++i)
currentDerivatives[0][i] =
0.5 * (forwardlinkVderiv[i] + backlinkVderiv[i]);
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("Velocity Derivative: [%.12lf "
"%.12lf %.12lf]\n", currentDerivatives[0][0],
currentDerivatives[0][1], currentDerivatives[0][2]);
#endif
}
else if (objPtr->GetParameterText(parameterID) == "CartesianX")
{
// Get the inverse of the orbit STM at the measurement epoch
// Will need adjustment if stm changes
Rmatrix stmInv(6,6);
GetInverseSTM(obj, stmInv);
Rvector6 forwardlinkDeriv;
GetRangeDerivative(forwardlinkLeg, stmInv, forwardlinkDeriv, false,
1, 2);
// Downlink leg
Rvector6 backlinkDeriv;
GetRangeDerivative(backlinkLeg, stmInv, backlinkDeriv, false, 1, 2);
// Add 'em up per eq tbd
for (Integer i = 0; i < 6; ++i)
currentDerivatives[0][i] =
0.5 * (forwardlinkDeriv[i] + backlinkDeriv[i]);
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("CartesianState Derivative: "
"[%.12lf %.12lf %.12lf %.12lf %.12lf %.12lf]\n",
currentDerivatives[0][0], currentDerivatives[0][1],
currentDerivatives[0][2], currentDerivatives[0][3],
currentDerivatives[0][4], currentDerivatives[0][5]);
#endif
}
else if (objPtr->GetParameterText(parameterID) == "Bias")
{
for (Integer i = 0; i < size; ++i)
currentDerivatives[0][i] = 1.0;
}
else
{
for (UnsignedInt i = 0; i < 3; ++i)
currentDerivatives[0][i] = 0.0;
}
}
else if (objNumber == 0) // measurement model
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Deriv is w.r.t. %s of the "
"measurement model\n",
objPtr->GetParameterText(parameterID).c_str());
#endif
if (objPtr->GetParameterText(parameterID) == "Bias")
{
for (Integer i = 0; i < size; ++i)
currentDerivatives[0][i] = 1.0;
}
}
else
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Deriv is w.r.t. %s of a non-"
"Participant\n",
objPtr->GetParameterText(parameterID).c_str());
#endif
for (UnsignedInt i = 0; i < 3; ++i)
currentDerivatives[0][i] = 0.0;
}
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Deriv =\n ");
for (Integer i = 0; i < size; ++i)
MessageInterface::ShowMessage(" %.12le",currentDerivatives[0][i]);
MessageInterface::ShowMessage("\n");
#endif
}
return currentDerivatives;
}
Variant _jobject_to_variant(JNIEnv * env, jobject obj) {
jclass c = env->GetObjectClass(obj);
bool array;
String name = _get_class_name(env, c, &array);
//print_line("name is " + name + ", array "+Variant(array));
if (name == "java.lang.String") {
return String::utf8(env->GetStringUTFChars( (jstring)obj, NULL ));
};
if (name == "[Ljava.lang.String;") {
jobjectArray arr = (jobjectArray)obj;
int stringCount = env->GetArrayLength(arr);
//print_line("String array! " + String::num(stringCount));
DVector<String> sarr;
for (int i=0; i<stringCount; i++) {
jstring string = (jstring) env->GetObjectArrayElement(arr, i);
const char *rawString = env->GetStringUTFChars(string, 0);
sarr.push_back(String(rawString));
}
return sarr;
};
if (name == "java.lang.Boolean") {
jmethodID boolValue = env->GetMethodID(c, "booleanValue", "()Z");
bool ret = env->CallBooleanMethod(obj, boolValue);
return ret;
};
if (name == "java.lang.Integer") {
jclass nclass = env->FindClass("java/lang/Number");
jmethodID intValue = env->GetMethodID(nclass, "intValue", "()I");
int ret = env->CallIntMethod(obj, intValue);
return ret;
};
if (name == "[I") {
jintArray arr = (jintArray)obj;
int fCount = env->GetArrayLength(arr);
DVector<int> sarr;
sarr.resize(fCount);
DVector<int>::Write w = sarr.write();
env->GetIntArrayRegion(arr,0,fCount,w.ptr());
w = DVector<int>::Write();
return sarr;
};
if (name == "[B") {
jbyteArray arr = (jbyteArray)obj;
int fCount = env->GetArrayLength(arr);
DVector<uint8_t> sarr;
sarr.resize(fCount);
DVector<uint8_t>::Write w = sarr.write();
env->GetByteArrayRegion(arr,0,fCount,reinterpret_cast<signed char*>(w.ptr()));
w = DVector<uint8_t>::Write();
return sarr;
};
if (name == "java.lang.Float" || name == "java.lang.Double") {
jclass nclass = env->FindClass("java/lang/Number");
jmethodID doubleValue = env->GetMethodID(nclass, "doubleValue", "()D");
double ret = env->CallDoubleMethod(obj, doubleValue);
return ret;
};
if (name == "[D") {
jdoubleArray arr = (jdoubleArray)obj;
int fCount = env->GetArrayLength(arr);
RealArray sarr;
sarr.resize(fCount);
RealArray::Write w = sarr.write();
for (int i=0; i<fCount; i++) {
double n;
env->GetDoubleArrayRegion(arr, i, 1, &n);
w.ptr()[i] = n;
};
return sarr;
};
if (name == "[F") {
jfloatArray arr = (jfloatArray)obj;
int fCount = env->GetArrayLength(arr);
RealArray sarr;
sarr.resize(fCount);
RealArray::Write w = sarr.write();
for (int i=0; i<fCount; i++) {
float n;
env->GetFloatArrayRegion(arr, i, 1, &n);
w.ptr()[i] = n;
};
return sarr;
};
if (name == "[Ljava.lang.Object;") {
jobjectArray arr = (jobjectArray)obj;
int objCount = env->GetArrayLength(arr);
Array varr;
for (int i=0; i<objCount; i++) {
jobject jobj = env->GetObjectArrayElement(arr, i);
Variant v = _jobject_to_variant(env, jobj);
varr.push_back(v);
}
return varr;
};
if (name == "com.android.godot.Dictionary") {
Dictionary ret;
jclass oclass = c;
jmethodID get_keys = env->GetMethodID(oclass, "get_keys", "()[Ljava/lang/String;");
jobjectArray arr = (jobjectArray)env->CallObjectMethod(obj, get_keys);
StringArray keys = _jobject_to_variant(env, arr);
jmethodID get_values = env->GetMethodID(oclass, "get_values", "()[Ljava/lang/Object;");
arr = (jobjectArray)env->CallObjectMethod(obj, get_values);
Array vals = _jobject_to_variant(env, arr);
//print_line("adding " + String::num(keys.size()) + " to Dictionary!");
for (int i=0; i<keys.size(); i++) {
ret[keys[i]] = vals[i];
};
return ret;
};
return Variant();
};
//------------------------------------------------------------------------------
void ExtendedKalmanInv::ComputeObs()
{
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("Computing obs and hTilde\n");
#endif
// Compute the O-C, Htilde, and Kalman gain
const MeasurementData *calculatedMeas = NULL;
std::vector<RealArray> stateDeriv;
const std::vector<ListItem*> *stateMap = esm.GetStateMap();
const ObservationData *currentObs = measManager.GetObsData();
hTilde.clear();
#ifdef DEBUG_ESTIMATION_DETAILS
MessageInterface::ShowMessage("StateMap size is %d\n", stateMap->size());
#endif
modelsToAccess = measManager.GetValidMeasurementList();
UnsignedInt rowCount;
// Currently assuming uniqueness in models to access
if (measManager.Calculate(modelsToAccess[0], true) >= 1)
{
calculatedMeas = measManager.GetMeasurement(modelsToAccess[0]);
RealArray hTrow;
hTrow.assign(stateSize, 0.0);
rowCount = calculatedMeas->value.size();
measSize = currentObs->value.size();
#ifdef DEBUG_ESTIMATION
if (rowCount != measSize)
MessageInterface::ShowMessage("Mismatch between rowCount (%d) and "
"measSize(%d)\n", rowCount, measSize);
#endif
for (UnsignedInt i = 0; i < rowCount; ++i)
hTilde.push_back(hTrow);
// Now walk the state vector and get elements of H-tilde for each piece
for (UnsignedInt i = 0; i < stateSize; ++i)
{
if ((*stateMap)[i]->subelement == 1)
{
stateDeriv = measManager.CalculateDerivatives(
(*stateMap)[i]->object, (*stateMap)[i]->elementID,
modelsToAccess[0]);
// Fill in the corresponding elements of hTilde
for (UnsignedInt j = 0; j < rowCount; ++j)
for (Integer k = 0; k < (*stateMap)[i]->length; ++k)
hTilde[j][i+k] = stateDeriv[j][k];
}
}
}
if (calculatedMeas == NULL)
throw EstimatorException("No measurement was calculated!");
Real ocDiff;
yi.clear();
for (UnsignedInt k = 0; k < measSize; ++k)
{
ocDiff = currentObs->value[k] - calculatedMeas->value[k];
measurementEpochs.push_back(currentEpoch);
measurementResiduals.push_back(ocDiff);
measurementResidualID.push_back(calculatedMeas->uniqueID);
yi.push_back(ocDiff);
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("*** Current O-C = %.12lf\n", ocDiff);
#endif
}
if (currentObs->noiseCovariance == NULL)
measCovariance = calculatedMeas->covariance;
else
measCovariance = currentObs->noiseCovariance;
}
//------------------------------------------------------------------------------
const std::vector<RealArray>& DSNTwoWayRange::CalculateMeasurementDerivatives(
GmatBase *obj, Integer id)
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("DSNTwoWayRange::CalculateMeasurement"
"Derivatives(%s, %d) called\n", obj->GetName().c_str(), id);
#endif
if (!initialized)
InitializeMeasurement();
GmatBase *objPtr = NULL;
Integer size = obj->GetEstimationParameterSize(id);
Integer objNumber = -1;
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" ParameterSize = %d\n", size);
#endif
if (size <= 0)
throw MeasurementException("The derivative parameter on derivative "
"object " + obj->GetName() + "is not recognized");
// Check to see if obj is a participant
for (UnsignedInt i = 0; i < this->participants.size(); ++i)
{
if (participants[i] == obj)
{
objPtr = participants[i];
objNumber = i + 1;
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Participant %s found\n",
objPtr->GetName().c_str());
#endif
break;
}
}
// Or if it is the measurement model for this object
if (obj->IsOfType(Gmat::MEASUREMENT_MODEL))
if (obj->GetRefObject(Gmat::CORE_MEASUREMENT, "") == this)
{
objPtr = obj;
objNumber = 0;
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" The measurement is the object\n",
objPtr->GetName().c_str());
#endif
}
if (objNumber == -1)
throw MeasurementException(
"DSNTwoWayRange error - object is neither participant nor "
"measurement model.");
RealArray oneRow;
oneRow.assign(size, 0.0);
currentDerivatives.clear();
currentDerivatives.push_back(oneRow);
Integer parameterID = GetParmIdFromEstID(id, obj);
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Looking up id %d\n", parameterID);
#endif
if (objPtr != NULL)
{
if (objNumber == 1) // participant number 1, either a GroundStation or a Spacecraft
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Deriv is w.r.t. %s of Participant"
" 1\n", objPtr->GetParameterText(parameterID).c_str());
#endif
if (objPtr->GetParameterText(parameterID) == "Position")
{
throw MeasurementException("Derivative w.r.t. " +
participants[0]->GetName() +" position is not yet implemented");
// CalculateRangeVectorInertial();
// Rvector3 tmp, result;
// Rvector3 rangeUnit = rangeVecInertial.GetUnitVector();
// #ifdef DEBUG_DERIVATIVES
// MessageInterface::ShowMessage(" RVInertial = %.12lf %.12lf %.12lf\n",
// rangeVecInertial[0], rangeVecInertial[1], rangeVecInertial[2]);
// MessageInterface::ShowMessage(" Unit RVInertial = %.12lf %.12lf %.12lf ",
// rangeUnit[0], rangeUnit[1], rangeUnit[2]);
// #endif
// if (stationParticipant)
// {
// for (UnsignedInt i = 0; i < 3; ++i)
// tmp[i] = - rangeUnit[i];
//
// // for a Ground Station, need to rotate to the F1 frame
// result = tmp * R_j2k_1;
// for (UnsignedInt jj = 0; jj < 3; jj++)
// currentDerivatives[0][jj] = result[jj];
// }
// else
// {
// // for a spacecraft participant 1, we don't need the rotation matrices (I33)
// for (UnsignedInt i = 0; i < 3; ++i)
// currentDerivatives[0][i] = - rangeUnit[i];
// }
}
else if (objPtr->GetParameterText(parameterID) == "Velocity")
{
throw MeasurementException("Derivative w.r.t. " +
participants[0]->GetName() +" velocity is not yet implemented");
// for (UnsignedInt i = 0; i < 3; ++i)
// currentDerivatives[0][i] = 0.0;
}
else if (objPtr->GetParameterText(parameterID) == "CartesianX")
{
throw MeasurementException("Derivative w.r.t. " +
participants[0]->GetName() +" Cartesian state is not yet "
"implemented");
// CalculateRangeVectorInertial();
// Rvector3 tmp, result;
// Rvector3 rangeUnit = rangeVecInertial.GetUnitVector();
// #ifdef DEBUG_DERIVATIVES
// MessageInterface::ShowMessage(" RVInertial = %.12lf %.12lf %.12lf\n",
// rangeVecInertial[0], rangeVecInertial[1], rangeVecInertial[2]);
// MessageInterface::ShowMessage(" Unit RVInertial = %.12lf %.12lf %.12lf ",
// rangeUnit[0], rangeUnit[1], rangeUnit[2]);
// #endif
// if (stationParticipant)
// {
// for (UnsignedInt i = 0; i < 3; ++i)
// tmp[i] = - rangeUnit[i];
//
// // for a Ground Station, need to rotate to the F1 frame
// result = tmp * R_j2k_1;
// for (UnsignedInt jj = 0; jj < 3; jj++)
// currentDerivatives[0][jj] = result[jj];
// }
// else
// {
// // for a spacecraft participant 1, we don't need the rotation matrices (I33)
// for (UnsignedInt i = 0; i < 3; ++i)
// currentDerivatives[0][i] = - rangeUnit[i];
// }
// // velocity all zeroes
// for (UnsignedInt ii = 3; ii < 6; ii++)
// currentDerivatives[0][ii] = 0.0;
}
else if (objPtr->GetParameterText(parameterID) == "Bias")
{
for (Integer i = 0; i < size; ++i)
currentDerivatives[0][i] = 1.0;
}
else
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Deriv is w.r.t. something "
"independent, so zero\n");
#endif
for (UnsignedInt i = 0; i < 3; ++i)
currentDerivatives[0][i] = 0.0;
}
}
else if (objNumber == 2) // participant 2, always a Spacecraft
{
Real fFactor = GetFrequencyFactor(frequency) /
(GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / GmatMathConstants::KM_TO_M);
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Deriv is w.r.t. %s of Participant"
" 2\n", objPtr->GetParameterText(parameterID).c_str());
MessageInterface::ShowMessage(" freq = %15lf, F = %15lf, F/c = "
"%15lf\n", frequency, GetFrequencyFactor(frequency), fFactor);
#endif
if (objPtr->GetParameterText(parameterID) == "Position")
{
// Get the inverse of the orbit STM at the measurement epoch
// Will need adjustment if stm changes
Rmatrix stmInv(6,6);
GetInverseSTM(obj, stmInv);
Rvector3 uplinkRderiv;
GetRangeDerivative(uplinkLeg, stmInv, uplinkRderiv, false, 0, 1,
true, false);
// Downlink leg
Rvector3 downlinkRderiv;
GetRangeDerivative(downlinkLeg, stmInv, downlinkRderiv, false, 0, 1,
true, false);
// Add 'em up per eq 7.80
for (Integer i = 0; i < 3; ++i)
currentDerivatives[0][i] =
fFactor * (uplinkRderiv[i] + downlinkRderiv[i]);
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("Position Derivative: [%.12lf "
"%.12lf %.12lf]\n", currentDerivatives[0][0],
currentDerivatives[0][1], currentDerivatives[0][2]);
#endif
}
else if (objPtr->GetParameterText(parameterID) == "Velocity")
{
// Get the inverse of the orbit STM at the measurement epoch
// Will need adjustment if stm changes
Rmatrix stmInv(6,6);
GetInverseSTM(obj, stmInv);
Rvector3 uplinkVderiv;
GetRangeDerivative(uplinkLeg, stmInv, uplinkVderiv, false, 0, 1,
false);
// Downlink leg
Rvector3 downlinkVderiv;
GetRangeDerivative(downlinkLeg, stmInv, downlinkVderiv, false, 0, 1,
false);
// Add 'em up per eq 7.81
for (Integer i = 0; i < 3; ++i)
currentDerivatives[0][i] =
fFactor * (uplinkVderiv[i] + downlinkVderiv[i]);
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("Velocity Derivative: [%.12lf "
"%.12lf %.12lf]\n", currentDerivatives[0][0],
currentDerivatives[0][1], currentDerivatives[0][2]);
#endif
}
else if (objPtr->GetParameterText(parameterID) == "CartesianX")
{
// Get the inverse of the orbit STM at the measurement epoch
// Will need adjustment if stm changes
Rmatrix stmInv(6,6);
GetInverseSTM(obj, stmInv);
Rvector6 uplinkDeriv;
GetRangeDerivative(uplinkLeg, stmInv, uplinkDeriv, false);
// Downlink leg
Rvector6 downlinkDeriv;
GetRangeDerivative(downlinkLeg, stmInv, downlinkDeriv, false);
// Add 'em up per eq 7.80 and 7.81
for (Integer i = 0; i < 6; ++i)
currentDerivatives[0][i] =
fFactor * (uplinkDeriv[i] + downlinkDeriv[i]);
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("CartesianState Derivative: "
"[%.12lf %.12lf %.12lf %.12lf %.12lf %.12lf]\n",
currentDerivatives[0][0], currentDerivatives[0][1],
currentDerivatives[0][2], currentDerivatives[0][3],
currentDerivatives[0][4], currentDerivatives[0][5]);
#endif
}
else if (objPtr->GetParameterText(parameterID) == "Bias")
{
for (Integer i = 0; i < size; ++i)
currentDerivatives[0][i] = 1.0;
}
else
{
for (UnsignedInt i = 0; i < 3; ++i)
currentDerivatives[0][i] = 0.0;
}
}
else if (objNumber == 0) // measurement model
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Deriv is w.r.t. %s of the "
"measurement model\n",
objPtr->GetParameterText(parameterID).c_str());
#endif
if (objPtr->GetParameterText(parameterID) == "Bias")
{
for (Integer i = 0; i < size; ++i)
currentDerivatives[0][i] = 1.0;
}
}
else
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Deriv is w.r.t. %s of a non-"
"Participant\n",
objPtr->GetParameterText(parameterID).c_str());
#endif
for (UnsignedInt i = 0; i < 3; ++i)
currentDerivatives[0][i] = 0.0;
}
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Deriv =\n ");
for (Integer i = 0; i < size; ++i)
MessageInterface::ShowMessage(" %.12le",currentDerivatives[0][i]);
MessageInterface::ShowMessage("\n");
#endif
}
return currentDerivatives;
}
RealArray PhysicalMeasurement::IonosphereCorrection(Real freq, Rvector3 r1, Rvector3 r2, Real epoch)
{
RealArray ionoCorrection;
if (ionosphere != NULL)
{
// 1. Set wave length:
Real wavelength = GmatPhysicalConstants::SPEED_OF_LIGHT_VACUUM / (freq*1.0e6); // unit: meter
ionosphere->SetWaveLength(wavelength);
// 2. Set time:
ionosphere->SetTime(epoch); // unit: Julian day
// 3. Set station and spacecraft positions:
GroundstationInterface* gs = (GroundstationInterface*)participants[0];
CoordinateSystem* cs = gs->GetBodyFixedCoordinateSystem();
Rvector inState(6, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
Rvector outState(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
// Rvector bogusOut = cs->FromMJ2000Eq(epoch, bogusIn, true);
// Rmatrix33 R_g_j2k = cs->GetLastRotationMatrix().Transpose();
CoordinateConverter* cv = new CoordinateConverter();
A1Mjd time(epoch);
CoordinateSystem* fk5cs = Moderator::Instance()->GetCoordinateSystem("EarthMJ2000Eq");
cv->Convert(time, inState, cs, outState, fk5cs); // convert EarthFK5 coordinate system to EarthBodyFixed coordinate system
Rmatrix33 R_g_j2k = cv->GetLastRotationMatrix().Transpose();
// MessageInterface::ShowMessage("[ %f %f %f\n", R_g_j2k.GetElement(0,0), R_g_j2k.GetElement(0,1), R_g_j2k.GetElement(0,2));
// MessageInterface::ShowMessage(" %f %f %f\n", R_g_j2k.GetElement(1,0), R_g_j2k.GetElement(1,1), R_g_j2k.GetElement(1,2));
// MessageInterface::ShowMessage(" %f %f %f]\n", R_g_j2k.GetElement(2,0), R_g_j2k.GetElement(2,1), R_g_j2k.GetElement(2,2));
ionosphere->SetStationPosition(R_g_j2k*r1); // unit: km
ionosphere->SetSpacecraftPosition(R_g_j2k*r2); // unit: km
// 4. Set earth radius:
SpacePoint* earth = (SpacePoint*)gs->GetRefObject(Gmat::SPACE_POINT, "Earth");
Real earthRadius = earth->GetRealParameter("EquatorialRadius");
ionosphere->SetEarthRadius(earthRadius); // unit: km
#ifdef DEBUG_IONOSPHERE_MEDIA_CORRECTION
MessageInterface::ShowMessage(" *Run Ionosphere media correction for:\n");
MessageInterface::ShowMessage(" +Earth radius = %lf km\n", earthRadius);
MessageInterface::ShowMessage(" +Wave length = %.12lf m\n", wavelength);
MessageInterface::ShowMessage(" +Time = %.12lf\n", epoch);
MessageInterface::ShowMessage(" +Station location in Earth body fixed coordinate system (km):\n"
" (%s)", (R_g_j2k*r1).ToString().c_str());
MessageInterface::ShowMessage(" +Spacecraft location in Earth body fixed coordinate system (km):\n"
" (%s)", (R_g_j2k*r2).ToString().c_str());
#endif
// 5. Run ionosphere correction:
ionoCorrection = ionosphere->Correction();
Real rangeCorrection = ionoCorrection[0]*GmatMathConstants::M_TO_KM; // unit: meter
#ifdef DEBUG_IONOSPHERE_MEDIA_CORRECTION
MessageInterface::ShowMessage(" *Ionosphere media correction result:\n");
MessageInterface::ShowMessage(" +Range correction = %.12lf m\n", rangeCorrection*GmatMathConstants::KM_TO_M);
#endif
}
else
{
ionoCorrection.push_back(0.0);
ionoCorrection.push_back(0.0);
ionoCorrection.push_back(0.0);
}
return ionoCorrection;
}
int LoadKappaZ (RealArray& kappa, RealArray& kappaEnergy, RealArray abundances)
{
// Get data directory from XSPEC xset variables
string windtabsDirectory = getXspecVariable ("WINDTABSDIRECTORY", "./");
// Get filename from XSPEC xset variables
string FITSfilename = windtabsDirectory + "/";
FITSfilename += getXspecVariable ("KAPPAZFILENAME", "kappa.fits");
// string KeywordNameMu ("mu");
// Set up load of 2D array kappaZ;
// dimensions are Z and energyx
RealArray kappaZ; // this gets appropriately resized when it is loaded
size_t ax1 (0);
size_t ax2 (0);
try {
auto_ptr<FITS> pInfile
(new FITS (FITSfilename, Read, true)); // Primary HDU - Image
PHDU& image = pInfile->pHDU ();
image.read (kappaZ); // this is a 1D representation of a 2D array
ax1 = image.axis (0);
ax2 = image.axis (1);
}
catch (FitsException& issue) {
cerr << "LoadKappaZ: CCfits / FITSio exception:" << endl;
cerr << issue.message () << endl;
cerr << "(failed reading KappaZ)" << endl;
return 1;
}
// Load energy axis for kappa table:
try {
int extensionNumber (1);
auto_ptr<FITS> pInfile
(new FITS (FITSfilename, Read, extensionNumber, false));
ExtHDU& table = pInfile->currentExtension ();
size_t NumberOfRows = table.column(1).rows ();
table.column(1).read (kappaEnergy, 1, NumberOfRows);
// table.column(2).read (kappaZ, 1, NumberOfRows); // see if this works???
}
catch (FitsException& issue) {
cerr << "LoadKappa: CCfits / FITSio exception:" << endl;
cerr << issue.message () << endl;
cerr << "(file probably doesn't exist)" << endl;
return 1;
}
kappaEnergy *= 1.e-3; // convert from eV to keV
// get mass fractions based on xspec abund, model abund parameter,
// and atomic masses
RealArray massFractions;
getMassFractions (abundances, massFractions);
// sum kappas weighted by mass fractions
size_t NEnergies (kappaEnergy.size ());
size_t NZ (massFractions.size ());
kappa.resize (NEnergies, 0.);
// It would be better if there was a vectorized way to do this,
// but I don't know what it is.
for (size_t i=0; i<NZ; i++) {
for (size_t j=0; j<NEnergies; j++){
size_t k = i*ax1 + j; // this is the 1d representation of the 2d array
kappa[j] += kappaZ[k] * massFractions[i];
}
}
return 0;
}
void KaiserWindowDesigner::getWindow(RealArray &w)
{
w.resize(size);
Real beta = getBeta();
calculateWindow(w, beta);
}
