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; }