Wrapper to protect main KFState_ structure from multithreading issues. More...

Detailed Description

Wrapper to protect main KFState_ structure from multithreading issues.

The main purpose of this structure is to allow the use of the const attribute, signifying whether the object is safe to be modified without multithreading locks.

When a KFState is passed to multithreading code, it should be passed as a const reference, preventing ordinary modification of its members, which are likely to collide during parallel calculations.

Wrapper functions cast the object so it is as-if it were const and then call the ordinary functions after obtaining the object's mutex

Inheritance diagram for KFState:

Collaboration diagram for KFState:

Public Member Functions
	KFState (const KFState &kfState)

	KFState ()

KFState &	operator= (const KFState &kfState)

template<class ARCHIVE >
void	serialize (ARCHIVE &ar, const unsigned int &version)

void	initFilterEpoch ()
	Clears and initialises the state transition matrix to identity at the beginning of an epoch.

int	getKFIndex (const KFKey &key) const
	Finds the position in the KF state vector of particular states.

E_Source	getKFValue (const KFKey &key, double &value, double variance=nullptr, double adjustment_ptr=nullptr, bool allowAlternate=true) const
	Returns the value and variance of a state within the kalman filter object.

bool	getKFSigma (const KFKey &key, double &sigma)
	Returns the standard deviation of a state within the kalman filter object.

bool	addKFState (const KFKey &kfKey, const InitialState &initialState={})
	Tries to add a state to the filter object.

void	setExponentialNoise (const KFKey &kfKey, const Exponential exponential)

void	setAccelerator (const KFKey &element, const KFKey &dotElement, const KFKey &dotDotElement, const double value, const InitialState &initialState={})

void	setKFTrans (const KFKey &dest, const KFKey &source, const double value, const InitialState &initialState={})
	Adds dynamics to a filter state by inserting off-diagonal, non-time dependent elements to transition matrix.

void	setKFTransRate (const KFKey &integral, const KFKey &rate, const double value, const InitialState &initialRateState={}, const InitialState &initialIntegralState={})
	Adds dynamics to a filter state by inserting off-diagonal, time dependent elements to transition matrix.

void	addNoiseElement (const KFKey &obsKey, const double variance)

void	removeState (const KFKey &kfKey)
	Remove a state from a kalman filter object.

void	stateTransition (Trace &trace, GTime newTime, MatrixXd *stm_ptr=nullptr)
	Add process noise and dynamics to filter object according to time gap.

void	manualStateTransition (Trace &trace, GTime newTime, MatrixXd &stm, MatrixXd &procNoise)
	Add process noise and dynamics to filter object manually.

void	preFitSigmaCheck (Trace &trace, KFMeas &kfMeas, KFKey &badStateKey, int &badMeasIndex, KFStatistics &statistics, int begX, int numX, int begH, int numH)
	Compare variances of measurements and pre-filtered states to detect unreasonable values Ref: Wang et al.

void	postFitSigmaChecks (Trace &trace, KFMeas &kfMeas, VectorXd &dx, int iteration, KFKey &badStateKey, int &badMeasIndex, KFStatistics &statistics, int begX, int numX, int begH, int numH)
	Compare variances of measurements and filtered states to detect unreasonable values.

double	stateChiSquare (Trace &trace, MatrixXd &Pp, VectorXd &dx, int begX, int numX, int begH, int numH)
	Compute Chi-square increment based on the change of fitting solution.

double	measChiSquare (Trace &trace, KFMeas &kfMeas, VectorXd &dx, int begX, int numX, int begH, int numH)
	Compute Chi-square increment based on post-fit residuals.

double	innovChiSquare (Trace &trace, KFMeas &kfMeas, int begX, int numX, int begH, int numH)
	Compute Chi-square increment based on pre-fit residuals (innovations)

bool	kFilter (Trace &trace, KFMeas &kfMeas, VectorXd &xp, MatrixXd &Pp, VectorXd &dx, int begX=0, int numX=-1, int begH=0, int numH=-1)
	Kalman filter.

