double SumaCuadrados::getEvaluation(Vector<double> argument)
{
if (argument.getSize() != numberOfVariables){
std::cout << std::endl
<< "Error: SumaCuadrados class. Metodo getEvaluation " << std::endl
<< "dimension vector coeficientes incorrecta" << std::endl
<< std::endl;
exit(1);
}
// valores estimados en la regresión
Vector<double> res = Residuals(argument);
// tbc
// error = sum(error^2)
double error = 0;
for (int irow=0;irow<ModelMatrix.getNumberOfRows();irow++){
error+=res[irow]*res[irow];
}
//tbc
return error;
}
Vector<double> SumaCuadrados::getGradient(Vector<double> argument) {
//return 2*transpose(ModelMatrix)*(res);
Vector<double> salida(argument.getSize());
Vector<double> res = Residuals(argument);
for(int icol = 0;icol<ModelMatrix.getNumberOfColumns();icol++){
salida[icol] = 0;
for(int jrow=0;jrow<ModelMatrix.getNumberOfRows();jrow++){
salida[icol]+=2*ModelMatrix[jrow][icol]*res[jrow];
}
}
return salida;
}
int PRSolution3::RAIM2Compute(const CommonTime& Tr,
vector<SatID>& Satellite,
const vector<double>& Pseudorange,
const XvtStore<SatID>& Eph,
TropModel *pTropModel)
throw(Exception)
{
try
{
int iret,N,Nreject,MinSV;
size_t i, j;
vector<bool> UseSat, UseSave;
vector<int> GoodIndexes;
// Use these to save the 'best' solution within the loop.
int BestNIter=0;
double BestRMS=0.0, BestSL=0.0, BestConv=0.0;
Vector<double> BestSol(3,0.0);
vector<bool> BestUse;
BestRMS = -1.0; // this marks the 'Best' set as unused.
Matrix<double> BestCovariance;
// ----------------------------------------------------------------
// initialize
Valid = false;
bool hasGlonass = false;
bool hasOther = false;
Vector<int> satSystems(Satellite.size());
//Check if we have a mixed system
for ( i = 0; i < Satellite.size(); ++i)
{
satSystems[i] = ((Satellite[i].system == SatID::systemGlonass) ? (1) : (0) );
if (satSystems[i] == 1) hasGlonass = true;
else hasOther = true;
}
// Save the input solution
// (for use in rejection when ResidualCriterion is false).
if (!hasGlonass && Solution.size() != 4)
{
Solution.resize(4);
}
else if (hasGlonass && hasOther && Solution.size() != 5)
{
Solution.resize(5);
}
Solution = 0.0;
// ----------------------------------------------------------------
// fill the SVP matrix, and use it for every solution
// NB this routine can set Satellite[.]=negative when no ephemeris
i = PrepareAutonomousSolution(Tr, Satellite, Pseudorange, Eph, SVP,
Debug?pDebugStream:0);
if(Debug && pDebugStream) {
*pDebugStream << "In RAIMCompute after PAS(): Satellites:";
for(j=0; j<Satellite.size(); j++) {
RinexSatID rs(::abs(Satellite[j].id), Satellite[j].system);
*pDebugStream << " " << (Satellite[j].id < 0 ? "-":"") << rs;
}
*pDebugStream << endl;
*pDebugStream << " SVP matrix("
<< SVP.rows() << "," << SVP.cols() << ")" << endl;
*pDebugStream << fixed << setw(16) << setprecision(3) << SVP << endl;
}
if (i) return i; // return is 0 (ok) or -4 (no ephemeris)
// count how many good satellites we have
// Initialize UseSat based on Satellite, and build GoodIndexes.
// UseSat is used to mark good sats (true) and those to ignore (false).
// UseSave saves the original so it can be reused for each solution.
// Let GoodIndexes be all the indexes of Satellites that are good:
// UseSat[GoodIndexes[.]] == true by definition
for (N=0,i=0; i<Satellite.size(); i++)
{
if (Satellite[i].id > 0)
{
N++;
UseSat.push_back(true);
GoodIndexes.push_back(i);
}
else
{
UseSat.push_back(false);
}
}
UseSave = UseSat;
// don't even try if there are not 4 good satellites
if (!hasGlonass && N < 4)
return -3;
else if (hasGlonass && hasOther && N < 5)
return -3;
// minimum number of sats needed for algorithm
MinSV = 5; // this would be RAIM
// ( not really RAIM || not RAIM at all - just one solution)
if (!ResidualCriterion || NSatsReject==0)
MinSV=4;
// how many satellites can RAIM reject, if we have to?
// default is -1, meaning as many as possible
Nreject = NSatsReject;
if (Nreject == -1 || Nreject > N-MinSV)
Nreject=N-MinSV;
// ----------------------------------------------------------------
// now compute the solution, first with all the data. If this fails,
// reject 1 satellite at a time and try again, then 2, etc.
// Slopes for each satellite are computed (cf. the RAIM algorithm)
Vector<double> Slopes(Pseudorange.size());
// Residuals stores the post-fit data residuals.
Vector<double> Residuals(Satellite.size(),0.0);
// compute all the Combinations of N satellites taken 4 at a time
Combinations Combo(N,N-4);
// compute a solution for each combination of marked satellites
do{
// Mark the satellites for this combination
UseSat = UseSave;
for (i = 0; i < GoodIndexes.size(); i++)
{
if (Combo.isSelected(i))
UseSat[i]=false;
}
// ----------------------------------------------------------------
// Compute a solution given the data; ignore ranges for marked
// satellites. Fill Vector 'Slopes' with slopes for each unmarked
// satellite.
// Return 0 ok
// -1 failed to converge
// -2 singular problem
// -3 not enough good data
NIterations = MaxNIterations; // pass limits in
Convergence = ConvergenceLimit;
iret = AutonomousPRSolution(Tr, UseSat, SVP, pTropModel, Algebraic,
NIterations, Convergence, Solution, Covariance, Residuals, Slopes,
pDebugStream, ((hasGlonass && hasOther)?(&satSystems):(NULL)) );
// ----------------------------------------------------------------
// Compute RMS residual...
// and in the usual case
RMSResidual = RMS(Residuals);
// ... and find the maximum slope
MaxSlope = 0.0;
if (iret == 0)
{
for (i=0; i<UseSat.size(); i++)
{
if (UseSat[i] && Slopes(i)>MaxSlope)
MaxSlope=Slopes[i];
}
}
// ----------------------------------------------------------------
// print solution with diagnostic information
if (Debug && pDebugStream)
{
GPSWeekSecond weeksec(Tr);
*pDebugStream << "RPS " << setw(2) << "4"
<< " " << setw(4) << weeksec.week
<< " " << fixed << setw(10) << setprecision(3) << weeksec.sow
<< " " << setw(2) << N-4 << setprecision(6)
<< " " << setw(16) << Solution(0)
<< " " << setw(16) << Solution(1)
<< " " << setw(16) << Solution(2)
<< " " << setw(14) << Solution(3);
if (hasGlonass && hasOther)
*pDebugStream << " " << setw(16) << Solution(4);
*pDebugStream << " " << setw(12) << RMSResidual
<< " " << fixed << setw(5) << setprecision(1) << MaxSlope
<< " " << NIterations
<< " " << scientific << setw(8) << setprecision(2)<< Convergence;
// print the SatID for good sats
for (i=0; i<UseSat.size(); i++)
{
if (UseSat[i])
*pDebugStream << " " << setw(3)<< Satellite[i].id;
else
*pDebugStream << " " << setw(3) << -::abs(Satellite[i].id);
}
// also print the return value
*pDebugStream << " (" << iret << ")" << endl;
}// end debug print
// ----------------------------------------------------------------
// deal with the results of AutonomousPRSolution()
if (iret)
{ // failure for this combination
RMSResidual = 0.0;
Solution = 0.0;
}
else
{ // success
// save 'best' solution for later
if (BestRMS < 0.0 || RMSResidual < BestRMS)
{
BestRMS = RMSResidual;
BestSol = Solution;
BestUse = UseSat;
BestSL = MaxSlope;
BestConv = Convergence;
BestNIter = NIterations;
}
// assign to the combo
ComboSolution.push_back(Solution);
ComboSolSat.push_back(UseSat);
// quit immediately?
if ((ReturnAtOnce) && RMSResidual < RMSLimit)
break;
}
// get the next Combinations and repeat
} while(Combo.Next() != -1);
/*
// end of the stage
if (BestRMS > 0.0 && BestRMS < RMSLimit)
{ // success
iret=0;
}
*/
// ----------------------------------------------------------------
// copy out the best solution and return
Convergence = BestConv;
NIterations = BestNIter;
RMSResidual = BestRMS;
Solution = BestSol;
MaxSlope = BestSL;
Covariance =BestCovariance;
for (Nsvs=0,i=0; i<BestUse.size(); i++)
{
if (!BestUse[i])
Satellite[i].id = -::abs(Satellite[i].id);
else
Nsvs++;
}
// FIXME should this be removed? I don't even know what slope is.
if (iret==0 && BestSL > SlopeLimit)
iret=1;
if (iret==0 && BestSL > SlopeLimit/2.0 && Nsvs == 5)
iret=1;
if (iret>=0 && BestRMS >= RMSLimit)
iret=2;
if (iret==0)
Valid=true;
return iret;
}
catch(Exception& e)
{
GPSTK_RETHROW(e);
}
} // end PRSolution2::RAIMCompute()
void OpenSMOKE_KPP_SingleReactor::SolveCSTR_CorrectorDiscrete_Smart(const double tau, BzzMatrix& tmpMatrix, BzzOdeStiffObject& o)
{
// Variables
double FInitial, F0, F1;
double eps = 1.e-6;
// Should be optimized
double hMinRequested = 0.01; // Minimum reduction coefficient to apply Newton
double requestedF0 = 1.e-16; // A reactor is ok if its residuals is lower
double maxSumF = 1.e-13; // The newton method is ok if the residual is lower
int maxIterationsNewtons = 15; // Maximum number of Newton iterations
int maxJacobianAge = 5; // Frequency to update the jacobian during the Newton method
double maxRatio = 0.75; // Max reduction factor in the newton method
int maxSubIterations = 4; // Pseudo-Armijio steps
double safetyReductionCoefficient = 0.90;
// Status
status.nJacobianEvaluations = 0;
status.failure = 0;
// 1. Checking initial composition
CheckMassFractions(omega_, eps);
// 2. If the network was never solved, the ODE solver is called
if (status.nTotalCalls == 0)
{
F1 = ODESystemIntegration(tau, omega_, residuals_, o);
if (F1 > 1.e-4)
status.failure = 1;
status.norm1_over_nc = F1;
status.normInf = residuals_.MaxAbs();
status.convergence = KPP_SINGLEREACTOR_CONVERGENCE_ODE_FIRST;
status.nTotalCalls++;
return;
}
// 3. Initial residuals
F0 = Residuals(omega_, residuals_);
FInitial = F0;
// 4. Check if this reactor is already OK
if(F0 < requestedF0)
{
status.norm1_over_nc = F0;
status.normInf = residuals_.MaxAbs();
status.convergence = KPP_SINGLEREACTOR_CONVERGENCE_DIRECT;
status.nTotalCalls++;
return;
}
// 4. First step of newton method
{
PrepareJacobianNewtonMethod(JacobianFactorizedGauss_, omega_, tmpMatrix);
status.nJacobianEvaluations++;
}
// 5a. Newton's correction
Solve(JacobianFactorizedGauss_, residuals_, &d);
double hMin = NewtonStepReductionFactor(omega_, d);
// 6. Solve ODE system if Newton method is not satisfactory
if(hMin<=hMinRequested)
{
F1 = ODESystemIntegration(tau, omega_, residuals_, o);
if (F1 > 1.e-4)
status.failure = 1;
status.norm1_over_nc = F1;
status.normInf = residuals_.MaxAbs();
status.convergence = KPP_SINGLEREACTOR_CONVERGENCE_ODE_FIRST;
status.nTotalCalls++;
return;
}
// 7. Again with Newton's Method
if(hMin != 1.)
{
hMin *= safetyReductionCoefficient;
Product(hMin,&d);
}
Sum(omega_, d, &omega1);
// 8. Checking mass fractions
CheckMassFractions(omega1, eps);
// 9. Update residuals
F1 = Residuals(omega1, residuals_);
// 10. ODE (II)
if (F1>=F0)
{
F1 = ODESystemIntegration(tau, omega_, residuals_, o);
status.norm1_over_nc = F1;
status.normInf = residuals_.MaxAbs();
status.convergence = KPP_SINGLEREACTOR_CONVERGENCE_ODE_SECOND;
status.nTotalCalls++;
return;
}
int jacobianAge = 0;
for(int iteration=1;iteration<=maxIterationsNewtons;iteration++)
{
jacobianAge++;
// Check convergence
if(F1 < maxSumF)
{
omega_ = omega1;
status.norm1_over_nc = F1;
status.normInf = residuals_.MaxAbs();
status.convergence = KPP_SINGLEREACTOR_CONVERGENCE_NEWTON;
status.nNewtonIterations = iteration;
status.nTotalCalls++;
return;
}
// Updating Solution
if (iteration>1)
{
if (jacobianAge == maxJacobianAge)
{
PrepareJacobianNewtonMethod(JacobianFactorizedGauss_, omega_, tmpMatrix);
jacobianAge = 0;
status.nJacobianEvaluations++;
}
Solve(JacobianFactorizedGauss_, residuals_, &d);
double hMin = NewtonStepReductionFactor(omega_, d);
if (hMin<hMinRequested)
{
F1 = ODESystemIntegration(tau, omega_, residuals_, o);
status.norm1_over_nc = F1;
status.normInf = residuals_.MaxAbs();
status.convergence = KPP_SINGLEREACTOR_CONVERGENCE_ODE_THIRD;
status.nTotalCalls++;
return;
}
if(hMin != 1.)
{
hMin *= safetyReductionCoefficient;
Product(hMin,&d);
}
Sum(omega_, d, &omega1);
CheckMassFractions(omega1, eps);
F1 = Residuals(omega1, residuals_);
}
// If reduction is not satisfactory
int subIterations = 1;
bool flagSubIterations = true;
while (F1/F0 > maxRatio)
{
if (subIterations == maxSubIterations)
{
flagSubIterations = false;
break;
}
Product(0.50,&d);
Sum(omega_, d, &omega1);
CheckMassFractions(omega1, eps);
F1 = Residuals(omega1, residuals_);
subIterations++;
}
if (flagSubIterations == true)
{
omega_ = omega1;
F0 = F1;
}
else
{
// If the jacobian is new, then it is better to perform a ODE integration
if (jacobianAge == 0)
{
F1 = ODESystemIntegration(tau, omega_, residuals_, o);
status.norm1_over_nc = F1;
status.normInf = residuals_.MaxAbs();
status.convergence = KPP_SINGLEREACTOR_CONVERGENCE_ODE_FOURTH;
status.nTotalCalls++;
return;
}
// else before performing the ODE integration is better to perform an additional attempt
else
{
PrepareJacobianNewtonMethod(JacobianFactorizedGauss_, omega_, tmpMatrix);
jacobianAge = -1;
status.nJacobianEvaluations++;
}
}
}
}
