Obs2DBeaconNoisy.cpp
Go to the documentation of this file.
00001 #include "Obs2DBeaconNoisy.h"
00002 #include "InRangeAllNto1VarRel.h"
00003 
00004 
00005 Obs2DBeaconNoisy::Obs2DBeaconNoisy(SharedPointer<Variable> obsVar, SharedPointer<Map2DPosVar> suv1PosVar, SharedPointer<Map2DPosVar> suv2PosVar, SharedPointer<Map2DPosVar> auvPosVar, SharedPointer<BooleanVariable> auvGetsGPSPos, SharedPointer<ProblemParams> params )
00006 {
00007    this->suv1PosVar = suv1PosVar;
00008     this->suv2PosVar = suv2PosVar;
00009     this->auvPosVar = auvPosVar;
00010         this->params = params;
00011         this->auvGetsGPSPosVar = auvGetsGPSPos;
00012         this->obsVar = obsVar;
00013 
00014     isCal = SharedPointer<BooleanVariable> ( new BooleanVariable("isCal", 1.0, 0.0) );
00015 
00016     auvPosEst = SharedPointer<InRangeAllNto1VarRel> (new InRangeAllNto1VarRel(isCal, auvPosVar, params->ModemRange));
00017     auvPosEst->addSrcVar(suv1PosVar);
00018     auvPosEst->addSrcVar(suv2PosVar);
00019 
00020     this->addSourceVar(suv1PosVar);
00021     this->addSourceVar(suv2PosVar);
00022     this->addSourceVar(auvPosVar);
00023     this->addSourceVar(auvGetsGPSPos);
00024     this->setDestVariable(obsVar);
00025 }
00026 
00027 Obs2DBeaconNoisy::~Obs2DBeaconNoisy(void)
00028 {
00029 }
00030 
00031 vector<SharedPointer<RelEntry> > Obs2DBeaconNoisy::getProb(map<string, SharedPointer<IVariableValue> > sourceVals)
00032 {
00033         vector<SharedPointer<RelEntry> > input = auvPosEst->getProb(sourceVals);
00034     vector<SharedPointer<RelEntry> > result;
00035 
00036         FOREACH_NOCONST(SharedPointer<RelEntry>, psrcEntry , input)
00037         {
00038                         SharedPointer<RelEntry>& srcEntry = *psrcEntry;
00039                         /*SharedPointer<IVariableValue> auvPos = srcEntry->sourceValues[auvPosVar->getVariableName()];
00040                         SharedPointer<IVariableValue> suv1Pos = srcEntry->sourceValues[suv1PosVar->getVariableName()];
00041             SharedPointer<IVariableValue> suv2Pos = srcEntry->sourceValues[suv2PosVar->getVariableName()];*/
00042 
00043                         SharedPointer<IVariableValue> auvPos = sourceVals[auvPosVar->getVariableName()];
00044                         SharedPointer<IVariableValue> suv1Pos = sourceVals[suv1PosVar->getVariableName()];
00045             SharedPointer<IVariableValue> suv2Pos = sourceVals[suv2PosVar->getVariableName()];
00046 
00047                         SharedPointer<IVariableValue> inRange = srcEntry->destValues[isCal->getVariableName()];
00048 
00049 
00050                         if(inRange->getIndex() == BooleanVariable::TrueValueIndex)
00051             {
00052                 // if in range
00053                                 if(sourceVals[auvGetsGPSPosVar->getVariableName()]->getIndex() == BooleanVariable::TrueValueIndex)
00054                                 {
00055                                         {
00056                                                 SharedPointer<RelEntry> newEntry ( new RelEntry() );
00057                                                 SharedPointer<IVariableValue> destValue = obsVar->getValueByName(auvPos->ToString()+"true");
00058                                                 newEntry->destValues[destValue->getVariableName()] = destValue;
00059                                                 newEntry->prob = params->AUVtoSUVProb;
00060                                                 result.push_back(newEntry);
00061                                         }
00062 
00063                                         {
00064                                                 SharedPointer<RelEntry> newEntry ( new RelEntry() );
00065                                                 SharedPointer<IVariableValue> destValue = obsVar->getValueByName("na");
00066                                                 newEntry->destValues[destValue->getVariableName()] = destValue;
00067                                                 newEntry->prob = 1 - params->AUVtoSUVProb;
00068                                                 result.push_back(newEntry);
00069                                         }
00070                                 }
00071                                 else
00072                                 {
00073                                         {
00074                                                 SharedPointer<RelEntry> newEntry ( new RelEntry() );
00075                                                 SharedPointer<IVariableValue> destValue  = obsVar->getValueByName("na");
00076                                                 newEntry->destValues[destValue->getVariableName()] =destValue;
00077                                                 newEntry->prob =  1 - params->NoisyAUVObsProb;
00078                                                 result.push_back(newEntry);
00079                                         }
00080 
00081                                         vector<SharedPointer<Map2DPosValue> >  adjCells =  auvPosVar->getAdjPos( dynamic_pointer_cast<Map2DPosValue>(auvPos), params->NoisyAUVObsRadius);
00082 
00083                                         FOREACH_NOCONST(SharedPointer<Map2DPosValue> , padjCell, adjCells)
00084                                         {
00085                                                 SharedPointer<Map2DPosValue>& adjCell = *padjCell;
00086                                                 SharedPointer<RelEntry> newEntry ( new RelEntry() );
00087                                                 SharedPointer<IVariableValue> destValue = obsVar->getValueByName(adjCell->ToString() + "false");
00088                                                 newEntry->destValues[destValue->getVariableName()] =destValue;
00089                                                 newEntry->prob =  params->NoisyAUVObsProb / adjCells.size();
00090                                                 result.push_back(newEntry);
00091                                         }
00092                                 }
00093             }
00094             else
00095             {
00096                 // if not in range, then obs is useless
00097                                 SharedPointer<RelEntry> newEntry ( new RelEntry() );
00098                                 SharedPointer<IVariableValue> destValue = obsVar->getValueByName("na");
00099                                 newEntry->destValues[destValue->getVariableName()] = destValue;
00100                 newEntry->prob = 1.0;
00101                                 result.push_back(newEntry);
00102 
00103             }
00104 
00105         }
00106         return result;
00107         
00108 }


appl
Author(s): petercai
autogenerated on Tue Jan 7 2014 11:02:29