Exemple #1
0
//------------------------------------------------------------------------------
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
}
Exemple #2
0
//------------------------------------------------------------------------------
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
}