bool	chiQC (Trace &trace, KFMeas &kfMeas, VectorXd &xp)
	Perform chi squared quality control.

void	outputStates (Trace &trace, string suffix="", int begX=0, int numX=-1)
	Output keys and states in human readable format.

void	outputConditionNumber (Trace &trace)

void	outputCorrelations (Trace &trace)

void	outputMeasurements (Trace &trace, KFMeas &meas)

bool	doStateRejectCallbacks (Trace &trace, KFMeas &kfMeas, KFKey &badKey, bool postFit)

bool	doMeasRejectCallbacks (Trace &trace, KFMeas &kfMeas, int badIndex, bool postFit)

void	filterKalman (Trace &trace, KFMeas &kfMeas, const string &suffix="", bool innovReady=false, map< string, FilterChunk > *filterChunkMap_ptr=nullptr)
	Kalman filter operation.

void	leastSquareInitStates (Trace &trace, KFMeas &kfMeas, bool initCovars=false, VectorXd *dx=nullptr, bool innovReady=false)
	Least squares estimator for new kalman filter states.

VectorXd	getSubState (map< KFKey, int > &kfKeyMap, MatrixXd covarMat_ptr=nullptr, VectorXd adjustVec_ptr=nullptr) const
	Get a portion of the state vector by passing a list of keys.

void	getSubState (map< KFKey, int > &kfKeyMap, KFState &kfState) const
	Get a portion of a state by passing in a list of keys.

KFState	getSubState (vector< KF >) const

void	removeState (const KFKey &kfKey) const

void	setExponentialNoise (const KFKey &kfKey, const Exponential exponential) const

bool	addKFState (const KFKey &kfKey, const InitialState &initialState={}) const

void	setKFTrans (const KFKey &dest, const KFKey &source, const double value, const InitialState &initialState={}) const

void	setKFTransRate (const KFKey &integral, const KFKey &rate, const double value, const InitialState &initialRateState={}, const InitialState &initialIntegralState={}) const

Data Fields
mutex	kfStateMutex

Data Fields inherited from KFState_
bool	lsqRequired = false
	Uninitialised parameters require least squares calculation.

GTime	time = {}

VectorXd	x
	State.

MatrixXd	P
	State Covariance.

VectorXd	dx
	Last filter update.

map< KFKey, int >	kfIndexMap
	Map from key to indexes of parameters in the state vector.

map< KFKey, map< KFKey, map< int, double > > >	stateTransitionMap

map< KFKey, double >	gaussMarkovTauMap

map< KFKey, double >	gaussMarkovMuMap

map< KFKey, double >	procNoiseMap

map< KFKey, double >	initNoiseMap

map< KFKey, Exponential >	exponentialNoiseMap

vector< StateRejectCallback >	stateRejectCallbacks

vector< MeasRejectCallback >	measRejectCallbacks

map< string, FilterChunk >	filterChunkMap

map< string, string >	metaDataMap

bool	chiQCPass = false

double	chi = 0

int	dof = 0

string	id = "KFState"

string	rts_basename = ""

bool	output_residuals = false

bool	outputMongoMeasurements = false

KFState *	alternate_ptr = nullptr

map< string, int >	statisticsMap

map< string, int >	statisticsMapSum

Data Fields inherited from FilterOptions
bool	simulate_filter_only = false

bool	assume_linearity = false

bool	advanced_postfits = false

bool	joseph_stabilisation = false

E_Inverter	inverter = E_Inverter::LDLT

PrefitOptions	prefitOpts

PostfitOptions	postfitOpts

ChiSquareOptions	chiSquareTest

Data Fields inherited from RtsOptions
string	rts_filename = "Filter-<CONFIG>-<RECEIVER>.rts"

string	rts_directory = "<OUTPUTS_ROOT>"

int	rts_lag = -1

int	rts_interval = 0

string	rts_smoothed_suffix = "_smoothed"

bool	output_intermediate_rts = false

bool	queue_rts_outputs = false

E_Inverter	rts_inverter = E_Inverter::LDLT

