//---------------------------------------------------------------------------
int SetState(double epoch, double state[], int stateDim)
{
int retval = -1;
if (pSetup != NULL)
{
GmatState *theState = pSetup->GetPropStateManager()->GetState();
if (stateDim <= theState->GetSize())
{
theState->SetEpoch(epoch);
theState->SetState(state, stateDim);
retval = 0;
}
else
{
char msg[128];
sprintf(msg, "ERROR: Incoming state size (%d) is larger than the "
"propagation state vector size (%d)!\n Epoch: %lf\n "
"State = [%lf %lf %lf %lf %lf %lf]\n", stateDim,
theState->GetSize(), epoch, state[0], state[1], state[2],
state[3], state[4], state[5]);
lastMsg = msg;
retval = -2;
}
}
else
lastMsg = "ERROR in SetState: The propagation setup is not yet set.";
return retval;
}
//---------------------------------------------------------------------------
const char *GetStateDescription()
{
lastMsg = "";
if (ode != NULL)
{
GmatState *theState = pSetup->GetPropStateManager()->GetState();
StringArray desc = theState->GetElementDescriptions();
for (unsigned int i = 0; i < desc.size(); ++i)
lastMsg += " " + desc[i] + "\n";
}
return lastMsg.c_str();
}
//------------------------------------------------------------------------------
void Formation::UpdateElements()
{
Integer size, index = 0;
GmatState *ps;
for (std::vector<SpaceObject*>::iterator i = components.begin();
i != components.end(); ++i)
{
ps = &((*i)->GetState());
size = ps->GetSize();
memcpy(ps->GetState(), &((state.GetState())[index]), size*sizeof(Real));
index += size;
if ((*i)->GetType() == Gmat::FORMATION)
((Formation*)(*i))->UpdateElements();
}
}
//------------------------------------------------------------------------------
const Rvector6 Formation::GetMJ2000State(const A1Mjd &atTime)
{
satCount = components.size();
if (j2000Body == NULL)
throw SpaceObjectException("MJ2000 body not yet set for " +
instanceName);
// First calculate the geometric center of the formation
Rvector6 centerState;
GmatState ps = GetState();
Real *st = ps.GetState();
if (satCount == 0)
{
MessageInterface::ShowMessage(
"Warning: Attempting to find MJ2000 state for an empty formation\n");
return centerState;
}
// The Formation GmatState contains state data for the spacecraft, tanks, and
// (eventually) attitude. The first 6*satcount elements are the spacecraft
// position and velocity data.
for (UnsignedInt i = 0; i < satCount; ++i)
{
for (Integer j = 0; j < 6; ++j)
centerState[j] += st[i*6+j];
}
centerState /= satCount;
// Then calculate the J2000 data based on that state
Rvector6 bodyState = j2000Body->GetMJ2000State(atTime);
// If origin is NULL, assume it is set at the J2000 origin.
if (origin)
{
Rvector6 offset = origin->GetMJ2000State(atTime);
bodyState -= offset;
}
return centerState - bodyState;
}
//------------------------------------------------------------------------------
void Formation::UpdateState()
{
Integer size, index = 0;
GmatState *ps;
Real ep0 = 0.0, ep;
for (std::vector<SpaceObject*>::iterator i = components.begin();
i != components.end(); ++i)
{
if (i == components.begin())
{
ep0 = (*i)->GetEpoch();
#ifdef DEBUG_FORMATION
MessageInterface::ShowMessage("Base epoch is %.12lf\n", ep0);
#endif
}
else
{
ep = (*i)->GetEpoch();
if (ep != ep0)
MessageInterface::ShowMessage(
"WARNING! Formation Member Epochs are not synchronized!\n"
"First spacecraft epoch is %.12lf, but %s has epoch %.12lf\n",
ep0, (*i)->GetName().c_str(), ep);
}
ps = &((*i)->GetState());
size = ps->GetSize();
#ifdef DEBUG_FORMATION_UPDATES
MessageInterface::ShowMessage(
"Formation: Updating(%d to %d) from %s::%s\n",
index, index + size - 1, instanceName.c_str(), (*i)->GetName().c_str());
#endif
memcpy(&((state.GetState())[index]), ps->GetState(), size*sizeof(Real));
index += size;
if ((*i)->GetType() == Gmat::FORMATION)
((Formation*)(*i))->UpdateState();
}
SetEpoch(ep0);
}
//------------------------------------------------------------------------------
bool Formation::SetRefObject(GmatBase *obj, const Gmat::ObjectType type,
const std::string &name)
{
SpaceObject *so;
if (type == Gmat::SPACECRAFT)
{
so = ((SpaceObject*)(obj));
if (find(components.begin(), components.end(), so) == components.end())
{
GmatState *ps = &(so->GetState());
Integer size = ps->GetSize();
dimension += size;
Real newepoch = so->GetEpoch();
if (components.size() == 0)
state.SetEpoch(newepoch);
else
if (state.GetEpoch() != newepoch)
{
char errorMsg[256];
sprintf(errorMsg, "Epochs (%lf) and (%lf) are not synchronized "
"in the formation %s", newepoch, state.GetEpoch(),
instanceName.c_str());
throw SpaceObjectException(errorMsg);
}
components.push_back(so);
}
if (type == Gmat::FORMATION)
{
throw SpaceObjectException("GMAT does not allow Formations of "
"Formations, so the Formation \"" + name + "\" cannot be added "
"to the Formation \"" + instanceName + "\".");
}
return true;
}
return FormationInterface::SetRefObject(obj, type, name);
}
//------------------------------------------------------------------------------
void BatchEstimator::CompleteInitialization()
{
#ifdef WALK_STATE_MACHINE
MessageInterface::ShowMessage("BatchEstimator state is INITIALIZING\n");
#endif
if (showAllResiduals)
{
StringArray plotMeasurements;
for (UnsignedInt i = 0; i < measurementNames.size(); ++i)
{
plotMeasurements.clear();
plotMeasurements.push_back(measurementNames[i]);
std::string plotName = instanceName + "_" + measurementNames[i] +
"_Residuals";
BuildResidualPlot(plotName, plotMeasurements);
}
}
if (advanceToEstimationEpoch == false)
{
PropagationStateManager *psm = propagator->GetPropStateManager();
GmatState *gs = psm->GetState();
estimationState = esm.GetState();
stateSize = estimationState->GetSize();
Estimator::CompleteInitialization();
// If estimation epoch not set, use the epoch from the prop state
if ((estEpochFormat == "FromParticipants") || (estimationEpoch <= 0.0))
{
ObjectArray participants;
esm.GetStateObjects(participants, Gmat::SPACEOBJECT);
for (UnsignedInt i = 0; i < participants.size(); ++i)
estimationEpoch = ((SpaceObject *)(participants[i]))->GetEpoch();
}
currentEpoch = gs->GetEpoch();
// Tell the measManager to complete its initialization
bool measOK = measManager.Initialize();
if (!measOK)
throw SolverException(
"BatchEstimator::CompleteInitialization - error initializing "
"MeasurementManager.\n");
// Now load up the observations
measManager.PrepareForProcessing();
measManager.LoadObservations();
if (!GmatMathUtil::IsEqual(currentEpoch, estimationEpoch))
{
advanceToEstimationEpoch = true;
nextMeasurementEpoch = estimationEpoch;
currentState = PROPAGATING;
return;
}
}
advanceToEstimationEpoch = false;
// First measurement epoch is the epoch of the first measurement. Duh.
nextMeasurementEpoch = measManager.GetEpoch();
#ifdef DEBUG_INITIALIZATION
MessageInterface::ShowMessage(
"Init complete!\n STM = %s\n Covariance = %s\n",
stm->ToString().c_str(), covariance->ToString().c_str());
#endif
hAccum.clear();
if (useApriori)
{
information = stateCovariance->GetCovariance()->Inverse();
}
else
{
information.SetSize(stateSize, stateSize);
for (UnsignedInt i = 0; i < stateSize; ++i)
for (UnsignedInt j = 0; j < stateSize; ++j)
information(i,j) = 0.0;
}
residuals.SetSize(stateSize);
x0bar.SetSize(stateSize);
measurementResiduals.clear();
measurementEpochs.clear();
for (Integer i = 0; i < information.GetNumRows(); ++i)
{
residuals[i] = 0.0;
if (useApriori)
x0bar[i] = (*estimationState)[i];
else
x0bar[i] = 0.0;
}
if (useApriori)
for (Integer i = 0; i < information.GetNumRows(); ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
residuals[i] += information(i,j) * x0bar[j];
}
esm.BufferObjects(&outerLoopBuffer);
esm.MapObjectsToVector();
converged = false;
isInitialized = true;
WriteToTextFile();
ReportProgress();
if (GmatMathUtil::IsEqual(currentEpoch, nextMeasurementEpoch))
currentState = CALCULATING;
else
{
timeStep = (nextMeasurementEpoch - currentEpoch) *
GmatTimeConstants::SECS_PER_DAY;
currentState = PROPAGATING;
}
#ifdef DEBUG_INITIALIZATION
MessageInterface::ShowMessage("BatchEstimator::CompleteInitialization "
"process complete\n");
MessageInterface::ShowMessage(" Estimation state = [");
for (UnsignedInt i = 0; i < stateSize; ++i)
MessageInterface::ShowMessage(" %.12lf ", (*estimationState)[i]);
MessageInterface::ShowMessage("]\n");
MessageInterface::ShowMessage(" Information Matrix = \n");
for (Integer i = 0; i < information.GetNumRows(); ++i)
{
MessageInterface::ShowMessage(" [");
for (Integer j = 0; j < information.GetNumColumns(); ++j)
{
MessageInterface::ShowMessage(" %.12lf ", information(i, j));
}
MessageInterface::ShowMessage("]\n");
}
MessageInterface::ShowMessage(" Residuals = [");
for (Integer i = 0; i < residuals.GetSize(); ++i)
MessageInterface::ShowMessage(" %.12lf ", residuals[i]);
MessageInterface::ShowMessage("]\n");
#endif
}
//------------------------------------------------------------------------------
void Formation::BuildState()
{
#ifdef DEBUG_FORMATION_ACTIONS
MessageInterface::ShowMessage("%s%s%s%d\n",
"In BuildState, Formation \"", instanceName.c_str(),
"\" has dimension ", dimension);
#endif
if (dimension <= 0)
throw SpaceObjectException(
"Error building Formation state; no spacecraft are set");
// Setup the GmatState
Real *data = new Real[dimension], *st;
Integer j = 0, k;
GmatState *ps;
if (state.GetSize() < dimension)
state.SetSize(dimension);
for (std::vector<SpaceObject*>::iterator i = components.begin();
i != components.end(); ++i)
{
if ((*i) == NULL)
throw SpaceObjectException(
"Error building Formation state; member spacecraft not set");
ps = &((*i)->GetState());
st = ps->GetState();
for (k = 0; k < ps->GetSize(); ++k)
{
data[j + k] = st[k];
}
j += ps->GetSize();
}
#ifdef DEBUG_FORMATION_ACTIONS
MessageInterface::ShowMessage("%s%s%s\n",
"In BuildState, Formation \"", instanceName.c_str(),
"\" consists of these spacecraft names:");
for (StringArray::iterator i = componentNames.begin();
i != componentNames.end(); ++i)
MessageInterface::ShowMessage(" \"%s\"\n", i->c_str());
MessageInterface::ShowMessage("%s%s%s\n",
"In BuildState, Formation \"", instanceName.c_str(),
"\" consists of these spacecraft:");
for (std::vector<SpaceObject *>::iterator j = components.begin();
j < components.end(); ++j)
{
MessageInterface::ShowMessage(" \"%s\"\n", (*j)->GetName().c_str());
}
#endif
if (!state.SetState(data, dimension))
{
delete [] data;
throw SpaceObjectException("Error building Formation state");
}
// as per kw report
// Shouldn't we delete data here since GmatState::SetState() copies the data
delete [] data;
}
//------------------------------------------------------------------------------
void SequentialEstimator::CompleteInitialization()
{
PropagationStateManager *psm = propagator->GetPropStateManager();
GmatState *gs = psm->GetState();
estimationState = esm.GetState();
stateSize = estimationState->GetSize();
Estimator::CompleteInitialization();
// // Reset the a priori covariance to something more reasonable
// for (UnsignedInt i = 0; i < stateSize; ++i)
// if (i < 3)
// (*stateCovariance)(i,i) = DEFAULT_POSITION_COVARIANCE;
// else if (i < 6)
// (*stateCovariance)(i,i) = DEFAULT_VELOCITY_COVARIANCE;
// else
// (*stateCovariance)(i,i) = DEFAULT_OTHER_COVARIANCE;
estimationEpoch = gs->GetEpoch();
currentEpoch = gs->GetEpoch();
// Tell the measManager to complete its initialization
bool measOK = measManager.Initialize();
if (!measOK)
throw SolverException(
"BatchEstimator::CompleteInitialization - error initializing "
"MeasurementManager.\n");
// Now load up the observations
measManager.PrepareForProcessing();
measManager.LoadObservations();
// First measurement epoch is the epoch of the first measurement. Duh.
nextMeasurementEpoch = measManager.GetEpoch();
hAccum.clear();
residuals.SetSize(stateSize);
x0bar.SetSize(stateSize);
dx.SetSize(stateSize);
esm.MapObjectsToVector();
measurementResiduals.clear();
isInitialized = true;
ReportProgress();
if (GmatMathUtil::IsEqual(currentEpoch, nextMeasurementEpoch))
currentState = CALCULATING;
else
{
timeStep = (nextMeasurementEpoch - currentEpoch) *
GmatTimeConstants::SECS_PER_DAY;
PrepareForStep();
currentState = PROPAGATING;
}
if (showAllResiduals)
{
StringArray plotMeasurements;
for (UnsignedInt i = 0; i < measurementNames.size(); ++i)
{
plotMeasurements.clear();
plotMeasurements.push_back(measurementNames[i]);
std::string plotName = instanceName + "_" + measurementNames[i] +
"_Residuals";
BuildResidualPlot(plotName, plotMeasurements);
}
}
#ifdef DEBUG_INITIALIZATION
MessageInterface::ShowMessage(
"Init complete!\n STM = %s\n Covariance = %s\n",
stm->ToString().c_str(), covariance->ToString().c_str());
#endif
}
