// 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'
// Get epoch data of the network // @gdsMap Object hold epoch observation data of the network // @return Is there more epoch data for the network bool NetworkObsStreams::readEpochData(gnssDataMap& gdsMap) throw(SynchronizeException) { // First, We clear the data map gdsMap.clear(); RinexObsStream* pRefObsStream = mapSourceStream[referenceSource]; gnssRinex gRef; if( (*pRefObsStream) >> gRef ) { gdsMap.addGnssRinex(gRef); std::map<SourceID, RinexObsStream*>::iterator it; for( it = mapSourceStream.begin(); it != mapSourceStream.end(); ++it) { if( it->first == referenceSource) continue; Synchronize* synchro = mapSourceSynchro[it->first]; synchro->setRoverData(gRef); gnssRinex gRin; try { gRin >> (*synchro); gdsMap.addGnssRinex(gRin); } catch(...) { if(synchronizeException) { std::stringstream ss; ss << "Exception when try to synchronize at epoch: " << gRef.header.epoch << std::endl; SynchronizeException e(ss.str()); GPSTK_THROW(e); } } } // End of 'for(std::map<SourceID, RinexObsStream*>::iterator it; return true; } // End of 'if( (*pRefObsStream) >> gRef )' return false; } // End of method 'NetworkObsStreams::readEpochData()'
// Get current sources (SourceID's) and satellites (SatID's) void EquationSystem::prepareCurrentSourceSat( gnssDataMap& gdsMap ) { // Clear "currentSatSet" and "currentSourceSet" currentSatSet.clear(); currentSourceSet.clear(); // Insert the corresponding SatID's in "currentSatSet" currentSatSet = gdsMap.getSatIDSet(); // Insert the corresponding SourceID's in "currentSourceSet" currentSourceSet = gdsMap.getSourceIDSet(); } // End of method 'EquationSystem::prepareCurrentSourceSat()'
// 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()'
// 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(...'
// Compute PhiMatrix void EquationSystem::getPhiQ( const gnssDataMap& gdsMap ) { const size_t numVar( varUnknowns.size() ); // Resize phiMatrix and qMatrix phiMatrix.resize( numVar, numVar, 0.0); qMatrix.resize( numVar, numVar, 0.0); // Set a counter int i(0); // Visit each "Variable" inside "varUnknowns" for( VariableSet::const_iterator itVar = varUnknowns.begin(); itVar != varUnknowns.end(); ++itVar ) { // Check if (*itVar) is inside 'currentUnknowns' if( currentUnknowns.find( (*itVar) ) != currentUnknowns.end() ) { // Get a 'gnssRinex' data structure gnssRinex gRin( gdsMap.getGnssRinex( (*itVar).getSource() ) ); // Prepare variable's stochastic model (*itVar).getModel()->Prepare( (*itVar).getSatellite(), gRin ); // Now, check if this is an 'old' variable if( oldUnknowns.find( (*itVar) ) != oldUnknowns.end() ) { // This variable is 'old'; compute its phi and q values phiMatrix(i,i) = (*itVar).getModel()->getPhi(); qMatrix(i,i) = (*itVar).getModel()->getQ(); } else { // This variable is 'new', so let's use its initial variance // instead of its stochastic model phiMatrix(i,i) = 0.0; qMatrix(i,i) = (*itVar).getInitialVariance(); } } else { // If (*itVar) is NOT inside 'currentUnknowns', then apply it // a white noise stochastic model to decorrelate it phiMatrix(i,i) = whiteNoiseModel.getPhi(); qMatrix(i,i) = whiteNoiseModel.getQ(); } // Increment counter ++i; } } // End of method 'EquationSystem::getPhiQ()'
// Compute prefit residuals vector void EquationSystem::getPrefit( gnssDataMap& gdsMap ) { // Declare temporal storage for values std::vector<double> tempPrefit; // Visit each Equation in "currentEquationsList" for( std::list<Equation>::const_iterator itEq = currentEquationsList.begin(); itEq != currentEquationsList.end(); ++itEq ) { // Store SourceID, SatID and TypeID of current equation tempPrefit.push_back( gdsMap.getValue( (*itEq).header.equationSource, (*itEq).header.equationSat, (*itEq).header.indTerm.getType() ) ); } // End of 'for( std::list<Equation>::const_iterator itEq = ...' // Then, finally get prefit residuals into appropriate gpstk::Vector measVector = tempPrefit; } // End of method 'EquationSystem::getPrefit()'
// Compute hMatrix and rMatrix void EquationSystem::getGeometryWeights( gnssDataMap& gdsMap ) { // Resize hMatrix and rMatrix hMatrix.resize( measVector.size(), varUnknowns.size(), 0.0); rMatrix.resize( measVector.size(), measVector.size(), 0.0); // Let's work with the first element of the data structure gnssDataMap gds2( gdsMap.frontEpoch() ); // Let's fill weights and geometry matrices int row(0); // Declare a counter for row number for( std::list<Equation>::const_iterator itRow = currentEquationsList.begin(); itRow != currentEquationsList.end(); ++itRow ) { // Create temporal GDS objects SourceID source( (*itRow).header.equationSource ); SatID sat( (*itRow).header.equationSat ); // Get a TypeIDSet with all the data types present in current GDS // Declare an appropriate object TypeIDSet typeSet; // We need a flag bool found( false ); // Iterate through data structure for( gnssDataMap::const_iterator itGDS = gds2.begin(); itGDS != gds2.end() && !found; ++itGDS ) { // Look for source sourceDataMap::const_iterator itSDM = (*itGDS).second.find(source); if( itSDM != (*itGDS).second.end() ) { // Get the types typeSet = (*itSDM).second.getTypeID(); found = true; } } // First, fill weights matrix // Check if current GDS has weight info. If you don't want those // weights to get into equations, please don't put them in GDS if( typeSet.find(TypeID::weight) != typeSet.end() ) { // Weights matrix = Equation weight * observation weight rMatrix(row,row) = (*itRow).header.constWeight * gds2.getValue(source, sat, TypeID::weight); } else { // Weights matrix = Equation weight rMatrix(row,row) = (*itRow).header.constWeight; } // Second, fill geometry matrix: Look for equation coefficients int col(0); // Declare a counter for column number for( VariableSet::const_iterator itCol = varUnknowns.begin(); itCol != varUnknowns.end(); ++itCol ) { // Check if unknown is in current equation and also is marked // as a current unknown if( (*itRow).body.find( (*itCol) ) != (*itRow).body.end() && currentUnknowns.find( (*itCol) ) != currentUnknowns.end() ) { // Check if '(*itCol)' unknown variable enforces a specific // coefficient if( (*itCol).isDefaultForced() ) { // Use default coefficient hMatrix(row,col) = (*itCol).getDefaultCoefficient(); } else { // Look the coefficient in provided data // Get type of current varUnknown TypeID type( (*itCol).getType() ); // Check if this type has an entry in current GDS type set if( typeSet.find(type) != typeSet.end() ) { // If type was found, insert value into hMatrix hMatrix(row,col) = gds2.getValue(source, sat, type); } else { // If value for current type is not in gdsMap, then // insert default coefficient for this variable hMatrix(row,col) = (*itCol).getDefaultCoefficient(); } } // End of 'if( (*itCol).isDefaultForced() ) ...' } // End of 'if( (*itRow).body.find( (*itCol) ) != ...' // Increment column counter ++col; } // End of 'for( VariableSet::const_iterator itCol = ...' // Handle type index variable for( VariableSet::const_iterator itCol = (*itRow).body.begin(); itCol != (*itRow).body.end(); ++itCol ) { VariableSet::const_iterator itr = rejectUnknowns.find( (*itCol) ); if( itr == rejectUnknowns.end() || (*itr).getTypeIndexed()) continue; Variable var(*itr); col = 0; for( VariableSet::const_iterator it = varUnknowns.begin(); it != varUnknowns.end(); it++) { if(((*itCol).getType() == (*it).getType()) && ((*itCol).getModel() == (*it).getModel()) && ((*itCol).getSourceIndexed() == (*it).getSourceIndexed())&& ((*itCol).getSatIndexed() == (*it).getSatIndexed()) && ((*itCol).getSource() == (*it).getSource()) && ((*itCol).getSatellite() == (*it).getSatellite()) ) { break; } col++; } // Check if '(*itCol)' unknown variable enforces a specific // coefficient if( (*itCol).isDefaultForced() ) { // Use default coefficient hMatrix(row,col) = (*itCol).getDefaultCoefficient(); } else { // Look the coefficient in provided data // Get type of current varUnknown TypeID type( (*itCol).getType() ); // Check if this type has an entry in current GDS type set if( typeSet.find(type) != typeSet.end() ) { // If type was found, insert value into hMatrix hMatrix(row,col) = gds2.getValue(source, sat, type); } else { // If value for current type is not in gdsMap, then // insert default coefficient for this variable hMatrix(row,col) = (*itCol).getDefaultCoefficient(); } } // End of 'if( (*itCol).isDefaultForced() ) ...' } // Increment row number ++row; } // End of 'std::list<Equation>::const_iterator itRow = ...' } // End of method 'EquationSystem::getGeometryWeights()'
// 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()'