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