void SteepestDescent::RunPerturbation()
{
// Calculate the perts, one at a time
if (pertNumber != -1)
// Back out the last pert applied
variable.at(pertNumber) = lastUnperturbedValue;
++pertNumber;
if (pertNumber == variableCount) // Current set of perts have been run
{
currentState = CALCULATING;
pertNumber = -1;
return;
}
lastUnperturbedValue = variable.at(pertNumber);
variable.at(pertNumber) += perturbation.at(pertNumber);
pertDirection.at(pertNumber) = 1.0;
if (variable[pertNumber] > variableMaximum[pertNumber])
{
pertDirection.at(pertNumber) = -1.0;
variable[pertNumber] -= 2.0 * perturbation[pertNumber];
}
if (variable[pertNumber] < variableMinimum[pertNumber])
{
pertDirection.at(pertNumber) = -1.0;
variable[pertNumber] -= 2.0 * perturbation[pertNumber];
}
WriteToTextFile();
}
//------------------------------------------------------------------------------
void Solver::CompleteInitialization()
{
OpenSolverTextFile();
WriteToTextFile();
currentState = NOMINAL;
// Reset initial values if in DiscardAndContinue mode
if (exitMode == DISCARD)
{
ResetVariables();
}
}
//------------------------------------------------------------------------------
void BatchEstimator::RunComplete()
{
#ifdef WALK_STATE_MACHINE
MessageInterface::ShowMessage("BatchEstimator state is FINALIZING\n");
#endif
measManager.ProcessingComplete();
// Report the results
WriteToTextFile();
ReportProgress();
if (showAllResiduals)
PlotResiduals();
}
//------------------------------------------------------------------------------
void BatchEstimator::CheckCompletion()
{
#ifdef WALK_STATE_MACHINE
MessageInterface::ShowMessage("BatchEstimator state is CHECKINGRUN\n");
#endif
#ifdef DEBUG_VERBOSE
MessageInterface::ShowMessage("\nCompleted iteration %d\n\n",
iterationsTaken+1);
#endif
std::string convergenceReason = "";
converged = TestForConvergence(convergenceReason);
#ifdef RUN_SINGLE_PASS
converged = true;
#endif
++iterationsTaken;
if ((converged) || (iterationsTaken >= maxIterations))
{
#ifdef DEBUG_VERBOSE
if (converged)
MessageInterface::ShowMessage("Estimation has converged\n%s\n\n",
convergenceReason.c_str());
else
MessageInterface::ShowMessage("Estimation has reached the maximum "
"iteration count, but has not converged\n\n");
#endif
currentState = FINISHED;
}
else
{
if (showAllResiduals)
PlotResiduals();
// Reset to the new initial state, clear the processed data, etc
esm.RestoreObjects(&outerLoopBuffer);
esm.MapVectorToObjects();
esm.MapObjectsToSTM();
currentEpoch = estimationEpoch;
measManager.Reset();
nextMeasurementEpoch = measManager.GetEpoch();
// Need to reset STM and covariances
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;
}
measurementResiduals.clear();
measurementEpochs.clear();
measurementResidualID.clear();
for (UnsignedInt i = 0; i < stateSize; ++i)
for (UnsignedInt j = 0; j < stateSize; ++j)
if (i == j)
(*stm)(i,j) = 1.0;
else
(*stm)(i,j) = 0.0;
esm.MapSTMToObjects();
for (Integer i = 0; i < information.GetNumRows(); ++i)
{
residuals[i] = 0.0;
x0bar[i] -= dx[i];
}
if (useApriori)
for (Integer i = 0; i < information.GetNumRows(); ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
{
residuals[i] += information(i,j) * x0bar[j];
}
}
#ifdef DEBUG_VERBOSE
MessageInterface::ShowMessage("Starting iteration %d\n\n",
iterationsTaken+1);
#endif
#ifdef DEBUG_ITERATIONS
MessageInterface::ShowMessage(
"Init complete!\n STM = %s\n Covariance = %s\n",
stm->ToString().c_str(), covariance->ToString().c_str());
#endif
WriteToTextFile();
ReportProgress();
if (GmatMathUtil::IsEqual(currentEpoch, nextMeasurementEpoch))
currentState = CALCULATING;
else
{
timeStep = (nextMeasurementEpoch - currentEpoch) *
GmatTimeConstants::SECS_PER_DAY;
currentState = PROPAGATING;
}
}
}
//------------------------------------------------------------------------------
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
}
//------------------------------------------------------------------------------
Solver::SolverState SteepestDescent::AdvanceState()
{
switch (currentState)
{
case INITIALIZING:
#ifdef SD_DEBUG_STATE_MACHINE
MessageInterface::ShowMessage(wxT("Entered state machine; ")
wxT("INITIALIZING\n"));
#endif
iterationsTaken = 0;
WriteToTextFile();
// ReportProgress();
CompleteInitialization();
#ifdef SD_DEBUG_STATE_MACHINE
MessageInterface::ShowMessage(
wxT("SteepestDescent State Transitions from %d to %d\n"),
INITIALIZING, currentState);
#endif
break;
case NOMINAL:
#ifdef SD_DEBUG_STATE_MACHINE
MessageInterface::ShowMessage(wxT("Entered state machine; ")
wxT("NOMINAL\n"));
#endif
// ReportProgress();
RunNominal();
// ReportProgress();
#ifdef SD_DEBUG_STATE_MACHINE
MessageInterface::ShowMessage(
wxT("SteepestDescent State Transitions from %d to %d\n"), NOMINAL,
currentState);
#endif
// ReportProgress();
break;
case PERTURBING:
#ifdef SD_DEBUG_STATE_MACHINE
MessageInterface::ShowMessage(wxT("Entered state machine; ")
wxT("PERTURBING\n"));
#endif
RunPerturbation();
#ifdef SD_DEBUG_STATE_MACHINE
MessageInterface::ShowMessage(
wxT("SteepestDescent State Transitions from %d to %d\n"), PERTURBING,
currentState);
#endif
// ReportProgress();
break;
case Solver::CALCULATING:
#ifdef SD_DEBUG_STATE_MACHINE
MessageInterface::ShowMessage(wxT("Entered state machine; ")
wxT("CALCULATING\n"));
#endif
// ReportProgress();
CalculateParameters();
#ifdef SD_DEBUG_STATE_MACHINE
MessageInterface::ShowMessage(
wxT("SteepestDescent State Transitions from %d to %d\n"), CALCULATING,
currentState);
#endif
break;
case CHECKINGRUN:
#ifdef SD_DEBUG_STATE_MACHINE
MessageInterface::ShowMessage(wxT("Entered state machine; ")
wxT("CHECKINGRUN\n"));
#endif
CheckCompletion();
#ifdef SD_DEBUG_STATE_MACHINE
MessageInterface::ShowMessage(
wxT("SteepestDescent State Transitions from %d to %d\n"), CHECKINGRUN,
currentState);
#endif
// ReportProgress();
break;
case FINISHED:
#ifdef SD_DEBUG_STATE_MACHINE
MessageInterface::ShowMessage(wxT("Entered state machine; ")
wxT("FINISHED\n"));
#endif
RunComplete();
#ifdef SD_DEBUG_STATE_MACHINE
MessageInterface::ShowMessage(
wxT("SteepestDescent State Transitions from %d to %d\n"), FINISHED,
currentState);
#endif
// ReportProgress();
break;
default:
throw SolverException(
wxT("Steepest Descent Solver \"") + instanceName +
wxT("\" encountered an unexpected state."));
}
return currentState;
}
