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