Static Public Attributes
static const KFKey	oneKey = {.type = KF::ONE}
	KFStates generally contain a ONE state as the first element, used for converting matrix additions to matrix multiplications.

Constructor & Destructor Documentation

◆ KFState() [1/2]

KFState::KFState ( const KFState & kfState )

inline

◆ KFState() [2/2]

KFState::KFState ( )

inline

Here is the call graph for this function:

Member Function Documentation

◆ addKFState() [1/2]

bool KFState::addKFState	(	const KFKey &	kfKey,
		const InitialState &	initialState = {} )

Tries to add a state to the filter object.

If it does not exist, it adds it to a list of states to be added. Call consolidateKFState() to apply the list to the filter object

Parameters

kfKey	The key to add to the state
initialState	The initial conditions to add to the state

Here is the caller graph for this function:

◆ addKFState() [2/2]

bool KFState::addKFState	(	const KFKey &	kfKey,
		const InitialState &	initialState = {} ) const

inline

◆ addNoiseElement()

void KFState::addNoiseElement	(	const KFKey &	obsKey,
		const double	variance )

◆ chiQC()

bool KFState::chiQC	(	Trace &	trace,
		KFMeas &	kfMeas,
		VectorXd &	xp )

Perform chi squared quality control.

Parameters

trace	Trace to output to
kfMeas	Measurements, noise, and design matrix
xp	Post filtered state vector

Here is the caller graph for this function:

◆ doMeasRejectCallbacks()

bool KFState::doMeasRejectCallbacks	(	Trace &	trace,
		KFMeas &	kfMeas,
		int	badIndex,
		bool	postFit )

Parameters

trace	Trace file for output
kfMeas	Measurements that were passed to the filter
badIndex	Index in measurement list that was unsatisfactory
postFit	Rejection occured during post-filtering checks

Here is the caller graph for this function:

◆ doStateRejectCallbacks()

bool KFState::doStateRejectCallbacks	(	Trace &	trace,
		KFMeas &	kfMeas,
		KFKey &	badKey,
		bool	postFit )

Parameters

trace	Trace file for output
kfMeas	Measurements that were passed to the filter
badKey	Key in state that was unsatisfactory
postFit	Rejection occured during post-filtering checks

Here is the caller graph for this function:

◆ filterKalman()

void KFState::filterKalman	(	Trace &	trace,
		KFMeas &	kfMeas,
		const string &	suffix = "",
		bool	innovReady = false,
		map< string, FilterChunk > *	filterChunkMap_ptr = nullptr )

Kalman filter operation.

Parameters

trace	Trace file for output
kfMeas	Measurement object
suffix	Suffix to append to residuals block
innovReady	Innovation already constructed
filterChunkMap_ptr	Optional map of chunks for parallel processing of sub filters

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getKFIndex()

int KFState::getKFIndex ( const KFKey & key ) const

Finds the position in the KF state vector of particular states.

Parameters

key	Key to search for in state

Here is the caller graph for this function:

◆ getKFSigma()

bool KFState::getKFSigma	(	const KFKey &	key,
		double &	sigma )

Returns the standard deviation of a state within the kalman filter object.

Parameters

key	Key to search for in state
sigma	Output value

Here is the caller graph for this function:

◆ getKFValue()

E_Source KFState::getKFValue	(	const KFKey &	key,
		double &	value,
		double *	variance_ptr = nullptr,
		double *	adjustment_ptr = nullptr,
		bool	allowAlternate = true ) const

Returns the value and variance of a state within the kalman filter object.

Parameters

key	Key to search for in state
value	Output value
variance_ptr	Optional variance output
adjustment_ptr	Optional adjustment output
allowAlternate	Optional flag to disable alternate filter

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getSubState() [1/3]

void KFState::getSubState	(	map< KFKey, int > &	kfKeyMap,
		KFState &	subState ) const

Get a portion of a state by passing in a list of keys.

Only gets some aspects, as most aren't required

Parameters

kfKeyMap	List of keys to return within substate
subState	Output state

Here is the call graph for this function:

