//------------------------------------------------------------------------------
void BatchEstimator::CalculateData()
{
#ifdef WALK_STATE_MACHINE
MessageInterface::ShowMessage("BatchEstimator state is CALCULATING\n");
#endif
// Update the STM
esm.MapObjectsToSTM();
// Tell the measurement manager to calculate the simulation data
if (measManager.CalculateMeasurements() == false)
{
// No measurements were possible
measManager.AdvanceObservation();
nextMeasurementEpoch = measManager.GetEpoch();
FindTimeStep();
if (currentEpoch < nextMeasurementEpoch)
currentState = PROPAGATING;
else
currentState = ESTIMATING;
}
else if (measManager.GetEventCount() > 0)
{
currentState = LOCATING;
locatingEvent = true;
}
else
currentState = ACCUMULATING;
}
//------------------------------------------------------------------------------
void SequentialEstimator::CalculateData()
{
// Update the STM
esm.MapObjectsToSTM();
esm.MapObjectsToVector();
// Tell the measurement manager to calculate the simulation data
if (measManager.CalculateMeasurements() == false)
{
// No measurements were possible
measManager.AdvanceObservation();
nextMeasurementEpoch = measManager.GetEpoch();
FindTimeStep();
if (currentEpoch < nextMeasurementEpoch)
{
currentState = PROPAGATING;
PrepareForStep();
}
else
currentState = CHECKINGRUN;
}
else if (measManager.GetEventCount() > 0)
{
currentState = LOCATING;
locatingEvent = true;
}
else
{
currentState = ESTIMATING;
}
}
//------------------------------------------------------------------------------
Solver::SolverState BatchEstimator::AdvanceState()
{
switch (currentState)
{
case INITIALIZING:
#ifdef DEBUG_STATE_MACHINE
MessageInterface::ShowMessage("Entered Estimator state machine: "
"INITIALIZING\n");
#endif
// ReportProgress();
CompleteInitialization();
break;
case PROPAGATING:
#ifdef DEBUG_STATE_MACHINE
MessageInterface::ShowMessage("Entered Estimator state machine: "
"PROPAGATING\n");
#endif
// ReportProgress();
FindTimeStep();
break;
case CALCULATING:
#ifdef DEBUG_STATE_MACHINE
MessageInterface::ShowMessage("Entered Estimator state machine: "
"CALCULATING\n");
#endif
// ReportProgress();
CalculateData();
break;
case LOCATING:
#ifdef DEBUG_STATE_MACHINE
MessageInterface::ShowMessage("Entered Estimator state machine: "
"LOCATING\n");
#endif
// ReportProgress();
ProcessEvent();
break;
case ACCUMULATING:
#ifdef DEBUG_STATE_MACHINE
MessageInterface::ShowMessage("Entered Estimator state machine: "
"ACCUMULATING\n");
#endif
// ReportProgress();
Accumulate();
break;
case ESTIMATING:
#ifdef DEBUG_STATE_MACHINE
MessageInterface::ShowMessage("Entered Estimator state machine: "
"ESTIMATING\n");
#endif
// ReportProgress();
Estimate();
break;
case CHECKINGRUN:
#ifdef DEBUG_STATE_MACHINE
MessageInterface::ShowMessage("Entered Estimator state machine: "
"CHECKINGRUN\n");
#endif
// ReportProgress();
CheckCompletion();
break;
case FINISHED:
#ifdef DEBUG_STATE_MACHINE
MessageInterface::ShowMessage("Entered Estimator state machine: "
"FINISHED\n");
#endif
RunComplete();
// ReportProgress();
break;
default:
#ifdef DEBUG_STATE_MACHINE
MessageInterface::ShowMessage("Entered Estimator state machine: "
"Bad state for an estimator.\n");
#endif
/* throw EstimatorException("Solver state not supported for the simulator")*/;
}
return currentState;
}
//------------------------------------------------------------------------------
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?
}
//------------------------------------------------------------------------------
Solver::SolverState SequentialEstimator::AdvanceState()
{
switch (currentState)
{
case INITIALIZING:
#ifdef WALK_STATE_MACHINE
MessageInterface::ShowMessage("Executing the INITIALIZING state\n");
#endif
CompleteInitialization();
break;
case PROPAGATING:
#ifdef WALK_STATE_MACHINE
MessageInterface::ShowMessage("Executing the PROPAGATING state\n");
#endif
FindTimeStep();
break;
case CALCULATING:
#ifdef WALK_STATE_MACHINE
MessageInterface::ShowMessage("Executing the CALCULATING state\n");
#endif
CalculateData();
break;
case LOCATING:
#ifdef WALK_STATE_MACHINE
MessageInterface::ShowMessage("Executing the LOCATING state\n");
#endif
ProcessEvent();
break;
case ESTIMATING:
#ifdef WALK_STATE_MACHINE
MessageInterface::ShowMessage("Executing the ESTIMATING state\n");
#endif
Estimate();
break;
case CHECKINGRUN:
#ifdef WALK_STATE_MACHINE
MessageInterface::ShowMessage("Executing the CHECKINGRUN state\n");
#endif
CheckCompletion();
break;
case FINISHED:
#ifdef WALK_STATE_MACHINE
MessageInterface::ShowMessage("Executing the FINISHED state\n");
#endif
RunComplete();
break;
default:
throw EstimatorException("Unknown state encountered in the " +
instanceName + " sequential estimator.");
}
return currentState;
}
