Loosely-coupled Kalman Filter for INS/GNSS integration. More...

Public Types
enum	KFMeas : uint8_t { dPosLat , dPosLon , dPosAlt , dVelN , dVelE , dVelD , dHgt , dPosX , dPosY , dPosZ , dVelX , dVelY , dVelZ }
	Measurement Keys of the Kalman filter. More...
enum	KFStates : uint8_t { Roll , Pitch , Yaw , VelN , VelE , VelD , PosLat , PosLon , PosAlt , AccBiasX , AccBiasY , AccBiasZ , GyrBiasX , GyrBiasY , GyrBiasZ , HeightBias , HeightScale , KFStates_COUNT , Psi_eb_1 , Psi_eb_2 , Psi_eb_3 , VelX , VelY , VelZ , PosX , PosY , PosZ }
	State Keys of the Kalman filter. More...
Public Types inherited from NAV::Node
enum class	Mode : uint8_t { REAL_TIME , POST_PROCESSING }
	Different Modes the Node can work in. More...
enum class	State : uint8_t { Disabled , Deinitialized , DoInitialize , Initializing , Initialized , DoDeinitialize , Deinitializing , DoShutdown , Shutdown }
	Possible states of the node. More...

Public Member Functions
void	guiConfig () override
	ImGui config window which is shown on double click.
	LooselyCoupledKF ()
	Default constructor.
	LooselyCoupledKF (const LooselyCoupledKF &)=delete
	Copy constructor.
	LooselyCoupledKF (LooselyCoupledKF &&)=delete
	Move constructor.
LooselyCoupledKF &	operator= (const LooselyCoupledKF &)=delete
	Copy assignment operator.
LooselyCoupledKF &	operator= (LooselyCoupledKF &&)=delete
	Move assignment operator.
void	restore (const json &j) override
	Restores the node from a json object.
json	save () const override
	Saves the node into a json object.
std::string	type () const override
	String representation of the class type.
	~LooselyCoupledKF () override
	Destructor.
Public Member Functions inherited from NAV::Node
virtual void	afterCreateLink (OutputPin &startPin, InputPin &endPin)
	Called when a new link was established.
virtual void	afterDeleteLink (OutputPin &startPin, InputPin &endPin)
	Called when a link was deleted.
bool	doDeinitialize (bool wait=false)
	Asks the node worker to deinitialize the node.
bool	doDisable (bool wait=false)
	Asks the node worker to disable the node.
bool	doEnable ()
	Enable the node.
bool	doInitialize (bool wait=false)
	Asks the node worker to initialize the node.
bool	doReinitialize (bool wait=false)
	Asks the node worker to reinitialize the node.
virtual void	flush ()
	Function called by the flow executer after finishing to flush out remaining data.
template<typename T>
std::optional< InputPin::IncomingLink::ValueWrapper< T > >	getInputValue (size_t portIndex) const
	Get Input Value connected on the pin. Only const data types.
Mode	getMode () const
	Get the current mode of the node.
const ImVec2 &	getSize () const
	Get the size of the node.
State	getState () const
	Get the current state of the node.
bool	hasInputPinWithSameTime (const InsTime &insTime) const
	Checks wether there is an input pin with the same time.
InputPin &	inputPinFromId (ax::NodeEditor::PinId pinId)
	Returns the pin with the given id.
size_t	inputPinIndexFromId (ax::NodeEditor::PinId pinId) const
	Returns the index of the pin.
void	invokeCallbacks (size_t portIndex, const std::shared_ptr< const NodeData > &data)
	Calls all registered callbacks on the specified output port.
bool	isDisabled () const
	Checks if the node is disabled.
bool	isInitialized () const
	Checks if the node is initialized.
bool	isOnlyRealtime () const
	Checks if the node is only working in real time (sensors, network interfaces, ...)
bool	isTransient () const
	Checks if the node is changing its state currently.
std::string	nameId () const
	Node name and id.
	Node (const Node &)=delete
	Copy constructor.
	Node (Node &&)=delete
	Move constructor.
	Node (std::string name)
	Constructor.
void	notifyOutputValueChanged (size_t pinIdx, const InsTime &insTime, const std::scoped_lock< std::mutex > &&guard)
	Notifies connected nodes about the change.
virtual bool	onCreateLink (OutputPin &startPin, InputPin &endPin)
	Called when a new link is to be established.
virtual void	onDeleteLink (OutputPin &startPin, InputPin &endPin)
	Called when a link is to be deleted.
Node &	operator= (const Node &)=delete
	Copy assignment operator.
Node &	operator= (Node &&)=delete
	Move assignment operator.
OutputPin &	outputPinFromId (ax::NodeEditor::PinId pinId)
	Returns the pin with the given id.
size_t	outputPinIndexFromId (ax::NodeEditor::PinId pinId) const
	Returns the index of the pin.
void	releaseInputValue (size_t portIndex)
	Unblocks the connected node. Has to be called when the input value should be released and getInputValue was not called.
std::scoped_lock< std::mutex >	requestOutputValueLock (size_t pinIdx)
	Blocks the thread till the output values was read by all connected nodes.
virtual bool	resetNode ()
	Resets the node. It is guaranteed that the node is initialized when this is called.
virtual void	restoreAtferLink (const json &j)
	Restores link related properties of the node from a json object.
void	wakeWorker ()
	Wakes the worker thread.
virtual	~Node ()
	Destructor.

Static Public Member Functions
static std::string	category ()
	String representation of the class category.
static std::string	typeStatic ()
	String representation of the class type.
Static Public Member Functions inherited from NAV::Node
static std::string	toString (State state)
	Converts the state into a printable text.

Private Types
enum class	BaroHeightMeasurementUncertaintyUnit : uint8_t { m2 , m }
	Possible Units for the barometric height measurement uncertainty (standard deviation σ or Variance σ²) More...
enum class	GnssMeasurementUncertaintyPositionUnit : uint8_t { meter2 , meter }
	Possible Units for the GNSS measurement uncertainty for the position (standard deviation σ or Variance σ²) More...
enum class	GnssMeasurementUncertaintyVelocityUnit : uint8_t { m2_s2 , m_s }
	Possible Units for the GNSS measurement uncertainty for the velocity (standard deviation σ or Variance σ²) More...
enum class	InitBiasAccelUnit : uint8_t { microg , m_s2 }
	Possible Units for the initial accelerometer biases. More...
enum class	InitBiasGyroUnit : uint8_t { rad_s , deg_s }
	Possible Units for the initial gyroscope biases. More...
enum class	InitCovarianceAttitudeAnglesUnit : uint8_t { rad2 , deg2 , rad , deg }
	Possible Units for the initial covariance for the attitude angles (standard deviation σ or Variance σ²) More...
enum class	InitCovarianceBiasAccelUnit : uint8_t { m2_s4 , m_s2 }
	Possible Units for the initial covariance for the accelerometer biases (standard deviation σ or Variance σ²) More...
enum class	InitCovarianceBiasGyroUnit : uint8_t { rad2_s2 , deg2_s2 , rad_s , deg_s }
	Possible Units for the initial covariance for the gyroscope biases (standard deviation σ or Variance σ²) More...
enum class	InitCovarianceBiasHeightUnit : uint8_t { m2 , m }
	Possible Units for the initial covariance for the height bias (standard deviation σ or Variance σ²) More...
enum class	InitCovariancePositionUnit : uint8_t { rad2_rad2_m2 , rad_rad_m , meter2 , meter }
	Possible Units for the initial covariance for the position (standard deviation σ or Variance σ²) More...
enum class	InitCovarianceScaleHeight : uint8_t { m2_m2 , m_m }
	Possible Units for the initial covariance for the height scale (standard deviation σ or Variance σ²) More...
enum class	InitCovarianceVelocityUnit : uint8_t { m2_s2 , m_s }
	Possible Units for the initial covariance for the velocity (standard deviation σ or Variance σ²) More...
enum class	PhiCalculationAlgorithm : uint8_t { Exponential , Taylor }
	GUI option for the Phi calculation algorithm. More...
enum class	QCalculationAlgorithm : uint8_t { VanLoan , Taylor1 }
	GUI option for the Q calculation algorithm. More...
enum class	RandomProcess : uint8_t { RandomWalk , GaussMarkov1 }
	Available Random processes. More...
enum class	StdevBaroHeightBiasUnits : uint8_t { m }
	Possible Units for the Variance of the barometric height bias. More...
enum class	StdevBaroHeightScaleUnits : uint8_t { m_m }
	Possible Units for the Variance of the barometric height scale. More...

Private Member Functions
void	deinitialize () override
	Deinitialize the node.
KeyedMatrix< double, KFMeas, KFMeas, 6, 6 >	e_measurementNoiseCovariance_R (const std::shared_ptr< const PosVel > &posVelObs) const
	Measurement noise covariance matrix 𝐑
KeyedMatrix< double, KFStates, KFStates, 17, 17 >	e_systemMatrix_F (const Eigen::Quaterniond &e_Quat_b, const Eigen::Vector3d &b_specForce_ib, const Eigen::Vector3d &e_position, const Eigen::Vector3d &e_gravitation, double r_eS_e, const Eigen::Vector3d &e_omega_ie, const Eigen::Vector3d &tau_bad, const Eigen::Vector3d &tau_bgd) const
	Calculates the system matrix 𝐅 for the ECEF frame.
bool	hasInputPinWithSameTime (const InsTime &insTime) const
	Checks wether there is an input pin with the same time.
KeyedMatrix< double, KFStates, KFStates, 17, 17 >	initialErrorCovarianceMatrix_P0 (const Eigen::Vector3d &variance_angles, const Eigen::Vector3d &variance_vel, const Eigen::Vector3d &variance_pos, const Eigen::Vector3d &variance_accelBias, const Eigen::Vector3d &variance_gyroBias, const double &variance_heightBias, const double &variance_heightScale) const
	Initial error covariance matrix P_0.
bool	initialize () override
	Initialize the node.
void	invokeCallbackWithPosVelAtt (const PosVelAtt &posVelAtt)
	Invoke the callback with a PosVelAtt solution (without LCKF specific output)
void	looselyCoupledPrediction (const std::shared_ptr< const PosVelAtt > &inertialNavSol, double tau_i, const ImuPos &imuPos)
	Predicts the state from the InertialNavSol.
void	looselyCoupledUpdate (const std::shared_ptr< const BaroHgt > &baroHgtObs)
	Updates the predicted state from the InertialNavSol with the Baro observation.
void	looselyCoupledUpdate (const std::shared_ptr< const PosVel > &posVelObs)
	Updates the predicted state from the InertialNavSol with the PosVel observation.
KeyedMatrix< double, KFMeas, KFMeas, 6, 6 >	n_measurementNoiseCovariance_R (const std::shared_ptr< const PosVel > &posVelObs, const Eigen::Vector3d &lla_position, double R_N, double R_E) const
	Measurement noise covariance matrix 𝐑