◆ getSubState() [2/3]

VectorXd KFState::getSubState	(	map< KFKey, int > &	kfKeyMap,
		MatrixXd *	covarMat_ptr = nullptr,
		VectorXd *	adjustVec_ptr = nullptr ) const

Get a portion of the state vector by passing a list of keys.

Parameters

kfKeyMap	List of keys to return within substate
covarMat_ptr	Optional pointer to a matrix for output of covariance submatrix
adjustVec_ptr	Optional pointer to a vector for output of last adjustments

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getSubState() [3/3]

KFState KFState::getSubState ( vector< KF > types ) const

◆ initFilterEpoch()

void KFState::initFilterEpoch ( )

Clears and initialises the state transition matrix to identity at the beginning of an epoch.

Also clears any noise that was being added for the initialisation of a new state.

Here is the caller graph for this function:

◆ innovChiSquare()

double KFState::innovChiSquare	(	Trace &	trace,
		KFMeas &	kfMeas,
		int	begX,
		int	numX,
		int	begH,
		int	numH )

Compute Chi-square increment based on pre-fit residuals (innovations)

Parameters

trace	Trace to output to
kfMeas	Measurements, noise, and design matrix
begX	Index of first state element to process
numX	Number of states elements to process
begH	Index of first measurement to process
numH	Number of measurements to process

Here is the caller graph for this function:

◆ kFilter()

bool KFState::kFilter	(	Trace &	trace,
		KFMeas &	kfMeas,
		VectorXd &	xp,
		MatrixXd &	Pp,
		VectorXd &	dx,
		int	begX = 0,
		int	numX = -1,
		int	begH = 0,
		int	numH = -1 )

Kalman filter.

Parameters

trace	Trace to output to
kfMeas	Measurements, noise, and design matrices
xp	Post-update state vector
Pp	Post-update covariance of states
dx	Post-update state innovation
begX	Index of first state element to process
numX	Number of state elements to process
begH	Index of first measurement to process
numH	Number of measurements to process

Here is the caller graph for this function:

◆ leastSquareInitStates()

void KFState::leastSquareInitStates	(	Trace &	trace,
		KFMeas &	kfMeas,
		bool	initCovars = false,
		VectorXd *	dx = nullptr,
		bool	innovReady = false )

Least squares estimator for new kalman filter states.

If new states have been added that do not contain variance values, the filter will assume that these states values and covariances should be estimated using least squares.

This function will extract the minimum required states from the existing state vector, and the minimum required measurements in order to perform least squares for the uninitialised states.

Parameters

trace	Trace file for output
kfMeas	Measurement object
initCovars	Option to also initialise off-diagonal covariance values
dx	Optional output of state deltas
innovReady	Perform conversion between V & Y

Here is the call graph for this function:

Here is the caller graph for this function:

◆ manualStateTransition()

void KFState::manualStateTransition	(	Trace &	trace,
		GTime	newTime,
		MatrixXd &	F,
		MatrixXd &	Q0 )

Add process noise and dynamics to filter object manually.

BEWARE! Not recommended for ordinary use, likely to break things. Dont touch unless you really know what you're doing. Hint - you dont really know what you're doing

Here is the call graph for this function:

Here is the caller graph for this function:

◆ measChiSquare()

double KFState::measChiSquare	(	Trace &	trace,
		KFMeas &	kfMeas,
		VectorXd &	dx,
		int	begX,
		int	numX,
		int	begH,
		int	numH )

Compute Chi-square increment based on post-fit residuals.

Parameters

trace	Trace file to output to
kfMeas	Measurements, noise, and design matrix
dx	The innovations from filtering to recalculate the deltas.
begX	Index of first state element to process
numX	Number of states elements to process
begH	Index of first measurement to process
numH	Number of measurements to process

Here is the caller graph for this function:

◆ operator=()

KFState & KFState::operator= ( const KFState & kfState )

inline

◆ outputConditionNumber()

void KFState::outputConditionNumber ( Trace & trace )

◆ outputCorrelations()

