//------------------------------------------------------------------------------
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 ExtendedKalmanInv::Estimate()
{
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("\n\n---------------------\n");
MessageInterface::ShowMessage("Current covariance:\n");
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf", (*covariance)(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
MessageInterface::ShowMessage("Current stm:\n");
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf", (*stm)(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
MessageInterface::ShowMessage("Current State: [ ");
for (UnsignedInt i = 0; i < stateSize; ++i)
MessageInterface::ShowMessage(" %.12lf ", (*estimationState)[i]);
MessageInterface::ShowMessage("\n");
#endif
// Perform the time update of the covariances, phi P phi^T, and the state
TimeUpdate();
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("Time updated matrix \\bar P:\n");
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf", pBar(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
#endif
// Construct the O-C data and H tilde
ComputeObs();
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("hTilde:\n");
for (UnsignedInt i = 0; i < measSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf", hTilde[i][j]);
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
#endif
// Then the Kalman gain
ComputeGain();
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("The Kalman gain is: \n");
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < measSize; ++j)
MessageInterface::ShowMessage(" %.12lf", kalman(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
#endif
// Finally, update everything
UpdateElements();
// Plot residuals if set
if (showAllResiduals)
PlotResiduals();
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("Updated covariance:\n");
for (UnsignedInt i = 0; i < stateSize; ++i)
{
for (UnsignedInt j = 0; j < stateSize; ++j)
MessageInterface::ShowMessage(" %.12lf", (*covariance)(i,j));
MessageInterface::ShowMessage("\n");
}
MessageInterface::ShowMessage("\n");
MessageInterface::ShowMessage("Updated State: [ ");
for (UnsignedInt i = 0; i < stateSize; ++i)
MessageInterface::ShowMessage(" %.12lf ", (*estimationState)[i]);
MessageInterface::ShowMessage("\n\n---------------------\n");
#endif
// ReportProgress();
// Advance MeasMan to the next measurement and get its epoch
measManager.AdvanceObservation();
nextMeasurementEpoch = measManager.GetEpoch();
FindTimeStep();
#ifdef DEBUG_ESTIMATION
MessageInterface::ShowMessage("CurrentEpoch = %.12lf, next "
"epoch = %.12lf, timeStep = %.12lf\n", currentEpoch,
nextMeasurementEpoch, timeStep);
#endif
if (currentEpoch < nextMeasurementEpoch)
{
// Reset the STM
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();
esm.MapVectorToObjects();
PropagationStateManager *psm = propagator->GetPropStateManager();
psm->MapObjectsToVector();
// Flag that a new current state has been loaded in the objects
resetState = true;
currentState = PROPAGATING;
}
else
currentState = CHECKINGRUN; // Should this just go to FINISHED?
}
