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

# /Users/fpy/titan-1-5-MBARI/Mers/Estimation/Discrete/ModeEstimatorBase.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 "ModeEstimatorBase.h"
    00010
    00011 #ifdef _MSC_VER
    00012 #pragma warning( disable : 4786 )
    00013 #endif
    00014
    00015 #include <Mers/Utils/Containers/IntegerArray.h>
    00016 #include <Mers/Utils/Containers/intList.h>
    00017
    00018 #include <Mers/Models/Base/BaseModelAssignment.h>
    00019 #include <Mers/Models/Base/BaseModelAssignmentArray.h>
    00020 #include <Mers/Models/Base/BaseModelVariable.h>
    00021 #include <Mers/Models/Base/BaseModelVariableArray.h>
    00022 #include <Mers/Models/Base/BaseModelValue.h>
    00023
    00024 #include "Estimate.h"
    00025 #include "EstimateHeap.h"
    00026 #include "Trajectories.h"
    00027 #include "Trigger.h"
    00028 #include "Dissent.h"
    00029 #include "CompiledTransition.h"
    00030
    00031 using namespace Mers::Estimation::Discrete;
    00032 using Mers::Models::Base::BaseModel;
    00033 using Mers::Models::Base::BaseModelAssignment;
    00034 using Mers::Models::Base::BaseModelAssignmentArray;
    00035 using Mers::Models::Base::BaseModelVariable;
    00036 using Mers::Models::Base::BaseModelVariableArray;
    00037 using Mers::Models::Base::BaseModelValue;
    00038 using Mers::Models::Base::BaseModelDomain;
    00039 using Mers::Utils::Containers::IntegerArray;
    00040 using Mers::Utils::Containers::intList;
    00041 using Mers::Utils::Lang::String;
    00042 using std::vector;
    00043 using std::ostream;
    00044 using std::cout;
    00045 using std::cerr;
    00046 using std::endl;
    00047
    00048
    00049
    00050
    00051         //-----------------------------------------------
    00052         //      CONSTRUCTOR
    00053         //-----------------------------------------------
    00054
    00055 ModeEstimatorBase::ModeEstimatorBase(const BaseModel &bm, vector<const Dissent *> dissents,
    00056                 vector<const CompiledTransition *> compiledTransitions,
    00057                 Trajectories * initialTrajectories)
    00058 : model(bm), compiledObjectTrigger(NULL),
    00059         objectiveFunction(NULL), currentTrajectories(initialTrajectories),
    00060         maxNumberEstimates(4), debug(0)
    00061 {
    00062         if(!currentTrajectories) {
    00063                 //cerr << "Creating seed trajectory." << endl;
    00064                 currentTrajectories = createSeedTrajectory(model);
    00065         }
    00066         nextTrajectories = NULL;
    00067
    00068         decisionVariables = new BaseModelVariableArray(false);
    00069         probabilityMap = new IntegerArray();
    00070
    00071         //cerr << "Determining the Decision Variables." << endl;
    00072         // Determine the decision variables and associated probability map
    00073         unsigned int nextVarPos = 0;
    00074         const BaseModelVariableArray * allBMVars = bm.getVariables();
    00075         for(unsigned int i=0; i<bm.getVariables()->getLength(); i++)
    00076         {
    00077                 if((*allBMVars)[i]->getVariableType() == BaseModelVariable::STATE)
    00078                 {
    00079                         decisionVariables->pushBack(const_cast<BaseModelVariable *>((*allBMVars)[i]));
    00080                         // Big hack that should be fixed by allowing BaseModelVariableArray to pass in constant BaseModelVariables
    00081                         probabilityMap->pushBack(nextVarPos);
    00082                         nextVarPos += (*allBMVars)[i]->getDomain()->getLength();
    00083                 }
    00084         }
    00085
    00086         //cerr << "Creating the trigger." << endl;
    00087         // Instantiate the trigger
    00088         compiledObjectTrigger = new Trigger(decisionVariables, probabilityMap,
    00089                 bm.getVariables()->getLength(), dissents, compiledTransitions,
    00090                 currentTrajectories, 0);
    00091
    00092
    00093         //cerr << "Reseting the trigger." << endl;
    00094
    00095         resetTrigger(currentTrajectories);
    00096
    00097         //cerr << "Finished creating ME Base" << endl;
    00098 }
    00099
    00100
    00101         //-----------------------------------------------
    00102         //      DESTRUCTOR
    00103         //-----------------------------------------------
    00104
    00105
    00106 ModeEstimatorBase::~ModeEstimatorBase()
    00107 {
    00108         if(currentTrajectories != NULL)
    00109                 delete currentTrajectories;
    00110         if(nextTrajectories != NULL)
    00111                 delete nextTrajectories;
    00112         delete decisionVariables;
    00113         delete compiledObjectTrigger;
    00114         delete probabilityMap;
    00115 }
    00116
    00117
    00118
    00119         //-----------------------------------------------
    00120         //      PUBLIC METHODS
    00121         //-----------------------------------------------
    00122
    00123 bool
    00124 ModeEstimatorBase::observe(const String var, const String val)
    00125 {
    00126         return assign(var, val, BaseModelVariable::OBSERVABLE);
    00127 }
    00128
    00129 bool
    00130 ModeEstimatorBase::observe(const unsigned int var, const unsigned int val)
    00131 {
    00132         return assign(var, val, BaseModelVariable::OBSERVABLE);
    00133 }
    00134
    00135 bool
    00136 ModeEstimatorBase::command(const String var, const String val, unsigned int estNum)
    00137 {
    00138         return assign(var, val, BaseModelVariable::CONTROL, estNum);
    00139 }
    00140
    00141 bool
    00142 ModeEstimatorBase::command(const unsigned int var, const unsigned int val, unsigned int estNum)
    00143 {
    00144         return assign(var, val, BaseModelVariable::CONTROL, estNum);
    00145 }
    00146
    00147 bool
    00148 ModeEstimatorBase::setCurrentState(const String var, const String val)
    00149 {
    00150         return assign(var, val, BaseModelVariable::STATE);
    00151 }
    00152
    00153 bool
    00154 ModeEstimatorBase::setCurrentState(const unsigned int var, const unsigned int val)
    00155 {
    00156         return assign(var, val, BaseModelVariable::STATE);
    00157 }
    00158
    00159 bool
    00160 ModeEstimatorBase::addTrajectory(Estimate * est)
    00161 {
    00162         if(est->getProbability() > currentTrajectories->getBeliefUncertainty() + 0.0000001) // Add 1/10000000 to fix machine error problem
    00163         {
    00164                 cout << "<ModeEstimatorBase> WARNING: New estimate probability is too high to fit in current belief state." << endl;
    00165                 cout << "                         Resetting trajectories to this single estimate." << endl;
    00166                 return setTrajectory(est);
    00167         }
    00168         return addEstimateToCurrentTrajectories(est);
    00169 }
    00170
    00171 bool
    00172 ModeEstimatorBase::setTrajectory(Estimate * est)
    00173 {
    00174         return addEstimateToCurrentTrajectories(est, true);
    00175 }
    00176
    00177
    00178 double
    00179 ModeEstimatorBase::getUncertainty() const
    00180 {
    00181         if(currentTrajectories == NULL)
    00182                 return 1.0;
    00183         else
    00184                 return currentTrajectories->getBeliefUncertainty();
    00185 }
    00186
    00187 const Estimate *
    00188 ModeEstimatorBase::findModeEstimate(void)
    00189 {
    00190         if(nextTrajectories == NULL)            // Need to generate a new nextTrajectories
    00191         {
    00192                 if(!generateTrajectories())
    00193                 {
    00194                         cerr << "<ModeEstimatorBase> ERROR: Couldn't generate new trajectories!" << endl;
    00195                         return NULL;
    00196                 }
    00197         }
    00198         return nextTrajectories->getMostLikelyEstimate();
    00199 }
    00200
    00201 intList const *
    00202 ModeEstimatorBase::findModeEstimateIntList(bool full)
    00203 {
    00204         if(findModeEstimate() == NULL)
    00205                 return NULL;
    00206         if(!full)
    00207                 return estimateToIntList(nextTrajectories->getMostLikelyEstimate(),
    00208                         currentTrajectories->getMostLikelyEstimate());
    00209         return estimateToIntList(nextTrajectories->getMostLikelyEstimate());
    00210 }
    00211
    00212 const Estimate *
    00213 ModeEstimatorBase::progressToEstimate(void)
    00214 {
    00215         if(findModeEstimate() == NULL)
    00216                 return NULL;
    00217         delete currentTrajectories;
    00218         currentTrajectories = nextTrajectories;
    00219         nextTrajectories = NULL;
    00220
    00221         if(!resetCommandsAndObservations())
    00222                 cerr << "<ModeEstimatorBase> ERROR: Couldn't reset!" << endl;
    00223
    00224         return currentTrajectories->getMostLikelyEstimate();
    00225 }
    00226
    00227
    00228 const Trajectories *
    00229 ModeEstimatorBase::getCurrentTrajectories(void) const
    00230 {
    00231         return currentTrajectories;
    00232 }
    00233
    00234
    00235 const Trajectories *
    00236 ModeEstimatorBase::findNextTrajectories(void)
    00237 {
    00238         if(findModeEstimate() == NULL)
    00239                 return NULL;
    00240         return nextTrajectories;
    00241 }
    00242
    00243
    00244 bool
    00245 ModeEstimatorBase::resetCommandsAndObservations(void)
    00246 {
    00247         if(nextTrajectories != NULL)
    00248                 delete nextTrajectories;
    00249         nextTrajectories = NULL;
    00250
    00251         // Reset the trigger with the current trajectories
    00252         return resetTrigger(currentTrajectories);
    00253 }
    00254
    00255
    00256 void
    00257 ModeEstimatorBase::showAllVars(ostream &out) const
    00258 {
    00259         showStateVars(out);
    00260         out << endl;
    00261         showCommandVars(out);
    00262         out << endl;
    00263         showObservationVars(out);
    00264 }
    00265
    00266 void
    00267 ModeEstimatorBase::showStateVars(ostream &out) const
    00268 {
    00269         showVars(out, BaseModelVariable::STATE);
    00270 }
    00271
    00272
    00273
    00274 void
    00275 ModeEstimatorBase::showCommandVars(ostream &out) const
    00276 {
    00277         showVars(out, BaseModelVariable::CONTROL);
    00278 }
    00279
    00280 void
    00281 ModeEstimatorBase::showObservationVars(ostream &out) const
    00282 {
    00283         showVars(out, BaseModelVariable::OBSERVABLE);
    00284 }
    00285
    00286
    00287 void
    00288 ModeEstimatorBase::showDissents(ostream &out) const
    00289 {
    00290         compiledObjectTrigger->showDissents(out);
    00291 }
    00292
    00293 void
    00294 ModeEstimatorBase::showCompiledTransitions(ostream &out) const
    00295 {
    00296         compiledObjectTrigger->showCompiledTransitions(out);
    00297 }
    00298
    00299 void
    00300 ModeEstimatorBase::showEnabled(ostream &out) const
    00301 {
    00302         compiledObjectTrigger->showEnabled(out);
    00303 }
    00304
    00305 void
    00306 ModeEstimatorBase::showConflicts(ostream &out) const
    00307 {
    00308         vector<const BaseModelAssignmentArray *> conflicts = compiledObjectTrigger->getConflicts();
    00309         out << "Conflicts Generated:" << endl;
    00310         for(unsigned int i=0; i<conflicts.size(); i++)
    00311                 cout << *(conflicts[i]) << endl;
    00312 }
    00313
    00314 void
    00315 ModeEstimatorBase::showCurrentTrajectories(ostream &out) const
    00316 {
    00317         if(currentTrajectories)
    00318                 out << "Leading estimates for the current trajectories: " << endl << *currentTrajectories;
    00319 }
    00320
    00321 void
    00322 ModeEstimatorBase::showNextTrajectories(ostream &out)
    00323 {
    00324         if(!nextTrajectories)
    00325         {
    00326                 if(findModeEstimate() == NULL)
    00327                         return;
    00328         }
    00329         if(nextTrajectories)
    00330                 out << "Leading estimates for the next trajectories: " << endl << *nextTrajectories;
    00331 }
    00332
    00333 void
    00334 ModeEstimatorBase::setNumberTrajectories(unsigned int k)
    00335 {
    00336         if(k < 1)
    00337         {
    00338                 cout << "<ModeEstimatorBase> WARNING: Maximum # trajectories must be > 0. Setting k=1." << endl;
    00339                 k = 1;
    00340         }
    00341         maxNumberEstimates = k;
    00342
    00343         if(nextTrajectories != NULL)
    00344                 delete nextTrajectories;
    00345         nextTrajectories = NULL;
    00346
    00347         return;
    00348 }
    00349
    00350 void
    00351 ModeEstimatorBase::setDebug(const unsigned short db)
    00352 {
    00353         unsigned short subDebug;
    00354         debug = db;
    00355         if(db < 1)                      // Reduce the debug level by 1 for internal data structures
    00356                 subDebug = 0;
    00357         else
    00358                 subDebug = db-1;
    00359         if(currentTrajectories)
    00360                 currentTrajectories->setDebug(subDebug);
    00361         if(nextTrajectories)
    00362                 nextTrajectories->setDebug(subDebug);
    00363         compiledObjectTrigger->setDebug(subDebug);
    00364 }
    00365
    00366
    00367 ostream&
    00368 Mers::Estimation::Discrete::operator<<(ostream& os, ModeEstimatorBase const & me)
    00369 {
    00370         os << "Current Trajectories: " << endl;
    00371         os << *(me.currentTrajectories);
    00372
    00373         return os;
    00374 }
    00375
    00376         //-----------------------------------------------
    00377         //      PROTECTED METHODS
    00378         //-----------------------------------------------
    00379
    00380
    00381 bool
    00382 ModeEstimatorBase::findNextBestStateAndPutInHeap(unsigned int generatorNum, const vector<double> & allProbs,
    00383                                                                                                  EstimateHeap * nextTrajsEstHeap, bool willWarn)
    00384 {
    00385         Estimate * anEst;
    00386         BaseModelAssignmentArray * estAssign;
    00387         double estimateProbability = 0.0;
    00388         unsigned int j = 0;
    00389
    00390         // Get the next best solution from opsat
    00391         const BaseModelAssignmentArray * res = generateBestSolution(generatorNum, (unsigned int)1e10, (unsigned int)1e10);
    00392                                                                                                                                                                 // Not sure what to use for these #s
    00393         if(res != NULL) {
    00394
    00395                 // Determine the estimate probability
    00396                 if(!calculateEstimateProbability(estimateProbability, res, generatorNum, allProbs))
    00397                 {
    00398                         delete res;
    00399                         return false;
    00400                 }
    00401
    00402                 if(res->getLength() > 0 && estimateProbability > 0.0)
    00403                 {
    00404                         estAssign = new BaseModelAssignmentArray(true);
    00405                         for(j=0; j<res->getLength(); j++)
    00406                                 estAssign->pushBack(new BaseModelAssignment((*res)[j]->getVariable(), (*res)[j]->getValue()));
    00407                         anEst = new Estimate(estAssign, estimateProbability, generatorNum);
    00408
    00409                         // Add the estimate to the heap or to nextTrajectories
    00410                         if(!addEstimate(anEst, nextTrajsEstHeap))
    00411                         {
    00412                                 delete res;
    00413                                 return false;
    00414                         }
    00415                         if(debug > 2)
    00416                         {
    00417                                 cout << "New estimate found and added to the heap (or nextTrajectories): " << endl;
    00418                                 cout << " " << *anEst << endl;
    00419                                 cout << " estimate probability: " << anEst->getProbability() << endl;
    00420                         }
    00421                 }
    00422                 if(res->getLength() == 0)
    00423                         cout << "<ModeEstimatorBase> WARNING: No consistent states found for trajectory " << generatorNum << "!" << endl;
    00424                 else if(estimateProbability <= 0.0)
    00425                 {
    00426                         cout << "<ModeEstimatorBase> WARNING: Zero probability estimate was generated." << endl;
    00427                         cout << "               res length: " << res->getLength() << endl;
    00428                         cout << "               est prob: " << estimateProbability << endl;
    00429                         cout << "       Candidate: " << endl << *res << endl;
    00430                 }
    00431                 delete res;
    00432         }
    00433         else if(willWarn)
    00434                 cout << "<ModeEstimatorBase> WARNING: No consistent states found for trajectory " << generatorNum << "!" << endl;
    00435         return true;
    00436 }
    00437
    00439
    00440 bool
    00441 ModeEstimatorBase::calculateEstimateProbability(double & estProbability, const BaseModelAssignmentArray * state,
    00442                                                                                                 unsigned int generatorNum, const vector<double> & allProbs) const
    00443 // This function really belongs in a general BFTE class
    00444 {
    00445                 estProbability = 1.0;
    00446                 for(unsigned int j=0; j < state->getLength(); j++)
    00447                 {
    00448                         // Assumes that assignment order is the same as the decision variable order
    00449                         estProbability *= allProbs[generatorNum * (allProbs.size()/currentTrajectories->getNumberEstimates())
    00450                                 + (*probabilityMap)[j] + (*state)[j]->getValue()->getUID()];
    00451                         //estimateProbability *= probabilities[(*probabilityMap)[j] + (*res)[j]->getValue()->getUID()];
    00452                         if((*state)[j]->getVariable()->getUID() != (*decisionVariables)[j]->getUID())
    00453                         {
    00454                                 cerr << "<ModeEstimatorBase> ERROR: Probabilities aren't in expected order!" << endl;
    00455                                 cerr << "j: " << j << "  state uid: " << (*state)[j]->getVariable()->getUID() << "  decision UID: " << (*decisionVariables)[j]->getUID() << endl;
    00456                                 return false;
    00457                         }
    00458                 }
    00459                 // Multiply state probability by the original estimate probability
    00460                 estProbability *= currentTrajectories->getEstimate(generatorNum)->getProbability();
    00461
    00462                 return true;
    00463 }
    00464
    00465 bool
    00466 ModeEstimatorBase::releaseMemoryAndNormalize(EstimateHeap * nextTrajsEstHeap)
    00467 {
    00468         // Release Heap Memory
    00469         if(nextTrajsEstHeap != NULL)
    00470                 nextTrajsEstHeap->makeEmpty();
    00471
    00472         // Normalize new trajectories to avoid machine error
    00473         if(nextTrajectories != NULL)
    00474                 nextTrajectories->normalize();
    00475
    00476         return true;
    00477 }
    00478
    00479 void
    00480 ModeEstimatorBase::cleanAfterFailure(EstimateHeap * nextTrajsEstHeap)
    00481 {
    00482         if(nextTrajectories != NULL)
    00483         {
    00484                 delete nextTrajectories;
    00485                 nextTrajectories = NULL;
    00486         }
    00487         releaseMemoryAndNormalize(nextTrajsEstHeap);
    00488         return;
    00489 }
    00490
    00491 bool
    00492 ModeEstimatorBase::addEstimateToCurrentTrajectories(Estimate * anEst,
    00493                                                                 bool createNewCurrentTrajectories)
    00494 {
    00495         bool successFlag = true;
    00496         if(createNewCurrentTrajectories)
    00497         {
    00498                 if(currentTrajectories != NULL)
    00499                         delete currentTrajectories;
    00500                 currentTrajectories = new Trajectories(0.0, debug);
    00501         }
    00502         if(!currentTrajectories->addEstimate(anEst))
    00503                 successFlag = false;
    00504         if(!resetCommandsAndObservations())
    00505                 successFlag = false;
    00506         return successFlag;
    00507 }
    00508
    00509         //-----------------------------------------------
    00510         //      PRIVATE METHODS
    00511         //-----------------------------------------------
    00512
    00513
    00514 bool
    00515 ModeEstimatorBase::assign(const String var, const String val,
    00516                                           const BaseModelVariable::VariableType varType, unsigned int estNum)
    00517 {
    00518         const BaseModelVariable * bmVar = model.getVariable(var.c_str());
    00519         if(bmVar == NULL)
    00520         {
    00521                 cout << "<ModeEstimatorBase> WARNING: Unable to find variable name " << var << ". Ignoring." << endl;
    00522                 return false;
    00523         }
    00524
    00525         if(bmVar->getVariableType() != varType)
    00526         {
    00527                 cerr << "<ModeEstimatorBase> ERROR: Invalid variable type!" << endl;
    00528                 cerr << "    Variable type " << BaseModelVariable::variableTypeToString(bmVar->getVariableType())
    00529                          << " should be of type " << BaseModelVariable::variableTypeToString(varType) << endl;
    00530                 return false;
    00531         }
    00532
    00533         const BaseModelValue * bmVal = bmVar->getDomain()->getValue(val.c_str());
    00534         if(bmVal == NULL)
    00535         {
    00536                 cout << "<ModeEstimatorBase> WARNING: Unable to find value " << val << ". Ignoring." << endl;
    00537                 return false;
    00538         }
    00539
    00540         return assign( bmVar, bmVal, estNum );
    00541 }
    00542
    00543
    00544 bool
    00545 ModeEstimatorBase::assign(const unsigned int var, const unsigned int val,
    00546                                           const BaseModelVariable::VariableType varType, unsigned int estNum)
    00547 {
    00548         if(var >= model.getVariables()->getLength())
    00549         {
    00550                 cout << "<ModeEstimatorBase> WARNING: Variable index out of range " << var << ". Ignoring." << endl;
    00551                 return false;
    00552         }
    00553         const BaseModelVariable * bmVar = model.getVariable(var);
    00554         if(bmVar == NULL)
    00555         {
    00556                 cerr << "<ModeEstimatorBase> ERROR: Unable to find variable UID " << var << "!" << endl;
    00557                 return false;
    00558         }
    00559         if(val >= bmVar->getDomain()->getLength())
    00560         {
    00561                 cout << "<ModeEstimatorBase> WARNING: Assignment value out of range " << val << ". Ignoring." << endl;
    00562                 return false;
    00563         }
    00564
    00565         if(bmVar->getVariableType() != varType)
    00566         {
    00567                 cerr << "<ModeEstimatorBase> ERROR: Invalid variable type!" << endl;
    00568                 cerr << "    Variable type " << BaseModelVariable::variableTypeToString(bmVar->getVariableType())
    00569                          << " should be of type " << BaseModelVariable::variableTypeToString(varType) << endl;
    00570                 return false;
    00571         }
    00572
    00573         const BaseModelValue * bmVal = bmVar->getDomain()->getValue(val);
    00574         if(bmVal == NULL)
    00575         {
    00576                 cerr << "<ModeEstimatorBase> ERROR: Unable to find value " << val << "!" << endl;
    00577                 return false;
    00578         }
    00579
    00580         return assign( bmVar, bmVal, estNum );
    00581 }
    00582
    00584
    00585 bool ModeEstimatorBase::assign( const BaseModelVariable * var,
    00586         const BaseModelValue * val, unsigned int estNum )
    00587 {
    00588         // Allocate the new assignment
    00589         BaseModelAssignment * bmAssign = new BaseModelAssignment( var, val );
    00590
    00591         return assign( bmAssign, estNum );
    00592 }
    00593
    00595
    00596 bool
    00597 ModeEstimatorBase::assign(BaseModelAssignment * bmAssign, unsigned int estNum)
    00598 {
    00599         if(debug > 1)
    00600                 cout << "Asserting " << *bmAssign << " into trigger..." << endl;
    00601
    00602         if(!bmAssign)
    00603         {
    00604                 cerr << "<ModeEstimatorBase> ERROR: Unknown assignment. Cannot insert into trigger." << endl;
    00605                 return false;
    00606         }
    00607
    00608         // next trajectory estimate no longer valid... deleting
    00609         if(nextTrajectories != NULL)
    00610         {
    00611                 delete nextTrajectories;
    00612                 nextTrajectories = NULL;
    00613         }
    00614
    00615         if(bmAssign->getVariable()->getVariableType() == BaseModelVariable::OBSERVABLE ||
    00616                 bmAssign->getVariable()->getVariableType() == BaseModelVariable::CONTROL)
    00617         {       // Observation/Command variable assignment
    00618                 bool result;
    00619                 if(estNum > 0)
    00620                         result = compiledObjectTrigger->insertAssignment(bmAssign, estNum);
    00621                 else
    00622                         result = compiledObjectTrigger->insertAssignment(bmAssign);
    00623                 delete bmAssign;
    00624                 return result;
    00625         }
    00626         else if(bmAssign->getVariable()->getVariableType() == BaseModelVariable::STATE)
    00627         {       // Change one state var assignment in the most likely estimate of the current
    00628                 // trajectories and set that states probability to 1, removing all
    00629                 // other estimates
    00630                 bool successFlag = true;
    00631                 BaseModelAssignmentArray * implicants = new BaseModelAssignmentArray(true);
    00632                 for(unsigned int i=0; i<currentTrajectories->getMostLikelyEstimate()->getComponentAssignments()->getLength(); i++)
    00633                 {
    00634                         if(bmAssign != NULL && *currentTrajectories->getMostLikelyEstimate()->getAssignment(i)->getVariable() == *bmAssign->getVariable()) {
    00635                                 implicants->pushBack(bmAssign);
    00636                                 bmAssign = NULL;
    00637                         } else {
    00638                                 implicants->pushBack(new BaseModelAssignment(*currentTrajectories->getMostLikelyEstimate()->getAssignment(i)));
    00639                         }
    00640                 }
    00641                 if(bmAssign != NULL)
    00642                         delete bmAssign;
    00643
    00644                 successFlag = addEstimateToCurrentTrajectories(new Estimate(implicants, 1.0), true);
    00645                 return successFlag;
    00646         }
    00647         else
    00648         {       // Unknown variable type
    00649                 cerr << "<ModeEstimatorBase> ERROR: Cannot add assignment to variable type "
    00650                         << BaseModelVariable::variableTypeToString(bmAssign->getVariable()->getVariableType()) << "!" << endl;
    00651                 delete bmAssign;
    00652                 return false;
    00653         }
    00654
    00655         // Should never get here
    00656         return false;
    00657 }
    00658
    00659
    00660 bool
    00661 ModeEstimatorBase::resetTrigger(const Trajectories * trajs)
    00662 {
    00663         //cerr << "ResetTrigger(" << (void *)trajs << ") COT(" << (void *)compiledObjectTrigger << ")" << endl;
    00664
    00665         bool successFlag;
    00666         BaseModelAssignment * anAssign;
    00667         successFlag = compiledObjectTrigger->changeTrajectories(trajs);
    00668
    00669         //cerr << "Extracting Control Variables." << endl;
    00670         for(unsigned int i=0; i<model.getVariables()->getLength(); i++)
    00671         {
    00672                 if((*model.getVariables())[i]->getVariableType() == BaseModelVariable::CONTROL)
    00673                 {
    00674                         const BaseModelVariable * bmVar = (*model.getVariables())[i];
    00675                         const BaseModelValue * bmVal = bmVar->getDomain()->find("NO-COMMAND");
    00676                         if(bmVal != NULL) {
    00677                                 anAssign = new BaseModelAssignment(bmVar, bmVal);
    00678                                 if(debug > 1)
    00679                                         cout << "Asserting " << *anAssign << " into the trigger..." << endl;
    00680                                 if(!compiledObjectTrigger->insertAssignment(anAssign))
    00681                                         successFlag = false;
    00682                                 delete anAssign;
    00683                         } else {
    00684                                 cerr << "Error: Command \"" << bmVar->getName()
    00685                                                  << "\" does not have an assignment called \"NO-COMMAND\"."
    00686                                                  << endl;
    00687                                 successFlag = false;
    00688                         }
    00689                 }
    00690         }
    00691         return successFlag;
    00692 }
    00693
    00694
    00695 bool
    00696 ModeEstimatorBase::showVars(ostream &out, const BaseModelVariable::VariableType varType) const
    00697 {
    00698         const BaseModelVariableArray * varList;
    00699         vector<double> probabilities;
    00700         if(varType == BaseModelVariable::STATE)
    00701         {
    00702                 varList = decisionVariables;
    00703                 probabilities = compiledObjectTrigger->getNormalizedEnabledTransitionProbabilities();
    00704                 out << "State Variables: " << endl;
    00705         }
    00706         else
    00707         {
    00708                 varList = model.getVariables();
    00709                 if(varType == BaseModelVariable::CONTROL)
    00710                         out << "Command Variables: " << endl;
    00711                 else if(varType == BaseModelVariable::OBSERVABLE)
    00712                         out << "Observation Variables: " << endl;
    00713                 else
    00714                 {
    00715                         cerr << "<ModeEstimatorBase> ERROR: Unknown variable type to display." << endl;
    00716                         return false;
    00717                 }
    00718         }
    00719         unsigned int currentValueUID;
    00720         unsigned int i, j;
    00721
    00722         for(i=0; i<varList->getLength(); i++)
    00723         {
    00724                 if((*varList)[i]->getVariableType() == varType)
    00725                 {
    00726                         out << (*varList)[i]->getName() << "[" << (*varList)[i]->getUID() << "] { ";
    00727                         if(varType == BaseModelVariable::STATE)
    00728                                 currentValueUID = currentTrajectories->getMostLikelyEstimate()->getAssignmentByUID((*varList)[i]->getUID())->getValue()->getUID();
    00729                         else
    00730                                 currentValueUID = compiledObjectTrigger->getCurrentCommandObservationValueUID((*varList)[i]->getUID());
    00731                         const BaseModelDomain * bmDomain =  (*varList)[i]->getDomain();
    00732                         for(j=0; j<bmDomain->getLength(); j++)
    00733                         {
    00734                                 if((*bmDomain)[j]->getUID() == currentValueUID)
    00735                                         out << ">";
    00736                                 if(varType == BaseModelVariable::STATE) // Only add in probabilities if state vars
    00737                                 {
    00738                                         if(probabilities[(*probabilityMap)[i]+j] > 0.0)
    00739                                                 out << "+";
    00740                                 }
    00741                                 out << (*bmDomain)[j]->getName() << "[" << (*bmDomain)[j]->getUID() << "]";
    00742                                 if(varType == BaseModelVariable::STATE)
    00743                                         out << ":" << probabilities[(*probabilityMap)[i]+j];
    00744                                 if(j != bmDomain->getLength()-1)
    00745                                         out << ", ";
    00746                         }
    00747                         out << " }" << endl;
    00748                 }
    00749         }
    00750         return true;
    00751 }
    00752
    00753 intList const *
    00754 ModeEstimatorBase::estimateToIntList(Estimate const * nextEst, Estimate const * currEst)  const
    00755 {
    00756         const BaseModelAssignmentArray * nextImp = nextEst->getComponentAssignments();
    00757         if(!nextImp)
    00758                 return NULL;
    00759         intList * res = new intList();
    00760         if(currEst)                     // current estimate to compare to (extract only the changed states)
    00761         {
    00762                 const BaseModelAssignmentArray * currImp = currEst->getComponentAssignments();
    00763                 for (unsigned int i=0; i<nextImp->getLength(); i++)
    00764                 {
    00765                         if((*nextImp)[i]->getVariable()->getUID() != (*currImp)[i]->getVariable()->getUID())
    00766                         {
    00767                                 cerr << "<ModeEstimatorBase> ERROR: Variables order doesn't match for mode change comparision." << endl;
    00768                                 delete res;
    00769                                 return NULL;
    00770                         }
    00771                         if((*nextImp)[i]->getValue()->getUID() != (*currImp)[i]->getValue()->getUID())
    00772                         {       // Value has changed (add to list)
    00773                                 res->push_back((*nextImp)[i]->getVariable()->getUID());
    00774                                 res->push_back((*nextImp)[i]->getValue()->getUID());
    00775                         }
    00776                 }
    00777         }
    00778         else                            // place all mode assignments into int list
    00779         {
    00780                 for (unsigned int i=0; i<nextImp->getLength(); i++)
    00781                 {
    00782                         res->push_back((*nextImp)[i]->getVariable()->getUID());
    00783                         res->push_back((*nextImp)[i]->getValue()->getUID());
    00784                 }
    00785         }
    00786         return res;
    00787 }
    00788
    00789
    00790 Trajectories *
    00791 ModeEstimatorBase::createSeedTrajectory(const BaseModel & bm)
    00792 {
    00793
    00794         Trajectories * trajs = new Trajectories(0.0);
    00795         BaseModelAssignmentArray * modeAssigns = new BaseModelAssignmentArray(true);
    00796         for(unsigned int i=0; i<bm.getVariables()->getLength(); i++)
    00797                 if(bm.getVariable(i)->getVariableType() == BaseModelVariable::STATE)    // Assumes UIDs are also the location in array
    00798                         modeAssigns->pushBack(new BaseModelAssignment(bm.getVariable(i), (*bm.getVariable(i)->getDomain())[0]));        // Pics the first possible assignment
    00799
    00800         Estimate * est = new Estimate(modeAssigns, 1.0);
    00801         if(!trajs->addEstimate(est))
    00802         {
    00803                 cerr << "<ModeEstimatorBase> ERROR: Unable to create seed trajectory!" << endl;
    00804                 return NULL;
    00805         }
    00806         return trajs;
    00807 }