void KFState::outputCorrelations ( Trace & trace )

Here is the call graph for this function:

Here is the caller graph for this function:

◆ outputMeasurements()

void KFState::outputMeasurements	(	Trace &	trace,
		KFMeas &	meas )

Here is the call graph for this function:

Here is the caller graph for this function:

◆ outputStates()

void KFState::outputStates	(	Trace &	output,
		string	suffix = "",
		int	begX = 0,
		int	numX = -1 )

Output keys and states in human readable format.

Parameters

output	Trace to output to
suffix	Suffix to append to state block info tag in trace files
begX	Index of first state element to process
numX	Number of state elements to process

Here is the call graph for this function:

Here is the caller graph for this function:

◆ postFitSigmaChecks()

void KFState::postFitSigmaChecks	(	Trace &	trace,
		KFMeas &	kfMeas,
		VectorXd &	dx,
		int	iteration,
		KFKey &	badStateKey,
		int &	badMeasIndex,
		KFStatistics &	statistics,
		int	begX,
		int	numX,
		int	begH,
		int	numH )

Compare variances of measurements and filtered states to detect unreasonable values.

Parameters

trace	Trace file to output to
kfMeas	Measurements, noise, and design matrix
dx	The innovations from filtering to recalculate the deltas.
iteration	Number of iterations prior to this check
badStateKey	Key to the state that has worst ratio (only if worse than badMeasIndex)
badMeasIndex	Index of the measurement that has the worst ratio
statistics	Test statistics
begX	Index of first state element to process
numX	Number of state elements to process
begH	Index of first measurement to process
numH	Number of measurements to process

Here is the caller graph for this function:

◆ preFitSigmaCheck()

void KFState::preFitSigmaCheck	(	Trace &	trace,
		KFMeas &	kfMeas,
		KFKey &	badStateKey,
		int &	badMeasIndex,
		KFStatistics &	statistics,
		int	begX,
		int	numX,
		int	begH,
		int	numH )

Compare variances of measurements and pre-filtered states to detect unreasonable values Ref: Wang et al.

(1997) - On Quality Control in Hydrographic GPS Surveying & Wieser et al. (2004) - Failure Scenarios to be Considered with Kinematic High Precision Relative GNSS Positioning - http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.573.9628&rep=rep1&type=pdf

Parameters

trace	Trace to output to
kfMeas	Measurements, noise, and design matrix
badStateKey	Key to the state that has worst ratio (only if worse than badMeasIndex)
badMeasIndex	Index of the measurement that has the worst ratio
statistics	Test statistics
begX	Index of first state element to process
numX	Number of states elements to process
begH	Index of first measurement to process
numH	Number of measurements to process

Here is the caller graph for this function:

◆ removeState() [1/2]

void KFState::removeState ( const KFKey & kfKey )

Remove a state from a kalman filter object.

Parameters

kfKey Key to search for in state

Here is the caller graph for this function:

◆ removeState() [2/2]

void KFState::removeState ( const KFKey & kfKey ) const

inline

◆ serialize()

template<class ARCHIVE >

void KFState::serialize	(	ARCHIVE &	ar,
		const unsigned int &	version )

inline

◆ setAccelerator()

void KFState::setAccelerator	(	const KFKey &	element,
		const KFKey &	dotElement,
		const KFKey &	dotDotElement,
		const double	value,
		const InitialState &	initialState = {} )

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setExponentialNoise() [1/2]

void KFState::setExponentialNoise	(	const KFKey &	kfKey,
		const Exponential	exponential )

Here is the caller graph for this function:

◆ setExponentialNoise() [2/2]

void KFState::setExponentialNoise	(	const KFKey &	kfKey,
		const Exponential	exponential ) const

inline

◆ setKFTrans() [1/2]

void KFState::setKFTrans	(	const KFKey &	dest,
		const KFKey &	source,
		const double	value,
		const InitialState &	initialState = {} )

Adds dynamics to a filter state by inserting off-diagonal, non-time dependent elements to transition matrix.

Parameters

