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