ConstraintSystem& ConstraintSystem::constraintMatrix(
const VariableSet& allVar,
Vector<double>& prefit,
Matrix<double>& design,
Matrix<double>& covariance )
throw(InvalidConstraintSystem)
{
const int rowSize = constraintList.size();
const int colSize = allVar.size();
prefit.resize(rowSize,0.0);
design.resize(rowSize,colSize,0.0);
covariance.resize(rowSize,rowSize,0.0);
int irow(0);
for(ConstraintList::iterator it=constraintList.begin();
it!=constraintList.end();
++it)
{
prefit[irow] = it->header.prefit;
covariance[irow][irow] = it->header.variance;
VariableDataMap dataMap = it->body;
for(VariableDataMap::iterator itv=dataMap.begin();
itv!=dataMap.end();
++itv)
{
///
VariableSet::const_iterator itt = allVar.find(itv->first);
if(itt==allVar.end())
{
InvalidConstraintSystem e("The variable not exist in the input");
GPSTK_THROW(e);
}
int icol(0);
for(VariableSet::const_iterator itt2=allVar.begin();
itt2!=allVar.end();
++itt2)
{
if(itt == itt2) break;
icol++;
}
design[irow][icol] = itv->second;
}
irow++;
}
return (*this);
} // End of method 'ConstraintSystem::constraintMatrix()'
// Impose the constraints system to the equation system
// the prefit residuals vector, hMatrix and rMatrix will be appended.
void EquationSystem::imposeConstraints()
{
if(!equationConstraints.hasConstraints()) return;
ConstraintList destList;
ConstraintList tempList = equationConstraints.getConstraintList();
for(ConstraintList::iterator it = tempList.begin();
it != tempList.end();
++it )
{
VariableDataMap dataMapOk;
bool validConstraint(true);
try
{
VariableDataMap dataMap = it->body;
for(VariableDataMap::iterator itv = dataMap.begin();
itv != dataMap.end();
++itv )
{
bool isFound(false);
VariableSet::iterator itv2 = varUnknowns.find(itv->first);
if(itv2!=varUnknowns.end())
{
isFound = true;
dataMapOk[*itv2] = dataMap[itv->first];
}
else
{
for(itv2 = varUnknowns.begin();
itv2 != varUnknowns.end();
++itv2 )
{
if( (itv->first.getType() == itv2->getType()) &&
(itv->first.getSource() == itv2->getSource()) &&
(itv->first.getSatellite() == itv2->getSatellite()) )
{
isFound = true;
dataMapOk[*itv2] = dataMap[itv->first];
break;
}
}
}
if( !isFound ) validConstraint = false;
}
}
catch(...)
{
validConstraint = false;
}
if(validConstraint)
{
destList.push_back(Constraint(it->header,dataMapOk));
}
else
{
// we discard all constraints
return;
}
}
// Update the equation system
equationConstraints.setConstraintList(destList);
// Now, we can append the matrix(prefit design and weight)
try
{
Vector<double> meas;
Matrix<double> design;
Matrix<double> cov;
equationConstraints.constraintMatrix(varUnknowns,
meas, design, cov);
const int oldSize = measVector.size();
const int newSize = oldSize + meas.size();
const int colSize = hMatrix.cols();
Vector<double> tempPrefit(newSize,0.0);
Matrix<double> tempGeometry(newSize,colSize,0.0);
Matrix<double> tempWeight(newSize,newSize,0.0);
for(int i=0; i< newSize; i++)
{
// prefit
if(i<oldSize) tempPrefit(i) = measVector(i);
else tempPrefit(i) = meas(i-oldSize);
// geometry
for(int j=0;j<colSize;j++)
{
if(i<oldSize) tempGeometry(i,j) = hMatrix(i,j);
else tempGeometry(i,j) = design(i-oldSize,j);
}
// weight
if(i<oldSize) tempWeight(i,i) = rMatrix(i,i);
else tempWeight(i,i) = 1.0/cov(i-oldSize,i-oldSize);
}
// Update these matrix
measVector = tempPrefit;
hMatrix = tempGeometry;
rMatrix = tempWeight;
}
catch(...)
{
return;
}
} // End of method 'EquationSystem::imposeConstraints()'
