VariableSet GeneralConstraint::getVariables( const SatID& sat,
const TypeIDSet& typeSet )
{
VariableSet vset;
VariableSet varSet = getVariables(sat);
for(VariableSet::iterator itv=varSet.begin();
itv!=varSet.end();
++itv)
{
TypeIDSet::const_iterator it = typeSet.find(itv->getType());
if( (it!=typeSet.end()) ) vset.insert(*itv);
}
return vset;
} // End of method 'GeneralConstraint::getVariables(...'
VariableSet GeneralConstraint::getVariables( const SourceID& source,
const TypeIDSet& typeSet )
{
VariableSet vset;
VariableSet varSet = getVariables(source);
for(VariableSet::iterator itv=varSet.begin();
itv!=varSet.end();
++itv)
{
TypeIDSet::const_iterator it = typeSet.find(itv->getType());
if( (it!=typeSet.end()) && itv->getSourceIndexed() )
{
vset.insert(*itv);
}
}
return vset;
} // End of method 'GeneralConstraint::getVariables'
// 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()'
