0.4.1
Loading...
Searching...
No Matches
NAV::SPP::Algorithm Class Reference

Single Point Positioning Algorithm. More...

Public Types

enum class  EstimatorType : uint8_t {
  LeastSquares ,
  WeightedLeastSquares ,
  KalmanFilter ,
  COUNT
}
 Possible SPP estimation algorithms. More...
 
enum  ReceiverType : uint8_t {
  Rover ,
  ReceiverType_COUNT
}
 Receiver Types. More...
 

Public Member Functions

std::shared_ptr< SppSolutioncalcSppSolution (const std::shared_ptr< const GnssObs > &gnssObs, const std::vector< const GnssNavInfo * > &gnssNavInfos, const std::string &nameId)
 Calculate the SPP solution.
 
EstimatorType getEstimatorType () const
 Get the Estimator Type.
 
const InsTimegetLastUpdateTime () const
 Get the last update time.
 
void reset ()
 Reset the algorithm.
 
bool ShowGuiWidgets (const char *id, float itemWidth, float unitWidth)
 Shows the GUI input to select the options.
 

Data Fields

bool _estimateInterFreqBiases
 Estimate Inter-frequency biases.
 
ObservationEstimator _obsEstimator
 Observation Estimator.
 
ObservationFilter _obsFilter
 Observation Filter.
 

Private Types

using Receiver
 Receiver.
 

Private Member Functions

void assignKalmanFilterResult (const KeyedVectorXd< States::StateKeyType > &state, const KeyedMatrixXd< States::StateKeyType, States::StateKeyType > &variance, const std::string &nameId)
 Assigns the result to the receiver variable.
 
void assignLeastSquaresResult (const KeyedVectorXd< States::StateKeyType > &state, const KeyedMatrixXd< States::StateKeyType, States::StateKeyType > &variance, const Eigen::Vector3d &e_oldPos, size_t nParams, size_t nUniqueDopplerMeas, double dt, const std::string &nameId)
 Assigns the result to the receiver variable.
 
KeyedMatrixXd< Meas::MeasKeyTypes, States::StateKeyTypecalcMatrixH (const std::vector< States::StateKeyType > &stateKeys, const std::vector< Meas::MeasKeyTypes > &measKeys, const Observations &observations, const std::string &nameId) const
 Calculates the measurement sensitivity matrix 𝐇
 
bool canCalculateVelocity (size_t nDoppMeas) const
 Checks if the SPP algorithm can calculate the position (always true for Kalman filter)
 
bool canEstimateInterFrequencyBias () const
 Checks if the SPP algorithm can estimate inter-frequency biases.
 
void computeDOPs (const std::shared_ptr< SppSolution > &sppSol, const KeyedMatrixXd< Meas::MeasKeyTypes, States::StateKeyType > &H, const std::string &nameId)
 Computes all DOP values (by reference)
 
std::vector< Meas::MeasKeyTypesdetermineMeasKeys (const Observations &observations, size_t nPsrMeas, size_t nDoppMeas, const std::string &nameId) const
 Returns a list of measurement keys.
 
std::vector< States::StateKeyTypedetermineStateKeys (const std::set< SatelliteSystem > &usedSatSystems, size_t nDoppMeas, const std::string &nameId) const
 Returns a list of state keys.
 
void updateInterFrequencyBiases (const Observations &observations, const std::string &nameId)
 Updates the inter frequency biases.
 

Static Private Member Functions

static KeyedMatrixXd< Meas::MeasKeyTypes, Meas::MeasKeyTypescalcMatrixR (const std::vector< Meas::MeasKeyTypes > &measKeys, const Observations &observations, const std::string &nameId)
 Calculates the measurement noise covariance matrix 𝐑
 
static KeyedVectorXd< Meas::MeasKeyTypescalcMeasInnovation (const std::vector< Meas::MeasKeyTypes > &measKeys, const Observations &observations, const std::string &nameId)
 Calculates the measurement innovation vector 𝜹𝐳
 

Private Attributes

Eigen::Matrix3d _e_lastPositionCovarianceMatrix
 Last position covariance matrix.
 