KeyedMatrix< double, KFStates, KFStates, 17, 17 >	n_systemMatrix_F (const Eigen::Quaterniond &n_Quat_b, const Eigen::Vector3d &b_specForce_ib, const Eigen::Vector3d &n_omega_in, const Eigen::Vector3d &n_velocity, const Eigen::Vector3d &lla_position, double R_N, double R_E, double g_0, double r_eS_e, const Eigen::Vector3d &tau_bad, const Eigen::Vector3d &tau_bgd) const
	Calculates the system matrix 𝐅 for the local navigation frame.
Eigen::Matrix< double, 17, 17 >	noiseScaleMatrix_W (const Eigen::Vector3d &sigma_ra, const Eigen::Vector3d &sigma_rg, const Eigen::Vector3d &sigma_bad, const Eigen::Vector3d &sigma_bgd, const Eigen::Vector3d &tau_bad, const Eigen::Vector3d &tau_bgd, const double &sigma_heightBias, const double &sigma_heightScale)
	Calculates the noise scale matrix 𝐖
void	recvBaroHeight (InputPin::NodeDataQueue &queue, size_t pinIdx)
	Receive Function for the BaroHgt observation.
void	recvImuObservation (InputPin::NodeDataQueue &queue, size_t pinIdx)
	Receive Function for the IMU observation.
void	recvPosVelAttInit (InputPin::NodeDataQueue &queue, size_t pinIdx)
	Receive Function for the PosVelAtt observation.
void	recvPosVelObservation (InputPin::NodeDataQueue &queue, size_t pinIdx)
	Receive Function for the PosVel observation.
void	setSolutionPosVelAttAndCov (const std::shared_ptr< PosVelAtt > &lckfSolution, double R_N, double R_E)
	Sets the covariance matrix P of the LCKF (and does the necessary unit conversions)
void	updateInputPins ()
	Update the input pins depending on the GUI.

Static Private Member Functions
static KeyedVector< double, KFMeas, 6 >	e_measurementInnovation_dz (const Eigen::Vector3d &e_positionMeasurement, const Eigen::Vector3d &e_positionEstimate, const Eigen::Vector3d &e_velocityMeasurement, const Eigen::Vector3d &e_velocityEstimate, const Eigen::Quaterniond &e_Quat_b, const Eigen::Vector3d &b_leverArm_InsGnss, const Eigen::Vector3d &b_omega_ib, const Eigen::Matrix3d &e_Omega_ie)
	Measurement innovation vector 𝜹𝐳
static KeyedMatrix< double, KFMeas, KFStates, 6, 17 >	e_measurementMatrix_H (const Eigen::Matrix3d &e_Dcm_b, const Eigen::Vector3d &b_omega_ib, const Eigen::Vector3d &b_leverArm_InsGnss, const Eigen::Matrix3d &e_Omega_ie)
	Measurement matrix for GNSS measurements at timestep k, represented in Earth frame coordinates.
static KeyedMatrix< double, KFMeas, KFStates, 1, 17 >	e_measurementMatrix_H (const Eigen::Vector3d &e_positionEstimate, const double &height, const double &scale)
	Measurement matrix for barometric height measurements at timestep k, represented in Earth frame coordinates.
static KeyedMatrix< double, KFStates, KFStates, 17, 17 >	e_systemNoiseCovarianceMatrix_Q (const Eigen::Vector3d &sigma2_ra, const Eigen::Vector3d &sigma2_rg, const Eigen::Vector3d &sigma2_bad, const Eigen::Vector3d &sigma2_bgd, const double &sigma_heightBias, const double &sigma_heightScale, const Eigen::Vector3d &tau_bad, const Eigen::Vector3d &tau_bgd, const Eigen::Matrix3d &e_F_21, const Eigen::Matrix3d &e_Dcm_b, const double &tau_s)
	System noise covariance matrix 𝐐_{k-1}.
static KeyedVector< double, KFMeas, 1 >	n_measurementInnovation_dz (const double &baroheight, const double &height, const double &heightbias, const double &heightscale)
	Measurement innovation vector 𝜹𝐳
static KeyedVector< double, KFMeas, 6 >	n_measurementInnovation_dz (const Eigen::Vector3d &lla_positionMeasurement, const Eigen::Vector3d &lla_positionEstimate, const Eigen::Vector3d &n_velocityMeasurement, const Eigen::Vector3d &n_velocityEstimate, const Eigen::Matrix3d &T_rn_p, const Eigen::Quaterniond &n_Quat_b, const Eigen::Vector3d &b_leverArm_InsGnss, const Eigen::Vector3d &b_omega_ib, const Eigen::Matrix3d &n_Omega_ie)
	Measurement innovation vector 𝜹𝐳
static KeyedMatrix< double, KFMeas, KFStates, 1, 17 >	n_measurementMatrix_H (const double &height, const double &scale)
	Measurement matrix for baro height measurements at timestep k, represented in navigation coordinates.
static KeyedMatrix< double, KFMeas, KFStates, 6, 17 >	n_measurementMatrix_H (const Eigen::Matrix3d &T_rn_p, const Eigen::Matrix3d &n_Dcm_b, const Eigen::Vector3d &b_omega_ib, const Eigen::Vector3d &b_leverArm_InsGnss, const Eigen::Matrix3d &n_Omega_ie)
	Measurement matrix for GNSS measurements at timestep k, represented in navigation coordinates.
static KeyedMatrix< double, KFMeas, KFMeas, 1, 1 >	n_measurementNoiseCovariance_R (const double &baroVarianceHeight)
	Measurement noise covariance matrix 𝐑
static KeyedMatrix< double, KFStates, KFStates, 17, 17 >	n_systemNoiseCovarianceMatrix_Q (const Eigen::Vector3d &sigma2_ra, const Eigen::Vector3d &sigma2_rg, const Eigen::Vector3d &sigma2_bad, const Eigen::Vector3d &sigma2_bgd, const double &sigma_heightBias, const double &sigma_heightScale, const Eigen::Vector3d &tau_bad, const Eigen::Vector3d &tau_bgd, const Eigen::Matrix3d &n_F_21, const Eigen::Matrix3d &T_rn_p, const Eigen::Matrix3d &n_Dcm_b, const double &tau_s)
	System noise covariance matrix 𝐐_{k-1}.
static KeyedMatrix< double, KFStates, KFStates, 17, 17 >	noiseInputMatrix_G (const Eigen::Quaterniond &ien_Quat_b)
	Calculates the noise input matrix 𝐆
static void	pinAddCallback (Node *node)
	Function to call to add a new pin.
static void	pinDeleteCallback (Node *node, size_t pinIdx)
	Function to call to delete a pin.

Private Attributes
Eigen::Vector3d	_accelBiasTotal
	Accumulator for acceleration bias [m/s²].
bool	_baroHeightMeasurementUncertaintyOverride
	Whether to override the barometric height uncertainty or use the one included in the measurement.
double	_barometricHeightMeasurementUncertainty
	GUI selection of the barometric height measurement uncertainty (standard deviation σ or Variance σ²)
BaroHeightMeasurementUncertaintyUnit	_barometricHeightMeasurementUncertaintyUnit
	Gui selection for the Unit of the barometric height measurement uncertainty.
bool	_checkKalmanMatricesRanks
	Check the rank of the Kalman matrices every iteration (computational expensive)
gui::widgets::DynamicInputPins	_dynamicInputPins
	Dynamic input pins.
bool	_enableBaroHgt
	Whether to enable barometric height (including the corresponding pin)
InsTime	_externalInitTime
	Time from the external init.
Eigen::Vector3d	_gnssMeasurementUncertaintyPosition
	GUI selection of the GNSS position measurement uncertainty (standard deviation σ or Variance σ²). SPP accuracy approx. 3m in horizontal direction and 3 times worse in vertical direction.
bool	_gnssMeasurementUncertaintyPositionOverride
	Whether to override the position uncertainty or use the one included in the measurement.
GnssMeasurementUncertaintyPositionUnit	_gnssMeasurementUncertaintyPositionUnit
	Gui selection for the Unit of the GNSS measurement uncertainty for the position.
Eigen::Vector3d	_gnssMeasurementUncertaintyVelocity
	GUI selection of the GNSS NED velocity measurement uncertainty (standard deviation σ or Variance σ²)
bool	_gnssMeasurementUncertaintyVelocityOverride
	Whether to override the velocity uncertainty or use the one included in the measurement.
GnssMeasurementUncertaintyVelocityUnit	_gnssMeasurementUncertaintyVelocityUnit
	Gui selection for the Unit of the GNSS measurement uncertainty for the velocity.
Eigen::Vector3d	_gyroBiasTotal
	Accumulator gyro bias [rad/s].
double	_heightBiasTotal
	Accumulator for height bias [m].
double	_heightScaleTotal
	Accumulator for height scale [m/m].
InertialIntegrator	_inertialIntegrator
	Inertial Integrator.
std::array< double, 3 >	_initalRollPitchYaw
	Roll, Pitch and Yaw angles in [deg] used for initialization if not taken from separate pin.
Eigen::Vector3d	_initBiasAccel
	GUI selection of the initial accelerometer biases.
InitBiasAccelUnit	_initBiasAccelUnit
	Gui selection for the unit of the initial accelerometer biases.
Eigen::Vector3d	_initBiasGyro
	GUI selection of the initial gyroscope biases.
InitBiasGyroUnit	_initBiasGyroUnit
	Gui selection for the unit of the initial gyroscope biases.
Eigen::Vector3d	_initCovarianceAttitudeAngles
	GUI selection of the initial covariance diagonal values for attitude angles (standard deviation σ or Variance σ²)
InitCovarianceAttitudeAnglesUnit	_initCovarianceAttitudeAnglesUnit
	Gui selection for the Unit of the initial covariance for the attitude angles.
Eigen::Vector3d	_initCovarianceBiasAccel
	GUI selection of the initial covariance diagonal values for accelerometer biases (standard deviation σ or Variance σ²)
InitCovarianceBiasAccelUnit	_initCovarianceBiasAccelUnit
	Gui selection for the Unit of the initial covariance for the accelerometer biases.
Eigen::Vector3d	_initCovarianceBiasGyro
	GUI selection of the initial covariance diagonal values for gyroscope biases (standard deviation σ or Variance σ²)
InitCovarianceBiasGyroUnit	_initCovarianceBiasGyroUnit
	Gui selection for the Unit of the initial covariance for the gyroscope biases.
double	_initCovarianceBiasHeight
	GUI selection of the initial covariance diagonal values for the height bias (standard deviation σ or Variance σ²)
InitCovarianceBiasHeightUnit	_initCovarianceBiasHeightUnit
	Gui selection for the Unit of the initial covariance for the height bias.
Eigen::Vector3d	_initCovariancePosition
	GUI selection of the initial covariance diagonal values for position (standard deviation σ or Variance σ²)
InitCovariancePositionUnit	_initCovariancePositionUnit
	Gui selection for the Unit of the initial covariance for the position.
double	_initCovarianceScaleHeight
	GUI selection of the initial covariance diagonal values for the height scale (standard deviation σ or Variance σ²)
