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

# /Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/ModeEstimation/ModeEstimator.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 "ModeEstimator.h"
    00010
    00011 #ifdef _MSC_VER
    00012 #pragma warning( disable : 4786 )
    00013 #endif
    00014
    00015
    00016 #include <Mers/Models/Base/BaseModelAssignment.h>
    00017 #include <Mers/Models/Base/BaseModelAssignmentArray.h>
    00018 #include <Mers/Models/Base/BaseModelVariable.h>
    00019 #include <Mers/Models/Base/BaseModelVariableArray.h>
    00020 #include <Mers/Models/Base/BaseModelClause.h>
    00021 #include <Mers/Models/Base/BaseModelClauseArray.h>
    00022 #include <Mers/Models/Base/BaseModelValue.h>
    00023 #include <Mers/Models/CCA/CCAAutomaton.h>
    00024 #include <Mers/Models/CCA/CCATransition.h>
    00025 #include <Mers/Models/CCA/CCATransitions.h>
    00026
    00027 #include <Mers/Generators/BFIG/BFIGGenerator.h>
    00028 #include <Mers/Solvers/SAT/GATSAT/OpSAT/Opsat.h>
    00029 #include <Mers/Solvers/SAT/AssignmentTesterInterface.h>
    00030
    00031 #include <Mers/Estimation/Discrete/BFTEObjectiveFunction.h>
    00032 #include "../Estimate.h"
    00033 #include "../EstimateHeap.h"
    00034 #include "../Trajectories.h"
    00035 #include "../Trigger.h"
    00036 #include "../Dissent.h"
    00037 #include "../CompiledTransition.h"
    00038
    00039
    00040 using namespace Mers::Estimation::Discrete::ModeEstimation;
    00041 using namespace Mers::Estimation::Discrete;
    00042 using Mers::Models::Base::BaseModelAssignment;
    00043 using Mers::Models::Base::BaseModelAssignmentArray;
    00044 using Mers::Models::Base::BaseModelVariable;
    00045 using Mers::Models::Base::BaseModelVariableArray;
    00046 using Mers::Models::Base::BaseModelClause;
    00047 using Mers::Models::Base::BaseModelClauseArray;
    00048 using Mers::Models::Base::BaseModelValue;
    00049 using Mers::Models::CCA::CCAModel;
    00050 using Mers::Models::CCA::CCASentence;
    00051 using Mers::Models::CCA::CCAAutomaton;
    00052 using Mers::Models::CCA::CCATransition;
    00053 using Mers::Models::CCA::CCATransitions;
    00054 using Mers::Generators::BFIG::BFIGGenerator;
    00055 using Mers::Solvers::SAT::GATSAT::OpSAT::Opsat;
    00056 using Mers::Utils::Containers::DynamicArrayTemplate;
    00057 using std::vector;
    00058 using std::map;
    00059 using std::cerr;
    00060 using std::cout;
    00061 using std::endl;
    00062
    00063
    00064         //-----------------------------------------------
    00065         //      CONSTRUCTOR
    00066         //-----------------------------------------------
    00067
    00068 ModeEstimator::ModeEstimator(const CCAModel &cca, Trajectories * initialTrajectories)
    00069 : ModeEstimatorBase(cca, emptyDissentVector(), extractTransitionsFromCCA(cca), initialTrajectories), myCCAmodel(cca)
    00070 {
    00071         observationVariables = new BaseModelVariableArray(false);
    00072         for(unsigned int i=0; i<cca.getVariables()->getLength(); i++)
    00073         {
    00074                 if((*cca.getVariables())[i]->getVariableType() == BaseModelVariable::OBSERVABLE)
    00075                         observationVariables->pushBack(const_cast<BaseModelVariable *>((*cca.getVariables())[i]));
    00076                                                         // This is a big hack but BaseModelVariableArray doesn't currently allow for const *.
    00077                                                         // Should be changed in the future.
    00078         }
    00079         globalConstraints = new CCASentence(myCCAmodel.get_all_constraints());
    00080
    00081         // -------------- Set up the objective function ----------------
    00082         // Probability map that maps UIDs instead of just array indices
    00083         map<unsigned int, unsigned int> varUIDtoProbMap;
    00084
    00085         unsigned int nextVarPos = 0;
    00086         for(unsigned int i=0; i < decisionVariables->getLength(); i++)
    00087         {
    00088                 varUIDtoProbMap[(*decisionVariables)[i]->getUID()] = nextVarPos;
    00089                 nextVarPos += (*decisionVariables)[i]->getDomain()->getLength();
    00090         }
    00091
    00092         vector<double> emptyTransProbs;
    00093         map<unsigned int,double> emptyStateProbs;
    00094
    00095         objectiveFunction = new BFTEObjectiveFunction(decisionVariables, varUIDtoProbMap, emptyTransProbs, emptyStateProbs);
    00096 }
    00097
    00098
    00099         //-----------------------------------------------
    00100         //      DESTRUCTOR
    00101         //-----------------------------------------------
    00102
    00103
    00104 ModeEstimator::~ModeEstimator()
    00105 {
    00106         // Release OPSAT Memory
    00107         Opsat * oldOpsat;
    00108         while(opsats.size() != 0)
    00109         {
    00110                 oldOpsat = opsats.back();
    00111                 opsats.pop_back();
    00112                 delete oldOpsat;
    00113         }
    00114
    00115         delete observationVariables;
    00116         delete globalConstraints;
    00117
    00118         if(objectiveFunction != NULL)
    00119         {
    00120                 delete objectiveFunction;
    00121                 objectiveFunction = NULL;
    00122         }
    00123 }
    00124
    00125
    00126         //-----------------------------------------------
    00127         //      PUBLIC METHODS
    00128         //-----------------------------------------------
    00129
    00130 void
    00131 ModeEstimator::setDebug(const unsigned short db)
    00132 {
    00133         unsigned short reducedDB = 0;
    00134         if(db > 2)
    00135                 reducedDB = db-3;
    00136         for(unsigned int i=0; i<opsats.size(); i++)
    00137                 opsats[i]->setDebug(reducedDB);
    00138
    00139         ModeEstimatorBase::setDebug(db);
    00140         return;
    00141 }
    00142
    00143         //-----------------------------------------------
    00144         //      PRIVATE METHODS
    00145         //-----------------------------------------------
    00146
    00147 bool
    00148 ModeEstimator::generateTrajectories(void)
    00149 {
    00150         unsigned int i, j;
    00151         unsigned int obsValueUID;
    00152         Estimate * anEst;
    00153         EstimateHeap bestFirstHeap = EstimateHeap(true);
    00154         BaseModelClauseArray * currentObservationAssignments = new BaseModelClauseArray(true);
    00155         BaseModelClause * aObservationClause;
    00156         vector<double> allProbabilities = compiledObjectTrigger->getNormalizedEnabledTransitionProbabilities();
    00157         vector<double> probabilities;
    00158         map<unsigned int, double> currentStateProbabilities;
    00159
    00160         if(currentTrajectories->getNumberEstimates() < 1)
    00161         {
    00162                 cerr << "<ModeEstimator> ERROR: There are no current trajectories!" << endl;
    00163                 cleanAfterFailure(&bestFirstHeap);
    00164                 return false;
    00165         }
    00166
    00167         if(debug > 1)
    00168         {
    00169                 cout << *compiledObjectTrigger << endl << endl;
    00170
    00171                 for(i=0; i<allProbabilities.size(); i++)
    00172                 {
    00173                         if(i%(allProbabilities.size()/currentTrajectories->getNumberEstimates()) == 0)
    00174                                 cout << endl << "Probabilities for Trajectory " <<
    00175                                 i/(allProbabilities.size()/currentTrajectories->getNumberEstimates()) << ": " << endl;
    00176                         cout << allProbabilities[i] << ", ";
    00177                 }
    00178                 cout << endl << endl;
    00179         }
    00180
    00181         // Gather all the current state probabilities
    00182         for(i=0; i < currentTrajectories->getNumberEstimates(); i++)
    00183                 currentStateProbabilities[i] = currentTrajectories->getEstimate(i)->getProbability();
    00184
    00185         // Update the objective function with current probability data
    00186         objectiveFunction->updateProbabilities(allProbabilities, currentStateProbabilities);
    00187
    00188         // Set the size of the vectors
    00189         probabilities.resize(allProbabilities.size()/currentTrajectories->getNumberEstimates());
    00190
    00191         // Create clause constraints for all the current observations
    00192         for(i=0; i<observationVariables->getLength(); i++)
    00193         {
    00194                 obsValueUID = compiledObjectTrigger->getCurrentCommandObservationValueUID((*observationVariables)[i]->getUID());
    00195                 if(obsValueUID != (unsigned int)Trigger::UNDEFINED)
    00196                 {
    00197                         aObservationClause = new BaseModelClause(BaseModelClause::DISJUNCTIVE, true);
    00198                         aObservationClause->pushBack(new BaseModelAssignment((*observationVariables)[i], (*observationVariables)[i]->getDomain()->getValue(obsValueUID)));
    00199                         currentObservationAssignments->pushBack(aObservationClause);
    00200                 }
    00201         }
    00202
    00203         // Get the best transition from each initial state in the current trajectories
    00204         for(i=0; i<currentTrajectories->getNumberEstimates(); i++)
    00205         {
    00206                 // Seperate out probabilities
    00207                 for(j=0; j<allProbabilities.size()/currentTrajectories->getNumberEstimates(); j++)
    00208                         probabilities[j] = allProbabilities[i * (allProbabilities.size()/currentTrajectories->getNumberEstimates()) + j];
    00209
    00210                 // Allocate opsat memory if necessary
    00211                 if(i == opsats.size())
    00212                 {
    00213                         opsats.resize(opsats.size()+1);
    00214                         opsats[i] = new Opsat(*decisionVariables, *probabilityMap, probabilities, *myCCAmodel.getVariables(), *globalConstraints);
    00215                         setDebug(debug);
    00216                 } else
    00217                         // Insert the current probabilities into BFIG
    00218                         ((BFIGGenerator *)opsats[i]->getGenerator())->updateAllProbabilities(*decisionVariables, *probabilityMap, probabilities);
    00219
    00220                 // Add all the observation constraints. The same constraints apply to all trajectories
    00221                 for(j=0; j<currentObservationAssignments->getLength(); j++)
    00222                 {
    00223                         if(debug > 1)
    00224                                 cout << "Adding " << (*currentObservationAssignments)[j] << " constraint to opsat." << endl;
    00225                         opsats[i]->getTester()->addTemporaryConstraint((*currentObservationAssignments)[j]);            // Assumes that clearTemporaryConstraint() does
    00226                                                                                                                                                                 // not physically delete contraints
    00227                 }
    00228
    00229                 // Get best estimate given orignal state using Opsat
    00230                 if(!findNextBestStateAndPutInHeap(i, allProbabilities, &bestFirstHeap, true))
    00231                 {
    00232                         cleanAfterFailure(&bestFirstHeap);
    00233                         return false;
    00234                 }
    00235
    00236         }
    00237
    00238         if(bestFirstHeap.isEmpty())
    00239         {
    00240                 cout << "<ModeEstimator> ERROR: No consistent states found!" << endl;
    00241                 cleanAfterFailure(&bestFirstHeap);
    00242                 return false;
    00243         }
    00244         else
    00245                 nextTrajectories = new Trajectories(currentTrajectories->getAccumulatedUncertainty(), debug);
    00246
    00247         // Keep popping off and adding new esimtates to the heap until k best trajectories
    00248         // have been found
    00249
    00250         while(!bestFirstHeap.isEmpty())
    00251         {
    00252                 bestFirstHeap.getFirst(anEst);                  // Pop next most likely off the top of heap
    00253                 if(!nextTrajectories->addEstimate(anEst))       // Add it to the next trajectories list
    00254                 {
    00255                         cerr << "<ModeEstimator> ERROR: Couldn't add estimate to next trajectories!" << endl;
    00256                         cleanAfterFailure(&bestFirstHeap);
    00257                         return false;
    00258                 }
    00259                 if(nextTrajectories->getNumberEstimates() == maxNumberEstimates)
    00260                         break;
    00261
    00262                 // Get best estimate given orignal state using Opsat
    00263                 if(!findNextBestStateAndPutInHeap(anEst->getOriginatedTrajectoryNumber(), allProbabilities, &bestFirstHeap))
    00264                 {
    00265                         cleanAfterFailure(&bestFirstHeap);
    00266                         return false;
    00267                 }
    00268
    00269         }
    00270
    00271         // Clean up unused memory (currentObservationAssignments, bestFirstHeap, opsats) and normalize trajectories
    00272         releaseMemoryAndNormalize(currentObservationAssignments, &bestFirstHeap);
    00273
    00274         return true;
    00275 }
    00276
    00277
    00279
    00280 const BaseModelAssignmentArray *
    00281 ModeEstimator::generateBestSolution(unsigned int generatorNum, long int maximumVisitedNodeCount,
    00282                                                                         long int maxQueueSize)
    00283 {
    00284         return opsats[generatorNum]->findSatisfyingSolution(maximumVisitedNodeCount, maxQueueSize);
    00285 }
    00286
    00288
    00289 bool
    00290 ModeEstimator::addEstimate(Estimate * theEst, EstimateHeap * nextTrajsEstHeap)
    00291 {
    00292         nextTrajsEstHeap->insert(theEst);
    00293         return true;
    00294 }
    00295
    00297
    00298
    00299 bool
    00300 ModeEstimator::releaseMemoryAndNormalize(BaseModelClauseArray * currentObservationAssignments, EstimateHeap * nextTrajsEstHeap)
    00301 {
    00302         // Reset Opsat Objects (deletion only occures when ModeEstimator is deleted)
    00303         resetOpsats();
    00304
    00305         // Clear observation constraints
    00306         delete currentObservationAssignments;
    00307
    00308         // Call the base function to release the heap memory and normalize
    00309         return ModeEstimatorBase::releaseMemoryAndNormalize(nextTrajsEstHeap);
    00310 }
    00311
    00313
    00314
    00315 vector<const Dissent *>
    00316 ModeEstimator::emptyDissentVector(void)
    00317 {
    00318         vector<const Dissent *> noDissents;
    00319         return noDissents;
    00320 }
    00321
    00322
    00323 vector<const CompiledTransition *>
    00324 ModeEstimator::extractTransitionsFromCCA(const CCAModel &aCCAmodel)
    00325 {
    00326         // Gathers all the transitions in the model and creates CompiledTransition objects for each. All the
    00327         // information is available in the CCAModel. The only challenge is to convert all negative literals
    00328         // to positive literal disjunctive clauses as required by the trigger.
    00329
    00330         unsigned int i, j, k;
    00331         vector<const CompiledTransition *> compTransitions;
    00332         BaseModelAssignmentArray * guards;
    00333
    00334         for(i=0; i<aCCAmodel.get_ccAutomata_p()->getLength(); i++)
    00335         {
    00336                 const CCAAutomaton * auton = (*aCCAmodel.get_ccAutomata_p())[i];
    00337                 const CCATransitions * transitions = auton->transitions_p();
    00338                 for(j=0; j < transitions->getLength(); j++)
    00339                 {
    00340                         const CCATransition * trans = (*transitions)[j];
    00341
    00342                         // If the guard clause is disjunctive, split them into multiple compiled transitions
    00343                         if(trans->guard_p()->getClauseType() == BaseModelClause::DISJUNCTIVE && trans->guard_p()->getLength() > 1)
    00344                         {
    00345 //                              cout << "DISJUNCTIVE: Type of guard array: " << trans->guard_p()->getClauseType() << endl;
    00346                                 for(k = 0; k < trans->guard_p()->getLength(); k++)
    00347                                 {
    00348                                         guards = new BaseModelAssignmentArray(true);
    00349                                         guards->pushBack(new BaseModelAssignment(*((*(trans->guard_p()))[k])));
    00350                                         extractDNFTransitions(auton, trans, guards, compTransitions);
    00351                                         delete guards;
    00352                                 }
    00353                         }
    00354                         else
    00355                         {
    00356                                 guards = new BaseModelAssignmentArray(true);
    00357                                 for(k = 0; k < trans->guard_p()->getLength(); k++)
    00358                                         guards->pushBack(new BaseModelAssignment(*((*(trans->guard_p()))[k])));
    00359                                 extractDNFTransitions(auton, trans, guards, compTransitions);
    00360                                 delete guards;
    00361                         }
    00362                 }
    00363         }
    00364
    00365         return compTransitions;
    00366 }
    00367
    00368
    00369 void
    00370 ModeEstimator::extractDNFTransitions(const CCAAutomaton * auton,
    00371                                                                         const CCATransition * trans,
    00372                                                                         const BaseModelAssignmentArray * guards,
    00373                                                                         vector<const CompiledTransition *> & compTransitions)
    00374 {
    00375         unsigned int k;
    00376         BaseModelAssignment * src;
    00377         BaseModelAssignment * targ;
    00378         double prob;
    00379         CompiledTransition * aCompTrans;
    00380
    00381         src = new BaseModelAssignment(
    00382                 (BaseModelVariable *)auton->variable_p(),
    00383                 (BaseModelValue *)
    00384                 trans->from_state_p()->getValue());
    00385         targ = new BaseModelAssignment((BaseModelVariable *)auton->variable_p(),
    00386                 (BaseModelValue *)trans->to_state_p()->getValue());
    00387
    00388         DynamicArrayTemplate<BaseModelAssignmentArray> guardsVector(false);
    00389         guardsVector.pushBack(new BaseModelAssignmentArray(true));                      // Intilialize a new guard array
    00390
    00391
    00392         for(k = 0; k < guards->getLength(); k++)
    00393         {
    00394                 BaseModelAssignment * asg =
    00395                         new BaseModelAssignment((*(*(guards))[k]));
    00396                 if(!addGuardAssign(asg, guardsVector))
    00397                 {
    00398                         while(guardsVector.getLength() > 0)
    00399                         {
    00400                                 delete guardsVector.popBack();
    00401                         }
    00402                         delete asg;
    00403                         // pop out of for loop. No guards in guard vector -> no transition
    00404                         break;
    00405                 }
    00406                 delete asg;
    00407         }
    00408
    00409         prob = trans->probability();
    00410         for(k = 0; k < guardsVector.getLength(); k++)
    00411         {
    00412                 if(k > 0)
    00413                         aCompTrans = new CompiledTransition(new BaseModelAssignment(*src),
    00414                                 guardsVector[k], new BaseModelAssignment(*targ), prob);
    00415                 else
    00416                         aCompTrans = new CompiledTransition(src, guardsVector[k], targ, prob);
    00417                 compTransitions.push_back(aCompTrans);
    00418         }
    00419         guardsVector.makeEmpty();
    00420         return;
    00421 }
    00422
    00423 bool
    00424 ModeEstimator::addGuardAssign(const BaseModelAssignment * constraint, DynamicArrayTemplate<BaseModelAssignmentArray> &gv)
    00425 {
    00426         // If a new constraint assignment is a negation the number of compiled transitions is multiplied by
    00427         // the the domain size of the variable-1 to get rid of the negation. As a result, all the guards are
    00428         // duplicated and then the negation is driven out of the new assignment and added to the guard list.If
    00429         // the constrain being added is a negative literal and the variable is already in the guard list, the
    00430         // assignment that is negated is removed from the guard list. If the variable exists in the guard but
    00431         // assignment that is now negated does not, an error is displayed because it is uncertain out to expand
    00432         // this negation (ideally the entire sentance should be considered at once).
    00433
    00434         unsigned int i, j, k;
    00435         bool removed = false;
    00436         unsigned int transIndex;
    00437         unsigned int currentNumTransitions = gv.getLength();
    00438
    00439         if(constraint->getVariable()->getVariableType() != BaseModelVariable::CONTROL &&
    00440                 constraint->getVariable()->getVariableType() != BaseModelVariable::STATE &&
    00441                 constraint->getVariable()->getVariableType() != BaseModelVariable::OBSERVABLE)
    00442         {
    00443                 cerr << "<ModeEstimator> WARNING: Gaurds are currently limited to Mode, Command, and Observable Variables. Removing transition." << endl;
    00444                 cerr << "    Vairiable Type: " << BaseModelVariable::variableTypeToString(constraint->getVariable()->getVariableType()) << endl;
    00445                 cerr << "    Constraint: " << *constraint << endl;
    00446                 return false;
    00447         }
    00448
    00449         if(constraint->getOperator() != BaseModelAssignment::NEGATIVE)
    00450         {
    00451                 for(i=0; i<gv.getLength(); i++)
    00452                         gv[i]->pushBack(new BaseModelAssignment(*constraint));
    00453         }
    00454         else    // Need to multiply the compiled transition guards
    00455         {
    00456                 for(i=0; i<gv[0]->getLength(); i++)
    00457                 {
    00458                         if((*gv[0])[i]->getVariable()->getUID() == constraint->getVariable()->getUID())
    00459                                 break;
    00460                 }
    00461                 if(i<gv[0]->getLength())                        // Variable already exists.. check to values
    00462                 {
    00463                         for(j=0; j<gv.getLength(); j++)
    00464                         {
    00465                                 if((*gv[j])[i]->getValue()->getUID() == constraint->getValue()->getUID())       // Conflicting guards, remove transition
    00466                                 {
    00467                                         removed = true;
    00468                                         delete gv[j];
    00469                                         for(k=j+1; k<gv.getLength(); k++)               // Shift all transitions over by one
    00470                                                 gv[k-1] = gv[k];
    00471                                         gv.resize(gv.getLength()-1);                            // Resize
    00472                                         if(gv.getLength() == 0)
    00473                                         {
    00474                                                 cerr << "<ModeEstimator> ERROR: Guard is always false." << endl;
    00475                                                 return false;
    00476                                         }
    00477                                 }
    00478                         }
    00479                         if(!removed)
    00480                         {
    00481                                 cerr << "<ModeEstimator> ERROR: Can't handle negated guard conversion. Removing the transition." << endl;
    00482                                 return false;
    00483                         }
    00484                 }
    00485                 else
    00486                 {
    00487                         for(i=currentNumTransitions; i<currentNumTransitions*(constraint->getVariable()->getDomain()->getLength()-1); i++)
    00488                         {
    00489                                 gv.resize(i+1);
    00490                                 gv[i] = new BaseModelAssignmentArray(*(gv[i-currentNumTransitions]));
    00491                         }
    00492                         transIndex = 0;
    00493                         for(i=0; i<constraint->getVariable()->getDomain()->getLength(); i++)
    00494                         {
    00495                                 if(constraint->getValue()->getUID() != (*constraint->getVariable()->getDomain())[i]->getUID())
    00496                                 {       // if NOT the value, add
    00497                                         for(j=0; j<currentNumTransitions; j++)  // one for each of the original transition guards
    00498                                         {
    00499                                                 gv[transIndex]->pushBack(new BaseModelAssignment(constraint->getVariable(), (*constraint->getVariable()->getDomain())[i]));
    00500                                                 transIndex++;
    00501                                         }
    00502                                 }
    00503                         }
    00504                         if(transIndex != gv.getLength())
    00505                                 cout << "<MEdriverUtils> ERROR: Didn't add the correct number of guards!" << endl;
    00506                 }
    00507
    00508         }
    00509
    00510         return true;
    00511 }
    00512
    00513
    00514 void
    00515 ModeEstimator::resetOpsats(void)
    00516 {
    00517         for(unsigned int i=0; i<opsats.size(); i++)
    00518         {
    00519                 opsats[i]->clearTemporaryConstraint();
    00520                 opsats[i]->getGenerator()->reset();
    00521         }
    00522         return;
    00523 }