Eigen::Matrix3d _e_lastVelocityCovarianceMatrix
 Last velocity covariance matrix.
 
EstimatorType _estimatorType
 Estimator type used for the calculations.
 
SPP::KalmanFilter _kalmanFilter
 SPP specific Kalman filter.
 
InsTime _lastUpdate
 Time of last update.
 
Receiver _receiver
 Receiver.
 
const std::array< SPP::States::StateKeyType, 3 > & PosKey
 All position keys.
 
const std::array< SPP::States::StateKeyType, 6 > & PosVelKey
 All position and velocity keys.
 
const std::array< SPP::States::StateKeyType, 3 > & VelKey
 All velocity keys.
 

Friends

void from_json (const json &j, Algorithm &obj)
 Converts the provided json object into a node object.
 
void to_json (json &j, const Algorithm &obj)
 Converts the provided object into json.
 

Detailed Description

Single Point Positioning Algorithm.

Definition at line 37 of file Algorithm.hpp.

Member Typedef Documentation

◆ Receiver

Receiver.

Definition at line 92 of file Algorithm.hpp.

Member Enumeration Documentation

◆ EstimatorType

enum class NAV::SPP::Algorithm::EstimatorType : uint8_t
strong

Possible SPP estimation algorithms.

Enumerator
LeastSquares 

Linear Least Squares.

WeightedLeastSquares 

Weighted Linear Least Squares.

KalmanFilter 

Kalman Filter.

COUNT 

Amount of items in the enum.

Definition at line 41 of file Algorithm.hpp.

◆ ReceiverType

Receiver Types.

Enumerator
Rover 

Rover.

ReceiverType_COUNT 

Amount of receiver types.

Definition at line 50 of file Algorithm.hpp.

Member Function Documentation

◆ assignKalmanFilterResult()

void NAV::SPP::Algorithm::assignKalmanFilterResult ( const KeyedVectorXd< States::StateKeyType > & state,
const KeyedMatrixXd< States::StateKeyType, States::StateKeyType > & variance,
const std::string & nameId )
private

Assigns the result to the receiver variable.

Parameters
stateTotal state
varianceVariance of the state
[in]nameIdName and id of the node calling this (only used for logging purposes)

Definition at line 695 of file Algorithm.cpp.

◆ assignLeastSquaresResult()

void NAV::SPP::Algorithm::assignLeastSquaresResult ( const KeyedVectorXd< States::StateKeyType > & state,
const KeyedMatrixXd< States::StateKeyType, States::StateKeyType > & variance,
const Eigen::Vector3d & e_oldPos,
size_t nParams,
size_t nUniqueDopplerMeas,
double dt,
const std::string & nameId )
private

Assigns the result to the receiver variable.

Parameters
[in]stateDelta state
[in]varianceVariance of the state
[in]e_oldPosOld position in ECEF coordinates in [m]
[in]nParamsNumber of parameters to estimate the position
[in]nUniqueDopplerMeasNumber of available doppler measurements (unique per satellite)
[in]dtTime step size in [s]
[in]nameIdName and id of the node calling this (only used for logging purposes)

Definition at line 593 of file Algorithm.cpp.

◆ calcMatrixH()

KeyedMatrixXd< Meas::MeasKeyTypes, States::StateKeyType > NAV::SPP::Algorithm::calcMatrixH ( const std::vector< States::StateKeyType > & stateKeys,
const std::vector< Meas::MeasKeyTypes > & measKeys,
const Observations & observations,
const std::string & nameId ) const
nodiscardprivate

Calculates the measurement sensitivity matrix 𝐇

Parameters
[in]stateKeysState Keys
[in]measKeysMeasurement Keys
[in]observationsList of GNSS observation data used for the calculation
[in]nameIdName and id of the node calling this (only used for logging purposes)
Returns
The 𝐇 matrix

Definition at line 485 of file Algorithm.cpp.

◆ calcMatrixR()

KeyedMatrixXd< Meas::MeasKeyTypes, Meas::MeasKeyTypes > NAV::SPP::Algorithm::calcMatrixR ( const std::vector< Meas::MeasKeyTypes > & measKeys,
const Observations & observations,
const std::string & nameId )
staticnodiscardprivate