InitCovarianceScaleHeight	_initCovarianceScaleHeightUnit
	Gui selection for the Unit of the initial covariance for the height scale.
Eigen::Vector3d	_initCovarianceVelocity
	GUI selection of the initial covariance diagonal values for velocity (standard deviation σ or Variance σ²)
InitCovarianceVelocityUnit	_initCovarianceVelocityUnit
	Gui selection for the Unit of the initial covariance for the velocity.
bool	_initializeStateOverExternalPin
	Whether to initialize the state over an external pin.
bool	_initialSensorBiasesApplied
	Whether the accumulated biases have been initialized in the 'inertialIntegrator'.
KeyedKalmanFilterD< KFStates, KFMeas >	_kalmanFilter
	Kalman Filter representation.
std::shared_ptr< const ImuObs >	_lastImuObs
	Last received IMU observation (to get ImuPos)
PhiCalculationAlgorithm	_phiCalculationAlgorithm
	GUI option for the Phi calculation algorithm.
int	_phiCalculationTaylorOrder
	GUI option for the order of the Taylor polynom to calculate the Phi matrix.
bool	_preferAccelerationOverDeltaMeasurements
	Prefer the raw acceleration measurements over the deltaVel & deltaTheta values.
QCalculationAlgorithm	_qCalculationAlgorithm
	GUI option for the Q calculation algorithm.
RandomProcess	_randomProcessAccel
	Random Process used to estimate the accelerometer biases.
RandomProcess	_randomProcessGyro
	Random Process used to estimate the gyroscope biases.
Eigen::Vector3d	_stdev_bad
	𝜎²_bad Variance of the accelerometer dynamic bias
Eigen::Vector3d	_stdev_bgd
	𝜎²_bgd Variance of the gyro dynamic bias
Eigen::Vector3d	_stdev_ra
	𝜎_ra Standard deviation of the noise on the accelerometer specific-force measurements
Eigen::Vector3d	_stdev_rg
	𝜎_rg Standard deviation of the noise on the gyro angular-rate measurements
Units::ImuAccelerometerFilterBiasUnits	_stdevAccelBiasUnits
	Gui selection for the Unit of the input variance_bad parameter.
Units::ImuAccelerometerFilterNoiseUnits	_stdevAccelNoiseUnits
	Gui selection for the Unit of the input stdev_ra parameter.
double	_stdevBaroHeightBias
	Uncertainty of the barometric height bias.
StdevBaroHeightBiasUnits	_stdevBaroHeightBiasUnits
	Gui selection for the Unit of the barometric height bias uncertainty.
double	_stdevBaroHeightScale
	Uncertainty of the barometric height scale.
StdevBaroHeightScaleUnits	_stdevBaroHeightScaleUnits
	Gui selection for the Unit of the barometric height scale uncertainty.
Units::ImuGyroscopeFilterBiasUnits	_stdevGyroBiasUnits
	Gui selection for the Unit of the input variance_bad parameter.
Units::ImuGyroscopeFilterNoiseUnits	_stdevGyroNoiseUnits
	Gui selection for the Unit of the input stdev_rg parameter.
Eigen::Vector3d	_tau_bad
	Correlation length of the accelerometer dynamic bias in [s].
Eigen::Vector3d	_tau_bgd
	Correlation length of the gyro dynamic bias in [s].

Static Private Attributes
static const std::vector< KFMeas >	dPos
	All position difference keys.
static const std::vector< KFMeas >	dVel
	All velocity difference keys.
static constexpr size_t	INPUT_PORT_INDEX_IMU
	Flow (ImuObs)
static constexpr size_t	INPUT_PORT_INDEX_POS_VEL_ATT_INIT
	Flow (PosVelAtt)
static const std::vector< KFStates >	KFAccBias
	All acceleration bias keys.
static const std::vector< KFStates >	KFAtt
	All attitude keys.
static const std::vector< KFStates >	KFGyrBias
	All gyroscope bias keys.
static const std::vector< KFStates >	KFPos
	All position keys.
static const std::vector< KFStates >	KFPosVel
	All position and velocity keys.
static const std::vector< KFStates >	KFPosVelAtt
	All position, velocity and attitude keys.
static const std::vector< KFStates >	KFVel
	All velocity keys.
static const std::vector< KFMeas >	Meas
	Vector with all measurement keys.
static constexpr size_t	OUTPUT_PORT_INDEX_SOLUTION
	Flow (InsGnssLCKFSolution)
static const std::vector< KFStates >	States
	Vector with all state keys.

Additional Inherited Members
Data Fields inherited from NAV::Node
bool	callbacksEnabled
	Enables the callbacks.
ax::NodeEditor::NodeId	id
	Unique Id of the Node.
std::vector< InputPin >	inputPins
	List of input pins.
Kind	kind
	Kind of the Node.
std::string	name
	Name of the Node.
std::vector< OutputPin >	outputPins
	List of output pins.
std::multimap< InsTime, std::pair< OutputPin *, size_t > >	pollEvents
	Map with callback events (sorted by time)
Protected Attributes inherited from NAV::Node
ImVec2	_guiConfigDefaultWindowSize
bool	_hasConfig
	Flag if the config window should be shown.
bool	_lockConfigDuringRun
	Lock the config when executing post-processing.
bool	_onlyRealTime
	Whether the node can run in post-processing or only real-time.

Detailed Description

Loosely-coupled Kalman Filter for INS/GNSS integration.

Definition at line 34 of file LooselyCoupledKF.hpp.

Member Enumeration Documentation

◆ BaroHeightMeasurementUncertaintyUnit

enum class NAV::LooselyCoupledKF::BaroHeightMeasurementUncertaintyUnit : uint8_t

strongprivate

Possible Units for the barometric height measurement uncertainty (standard deviation σ or Variance σ²)

Enumerator
m2	Variance [m²].
m	Standard deviation [m].

Definition at line 383 of file LooselyCoupledKF.hpp.

◆ GnssMeasurementUncertaintyPositionUnit

enum class NAV::LooselyCoupledKF::GnssMeasurementUncertaintyPositionUnit : uint8_t

strongprivate

Possible Units for the GNSS measurement uncertainty for the position (standard deviation σ or Variance σ²)

Enumerator
meter2	Variance [m^2, m^2, m^2].
meter	Standard deviation [m, m, m].

Definition at line 348 of file LooselyCoupledKF.hpp.

◆ GnssMeasurementUncertaintyVelocityUnit

enum class NAV::LooselyCoupledKF::GnssMeasurementUncertaintyVelocityUnit : uint8_t

strongprivate

Possible Units for the GNSS measurement uncertainty for the velocity (standard deviation σ or Variance σ²)

Enumerator
m2_s2	Variance [m^2/s^2].
m_s	Standard deviation [m/s].

Definition at line 366 of file LooselyCoupledKF.hpp.

◆ InitBiasAccelUnit

enum class NAV::LooselyCoupledKF::InitBiasAccelUnit : uint8_t

strongprivate

Possible Units for the initial accelerometer biases.

Enumerator
microg	[µg]
m_s2	acceleration [m/s^2]

Definition at line 504 of file LooselyCoupledKF.hpp.

◆ InitBiasGyroUnit

enum class NAV::LooselyCoupledKF::InitBiasGyroUnit : uint8_t

strongprivate

Possible Units for the initial gyroscope biases.

Enumerator
rad_s	angular rate [rad/s]
deg_s	angular rate [deg/s]

Definition at line 518 of file LooselyCoupledKF.hpp.

◆ InitCovarianceAttitudeAnglesUnit

enum class NAV::LooselyCoupledKF::InitCovarianceAttitudeAnglesUnit : uint8_t

strongprivate

Possible Units for the initial covariance for the attitude angles (standard deviation σ or Variance σ²)

Enumerator
rad2	Variance [rad^2].
deg2	Variance [deg^2].
rad	Standard deviation [rad].
deg	Standard deviation [deg].

Definition at line 430 of file LooselyCoupledKF.hpp.

◆ InitCovarianceBiasAccelUnit

enum class NAV::LooselyCoupledKF::InitCovarianceBiasAccelUnit : uint8_t

strongprivate

Possible Units for the initial covariance for the accelerometer biases (standard deviation σ or Variance σ²)

Enumerator
m2_s4	Variance [m^2/s^4].
m_s2	Standard deviation [m/s^2].

Definition at line 446 of file LooselyCoupledKF.hpp.

◆ InitCovarianceBiasGyroUnit

enum class NAV::LooselyCoupledKF::InitCovarianceBiasGyroUnit : uint8_t

strongprivate

Possible Units for the initial covariance for the gyroscope biases (standard deviation σ or Variance σ²)

Enumerator
rad2_s2	Variance [rad²/s²].
deg2_s2	Variance [deg²/s²].
rad_s	Standard deviation [rad/s].
deg_s	Standard deviation [deg/s].

Definition at line 460 of file LooselyCoupledKF.hpp.

◆ InitCovarianceBiasHeightUnit

enum class NAV::LooselyCoupledKF::InitCovarianceBiasHeightUnit : uint8_t

strongprivate

Possible Units for the initial covariance for the height bias (standard deviation σ or Variance σ²)

Enumerator
m2	Variance [m²].
m	Standard deviation [m].

Definition at line 476 of file LooselyCoupledKF.hpp.

◆ InitCovariancePositionUnit

enum class NAV::LooselyCoupledKF::InitCovariancePositionUnit : uint8_t

strongprivate

Possible Units for the initial covariance for the position (standard deviation σ or Variance σ²)

Enumerator
rad2_rad2_m2	Variance LatLonAlt^2 [rad^2, rad^2, m^2].
rad_rad_m	Standard deviation LatLonAlt [rad, rad, m].
meter2	Variance NED [m^2, m^2, m^2].
meter	Standard deviation NED [m, m, m].

Definition at line 400 of file LooselyCoupledKF.hpp.

◆ InitCovarianceScaleHeight

enum class NAV::LooselyCoupledKF::InitCovarianceScaleHeight : uint8_t

strongprivate

Possible Units for the initial covariance for the height scale (standard deviation σ or Variance σ²)

Enumerator
m2_m2	Variance [m²/m²].
m_m	Standard devation [m/m].

Definition at line 490 of file LooselyCoupledKF.hpp.

◆ InitCovarianceVelocityUnit

enum class NAV::LooselyCoupledKF::InitCovarianceVelocityUnit : uint8_t

strongprivate

Possible Units for the initial covariance for the velocity (standard deviation σ or Variance σ²)

Enumerator
m2_s2	Variance [m^2/s^2].
m_s	Standard deviation [m/s].

Definition at line 416 of file LooselyCoupledKF.hpp.

◆ KFMeas

enum NAV::LooselyCoupledKF::KFMeas : uint8_t

Measurement Keys of the Kalman filter.

