Пример #1
0
      // Feed the  constraint equations to the solver
   void GeneralConstraint::process( gnssDataMap& gdsMap,
                                    GeneralEquations* gEquPtr )
   { 
      if(gEquPtr)
      {
         solver.setEquationSystemConstraints(
                                         gEquPtr->getConstraintSystem(gdsMap) );

         CommonTime time( ( *gdsMap.begin() ).first );
         updateRefSat( time,
                       gEquPtr->getRefSatSourceMap(),
                       gEquPtr->getSourceRefSatMap() );
         
         solver.Process(gdsMap); 

         refsatSourceMap = gEquPtr->getRefSatSourceMap();
         sourceRefsatMap = gEquPtr->getSourceRefSatMap();

         constraint(gdsMap); 
      }
      else
      {
         solver.Process(gdsMap); 
         constraint(gdsMap); 
      }

   }  // End of method 'GeneralConstraint::process(...'
Пример #2
0
// update the satellite data due to the input GDS object
void GeneralEquations::updateSourceSatDataMap( const gnssDataMap& gdsMap )
{

    SourceSatDataMap dataMap;

    // Iterate through all items in the gnssDataMap
    for( gnssDataMap::const_iterator it = gdsMap.begin();
            it != gdsMap.end();
            ++it )
    {

        // Look for current SourceID
        sourceDataMap::const_iterator sdmIter; //(it->second.find(source));
        for( sdmIter=it->second.begin();
                sdmIter!=it->second.end();
                ++sdmIter )
        {

            SourceID source(sdmIter->first);
            SatData data;

            // Iterate through corresponding 'satTypeValueMap'
            satTypeValueMap::const_iterator stvmIter;
            for( stvmIter = sdmIter->second.begin();
                    stvmIter != sdmIter->second.end();
                    ++stvmIter )
            {

                SatID sat(stvmIter->first);

                typeValueMap::const_iterator itt1 =
                    stvmIter->second.find(TypeID::elevation);

                typeValueMap::const_iterator itt2 =
                    stvmIter->second.find(TypeID::CSL1);

                if( (itt1==stvmIter->second.end()) ||
                        (itt2==stvmIter->second.end())   )
                {
                    Exception e("Elevation was not found.");
                    GPSTK_THROW(e);
                }

                data.addData(sat, itt1->second,
                             (itt2->second!=0.0)?true:false, false);

            }  // End of 'for( satTypeValueMap::const_iterator ...'

            dataMap[source] = data;

        }  // End of 'for( sdmIter=it->second.begin();...'

    }  // End of 'for( gnssDataMap::const_iterator it = ...'

    sourceSatDataMap = dataMap;

}  // End of method 'void GeneralEquations::updateSourceSatDataMap'
Пример #3
0
// Synchronize the CS flag of input GDS object with the
// SourceSatDataMap object
void GeneralEquations::synchronizeCSFlag( const SourceSatDataMap& dataMap,
        gnssDataMap& gdsMap )
{

    // Iterate through the gnssDatamap
    for( gnssDataMap::iterator it = gdsMap.begin();
            it != gdsMap.end();
            ++it )
    {

        // Look for current SourceID
        for( sourceDataMap::iterator sdmIter = it->second.begin();
                sdmIter != it->second.end();
                ++sdmIter)
        {

            SourceID source(sdmIter->first);

            // Iterate through corresponding 'satTypeValueMap'
            for( satTypeValueMap::iterator stvmIter = sdmIter->second.begin();
                    stvmIter != sdmIter->second.end();
                    ++stvmIter )
            {

                SatID sat(stvmIter->first);

                SourceSatDataMap::const_iterator its = dataMap.find(source);
                if( its!=dataMap.end() )
                {

                    int index = its->second.indexOfSat(sat);
                    if( index>=0 )
                    {

                        double csValue = its->second.csflag[index]?1.0:0.0;

                        stvmIter->second[TypeID::CSL1] = csValue;
                        stvmIter->second[TypeID::CSL2] = csValue;

                    }  // End of 'if( index>=0 )'

                }  // End of 'if( its!=dataMap.end() )'

            }  // End of 'for( satTypeValueMap::const_iterator ...'

        }  // End of 'for(sourceDataMap::iterator '

    }  // End of 'for( gnssDataMap::const_iterator it = ...'

}  // End of method 'GeneralEquations::synchronizeCSFlag()'
Пример #4
0
      // Prepare set of current unknowns and list of current equations
   VariableSet EquationSystem::prepareCurrentUnknownsAndEquations(
                                                         gnssDataMap& gdsMap )
   {

         // Let's clear the current equations list
      currentEquationsList.clear();

         // Let's create 'currentUnkSet' set
      VariableSet currentUnkSet;

         // Get "currentSatSet" and "currentSourceSet"
         // and stored in currentSourceSet and currentSatSet
      prepareCurrentSourceSat( gdsMap );


         // Visit each "Equation" in "equationDescriptionList"
      for( std::list<Equation>::const_iterator itEq =
                                                equationDescriptionList.begin();
           itEq != equationDescriptionList.end();
           ++itEq )
      {

            // First, get the SourceID set for this equation description
         SourceIDSet equSourceSet;

            // Check if current equation description is valid for all sources
         if ( (*itEq).getEquationSource() == Variable::allSources )
         {
            equSourceSet = currentSourceSet;
         }
         else
         {

               // Check if equation description is valid for some sources
            if ( (*itEq).getEquationSource() == Variable::someSources )
            {

                  // We have to find the intersection between equation
                  // description SourceID's and available SourceID's.
               SourceIDSet tempSourceSet( (*itEq).getSourceSet() );

                  // Declare an 'insert_iterator' to be used by
                  // 'set_intersection' algorithm (provided by STL)
               std::insert_iterator< SourceIDSet >
                                 itOut( equSourceSet, equSourceSet.begin() );

                  // Let's intersect both sets
               set_intersection( tempSourceSet.begin(), tempSourceSet.end(),
                              currentSourceSet.begin(), currentSourceSet.end(),
                              itOut );

            }
            else
            {
                  // In this case, we take directly the source into the
                  // equation source set
               equSourceSet.insert( (*itEq).getEquationSource() );
            }

         }  // End of 'if ( (*itEq).getEquationSource() == ...'
         
            // Second, get the SatID set for this equation description
         SatIDSet equSatSet = (*itEq).getSatSet();
         

            // We have the SourceID set that is applicable to this
            // equation description.

            // Now we must get the satellites visible from each
            // particular SourceID
         for( SourceIDSet::const_iterator itSource = equSourceSet.begin();
              itSource != equSourceSet.end();
              ++itSource )
         {

               // Get visible satellites from this SourceID
            SatIDSet visibleSatSet;

               // Iterate through all items in the gnssDataMap
            for( gnssDataMap::const_iterator it = gdsMap.begin();
                 it != gdsMap.end();
                 ++it )
            {

                  // Look for current SourceID
               sourceDataMap::const_iterator sdmIter(
                                             (*it).second.find( (*itSource) ) );

                  // If SourceID was found, then look for satellites
               if( sdmIter != (*it).second.end() )
               {

                     // Iterate through corresponding 'satTypeValueMap'
                  for( satTypeValueMap::const_iterator stvmIter =
                                                      (*sdmIter).second.begin();
                       stvmIter != (*sdmIter).second.end();
                       stvmIter++ )
                  {
                        // for some sat   
                     if((equSatSet.size() > 0)                           &&
                        (equSatSet.find((*stvmIter).first) == equSatSet.end()))
                     {
                        continue;
                     }

                        // Add current SatID to 'visibleSatSet'
                     visibleSatSet.insert( (*stvmIter).first );

                  }  // End of 'for( satTypeValueMap::const_iterator ...'

               }  // End of 'for( sourceDataMap::const_iterator sdmIter = ...'

            }  // End of 'for( gnssDataMap::const_iterator it = ...'

               // We have the satellites visible from this SourceID

               
               // We need a copy of current Equation object description
            Equation tempEquation( (*itEq) );

               // Remove all variables from current equation
            tempEquation.clear();

               // Update equation independent term with SourceID information
            tempEquation.header.equationSource = (*itSource);

               // Now, let's visit all Variables in this equation description
            for( VariableSet::const_iterator itVar = (*itEq).body.begin();
                 itVar != (*itEq).body.end();
                 ++itVar )
            {

                  // We will work with a copy of current Variable
               Variable var( (*itVar) );

                  // Check what type of variable we are working on

                  // If variable is source-indexed, set SourceID
               if( var.getSourceIndexed() )
               {
                  var.setSource( (*itSource) );
               }

                  // Add this variable to current equation description. Please
                  // be aware that satellite-indexed variables inside current
                  // equations will be handled later
               tempEquation.addVariable(var);

                  // If variable is not satellite-indexed, we just need to
                  // add it to "currentUnkSet
               if( !var.getSatIndexed() )
               {
                     // Insert the result in "currentUnkSet" and
                     // current equation
                  currentUnkSet.insert(var);
                  //tempEquation.addVariable(var);
               }
               else
               {
                     // If variable IS satellite-indexed, we have to visit all
                     // visible satellites (from current SourceID) and set the
                     // satellite before adding variable to "currentUnkSet
                  for( SatIDSet::const_iterator itSat = visibleSatSet.begin();
                       itSat != visibleSatSet.end();
                       ++itSat )
                  {

                        // Set satellite
                     var.setSatellite( (*itSat) );

                        // Insert the result in "currentUnkSet" and
                        // current equation
                     currentUnkSet.insert(var);
                  }

               }  // End of 'if( !var.getSatIndexed() )...'

            }  // End of 'for( VariableSet::const_iterator itVar = ...'


               // Let's generate the current equations starting from this
               // equation description. Therefore, we update equation
               // independent term with SatID information and add each instance
               // to 'currentEquationsList'.
            for( SatIDSet::const_iterator itSat = visibleSatSet.begin();
                 itSat != visibleSatSet.end();
                 ++itSat )
            {
               tempEquation.header.equationSat = (*itSat);

                  // New equation is complete: Add it to 'currentEquationsList'
               currentEquationsList.push_back( tempEquation );
            }

         }  // End of 'for( SourceIDSet::const_iterator itSource = ...'

      }  // End of 'for( std::list<Equation>::const_iterator itEq = ...'


         // Now we will take care of satellite-indexed variables inside each
         // specific "Equation" in "currentEquationsList"
      const size_t eqListSize( currentEquationsList.size() );
      for( size_t i = 0; i < eqListSize; ++i )
      {

            // Get a copy of first equation on 'currentEquationsList'
         Equation tempEqu( currentEquationsList.front() );

            // Remove the original equation at the beginning of the list.
         currentEquationsList.pop_front();

            // Get a copy of variables inside this equation
         VariableSet varSet( tempEqu.body );

            // Clear the variables from this equation
         tempEqu.clear();

            // Visit each variable inside 'varSet', check if it is
            // satellite-indexed, and add it to equation
         for( VariableSet::iterator itVar = varSet.begin();
              itVar != varSet.end();
              ++itVar )
         {

               // Check if it is satellite-indexed
            if( !(*itVar).getSatIndexed() )
            {
                  // If not satellite-indexed, just add it back
               tempEqu.addVariable( (*itVar) );
            }
            else
            {
               // If 'itVar' is satellite-indexed, let's index a copy of it
               // and add it to equation
               Variable var( (*itVar) );
               var.setSatellite( tempEqu.header.equationSat );

               tempEqu.addVariable( var );
            }

         }  // End of 'for( VariableSet::iterator itVar = varSet.begin(); ...'

            // Our equation is ready, let's add it to the end of the list
         currentEquationsList.push_back( tempEqu );

      }  // End of 'for( int i = 0; i < eqListSize; ++i ) ...'


         // Return set of current unknowns
      return currentUnkSet;

   }  // End of method 'EquationSystem::prepareCurrentUnknownsAndEquations()'