Calculates the measurement noise covariance matrix 𝐑

Parameters
[in]measKeysMeasurement Keys
[in]observationsList of GNSS observation data used for the calculation
[in]nameIdName and id of the node calling this (only used for logging purposes)
Returns
The 𝐑 matrix

Definition at line 529 of file Algorithm.cpp.

◆ calcMeasInnovation()

KeyedVectorXd< Meas::MeasKeyTypes > NAV::SPP::Algorithm::calcMeasInnovation ( const std::vector< Meas::MeasKeyTypes > & measKeys,
const Observations & observations,
const std::string & nameId )
staticnodiscardprivate

Calculates the measurement innovation vector 𝜹𝐳

Parameters
[in]measKeysMeasurement Keys
[in]observationsList of GNSS observation data used for the calculation
[in]nameIdName and id of the node calling this (only used for logging purposes)
Returns
The measurement innovation 𝜹𝐳 vector

Definition at line 562 of file Algorithm.cpp.

◆ calcSppSolution()

std::shared_ptr< SppSolution > NAV::SPP::Algorithm::calcSppSolution ( const std::shared_ptr< const GnssObs > & gnssObs,
const std::vector< const GnssNavInfo * > & gnssNavInfos,
const std::string & nameId )

Calculate the SPP solution.

Parameters
[in]gnssObsGNSS observation
[in]gnssNavInfosCollection of GNSS Nav information
[in]nameIdName and id of the node calling this (only used for logging purposes)
Returns
The SPP Solution if it could be calculated, otherwise nullptr

Definition at line 73 of file Algorithm.cpp.

◆ canCalculateVelocity()

bool NAV::SPP::Algorithm::canCalculateVelocity ( size_t nDoppMeas) const
nodiscardprivate

Checks if the SPP algorithm can calculate the position (always true for Kalman filter)

Parameters
[in]nDoppMeasAmount of Doppler measurements

Definition at line 378 of file Algorithm.cpp.

◆ canEstimateInterFrequencyBias()

bool NAV::SPP::Algorithm::canEstimateInterFrequencyBias ( ) const
nodiscardprivate

Checks if the SPP algorithm can estimate inter-frequency biases.

Definition at line 385 of file Algorithm.cpp.

◆ computeDOPs()

void NAV::SPP::Algorithm::computeDOPs ( const std::shared_ptr< SppSolution > & sppSol,
const KeyedMatrixXd< Meas::MeasKeyTypes, States::StateKeyType > & H,
const std::string & nameId )
private

Computes all DOP values (by reference)

Parameters
[in,out]sppSolSppSol to fill with DOP values
[in]HMeasurement sensitivity matrix 𝐇
[in]nameIdName and id of the node calling this (only used for logging purposes)

Definition at line 769 of file Algorithm.cpp.

◆ determineMeasKeys()

std::vector< Meas::MeasKeyTypes > NAV::SPP::Algorithm::determineMeasKeys ( const Observations & observations,
size_t nPsrMeas,
size_t nDoppMeas,
const std::string & nameId ) const
private

Returns a list of measurement keys.

Parameters
[in]observationsList of GNSS observation data used for the calculation
[in]nPsrMeasAmount of pseudo-range measurements
[in]nDoppMeasAmount of doppler measurements
[in]nameIdName and id of the node calling this (only used for logging purposes)

Definition at line 455 of file Algorithm.cpp.

◆ determineStateKeys()

std::vector< States::StateKeyType > NAV::SPP::Algorithm::determineStateKeys ( const std::set< SatelliteSystem > & usedSatSystems,
size_t nDoppMeas,
const std::string & nameId ) const
private

Returns a list of state keys.

Parameters
[in]usedSatSystemsUsed Satellite systems this epoch
[in]nDoppMeasAmount of Doppler measurements
[in]nameIdName and id of the node calling this (only used for logging purposes)

Definition at line 421 of file Algorithm.cpp.

◆ getEstimatorType()

EstimatorType NAV::SPP::Algorithm::getEstimatorType ( ) const
inlinenodiscard

Get the Estimator Type.

