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

# /Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/ModeEstimationPerformance/Monitor/MEPerformanceMonitor.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 "MEPerformanceMonitor.h"
    00010
    00011 #ifdef _MSC_VER
    00012 #pragma warning( disable : 4786 )
    00013 #endif
    00014
    00015 #include <Mers/Estimation/Discrete/Estimate.h>
    00016 #include <Mers/Estimation/Discrete/Trajectories.h>
    00017 #include <Mers/Estimation/Discrete/MEObjectiveFunction.h>
    00018 #include <Mers/Generators/BFIG/BFIGGenerator.h>
    00019 #include <Mers/Solvers/SAT/GATSAT/OpSAT/Opsat.h>
    00020 #include <Mers/Models/Base/BaseModelAssignment.h>
    00021 #include <Mers/Models/CCA/CCAModel.h>
    00022 #include <Mers/Models/CCA/CCAState.h>
    00023 #include <Mers/Utils/Containers/IntegerArray.h>
    00024
    00025 using Mers::Estimation::Discrete::ModeEstimationPerformance::MEPerformanceMonitor;
    00026 using Mers::Estimation::Discrete::Trajectories;
    00027 using Mers::Estimation::Discrete::MEObjectiveFunction;
    00028 using Mers::Estimation::Discrete::Estimate;
    00029 using Mers::Generators::BFIG::BFIGGenerator;
    00030 using Mers::Solvers::SAT::GATSAT::OpSAT::Opsat;
    00031 using Mers::Models::Base::BaseModelAssignment;
    00032 using Mers::Models::Base::BaseModelClauseArray;
    00033 using Mers::Models::CCA::CCAModel;
    00034 using Mers::Models::CCA::CCAState;
    00035 using Mers::Utils::Containers::IntegerArray;
    00036 using std::vector;
    00037 using std::cerr;
    00038 using std::endl;
    00039 using std::cout;
    00040 using std::map;
    00041
    00042         //-----------------------------------------------
    00043         //      CONSTRUCTOR
    00044         //-----------------------------------------------
    00045
    00046 MEPerformanceMonitor::MEPerformanceMonitor()
    00047 {
    00048         numEstimatesGenerated = 0;
    00049         numConflictsGenerated = 0;
    00050         maxQueueSizeInBFIG = 0;
    00051         numElementsExpanded = 0;
    00052         numCandidatesGenerated = 0;
    00053         numElementsPreviouslyExpanded = 0;
    00054 }
    00055
    00056
    00057         //-----------------------------------------------
    00058         //      DESTRUCTOR
    00059         //-----------------------------------------------
    00060
    00061
    00062 MEPerformanceMonitor::~MEPerformanceMonitor()
    00063 {
    00064 }
    00065
    00066
    00067         //-----------------------------------------------
    00068         //      PUBLIC METHODS
    00069         //-----------------------------------------------
    00070 void
    00071 MEPerformanceMonitor::gatherEstimationCycleResults(vector<BFIGGenerator *> & bfigs)
    00072 {
    00073         // Number of conflicts generated w/o double counting them
    00074         numConflictsGenerated = bfigs[0]->getNumberOfConflicts();
    00075
    00076         // Count up the number of elements expanded in all the queues;
    00077         numElementsExpanded = 0;
    00078         maxQueueSizeInBFIG = 0;
    00079         numCandidatesGenerated = 0;
    00080         for(size_t i=0; i<bfigs.size(); i++)
    00081         {
    00082                 numElementsExpanded += bfigs[i]->getTotalNumberNodesExpanded();
    00083                 maxQueueSizeInBFIG += bfigs[i]->getLargestQueueSinceReset();
    00084                 numCandidatesGenerated += bfigs[i]->getNumberCandidatesSinceReset();
    00085         }
    00086         numElementsExpanded -= numElementsPreviouslyExpanded;
    00087         numElementsPreviouslyExpanded += numElementsExpanded;
    00088
    00089         return;
    00090 }
    00091
    00092 void
    00093 MEPerformanceMonitor::gatherEstimationCycleResults(vector<Opsat *> & opsats)
    00094 {
    00095         vector<BFIGGenerator *> bfigs;
    00096         for(size_t i=0; i<opsats.size(); i++)
    00097                 bfigs.push_back((BFIGGenerator *)opsats[i]->getGenerator());
    00098
    00099         return gatherEstimationCycleResults(bfigs);
    00100 }
    00101
    00102
    00103 unsigned int
    00104 MEPerformanceMonitor::getNumConflictsGenerated(void) const
    00105 {
    00106         return numConflictsGenerated;
    00107 }
    00108
    00109 unsigned int
    00110 MEPerformanceMonitor::getNumEstimatesGenerated(void) const
    00111 {
    00112         return numEstimatesGenerated;
    00113 }
    00114
    00115 unsigned int
    00116 MEPerformanceMonitor::getNumElementsExpanded(void) const
    00117 {
    00118         return numElementsExpanded;
    00119 }
    00120
    00121 unsigned int
    00122 MEPerformanceMonitor::getMaxQueueSize(void) const
    00123 {
    00124         return maxQueueSizeInBFIG;
    00125 }
    00126
    00127 unsigned int
    00128 MEPerformanceMonitor::getNumCandidatesGenerated(void) const
    00129 {
    00130         return numCandidatesGenerated;
    00131 }
    00132
    00133 void
    00134 MEPerformanceMonitor::adjustUncertaintyForNewEstimate(Estimate * anEst, bool createNewCurrentTrajectories)
    00135 {
    00136         if(createNewCurrentTrajectories)
    00137         {
    00138                 currentStateProbsWithoutNormalization.clear();
    00139                 currentStateProbsWithoutNormalization[0] = anEst->getProbability();
    00140         }
    00141         else
    00142                 currentStateProbsWithoutNormalization[(unsigned int)currentStateProbsWithoutNormalization.size()] = anEst->getProbability();
    00143
    00144         double totalProb = 0.0;
    00145         for(size_t i = 0; i<currentStateProbsWithoutNormalization.size(); i++)
    00146                 totalProb += currentStateProbsWithoutNormalization[i];
    00147
    00148         if(totalProb >= 1.0000001)
    00149                 cout << "<ModeEstimatorDebug> WARNING: Belief state probability is greater than 1!" << endl;
    00150
    00151         return;
    00152 }
    00153
    00154 unsigned int
    00155 MEPerformanceMonitor::getNumberFaultTransitions(const Trajectories * nextTrajs, const Trajectories * currentTrajs, const CCAModel & cca) const
    00156 {
    00157         if(nextTrajs == NULL || currentTrajs == NULL)
    00158         {
    00159                 cerr << "<MEPerformanceMonitor> ERROR: No current/next trajectories to search for fault transitions!" << endl;
    00160                 return 0;
    00161         }
    00162
    00163         unsigned int i;
    00164         unsigned int numFaults = 0;
    00165
    00166
    00167         for(i=0; i<nextTrajs->getNumberEstimates(); i++)
    00168                 numFaults += getEstimateFaultTransitions(i, nextTrajs, currentTrajs, cca);
    00169
    00170         return numFaults;
    00171 }
    00172
    00173
    00174 unsigned int
    00175 MEPerformanceMonitor::getEstimateFaultTransitions(unsigned int estNum, const Trajectories * nextTrajs,
    00176                                                 const Trajectories * currentTrajs, const CCAModel & cca) const
    00177 {
    00178         if(nextTrajs == NULL)
    00179         {
    00180                 cerr << "<MEPerformanceMonitor> ERROR: No trajectories to search for fault transitions!" << endl;
    00181                 return 0;
    00182         }
    00183         if(estNum >= nextTrajs->getNumberEstimates())
    00184         {
    00185                 cerr << "<MEPerformanceMonitor> ERROR: Requested estimate number larger than the number of trajectores!" << endl;
    00186                 return 0;
    00187         }
    00188
    00189         unsigned int j, k;
    00190         unsigned int numFaults = 0;
    00191         IntegerArray * sourceVar = new IntegerArray();
    00192         IntegerArray * sourceVal = new IntegerArray();
    00193         IntegerArray * targetVar = new IntegerArray();
    00194         IntegerArray * targetVal = new IntegerArray();
    00195         bool faultTransitionMatch = false;
    00196
    00197         Estimate const * est = nextTrajs->getEstimate(estNum);
    00198         for(j=0; j<est->getComponentAssignments()->getLength(); j++)
    00199         {
    00200                 const BaseModelAssignment * nextModeAssign = est->getAssignment(j);
    00201                 const BaseModelAssignment * currentModeAssign = currentTrajs->getEstimate(est->getOriginatedTrajectoryNumber())->getAssignment(j);
    00202                 if(cca.automaton_p(nextModeAssign->getVariable()->getUID())->state_p(*nextModeAssign->getValue())->getStateType()
    00203                         == CCAState::FAILURE && currentModeAssign->getValue()->getUID() != nextModeAssign->getValue()->getUID())
    00204                 {
    00205                         if(currentTrajs->getEstimate(est->getOriginatedTrajectoryNumber())->getAssignment(j)->getVariable()->getUID()
    00206                                 != nextModeAssign->getVariable()->getUID())
    00207                         {
    00208                                 cerr << "<MEPerformanceMonitor> ERROR: Source and target variables do not match!" << endl;
    00209                                 return 0;
    00210                         }
    00211                         for(k=0; k<sourceVar->size(); k++)
    00212                         {
    00213                                 if(currentModeAssign->getVariable()->getUID() == (*sourceVar)[k] && currentModeAssign->getValue()->getUID() ==
    00214                                         (*sourceVal)[k] && nextModeAssign->getVariable()->getUID() == (*targetVar)[k] &&
    00215                                         nextModeAssign->getValue()->getUID() == (*targetVal)[k])
    00216                                 {
    00217                                         faultTransitionMatch = true;
    00218                                         break;
    00219                                 }
    00220                         }
    00221                         if(!faultTransitionMatch)
    00222                         {
    00223                                 sourceVar->pushBack(currentModeAssign->getVariable()->getUID());
    00224                                 sourceVal->pushBack(currentModeAssign->getValue()->getUID());
    00225                                 targetVar->pushBack(nextModeAssign->getVariable()->getUID());
    00226                                 targetVal->pushBack(nextModeAssign->getValue()->getUID());
    00227                                 numFaults++;
    00228                         }
    00229                         faultTransitionMatch = false;
    00230                 }
    00231         }
    00232         delete sourceVar;
    00233         delete sourceVal;
    00234         delete targetVar;
    00235         delete targetVal;
    00236         return numFaults;
    00237 }
    00238
    00239
    00240 double
    00241 MEPerformanceMonitor::computeAccumulatedBeliefStateUncertainty(
    00242                                                                                                                 MEObjectiveFunction * objectiveFunction,
    00243                                                                                                                 const Trajectories * nextTrajectories ) const
    00244 {
    00245         double uncertainty = 1.0;
    00246         map<unsigned int, double> priorNextStateProbs = getPriorNextStateProbs(objectiveFunction, nextTrajectories);
    00247         for(size_t i=0; i<priorNextStateProbs.size(); i++)
    00248                 uncertainty -= priorNextStateProbs[i];
    00249         return uncertainty;
    00250 }
    00251
    00252
    00253
    00254 void
    00255 MEPerformanceMonitor::progressPriorCurrentStateProbs(   MEObjectiveFunction * objectiveFunction,
    00256                                                                                                                 const Trajectories * nextTrajectories )
    00257 {
    00258         currentStateProbsWithoutNormalization = getPriorNextStateProbs(objectiveFunction, nextTrajectories);
    00259         return;
    00260 }
    00261
    00262
    00263 map<unsigned int, double>
    00264 MEPerformanceMonitor::getPriorNextStateProbs( MEObjectiveFunction * objectiveFunction,
    00265                                                                                                 const Trajectories * nextTrajectories ) const
    00266 {
    00267         map<unsigned int, double> nextStateProbs;
    00268         // Update the objective function with the un-normalized probabilities for tracking belief state uncertainty
    00269         objectiveFunction->updateStateProbabilities(currentStateProbsWithoutNormalization);
    00270
    00271         // Get the a priori state probabilities for the next trajectories
    00272         for(unsigned int i=0; i<nextTrajectories->getNumberEstimates(); i++)
    00273                 nextStateProbs[i] = objectiveFunction->calculatePropagationUtility(
    00274                                                 nextTrajectories->getEstimate(i)->getComponentAssignments(),
    00275                                                 nextTrajectories->getEstimate(i)->getOriginatedTrajectoryNumber());
    00276
    00277         return nextStateProbs;
    00278 }
    00279         //-----------------------------------------------
    00280         //      PRIVATE METHODS
    00281         //-----------------------------------------------
    00282