Enumerator
dPosLat	Latitude difference.
dPosLon	Longitude difference.
dPosAlt	Altitude difference.
dVelN	Velocity North difference.
dVelE	Velocity East difference.
dVelD	Velocity Down difference.
dHgt	height difference
dPosX	ECEF Position X difference.
dPosY	ECEF Position Y difference.
dPosZ	ECEF Position Z difference.
dVelX	ECEF Velocity X difference.
dVelY	ECEF Velocity Y difference.
dVelZ	ECEF Velocity Z difference.

Definition at line 103 of file LooselyCoupledKF.hpp.

◆ KFStates

enum NAV::LooselyCoupledKF::KFStates : uint8_t

State Keys of the Kalman filter.

Enumerator
Roll	Roll.
Pitch	Pitch.
Yaw	Yaw.
VelN	Velocity North.
VelE	Velocity East.
VelD	Velocity Down.
PosLat	Latitude.
PosLon	Longitude.
PosAlt	Altitude.
AccBiasX	Accelerometer Bias X.
AccBiasY	Accelerometer Bias Y.
AccBiasZ	Accelerometer Bias Z.
GyrBiasX	Gyroscope Bias X.
GyrBiasY	Gyroscope Bias Y.
GyrBiasZ	Gyroscope Bias Z.
HeightBias	Baro Height Bias.
HeightScale	Baro Height Scale.
KFStates_COUNT	Amount of states.
Psi_eb_1	Angle between Earth and Body frame around 1. axis.
Psi_eb_2	Angle between Earth and Body frame around 2. axis.
Psi_eb_3	Angle between Earth and Body frame around 3. axis.
VelX	ECEF Velocity X.
VelY	ECEF Velocity Y.
VelZ	ECEF Velocity Z.
PosX	ECEF Position X.
PosY	ECEF Position Y.
PosZ	ECEF Position Z.

Definition at line 70 of file LooselyCoupledKF.hpp.

◆ PhiCalculationAlgorithm

enum class NAV::LooselyCoupledKF::PhiCalculationAlgorithm : uint8_t

strongprivate

GUI option for the Phi calculation algorithm.

Enumerator
Exponential	Van-Loan.
Taylor	Taylor.

Definition at line 532 of file LooselyCoupledKF.hpp.

◆ QCalculationAlgorithm

enum class NAV::LooselyCoupledKF::QCalculationAlgorithm : uint8_t

strongprivate

GUI option for the Q calculation algorithm.

Enumerator
VanLoan	Van-Loan.
Taylor1	Taylor.

Definition at line 544 of file LooselyCoupledKF.hpp.

◆ RandomProcess

enum class NAV::LooselyCoupledKF::RandomProcess : uint8_t

strongprivate

Available Random processes.

Enumerator
RandomWalk	Random Walk.
GaussMarkov1	Gauss-Markov 1st Order.

Definition at line 328 of file LooselyCoupledKF.hpp.

◆ StdevBaroHeightBiasUnits

enum class NAV::LooselyCoupledKF::StdevBaroHeightBiasUnits : uint8_t

strongprivate

Possible Units for the Variance of the barometric height bias.

Enumerator
m	[m]

Definition at line 302 of file LooselyCoupledKF.hpp.

◆ StdevBaroHeightScaleUnits

enum class NAV::LooselyCoupledKF::StdevBaroHeightScaleUnits : uint8_t

strongprivate

Possible Units for the Variance of the barometric height scale.

Enumerator
m_m	[m/m]

Definition at line 315 of file LooselyCoupledKF.hpp.

Constructor & Destructor Documentation

◆ LooselyCoupledKF() [1/3]

NAV::LooselyCoupledKF::LooselyCoupledKF ( )

Default constructor.

Definition at line 55 of file LooselyCoupledKF.cpp.

◆ ~LooselyCoupledKF()

NAV::LooselyCoupledKF::~LooselyCoupledKF ( )

override

Destructor.

Definition at line 77 of file LooselyCoupledKF.cpp.

◆ LooselyCoupledKF() [2/3]

NAV::LooselyCoupledKF::LooselyCoupledKF ( const LooselyCoupledKF & )

delete

Copy constructor.

◆ LooselyCoupledKF() [3/3]

NAV::LooselyCoupledKF::LooselyCoupledKF ( LooselyCoupledKF && )

delete

Move constructor.

Member Function Documentation

◆ category()

std::string NAV::LooselyCoupledKF::category ( )

staticnodiscard

String representation of the class category.

Definition at line 92 of file LooselyCoupledKF.cpp.

◆ deinitialize()

void NAV::LooselyCoupledKF::deinitialize ( )

overrideprivatevirtual

Deinitialize the node.

Reimplemented from NAV::Node.

Definition at line 1085 of file LooselyCoupledKF.cpp.

◆ e_measurementInnovation_dz()

NAV::KeyedVector< double, NAV::LooselyCoupledKF::KFMeas, 6 > NAV::LooselyCoupledKF::e_measurementInnovation_dz	(	const Eigen::Vector3d &	e_positionMeasurement,
		const Eigen::Vector3d &	e_positionEstimate,
		const Eigen::Vector3d &	e_velocityMeasurement,
		const Eigen::Vector3d &	e_velocityEstimate,
		const Eigen::Quaterniond &	e_Quat_b,
		const Eigen::Vector3d &	b_leverArm_InsGnss,
		const Eigen::Vector3d &	b_omega_ib,
		const Eigen::Matrix3d &	e_Omega_ie )

staticnodiscardprivate

Measurement innovation vector 𝜹𝐳

Parameters

[in]	e_positionMeasurement	Position measurement in ECEF coordinates in [m]
[in]	e_positionEstimate	Position estimate in ECEF coordinates in [m]
[in]	e_velocityMeasurement	Velocity measurement in the e frame in [m/s]
[in]	e_velocityEstimate	Velocity estimate in the e frame in [m/s]
[in]	e_Quat_b	Rotation quaternion from body to Earth coordinates
[in]	b_leverArm_InsGnss	l_{ba}^b lever arm from the INS to the GNSS antenna in body-frame coordinates [m]
[in]	b_omega_ib	Angular rate of body with respect to inertial system in body-frame coordinates in [rad/s]
[in]	e_Omega_ie	Skew-symmetric matrix of the Earth-rotation vector in Earth frame axes

Returns: The 6x1 measurement innovation vector 𝜹𝐳

Definition at line 2451 of file LooselyCoupledKF.cpp.

◆ e_measurementMatrix_H() [1/2]

NAV::KeyedMatrix< double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFStates, 6, NAV::LooselyCoupledKF::KFStates_COUNT > NAV::LooselyCoupledKF::e_measurementMatrix_H	(	const Eigen::Matrix3d &	e_Dcm_b,
		const Eigen::Vector3d &	b_omega_ib,
		const Eigen::Vector3d &	b_leverArm_InsGnss,
		const Eigen::Matrix3d &	e_Omega_ie )

staticnodiscardprivate

Measurement matrix for GNSS measurements at timestep k, represented in Earth frame coordinates.

Parameters

[in]	e_Dcm_b	Direction Cosine Matrix from body to Earth coordinates
[in]	b_omega_ib	Angular rate of body with respect to inertial system in body-frame coordinates in [rad/s]
[in]	b_leverArm_InsGnss	l_{ba}^b lever arm from the INS to the GNSS antenna in body-frame coordinates [m]
[in]	e_Omega_ie	Skew-symmetric matrix of the Earth-rotation vector in Earth frame axes

Returns: The 6x17 measurement matrix 𝐇

Definition at line 2257 of file LooselyCoupledKF.cpp.

◆ e_measurementMatrix_H() [2/2]

NAV::KeyedMatrix< double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFStates, 1, NAV::LooselyCoupledKF::KFStates_COUNT > NAV::LooselyCoupledKF::e_measurementMatrix_H	(	const Eigen::Vector3d &	e_positionEstimate,
		const double &	height,
		const double &	scale )

staticnodiscardprivate

Measurement matrix for barometric height measurements at timestep k, represented in Earth frame coordinates.

Parameters

[in]	e_positionEstimate	predicted position
[in]	height	predicted height (assuming that the KF internally converts to LLA at every epoch)
[in]	scale	height scale

Returns: The 1x17 measurement matrix 𝐇

Definition at line 2282 of file LooselyCoupledKF.cpp.

◆ e_measurementNoiseCovariance_R()

NAV::KeyedMatrix< double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFMeas, 6, 6 > NAV::LooselyCoupledKF::e_measurementNoiseCovariance_R ( const std::shared_ptr< const PosVel > & posVelObs ) const

nodiscardprivate

Measurement noise covariance matrix 𝐑

Parameters

[in] posVelObs Position and velocity observation

Returns: The 6x6 measurement covariance matrix 𝐑

Definition at line 2370 of file LooselyCoupledKF.cpp.

◆ e_systemMatrix_F()

NAV::KeyedMatrix< double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT > NAV::LooselyCoupledKF::e_systemMatrix_F	(	const Eigen::Quaterniond &	e_Quat_b,
		const Eigen::Vector3d &	b_specForce_ib,
		const Eigen::Vector3d &	e_position,
		const Eigen::Vector3d &	e_gravitation,
		double	r_eS_e,
		const Eigen::Vector3d &	e_omega_ie,
		const Eigen::Vector3d &	tau_bad,
		const Eigen::Vector3d &	tau_bgd ) const

nodiscardprivate

Calculates the system matrix 𝐅 for the ECEF frame.

Parameters

[in]	e_Quat_b	Attitude of the body with respect to e-system
[in]	b_specForce_ib	Specific force of the body with respect to inertial frame in [m / s^2], resolved in body coordinates
[in]	e_position	Position in ECEF coordinates in [m]
[in]	e_gravitation	Gravitational acceleration in [m/s^2]
[in]	r_eS_e	Geocentric radius. The distance of a point on the Earth's surface from the center of the Earth in [m]
[in]	e_omega_ie	Angular velocity of Earth with respect to inertial system, represented in e-sys in [rad/s]
[in]	tau_bad	Correlation length for the accelerometer in [s]
[in]	tau_bgd	Correlation length for the gyroscope in [s]

Note: See Groves (2013) chapter 14.2.3, equation (14.48)

Definition at line 1978 of file LooselyCoupledKF.cpp.

◆ e_systemNoiseCovarianceMatrix_Q()

NAV::KeyedMatrix< double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT > NAV::LooselyCoupledKF::e_systemNoiseCovarianceMatrix_Q	(	const Eigen::Vector3d &	sigma2_ra,
		const Eigen::Vector3d &	sigma2_rg,
		const Eigen::Vector3d &	sigma2_bad,
		const Eigen::Vector3d &	sigma2_bgd,
		const double &	sigma_heightBias,
		const double &	sigma_heightScale,
		const Eigen::Vector3d &	tau_bad,
		const Eigen::Vector3d &	tau_bgd,
		const Eigen::Matrix3d &	e_F_21,
		const Eigen::Matrix3d &	e_Dcm_b,
		const double &	tau_s )

staticnodiscardprivate

System noise covariance matrix 𝐐_{k-1}.

Parameters