Definition at line 66 of file Algorithm.hpp.

◆ getLastUpdateTime()

const InsTime & NAV::SPP::Algorithm::getLastUpdateTime ( ) const
inline

Get the last update time.

Definition at line 69 of file Algorithm.hpp.

◆ reset()

void NAV::SPP::Algorithm::reset ( )

Reset the algorithm.

Definition at line 65 of file Algorithm.cpp.

◆ ShowGuiWidgets()

bool NAV::SPP::Algorithm::ShowGuiWidgets ( const char * id,
float itemWidth,
float unitWidth )

Shows the GUI input to select the options.

Parameters
[in]idUnique id for ImGui.
[in]itemWidthWidth of the widgets
[in]unitWidthWidth on unit inputs

Definition at line 40 of file Algorithm.cpp.

◆ updateInterFrequencyBiases()

void NAV::SPP::Algorithm::updateInterFrequencyBiases ( const Observations & observations,
const std::string & nameId )
private

Updates the inter frequency biases.

Parameters
[in]observationsList of GNSS observation data used for the calculation
[in]nameIdName and id of the node calling this (only used for logging purposes)

Definition at line 396 of file Algorithm.cpp.

Friends And Related Symbol Documentation

◆ from_json

void from_json ( const json & j,
Algorithm & obj )
friend

Converts the provided json object into a node object.

Parameters
[in]jJson object with the needed values
[out]objObject to fill from the json

Definition at line 806 of file Algorithm.cpp.

◆ to_json

void to_json ( json & j,
const Algorithm & obj )
friend

Converts the provided object into json.

Parameters
[out]jJson object which gets filled with the info
[in]objObject to convert into json

Definition at line 793 of file Algorithm.cpp.

Field Documentation

◆ _e_lastPositionCovarianceMatrix

Eigen::Matrix3d NAV::SPP::Algorithm::_e_lastPositionCovarianceMatrix
private

Last position covariance matrix.

Definition at line 196 of file Algorithm.hpp.

◆ _e_lastVelocityCovarianceMatrix

Eigen::Matrix3d NAV::SPP::Algorithm::_e_lastVelocityCovarianceMatrix
private

Last velocity covariance matrix.

Definition at line 197 of file Algorithm.hpp.

◆ _estimateInterFreqBiases

bool NAV::SPP::Algorithm::_estimateInterFreqBiases

Estimate Inter-frequency biases.

Definition at line 89 of file Algorithm.hpp.

◆ _estimatorType

EstimatorType NAV::SPP::Algorithm::_estimatorType
private

Estimator type used for the calculations.

Definition at line 188 of file Algorithm.hpp.

◆ _kalmanFilter

SPP::KalmanFilter NAV::SPP::Algorithm::_kalmanFilter
private

SPP specific Kalman filter.

Definition at line 191 of file Algorithm.hpp.

◆ _lastUpdate

InsTime NAV::SPP::Algorithm::_lastUpdate
private

Time of last update.

Definition at line 194 of file Algorithm.hpp.

◆ _obsEstimator

ObservationEstimator NAV::SPP::Algorithm::_obsEstimator

Observation Estimator.

Definition at line 86 of file Algorithm.hpp.

◆ _obsFilter

ObservationFilter NAV::SPP::Algorithm::_obsFilter

Observation Filter.

Definition at line 81 of file Algorithm.hpp.

◆ _receiver

Receiver NAV::SPP::Algorithm::_receiver
private

Receiver.

Definition at line 185 of file Algorithm.hpp.

◆ PosKey

const std::array<SPP::States::StateKeyType, 3>& NAV::SPP::Algorithm::PosKey
private

All position keys.

Definition at line 95 of file Algorithm.hpp.

◆ PosVelKey

const std::array<SPP::States::StateKeyType, 6>& NAV::SPP::Algorithm::PosVelKey
private

All position and velocity keys.

Definition at line 99 of file Algorithm.hpp.

◆ VelKey

const std::array<SPP::States::StateKeyType, 3>& NAV::SPP::Algorithm::VelKey
private

All velocity keys.

Definition at line 97 of file Algorithm.hpp.


The documentation for this class was generated from the following files: