This class defines an interface to routines which compute a position and time solution from pseudorange data, with a data editing algorithm based on Receiver Autonomous Integrity Monitoring (RAIM) concepts. RAIM ref. "A Baseline GPS RAIM Scheme and a Note on the Equivalence of Three RAIM Methods," by R. Grover Brown, Journal of the Institute of Navigation, Vol. 39, No. 3, Fall 1992, pg 301.
The main point of entry is RAIMCompute(); it will compute a solution given the pseudoranges from a number of satellites, using a RAIM-based algorithm to detect and exclude 'bad' data from the solution. Alternately, the user may compute a straightforward solution using all the input data, without the RAIM algorithm; this is done by first calling PreparePRSolution() and then SimplePRSolution().
The class is able to use satellite data from any GNSS (defined in SatID.hpp). The user MUST specify allowed GNSS in the vector allowedGNSS before processing. The algorithm estimates a separate clock bias for each system; the state vector (Solution) thus has components X,Y,Z,clk1,clk2,clk3... where the clocks are in the order given by the vector allowedGNSS. The time system of the clocks will be determined by the input ephemerides; usually IGS SP3 ephemerides use GPS time for all the systems (but there is still a system time offset relative to GPS for each other GNSS).
Note that at any epoch it may happen that no satellites from some system are available (either in the data or after the RAIM algorithm), in this case the clock bias for that system is undefined and set to zero.
Definition at line 216 of file PRSolution.hpp.
#include <PRSolution.hpp>
Public Member Functions | |
void | addToMemory (const Vector< double > &Sol, const Matrix< double > &Cov, const Vector< double > &PreFitResid, const Matrix< double > &Partials, const Matrix< double > &invMeasCov) |
std::string | configString (std::string tag) |
A convenience for printing the current configuarion. More... | |
int | DOPCompute (void) |
void | dumpSolution (std::ostream &os, std::string msg="PRS") |
std::string | errorCodeString (int iret) |
A convenience for printing the error code (return value) More... | |
void | fixAPSolution (const double &X, const double &Y, const double &Z) |
double | getAPV (void) |
bool | isValid () const |
Return the status of solution. More... | |
std::string | outputCLKString (std::string tag, int iret=-99) |
return string of {SYS clock} for all systems, error code and V/NV More... | |
std::string | outputNAVString (std::string tag, int iret=-99, const Vector< double > &Vec=PRSNullVector) |
return string of info in POS and CLK More... | |
std::string | outputPOSString (std::string tag, int iret=-99, const Vector< double > &Vec=PRSNullVector) |
std::string | outputRMSString (std::string tag, int iret=-99) |
std::string | outputString (std::string tag, int iret=-99, const Vector< double > &Vec=PRSNullVector) |
return string of NAV and RMS strings More... | |
std::string | outputStringHeader (std::string tag) |
return string of the form "#tag label etc" which is header for data strings More... | |
std::string | outputValidString (int iret=-99) |
int | PreparePRSolution (const CommonTime &Tr, std::vector< SatID > &Sats, const std::vector< double > &Pseudorange, NavLibrary &eph, Matrix< double > &SVP, NavSearchOrder order=NavSearchOrder::User) const |
PRSolution () | |
Constructor. More... | |
int | RAIMCompute (const CommonTime &Tr, std::vector< SatID > &Satellites, const std::vector< double > &Pseudorange, const Matrix< double > &invMC, NavLibrary &eph, TropModel *pTropModel, NavSearchOrder order=NavSearchOrder::User) |
int | RAIMComputeUnweighted (const CommonTime &Tr, std::vector< SatID > &Satellites, const std::vector< double > &Pseudorange, NavLibrary &eph, TropModel *pTropModel, NavSearchOrder order=NavSearchOrder::User) |
int | SimplePRSolution (const CommonTime &Tr, const std::vector< SatID > &Sats, const Matrix< double > &SVP, const Matrix< double > &invMC, TropModel *pTropModel, const int &niterLimit, const double &convLimit, Vector< double > &Resids, Vector< double > &Slopes) |
void | updateAPSolution (const Vector< double > &Sol) |
Public Attributes | |
std::vector< SatelliteSystem > | allowedGNSS |
Vector< double > | APSolution |
double | APV |
double | Convergence |
the RSS change in solution at the end of iterations. More... | |
double | ConvergenceLimit |
Matrix< double > | Covariance |
std::vector< SatelliteSystem > | dataGNSS |
bool | fixedAPriori |
Triple | fixedAPrioriPos |
double | GDOP |
bool | hasMemory |
Matrix< double > | invMeasCov |
int | MaxNIterations |
double | MaxSlope |
int | ndata |
int | ndof |
int | NIterations |
the actual number of iterations used More... | |
int | NSatsReject |
int | nsol |
int | Nsvs |
the number of good satellites used in the final computation More... | |
Matrix< double > | Partials |
Matrix<double> containing the partials matrix used in the final solution. More... | |
double | PDOP |
Vector< double > | PreFitResidual |
bool | RMSFlag |
double | RMSLimit |
RMS limit (m) on residual of fit. More... | |
double | RMSResidual |
std::vector< SatID > | SatelliteIDs |
bool | SlopeFlag |
double | SlopeLimit |
Slope limit (dimensionless). More... | |
Vector< double > | Solution |
double | TDOP |
DOPs computed in a call to DOPCompute() or outputString() More... | |
bool | TropFlag |
WtdAveStats | was |
The "memory" of this object, used only when hasMemory is true. More... | |
Private Attributes | |
CommonTime | currTime |
time tag of the current solution More... | |
bool | Valid |
flag: output content is valid. More... | |
Static Private Attributes | |
static const std::string | calfmt = string("%04Y/%02m/%02d %02H:%02M:%02S %P") |
time formats used in prints More... | |
static const std::string | gpsfmt = string("%4F %10.3g") |
static const GNSSTK_EXPORT Vector< double > | PRSNullVector |
empty vector used to detect default More... | |
static const std::string | timfmt = gpsfmt + string(" ") + calfmt |
|
inline |
Constructor.
Definition at line 220 of file PRSolution.hpp.
|
inline |
add newly computed solution (must be valid); update counts, APV and apriori. input parameters are from PRSolution after computing a solution.
Definition at line 606 of file PRSolution.hpp.
string gnsstk::PRSolution::configString | ( | std::string | tag | ) |
A convenience for printing the current configuarion.
Definition at line 1053 of file PRSolution.cpp.
int gnsstk::PRSolution::DOPCompute | ( | void | ) |
Compute DOPs using the partials matrix from the last successful solution. RAIMCompute(), if successful, calls this before returning. Results stored in PRSolution::TDOP,PDOP,GDOP.
Definition at line 842 of file PRSolution.cpp.
|
inline |
Definition at line 544 of file PRSolution.hpp.
string gnsstk::PRSolution::errorCodeString | ( | int | iret | ) |
A convenience for printing the error code (return value)
Definition at line 1041 of file PRSolution.cpp.
|
inline |
Fix the apriori solution to the given constant value (XYZ,m) and initialize the
Definition at line 521 of file PRSolution.hpp.
|
inline |
get the aposteriori variance of unit weight; return zero if not enough data has been collected.
Definition at line 538 of file PRSolution.hpp.
|
inline |
Return the status of solution.
Definition at line 235 of file PRSolution.hpp.
string gnsstk::PRSolution::outputCLKString | ( | std::string | tag, |
int | iret = -99 |
||
) |
return string of {SYS clock} for all systems, error code and V/NV
Definition at line 937 of file PRSolution.cpp.
string gnsstk::PRSolution::outputNAVString | ( | std::string | tag, |
int | iret = -99 , |
||
const Vector< double > & | Vec = PRSNullVector |
||
) |
return string of info in POS and CLK
Definition at line 875 of file PRSolution.cpp.
string gnsstk::PRSolution::outputPOSString | ( | std::string | tag, |
int | iret = -99 , |
||
const Vector< double > & | Vec = PRSNullVector |
||
) |
conveniences for printing the results of the pseudorange solution algorithm return string of position, error code and V/NV
Definition at line 908 of file PRSolution.cpp.
string gnsstk::PRSolution::outputRMSString | ( | std::string | tag, |
int | iret = -99 |
||
) |
return string of Nsvs, RMS residual, TDOP, PDOP, GDOP, Slope, niter, conv, satellites, error code and V/NV
Definition at line 967 of file PRSolution.cpp.
string gnsstk::PRSolution::outputString | ( | std::string | tag, |
int | iret = -99 , |
||
const Vector< double > & | Vec = PRSNullVector |
||
) |
return string of NAV and RMS strings
Definition at line 1031 of file PRSolution.cpp.
|
inline |
return string of the form "#tag label etc" which is header for data strings
Definition at line 509 of file PRSolution.hpp.
string gnsstk::PRSolution::outputValidString | ( | int | iret = -99 | ) |
Definition at line 859 of file PRSolution.cpp.
int gnsstk::PRSolution::PreparePRSolution | ( | const CommonTime & | Tr, |
std::vector< SatID > & | Sats, | ||
const std::vector< double > & | Pseudorange, | ||
NavLibrary & | eph, | ||
Matrix< double > & | SVP, | ||
NavSearchOrder | order = NavSearchOrder::User |
||
) | const |
Compute the satellite position / corrected range matrix (SVP) which is used by SimplePRSolution(). SVP is output, dimensioned (N,4) where N is the number of satellites and the length of both Satellite and Pseudorange. Data is ignored whenever Sats[i].id is < 0 and when system is not in allowedGNSS. NB caller should verify that the number of good entries (Satellite[.] > 0) is > 4 before proceeding. Even though this is a member function, it changes none of the member data.
Tr | input Measured time of reception of the data. | |
Sats | input std::vector<SatID> of satellites; satellites that are to be excluded by the algorithm are marked by a negative 'id' member; this call will mark satellites for which there is no ephemeris. | |
Pseudorange | input std::vector<double> of raw pseudoranges (parallel to Sats), in meters | |
eph[in] | NavLibrary to be used | |
SVP | output gnsstk::Matrix<double> of dimension (N,4), N is the number of satellites in Sats[] (marked or not), on output this contains the satellite positions at transmit time (cols 0-2), the corrected pseudorange (1). | |
[in] | order | How NavLibrary searches are performed. |
Definition at line 66 of file PRSolution.cpp.
int gnsstk::PRSolution::RAIMCompute | ( | const CommonTime & | Tr, |
std::vector< SatID > & | Satellites, | ||
const std::vector< double > & | Pseudorange, | ||
const Matrix< double > & | invMC, | ||
NavLibrary & | eph, | ||
TropModel * | pTropModel, | ||
NavSearchOrder | order = NavSearchOrder::User |
||
) |
Compute a position/time solution, given satellite PRNs and pseudoranges using a RAIM algorithm. This is the main computation done by this class. Before this call, allowedGNSS must be defined.
Tr | Measured time of reception of the data. |
Satellites | std::vector<SatID> of satellites; on successful return, satellites that were excluded by the algorithm are marked by a negative 'id' member. Also systems not in allowedGNSS are ignored. |
Pseudorange | std::vector<double> of raw pseudoranges (parallel to Satellite), in meters. |
invMC | gnsstk::Matrix<double> NXN measurement covariance matrix inverse (meter^-2) of the pseudorange data (for N see Sats). If this matrix has dimension 0, no weighting of the data is done. |
eph | NavLibrary to be used in the algorithm. |
pTropModel | pointer to gnsstk::TropModel for trop correction. |
Definition at line 541 of file PRSolution.cpp.
int gnsstk::PRSolution::RAIMComputeUnweighted | ( | const CommonTime & | Tr, |
std::vector< SatID > & | Satellites, | ||
const std::vector< double > & | Pseudorange, | ||
NavLibrary & | eph, | ||
TropModel * | pTropModel, | ||
NavSearchOrder | order = NavSearchOrder::User |
||
) |
Compute a RAIM solution without the measurement covariance matrix, i.e. without measurement weighting.
Definition at line 521 of file PRSolution.cpp.
int gnsstk::PRSolution::SimplePRSolution | ( | const CommonTime & | Tr, |
const std::vector< SatID > & | Sats, | ||
const Matrix< double > & | SVP, | ||
const Matrix< double > & | invMC, | ||
TropModel * | pTropModel, | ||
const int & | niterLimit, | ||
const double & | convLimit, | ||
Vector< double > & | Resids, | ||
Vector< double > & | Slopes | ||
) |
Compute a single autonomous pseudorange solution, after calling PreparePRSolution(). On output, all the member data is filled with results.
Input only (first 3 should be just as returned from PreparePRSolution()):
Tr | const. Measured time of reception of the data. On output member currTime set to this. |
Sats | const std::vector<SatID> of satellites. Satellites that are to be excluded by the algorithm are marked by a negative 'id' member. Length N. Also systems not in allowedGNSS are ignored. On output member SatelliteIDs set to this. |
SVP | const Matrix<double> of dimension (N,5) contains sat. direction cosines and corrected pseudorange data. |
invMC | const gnsstk::Matrix<double> NXN measurement covariance matrix inverse (meter^-2) of the pseudorange data (for N see Sats). If this matrix has dimension 0, no weighting of the data is done. |
pTropModel | pointer to a gnsstk::TropModel for trop correction. |
Input and output:
niterLimit | integer limit on the number of iterations. On output, member NIterations = number of iterations actually used. |
convLimit | double convergence criterion, = RSS change in solution, in meters. On output, member Convergence = final value. |
Output: (these will be resized within the function)
Resids | gnsstk::Vector<double> post-fit range residuals for each satellite (m), the length of this Vector is the number of satellites actually used (see Sats). |
Slopes | gnsstk::Vector<double> slope value used in RAIM for each good satellite, length m. |
Definition at line 188 of file PRSolution.cpp.
update apriori solution with a known solution; this is done at the end of both SimplePRSolution() and RAIMCompute()
Definition at line 576 of file PRSolution.hpp.
std::vector<SatelliteSystem> gnsstk::PRSolution::allowedGNSS |
vector<SatelliteSystem> containing the satellite systems allowed in the solution. This vector MUST be defined before computing solutions. It is used to determine which clock biases are included in the solution, as well as the apriori state vector (see hasMemory below)
Definition at line 263 of file PRSolution.hpp.
Vector<double> gnsstk::PRSolution::APSolution |
Caller is responsible for setting APSolution before first call, if desired; after that SimplePRSolution() and RAIMCompute() will update it.
Definition at line 323 of file PRSolution.hpp.
double gnsstk::PRSolution::APV |
Definition at line 315 of file PRSolution.hpp.
|
staticprivate |
time formats used in prints
Definition at line 646 of file PRSolution.hpp.
double gnsstk::PRSolution::Convergence |
the RSS change in solution at the end of iterations.
Definition at line 347 of file PRSolution.hpp.
double gnsstk::PRSolution::ConvergenceLimit |
Convergence limit (m): continue iteration loop while RSS change in solution exceeds this.
Definition at line 257 of file PRSolution.hpp.
Matrix<double> gnsstk::PRSolution::Covariance |
Matrix<double> containing the computed solution covariance (meter^2); see doc. for Solution for the components; valid only when isValid() is true.
Definition at line 299 of file PRSolution.hpp.
|
private |
time tag of the current solution
Definition at line 643 of file PRSolution.hpp.
std::vector<SatelliteSystem> gnsstk::PRSolution::dataGNSS |
vector<SatelliteSystem> containing the satellite systems found in the data at each epoch, after calls to SimplePRSolution and RAIMCompute. See also allowedGNSS.
Definition at line 311 of file PRSolution.hpp.
bool gnsstk::PRSolution::fixedAPriori |
if true, use the given APriori position instead of the current solution define by calling void fixAPSolution(X,Y,Z)
Definition at line 319 of file PRSolution.hpp.
Triple gnsstk::PRSolution::fixedAPrioriPos |
Definition at line 320 of file PRSolution.hpp.
double gnsstk::PRSolution::GDOP |
Definition at line 341 of file PRSolution.hpp.
|
staticprivate |
Definition at line 646 of file PRSolution.hpp.
bool gnsstk::PRSolution::hasMemory |
This determines whether this object will maintain a "memory" of all the solutions it has computed. This is used for several things, including the computation of pre-fit residuals, and thus of the aposteriori variance of unit weight (APV), the number of data, solutions and degrees of freedom and a combined weighted average solution. Most importantly, it causes the estimation algorithm at each epoch to be initialized with an apriori solution, which it 'remembers' from previous epochs. If multiple GNSS are used in the estimation, at any epoch, then then setAprioriGNSS() must be called before any processing, otherwise the system clock of any missing system will not be part of the apriori state.
Definition at line 275 of file PRSolution.hpp.
Matrix<double> gnsstk::PRSolution::invMeasCov |
Matrix<double> containing the inverse measurement covariance matrix (m^-2) that was used in computing the final solution.
Definition at line 303 of file PRSolution.hpp.
int gnsstk::PRSolution::MaxNIterations |
Maximum number of iterations allowed in the linearized least squares algorithm.
Definition at line 253 of file PRSolution.hpp.
double gnsstk::PRSolution::MaxSlope |
Slope computed in the RAIM algorithm (largest of all satellite values) for the returned solution, dimensionless.
Definition at line 338 of file PRSolution.hpp.
int gnsstk::PRSolution::ndata |
Definition at line 316 of file PRSolution.hpp.
int gnsstk::PRSolution::ndof |
Definition at line 316 of file PRSolution.hpp.
int gnsstk::PRSolution::NIterations |
the actual number of iterations used
Definition at line 344 of file PRSolution.hpp.
int gnsstk::PRSolution::NSatsReject |
Maximum number of satellites that may be rejected in the RAIM algorithm; if this = -1, as many as possible will be rejected (RAIM requires at least 5 satellites). A (single) non-RAIM solution can be obtained by setting this to 0 before calling RAIMCompute().
Definition at line 249 of file PRSolution.hpp.
int gnsstk::PRSolution::nsol |
Definition at line 316 of file PRSolution.hpp.
int gnsstk::PRSolution::Nsvs |
the number of good satellites used in the final computation
Definition at line 350 of file PRSolution.hpp.
Matrix<double> gnsstk::PRSolution::Partials |
Matrix<double> containing the partials matrix used in the final solution.
Definition at line 306 of file PRSolution.hpp.
double gnsstk::PRSolution::PDOP |
Definition at line 341 of file PRSolution.hpp.
Vector<double> gnsstk::PRSolution::PreFitResidual |
Prefit residuals; only valid if memory exists b/c it needs apriori solution. Vector<double> of 'pre-fit' residuals, computed by the solution routines, but only if APrioriSol is defined; equal to Partials*(Sol-APrioriSol)-Resid where Resid is the data residual vector on the first iteration.
Definition at line 330 of file PRSolution.hpp.
empty vector used to detect default
Definition at line 649 of file PRSolution.hpp.
bool gnsstk::PRSolution::RMSFlag |
if true, the returned solution may be degraded because the RMS residual or the slope is large; applies only after calls to RAIMCompute().
Definition at line 359 of file PRSolution.hpp.
double gnsstk::PRSolution::RMSLimit |
RMS limit (m) on residual of fit.
Definition at line 240 of file PRSolution.hpp.
double gnsstk::PRSolution::RMSResidual |
Root mean square residual of fit (except when RMSDistanceFlag is set, then RMS distance from apriori position); in meters.
Definition at line 334 of file PRSolution.hpp.
std::vector<SatID> gnsstk::PRSolution::SatelliteIDs |
vector<SatID> containing satellite IDs for all the satellites input, with bad (excluded) ones identified by (Satellite[.] < 0). This vector is saved after each call to the computation routines (SimplePRSolution and RAIMCompute) and used for printing.
Definition at line 283 of file PRSolution.hpp.
bool gnsstk::PRSolution::SlopeFlag |
Definition at line 359 of file PRSolution.hpp.
double gnsstk::PRSolution::SlopeLimit |
Slope limit (dimensionless).
Definition at line 243 of file PRSolution.hpp.
Vector<double> gnsstk::PRSolution::Solution |
Vector<double> containing the computed position solution (3 components, ECEF in the frame of the ephemeris, meter), the receiver clock bias (m), and the GPS-GLO time offset (m). In the case of GPS-only or GLO-only data, the last element is zero and has no meaning. This vector is valid only when isValid() is true. If this vector is defined on input, it is used as an apriori position, both to initialized the iterative algorithm, and to compute position residuals after a good solution is found.
Definition at line 295 of file PRSolution.hpp.
double gnsstk::PRSolution::TDOP |
DOPs computed in a call to DOPCompute() or outputString()
Definition at line 341 of file PRSolution.hpp.
Definition at line 646 of file PRSolution.hpp.
bool gnsstk::PRSolution::TropFlag |
if true, the returned solution may be degraded because the tropospheric correction was not applied to one or more satellites; applies after calls to both SimplePRSolution() and RAIMCompute().
Definition at line 355 of file PRSolution.hpp.
|
private |
flag: output content is valid.
Definition at line 640 of file PRSolution.hpp.
WtdAveStats gnsstk::PRSolution::was |
The "memory" of this object, used only when hasMemory is true.
Definition at line 314 of file PRSolution.hpp.