[in]	sigma2_ra	Variance of the noise on the accelerometer specific-force measurements
[in]	sigma2_rg	Variance of the noise on the gyro angular-rate measurements
[in]	sigma2_bad	Variance of the accelerometer dynamic bias
[in]	sigma2_bgd	Variance of the gyro dynamic bias
[in]	sigma_heightBias	Standard deviation of the height bias
[in]	sigma_heightScale	Standard deviation of the height scale
[in]	tau_bad	Correlation length for the accelerometer in [s]
[in]	tau_bgd	Correlation length for the gyroscope in [s]
[in]	e_F_21	Submatrix 𝐅_21 of the system matrix 𝐅
[in]	e_Dcm_b	Direction Cosine Matrix from body to Earth coordinates
[in]	tau_s	Time interval in [s]

Returns: The 17x17 matrix of system noise covariances

Definition at line 2127 of file LooselyCoupledKF.cpp.

◆ guiConfig()

void NAV::LooselyCoupledKF::guiConfig ( )

overridevirtual

ImGui config window which is shown on double click.

Attention: Don't forget to set _hasConfig to true in the constructor of the node

Reimplemented from NAV::Node.

Definition at line 145 of file LooselyCoupledKF.cpp.

◆ hasInputPinWithSameTime()

bool NAV::LooselyCoupledKF::hasInputPinWithSameTime ( const InsTime & insTime ) const

private

Checks wether there is an input pin with the same time.

Parameters

[in] insTime Time to check

Definition at line 1090 of file LooselyCoupledKF.cpp.

◆ initialErrorCovarianceMatrix_P0()

NAV::KeyedMatrix< double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT > NAV::LooselyCoupledKF::initialErrorCovarianceMatrix_P0	(	const Eigen::Vector3d &	variance_angles,
		const Eigen::Vector3d &	variance_vel,
		const Eigen::Vector3d &	variance_pos,
		const Eigen::Vector3d &	variance_accelBias,
		const Eigen::Vector3d &	variance_gyroBias,
		const double &	variance_heightBias,
		const double &	variance_heightScale ) const

nodiscardprivate

Initial error covariance matrix P_0.

Parameters

[in]	variance_angles	Initial Covariance of the attitude angles in [rad²]
[in]	variance_vel	Initial Covariance of the velocity in [m²/s²]
[in]	variance_pos	Initial Covariance of the position in [rad² rad² m²] n-frame / [m²] i,e-frame
[in]	variance_accelBias	Initial Covariance of the accelerometer biases in [m^2/s^4]
[in]	variance_gyroBias	Initial Covariance of the gyroscope biases in [rad^2/s^2]
[in]	variance_heightBias	Initial Covariance of the height bias [m^2]
[in]	variance_heightScale	Initial Covariance of the height scale in [m^2 / m^2]

Returns: The 17x17 matrix of initial state variances

Definition at line 2185 of file LooselyCoupledKF.cpp.

◆ initialize()

bool NAV::LooselyCoupledKF::initialize ( )

overrideprivatevirtual

Initialize the node.

Reimplemented from NAV::Node.

Definition at line 922 of file LooselyCoupledKF.cpp.

◆ invokeCallbackWithPosVelAtt()

void NAV::LooselyCoupledKF::invokeCallbackWithPosVelAtt ( const PosVelAtt & posVelAtt )

private

Invoke the callback with a PosVelAtt solution (without LCKF specific output)

Parameters

[in] posVelAtt PosVelAtt solution

Definition at line 1097 of file LooselyCoupledKF.cpp.

◆ looselyCoupledPrediction()

void NAV::LooselyCoupledKF::looselyCoupledPrediction	(	const std::shared_ptr< const PosVelAtt > &	inertialNavSol,
		double	tau_i,
		const ImuPos &	imuPos )

private

Predicts the state from the InertialNavSol.

Parameters

[in]	inertialNavSol	Inertial navigation solution triggering the prediction
[in]	tau_i	Time since the last prediction in [s]
[in]	imuPos	IMU platform frame position with respect to body frame

Definition at line 1297 of file LooselyCoupledKF.cpp.

◆ looselyCoupledUpdate() [1/2]

void NAV::LooselyCoupledKF::looselyCoupledUpdate ( const std::shared_ptr< const BaroHgt > & baroHgtObs )

private

Updates the predicted state from the InertialNavSol with the Baro observation.

Parameters

[in] baroHgtObs Barometric height measurement triggering the update

Definition at line 1703 of file LooselyCoupledKF.cpp.

◆ looselyCoupledUpdate() [2/2]

void NAV::LooselyCoupledKF::looselyCoupledUpdate ( const std::shared_ptr< const PosVel > & posVelObs )

private

Updates the predicted state from the InertialNavSol with the PosVel observation.

Parameters

[in] posVelObs PosVel measurement triggering the update

Definition at line 1500 of file LooselyCoupledKF.cpp.

◆ n_measurementInnovation_dz() [1/2]

NAV::KeyedVector< double, NAV::LooselyCoupledKF::KFMeas, 1 > NAV::LooselyCoupledKF::n_measurementInnovation_dz	(	const double &	baroheight,
		const double &	height,
		const double &	heightbias,
		const double &	heightscale )

staticnodiscardprivate

Measurement innovation vector 𝜹𝐳

Parameters

[in]	baroheight	barometric height measurement in [m]
[in]	height	predicted height in [m]
[in]	heightbias	height bias in [m]
[in]	heightscale	height scale [m/m]

Returns: The 1x1 measurement innovation vector 𝜹𝐳

Definition at line 2442 of file LooselyCoupledKF.cpp.

◆ n_measurementInnovation_dz() [2/2]

NAV::KeyedVector< double, NAV::LooselyCoupledKF::KFMeas, 6 > NAV::LooselyCoupledKF::n_measurementInnovation_dz	(	const Eigen::Vector3d &	lla_positionMeasurement,
		const Eigen::Vector3d &	lla_positionEstimate,
		const Eigen::Vector3d &	n_velocityMeasurement,
		const Eigen::Vector3d &	n_velocityEstimate,
		const Eigen::Matrix3d &	T_rn_p,
		const Eigen::Quaterniond &	n_Quat_b,
		const Eigen::Vector3d &	b_leverArm_InsGnss,
		const Eigen::Vector3d &	b_omega_ib,
		const Eigen::Matrix3d &	n_Omega_ie )

staticnodiscardprivate

Measurement innovation vector 𝜹𝐳

Parameters

[in]	lla_positionMeasurement	Position measurement as Lat Lon Alt in [rad rad m]
[in]	lla_positionEstimate	Position estimate as Lat Lon Alt in [rad rad m]
[in]	n_velocityMeasurement	Velocity measurement in the n frame in [m/s]
[in]	n_velocityEstimate	Velocity estimate in the n frame in [m/s]
[in]	T_rn_p	Conversion matrix between cartesian and curvilinear perturbations to the position
[in]	n_Quat_b	Rotation quaternion from body to navigation coordinates
[in]	b_leverArm_InsGnss	l_{ba}^b lever arm from the INS to the GNSS antenna in body-frame coordinates [m]
[in]	b_omega_ib	Angular rate of body with respect to inertial system in body-frame coordinates in [rad/s]
[in]	n_Omega_ie	Skew-symmetric matrix of the Earth-rotation vector in local navigation frame axes

Returns: The 6x1 measurement innovation vector 𝜹𝐳

Definition at line 2424 of file LooselyCoupledKF.cpp.

◆ n_measurementMatrix_H() [1/2]

NAV::KeyedMatrix< double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFStates, 1, NAV::LooselyCoupledKF::KFStates_COUNT > NAV::LooselyCoupledKF::n_measurementMatrix_H	(	const double &	height,
		const double &	scale )

staticnodiscardprivate

Measurement matrix for baro height measurements at timestep k, represented in navigation coordinates.

Parameters

[in]	height	predicted height
[in]	scale	height scale

Returns: The 6x17 measurement matrix 𝐇

Definition at line 2244 of file LooselyCoupledKF.cpp.

◆ n_measurementMatrix_H() [2/2]

NAV::KeyedMatrix< double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFStates, 6, NAV::LooselyCoupledKF::KFStates_COUNT > NAV::LooselyCoupledKF::n_measurementMatrix_H	(	const Eigen::Matrix3d &	T_rn_p,
		const Eigen::Matrix3d &	n_Dcm_b,
		const Eigen::Vector3d &	b_omega_ib,
		const Eigen::Vector3d &	b_leverArm_InsGnss,
		const Eigen::Matrix3d &	n_Omega_ie )

staticnodiscardprivate

Measurement matrix for GNSS measurements at timestep k, represented in navigation coordinates.

Parameters

[in]	T_rn_p	Conversion matrix between cartesian and curvilinear perturbations to the position
[in]	n_Dcm_b	Direction Cosine Matrix from body to navigation coordinates
[in]	b_omega_ib	Angular rate of body with respect to inertial system in body-frame coordinates in [rad/s]
[in]	b_leverArm_InsGnss	l_{ba}^b lever arm from the INS to the GNSS antenna in body-frame coordinates [m]
[in]	n_Omega_ie	Skew-symmetric matrix of the Earth-rotation vector in local navigation frame axes

Returns: The 6x17 measurement matrix 𝐇

Definition at line 2216 of file LooselyCoupledKF.cpp.

◆ n_measurementNoiseCovariance_R() [1/2]

NAV::KeyedMatrix< double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFMeas, 1, 1 > NAV::LooselyCoupledKF::n_measurementNoiseCovariance_R ( const double & baroVarianceHeight )

staticnodiscardprivate

Measurement noise covariance matrix 𝐑

Parameters

[in] baroVarianceHeight Variance of height in [m²]

Returns: The 1x1 measurement covariance matrix 𝐑

Definition at line 2361 of file LooselyCoupledKF.cpp.

◆ n_measurementNoiseCovariance_R() [2/2]

NAV::KeyedMatrix< double, NAV::LooselyCoupledKF::KFMeas, NAV::LooselyCoupledKF::KFMeas, 6, 6 > NAV::LooselyCoupledKF::n_measurementNoiseCovariance_R	(	const std::shared_ptr< const PosVel > &	posVelObs,
		const Eigen::Vector3d &	lla_position,
		double	R_N,
		double	R_E ) const

nodiscardprivate

Measurement noise covariance matrix 𝐑

Parameters

[in]	posVelObs	Position and velocity observation
[in]	lla_position	Position as Lat Lon Alt in [rad rad m]
[in]	R_N	Meridian radius of curvature in [m]
[in]	R_E	Prime vertical radius of curvature (East/West) [m]

Returns: The 6x6 measurement covariance matrix 𝐑

Definition at line 2295 of file LooselyCoupledKF.cpp.

◆ n_systemMatrix_F()

NAV::KeyedMatrix< double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT > NAV::LooselyCoupledKF::n_systemMatrix_F	(	const Eigen::Quaterniond &	n_Quat_b,
		const Eigen::Vector3d &	b_specForce_ib,
		const Eigen::Vector3d &	n_omega_in,
		const Eigen::Vector3d &	n_velocity,
		const Eigen::Vector3d &	lla_position,
		double	R_N,
		double	R_E,
		double	g_0,
		double	r_eS_e,
		const Eigen::Vector3d &	tau_bad,
		const Eigen::Vector3d &	tau_bgd ) const