dest	Key to search for in state to change in transition
source	Key to search for in state as source
value	Input value
initialState	Initial state.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setKFTrans() [2/2]

void KFState::setKFTrans	(	const KFKey &	dest,
		const KFKey &	source,
		const double	value,
		const InitialState &	initialState = {} ) const

inline

◆ setKFTransRate() [1/2]

void KFState::setKFTransRate	(	const KFKey &	integralKey,
		const KFKey &	rateKey,
		const double	value,
		const InitialState &	initialRateState = {},
		const InitialState &	initialIntegralState = {} )

Adds dynamics to a filter state by inserting off-diagonal, time dependent elements to transition matrix.

Parameters

integralKey	Key to search for in state to change in transition
rateKey	Key to search for in state as source
value	Input value
initialRateState	Initial state for rate state.
initialIntegralState	Initial state for the thing that is modified by the rate

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setKFTransRate() [2/2]

void KFState::setKFTransRate	(	const KFKey &	integral,
		const KFKey &	rate,
		const double	value,
		const InitialState &	initialRateState = {},
		const InitialState &	initialIntegralState = {} ) const

inline

◆ stateChiSquare()

double KFState::stateChiSquare	(	Trace &	trace,
		MatrixXd &	Pp,
		VectorXd &	dx,
		int	begX,
		int	numX,
		int	begH,
		int	numH )

Compute Chi-square increment based on the change of fitting solution.

Parameters

trace	Trace file to output to
Pp	Post-update covariance of states
dx	The innovations from filtering to recalculate the deltas.
begX	Index of first state element to process
numX	Number of states elements to process
begH	Index of first measurement to process
numH	Number of measurements to process

Here is the caller graph for this function:

◆ stateTransition()

void KFState::stateTransition	(	Trace &	trace,
		GTime	newTime,
		MatrixXd *	stm_ptr = nullptr )

Add process noise and dynamics to filter object according to time gap.

This will also sort states according to their kfKey as a result of the way the state transition matrix is generated.

Parameters

trace	Trace file for output
newTime	Time of update for process noise and dynamics (s)
stm_ptr	Optional pointer to output state transition matrix

Here is the call graph for this function:

Here is the caller graph for this function:

Field Documentation

◆ kfStateMutex

mutex KFState::kfStateMutex

◆ oneKey

const KFKey KFState::oneKey = {.type = KF::ONE}

static

KFStates generally contain a ONE state as the first element, used for converting matrix additions to matrix multiplications.

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

/ginan/src/cpp/common/algebra.hpp
/ginan/src/cpp/common/algebra.cpp

Detailed Description

Public Member Functions

Data Fields

Static Public Attributes

Constructor & Destructor Documentation

◆ KFState() [1/2]

◆ KFState() [2/2]

Member Function Documentation

◆ addKFState() [1/2]

◆ addKFState() [2/2]

◆ addNoiseElement()

◆ chiQC()

◆ doMeasRejectCallbacks()

◆ doStateRejectCallbacks()

◆ filterKalman()

◆ getKFIndex()

◆ getKFSigma()

◆ getKFValue()

◆ getSubState() [1/3]

◆ getSubState() [2/3]

◆ getSubState() [3/3]

◆ initFilterEpoch()

◆ innovChiSquare()

◆ kFilter()

◆ leastSquareInitStates()

◆ manualStateTransition()

◆ measChiSquare()

◆ operator=()

◆ outputConditionNumber()

◆ outputCorrelations()

◆ outputMeasurements()

◆ outputStates()

◆ postFitSigmaChecks()

◆ preFitSigmaCheck()

◆ removeState() [1/2]

◆ removeState() [2/2]

◆ serialize()

◆ setAccelerator()

◆ setExponentialNoise() [1/2]

◆ setExponentialNoise() [2/2]

◆ setKFTrans() [1/2]

◆ setKFTrans() [2/2]

◆ setKFTransRate() [1/2]

◆ setKFTransRate() [2/2]

◆ stateChiSquare()

◆ stateTransition()

Field Documentation

◆ kfStateMutex

◆ oneKey