//------------------------------------------------------------------------------
const std::vector<RealArray>& USNTwoWayRange::CalculateMeasurementDerivatives(
GmatBase *obj, Integer id)
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage("USNTwoWayRange::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 < 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(
"USNTwoWayRange 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() + " CartesianState 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 < size; ++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 < size; 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 < size; ++i)
// currentDerivatives[0][i] = - rangeUnit[i];
// }
// // velocity all zeroes
// for (UnsignedInt ii = 3; ii < size; 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 (Integer i = 0; i < size; ++i)
currentDerivatives[0][i] = 0.0;
}
}
else if (objNumber == 2) // participant 2, always a Spacecraft
{
#ifdef DEBUG_DERIVATIVES
MessageInterface::ShowMessage(" Deriv is w.r.t. %s of Participant"
" 2\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 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.52 and 7.53
for (Integer i = 0; i < size; ++i)
currentDerivatives[0][i] =
0.5 * (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.52 and 7.53
for (Integer i = 0; i < size; ++i)
currentDerivatives[0][i] =
0.5 * (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.52 and 7.53
for (Integer i = 0; i < 6; ++i)
currentDerivatives[0][i] =
0.5 * (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 (Integer i = 0; i < size; ++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 (Integer i = 0; i < size; ++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;
}
//------------------------------------------------------------------------------
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;
}