nodiscardprivate

Calculates the system matrix 𝐅 for the local navigation frame.

Parameters

[in]	n_Quat_b	Attitude of the body with respect to n-system
[in]	b_specForce_ib	Specific force of the body with respect to inertial frame in [m / s^2], resolved in body coordinates
[in]	n_omega_in	Angular rate of navigation system with respect to the inertial system [rad / s], resolved in navigation coordinates.
[in]	n_velocity	Velocity in n-system in [m / s]
[in]	lla_position	Position as Lat Lon Alt in [rad rad m]
[in]	R_N	Meridian radius of curvature in [m]
[in]	R_E	Prime vertical radius of curvature (East/West) [m]
[in]	g_0	Magnitude of the gravity vector in [m/s^2] (see [18] Groves, ch. 2.4.7, eq. 2.135, p. 70)
[in]	r_eS_e	Geocentric radius. The distance of a point on the Earth's surface from the center of the Earth in [m]
[in]	tau_bad	Correlation length for the accelerometer in [s]
[in]	tau_bgd	Correlation length for the gyroscope in [s]

Note: See Groves (2013) chapter 14.2.4, equation (14.63)

Definition at line 1918 of file LooselyCoupledKF.cpp.

◆ n_systemNoiseCovarianceMatrix_Q()

NAV::KeyedMatrix< double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT > NAV::LooselyCoupledKF::n_systemNoiseCovarianceMatrix_Q	(	const Eigen::Vector3d &	sigma2_ra,
		const Eigen::Vector3d &	sigma2_rg,
		const Eigen::Vector3d &	sigma2_bad,
		const Eigen::Vector3d &	sigma2_bgd,
		const double &	sigma_heightBias,
		const double &	sigma_heightScale,
		const Eigen::Vector3d &	tau_bad,
		const Eigen::Vector3d &	tau_bgd,
		const Eigen::Matrix3d &	n_F_21,
		const Eigen::Matrix3d &	T_rn_p,
		const Eigen::Matrix3d &	n_Dcm_b,
		const double &	tau_s )

staticnodiscardprivate

System noise covariance matrix 𝐐_{k-1}.

Parameters

[in]	sigma2_ra	Variance of the noise on the accelerometer specific-force measurements
[in]	sigma2_rg	Variance of the noise on the gyro angular-rate measurements
[in]	sigma2_bad	Variance of the accelerometer dynamic bias
[in]	sigma2_bgd	Variance of the gyro dynamic bias
[in]	sigma_heightBias	Standard deviation of the height bias
[in]	sigma_heightScale	Standard deviation of the height scale
[in]	tau_bad	Correlation length for the accelerometer in [s]
[in]	tau_bgd	Correlation length for the gyroscope in [s]
[in]	n_F_21	Submatrix 𝐅_21 of the system matrix 𝐅
[in]	T_rn_p	Conversion matrix between cartesian and curvilinear perturbations to the position
[in]	n_Dcm_b	Direction Cosine Matrix from body to navigation coordinates
[in]	tau_s	Time interval in [s]

Returns: The 17x17 matrix of system noise covariances

Definition at line 2071 of file LooselyCoupledKF.cpp.

◆ noiseInputMatrix_G()

NAV::KeyedMatrix< double, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT > NAV::LooselyCoupledKF::noiseInputMatrix_G ( const Eigen::Quaterniond & ien_Quat_b )

staticnodiscardprivate

Calculates the noise input matrix 𝐆

Parameters

[in] ien_Quat_b Quaternion from body frame to {i,e,n} frame

Note: See [18] Groves, ch. 14.2.6, eq. 14.79, p. 590

Definition at line 2032 of file LooselyCoupledKF.cpp.

◆ noiseScaleMatrix_W()

Eigen::Matrix< double, NAV::LooselyCoupledKF::KFStates_COUNT, NAV::LooselyCoupledKF::KFStates_COUNT > NAV::LooselyCoupledKF::noiseScaleMatrix_W	(	const Eigen::Vector3d &	sigma_ra,
		const Eigen::Vector3d &	sigma_rg,
		const Eigen::Vector3d &	sigma_bad,
		const Eigen::Vector3d &	sigma_bgd,
		const Eigen::Vector3d &	tau_bad,
		const Eigen::Vector3d &	tau_bgd,
		const double &	sigma_heightBias,
		const double &	sigma_heightScale )

nodiscardprivate

Calculates the noise scale matrix 𝐖

Parameters

[in]	sigma_ra	Standard deviation of the noise on the accelerometer specific-force measurements
[in]	sigma_rg	Standard deviation of the noise on the gyro angular-rate measurements
[in]	sigma_bad	Standard deviation of the accelerometer dynamic bias
[in]	sigma_bgd	Standard deviation of the gyro dynamic bias
[in]	tau_bad	Correlation length for the accelerometer in [s]
[in]	tau_bgd	Correlation length for the gyroscope in [s]
[in]	sigma_heightBias	Standard deviation of the height bias
[in]	sigma_heightScale	Standard deviation of the height scale

Note: See [18] Groves, ch. 14.2.6, eq. 14.79, p. 590

Definition at line 2051 of file LooselyCoupledKF.cpp.

◆ operator=() [1/2]

LooselyCoupledKF & NAV::LooselyCoupledKF::operator= ( const LooselyCoupledKF & )

delete

Copy assignment operator.

◆ operator=() [2/2]

LooselyCoupledKF & NAV::LooselyCoupledKF::operator= ( LooselyCoupledKF && )

delete

Move assignment operator.

◆ pinAddCallback()

void NAV::LooselyCoupledKF::pinAddCallback ( Node * node )

staticprivate

Function to call to add a new pin.

Parameters

[in,out] node Pointer to this node

Definition at line 129 of file LooselyCoupledKF.cpp.

◆ pinDeleteCallback()

void NAV::LooselyCoupledKF::pinDeleteCallback	(	Node *	node,
		size_t	pinIdx )

staticprivate

Function to call to delete a pin.

Parameters

[in,out]	node	Pointer to this node
[in]	pinIdx	Input pin index to delete

Definition at line 140 of file LooselyCoupledKF.cpp.

◆ recvBaroHeight()

void NAV::LooselyCoupledKF::recvBaroHeight	(	InputPin::NodeDataQueue &	queue,
		size_t	pinIdx )

private

Receive Function for the BaroHgt observation.

Parameters

[in]	queue	Queue with all the received data messages
[in]	pinIdx	Index of the pin the data is received on

Definition at line 1252 of file LooselyCoupledKF.cpp.

◆ recvImuObservation()

void NAV::LooselyCoupledKF::recvImuObservation	(	InputPin::NodeDataQueue &	queue,
		size_t	pinIdx )

private

Receive Function for the IMU observation.

Parameters

[in]	queue	Queue with all the received data messages
[in]	pinIdx	Index of the pin the data is received on

Definition at line 1140 of file LooselyCoupledKF.cpp.

◆ recvPosVelAttInit()

void NAV::LooselyCoupledKF::recvPosVelAttInit	(	InputPin::NodeDataQueue &	queue,
		size_t	pinIdx )

private

Receive Function for the PosVelAtt observation.

Parameters

[in]	queue	Queue with all the received data messages
[in]	pinIdx	Index of the pin the data is received on

Definition at line 1271 of file LooselyCoupledKF.cpp.

◆ recvPosVelObservation()

void NAV::LooselyCoupledKF::recvPosVelObservation	(	InputPin::NodeDataQueue &	queue,
		size_t	pinIdx )

private

Receive Function for the PosVel observation.

Parameters

[in]	queue	Queue with all the received data messages
[in]	pinIdx	Index of the pin the data is received on

Definition at line 1203 of file LooselyCoupledKF.cpp.

◆ restore()

void NAV::LooselyCoupledKF::restore ( const json & j )

overridevirtual

Restores the node from a json object.

Parameters

[in] j Json object with the node state

Reimplemented from NAV::Node.

Definition at line 694 of file LooselyCoupledKF.cpp.

◆ save()

json NAV::LooselyCoupledKF::save ( ) const

nodiscardoverridevirtual

Saves the node into a json object.

Reimplemented from NAV::Node.

Definition at line 624 of file LooselyCoupledKF.cpp.

◆ setSolutionPosVelAttAndCov()

void NAV::LooselyCoupledKF::setSolutionPosVelAttAndCov	(	const std::shared_ptr< PosVelAtt > &	lckfSolution,
		double	R_N,
		double	R_E )

private

Sets the covariance matrix P of the LCKF (and does the necessary unit conversions)

Parameters

[in]	lckfSolution	LCKF solution from prediction or update
[in]	R_N	Prime vertical radius of curvature (East/West) [m]
[in]	R_E	Meridian radius of curvature in [m]

Definition at line 1875 of file LooselyCoupledKF.cpp.

◆ type()

std::string NAV::LooselyCoupledKF::type ( ) const

nodiscardoverridevirtual

String representation of the class type.

Implements NAV::Node.

Definition at line 87 of file LooselyCoupledKF.cpp.

◆ typeStatic()

std::string NAV::LooselyCoupledKF::typeStatic ( )

staticnodiscard

String representation of the class type.

Definition at line 82 of file LooselyCoupledKF.cpp.

◆ updateInputPins()

void NAV::LooselyCoupledKF::updateInputPins ( )

private

Update the input pins depending on the GUI.

Definition at line 97 of file LooselyCoupledKF.cpp.

Field Documentation

◆ _accelBiasTotal

Eigen::Vector3d NAV::LooselyCoupledKF::_accelBiasTotal

private

Accumulator for acceleration bias [m/s²].

Definition at line 194 of file LooselyCoupledKF.hpp.

◆ _baroHeightMeasurementUncertaintyOverride

bool NAV::LooselyCoupledKF::_baroHeightMeasurementUncertaintyOverride

private

Whether to override the barometric height uncertainty or use the one included in the measurement.

Definition at line 395 of file LooselyCoupledKF.hpp.

◆ _barometricHeightMeasurementUncertainty

double NAV::LooselyCoupledKF::_barometricHeightMeasurementUncertainty

private

GUI selection of the barometric height measurement uncertainty (standard deviation σ or Variance σ²)

Definition at line 392 of file LooselyCoupledKF.hpp.

◆ _barometricHeightMeasurementUncertaintyUnit

BaroHeightMeasurementUncertaintyUnit NAV::LooselyCoupledKF::_barometricHeightMeasurementUncertaintyUnit

private

Gui selection for the Unit of the barometric height measurement uncertainty.

Definition at line 389 of file LooselyCoupledKF.hpp.

◆ _checkKalmanMatricesRanks

bool NAV::LooselyCoupledKF::_checkKalmanMatricesRanks

private

Check the rank of the Kalman matrices every iteration (computational expensive)

Definition at line 253 of file LooselyCoupledKF.hpp.

