//------------------------------------------------------------------------------
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 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
}
