static void cancel (
int rows ,
int cols ,
int lower ,
int index ,
double matnorm ,
double *a ,
double *w ,
double *work
)
{
int col, row ;
double c, rr, ww, hypot, s, pp, qq ;
s = 1.0 ;
for (col=lower ; col<=index ; col++) {
rr = s * work[col] ;
if (fabs (rr) + matnorm != matnorm) {
ww = w[col] ;
hypot = RSS ( rr , ww ) ;
w[col] = hypot ;
c = ww / hypot ;
s = -rr / hypot ;
for (row=0 ; row<rows ; row++) {
pp = a[row*cols+lower-1] ;
qq = a[row*cols+col] ;
a[row*cols+lower-1] = qq * s + pp * c ;
a[row*cols+col] = qq * c - pp * s ;
}
}
}
}
void CorrectedEphemerisRange::rotateEarth(const Position& Rx)
{
GPSEllipsoid ellipsoid;
double tof = RSS(svPosVel.x[0]-Rx.X(),
svPosVel.x[1]-Rx.Y(),
svPosVel.x[2]-Rx.Z())/ellipsoid.c();
double wt = ellipsoid.angVelocity()*tof;
double sx = ::cos(wt)*svPosVel.x[0] + ::sin(wt)*svPosVel.x[1];
double sy = -::sin(wt)*svPosVel.x[0] + ::cos(wt)*svPosVel.x[1];
svPosVel.x[0] = sx;
svPosVel.x[1] = sy;
sx = ::cos(wt)*svPosVel.v[0] + ::sin(wt)*svPosVel.v[1];
sy = -::sin(wt)*svPosVel.v[0] + ::cos(wt)*svPosVel.v[1];
svPosVel.v[0] = sx;
svPosVel.v[1] = sy;
}
shared_ptr<IDbResult> Connection::_query(const String &sql)
{
OS_LOCK(m_cs);
#ifdef OS_ENABLE_DATABASE_ANALYZE
RealtimeStatsScopeTimer RSS(_S("Debug"), _S("Sqlite - ") + preAnalyze(sql));
#endif
shared_ptr<IDbResult> result = doQuery(m_connection, sql);
#ifdef OS_ENABLE_DATABASE_ANALYZE
postAnalyze(sql);
#endif
return result;
}
uint32 Connection::_execute(const String &sql)
{
OS_LOCK(m_cs);
//RealtimeStatsScopeTimer RSS(_S("Debug"), _S("Sqlite - Connection::execute"));
//OS_LOG_WARNING(sql);
#ifdef OS_ENABLE_DATABASE_ANALYZE
RealtimeStatsScopeTimer RSS(_S("Debug"), _S("Sqlite - ") + preAnalyze(sql));
#endif
uint32 result = doExecute(m_connection, sql);
#ifdef OS_ENABLE_DATABASE_ANALYZE
postAnalyze(sql);
#endif
return result;
}
void CorrectedEphemerisRange::rotateEarth(const Position& Rx)
{
GPSGeoid geoid;
double tof = RSS(svPosVel.x[0]-Rx.X(),
svPosVel.x[1]-Rx.Y(),
svPosVel.x[2]-Rx.Z())/geoid.c();
double wt = geoid.angVelocity()*tof;
double sx = cos(wt)*svPosVel.x[0] + sin(wt)*svPosVel.x[1];
double sy = -sin(wt)*svPosVel.x[0] + cos(wt)*svPosVel.x[1];
svPosVel.x[0] = sx;
svPosVel.x[1] = sy;
sx = cos(wt)*svPosVel.v[0] + sin(wt)*svPosVel.v[1];
sy = -sin(wt)*svPosVel.v[0] + cos(wt)*svPosVel.v[1];
svPosVel.v[0] = sx;
svPosVel.v[1] = sy;
}
shared_ptr<IDbStatement> Connection::_prepare(const String &sql)
{
//RealtimeStatsScopeTimer RSS(_S("Debug"), _S("Sqlite - Connection::prepare"));
//OS_LOG_WARNING(sql);
#ifdef OS_ENABLE_DATABASE_ANALYZE
RealtimeStatsScopeTimer RSS(_S("Debug"), _S("Sqlite - ") + preAnalyze(sql));
#endif
shared_ptr<Statement> statement(OS_NEW Statement(m_cs));
statement->prepare(m_connection, sql);
#ifdef OS_ENABLE_DATABASE_ANALYZE
postAnalyze(sql);
#endif
return statement;
}
// Compute the corrected range at RECEIVE time, from receiver at position Rx,
// to the GPS satellite given by SatID sat, as well as all the CER quantities,
// given the nominal receive time tr_nom and an EphemerisStore. Note that this
// routine does not intrinsicly account for the receiver clock error
// like the ComputeAtTransmitTime routine does.
double CorrectedEphemerisRange::ComputeAtReceiveTime(
const CommonTime& tr_nom,
const Position& Rx,
const SatID sat,
const XvtStore<SatID>& Eph)
{
try {
int nit;
double tof,tof_old;
GPSEllipsoid ellipsoid;
nit = 0;
tof = 0.07; // initial guess 70ms
do {
// best estimate of transmit time
transmit = tr_nom;
transmit -= tof;
tof_old = tof;
// get SV position
try {
svPosVel = Eph.getXvt(sat, transmit);
}
catch(InvalidRequest& e) {
GPSTK_RETHROW(e);
}
rotateEarth(Rx);
// update raw range and time of flight
rawrange = RSS(svPosVel.x[0]-Rx.X(),
svPosVel.x[1]-Rx.Y(),
svPosVel.x[2]-Rx.Z());
tof = rawrange/ellipsoid.c();
} while(ABS(tof-tof_old)>1.e-13 && ++nit<5);
updateCER(Rx);
return (rawrange-svclkbias-relativity);
}
catch(gpstk::Exception& e) {
GPSTK_RETHROW(e);
}
} // end CorrectedEphemerisRange::ComputeAtReceiveTime
// Compute the corrected range at TRANSMIT time, from receiver at position Rx,
// to the GPS satellite given by SatID sat, as well as all the CER quantities,
// given the nominal receive time tr_nom and an EphemerisStore, as well as
// the raw measured pseudorange.
double CorrectedEphemerisRange::ComputeAtTransmitTime(
const CommonTime& tr_nom,
const double& pr,
const Position& Rx,
const SatID sat,
const XvtStore<SatID>& Eph)
{
try {
CommonTime tt;
// 0-th order estimate of transmit time = receiver - pseudorange/c
transmit = tr_nom;
transmit -= pr/C_MPS;
tt = transmit;
// correct for SV clock
for(int i=0; i<2; i++) {
// get SV position
try {
svPosVel = Eph.getXvt(sat,tt);
}
catch(InvalidRequest& e) {
GPSTK_RETHROW(e);
}
tt = transmit;
// remove clock bias and relativity correction
tt -= (svPosVel.clkbias + svPosVel.relcorr);
}
rotateEarth(Rx);
// raw range
rawrange = RSS(svPosVel.x[0]-Rx.X(),
svPosVel.x[1]-Rx.Y(),
svPosVel.x[2]-Rx.Z());
updateCER(Rx);
return (rawrange-svclkbias-relativity);
}
catch(gpstk::Exception& e) {
GPSTK_RETHROW(e);
}
} // end CorrectedEphemerisRange::ComputeAtTransmitTime
/* 'Data' Matrix of data containing observation data in rows, one
* row per observation and complying with this format:
* x y z P
* Where x,y,z are satellite coordinates in an ECEF system
* and P is pseudorange (corrected as much as possible,
* specially from satellite clock errors), all expresed
* in meters.
*
* 'X' Vector of position solution, in meters. There may be
* another solution, that may be accessed with vector
* "SecondSolution" if "ChooseOne" is set to "false".
*
* Return values:
* 0 Ok
* -1 Not enough good data
* -2 Singular problem
*/
int Bancroft::Compute( Matrix<double>& Data,
Vector<double>& X )
throw(Exception)
{
try
{
int N = Data.rows();
Matrix<double> B(0,4); // Working matrix
// Let's test the input data
if( testInput )
{
double satRadius = 0.0;
// Check each row of B Matrix
for( int i=0; i < N; i++ )
{
// If Data(i,3) -> Pseudorange is NOT between the allowed
// range, then drop line immediately
if( !( (Data(i,3) >= minPRange) && (Data(i,3) <= maxPRange) ) )
{
continue;
}
// Let's compute distance between Earth center and
// satellite position
satRadius = RSS(Data(i,0), Data(i,1) , Data(i,2));
// If satRadius is NOT between the allowed range, then drop
// line immediately
if( !( (satRadius >= minRadius) && (satRadius <= maxRadius) ) )
{
continue;
}
// If everything is ok so far, then extract the good
// data row and add it to working matrix
MatrixRowSlice<double> goodRow(Data,i);
B = B && goodRow;
}
// Let's redefine "N" and check if we have enough data rows
// left in a single step
if( (N = B.rows()) < 4 )
{
return -1; // We need at least 4 data rows
}
} // End of 'if( testInput )...'
else
{
// No input filtering. Working matrix (B) and
// input matrix (Data) are equal
B = Data;
}
Matrix<double> BT=transpose(B);
Matrix<double> BTBI(4,4), M(4,4,0.0);
Vector<double> aux(4), alpha(N), solution1(4), solution2(4);
// Temporary storage for BT*B. It will be inverted later
BTBI = BT * B;
// Let's try to invert BTB matrix
try
{
BTBI = inverseChol( BTBI );
}
catch(...)
{
return -2;
}
// Now, let's compute alpha vector
for( int i=0; i < N; i++ )
{
// First, fill auxiliar vector with corresponding satellite
// position and pseudorange
aux(0) = B(i,0);
aux(1) = B(i,1);
aux(2) = B(i,2);
aux(3) = B(i,3);
alpha(i) = 0.5 * Minkowski(aux, aux);
}
Vector<double> tau(N,1.0), BTBIBTtau(4), BTBIBTalpha(4);
BTBIBTtau = BTBI * BT * tau;
BTBIBTalpha = BTBI * BT * alpha;
// Now, let's find the coeficients of the second order-equation
double a(Minkowski(BTBIBTtau, BTBIBTtau));
double b(2.0 * (Minkowski(BTBIBTtau, BTBIBTalpha) - 1.0));
double c(Minkowski(BTBIBTalpha, BTBIBTalpha));
// Calculate discriminant and exit if negative
double discriminant = b*b - 4.0 * a * c;
if (discriminant < 0.0)
{
return -2;
}
// Find possible DELTA values
double DELTA1 = ( -b + SQRT(discriminant) ) / ( 2.0 * a );
double DELTA2 = ( -b - SQRT(discriminant) ) / ( 2.0 * a );
// We need to define M matrix
M(0,0) = 1.0;
M(1,1) = 1.0;
M(2,2) = 1.0;
M(3,3) = - 1.0;
// Find possible position solutions with their implicit radii
solution1 = M * BTBI * ( BT * DELTA1 * tau + BT * alpha );
double radius1(RSS(solution1(0), solution1(1), solution1(2)));
solution2 = M * BTBI * ( BT * DELTA2 * tau + BT * alpha );
double radius2(RSS(solution2(0), solution2(1), solution2(2)));
// Let's choose the right solution
if ( ChooseOne )
{
if ( ABS(CloseTo-radius1) < ABS(CloseTo-radius2) )
{
X = solution1;
}
else
{
X = solution2;
}
}
else
{
// Both solutions will be reported
X = solution1;
SecondSolution = solution2;
}
return 0;
} // end of first "try"
catch(Exception& e)
{
GPSTK_RETHROW(e);
}
} // end Bancroft::Compute()
//--------------------------------------------------------------------------------
// SolveConstitutiveEquations
// - Solves the constitutive equation given the applied strain rate; i.e.,
// find the stress state that is compatible with the strain rate defined.
//
// Applying Newton Raphson to solve the the stress state given the strain state of
// the system.
//
//--------------------------------------------------------------------------------
EigenRep SolveConstitutiveEquations( const EigenRep & InitialStressState, // initial guess, either from Sach's or previous iteration
const vector<EigenRep> & SchmidtTensors,
const vector<Float> & CRSS,
const vector<Float> & GammaDotBase, // reference shear rate
const vector<int> & RateSensitivity,
const EigenRep & StrainRate, // Current strain rate - constant from caller
Float EpsilonConvergence,
Float MaxResolvedStress,
int MaxNumIterations )
{
const Float Coeff = 0.2; // global fudge factor
EigenRep CurrentStressState = InitialStressState;
int RemainingIterations = MaxNumIterations;
EigenRep NewStressState = InitialStressState;
EigenRep SavedState(0, 0, 0, 0, 0); // used to return to old state
while( RemainingIterations > 0 )
{
bool AdmissibleStartPointFound = false;
std::vector<Float> RSS( SchmidtTensors.size(), 0 ); // This is really RSS / tau
do // refresh critical resolved shear stress.
// check to see if it is outside of the yield
// surface, and therefore inadmissible
{
AdmissibleStartPointFound = true;
for( int i = 0; i < SchmidtTensors.size(); i ++ )
{
RSS[i] = InnerProduct( SchmidtTensors[i], CurrentStressState) / CRSS[i];
if( std::fabs( RSS[i] ) < 1e-10 )
RSS[i] = 0;
if( std::fabs( RSS[i] ) > MaxResolvedStress )
AdmissibleStartPointFound = false;
}
if( !AdmissibleStartPointFound )
CurrentStressState = SavedState + ( CurrentStressState - SavedState ) * Coeff;
RemainingIterations --;
if( RemainingIterations < 0 )
return NewStressState;
} while ( !AdmissibleStartPointFound );
std::vector<Float> GammaDot( SchmidtTensors.size(), 0 ); // This is really RSS / tau
for( int i = 0; i < SchmidtTensors.size(); i ++ )
{
if( RateSensitivity[i] -1 > 0 )
GammaDot[i] = GammaDotBase[i] * std::pow( std::fabs( RSS[i] ), static_cast<int>( RateSensitivity[i] - 1 ) );
else
GammaDot[i] = GammaDotBase[i];
}
// Construct residual vector R(Sigma), where Sigma is stress. R(Sigma) is a 5d vector
EigenRep Residual( 0, 0, 0, 0, 0 ); // current estimate
SMatrix5x5 ResidualJacobian;
ResidualJacobian.SetZero();
for( int i = 0; i < SchmidtTensors.size(); i ++ )
{
Residual += SchmidtTensors[i] * GammaDot[i] * RSS[i]; // Residual = StrainRate - sum_{k} [ m^{k}_i * |r_ss/ crss|^n ]
// Construct F', or the Jacobian
ResidualJacobian -= OuterProduct( SchmidtTensors[i], SchmidtTensors[i] )
* RateSensitivity[i] * GammaDot[i] / CRSS[i];
}
Residual = Residual - StrainRate ; // need the negative residual, instead of E - R
SavedState = CurrentStressState;
EigenRep Delta_Stress = LU_Solver( ResidualJacobian, Residual ); // <----------- Need to hangle error from this
NewStressState = CurrentStressState + Delta_Stress;
Float RelativeError = static_cast<Float>(2) * Delta_Stress.Norm() / ( NewStressState + CurrentStressState ).Norm();
CurrentStressState = NewStressState;
if( RelativeError < EpsilonConvergence )
{
break;
}
} // end while
return NewStressState;
}
//--------------------------------------------------------------------------------
// SolveConstrainedConstitutiveEquations
//
// -- This is *REALLY* close to the normal SolveConstitutiveEquations. One could
// replace both of these functions with a function that takes a functor... but
// may not do this till later.
//--------------------------------------------------------------------------------
EigenRep SolveConstrainedConstitutiveEquations( const EigenRep & InitialStressState,
const vector<EigenRep> & SchmidtTensors,
const vector<Float> & CRSS,
const vector<Float> & GammaDotBase, // reference shear rate
const vector<int> & RateSensitivity,
const EigenRep & MacroscopicStrainRate,
const EigenRep & LagrangeMultiplier, // also known as - lagrange multiplier, or lambda(x)
const EigenRep & LocalDisplacementVariation,
const SMatrix5x5 & HomogeonousReference,
Float EpsilonConvergence,
Float MaxResolvedStress,
int MaxNumIterations,
Float *NR_Error_Out )
{
const Float Coeff = 0.2; // global fudge factor
EigenRep CurrentStressState = InitialStressState;
int RemainingIterations = MaxNumIterations;
EigenRep NewStressState( -1, -3, -7, -9, -11 );
Float RelativeError = 1e5;
EigenRep SavedState = InitialStressState;
//-------------------
// DEBUG
// std::cout << "MacroscopicStrainRate " << MacroscopicStrainRate << std::endl;
// std::cout << "LagrangeMultiplier " << LagrangeMultiplier << std::endl;
// std::cout << "Local d-dot " << LocalDisplacementVariation << std::endl;
// std::cout << "Local L \n " << HomogeonousReference << std::endl;
//-------------------
while( RemainingIterations > 0 )
{
EigenRep LoopStartStressState = CurrentStressState;
bool AdmissibleStartPointFound = false;
std::vector<Float> RSS( SchmidtTensors.size(), 0 ); // This is really RSS / tau
do // refresh critical resolved shear stress.
// check to see if it is outside of the yield
// surface, and therefore inadmissible
{
AdmissibleStartPointFound = true;
Float MaxCRSS = 0;
for( int i = 0; i < SchmidtTensors.size(); i ++ )
{
RSS[i] = InnerProduct( SchmidtTensors[i], CurrentStressState) / CRSS[i];
MaxCRSS = std::max( std::fabs( RSS[i] ), MaxCRSS );
if( std::fabs( RSS[i] ) > MaxResolvedStress )
AdmissibleStartPointFound = false;
}
if( !AdmissibleStartPointFound )
CurrentStressState = SavedState + ( CurrentStressState - SavedState ) * Coeff;
RemainingIterations --;
if( RemainingIterations < 0 )
{
return CurrentStressState; // InitialStressState; // not failing gracefully at all
}
} while ( !AdmissibleStartPointFound );
std::vector<Float> GammaDot( SchmidtTensors.size(), 0 ); // This is really RSS / tau
for( int i = 0; i < SchmidtTensors.size(); i ++ )
{
if( RateSensitivity[i] != 1 )
GammaDot[i] = GammaDotBase[i] * std::pow( std::fabs( RSS[i] ), ( RateSensitivity[i] - 1 ) );
else
GammaDot[i] = GammaDotBase[i];
}
// Construct residual vector R(Sigma), where Sigma is stress. R(Sigma) is a 5d vector
EigenRep ResolvedStressSum( 0, 0, 0, 0, 0 ); // current estimate
SMatrix5x5 ResidualJacobian;
ResidualJacobian.SetZero();
for( int i = 0; i < SchmidtTensors.size(); i ++ )
{
ResolvedStressSum += SchmidtTensors[i] * GammaDot[i] * RSS[i]; // Residual = StrainRate - sum_{k} [ m^{k}_i * |r_ss/ crss|^n ]
// Construct F', or the Jacobian
ResidualJacobian += OuterProduct( SchmidtTensors[i], SchmidtTensors[i] ) * RateSensitivity[i] * GammaDot[i] / CRSS[i]; // may need a sign
}
// -------------------------------
// If we use Eigen for this section, the
// expression templating will significantly improve
// the efficiency of this section significantly via
// explicit vectorization.
// -------------------------------
SMatrix5x5 Identity5x5;
Identity5x5.SetIdentity();
ResidualJacobian = -Identity5x5 - (HomogeonousReference * ResidualJacobian) ; // was I -
EigenRep Residual = CurrentStressState - LagrangeMultiplier
+ HomogeonousReference
* ( ResolvedStressSum - MacroscopicStrainRate - LocalDisplacementVariation ); // validated
SavedState = CurrentStressState;
EigenRep Delta_Stress = LU_Solver( ResidualJacobian, Residual ); // <----------- Need to hangle error from this
NewStressState = CurrentStressState + Delta_Stress; // was - Delta
RelativeError = (NewStressState - SavedState).Norm() /( ( NewStressState + SavedState) * static_cast<Float>(0.5)).Norm();
CurrentStressState = NewStressState;
if( RelativeError < EpsilonConvergence )
break;
} // end while
*NR_Error_Out = RelativeError;
return NewStressState;
}
static void qr (
int rows ,
int cols ,
int lower ,
int index ,
double *a ,
double *v ,
double *w ,
double *work )
{
int col, colp1, row ;
double c, cn, s, sn, thisw, rot1, rot2, hypot, temp, ww ;
ww = w[index] ;
sn = work[index] ;
rot1 = work[index-1] ;
rot2 = w[index-1] ;
temp = ((rot2-ww) * (rot2+ww) + (rot1-sn) * (rot1+sn)) / (2.0 * sn * rot2) ;
hypot = RSS ( temp , 1.0 ) ;
thisw = w[lower] ;
cn = ((thisw-ww) * (thisw+ww) + sn *
((rot2 / (temp + SIGN(hypot,temp))) - sn )) / thisw ;
c = s = 1.0 ;
for (col=lower ; col<index ; col++) {
colp1 = col+1 ;
rot1 = work[colp1] ;
sn = s * rot1 ;
rot1 = c * rot1 ;
hypot = RSS ( cn , sn ) ;
work[col] = hypot ;
c = cn / hypot ;
s = sn / hypot ;
cn = thisw * c + rot1 * s ;
rot1 = rot1 * c - thisw * s ;
rot2 = w[colp1] ;
sn = rot2 * s ;
rot2 *= c ;
for (row=0 ; row<cols ; row++) {
thisw = v[row*cols+col] ;
temp = v[row*cols+colp1] ;
v[row*cols+col] = thisw * c + temp * s ;
v[row*cols+colp1] = temp * c - thisw * s ;
}
hypot = RSS ( cn , sn ) ;
w[col] = hypot ;
if (hypot != 0.0) {
c = cn / hypot ;
s = sn / hypot ;
}
cn = c * rot1 + s * rot2 ;
thisw = c * rot2 - s * rot1 ;
for (row=0 ; row<rows ; row++) {
rot1 = a[row*cols+col] ;
rot2 = a[row*cols+colp1] ;
a[row*cols+col] = rot1 * c + rot2 * s ;
a[row*cols+colp1] = rot2 * c - rot1 * s ;
}
}
w[index] = thisw ;
work[lower] = 0.0 ;
work[index] = cn ;
}