◆ _dynamicInputPins

gui::widgets::DynamicInputPins NAV::LooselyCoupledKF::_dynamicInputPins

private

Dynamic input pins.

Attention: This should always be the last variable in the header, because it accesses others through the function callbacks

Definition at line 569 of file LooselyCoupledKF.hpp.

◆ _enableBaroHgt

bool NAV::LooselyCoupledKF::_enableBaroHgt

private

Whether to enable barometric height (including the corresponding pin)

Definition at line 204 of file LooselyCoupledKF.hpp.

◆ _externalInitTime

InsTime NAV::LooselyCoupledKF::_externalInitTime

private

Time from the external init.

Definition at line 207 of file LooselyCoupledKF.hpp.

◆ _gnssMeasurementUncertaintyPosition

Eigen::Vector3d NAV::LooselyCoupledKF::_gnssMeasurementUncertaintyPosition

private

GUI selection of the GNSS position measurement uncertainty (standard deviation σ or Variance σ²). SPP accuracy approx. 3m in horizontal direction and 3 times worse in vertical direction.

Definition at line 358 of file LooselyCoupledKF.hpp.

◆ _gnssMeasurementUncertaintyPositionOverride

bool NAV::LooselyCoupledKF::_gnssMeasurementUncertaintyPositionOverride

private

Whether to override the position uncertainty or use the one included in the measurement.

Definition at line 361 of file LooselyCoupledKF.hpp.

◆ _gnssMeasurementUncertaintyPositionUnit

GnssMeasurementUncertaintyPositionUnit NAV::LooselyCoupledKF::_gnssMeasurementUncertaintyPositionUnit

private

Gui selection for the Unit of the GNSS measurement uncertainty for the position.

Definition at line 354 of file LooselyCoupledKF.hpp.

◆ _gnssMeasurementUncertaintyVelocity

Eigen::Vector3d NAV::LooselyCoupledKF::_gnssMeasurementUncertaintyVelocity

private

GUI selection of the GNSS NED velocity measurement uncertainty (standard deviation σ or Variance σ²)

Definition at line 375 of file LooselyCoupledKF.hpp.

◆ _gnssMeasurementUncertaintyVelocityOverride

bool NAV::LooselyCoupledKF::_gnssMeasurementUncertaintyVelocityOverride

private

Whether to override the velocity uncertainty or use the one included in the measurement.

Definition at line 378 of file LooselyCoupledKF.hpp.

◆ _gnssMeasurementUncertaintyVelocityUnit

GnssMeasurementUncertaintyVelocityUnit NAV::LooselyCoupledKF::_gnssMeasurementUncertaintyVelocityUnit

private

Gui selection for the Unit of the GNSS measurement uncertainty for the velocity.

Definition at line 372 of file LooselyCoupledKF.hpp.

◆ _gyroBiasTotal

Eigen::Vector3d NAV::LooselyCoupledKF::_gyroBiasTotal

private

Accumulator gyro bias [rad/s].

Definition at line 196 of file LooselyCoupledKF.hpp.

◆ _heightBiasTotal

double NAV::LooselyCoupledKF::_heightBiasTotal

private

Accumulator for height bias [m].

Definition at line 189 of file LooselyCoupledKF.hpp.

◆ _heightScaleTotal

double NAV::LooselyCoupledKF::_heightScaleTotal

private

Accumulator for height scale [m/m].

Definition at line 191 of file LooselyCoupledKF.hpp.

◆ _inertialIntegrator

InertialIntegrator NAV::LooselyCoupledKF::_inertialIntegrator

private

Inertial Integrator.

Definition at line 181 of file LooselyCoupledKF.hpp.

◆ _initalRollPitchYaw

std::array<double, 3> NAV::LooselyCoupledKF::_initalRollPitchYaw

private

Roll, Pitch and Yaw angles in [deg] used for initialization if not taken from separate pin.

Definition at line 199 of file LooselyCoupledKF.hpp.

◆ _initBiasAccel

Eigen::Vector3d NAV::LooselyCoupledKF::_initBiasAccel

private

GUI selection of the initial accelerometer biases.

Definition at line 513 of file LooselyCoupledKF.hpp.

◆ _initBiasAccelUnit

InitBiasAccelUnit NAV::LooselyCoupledKF::_initBiasAccelUnit

private

Gui selection for the unit of the initial accelerometer biases.

Definition at line 510 of file LooselyCoupledKF.hpp.

◆ _initBiasGyro

Eigen::Vector3d NAV::LooselyCoupledKF::_initBiasGyro

private

GUI selection of the initial gyroscope biases.

Definition at line 527 of file LooselyCoupledKF.hpp.

◆ _initBiasGyroUnit

InitBiasGyroUnit NAV::LooselyCoupledKF::_initBiasGyroUnit

private

Gui selection for the unit of the initial gyroscope biases.

Definition at line 524 of file LooselyCoupledKF.hpp.

◆ _initCovarianceAttitudeAngles

Eigen::Vector3d NAV::LooselyCoupledKF::_initCovarianceAttitudeAngles

private

GUI selection of the initial covariance diagonal values for attitude angles (standard deviation σ or Variance σ²)

Definition at line 441 of file LooselyCoupledKF.hpp.

◆ _initCovarianceAttitudeAnglesUnit

InitCovarianceAttitudeAnglesUnit NAV::LooselyCoupledKF::_initCovarianceAttitudeAnglesUnit

private

Gui selection for the Unit of the initial covariance for the attitude angles.

Definition at line 438 of file LooselyCoupledKF.hpp.

◆ _initCovarianceBiasAccel

Eigen::Vector3d NAV::LooselyCoupledKF::_initCovarianceBiasAccel

private

GUI selection of the initial covariance diagonal values for accelerometer biases (standard deviation σ or Variance σ²)

Definition at line 455 of file LooselyCoupledKF.hpp.

◆ _initCovarianceBiasAccelUnit

InitCovarianceBiasAccelUnit NAV::LooselyCoupledKF::_initCovarianceBiasAccelUnit

private

Gui selection for the Unit of the initial covariance for the accelerometer biases.

Definition at line 452 of file LooselyCoupledKF.hpp.

◆ _initCovarianceBiasGyro

Eigen::Vector3d NAV::LooselyCoupledKF::_initCovarianceBiasGyro

private

GUI selection of the initial covariance diagonal values for gyroscope biases (standard deviation σ or Variance σ²)

Definition at line 471 of file LooselyCoupledKF.hpp.

◆ _initCovarianceBiasGyroUnit

InitCovarianceBiasGyroUnit NAV::LooselyCoupledKF::_initCovarianceBiasGyroUnit

private

Gui selection for the Unit of the initial covariance for the gyroscope biases.

Definition at line 468 of file LooselyCoupledKF.hpp.

◆ _initCovarianceBiasHeight

double NAV::LooselyCoupledKF::_initCovarianceBiasHeight

private

GUI selection of the initial covariance diagonal values for the height bias (standard deviation σ or Variance σ²)

Definition at line 485 of file LooselyCoupledKF.hpp.

◆ _initCovarianceBiasHeightUnit

InitCovarianceBiasHeightUnit NAV::LooselyCoupledKF::_initCovarianceBiasHeightUnit

private

Gui selection for the Unit of the initial covariance for the height bias.

Definition at line 482 of file LooselyCoupledKF.hpp.

◆ _initCovariancePosition

Eigen::Vector3d NAV::LooselyCoupledKF::_initCovariancePosition

private

GUI selection of the initial covariance diagonal values for position (standard deviation σ or Variance σ²)

Definition at line 411 of file LooselyCoupledKF.hpp.

◆ _initCovariancePositionUnit

InitCovariancePositionUnit NAV::LooselyCoupledKF::_initCovariancePositionUnit

private

Gui selection for the Unit of the initial covariance for the position.

Definition at line 408 of file LooselyCoupledKF.hpp.

◆ _initCovarianceScaleHeight

double NAV::LooselyCoupledKF::_initCovarianceScaleHeight

private

GUI selection of the initial covariance diagonal values for the height scale (standard deviation σ or Variance σ²)

Definition at line 499 of file LooselyCoupledKF.hpp.

◆ _initCovarianceScaleHeightUnit

InitCovarianceScaleHeight NAV::LooselyCoupledKF::_initCovarianceScaleHeightUnit

private

Gui selection for the Unit of the initial covariance for the height scale.

Definition at line 496 of file LooselyCoupledKF.hpp.

◆ _initCovarianceVelocity

Eigen::Vector3d NAV::LooselyCoupledKF::_initCovarianceVelocity

private

GUI selection of the initial covariance diagonal values for velocity (standard deviation σ or Variance σ²)

Definition at line 425 of file LooselyCoupledKF.hpp.

◆ _initCovarianceVelocityUnit

InitCovarianceVelocityUnit NAV::LooselyCoupledKF::_initCovarianceVelocityUnit

private

Gui selection for the Unit of the initial covariance for the velocity.

Definition at line 422 of file LooselyCoupledKF.hpp.

◆ _initializeStateOverExternalPin

bool NAV::LooselyCoupledKF::_initializeStateOverExternalPin

private

Whether to initialize the state over an external pin.

Definition at line 201 of file LooselyCoupledKF.hpp.

◆ _initialSensorBiasesApplied

bool NAV::LooselyCoupledKF::_initialSensorBiasesApplied

private

Whether the accumulated biases have been initialized in the 'inertialIntegrator'.

Definition at line 210 of file LooselyCoupledKF.hpp.

◆ _kalmanFilter

KeyedKalmanFilterD<KFStates, KFMeas> NAV::LooselyCoupledKF::_kalmanFilter

private

Kalman Filter representation.

Definition at line 246 of file LooselyCoupledKF.hpp.

◆ _lastImuObs

std::shared_ptr<const ImuObs> NAV::LooselyCoupledKF::_lastImuObs

private

Last received IMU observation (to get ImuPos)

Definition at line 186 of file LooselyCoupledKF.hpp.

◆ _phiCalculationAlgorithm

PhiCalculationAlgorithm NAV::LooselyCoupledKF::_phiCalculationAlgorithm

private

GUI option for the Phi calculation algorithm.

Definition at line 538 of file LooselyCoupledKF.hpp.

◆ _phiCalculationTaylorOrder

int NAV::LooselyCoupledKF::_phiCalculationTaylorOrder

private

GUI option for the order of the Taylor polynom to calculate the Phi matrix.

Definition at line 541 of file LooselyCoupledKF.hpp.

◆ _preferAccelerationOverDeltaMeasurements

bool NAV::LooselyCoupledKF::_preferAccelerationOverDeltaMeasurements

private

Prefer the raw acceleration measurements over the deltaVel & deltaTheta values.

Definition at line 183 of file LooselyCoupledKF.hpp.

◆ _qCalculationAlgorithm

QCalculationAlgorithm NAV::LooselyCoupledKF::_qCalculationAlgorithm

private

GUI option for the Q calculation algorithm.

Definition at line 550 of file LooselyCoupledKF.hpp.

