// 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()'
