- Main Page
- Namespaces
- Classes
- Files
- Related Pages

# /Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/ModeEstimationPerformance/Monitor/ModeEstimatorDebug.cpp

    00001 //==============================================================================
    00002 // Copyright (c) 2004           Model-based Embedded and Robotic Systems Group
    00003 // All Rights Reserved          Massachusetts Institute of Technology
    00004 //
    00005 // This software is provided as is. Model-based Embedded and Robotic Systems
    00006 // (MERS) group does not assume any responsibility.
    00007 //==============================================================================
    00008
    00009 #include "ModeEstimatorDebug.h"
    00010
    00011 #ifdef _MSC_VER
    00012 #pragma warning( disable : 4786 )
    00013 #endif
    00014
    00015 #include <Mers/Estimation/Discrete/BFTEObjectiveFunction.h>
    00016 #include <Mers/Estimation/Discrete/Estimate.h>
    00017 #include <Mers/Estimation/Discrete/EstimateHeap.h>
    00018 #include <Mers/Estimation/Discrete/Trajectories.h>
    00019 #include <Mers/Models/Base/BaseModelClauseArray.h>
    00020 #include <Mers/Models/CCA/CCAModel.h>
    00021 #include <Mers/Generators/BFIG/BFIGGenerator.h>
    00022 #include <Mers/Solvers/SAT/GATSAT/OpSAT/Opsat.h>
    00023
    00024 using Mers::Estimation::Discrete::ModeEstimationPerformance::ModeEstimatorDebug;
    00025 using Mers::Estimation::Discrete::ModeEstimationPerformance::MEPerformanceMonitor;
    00026 using Mers::Estimation::Discrete::ModeEstimation::ModeEstimator;
    00027 using Mers::Estimation::Discrete::BFTEObjectiveFunction;
    00028 using Mers::Estimation::Discrete::Trajectories;
    00029 using Mers::Estimation::Discrete::Estimate;
    00030 using Mers::Estimation::Discrete::EstimateHeap;
    00031 using Mers::Estimation::Discrete::Dissent;
    00032 using Mers::Estimation::Discrete::CompiledTransition;
    00033 using Mers::Models::Base::BaseModel;
    00034 using Mers::Models::Base::BaseModelClauseArray;
    00035 using Mers::Models::CCA::CCAModel;
    00036 using Mers::Generators::BFIG::BFIGGenerator;
    00037 using Mers::Solvers::SAT::GATSAT::OpSAT::Opsat;
    00038 using std::vector;
    00039 using std::cerr;
    00040 using std::endl;
    00041
    00042         //-----------------------------------------------
    00043         //      CONSTRUCTOR
    00044         //-----------------------------------------------
    00045
    00046 ModeEstimatorDebug::ModeEstimatorDebug(const CCAModel &cca, Trajectories * initialTrajectories)
    00047 : ModeEstimator(cca, initialTrajectories), MEPerformanceMonitor() //, MEDebugInterface(this)
    00048 {
    00049 }
    00050
    00051         //-----------------------------------------------
    00052         //      DESTRUCTOR
    00053         //-----------------------------------------------
    00054
    00055
    00056 ModeEstimatorDebug::~ModeEstimatorDebug()
    00057 {
    00058 }
    00059
    00060
    00061         //-----------------------------------------------
    00062         //      PUBLIC METHODS
    00063         //-----------------------------------------------
    00064
    00065
    00066 const Estimate *
    00067 ModeEstimatorDebug::findModeEstimate(void)
    00068 {
    00069         if(nextTrajectories != NULL)            // For testing, always have the estimator generate new
    00070         {                                                                       // trajectories; even if nothing has changed
    00071                 delete nextTrajectories;
    00072                 nextTrajectories = NULL;
    00073         }
    00074         if(!generateTrajectories())
    00075         {
    00076                 cerr << "<ModeEstimatorBase> ERROR: Couldn't generate new trajectories!" << endl;
    00077                 return NULL;
    00078         }
    00079         return nextTrajectories->getMostLikelyEstimate();
    00080 }
    00081
    00082
    00083 const Estimate *
    00084 ModeEstimatorDebug::progressToEstimate(void)
    00085 {
    00086         findModeEstimate();
    00087         MEPerformanceMonitor::progressPriorCurrentStateProbs(objectiveFunction, nextTrajectories);
    00088         return ModeEstimatorBase::progressToEstimate();
    00089 }
    00090
    00091
    00092 unsigned int
    00093 ModeEstimatorDebug::getNumberFaultTransitions()
    00094 {
    00095         if(nextTrajectories == NULL)
    00096         {
    00097                 cerr << "<ModeEstimatorDebug> ERROR: No next trajectories to get fault transitions for!" << endl;
    00098                 return 0;
    00099         }
    00100         else if(currentTrajectories == NULL)
    00101         {
    00102                 cerr << "<ModeEstimatorDebug> ERROR: No current trajectories to get fault transitions from!" << endl;
    00103                 return 0;
    00104         }
    00105
    00106         return MEPerformanceMonitor::getNumberFaultTransitions(nextTrajectories, currentTrajectories, myCCAmodel);
    00107 }
    00108
    00109
    00110 unsigned int
    00111 ModeEstimatorDebug::getEstimateFaultTransitions(unsigned int estIndex)
    00112 {
    00113         if(!ModeEstimator::findNextTrajectories())
    00114         {
    00115                 cerr << "<ModeEstimatorDebug> ERROR: Couldn't generate next trajectories!" << endl;
    00116                 return 0;
    00117         }
    00118
    00119         return MEPerformanceMonitor::getEstimateFaultTransitions(estIndex, nextTrajectories, currentTrajectories, myCCAmodel);
    00120 }
    00121
    00122 double
    00123 ModeEstimatorDebug::getAccumulatedUncertainty(void) const
    00124 {
    00125         if(nextTrajectories == NULL)
    00126         {
    00127                 std::cout << "<ModeEstimatorDebug> WARNING: nextTrajectories does not exist! Returning 0.0" << endl;
    00128                 return 0.0;
    00129         }
    00130         return MEPerformanceMonitor::computeAccumulatedBeliefStateUncertainty(objectiveFunction, nextTrajectories);
    00131 }
    00132
    00133         //-----------------------------------------------
    00134         //      PROTECTED METHODS
    00135         //-----------------------------------------------
    00136
    00137 bool
    00138 ModeEstimatorDebug::generateTrajectories(void)
    00139 {
    00140         // Intialize estimate counter (timing requirements)
    00141         numEstimatesGenerated = 0;
    00142
    00143         // Call base clasee function
    00144         return ModeEstimator::generateTrajectories();
    00145 }
    00146
    00147 bool
    00148 ModeEstimatorDebug::findNextBestStateAndPutInHeap(unsigned int opsatNum, const vector<double> & allProbs,
    00149                                                                 EstimateHeap * nextTrajsEstHeap, bool willWarn)
    00150 {
    00151         // Increment the number of estimates generated
    00152         numEstimatesGenerated++;
    00153
    00154         // Call base class function
    00155         return ModeEstimatorBase::findNextBestStateAndPutInHeap(opsatNum, allProbs, nextTrajsEstHeap, willWarn);
    00156 }
    00157
    00158 bool
    00159 ModeEstimatorDebug::releaseMemoryAndNormalize(BaseModelClauseArray * currentObservationAssignments,
    00160                                                                 EstimateHeap * nextTrajsEstHeap)
    00161 {
    00162         // Collect all the performance data from BFIG (function located in MEPerformanceMonitor)
    00163         gatherEstimationCycleResults(opsats);
    00164
    00165         // Call base clasee function
    00166         return ModeEstimator::releaseMemoryAndNormalize(currentObservationAssignments, nextTrajsEstHeap);
    00167 }
    00168
    00169 bool
    00170 ModeEstimatorDebug::addEstimateToCurrentTrajectories(Estimate * anEst, bool createNewCurrentTrajectories)
    00171 {
    00172         bool statusFlag = ModeEstimatorBase::addEstimateToCurrentTrajectories(anEst, createNewCurrentTrajectories);
    00173         if(statusFlag)
    00174                 MEPerformanceMonitor::adjustUncertaintyForNewEstimate(anEst, createNewCurrentTrajectories);
    00175         return statusFlag;
    00176 }