◆ _randomProcessAccel

RandomProcess NAV::LooselyCoupledKF::_randomProcessAccel

private

Random Process used to estimate the accelerometer biases.

Definition at line 341 of file LooselyCoupledKF.hpp.

◆ _randomProcessGyro

RandomProcess NAV::LooselyCoupledKF::_randomProcessGyro

private

Random Process used to estimate the gyroscope biases.

Definition at line 343 of file LooselyCoupledKF.hpp.

◆ _stdev_bad

Eigen::Vector3d NAV::LooselyCoupledKF::_stdev_bad

private

𝜎²_bad Variance of the accelerometer dynamic bias

Note: Value from VN-310 Datasheet (In-Run Bias Stability (Allan Variance))

Definition at line 282 of file LooselyCoupledKF.hpp.

◆ _stdev_bgd

Eigen::Vector3d NAV::LooselyCoupledKF::_stdev_bgd

private

𝜎²_bgd Variance of the gyro dynamic bias

Note: Value from VN-310 Datasheet (In-Run Bias Stability (Allan Variance))

Definition at line 294 of file LooselyCoupledKF.hpp.

◆ _stdev_ra

Eigen::Vector3d NAV::LooselyCoupledKF::_stdev_ra

private

𝜎_ra Standard deviation of the noise on the accelerometer specific-force measurements

Note: Value from VN-310 Datasheet but verify with values from Brown (2012) table 9.3 for 'High quality'

Definition at line 264 of file LooselyCoupledKF.hpp.

◆ _stdev_rg

Eigen::Vector3d NAV::LooselyCoupledKF::_stdev_rg

private

𝜎_rg Standard deviation of the noise on the gyro angular-rate measurements

Note: Value from VN-310 Datasheet but verify with values from Brown (2012) table 9.3 for 'High quality'

Definition at line 273 of file LooselyCoupledKF.hpp.

◆ _stdevAccelBiasUnits

Units::ImuAccelerometerFilterBiasUnits NAV::LooselyCoupledKF::_stdevAccelBiasUnits

private

Gui selection for the Unit of the input variance_bad parameter.

Definition at line 278 of file LooselyCoupledKF.hpp.

◆ _stdevAccelNoiseUnits

Units::ImuAccelerometerFilterNoiseUnits NAV::LooselyCoupledKF::_stdevAccelNoiseUnits

private

Gui selection for the Unit of the input stdev_ra parameter.

Definition at line 260 of file LooselyCoupledKF.hpp.

◆ _stdevBaroHeightBias

double NAV::LooselyCoupledKF::_stdevBaroHeightBias

private

Uncertainty of the barometric height bias.

Definition at line 310 of file LooselyCoupledKF.hpp.

◆ _stdevBaroHeightBiasUnits

StdevBaroHeightBiasUnits NAV::LooselyCoupledKF::_stdevBaroHeightBiasUnits

private

Gui selection for the Unit of the barometric height bias uncertainty.

Definition at line 307 of file LooselyCoupledKF.hpp.

◆ _stdevBaroHeightScale

double NAV::LooselyCoupledKF::_stdevBaroHeightScale

private

Uncertainty of the barometric height scale.

Definition at line 323 of file LooselyCoupledKF.hpp.

◆ _stdevBaroHeightScaleUnits

StdevBaroHeightScaleUnits NAV::LooselyCoupledKF::_stdevBaroHeightScaleUnits

private

Gui selection for the Unit of the barometric height scale uncertainty.

Definition at line 320 of file LooselyCoupledKF.hpp.

◆ _stdevGyroBiasUnits

Units::ImuGyroscopeFilterBiasUnits NAV::LooselyCoupledKF::_stdevGyroBiasUnits

private

Gui selection for the Unit of the input variance_bad parameter.

Definition at line 290 of file LooselyCoupledKF.hpp.

◆ _stdevGyroNoiseUnits

Units::ImuGyroscopeFilterNoiseUnits NAV::LooselyCoupledKF::_stdevGyroNoiseUnits

private

Gui selection for the Unit of the input stdev_rg parameter.

Definition at line 269 of file LooselyCoupledKF.hpp.

◆ _tau_bad

Eigen::Vector3d NAV::LooselyCoupledKF::_tau_bad

private

Correlation length of the accelerometer dynamic bias in [s].

Definition at line 285 of file LooselyCoupledKF.hpp.

◆ _tau_bgd

Eigen::Vector3d NAV::LooselyCoupledKF::_tau_bgd

private

Correlation length of the gyro dynamic bias in [s].

Definition at line 297 of file LooselyCoupledKF.hpp.

◆ dPos

const std::vector<KFMeas> NAV::LooselyCoupledKF::dPos

inlinestaticprivate

All position difference keys.

Definition at line 241 of file LooselyCoupledKF.hpp.

◆ dVel

const std::vector<KFMeas> NAV::LooselyCoupledKF::dVel

inlinestaticprivate

All velocity difference keys.

Definition at line 243 of file LooselyCoupledKF.hpp.

◆ INPUT_PORT_INDEX_IMU

size_t NAV::LooselyCoupledKF::INPUT_PORT_INDEX_IMU

staticconstexprprivate

Flow (ImuObs)

Definition at line 122 of file LooselyCoupledKF.hpp.

◆ INPUT_PORT_INDEX_POS_VEL_ATT_INIT

size_t NAV::LooselyCoupledKF::INPUT_PORT_INDEX_POS_VEL_ATT_INIT

staticconstexprprivate

Flow (PosVelAtt)

Definition at line 123 of file LooselyCoupledKF.hpp.

◆ KFAccBias

const std::vector<KFStates> NAV::LooselyCoupledKF::KFAccBias

inlinestaticprivate

All acceleration bias keys.

Definition at line 226 of file LooselyCoupledKF.hpp.

◆ KFAtt

const std::vector<KFStates> NAV::LooselyCoupledKF::KFAtt

inlinestaticprivate

All attitude keys.

Definition at line 224 of file LooselyCoupledKF.hpp.

◆ KFGyrBias

const std::vector<KFStates> NAV::LooselyCoupledKF::KFGyrBias

inlinestaticprivate

All gyroscope bias keys.

Definition at line 228 of file LooselyCoupledKF.hpp.

◆ KFPos

const std::vector<KFStates> NAV::LooselyCoupledKF::KFPos

inlinestaticprivate

All position keys.

Definition at line 220 of file LooselyCoupledKF.hpp.

◆ KFPosVel

const std::vector<KFStates> NAV::LooselyCoupledKF::KFPosVel

inlinestaticprivate

All position and velocity keys.

Definition at line 231 of file LooselyCoupledKF.hpp.

◆ KFPosVelAtt

const std::vector<KFStates> NAV::LooselyCoupledKF::KFPosVelAtt

inlinestaticprivate

All position, velocity and attitude keys.

Definition at line 234 of file LooselyCoupledKF.hpp.

◆ KFVel

const std::vector<KFStates> NAV::LooselyCoupledKF::KFVel

inlinestaticprivate

All velocity keys.

Definition at line 222 of file LooselyCoupledKF.hpp.

◆ Meas

const std::vector<KFMeas> NAV::LooselyCoupledKF::Meas

inlinestaticprivate

Vector with all measurement keys.

Definition at line 239 of file LooselyCoupledKF.hpp.

◆ OUTPUT_PORT_INDEX_SOLUTION

size_t NAV::LooselyCoupledKF::OUTPUT_PORT_INDEX_SOLUTION

staticconstexprprivate

Flow (InsGnssLCKFSolution)

Definition at line 124 of file LooselyCoupledKF.hpp.

◆ States

const std::vector<KFStates> NAV::LooselyCoupledKF::States

inlinestaticprivate

Vector with all state keys.

Definition at line 213 of file LooselyCoupledKF.hpp.

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

/home/runner/work/INSTINCT/INSTINCT/src/Nodes/DataProcessor/KalmanFilter/LooselyCoupledKF.hpp
/home/runner/work/INSTINCT/INSTINCT/src/Nodes/DataProcessor/KalmanFilter/LooselyCoupledKF.cpp

Public Types

Public Member Functions

Static Public Member Functions

Private Types

Private Member Functions

Static Private Member Functions

Private Attributes

Static Private Attributes

Additional Inherited Members

Detailed Description

Member Enumeration Documentation

◆ BaroHeightMeasurementUncertaintyUnit

◆ GnssMeasurementUncertaintyPositionUnit

◆ GnssMeasurementUncertaintyVelocityUnit

◆ InitBiasAccelUnit

◆ InitBiasGyroUnit

◆ InitCovarianceAttitudeAnglesUnit

◆ InitCovarianceBiasAccelUnit

◆ InitCovarianceBiasGyroUnit

◆ InitCovarianceBiasHeightUnit

◆ InitCovariancePositionUnit

◆ InitCovarianceScaleHeight

◆ InitCovarianceVelocityUnit

◆ KFMeas

◆ KFStates

◆ PhiCalculationAlgorithm

◆ QCalculationAlgorithm

◆ RandomProcess

◆ StdevBaroHeightBiasUnits

◆ StdevBaroHeightScaleUnits

Constructor & Destructor Documentation

◆ LooselyCoupledKF() [1/3]

◆ ~LooselyCoupledKF()

◆ LooselyCoupledKF() [2/3]

◆ LooselyCoupledKF() [3/3]

Member Function Documentation

◆ category()

◆ deinitialize()

◆ e_measurementInnovation_dz()

◆ e_measurementMatrix_H() [1/2]

◆ e_measurementMatrix_H() [2/2]

◆ e_measurementNoiseCovariance_R()

◆ e_systemMatrix_F()

◆ e_systemNoiseCovarianceMatrix_Q()

◆ guiConfig()

◆ hasInputPinWithSameTime()

◆ initialErrorCovarianceMatrix_P0()

◆ initialize()

◆ invokeCallbackWithPosVelAtt()

◆ looselyCoupledPrediction()

◆ looselyCoupledUpdate() [1/2]

◆ looselyCoupledUpdate() [2/2]

◆ n_measurementInnovation_dz() [1/2]

◆ n_measurementInnovation_dz() [2/2]

◆ n_measurementMatrix_H() [1/2]

◆ n_measurementMatrix_H() [2/2]

◆ n_measurementNoiseCovariance_R() [1/2]

◆ n_measurementNoiseCovariance_R() [2/2]

◆ n_systemMatrix_F()

◆ n_systemNoiseCovarianceMatrix_Q()

◆ noiseInputMatrix_G()

◆ noiseScaleMatrix_W()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ pinAddCallback()

◆ pinDeleteCallback()

◆ recvBaroHeight()

◆ recvImuObservation()

◆ recvPosVelAttInit()

◆ recvPosVelObservation()

◆ restore()

◆ save()

◆ setSolutionPosVelAttAndCov()

◆ type()

◆ typeStatic()

◆ updateInputPins()

Field Documentation

◆ _accelBiasTotal

◆ _baroHeightMeasurementUncertaintyOverride

◆ _barometricHeightMeasurementUncertainty